Detailed Description

Optimizes a trajectory, q(t) subject to costs and constraints on the trajectory and its derivatives.

This is accomplished using a path, r(s), represented as a BsplineTrajectory on the interval s∈[0,1], and a separate duration, T, which maps [0,1] => [0,T].

The q(t) trajectory is commonly associated with, for instance, the generalized positions of a MultibodyPlant by adding multibody costs and constraints; in this case take note that the velocities in this optimization are q̇(t), not v(t).

Use solvers::Solve to solve the problem. A typical use case could look like:

  KinematicTrajectoryOptimization trajopt(2, 10);
  // add costs and constraints
  trajopt.SetInitialGuess(...);
  auto result = solvers::Solve(trajopt.prog());
  auto traj = trajopt.ReconstructTrajectory(result);

When possible this class attempts to formulate convex forms of the costs and constraints.

#include <drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h>

Public Member Functions
	KinematicTrajectoryOptimization (int num_positions, int num_control_points, int spline_order=4, double duration=1.0)
	Constructs an optimization problem for a position trajectory represented as a B-spline. More...

	KinematicTrajectoryOptimization (const trajectories::BsplineTrajectory< double > &trajectory)
	Constructs an optimization problem for a trajectory represented by a B-spline with the same order and number of control points as `trajectory`. More...

int	num_positions () const
	Returns the number of position variables. More...

int	num_control_points () const
	Returns the number of control points used for the path. More...

const math::BsplineBasis< double > &	basis () const
	Returns the basis used to represent the path, r(s), over s∈[0,1]. More...

const solvers::MatrixXDecisionVariable &	control_points () const
	Returns the control points defining the path as an M-by-N matrix, where M is the number of positions and N is the number of control points. More...

const symbolic::Variable &	duration () const
	Returns the decision variable defining the time duration of the trajectory. More...

const solvers::MathematicalProgram &	prog () const
	Getter for the optimization program. More...

solvers::MathematicalProgram &	get_mutable_prog ()
	Getter for a mutable pointer to the optimization program. More...

void	SetInitialGuess (const trajectories::BsplineTrajectory< double > &trajectory)
	Sets the initial guess for the path and duration to match `trajectory`. More...

trajectories::BsplineTrajectory< double >	ReconstructTrajectory (const solvers::MathematicalProgramResult &result) const
	Returns the trajectory q(t) from the `result` of solving `prog()`. More...

solvers::Binding< solvers::LinearConstraint >	AddPathPositionConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, double s)
	Adds a linear constraint on the value of the path, `lb` ≤ r(s) ≤ `ub`. More...

solvers::Binding< solvers::Constraint >	AddPathPositionConstraint (const std::shared_ptr< solvers::Constraint > &constraint, double s)
	Adds a (generic) constraint on path. More...

solvers::Binding< solvers::LinearConstraint >	AddPathVelocityConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, double s)
	Adds a linear constraint on the derivative of the path, `lb` ≤ ṙ(s) ≤ `ub`. More...

solvers::Binding< solvers::Constraint >	AddVelocityConstraintAtNormalizedTime (const std::shared_ptr< solvers::Constraint > &constraint, double s)
	Adds a (generic) constraint on trajectory velocity `q̇(t)`, evaluated at `s`. More...

std::vector< solvers::Binding< solvers::LinearConstraint > >	AddVelocityConstraintAtNormalizedTime (const solvers::Binding< solvers::LinearConstraint > &binding, double s)
	Adds a linear constraint on some (or all) of the placeholder variables qdot, evaluated at a normalized time s. More...

solvers::Binding< solvers::LinearConstraint >	AddPathAccelerationConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, double s)
	Adds a linear constraint on the second derivative of the path, `lb` ≤ r̈(s) ≤ `ub`. More...

solvers::Binding< solvers::BoundingBoxConstraint >	AddDurationConstraint (std::optional< double > lb, std::optional< double > ub)
	Adds bounding box constraints for upper and lower bounds on the duration of the trajectory. More...

std::vector< solvers::Binding< solvers::BoundingBoxConstraint > >	AddPositionBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
	Adds bounding box constraints to enforce upper and lower bounds on the positions trajectory, q(t). More...

