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

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

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

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<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).

These constraints will be respected at all times. Note that this does NOT directly constrain r̈(s).

Returns: A vector of bindings with the ith element is itself a vector of constraints (one per dof) adding a constraint to the ith control point of the acceleration trajectory.

◆ 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.

◆ AddJerkBounds()

std::vector<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).

These constraints will be respected at all times. Note that this does NOT directly constrain d³rds³(s).

Returns: A vector of bindings with the ith element is itself a vector of constraints (one per dof) adding a constraint to the ith control point of the jerk trajectory.

◆ 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.

◆ 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.

◆ 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.

◆ 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).

These bounds will be respected at all times. Note this does NOT directly constrain ṙ(s).

Returns: A vector of bindings with the ith element adding a constraint to the ith control point of the derivative trajectory.

◆ AddVelocityConstraintAtNormalizedTime()

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.

◆ 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.

◆ 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()

◆ AddJerkBounds()

◆ AddPathAccelerationConstraint()

◆ AddPathEnergyCost()

◆ AddPathLengthCost()

◆ AddPathPositionConstraint() [1/2]

◆ AddPathPositionConstraint() [2/2]

◆ AddPathVelocityConstraint()

◆ AddPositionBounds()

◆ AddVelocityBounds()

◆ AddVelocityConstraintAtNormalizedTime()

◆ basis()

◆ control_points()

◆ duration()

◆ get_mutable_prog()

◆ num_control_points()

◆ num_positions()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ prog()

◆ ReconstructTrajectory()

◆ SetInitialGuess()