Drake
Drake C++ Documentation
Toppra Class Reference

## Detailed Description

Solves a Time Optimal Path Parameterization based on Reachability Analysis (TOPPRA) to find the fastest traversal of a given path, satisfying the given constraints.

The approach is described in "A new approach to Time-Optimal Path Parameterization based on Reachability Analysis" by Hung Pham and Quang Cuong Pham, IEEE Transactions on Robotics, 2018.

#include <drake/multibody/optimization/toppra.h>

## Public Member Functions

~Toppra ()

Toppra (const Trajectory< double > &path, const MultibodyPlant< double > &plant, const Eigen::Ref< const Eigen::VectorXd > &gridpoints)
Constructs an inverse kinematics problem for a MultibodyPlant. More...

std::optional< PiecewisePolynomial< double > > SolvePathParameterization ()
Solves the TOPPRA optimization and returns the time optimized path parameterization s(t). More...

Binding< BoundingBoxConstraintAddJointVelocityLimit (const Eigen::Ref< const Eigen::VectorXd > &lower_limit, const Eigen::Ref< const Eigen::VectorXd > &upper_limit)
Adds a velocity limit to all the degrees of freedom in the plant. More...

std::pair< Binding< LinearConstraint >, Binding< LinearConstraint > > AddJointAccelerationLimit (const Eigen::Ref< const Eigen::VectorXd > &lower_limit, const Eigen::Ref< const Eigen::VectorXd > &upper_limit, ToppraDiscretization discretization=ToppraDiscretization::kInterpolation)
Adds an acceleration limit to all the degrees of freedom in the plant. More...

std::pair< Binding< LinearConstraint >, Binding< LinearConstraint > > AddJointTorqueLimit (const Eigen::Ref< const Eigen::VectorXd > &lower_limit, const Eigen::Ref< const Eigen::VectorXd > &upper_limit, ToppraDiscretization discretization=ToppraDiscretization::kInterpolation)
Adds a torque limit to all the degrees of freedom in the plant. More...

Binding< BoundingBoxConstraintAddFrameVelocityLimit (const Frame< double > &constraint_frame, const Eigen::Ref< const Vector6d > &lower_limit, const Eigen::Ref< const Vector6d > &upper_limit)
Adds a limit on the elements of the spatial velocity of the given frame, measured and and expressed in the world frame. More...

Binding< BoundingBoxConstraintAddFrameTranslationalSpeedLimit (const Frame< double > &constraint_frame, const double &upper_limit)
Adds a limit on the magnitude of the translational velocity of the given frame, measured and expressed in the world frame. More...

Binding< BoundingBoxConstraintAddFrameTranslationalSpeedLimit (const Frame< double > &constraint_frame, const Trajectory< double > &upper_limit)
A version of the frame translational speed limit that uses a trajectory for the limit. More...

std::pair< Binding< LinearConstraint >, Binding< LinearConstraint > > AddFrameAccelerationLimit (const Frame< double > &constraint_frame, const Eigen::Ref< const Vector6d > &lower_limit, const Eigen::Ref< const Vector6d > &upper_limit, ToppraDiscretization discretization=ToppraDiscretization::kInterpolation)
Adds a limit on the elements of the spatial acceleration of the given frame, measured and expressed in the world frame. More...

std::pair< Binding< LinearConstraint >, Binding< LinearConstraint > > AddFrameAccelerationLimit (const Frame< double > &constraint_frame, const Trajectory< double > &lower_limit, const Trajectory< double > &upper_limit, ToppraDiscretization discretization=ToppraDiscretization::kInterpolation)
A version of acceleration limit that uses a trajectory for the upper and lower limits. More...

Does not allow copy, move, or assignment
Toppra (const Toppra &)=delete

Toppraoperator= (const Toppra &)=delete

Toppra (Toppra &&)=delete

Toppraoperator= (Toppra &&)=delete

## Static Public Member Functions

static Eigen::VectorXd CalcGridPoints (const Trajectory< double > &path, const CalcGridPointsOptions &options)
Takes a path and generates a sequence of gridpoints selected to control the interpolation error of the optimization. More...

