|
Drake
|
|
Drake
|
MultipleShooting is an abstract class for trajectory optimization that creates decision variables for inputs, states, and (optionally) sample times along the trajectory, then provides a number of methods for working with those decision variables.
MultipleShooting classes add decision variables, costs, and constraints to a MathematicalProgram. You can retrieve that program using prog(), and add additional variables, costs, and constraints using the MathematicalProgram interface directly.
Subclasses must implement the abstract methods: DoAddRunningCost() ReconstructInputTrajectory() ReconstructStateTrajectory() using all of the correct interpolation schemes for the specific transcription method, and should add the constraints to impose the System% dynamics in their constructor.
This class assumes that there are a fixed number (N) time steps/samples, and that the trajectory is discretized into time steps h (N-1 of these), state x (N of these), and control input u (N of these).
#include <drake/planning/trajectory_optimization/multiple_shooting.h>
Public Types | |
| 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 |
Public Member Functions | |
| virtual | ~MultipleShooting () |
| solvers::MathematicalProgram & | prog () |
| Returns a reference to the MathematicalProgram associated with the trajectory optimization problem. More... | |
| const solvers::MathematicalProgram & | prog () 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::VectorXDecisionVariable & | state () 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::VectorXDecisionVariable & | input () 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::VectorXDecisionVariable > | state (int index) const |
Returns the decision variables associated with the state, x, at time index index. More... | |
| Eigen::VectorBlock< const solvers::VectorXDecisionVariable > | initial_state () const |
| Returns the decision variables associated with the state, x, at the initial time index. More... | |
| Eigen::VectorBlock< const solvers::VectorXDecisionVariable > | final_state () const |
| Returns the decision variables associated with the state, x, at the final time index. More... | |
| Eigen::VectorBlock< const solvers::VectorXDecisionVariable > | input (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::BoundingBoxConstraint > | AddTimeIntervalBounds (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::LinearConstraint > | AddDurationBounds (double lower_bound, double upper_bound) |
| Adds a constraint on the total duration of the trajectory. More... | |
| solvers::Binding< solvers::Cost > | AddFinalCost (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::Cost > | AddFinalCost (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::VisualizationCallback > | AddInputTrajectoryCallback (const TrajectoryCallback &callback) |
| Adds a callback method to visualize intermediate results of input variables used in the trajectory optimization. More... | |
| solvers::Binding< solvers::VisualizationCallback > | AddStateTrajectoryCallback (const TrajectoryCallback &callback) |
| Adds a callback method to visualize intermediate results of state variables used in the trajectory optimization. More... | |
| solvers::Binding< solvers::VisualizationCallback > | AddCompleteTrajectoryCallback (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... | |
| virtual trajectories::PiecewisePolynomial< double > | ReconstructInputTrajectory (const solvers::MathematicalProgramResult &) const =0 |
| Get the input trajectory at the solution as a PiecewisePolynomial. More... | |
| virtual trajectories::PiecewisePolynomial< double > | ReconstructStateTrajectory (const solvers::MathematicalProgramResult &) const =0 |
| Get the state trajectory at the solution as a PiecewisePolynomial. More... | |
| double | fixed_time_step () const |
Does not allow copy, move, or assignment | |
| MultipleShooting (const MultipleShooting &)=delete | |
| MultipleShooting & | operator= (const MultipleShooting &)=delete |
| MultipleShooting (MultipleShooting &&)=delete | |
| MultipleShooting & | operator= (MultipleShooting &&)=delete |
| 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 |
| typedef std::function<void( const Eigen::Ref<const Eigen::VectorXd>& sample_times, const Eigen::Ref<const Eigen::MatrixXd>& values)> TrajectoryCallback |
|
delete |
|
delete |
|
virtual |
|
protected |
Constructs a MultipleShooting instance with fixed sample times.
It creates new placeholder variables for input and state.
| num_inputs | Number of inputs at each sample point. |
| num_states | Number of states at each sample point. |
| num_time_samples | Number of time samples. |
| fixed_time_step | The spacing between sample times. |
| prog | (optional). If non-null, then additional decision variables, costs, and constraints will be added into the existing MathematicalProgram. This can be useful for, e.g., combining multiple trajectory optimizations into a single program, coupled by a few constraints. If nullptr, then a new MathematicalProgram will be allocated. |
|
protected |
Constructs a MultipleShooting instance with fixed sample times.
It uses the provided input and state as placeholders instead of creating new placeholder variables for them.
| input | Placeholder variables for input. |
| state | Placeholder variables for state. |
| num_time_samples | Number of time samples. |
| fixed_time_step | The spacing between sample times. |
| prog | (optional). If non-null, then additional decision variables, costs, and constraints will be added into the existing MathematicalProgram. This can be useful for, e.g., combining multiple trajectory optimizations into a single program, coupled by a few constraints. If nullptr, then a new MathematicalProgram will be allocated. |
|
protected |
Constructs a MultipleShooting instance with sample times as decision variables.
It creates new placeholder variables for input, state, and time.
| num_inputs | Number of inputs at each sample point. |
| num_states | Number of states at each sample point. |
| num_time_samples | Number of time samples. |
| minimum_time_step | Minimum spacing between sample times. |
| maximum_time_step | Maximum spacing between sample times. |
| prog | (optional). If non-null, then additional decision variables, costs, and constraints will be added into the existing MathematicalProgram. This can be useful for, e.g., combining multiple trajectory optimizations into a single program, coupled by a few constraints. If nullptr, then a new MathematicalProgram will be allocated. |
|
protected |
Constructs a MultipleShooting instance with sample times as decision variables.
It uses the provided input, state, and time as placeholders instead of creating new placeholder variables for them.
| input | Placeholder variables for input. |
| state | Placeholder variables for state. |
| time | Placeholder variable for time. |
| num_time_samples | Number of time samples. |
| minimum_time_step | Minimum spacing between sample times. |
| maximum_time_step | Maximum spacing between sample times. |
| prog | (optional). If non-null, then additional decision variables, costs, and constraints will be added into the existing MathematicalProgram. This can be useful for, e.g., combining multiple trajectory optimizations into a single program, coupled by a few constraints. If nullptr, then a new MathematicalProgram will be allocated. |
| solvers::Binding<solvers::VisualizationCallback> AddCompleteTrajectoryCallback | ( | 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.
The callback should be of the form MyVisualization(sample_times, states, inputs, values), where sample_times is an N-by-1 VectorXd of sample times, states is a num_states-by-N MatrixXd of the current (intermediate) state trajectory at the break points, inputs is a num_inputs-by-N MatrixXd of the current (intermediate) input trajectory at the break points and values is a vector of num_rows-by-N MatrixXds of the current (intermediate) extra sequential variables specified by names at the break points.
| 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.
| 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.
| std::vector<solvers::Binding<solvers::Constraint> > AddConstraintToAllKnotPoints | ( | const Eigen::Ref< const VectorX< symbolic::Formula >> & | f | ) |
Variant of AddConstraintToAllKnotPoints that accepts a vector of formulas.
| solvers::Binding<solvers::LinearConstraint> AddDurationBounds | ( | double | lower_bound, |
| double | upper_bound | ||
| ) |
Adds a constraint on the total duration of the trajectory.
| lower_bound | A scalar double lower bound. |
| upper_bound | A scalar double upper bound. |
| std::exception | if time steps are not declared as decision variables. |
| std::vector<solvers::Binding<solvers::LinearConstraint> > AddEqualTimeIntervalsConstraints | ( | ) |
Adds constraints to enforce that all time steps have equal duration.
| std::exception | if time steps are not declared as decision variables. |
| solvers::Binding<solvers::Cost> AddFinalCost | ( | 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.
| solvers::Binding<solvers::Cost> AddFinalCost | ( | 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.
| solvers::Binding<solvers::VisualizationCallback> AddInputTrajectoryCallback | ( | const TrajectoryCallback & | callback | ) |
Adds a callback method to visualize intermediate results of input variables used in the trajectory optimization.
The callback should be of the form MyVisualization(sample_times, values), where breaks is a N-by-1 VectorXd of sample times, and values is a num_inputs-by-N MatrixXd representing the current (intermediate) value of the input trajectory at the break points in each column.
| 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.
The particular integration scheme is determined by the derived class implementation.
| 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.
| solvers::Binding<solvers::VisualizationCallback> AddStateTrajectoryCallback | ( | const TrajectoryCallback & | callback | ) |
Adds a callback method to visualize intermediate results of state variables used in the trajectory optimization.
The callback should be of the form MyVisualization(sample_times, values), where sample_times is a N-by-1 VectorXd of sample times, and values is a num_states-by-N MatrixXd representing the current (intermediate) value of the state trajectory at the break points in each column.
| solvers::Binding<solvers::BoundingBoxConstraint> AddTimeIntervalBounds | ( | double | lower_bound, |
| double | upper_bound | ||
| ) |
Adds bounds on all time intervals.
| lower_bound | A scalar double lower bound. |
| upper_bound | A scalar double upper bound. |
| std::exception | if time steps are not declared as decision variables. |
| Eigen::VectorBlock<const solvers::VectorXDecisionVariable> final_state | ( | ) | const |
Returns the decision variables associated with the state, x, at the final time index.
| double fixed_time_step | ( | ) | const |
| Eigen::MatrixXd GetInputSamples | ( | const solvers::MathematicalProgramResult & | result | ) | const |
Returns a matrix containing the input values (arranged in columns) at each breakpoint at the solution.
| Eigen::VectorXd GetSampleTimes | ( | const Eigen::Ref< const Eigen::VectorXd > & | h_var_values | ) | const |
Returns a vector containing the elapsed time at each breakpoint.
| Eigen::VectorXd GetSampleTimes | ( | const solvers::MathematicalProgramResult & | result | ) | const |
Returns a vector containing the elapsed time at each breakpoint at the solution.
|
protected |
Returns the decision variables associated with the sequential variable name.
| solvers::VectorXDecisionVariable GetSequentialVariableAtIndex | ( | const std::string & | name, |
| int | index | ||
| ) | const |
Returns the decision variables associated with the sequential variable name at time index index.
| 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.
| name | The name of sequential variable to get the results for. Must correspond to an already added sequential variable. |
| Eigen::MatrixXd GetStateSamples | ( | const solvers::MathematicalProgramResult & | result | ) | const |
Returns a matrix containing the state values (arranged in columns) at each breakpoint at the solution.
|
protected |
| Eigen::VectorBlock<const solvers::VectorXDecisionVariable> initial_state | ( | ) | const |
Returns the decision variables associated with the state, x, at the initial time index.
| const solvers::VectorXDecisionVariable& input | ( | ) | 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.
These variables will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing these variables directly into objectives/constraints for the parent classes will result in an error.
| Eigen::VectorBlock<const solvers::VectorXDecisionVariable> input | ( | int | index | ) | const |
Returns the decision variables associated with the input, u, at time index index.
|
protected |
| 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).
This variable will be substituted for real decision variables at particular times in methods like AddRunningCost(). Passing this variable directly into objectives/constraints for the parent classes will result in an error.
|
protected |
|
protected |
|
delete |
|
delete |
| solvers::MathematicalProgram& prog | ( | ) |
Returns a reference to the MathematicalProgram associated with the trajectory optimization problem.
| const solvers::MathematicalProgram& prog | ( | ) | const |
Returns a const reference to the MathematicalProgram associated with the trajectory optimization problem.
|
pure virtual |
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. Requires that the system has at least one input port.
Implemented in DirectTranscription, and DirectCollocation.
|
pure virtual |
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.
Implemented in DirectTranscription, and DirectCollocation.
| 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.
| traj_init_u | Initial guess for trajectory for control input. The number of rows for each segment in traj_init_u must be equal to num_inputs (the first param of the constructor). If empty, then a default small non-zero initial value is used instead. |
| traj_init_x | Initial guess for trajectory for state input. The number of rows for each segment in traj_init_x must be equal to num_states (the second param of the constructor). If empty, then a default small non-zero initial value is used instead. |
If time steps are decision variables, then the initial guess for the time steps are evenly distributed to match the duration of the traj_init_u and traj_init_x.
| std::exception | if traj_init_u and traj_init_x are both empty, or if traj_init_u and traj_init_x are both non-empty, and have different start and end times. |
| const solvers::VectorXDecisionVariable& state | ( | ) | 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.
These variables will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing these variables directly into objectives/constraints for the parent classes will result in an error.
| Eigen::VectorBlock<const solvers::VectorXDecisionVariable> state | ( | int | index | ) | const |
Returns the decision variables associated with the state, x, at time index index.
|
protected |
Replaces e.g.
placeholder_x_var_ with x_vars_ at time interval interval_index, for all placeholder variables.
|
protected |
Replaces e.g.
placeholder_x_var_ with x_vars_ at time interval interval_index, for all placeholder variables.
| 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.
This variable will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing this variable directly into objectives/constraints for the parent classes will result in an error.
| const solvers::VectorDecisionVariable<1> time_step | ( | int | index | ) | const |
Returns the decision variable associated with the time step, h, at time index index.
| std::exception | if time steps are not declared as decision variables. |
|
protected |
|
protected |
|
protected |