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

wpts = [1 4 4 3 -2 0; 0 1 2 4 3 1];
tpts = 0:5;

%
% Specify a time vector for sampling the trajectory. Sample at a smaller interval than the specified time points.

tvec = 0:0.01:5;

%
% Compute the cubic trajectory. The function outputs the trajectory positions (q), velocity (qd), acceleration (qdd), and polynomial coefficients (pp) of the cubic polynomial.

[q, qd, qdd, pp] = cubicpolytraj(wpts, tpts, tvec);

%
% Plot the cubic trajectories for the x- and y-positions. Compare the trajectory with each waypoint.

plot(tvec, q)
hold all
plot(tpts, wpts, 'x')
xlabel('t')
ylabel('Positions')
legend('X-positions','Y-positions')
hold off

%
% 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')
xlabel('X')
ylabel('Y')

% Copyright 2018 The MathWorks, Inc.