## ◆ Toppra() [1/3]

 Toppra ( const Toppra & )
delete

## ◆ Toppra() [2/3]

 Toppra ( Toppra && )
delete

## ◆ ~Toppra()

 ~Toppra ( )

## ◆ Toppra() [3/3]

 Toppra ( const Trajectory< double > & path, const MultibodyPlant< double > & plant, const Eigen::Ref< const Eigen::VectorXd > & gridpoints )

Constructs an inverse kinematics problem for a MultibodyPlant.

This constructor will create and own a context for

Parameters
 plant. path The trajectory on which the TOPPRA problem will be solved. plant The robot that will follow the solved trajectory. Used for enforcing torque and frame specific constraints. gridpoints The points along the path to discretize the problem and enforce constraints at. The first and last gridpoint must equal the path start and end time respectively. Gridpoints must also be monotonically increasing.
Note
Toppra does not currently support plants that contain bodies with quaternion degrees of freedom. In addition, any plant where q̇ ≠ v will have undefined behavior.
The path velocity, ṡ(t), is limited to be between 0 and 1e8 to ensure the reachable set calculated in the backward pass is always bounded.

## Member Function Documentation

 std::pair, Binding > AddFrameAccelerationLimit ( const Frame< double > & constraint_frame, const Eigen::Ref< const Vector6d > & lower_limit, const Eigen::Ref< const Vector6d > & upper_limit, ToppraDiscretization discretization = ToppraDiscretization::kInterpolation )

Adds a limit on the elements of the spatial acceleration of the given frame, measured and expressed in the world frame.

The limits should be given as [α_WF, a_WF], where α_WF is the frame's angular acceleration and a_WF is the frame's translational acceleration.

Parameters
 constraint_frame The frame to limit the acceleration of. lower_limit The lower acceleration limit for constraint_frame. upper_limit The upper acceleration limit for constraint_frame. discretization The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.
Returns
A pair containing the linear constraints that will enforce the frame acceleration limit on the backward pass and forward pass respectively.

 std::pair, Binding > AddFrameAccelerationLimit ( const Frame< double > & constraint_frame, const Trajectory< double > & lower_limit, const Trajectory< double > & upper_limit, ToppraDiscretization discretization = ToppraDiscretization::kInterpolation )

A version of acceleration limit that uses a trajectory for the upper and lower limits.

Parameters
 constraint_frame The frame to limit the acceleration of. lower_limit The lower acceleration limit trajectory for constraint_frame. upper_limit The upper acceleration limit trajectory for constraint_frame. discretization The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.
Returns
A pair containing the linear constraints that will enforce the frame acceleration limit on the backward pass and forward pass respectively.
Precondition
Both lower_limit and upper_limit trajectories must have values in ℜ⁶. The six-dimensional column vector is interpreted as [α_WF, a_WF], where α_WF is the frame's angular acceleration and a_WF is the frame's translational acceleration.
Exceptions
 If the intervals [upper_limit.start_time(), upper_limit.end_time()] and [lower_limit.start_time(), lower_limit.end_time()] don't overlap with [path.start_time(), path.end_time()].
Note
The constraints are only added in the constraint trajectories domains (where they overlap the path). The rest of the path is not constrained.

 Binding AddFrameTranslationalSpeedLimit ( const Frame< double > & constraint_frame, const double & upper_limit )

Adds a limit on the magnitude of the translational velocity of the given frame, measured and expressed in the world frame.

Parameters
 constraint_frame The frame to limit the translational speed of. upper_limit The upper translational speed limit for constraint_frame.
Returns
The bounding box constraint that will enforce the frame translational speed limit during the backward pass.

 Binding AddFrameTranslationalSpeedLimit ( const Frame< double > & constraint_frame, const Trajectory< double > & upper_limit )

A version of the frame translational speed limit that uses a trajectory for the limit.

Parameters
 constraint_frame The frame to limit the translational speed of. upper_limit The upper translational speed limit trajectory for constraint_frame.
