contopptraj

Generate trajectory subject to kinematic constraints

Since R2022b

Syntax

[q,qd,qdd,t] = contopptraj(waypoints,vellim,accellim)

[___] = contopptraj(polypath,vellim,accellim)

[___] = contopptraj(___,NumSamples=N)

[___,solninfo] = contopptraj(___)

Description

Note

The contopptraj function requires Optimization Toolbox™.

[q,qd,qdd,t] = contopptraj(waypoints,vellim,accellim) generates a trajectory by fitting a path to a set of waypoints waypoints. The function returns a time-optimal trajectory along the path for position q, velocity qd, and acceleration qdd at sample times t, while constrained by the velocity vellim and acceleration limits accellim.

[___] = contopptraj(polypath,vellim,accellim) generates a trajectory along the specified polynomial path polypath using all output arguments from the previous syntax.

[___] = contopptraj(___,NumSamples=N) specifies the number of samples to use when generating the trajectory, in addition to any combination of arguments from previous syntaxes.

[___,solninfo] = contopptraj(___) outputs solution information solninfo with diagnostic information associated with the output trajectory, in addition to any combination of arguments from previous syntaxes.

Examples

Create Kinematically Constrained Trajectory

Create waypoints, velocity limits, and acceleration limits.

waypoints = [0 5 10 -10; 0 8 -10 5; 0 -10 15 5];
velLimits = [-1 1; -2 2; -3 3];
accelLimits = [-1 1; -2 2; -3 3];

Generate the position, velocity, acceleration, and time vector of the trajectory with 200 samples.

[q,qd,qdd,t] = contopptraj(waypoints,velLimits,accelLimits,NumSamples=200);

Plot the positions, velocities, and accelerations against the sample times t.

figure
title("Position vs Sample Time")
ylim("padded")
hold on
q = real(q);
qd = real(qd);
qdd = real(qdd);
t = real(t);
plot(t,q(1,:)) % X Position
plot(t,q(2,:)) % Y Position
plot(t,q(3,:)) % Z Position
xlabel("Sample Time (s)")
ylabel("Position (m)")
legend(["X","Y","Z"])
hold off

helperPlotConstrainedTrajectory(qd,t,velLimits,"Velocity")
ylabel("Velocity (m/s)")

helperPlotConstrainedTrajectory(qdd,t,accelLimits,"Acceleration")
ylabel("Acceleration (m/s^2)")

Input Arguments

waypoints — Trajectory waypoints for path fitting

n-by-p matrix

Trajectory waypoints for path fitting, specified as an n-by-p matrix. n is the number of elements in the state space, and p is the number of distinct waypoints.

Example: [1 1 2 3; 2 5 4 5; 5 7 6 8] is a set of four waypoints with three state space elements each.

vellim — Minimum and maximum velocity limits of trajectory

n-by-2 matrix

Minimum and maximum velocity limits of the trajectory, specified as an n-by-2 matrix, in units per second. n is the number of elements in the state space, and each row is of the form [minimumLimit maximumLimit], specifying the minimum and maximum velocity for each state space element. To generate a trajectory without velocity limits, specify this argument as an empty array.

Example: [-1 1; -2 5; -5 8] is a set of velocity limits for three state space elements.

accellim — Minimum and maximum acceleration limits of trajectory

n-by-2 matrix

Minimum and maximum acceleration limits of the trajectory, specified as an n-by-2 matrix, in units per seconds squared. n is the number of elements in the state space, and each row is of the form [minimumLimit maximumLimit], specifying the minimum and maximum velocity for each state space element. To generate a trajectory without acceleration limits, specify this argument as an empty array.

Example: [-1 1; -2 5; -5 8] is a set of acceleration limits for three state space elements.

polypath — Piecewise polynomial path for trajectory generation

structure

Piecewise polynomial path for trajectory generation, specified as a structure such as the output returned from the mkpp or spline functions. For more information on the fields of this structure, see the pp argument of the mkpp the function.

Data Types: struct

N — Number of samples for trajectory generation

100 (default) | positive integer

Number of samples for trajectory generation, specified as a positive integer.

Example: NumSamples=200

Output Arguments

q — Trajectory positions

n-by-m matrix

Trajectory positions, returned as a n-by-m matrix. n is the number of elements in the state space, and m is the number of samples in the trajectory.

qd — Trajectory velocities

n-by-m matrix

Trajectory velocities, returned as a n-by-m matrix. n is the number of elements in the state space, and m is the number of samples in the trajectory.

qdd — Trajectory accelerations

n-by-m matrix

Trajectory accelerations, returned as a n-by-m matrix. n is the number of elements in the state space, and m is the number of samples in the trajectory.

t — Sample times

m-element vector

Sample times, returned as a m-element vector, in seconds. m is the number of samples in the trajectory.

solninfo — Solution information

structure

Solution information, specified as a structure containing these fields:

• ExitFlag — Number indicating whether contopptraj could find a solution. A value of 1 indicates the function successfully found a solution, and a value of 0 indicates that the function was not able to find a feasible solution.

References

[1] Pham, Hung, and Quang-Cuong Pham. “A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis.” IEEE Transactions on Robotics, 34, no. 3 (June 2018): 645–59. https://doi.org/10.1109/TRO.2018.2819195.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022b