How to constrain the time and how to derive the acceleration and position trajectories from given velocity trajectory?

Dear all,

I am trying to generate trajectories in matlab for a robot to pass through. It is 1-D. There are 2 questions:

1- Using the trapveltraj built-in function in matlab, how can I assign a constrained time to acheive the desired velocity/position trajectory? (I know EndTime Name-Value argument exists but looks like it sets the time duration between each sample?!).

2- second question is imagine if there is a given velocity profile, how can I derive the corresponding displacement/position and acceleration trajectories?

(using trapveltraj we usually define the waypoints to be passed and the number of samples)

Answer 1

[William Rose]

@Alireza,

You are correct: trapveltraj(,EndTime=tEnd) assumes tEnd is a scalar or a vector of segment durations.

Example 1: The desired time and position values for a 1D trajectory are t=0,2,4,6,8 and

x=0,2,3,2,0. Make a 41-point trajectory for this motion.

Since the segment durations are all the same, we pass a scalar for EndTime.

wayPoints=[0,2,3,2,0]; % x-values for 1-D trajectory
Td=2;  % segment duration
numSamples=41;
[q,qd,qdd,tSamples,pp] = trapveltraj(wayPoints,numSamples,EndTime=Td);
% Plot path
tPoints=(0:length(wayPoints)-1)*Td;  % needed for plot of waypoints 
plot(tSamples,q,'-r.',tPoints,wayPoints,'bs') 
grid on; xlabel('Time'); ylabel('Position') 
legend('Trajectory','Waypoints'); title('Example 1')

Example 2. The desired time and position values for a 1D trajectory are t=0,2,3,4,6 and

x=0,2,3,2,0. Make a 31-point trajectory for this motion.

The segment durations vary, so we pass a vector for EndTime.

xPts=[0,2,3,2,0]; % x-values for 1-D trajectory
tPts=[0,2,3,4,6]; % time points for 1-D trajectory
numSamples=31;
Td=diff(tPts);    % segment durations
[q,qd,qdd,tSamples,pp] = trapveltraj(wayPoints,numSamples,EndTime=Td);
% Plot path
plot(tSamples,q,'-r.',tPts,xPts,'bs') 
grid on; xlabel('Time'); ylabel('Position') 
legend('Trajectory','Waypoints'); title('Example 2')

Example 2, above, shows that the trajectory slows down as it passes through waypoints 2 and 4, even though it wouldn't have to do so.

Your question 2:

If you have a velocity profile, then you assume a starting x-value, and estimate the remaining x-values by integration.

Example 3. Make a trajectory with 4 segments (i.e. 5 waypoints) and 41 points. The segments are 2 seconds long each and the average velocity in each segment is 1,2,3,2.

Td=2;               % duration of each segment
numSamples=41;
vAvg=[1,2,3,2];     % average velocity per segment
x0=0;               % initial position
xPts=cumsum([x0,vAvg*Td]);  % estimate x-values by integration
[q,qd,qdd,tSamples,pp] = trapveltraj(xPts,numSamples,EndTime=Td);
% Plot path
tPts=Td*(0:length(vAvg));  % needed for plot of waypoints 
plot(tSamples,q,'-r.',tPts,xPts,'bs') 
grid on; xlabel('Time'); ylabel('Position') 
legend('Trajectory','Waypoints'); title('Example 3')

Example 3, above, shows the correct average velocity for each segment.

[William Rose]

@Alireza,

In the examples above, the trajectories decelerate and accelerate at each waypoint. Therefore the instantaneous velocity is not equal to the average velocity. If you want to specify the exact velocity, make the acceleration time very small. When I tried AccelTime=0, I got an error, so I used AccelTime=1e-3. This can result in very high peak accelerations, which will require a very powerful robot.

Example 4. Make a trajectory with 4 segments (i.e. 5 waypoints) and 81 points. The segments are 2 seconds long each and the instantaneous velocity in each segment is 1,2,3,2.

Td=2;               % segment duration
Tacc=1e-3;          % duration of acceleration and deceleration
numSamples=81;
vAvg=[1,2,3,2];     % instantaneous velocity per segment
x0=0;               % initial position
xPts=cumsum([x0,vAvg*Td]);  % estimate x-values by integration
[q,qd,qdd,tSamples,pp] = trapveltraj(xPts,numSamples,EndTime=Td,AccelTime=Tacc);
% Plot path
tPts=Td*(0:length(vAvg));  % needed for plot of waypoints 
plot(tSamples,q,'-r.',tPts,xPts,'bs') 
grid on; xlabel('Time'); ylabel('Position') 
legend('Trajectory','Waypoints'); title('Example 4')

Example 4 has straight trajectory segments with sharp corners, and example 3 has rounded trajectory segments with gradual corners. The difference is because AccelTime =1e-3 in example 4, and AccelTime is not specified (therefore at the default value) in example 3.

Answer 2

[Alireza]

@William Rose

thanks for your comprehensive answers.

I think you know all this via experience.

But what if i want to generate velocity profile of a reciprocating robot, imagine it goes in y axis only in a back-and-forth fashion.

the velocity profile i need to generate is kind of symmetric and has alsmot 10 phase shifts: accelerate to start from 0 vel to reach to max vel at 5 m/s,

phase 2 is to keep this constant vel and then decelerate to 0 vel (where robot has already hit the maximum range in y axis defined as 'b' in my code snippet) and reaching to negative vel to come back. Now the robot needs to go to -b and then come back to y=0.

Whatever number of samples or EndTime I use I cannot get a symmetric and equally-spaced ve profile.

Any hints?

