Main Content

contopptraj

Generate trajectory subject to kinematic constraints

Since R2022b

collapse all in page

    Syntax

    [q,qd,qdd,t] = contopptraj(waypoints,vellim,accellim)
    [___] = contopptraj(polypath,vellim,accellim)
    [___] = contopptraj(___,NumSamples=N)
    [___,solninfo] = contopptraj(___)

    Description

    [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. This function requires Optimization Toolbox™.

    example

    [___] = 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

    collapse all

    Open Live Script

    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

    Figure contains an axes object. The axes object with title Position vs Sample Time, xlabel Sample Time (s), ylabel Position (m) contains 3 objects of type line. These objects represent X, Y, Z.

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

    Figure contains an axes object. The axes object with title Velocity vs Sample Time, xlabel Sample Time (s), ylabel Velocity (m/s) contains 9 objects of type line, constantline. These objects represent X, Y, Z, X Limits, Y Limits, Z Limits.

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

    Figure contains an axes object. The axes object with title Acceleration vs Sample Time, xlabel Sample Time (s), ylabel Acceleration (m/s Squared baseline ) contains 9 objects of type line, constantline. These objects represent X, Y, Z, X Limits, Y Limits, Z Limits.

    Input Arguments

    collapse all

    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.

    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.

    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.

    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

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

    Example: NumSamples=200

    Output Arguments

    collapse all

    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.

    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.

    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.

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

    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

    See Also

    Functions

    Topics