%% Compute B-Spline Trajectory for 2-D Planar Motion
% Use the bsplinepolytraj function with a given set of 2-D xy control points. The B-spline uses these control points to create a trajectory inside the polygon. Time points for the waypoints are also given.

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

% Compute the B-spline trajectory. 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.

tvec = 0:0.01:5;
[q, qd, qdd, pp] = bsplinepolytraj(cpts,tpts,tvec);

% Plot the results. Show the control points and the resulting trajectory inside them.

figure
plot(cpts(1,:),cpts(2,:),'xb-')
hold all
plot(q(1,:), q(2,:))
xlabel('X')
ylabel('Y')
hold off

%
% Plot the position of each element of the B-spline trajectory. These trajectories are cubic piecewise polynomials parameterized in time.

figure
plot(tvec,q)
hold all
plot([0:length(cpts)-1],cpts,'x')
xlabel('t')
ylabel('Position Value')
legend('X-positions','Y-positions')
hold off

% Copyright 2018 The MathWorks, Inc.