std::vector< solvers::Binding< solvers::LinearConstraint > >	AddVelocityBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
	Adds linear constraints to enforce upper and lower bounds on the velocity trajectory, q̇(t). More...

std::vector< solvers::Binding< solvers::Constraint > >	AddAccelerationBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
	Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the acceleration trajectory, q̈(t). More...

std::vector< solvers::Binding< solvers::Constraint > >	AddJerkBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
	Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the jerk trajectory, d³qdt³(t). More...

std::vector< solvers::Binding< solvers::Constraint > >	AddEffortBoundsAtNormalizedTimes (const multibody::MultibodyPlant< double > &plant, const Eigen::Ref< const Eigen::VectorXd > &s, const std::optional< Eigen::Ref< const Eigen::VectorXd >> &lb=std::nullopt, const std::optional< Eigen::Ref< const Eigen::VectorXd >> &ub=std::nullopt, const systems::Context< double > *plant_context=nullptr)
	Adds generic (nonlinear) constraints to enforce the effort limits defined in the plant at a sequence of normalized times, `s`: More...

solvers::Binding< solvers::LinearCost >	AddDurationCost (double weight=1.0)
	Adds a linear cost on the duration of the trajectory. More...

std::vector< solvers::Binding< solvers::Cost > >	AddPathLengthCost (double weight=1.0, bool use_conic_constraint=false)
	Adds a cost on an upper bound of the length of the path, ∫₀ᵀ \|q̇(t)\|₂ dt, or equivalently ∫₀¹ \|ṙ(s)\|₂ ds, by summing the distance between the path control points. More...

std::vector< solvers::Binding< solvers::Cost > >	AddPathEnergyCost (double weight=1.0)
	Adds a convex quadratic cost on an upper bound on the energy of the path, ∫₀¹ \|ṙ(s)\|₂² ds, by summing the squared distance between the path control points. More...

const VectorX< symbolic::Variable > &	q () const
	Returns the placeholder variable for generalized position q. More...

const VectorX< symbolic::Variable > &	qdot () const
	Returns the placeholder variable for the time derivative of generalized position. More...

const VectorX< symbolic::Variable > &	qddot () const
	Returns the placeholder variable for the second time derivative of generalized position. More...

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

KinematicTrajectoryOptimization &	operator= (const KinematicTrajectoryOptimization &)=delete

	KinematicTrajectoryOptimization (KinematicTrajectoryOptimization &&)=delete

KinematicTrajectoryOptimization &	operator= (KinematicTrajectoryOptimization &&)=delete

Constructor & Destructor Documentation

◆ KinematicTrajectoryOptimization() [1/4]

KinematicTrajectoryOptimization ( const KinematicTrajectoryOptimization & )

delete

◆ KinematicTrajectoryOptimization() [2/4]

KinematicTrajectoryOptimization ( KinematicTrajectoryOptimization && )

delete

◆ KinematicTrajectoryOptimization() [3/4]

KinematicTrajectoryOptimization	(	int	num_positions,
		int	num_control_points,
		int	spline_order = `4`,
		double	duration = `1.0`
	)

Constructs an optimization problem for a position trajectory represented as a B-spline.

The initial guess is the zero trajectory over the time interval [0, T].

Parameters

num_positions	The number of rows in the B-spline.
num_control_points	The number of B-spline control points.
spline_order	The order of the B-spline.
duration	The duration (in seconds) of the initial guess.

◆ KinematicTrajectoryOptimization() [4/4]

KinematicTrajectoryOptimization ( const trajectories::BsplineTrajectory< double > & trajectory )

explicit

Constructs an optimization problem for a trajectory represented by a B-spline with the same order and number of control points as trajectory.

Additionally sets trajectory as the initial guess for the optimization.

Member Function Documentation

◆ AddAccelerationBounds()

std::vector<solvers::Binding<solvers::Constraint> > AddAccelerationBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub
	)

Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the acceleration trajectory, q̈(t).

By leveraging the convex hull property of B-splines, these bounds are applied at the (derivative) control points, but will be respected for all times, t ∈ [0,T]. Note that this does NOT directly constrain r̈(s).

