%% Track Task-Space Trajectories for a Manipulator using a Trapezoidal Velocity Profile
% This example shows how a robot can trace a task-space trajectory used a defined velocity profile. Trajectory utilities are used to define a reference path in the task-space, and time scaling is used to define how the robot traverses the path in time.
%% Load a UR5 Manipulator
% Load the robot and visualize it.

ur5 = loadrobot('universalUR5','DataFormat',"column");
ax = show(ur5);

%
%% Define the Waypoints
% The robot will follow a trajectory between two waypoints are defined in SE(3), such that each waypoint has both a position and an orientation component.

% Define the two waypoints
wpt1 = trvec2tform([0.4 0.1 0.4]);
wpt2 = trvec2tform([0.4 -0.4 0.1])*axang2tform([0 1 0 -pi/2]);

% Plot the vertices
hold all
plotTransforms(tform2trvec(wpt1), tform2quat(wpt1), 'Parent',ax,"FrameSize",0.1);
plotTransforms(tform2trvec(wpt2), tform2quat(wpt2), 'Parent',ax,"FrameSize",0.1);

%
%% Create a Trajectory that Connects the Waypoints
% Connect the waypoints using a trajectory that interpolates in SE(3).

% Define a time vector
tvec = 0:0.1:2;

% Connect the two poses
[tfInterp1, v1, a1] = transformtraj(wpt1, wpt2, [tvec(1) tvec(end)], tvec);

% By default, this trajectory uses linear interpolation, which means the the intermediate transforms will be uniformly spaced in time. However, more natural trajectories can be created by making use of the time scaling, which changes how the poses are sampled in time.
%% Apply a Trapezoidal Velocity Profile to the Trajectory Time Scaling
% One common trajectory used for manipulators is the trapezoidal velocity profile, in which the trajectory ramps up to a top speed and then ramps back down to zero at each waypoint. The name is derived from the resultant trapezoidal shape of the velocity profile.
% To apply the trapezoidal profile to the interpolation between two transforms, a time scaling is used. With a non-uniform time scaling, instead of defining the trajectory tfInterp1(t) as a function of time, it is instead defined as a function of s(t), where the time scaling s(t) is a function of time defined from zero to one, such that s(min_time) =0 and s(max_time)=1. When the time scaling is linear, s(t) = min_time + t/(max_time - min_time) and the trajectory is simply a linear interpolation.
% To change the trajectory to use a custom time scaling, s(t) and its first two derivatives are defined using the trapveltraj function:

% Define a trapezoidal velocity profile trajectory from 0 to 1 with a
% 2-second duration
numTimeSamples = numel(tvec);
[s, sd, sdd] = trapveltraj([0 1], numTimeSamples,'EndTime',tvec(end));

% The time scaling can subsequently be used to compute a new trajectory by providing it as a matrix of discrete instances defining the values of s, ds/dt, and d^2/dt^2 at each of the numTimeSamples points at which time is sampled.

% Connect the two poses using a trapezoidal velocity profile time scaling
[tfInterp2, v2, a2] = transformtraj(wpt1, wpt2, [tvec(1) tvec(end)], tvec, 'TimeScaling', [s; sd; sdd]);

%% Plot the two trajectories in space
% Plot the trajectories in space using plotTransforms.

% Make a copy of the robot for visualization
ur5copy = copy(ur5);

% Plot the trajectories side-by-side
figure;
ax1 = subplot(1,2,1);
show(ur5, 'Parent', ax1);
hold all;
plotTransforms(tform2trvec(tfInterp1), tform2quat(tfInterp1), 'FrameSize',0.1);
title('Trajectory 1: Linear Time Scaling')
ax2 = subplot(1,2,2);
show(ur5, 'Parent', ax2);
hold all;
plotTransforms(tform2trvec(tfInterp2), tform2quat(tfInterp2), 'FrameSize',0.1);
title('Trajectory 2: Trapezoidal Velocity Profile Time Scaling')


%%
%% Observe the differences between the two trajectories
% In space, the trajectories look quite similar. However, when plotted against time, the differences are clear. To make this point evident, plot just the cartesian position against time for each of the two trajectories.

figure;
subplot(2,1,1);
plot(tvec, reshape(tfInterp1(1:3,4,:),3,numTimeSamples));
hold all
plot(tvec, reshape(tfInterp2(1:3,4,:),3,numTimeSamples),'--');
legend('x1','y1','z1','x2','y2','z2')
title('Trajectory 1 and 2 Position')
xlabel('Time (s)')
ylabel('Position (m)')
subplot(2,1,2);
plot(tvec, v1(4:6,:));
hold all
plot(tvec, v2(4:6,:),'--');
legend('x1dot','y1dot','z1dot','x2dot','y2dot','z2dot')
title('Trajectory 1 and 2 Velocity')
xlabel('Time (s)')
ylabel('Velocity (m/s)')

%
%% Compute the Joint Angles to Execute the Trajectories
% Since these trajectories are defined in the task-space, use an inverse kinematics solver to determine the joint angles needed to execute them and to visualize the results.

% Create an inverse kinematics solver for the robot
ik = inverseKinematics('RigidBodyTree', ur5);

% Initialize the vector of joint angles for each of the two trajectories
jtAngles1 = zeros(6,numTimeSamples);
jtAngles2 = zeros(6,numTimeSamples);

% Set seed for consistency
s = rng(0,'twister');

% Iterate over the sampled poses and compute a corresponding joint angle
% for each trajectory
initGuess1 = zeros(6,1);
initGuess2 = zeros(6,1);
for i = 1:numTimeSamples
    % Compute the joint angles
    jtAngles1(:,i) = ik('tool0', tfInterp1(:,:,i), ones(1,6), initGuess1);
    jtAngles2(:,i) = ik('tool0', tfInterp2(:,:,i), ones(1,6), initGuess2);
    
    % Update the initial guess to the last solution
    initGuess1 = jtAngles1(:,i);
    initGuess2 = jtAngles2(:,i);
end

%
%% Play Back the Trajectories
% Plot the trajectories side-by-side using the figure created earlier to visualize the trajectories.

rcObj = rateControl(20);
for j = 1:numTimeSamples
    show(ur5, jtAngles1(:,j), 'PreservePlot', false, 'Parent', ax1);
    show(ur5, jtAngles2(:,j), 'PreservePlot', false, 'Parent', ax2);
    drawnow;
    waitfor(rcObj);
end

% Copyright 2020 The MathWorks, Inc.