%% Generate Trajectories for Rigid Body Tree Manipulator
% This example shows how to generate cubic trajectories for joints in a rigid body tree manipulator. Specific joint configurations are found using inverse kinematics to achieve a desired end-effector position. The cubicpolytraj function then generates cubic

robots = load('exampleRobots')
robot = robots.lbr;
robot.DataFormat = 'row'
show(robot)

%
% To generate the waypoints for the robot, you must know the number of non-fixed bodies in the robot.

numNonFixedBodies = length(homeConfiguration(robot));

%
% Generate a set of waypoints by solving inverse kinematics for specific end-effector points. Specify the waypoints for an end-effector. Solve for joint configurations for the robot

x = 0.5*zeros(1,4)+0.25;
y = 0.25*[-1 -1 1 1];
z = 0.25*[-1 1 -1 1] + 0.75;
tpoints = 0:3;

ik = inverseKinematics('RigidBodyTree',robot)

for i = 1:length(x)
    pose = trvec2tform([x(i),y(i),z(i)]);
    weights = [1,1,1,0.25,0.25,0.25];
    [waypoints(:,i),~] = ik('tool0',pose,weights,zeros(1,numNonFixedBodies));
end

%
% Generate joint trajectory based on these waypoints. Specify a time vector for sampling. Plot the separate trajectories on the same time axis.

tvec = 0:0.01:3;

[q,qd,qdd,pp] = cubicpolytraj(waypoints,tpoints,tvec)
plot(tvec,q)

%
% Plot the robot going through the trajectory generated. This animation will take some time to execute. Watch the figure that is generated to verify the robot configurations are valid.

figure("Visible","on")

for i=1:length(tvec)
    show(robot,q(:,i)');
    drawnow;
end

hold on
plot3(x,y,z,'--r','LineWidth',3)
hold off

%
%
% Copyright 2018 The MathWorks, Inc.