Returns: A vector of bindings with interleaved lower and then upper bounds, constraining all of the control points for one element of q̈ (e.g. the acceleration of the ith joint at all times). However, this specific interpretation of these constraints may change without deprecation.

◆ AddDurationConstraint()

solvers::Binding<solvers::BoundingBoxConstraint> AddDurationConstraint	(	std::optional< double >	lb,
		std::optional< double >	ub
	)

Adds bounding box constraints for upper and lower bounds on the duration of the trajectory.

◆ AddDurationCost()

solvers::Binding<solvers::LinearCost> AddDurationCost ( double weight = 1.0 )

Adds a linear cost on the duration of the trajectory.

◆ AddEffortBoundsAtNormalizedTimes()

std::vector<solvers::Binding<solvers::Constraint> > AddEffortBoundsAtNormalizedTimes	(	const multibody::MultibodyPlant< double > &	plant,
		const Eigen::Ref< const Eigen::VectorXd > &	s,
		const std::optional< Eigen::Ref< const Eigen::VectorXd >> &	lb = `std::nullopt`,
		const std::optional< Eigen::Ref< const Eigen::VectorXd >> &	ub = `std::nullopt`,
		const systems::Context< double > *	plant_context = `nullptr`
	)

Adds generic (nonlinear) constraints to enforce the effort limits defined in the plant at a sequence of normalized times, s:

B lb ≤ M(q)v̇ + C(q, v)v - τ_g(q) - τ_app ≤ B ub

where q, v, and v̇ are evaluated at s. B is the plant's actuation matrix, and M, C, τ_g, and τ_app are the plant's mass matrix, Coriolis force, gravity, and applied force, respectively. ub and lb are the upper and lower effort bounds, respectively; if they are not provided then plant.GetEffortLowerLimits() and plant.GetEffortUpperLimits() are used.

Pass plant_context if you have non-default parameters in the context. Note that there are no lifetime requirements on plant nor plant_context.

Note that the convex hull property of the B-splines is not guaranteed to hold here – effort limits maybe be violated away from the normalized times s.

Precondition: plant.is_finalized(); plant.num_positions() == num_positions(); plant.IsVelocityEqualToQDot() == true; s[i] ∈ [0, 1] for all i; B lb ≤ B ub

Returns: A vector of bindings with one effort limit constraint for each s.

◆ AddJerkBounds()

std::vector<solvers::Binding<solvers::Constraint> > AddJerkBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub
	)

Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the jerk trajectory, d³qdt³(t).

By leveraging the convex hull property of B-splines, these bounds are applied at the (derivative) control points, but will be respected for all times, t ∈ [0,T]. Note that this does NOT directly constrain d³rds³(s).

Returns: A vector of bindings with interleaved lower and then upper bounds, constraining all of the control points for one element of d³qdt³ (e.g. the jerk of the ith joint at all times). However, this specific interpretation of these constraints may change without deprecation.

◆ AddPathAccelerationConstraint()

solvers::Binding<solvers::LinearConstraint> AddPathAccelerationConstraint	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub,
		double	s
	)

Adds a linear constraint on the second derivative of the path, lb ≤ r̈(s) ≤ ub.

Note that this does NOT directly constrain q̈(t).

Precondition: 0 <= s <= 1.

◆ AddPathEnergyCost()

std::vector<solvers::Binding<solvers::Cost> > AddPathEnergyCost ( double weight = 1.0 )

Adds a convex quadratic cost on an upper bound on the energy of the path, ∫₀¹ |ṙ(s)|₂² ds, by summing the squared distance between the path control points.

In the limit of infinitely many control points, minimizers for AddPathLengthCost and AddPathEnergyCost will follow the same path, but potentially with different timing. They may have different values if additional costs and constraints are imposed. This cost yields simpler gradients than AddPathLengthCost, and biases the control points towards being evenly spaced.

Returns: A vector of bindings with the ith element adding a cost to the ith control point of the velocity trajectory. However, this specific interpretation of these constraints may change without deprecation.

◆ AddPathLengthCost()

std::vector<solvers::Binding<solvers::Cost> > AddPathLengthCost	(	double	weight = `1.0`,
		bool	use_conic_constraint = `false`
	)

Adds a cost on an upper bound of the length of the path, ∫₀ᵀ |q̇(t)|₂ dt, or equivalently ∫₀¹ |ṙ(s)|₂ ds, by summing the distance between the path control points.

If use_conic_constraint = false, then costs are added via MathematicalProgram::AddL2NormCost; otherwise they are added via MathematicalProgram::AddL2NormCostUsingConicConstraint.

Returns: A vector of bindings with the ith element adding a cost to the ith control point of the velocity trajectory. However, this specific interpretation of these constraints may change without deprecation.

◆ AddPathPositionConstraint() [1/2]

solvers::Binding<solvers::LinearConstraint> AddPathPositionConstraint	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub,
		double	s
	)

Adds a linear constraint on the value of the path, lb ≤ r(s) ≤ ub.

Precondition: 0 <= s <= 1.

◆ AddPathPositionConstraint() [2/2]

solvers::Binding<solvers::Constraint> AddPathPositionConstraint	(	const std::shared_ptr< solvers::Constraint > &	constraint,
		double	s
	)

Adds a (generic) constraint on path.

The constraint will be evaluated as if it is bound with variables corresponding to r(s).

Precondition: constraint.num_vars() == num_positions(); 0 <= s <= 1.

◆ AddPathVelocityConstraint()

solvers::Binding<solvers::LinearConstraint> AddPathVelocityConstraint	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub,
		double	s
	)

Adds a linear constraint on the derivative of the path, lb ≤ ṙ(s) ≤ ub.

Note that this does NOT directly constrain q̇(t).

Precondition: 0 <= s <= 1.

◆ AddPositionBounds()

std::vector<solvers::Binding<solvers::BoundingBoxConstraint> > AddPositionBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub
	)

Adds bounding box constraints to enforce upper and lower bounds on the positions trajectory, q(t).

These bounds will be respected at all times, t∈[0,T]. This also implies the constraints are satisfied for r(s), for all s∈[0,1].

Returns: A vector of bindings with the ith element adding a constraint to the ith control point. However, this specific interpretation of these constraints may change without deprecation.

◆ AddVelocityBounds()

std::vector<solvers::Binding<solvers::LinearConstraint> > AddVelocityBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub
	)

Adds linear constraints to enforce upper and lower bounds on the velocity trajectory, q̇(t).

By leveraging the convex hull property of B-splines, these bounds are applied at the (derivative) control points, but will be respected for all times, t ∈ [0,T]. Note this does NOT directly constrain ṙ(s).

Returns: A vector of bindings with interleaved lower and then upper bounds, constraining all of the control points for one element of q̇ (e.g. the velocity of the ith joint at all times). However, this specific interpretation of these constraints may change without deprecation..

◆ AddVelocityConstraintAtNormalizedTime() [1/2]

solvers::Binding<solvers::Constraint> AddVelocityConstraintAtNormalizedTime	(	const std::shared_ptr< solvers::Constraint > &	constraint,
		double	s
	)

Adds a (generic) constraint on trajectory velocity q̇(t), evaluated at s.

The constraint will be evaluated as if it is bound with variables corresponding to [q(T*s), q̇(T*s)].

This is a potentially confusing mix of s and t, but it is important in practice. For instance if you want to constrain the true (trajectory) velocity at the final time, one would naturally want to write AddVelocityConstraint(constraint, s=1).

This method should be compared with AddPathVelocityConstraint, which only constrains ṙ(s) because it does not reason about the time scaling, T. However, AddPathVelocityConstraint adds convex constraints, whereas this method adds nonconvex generic constraints.

Precondition: constraint.num_vars() == num_positions(); 0 <= s <= 1.

◆ AddVelocityConstraintAtNormalizedTime() [2/2]

std::vector<solvers::Binding<solvers::LinearConstraint> > AddVelocityConstraintAtNormalizedTime	(	const solvers::Binding< solvers::LinearConstraint > &	binding,
		double	s
	)

Adds a linear constraint on some (or all) of the placeholder variables qdot, evaluated at a normalized time s.

Precondition: binding Can only associate with qdot().; 0 <= s <= 1.

Binding<LinearConstraint> b(LinearConstraint(A, b), trajopt.qdot());

trajopt.AddVelocityConstraintAtNormalizedTime(b, 0);

◆ basis()

const math::BsplineBasis<double>& basis ( ) const

Returns the basis used to represent the path, r(s), over s∈[0,1].

◆ control_points()

const solvers::MatrixXDecisionVariable& control_points ( ) const

Returns the control points defining the path as an M-by-N matrix, where M is the number of positions and N is the number of control points.

◆ duration()

const symbolic::Variable& duration ( ) const

Returns the decision variable defining the time duration of the trajectory.

◆ get_mutable_prog()

solvers::MathematicalProgram& get_mutable_prog ( )

Getter for a mutable pointer to the optimization program.

◆ num_control_points()

int num_control_points ( ) const

Returns the number of control points used for the path.

◆ num_positions()

int num_positions ( ) const

Returns the number of position variables.

◆ operator=() [1/2]

KinematicTrajectoryOptimization& operator= ( KinematicTrajectoryOptimization && )

delete

◆ operator=() [2/2]

KinematicTrajectoryOptimization& operator= ( const KinematicTrajectoryOptimization & )

delete

◆ prog()

const solvers::MathematicalProgram& prog ( ) const

Getter for the optimization program.

◆ q()

const VectorX<symbolic::Variable>& q ( ) const

Returns the placeholder variable for generalized position q.

Note these are NOT decision variables in the MathematicalProgram. These variables will be substituted for the real decision variables at particular times. Passing these variables directily into objective/constraints for the MathematicalProgram will result in an error.

◆ qddot()

const VectorX<symbolic::Variable>& qddot ( ) const

Returns the placeholder variable for the second time derivative of generalized position.

Note these are NOT decision variables in the MathematicalProgram. These variables will be substituted for the real decision variables at particular times. Passing these variables directily into objective/constraints for the MathematicalProgram will result in an error.

◆ qdot()

const VectorX<symbolic::Variable>& qdot ( ) const

Returns the placeholder variable for the time derivative of generalized position.

Note these are NOT decision variables in the MathematicalProgram. These variables will be substituted for the real decision variables at particular times. Passing these variables directily into objective/constraints for the MathematicalProgram will result in an error.

◆ ReconstructTrajectory()

trajectories::BsplineTrajectory<double> ReconstructTrajectory ( const solvers::MathematicalProgramResult & result ) const

Returns the trajectory q(t) from the result of solving prog().

◆ SetInitialGuess()

void SetInitialGuess ( const trajectories::BsplineTrajectory< double > & trajectory )

Sets the initial guess for the path and duration to match trajectory.

Precondition: trajectory.rows() == num_positions(); trajectory.columns() == 1

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

drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h

Detailed Description

Public Member Functions

Constructor & Destructor Documentation

◆ KinematicTrajectoryOptimization() [1/4]

◆ KinematicTrajectoryOptimization() [2/4]

◆ KinematicTrajectoryOptimization() [3/4]

◆ KinematicTrajectoryOptimization() [4/4]

Member Function Documentation

◆ AddAccelerationBounds()

◆ AddDurationConstraint()

◆ AddDurationCost()

◆ AddEffortBoundsAtNormalizedTimes()

◆ AddJerkBounds()

◆ AddPathAccelerationConstraint()

◆ AddPathEnergyCost()

◆ AddPathLengthCost()

◆ AddPathPositionConstraint() [1/2]

◆ AddPathPositionConstraint() [2/2]

◆ AddPathVelocityConstraint()

◆ AddPositionBounds()

◆ AddVelocityBounds()

◆ AddVelocityConstraintAtNormalizedTime() [1/2]

◆ AddVelocityConstraintAtNormalizedTime() [2/2]

◆ basis()

◆ control_points()

◆ duration()

◆ get_mutable_prog()

◆ num_control_points()

◆ num_positions()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ prog()

◆ q()

◆ qddot()

◆ qdot()

◆ ReconstructTrajectory()

◆ SetInitialGuess()