# Plan Minimum Jerk Trajectory for Robot Arm

This example shows how to plan a minimum jerk polynomial trajectory for a robotic manipulator. The example shows how to load an included robot model, plan a path for the robot model in an environment with obstacles, generate a minimum jerk trajectory from the path, and visualize the generated trajectories and the robot motion.

### Set Up Robot Model and Environment

This example uses a model of the KUKA LBR iiwa, a 7 degree-of-freedom robot manipulator. Use loadrobot to load the robot model into the workspace as a rigidBodyTree object. Set the output format for configurations to "row".

Generate the environment for the robot. Create collision objects and specify their poses relative to the robot base. Visualize the environment.

env = {collisionBox(0.5,0.5,0.05) collisionSphere(0.3)};
env{1}.Pose(3,end) = -0.05;
env{2}.Pose(1:3,end) = [0.1 0.2 0.8];

show(robot);
hold on
show(env{1})
show(env{2})

### Plan Path Using manipulatorRRT

Create the RRT planner for the robot model using manipulatorRRT. Set the ValidationDistance property to increase the number of intermediate states while interpolating the path.

rrt = manipulatorRRT(robot,env);
rrt.ValidationDistance = 0.2;
rrt.SkippedSelfCollisions = "parent";

Specify a start and a goal configuration.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Plan the path. Due to the randomness of the RRT algorithm, set the rng seed for repeatability.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Interpolate the path and retrieve the waypoints.

interpPath = interpolate(rrt,path);
wpts = interpPath';

### Generate Minimum Jerk Polynomial Trajectory

The planner returns a path as an ordered set of waypoints. To pass these to a robot, you must first determine a trajectory through them. The minjerkpolytraj function creates a smooth trajectory with minimum jerk that hits all the specified waypoints.

Provide an initial guess for the times at which the robot arm arrives at the waypoints.

initialGuess = linspace(0,size(wpts,2)*0.2,size(wpts,2));

Specify the number of samples to take while estimating the trajectory.

numSamples = 100;

Compute the minimum jerk polynomial trajectory.

[q,qd,qdd,qddd,pp,tpts,tSamples] = minjerkpolytraj(wpts,initialGuess,numSamples);

### Visualize Trajectories and Waypoints

Plot the trajectories and the waypoints over time.

minJerkPath = q';
figure
plot(tSamples,q)
hold all
plot(tpts,wpts,"x")

### Visualize Robot Motion

Use the show object function to animate the resulting motion. This visualization enables fast updates to ensure a smooth animation.

figure;
ax = show(robot,startConfig);
hold all

% Ensure the figure pops out of the Live Editor so animations are visible
set(gcf,"Visible","on");
for i = 1:length(env)
show(env{i},"Parent",ax);
end

for i = 1:size(minJerkPath,1)
show(robot,minJerkPath(i,:),"PreservePlot",false,"FastUpdate",true);
drawnow;
end

hold off