Drake
Drake C++ Documentation
DirectTranscription Class Reference

Detailed Description

DirectTranscription is perhaps the simplest implementation of a multiple shooting method, where we have decision variables representing the control and input at every sample time in the trajectory, and one-step of numerical integration provides the dynamic constraints between those decision variables.

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

Public Member Functions

 DirectTranscription (const systems::System< double > *system, const systems::Context< double > &context, int num_time_samples, const std::variant< systems::InputPortSelection, systems::InputPortIndex > &input_port_index=systems::InputPortSelection::kUseFirstInputIfItExists)
 Constructs the MathematicalProgram and adds the dynamic constraints. More...
 
 DirectTranscription (const systems::TimeVaryingLinearSystem< double > *system, const systems::Context< double > &context, int num_time_samples, const std::variant< systems::InputPortSelection, systems::InputPortIndex > &input_port_index=systems::InputPortSelection::kUseFirstInputIfItExists)
 Constructs the MathematicalProgram and adds the dynamic constraints. More...
 
 DirectTranscription (const systems::System< double > *system, const systems::Context< double > &context, int num_time_samples, TimeStep fixed_time_step, const std::variant< systems::InputPortSelection, systems::InputPortIndex > &input_port_index=systems::InputPortSelection::kUseFirstInputIfItExists)
 Constructs the MathematicalProgram and adds the dynamic constraints. More...
 
 ~DirectTranscription () override
 
trajectories::PiecewisePolynomial< doubleReconstructInputTrajectory (const solvers::MathematicalProgramResult &result) const override
 Get the input trajectory at the solution as a PiecewisePolynomial. More...
 
trajectories::PiecewisePolynomial< doubleReconstructStateTrajectory (const solvers::MathematicalProgramResult &result) const override
 Get the state trajectory at the solution as a PiecewisePolynomial. More...
 
Does not allow copy, move, or assignment
 DirectTranscription (const DirectTranscription &)=delete
 
DirectTranscriptionoperator= (const DirectTranscription &)=delete
 
 DirectTranscription (DirectTranscription &&)=delete
 
DirectTranscriptionoperator= (DirectTranscription &&)=delete
 
- Public Member Functions inherited from MultipleShooting
virtual ~MultipleShooting ()
 
solvers::MathematicalProgramprog ()
 Returns a reference to the MathematicalProgram associated with the trajectory optimization problem. More...
 
const solvers::MathematicalProgramprog () const
 Returns a const reference to the MathematicalProgram associated with the trajectory optimization problem. More...
 
const solvers::VectorDecisionVariable< 1 > time_step (int index) const
 Returns the decision variable associated with the time step, h, at time index index. More...
 
const solvers::VectorDecisionVariable< 1 > & time () const
 Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the time, t. More...
 
const solvers::VectorXDecisionVariablestate () const
 Returns placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the state, x, but with the time-index undetermined. More...
 
const solvers::VectorXDecisionVariableinput () const
 Returns placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the input, u, but with the time-index undetermined. More...
 
Eigen::VectorBlock< const solvers::VectorXDecisionVariablestate (int index) const
 Returns the decision variables associated with the state, x, at time index index. More...
 
Eigen::VectorBlock< const solvers::VectorXDecisionVariableinitial_state () const
 Returns the decision variables associated with the state, x, at the initial time index. More...
 
Eigen::VectorBlock< const solvers::VectorXDecisionVariablefinal_state () const
 Returns the decision variables associated with the state, x, at the final time index. More...
 
Eigen::VectorBlock< const solvers::VectorXDecisionVariableinput (int index) const
 Returns the decision variables associated with the input, u, at time index index. More...
 
solvers::VectorXDecisionVariable NewSequentialVariable (int rows, const std::string &name)
 Adds a sequential variable (a variable that has associated decision variables for each time index) to the optimization problem and returns a placeholder variable (not actually declared as a decision variable in the MathematicalProgram). More...
 
solvers::VectorXDecisionVariable GetSequentialVariableAtIndex (const std::string &name, int index) const
 Returns the decision variables associated with the sequential variable name at time index index. More...
 
void AddRunningCost (const symbolic::Expression &g)
 Adds an integrated cost to all time steps, of the form

\[ cost = \int_0^T g(t,x,u) dt, \]

where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index. More...

 
template<typename Derived >
void AddRunningCost (const Eigen::MatrixBase< Derived > &g)
 Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations. More...
 
template<typename C >
std::vector< solvers::Binding< C > > AddConstraintToAllKnotPoints (std::shared_ptr< C > constraint, const Eigen::Ref< const VectorX< symbolic::Variable >> &vars)
 Adds a constraint to all breakpoints, where any instances in vars of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index. More...
 
std::vector< solvers::Binding< solvers::Constraint > > AddConstraintToAllKnotPoints (const symbolic::Formula &f)
 Adds a constraint to all breakpoints, where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index. More...
 
std::vector< solvers::Binding< solvers::Constraint > > AddConstraintToAllKnotPoints (const Eigen::Ref< const VectorX< symbolic::Formula >> &f)
 Variant of AddConstraintToAllKnotPoints that accepts a vector of formulas. More...
 