Precondition
upper_limit must have scalar values (a 1x1 matrix).
Returns
The bounding box constraint that will enforce the frame translational speed limit during the backward pass.
Exceptions
 If the interval [upper_limit.start_time(), upper_limit.end_time()] doesn't overlap with [path.start_time(), path.end_time()].
Note
The constraints are only added between upper_limit.start_time() and upper_limit.end_time(). The rest of the path is not constrained.

 Binding AddFrameVelocityLimit ( const Frame< double > & constraint_frame, const Eigen::Ref< const Vector6d > & lower_limit, const Eigen::Ref< const Vector6d > & upper_limit )

Adds a limit on the elements of the spatial velocity of the given frame, measured and and expressed in the world frame.

The limits should be given as [ω_WF, v_WF], where ω_WF is the frame's angular velocity and v_WF is the frame's translational velocity.

Parameters
 constraint_frame The frame to limit the velocity of. lower_limit The lower velocity limit for constraint_frame. upper_limit The upper velocity limit for constraint_frame.
Returns
The bounding box constraint that will enforce the frame velocity limit during the backward pass.

 std::pair, Binding > AddJointAccelerationLimit ( const Eigen::Ref< const Eigen::VectorXd > & lower_limit, const Eigen::Ref< const Eigen::VectorXd > & upper_limit, ToppraDiscretization discretization = ToppraDiscretization::kInterpolation )

Adds an acceleration limit to all the degrees of freedom in the plant.

The limits must be arranged in the same order as the entries in the path.

Parameters
 lower_limit The lower acceleration limit for each degree of freedom. upper_limit The upper acceleration limit for each degree of freedom. discretization The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.
Returns
A pair containing the linear constraints that will enforce the acceleration limit on the backward pass and forward pass respectively.

 std::pair, Binding > AddJointTorqueLimit ( const Eigen::Ref< const Eigen::VectorXd > & lower_limit, const Eigen::Ref< const Eigen::VectorXd > & upper_limit, ToppraDiscretization discretization = ToppraDiscretization::kInterpolation )

Adds a torque limit to all the degrees of freedom in the plant.

The limits must be arranged in the same order as the entries in the path. This constrains the generalized torques applied to the plant and does not reason about contact forces.

Parameters
 lower_limit The lower torque limit for each degree of freedom. upper_limit The upper torque limit for each degree of freedom. discretization The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.
Returns
A pair containing the linear constraints that will enforce the torque limit on the backward pass and forward pass respectively.

 Binding AddJointVelocityLimit ( const Eigen::Ref< const Eigen::VectorXd > & lower_limit, const Eigen::Ref< const Eigen::VectorXd > & upper_limit )

Adds a velocity limit to all the degrees of freedom in the plant.

The limits must be arranged in the same order as the entries in the path.

Parameters
 lower_limit The lower velocity limit for each degree of freedom. upper_limit The upper velocity limit for each degree of freedom.

## ◆ CalcGridPoints()

 static Eigen::VectorXd CalcGridPoints ( const Trajectory< double > & path, const CalcGridPointsOptions & options )
static

Takes a path and generates a sequence of gridpoints selected to control the interpolation error of the optimization.

The gridpoints are selected such that the distance between them is below max_seg_length, there are at least min_points number of gridpoints and the interpolation error, estimated with the equation

errₑₛₜ = max(|q̈ Δₛ²|) / 2

where Δₛ is the distance between sequential gridpoints, is less than max_err. Gridpoints are selected by adding the midpoint between two gridpoints whenever the distance between them is too large or the estimated error is too high. This results in more points in parts of the path with higher curvature. All grid points will lie between path.start_time() and path.end_time().

## ◆ operator=() [1/2]

 Toppra& operator= ( Toppra && )
delete

## ◆ operator=() [2/2]

 Toppra& operator= ( const Toppra & )
delete

## ◆ SolvePathParameterization()

 std::optional > SolvePathParameterization ( )

Solves the TOPPRA optimization and returns the time optimized path parameterization s(t).

This can be used with the original path q(s) to generate a time parameterized trajectory. The path parameterization has the same start time as the original path's starting break.

The documentation for this class was generated from the following file: