%% Compute Trapezoidal Velocity Trajectory for 2-D Planar Motion
% Use the trapveltraj function with a given set of 2-D xy waypoints. Time points for the waypoints are also given.

wpts = [0 45 15 90 45; 90 45 -45 15 90];

%
% Compute the trajectory for a given number of samples (501). The function outputs the trajectory positions (q), velocity (qd), acceleration (qdd), time vector (tvec), and polynomial coefficients (pp) of the polynomial that achieves the waypoints using trapezoidal velocities.

[q, qd, qdd, tvec, pp] = trapveltraj(wpts, 501);

%
% Plot the trajectories for the x- and y-positions and the trapezoial velocity profile between each waypoint.

subplot(2,1,1)
plot(tvec, q)
xlabel('t')
ylabel('Positions')
legend('X','Y')
subplot(2,1,2)
plot(tvec, qd)
xlabel('t')
ylabel('Velocities')
legend('X','Y')

%
% You can also verify the actual positions in the 2-D plane. Plot the separate rows of the q vector and the waypoints as x- and y-positions.

figure
plot(q(1,:),q(2,:),'-b',wpts(1,:),wpts(2,:),'or')

% Copyright 2018 The MathWorks, Inc.