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".

robot = loadrobot("kukaIiwa14","DataFormat","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