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 (double s_dot_start=0, double s_dot_end=0)
	Solves the TOPPRA optimization and returns the time optimized path parameterization s(t). More...

Binding< BoundingBoxConstraint >	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. 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< BoundingBoxConstraint >	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. More...

Binding< BoundingBoxConstraint >	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. More...

Binding< BoundingBoxConstraint >	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. 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

Toppra &	operator= (const Toppra &)=delete

	Toppra (Toppra &&)=delete

Toppra &	operator= (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...

Constructor & Destructor Documentation

◆ 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

◆ AddFrameAccelerationLimit() [1/2]

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.

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.

◆ AddFrameAccelerationLimit() [2/2]

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.

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.

◆ AddFrameTranslationalSpeedLimit() [1/2]

Binding<BoundingBoxConstraint> 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.

◆ AddFrameTranslationalSpeedLimit() [2/2]

Binding<BoundingBoxConstraint> 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.

◆ AddFrameVelocityLimit()

Binding<BoundingBoxConstraint> 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.

◆ AddJointAccelerationLimit()

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.

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.

◆ AddJointTorqueLimit()

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.

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.

◆ AddJointVelocityLimit()

Binding<BoundingBoxConstraint> 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<PiecewisePolynomial<double> > SolvePathParameterization	(	double	s_dot_start = `0`,
		double	s_dot_end = `0`
	)

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.

Parameters

s_dot_start	ṡ(0). The default value is zero (trajectory starts at zero velocity).
s_dot_end	ṡ(T), where T is the end break of the path. The default value is zero (trajectory ends at zero velocity).

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

drake/multibody/optimization/toppra.h

Detailed Description

Public Member Functions

Static Public Member Functions

Constructor & Destructor Documentation

◆ Toppra() [1/3]

◆ Toppra() [2/3]

◆ ~Toppra()

◆ Toppra() [3/3]

Member Function Documentation

◆ AddFrameAccelerationLimit() [1/2]

◆ AddFrameAccelerationLimit() [2/2]

◆ AddFrameTranslationalSpeedLimit() [1/2]

◆ AddFrameTranslationalSpeedLimit() [2/2]

◆ AddFrameVelocityLimit()

◆ AddJointAccelerationLimit()

◆ AddJointTorqueLimit()

◆ AddJointVelocityLimit()

◆ CalcGridPoints()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ SolvePathParameterization()