solvers::Binding< solvers::BoundingBoxConstraintAddTimeIntervalBounds (double lower_bound, double upper_bound)
 Adds bounds on all time intervals. More...
 
std::vector< solvers::Binding< solvers::LinearConstraint > > AddEqualTimeIntervalsConstraints ()
 Adds constraints to enforce that all time steps have equal duration. More...
 
solvers::Binding< solvers::LinearConstraintAddDurationBounds (double lower_bound, double upper_bound)
 Adds a constraint on the total duration of the trajectory. More...
 
solvers::Binding< solvers::CostAddFinalCost (const symbolic::Expression &e)
 Adds a cost to the final time, of the form

\[ cost = e(t,x,u), \]

where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for the final time index. More...

 
solvers::Binding< solvers::CostAddFinalCost (const Eigen::Ref< const MatrixX< symbolic::Expression >> &matrix)
 Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations. More...
 
solvers::Binding< solvers::VisualizationCallbackAddInputTrajectoryCallback (const TrajectoryCallback &callback)
 Adds a callback method to visualize intermediate results of input variables used in the trajectory optimization. More...
 
solvers::Binding< solvers::VisualizationCallbackAddStateTrajectoryCallback (const TrajectoryCallback &callback)
 Adds a callback method to visualize intermediate results of state variables used in the trajectory optimization. More...
 
solvers::Binding< solvers::VisualizationCallbackAddCompleteTrajectoryCallback (const CompleteTrajectoryCallback &callback, const std::vector< std::string > &names)
 Adds a callback method to visualize intermediate results of all variables used in the trajectory optimization. More...
 
void SetInitialTrajectory (const trajectories::PiecewisePolynomial< double > &traj_init_u, const trajectories::PiecewisePolynomial< double > &traj_init_x)
 Set the initial guess for the trajectory decision variables. More...
 
Eigen::VectorXd GetSampleTimes (const Eigen::Ref< const Eigen::VectorXd > &h_var_values) const
 Returns a vector containing the elapsed time at each breakpoint. More...
 
Eigen::VectorXd GetSampleTimes (const solvers::MathematicalProgramResult &result) const
 Returns a vector containing the elapsed time at each breakpoint at the solution. More...
 
Eigen::MatrixXd GetInputSamples (const solvers::MathematicalProgramResult &result) const
 Returns a matrix containing the input values (arranged in columns) at each breakpoint at the solution. More...
 
Eigen::MatrixXd GetStateSamples (const solvers::MathematicalProgramResult &result) const
 Returns a matrix containing the state values (arranged in columns) at each breakpoint at the solution. More...
 
Eigen::MatrixXd GetSequentialVariableSamples (const solvers::MathematicalProgramResult &result, const std::string &name) const
 Returns a matrix containing the sequential variable values (arranged in columns) at each breakpoint at the solution. More...
 
double fixed_time_step () const
 
 MultipleShooting (const MultipleShooting &)=delete
 
MultipleShootingoperator= (const MultipleShooting &)=delete
 
 MultipleShooting (MultipleShooting &&)=delete
 
MultipleShootingoperator= (MultipleShooting &&)=delete
 

Additional Inherited Members

- Public Types inherited from MultipleShooting
typedef std::function< void(const Eigen::Ref< const Eigen::VectorXd > &sample_times, const Eigen::Ref< const Eigen::MatrixXd > &values)> TrajectoryCallback
 
typedef std::function< void(const Eigen::Ref< const Eigen::VectorXd > &sample_times, const Eigen::Ref< const Eigen::MatrixXd > &states, const Eigen::Ref< const Eigen::MatrixXd > &inputs, const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &values)> CompleteTrajectoryCallback
 
- Protected Member Functions inherited from MultipleShooting
 MultipleShooting (int num_inputs, int num_states, int num_time_samples, double fixed_time_step, solvers::MathematicalProgram *prog=nullptr)
 Constructs a MultipleShooting instance with fixed sample times. More...
 
 MultipleShooting (const solvers::VectorXDecisionVariable &input, const solvers::VectorXDecisionVariable &state, int num_time_samples, double fixed_time_step, solvers::MathematicalProgram *prog=nullptr)
 Constructs a MultipleShooting instance with fixed sample times. More...
 
 MultipleShooting (int num_inputs, int num_states, int num_time_samples, double minimum_time_step, double maximum_time_step, solvers::MathematicalProgram *prog=nullptr)
 Constructs a MultipleShooting instance with sample times as decision variables. More...
 
 MultipleShooting (const solvers::VectorXDecisionVariable &input, const solvers::VectorXDecisionVariable &state, const solvers::DecisionVariable &time, int num_time_samples, double minimum_time_step, double maximum_time_step, solvers::MathematicalProgram *prog=nullptr)
 Constructs a MultipleShooting instance with sample times as decision variables. More...
 
symbolic::Expression SubstitutePlaceholderVariables (const symbolic::Expression &e, int interval_index) const
 Replaces e.g. More...
 