b = 1e-2;% [m] maximum range of robot 
wp1linsp1 = linspace(0, b, 10);
wp1linsp2 = linspace(b, 0, 10);
wp1linsp3 = linspace(0, -b, 10);
wp1linsp4 = linspace(-b, 0, 10);
wp1 = [wp1linsp1, wp1linsp2, wp1linsp3, wp1linsp4]; % waypoints 
NumSamp = 10;
% timepoints = linspace(0, 1, length(wp1));
[q, qd, qdd, tvec, pp] = trapveltraj(wp1, NumSamp, 'EndTime', 0.1); 

[William Rose]

@Alireza,

You say: Max range=+-0.01 m, max velocity=5 m/s.

I assume, in my analysis and code below, that the robot starts moving in the opposite direction as soon as it reaches point b or point -b.

Function trapveltraj() will make a trajectory meeting the requirements, if we tell it two of the following four things:

• End Time — Duration of each segment between two waypoints
• Peak Velocity — Peak velocity for each segment
• Acceleration Time — Time spent in the acceleration and deceleration phases of each segment
• Peak Acceleration — Magnitude of the acceleration applied during the acceleration and deceleration phases of each segment

I think acceleration times (option A) and peak acceleration (option B) are the more appealing options. We will consider both options.

A. Specify acceleration times. To do this, we could just specify a time value in seconds. I think it would be more elegant to specify accFrac=the fraction of each segment spent accelerating. It must be that 0<=accFrac<=0.5, because the total time spent accelerating and decelerating must not exceed the segment duration. With a little algebra, you can verify that time duration of segment i is

,

where Ti=time duration and Di=distance covered in segment i.

We need a function, wayPointAccTime(), that returns the waypoints (i.e. position values) and acceleration times, given vmax and b and accFrac and Nc=desired number of cycles. It will also return the total elapsed time at each waypoint, which will be useful for plotting. Each cycle goes from position 0 to +b to -b to 0. If there is more than 1 cycle, the robot does not slow down as it goes from -b to +b. The waypoints and acceleration times from wayPointAccTime() will be inputs to trapveltraj().

function [wayPts,accTime,elapTime] = wayPointAccTime(b,vmax,Nc,accFrac)
% WAYPOINTACCTIME Compute waypoints and acceleraiton times for a 1-D trajectory.
% See @Alireza, "How to constrain the time...", Matlab Answers 2024-12-16.
% Alireza and W. Rose
% Inputs
%   b = max + and - position of robot
%   vmax = maximum velocity
%   Nc = number of cycles
%   accFrac= time fraction of each cycle spent accelerating (0<=accFrac<=0.5)
% Outputs
%   wayPts=vector of 2*Nc+2 positions. wayPt(1)=wayPt(end)=0.
%   accTime = vector of 2*Nc+1 times = duration of acceleration in each segment 
%                                    = duration of deceleration in each segment.
%   elapTime=vector of 2*Nc+2 times=total elapsed time at each waypoint.
% Outputs wayPts and accTime are inputs to trapveltraj().
wayPts=b*(-1).^(1:2*Nc+2);      % [-b,+b,-b,+b...], length 2*Nc+2 
wayPts(1)=0; wayPts(end)=0;     % fist and last elements=0
d=abs(diff(wayPts));            % distances between waypoints
T=d/(vmax*(1-accFrac));         % duration of each segment
accTime=T*accFrac;              % acceleration durations for each segment
elapTime=cumsum([0,T]);         % total elapsed time to each waypoint
end

Check that function wayPointAccTime() works as expected:

b=0.05;         % max displacement [m]
vmax=10;        % max veloc [m/s]
Nc=2;           % number of cycles
accFrac=0.25;   % time fraction of each segment spent accelerating
[wayPts,accTime,elapTime] = wayPointAccTime(b,vmax,Nc,accFrac);
% Display outputs from function wayPointAccTime()
fprintf('Way points: %.3f %.3f %.3f %.3f %.3f %.3f \n',wayPts)

Way points: 0.000 0.050 -0.050 0.050 -0.050 0.000 

fprintf('Elapsed time x 1000: %.2f %.2f %.2f %.2f %.2f %.2f \n',elapTime*1000)

Elapsed time x 1000: 0.00 6.67 20.00 33.33 46.67 53.33 

fprintf('Accel. time x 1000:  %.2f %.2f %.2f %.2f %.2f \n',accTime*1000)

Accel. time x 1000:  1.67 3.33 3.33 3.33 1.67 

The results above look reasonable.

Use trapveltraj() to make a trajectory. The inputs to trapveltraj() will include the waypoints and acceleration times.

numSamples=24*length(accTime)+1;  % number of points=24*number of segements, +1
[q,qd,qdd,tSamples,pp] = ...
    trapveltraj(wayPts,numSamples,AccelTime=accTime,PeakVelocity=vmax);
% Plot results
figure
subplot(211), plot(elapTime,wayPts,'bs',tSamples,q,'-r.') 
ylabel('Position (m)'); grid on
legend('Way Points','Trajectory'); title('Position')
subplot(212), plot(tSamples,qd,'-r.')
ylabel('Velocity (m/s)'); xlabel('Time (s)'); grid on
legend('Trajectory'); title('Velocity')

The plot above shows that the functions are working as expected and they produce a reasonable trajectory.

B. Specify peak acceleration, accMax, instead of acceleration times. With this option, we will not specify acceleration fraction (accFrac). With this option, we do not need function wayPointAccTime(), because we don't need the acceleration time vector. You can show that accMax must satisfy

in order that the peak velocity, vMax, be reached at least momentarily in every segment. You can also show, with some algebra, that Ti, the fastest allowed time duration of segment i, is given by

where Di = distance covered in segment i. You can work out the details to specify a trajectory using vMax and accMax.