Generate Time-Optimal Trajectories with Constraints Using TOPP-RA Solver

Background

This example solves a TOPP problem, which is a robotics problem in which the goal is to find the fastest way to traverse a path subject to system constraints. In this example, you use the contopptraj function which solves a TOPP problem subject to velocity and acceleration constraints by using a method based on reachability analysis (RA), coined TOPP-RA [1]. Other methods to solve TOPP problems rely on numerical integration (NI) or convex optimization (CO). Applications of TOPP include:

You can use the contopptraj function in these ways:

In this example, you update an existing trajectory that connects five 2-D waypoints. The initial path is interpolated with an initial trajectory based on a quintic polynomial, which provides the shape. You then use the contopptraj function to apply velocity and acceleration limits to the initial path.

Define Waypoints and Initial Trajectory

Both trajectories connect the same set of waypoints, subject to a set of velocity and acceleration limits. Specify the waypoints.

waypts = (deg2rad(30))*[-1 1 2 -1 -1.5; 1 0 -1 -1.1 1];

Different trajectories have different geometric and kinematic attributes, which affects how they navigate a path in space. The Choose Trajectories for Manipulator Paths example illustrates some differences between the various trajectory functions provided in Robotics System Toolbox™.

For this example, use a quintic polynomial trajectory to connect the waypoints. The quintic polynomial connects segments by using smooth velocity and acceleration profiles.

tpts = [0 1 2 3 5];
timeVec = linspace(tpts(1),tpts(end),100);
[q1,qd1,qdd1,pp1] = quinticpolytraj(waypts,tpts,timeVec);

Refine Trajectory Using Constrained TOPP-RA Solver

The quintic polynomial produces an output that, with default parameters, stops at each waypoint. You can use a TOPP-RA solver to compute the fastest possible way to traverse the path while still stopping at each waypoint, given bounded velocity and acceleration. Use the contopptraj function to generate a trajectory that traverses the initial path as quickly as possible while satisfying the specified velocity and acceleration limits.

vellim = [-40 40; -25 25]; 
accellim = [-10 10; -10 10];
[q2,qd2,qdd2,t2] = contopptraj(pp1,vellim,accellim,numsamples=100);

Plot the initial and modified trajectory on the same plot. Notice how the TOPP-RA solver speeds up the trajectory while still satisfying the known constraints.

% Create a figure and adjust the color order so that the second two lines
% have the same color as the associated first two lines
figure
c = colororder("default");
c(3:4,:) = c(1:2,:);
colororder(c)

% Plot results
subplot(3,1,1)
plot(timeVec,q1);
hold on
plot(t2,q2,"--")
legend("Quintic 1","Quintic 2","Constrained 1","Constrained 2")
title("Joint Configuration")
xlim([0 tpts(end)])

subplot(3,1,2)
hold on
plot(timeVec,qd1)
plot(t2,qd2,"--")
title("Joint Velocity")
xlim([0 tpts(end)])

subplot(3,1,3)
hold on
plot(timeVec,qdd1)
plot(t2,qdd2,"--")
title("Joint Acceleration")
xlabel("Time (s)")
xlim([0 tpts(end)])

References