symbolic::Formula SubstitutePlaceholderVariables (const symbolic::Formula &f, int interval_index) const
 Replaces e.g. More...
 
int num_inputs () const
 
int num_states () const
 
int N () const
 
bool time_steps_are_decision_variables () const
 
const solvers::VectorXDecisionVariableh_vars () const
 
const solvers::VectorXDecisionVariableu_vars () const
 
const solvers::VectorXDecisionVariablex_vars () const
 
const solvers::VectorXDecisionVariable GetSequentialVariable (const std::string &name) const
 Returns the decision variables associated with the sequential variable name. More...
 

Constructor & Destructor Documentation

◆ DirectTranscription() [1/5]

◆ DirectTranscription() [2/5]

◆ DirectTranscription() [3/5]

DirectTranscription ( const systems::System< double > *  system,
const systems::Context< double > &  context,
int  num_time_samples,
const std::variant< systems::InputPortSelection, systems::InputPortIndex > &  input_port_index = systems::InputPortSelection::kUseFirstInputIfItExists 
)

Constructs the MathematicalProgram and adds the dynamic constraints.

This version of the constructor is only for simple discrete-time systems (with a single periodic time step update). Continuous-time systems must call one of the constructors that takes bounds on the time step as an argument.

Parameters
systemA dynamical system to be used in the dynamic constraints. This system must support System::ToAutoDiffXd. Note that this is aliased for the lifetime of this object.
contextRequired to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be "cloned" by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
num_time_samplesThe number of breakpoints in the trajectory.
input_port_indexA valid input port index or valid InputPortSelection for system. All other inputs on the system will be left disconnected (if they are disconnected in context) or will be set to their current values (if they are connected/fixed in context).
Default: kUseFirstInputIfItExists.
Exceptions
std::exceptionif context.has_only_discrete_state() == false.

◆ DirectTranscription() [4/5]

DirectTranscription ( const systems::TimeVaryingLinearSystem< double > *  system,
const systems::Context< double > &  context,
int  num_time_samples,
const std::variant< systems::InputPortSelection, systems::InputPortIndex > &  input_port_index = systems::InputPortSelection::kUseFirstInputIfItExists 
)

Constructs the MathematicalProgram and adds the dynamic constraints.

This version of the constructor is only for linear time-varying discrete-time systems (with a single periodic time step update). This constructor adds value because the symbolic short-cutting does not yet support systems that are affine in state/input, but not time.

Parameters
systemA linear time-varying system to be used in the dynamic constraints. Note that this is aliased for the lifetime of this object.
contextRequired to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be "cloned" by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
num_time_samplesThe number of breakpoints in the trajectory.
input_port_indexA valid input port index or valid InputPortSelection for system. All other inputs on the system will be left disconnected (if they are disconnected in context) or will be set to their current values (if they are connected/fixed in context).
Default: kUseFirstInputIfItExists.
Exceptions
std::exceptionif context.has_only_discrete_state() == false.

◆ DirectTranscription() [5/5]

DirectTranscription ( const systems::System< double > *  system,
const systems::Context< double > &  context,
int  num_time_samples,
TimeStep  fixed_time_step,
const std::variant< systems::InputPortSelection, systems::InputPortIndex > &  input_port_index = systems::InputPortSelection::kUseFirstInputIfItExists 
)

Constructs the MathematicalProgram and adds the dynamic constraints.

This version of the constructor is only for continuous-time systems; the dynamics constraints use explicit forward Euler integration.

Parameters
systemA dynamical system to be used in the dynamic constraints. This system must support System::ToAutoDiffXd. Note that this is aliased for the lifetime of this object.
contextRequired to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be "cloned" by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
num_time_samplesThe number of breakpoints in the trajectory.
fixed_time_stepThe spacing between sample times.
input_port_indexA valid input port index or valid InputPortSelection for system. All other inputs on the system will be left disconnected (if they are disconnected in context) or will be set to their current values (if they are connected/fixed in context).
Default: kUseFirstInputIfItExists.
Exceptions
std::exceptionif context.has_only_continuous_state() == false.

◆ ~DirectTranscription()

~DirectTranscription ( )
override

Member Function Documentation

◆ operator=() [1/2]

DirectTranscription& operator= ( const DirectTranscription )
delete

◆ operator=() [2/2]

DirectTranscription& operator= ( DirectTranscription &&  )
delete

◆ ReconstructInputTrajectory()

trajectories::PiecewisePolynomial<double> ReconstructInputTrajectory ( const solvers::MathematicalProgramResult result) const
overridevirtual

Get the input trajectory at the solution as a PiecewisePolynomial.

The order of the trajectory will be determined by the integrator used in the dynamic constraints.

Implements MultipleShooting.

◆ ReconstructStateTrajectory()

trajectories::PiecewisePolynomial<double> ReconstructStateTrajectory ( const solvers::MathematicalProgramResult result) const
overridevirtual

Get the state trajectory at the solution as a PiecewisePolynomial.

The order of the trajectory will be determined by the integrator used in the dynamic constraints.

Implements MultipleShooting.


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