Drake
DircolTrajectoryOptimization Class Reference

DircolTrajectoryOptimization implements a direct colocation approach to trajectory optimization, as described in C. More...

#include <drake/systems/trajectory_optimization/direct_collocation.h>

Inheritance diagram for DircolTrajectoryOptimization:
Collaboration diagram for DircolTrajectoryOptimization:

Public Member Functions

 DircolTrajectoryOptimization (const System< double > *system, const Context< double > &context, int num_time_samples, double trajectory_time_lower_bound, double trajectory_time_upper_bound)
 
 ~DircolTrajectoryOptimization () override
 
PiecewisePolynomialTrajectory ReconstructStateTrajectory () const override
 Get the state trajectory as a PiecewisePolynomialTrajectory. More...
 
Does not allow copy, move, or assignment
 DircolTrajectoryOptimization (const DircolTrajectoryOptimization &)=delete
 
DircolTrajectoryOptimizationoperator= (const DircolTrajectoryOptimization &)=delete
 
 DircolTrajectoryOptimization (DircolTrajectoryOptimization &&)=delete
 
DircolTrajectoryOptimizationoperator= (DircolTrajectoryOptimization &&)=delete
 
- Public Member Functions inherited from DirectTrajectoryOptimization
 ~DirectTrajectoryOptimization () override
 
const solvers::VectorDecisionVariable< 1 > timestep (int index) const
 Returns the decision variable associated with the timestep, 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...
 
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 are substituted with the relevant variables for each current 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 F >
std::enable_if< solvers::detail::is_cost_functor_candidate< F >::value, std::shared_ptr< solvers::Cost > >::type AddRunningCostFunc (F &&f)
 Adds an integrated cost to all time steps. More...
 
void AddRunningCost (std::shared_ptr< solvers::Cost > cost)
 
void AddInputBounds (const Eigen::VectorXd &lower_bound, const Eigen::VectorXd &upper_bound)
 Add upper and lower bounds on the input values. More...
 
template<typename ConstraintT >
void AddInputConstraint (std::shared_ptr< ConstraintT > constraint, const std::vector< int > &time_indices)
 Add a constraint on the input at the specified time indices. More...
 
template<typename ConstraintT >
void AddStateConstraint (std::shared_ptr< ConstraintT > constraint, const std::vector< int > &time_indices)
 Add a constraint on the state at the specified time indices. More...
 
void AddTimeIntervalBounds (const Eigen::VectorXd &lower_bound, const Eigen::VectorXd &upper_bound, const std::vector< int > &interval_indices)
 Add bounds on a set of time intervals, such that lower_bound(i) <= h_vars_(interval_indices[i]) <= upper_bound(i) where h_vars_[j] is the time interval between j'th and j+1'th sample (starting from 0'th sample). More...
 
void AddTimeIntervalBounds (const Eigen::VectorXd &lower_bound, const Eigen::VectorXd &upper_bound)
 Add bounds on all time intervals, such that lower_bound(i) <= h_vars_(i) <= upper_bound(i) where h_vars_[i] is the time interval between i'th and i+1'th sample (starting from 0'th sample). More...
 
void AddTimeIntervalBounds (double lower_bound, double upper_bound)
 Add bounds on all time intervals, such that lower_bound <= h_vars_(i) <= upper_bound for all time intervals. More...
 
void AddFinalCost (std::shared_ptr< solvers::Cost > cost)
 Add a cost to the final state and total time. More...
 
void 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 are substituted with the relevant variables for each current time index. More...

 
void AddFinalCost (const Eigen::Ref< const MatrixX< symbolic::Expression >> &e)
 Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations. More...
 
template<typename F >
std::enable_if< solvers::detail::is_cost_functor_candidate< F >::value, std::shared_ptr< solvers::Cost > >::type AddFinalCostFunc (F &&f)
 Add a cost to the final state and total time. More...
 
solvers::SolutionResult SolveTraj (double timespan_init, const PiecewisePolynomial< double > &traj_init_u, const PiecewisePolynomial< double > &traj_init_x)
 Solve the nonlinear program and return the resulting trajectory. More...
 
void GetResultSamples (Eigen::MatrixXd *inputs, Eigen::MatrixXd *states, std::vector< double > *times) const
 Extract the result of the trajectory solution as a set of discrete samples. More...
 
PiecewisePolynomialTrajectory ReconstructInputTrajectory () const
 Get the input trajectory as a PiecewisePolynomialTrajectory. More...
 
 DirectTrajectoryOptimization (const DirectTrajectoryOptimization &)=delete
 
DirectTrajectoryOptimizationoperator= (const DirectTrajectoryOptimization &)=delete
 
 DirectTrajectoryOptimization (DirectTrajectoryOptimization &&)=delete
 
DirectTrajectoryOptimizationoperator= (DirectTrajectoryOptimization &&)=delete
 
- Public Member Functions inherited from MathematicalProgram
 MathematicalProgram ()
 
virtual ~MathematicalProgram ()
 
VectorXDecisionVariable NewContinuousVariables (int rows, const std::string &name="x")
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables (int rows, int cols, const std::string &name)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int Rows, int Cols = 1>
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables (const std::string &name="X")
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables (int rows, int cols, const std::string &name)
 Adds binary variables, appending them to an internal vector of any existing vars. More...
 
template<int Rows, int Cols = 1>
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables (const std::string &name="b")
 Adds a matrix of binary variables into the optimization program. More...
 
VectorXDecisionVariable NewBinaryVariables (int rows, const std::string &name="b")
 Adds binary variables to this MathematicalProgram. More...
 
MatrixXDecisionVariable NewSymmetricContinuousVariables (int rows, const std::string &name="Symmetric")
 Adds a runtime sized symmetric matrix as decision variables to this MathematicalProgram. More...
 
template<int rows>
MatrixDecisionVariable< rows, rows > NewSymmetricContinuousVariables (const std::string &name="Symmetric")
 Adds a static sized symmetric matrix as decision variables to this MathematicalProgram. More...
 
symbolic::Polynomial NewFreePolynomial (const symbolic::Variables &indeterminates, int degree, const std::string &coeff_name="a")
 Returns a free polynomial in a monomial basis over indeterminates of a given degree. More...
 
std::pair< symbolic::Polynomial, Binding< PositiveSemidefiniteConstraint > > NewSosPolynomial (const symbolic::Variables &indeterminates, int degree)
 Returns a pair of a SOS polynomial p = xᵀQx of degree degree and a PSD constraint for the coefficients matrix Q, where x is a monomial basis over indeterminates of degree @p degree / 2. More...
 
template<int rows, int cols>
MatrixIndeterminate< rows, cols > NewIndeterminates (const std::array< std::string, rows *cols > &names)
 Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...
 
template<int rows>
VectorIndeterminate< rows > NewIndeterminates (const std::array< std::string, rows > &names)
 Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...
 
template<int rows, int cols>
MatrixIndeterminate< rows, cols > NewIndeterminates (const std::string &name="X")
 Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...
 
template<int rows>
VectorIndeterminate< rows > NewIndeterminates (const std::string &name="x")
 Adds indeterminates to the program. More...
 
VectorXIndeterminate NewIndeterminates (int rows, const std::vector< std::string > &names)
 Adds indeterminates to this MathematicalProgram. More...
 
VectorXIndeterminate NewIndeterminates (int rows, const std::string &name="x")
 Adds indeterminates to this MathematicalProgram, with default name "x". More...
 
MatrixXIndeterminate NewIndeterminates (int rows, int cols, const std::vector< std::string > &names)
 Adds indeterminates, appending them to an internal vector of any existing vars. More...
 
MatrixXIndeterminate NewIndeterminates (int rows, int cols, const std::string &name="X")
 Adds indeterminates to this MathematicalProgram, with default name "X". More...
 
Binding< CostAddCost (const Binding< Cost > &binding)
 Adds a generic cost to the optimization program. More...
 
template<typename C >
auto AddCost (const std::shared_ptr< C > &obj, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost type to the optimization program. More...
 
template<typename C >
auto AddCost (const std::shared_ptr< C > &obj, const VariableRefList &vars)
 Adds a generic cost to the optimization program. More...
 
template<typename F >
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost (F &&f, const VariableRefList &vars)
 Adds a cost to the optimization program on a list of variables. More...
 
template<typename F >
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost (F &&f, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost to the optimization program on an Eigen::Vector containing decision variables. More...
 
template<typename F , typename Vars >
std::enable_if< detail::assert_if_is_constraint< F >::value, Binding< Cost > >::type AddCost (F &&, Vars &&)
 Statically assert if a user inadvertently passes a binding-compatible Constraint. More...
 
Binding< LinearCostAddCost (const Binding< LinearCost > &binding)
 Adds a cost term of the form c'*x. More...
 
Binding< LinearCostAddLinearCost (const symbolic::Expression &e)
 Adds a linear cost term of the form a'*x + b. More...
 
Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, double b, const VariableRefList &vars)
 Adds a linear cost term of the form a'*x + b. More...
 
Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, double b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a linear cost term of the form a'*x + b. More...
 
template<typename VarType >
Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, const VarType &vars)
 Adds a linear cost term of the form a'*x. More...
 
Binding< QuadraticCostAddCost (const Binding< QuadraticCost > &binding)
 Adds a cost term of the form 0.5*x'*Q*x + b'x. More...
 
Binding< QuadraticCostAddQuadraticCost (const symbolic::Expression &e)
 Add a quadratic cost term of the form 0.5*x'*Q*x + b'*x + c. More...
 
Binding< QuadraticCostAddQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const VariableRefList &vars)
 Adds a cost term of the form (x-x_desired)'Q(x-x_desired). More...
 
Binding< QuadraticCostAddQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost term of the form (x-x_desired)'Q(x-x_desired). More...
 
Binding< QuadraticCostAddL2NormCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
 Adds a cost term of the form | Ax - b |^2. More...
 
Binding< QuadraticCostAddL2NormCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost term of the form | Ax - b |^2. More...
 
Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
 Adds a cost term of the form 0.5*x'*Q*x + b'x. More...
 
Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, double c, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost term of the form 0.5*x'*Q*x + b'x + c Applied to subset of the variables. More...
 
Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a cost term of the form 0.5*x'*Q*x + b'x Applied to subset of the variables. More...
 
Binding< PolynomialCostAddPolynomialCost (const symbolic::Expression &e)
 Adds a cost term in the polynomial form. More...
 
Binding< CostAddCost (const symbolic::Expression &e)
 Adds a cost in the symbolic form. More...
 
Binding< ConstraintAddConstraint (const Binding< Constraint > &binding)
 Adds a generic constraint to the program. More...
 
template<typename C >
auto AddConstraint (std::shared_ptr< C > con, const VariableRefList &vars)
 Adds a generic constraint to the program. More...
 
template<typename C >
auto AddConstraint (std::shared_ptr< C > con, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a generic constraint to the program. More...
 
Binding< LinearConstraintAddConstraint (const Binding< LinearConstraint > &binding)
 Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
 Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const VariableRefList &vars)
 Adds one row of linear constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds one row of linear constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LinearConstraintAddLinearConstraint (const symbolic::Expression &e, double lb, double ub)
 Adds one row of linear constraint lb <= e <= ub where e is a symbolic expression. More...
 
Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
 Adds linear constraints represented by symbolic expressions to the program. More...
 
Binding< LinearConstraintAddLinearConstraint (const symbolic::Formula &f)
 Add a linear constraint represented by a symbolic formula to the program. More...
 
template<typename Derived >
std::enable_if< detail::is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< LinearConstraint > >::type AddLinearConstraint (const Eigen::ArrayBase< Derived > &formulas)
 Add a linear constraint represented by an Eigen::Array<symbolic::Formula> to the program. More...
 
Binding< LinearEqualityConstraintAddConstraint (const Binding< LinearEqualityConstraint > &binding)
 Adds linear equality constraints referencing potentially a subset of the decision variables. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const symbolic::Expression &e, double b)
 Adds one row of linear constraint e = b where e is a symbolic expression. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const symbolic::Formula &f)
 Adds a linear equality constraint represented by a symbolic formula to the program. More...
 
template<typename DerivedV , typename DerivedB >
std::enable_if< detail::is_eigen_vector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint (const Eigen::MatrixBase< DerivedV > &v, const Eigen::MatrixBase< DerivedB > &b)
 Adds linear equality constraints \( v = b \), where v(i) is a symbolic linear expression. More...
 
template<typename DerivedV , typename DerivedB >
std::enable_if< detail::is_eigen_matrix_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint (const Eigen::MatrixBase< DerivedV > &V, const Eigen::MatrixBase< DerivedB > &B, bool lower_triangle=false)
 Adds a linear equality constraint for a matrix of linear expression V, such that V(i, j) = B(i, j). More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const VariableRefList &vars)
 AddLinearEqualityConstraint. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 AddLinearEqualityConstraint. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const VariableRefList &vars)
 Adds one row of linear equality constraint referencing potentially a subset of decision variables. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds one row of linear equality constraint referencing potentially a subset of decision variables. More...
 
Binding< BoundingBoxConstraintAddConstraint (const Binding< BoundingBoxConstraint > &binding)
 Adds bounding box constraints referencing potentially a subest of the decision variables. More...
 
Binding< BoundingBoxConstraintAddBoundingBoxConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
 AddBoundingBoxConstraint. More...
 
Binding< BoundingBoxConstraintAddBoundingBoxConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds bounding box constraints referencing potentially a subset of the decision variables. More...
 
Binding< BoundingBoxConstraintAddBoundingBoxConstraint (double lb, double ub, const symbolic::Variable &var)
 Adds bounds for a single variable. More...
 
Binding< BoundingBoxConstraintAddBoundingBoxConstraint (double lb, double ub, const VariableRefList &vars)
 Adds the same scalar lower and upper bound to every variable in the list. More...
 
template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime==1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint (double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
 Adds the same scalar lower and upper bound to every variable in vars. More...
 
template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime!=1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint (double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
 Adds the same scalar lower and upper bound to every variable in vars. More...
 
Binding< LorentzConeConstraintAddConstraint (const Binding< LorentzConeConstraint > &binding)
 Adds Lorentz cone constraint referencing potentially a subset of the decision variables. More...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v)
 Adds Lorentz cone constraint referencing potentially a subset of the decision variables. More...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const symbolic::Expression &v1, const symbolic::Expression &v2)
 Adds Lorentz cone constraint on the linear expression v1 and quadratic expression v2, such that v1 >= sqrt(v2) More...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
 Adds Lorentz cone constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds Lorentz cone constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const VariableRefList &vars)
 Imposes that a vector \( x\in\mathbb{R}^m \) lies in Lorentz cone. More...
 
template<int rows>
Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
 Imposes that a vector \( x\in\mathbb{R}^m \) lies in Lorentz cone. More...
 
Binding< RotatedLorentzConeConstraintAddConstraint (const Binding< RotatedLorentzConeConstraint > &binding)
 Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables. More...
 
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v)
 Adds a constraint that a symbolic expression. More...
 
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
 Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables, The linear expression \( z=Ax+b \) is in rotated Lorentz cone. More...
 
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables, The linear expression \( z=Ax+b \) is in rotated Lorentz cone. More...
 
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const VariableRefList &vars)
 Impose that a vector \( x\in\mathbb{R}^m \) is in rotated Lorentz cone. More...
 
template<int rows>
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
 Impose that a vector \( x\in\mathbb{R}^m \) is in rotated Lorentz cone. More...
 
Binding< LinearComplementarityConstraintAddConstraint (const Binding< LinearComplementarityConstraint > &binding)
 Adds a linear complementarity constraints referencing a subset of the decision variables. More...
 
Binding< LinearComplementarityConstraintAddLinearComplementarityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &M, const Eigen::Ref< const Eigen::VectorXd > &q, const VariableRefList &vars)
 Adds a linear complementarity constraints referencing a subset of the decision variables. More...
 
Binding< LinearComplementarityConstraintAddLinearComplementarityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &M, const Eigen::Ref< const Eigen::VectorXd > &q, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a linear complementarity constraints referencing a subset of the decision variables. More...
 
Binding< ConstraintAddPolynomialConstraint (const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const VariableRefList &vars)
 Adds a polynomial constraint to the program referencing a subset of the decision variables (defined in the vars parameter). More...
 
Binding< ConstraintAddPolynomialConstraint (const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a polynomial constraint to the program referencing a subset of the decision variables (defined in the vars parameter). More...
 
Binding< PositiveSemidefiniteConstraintAddConstraint (const Binding< PositiveSemidefiniteConstraint > &binding)
 Adds a positive semidefinite constraint on a symmetric matrix. More...
 
Binding< PositiveSemidefiniteConstraintAddConstraint (std::shared_ptr< PositiveSemidefiniteConstraint > con, const Eigen::Ref< const MatrixXDecisionVariable > &symmetric_matrix_var)
 Adds a positive semidefinite constraint on a symmetric matrix. More...
 
Binding< PositiveSemidefiniteConstraintAddPositiveSemidefiniteConstraint (const Eigen::Ref< const MatrixXDecisionVariable > &symmetric_matrix_var)
 Adds a positive semidefinite constraint on a symmetric matrix. More...
 
template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Expression >::value, Binding< PositiveSemidefiniteConstraint > >::type AddPositiveSemidefiniteConstraint (const Eigen::MatrixBase< Derived > &e)
 Adds a positive semidefinite constraint on a symmetric matrix of symbolic espressions e. More...
 
Binding< LinearMatrixInequalityConstraintAddConstraint (const Binding< LinearMatrixInequalityConstraint > &binding)
 Adds a linear matrix inequality constraint to the program. More...
 
Binding< LinearMatrixInequalityConstraintAddLinearMatrixInequalityConstraint (const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &F, const VariableRefList &vars)
 Adds a linear matrix inequality constraint to the program. More...
 
Binding< LinearMatrixInequalityConstraintAddLinearMatrixInequalityConstraint (const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &F, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 Adds a linear matrix inequality constraint to the program. More...
 
std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Polynomial &p)
 Adds constraints that a given polynomial p is a sums-of-squares (SOS), that is, p can be decomposed into xᵀQx. More...
 
std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Expression &e)
 Adds constraints that a given symbolic expression e is a sums-of-squares (SOS), that is, e can be decomposed into xTQx. More...
 
template<typename DerivedA , typename DerivedB >
void SetInitialGuess (const Eigen::MatrixBase< DerivedA > &decision_variable_mat, const Eigen::MatrixBase< DerivedB > &x0)
 Set the initial guess for the decision variables stored in var to be x0. More...
 
template<typename Derived >
void SetInitialGuessForAllVariables (const Eigen::MatrixBase< Derived > &x0)
 Set the intial guess for ALL decision variables. More...
 
SolutionResult Solve ()
 Solve the MathematicalProgram. More...
 
void PrintSolution ()
 
void SetDecisionVariableValues (const Eigen::Ref< const Eigen::VectorXd > &values)
 Sets the values of all decision variables, such that the value of decision_variables_(i) is values(i). More...
 
void SetDecisionVariableValues (const Eigen::Ref< const VectorXDecisionVariable > &variables, const Eigen::Ref< const Eigen::VectorXd > &values)
 Sets the value of some decision variables, such that the value of variables(i) is values(i). More...
 
void SetDecisionVariableValue (const symbolic::Variable &var, double value)
 Sets the value of a single decision variable in the optimization program. More...
 
void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, double option_value)
 Set an option for a particular solver. More...
 
void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, int option_value)
 
void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, const std::string &option_value)
 
const std::map< std::string, double > & GetSolverOptionsDouble (const SolverId &solver_id)
 
const std::map< std::string, int > & GetSolverOptionsInt (const SolverId &solver_id)
 
const std::map< std::string, std::string > & GetSolverOptionsStr (const SolverId &solver_id)
 
void SetSolverId (SolverId solver_id)
 Sets the ID of the solver that was used to solve this program. More...
 
optional< SolverIdGetSolverId () const
 Returns the ID of the solver that was used to solve this program. More...
 
double GetOptimalCost () const
 Getter for optimal cost at the solution. More...
 
void SetOptimalCost (double optimal_cost)
 
double GetLowerBoundCost () const
 Getter for lower bound on optimal cost. More...
 
void SetLowerBoundCost (double lower_bound_cost)
 Setter for lower bound on optimal cost. More...
 
const std::vector< Binding< Cost > > & generic_costs () const
 Getter for all generic costs. More...
 
const std::vector< Binding< Constraint > > & generic_constraints () const
 Getter for all generic constraints. More...
 
const std::vector< Binding< LinearEqualityConstraint > > & linear_equality_constraints () const
 Getter for linear equality constraints. More...
 
const std::vector< Binding< LinearCost > > & linear_costs () const
 Getter for linear costs. More...
 
const std::vector< Binding< QuadraticCost > > & quadratic_costs () const
 Getter for quadratic costs. More...
 
const std::vector< Binding< LinearConstraint > > & linear_constraints () const
 Getter for linear constraints. More...
 
const std::vector< Binding< LorentzConeConstraint > > & lorentz_cone_constraints () const
 Getter for Lorentz cone constraint. More...
 
const std::vector< Binding< RotatedLorentzConeConstraint > > & rotated_lorentz_cone_constraints () const
 Getter for rotated Lorentz cone constraint. More...
 
const std::vector< Binding< PositiveSemidefiniteConstraint > > & positive_semidefinite_constraints () const
 Getter for positive semidefinite constraint. More...
 
const std::vector< Binding< LinearMatrixInequalityConstraint > > & linear_matrix_inequality_constraints () const
 Getter for linear matrix inequality constraint. More...
 
std::vector< Binding< Cost > > GetAllCosts () const
 Getter returning all costs (for now linear costs appended to generic costs, then quadratic costs appended to generic costs). More...
 
std::vector< Binding< LinearConstraint > > GetAllLinearConstraints () const
 Getter returning all linear constraints (both linear equality and inequality constraints). More...
 
const std::vector< Binding< BoundingBoxConstraint > > & bounding_box_constraints () const
 Getter for all bounding box constraints. More...
 
const std::vector< Binding< LinearComplementarityConstraint > > & linear_complementarity_constraints () const
 Getter for all linear complementarity constraints. More...
 
template<typename T >
std::shared_ptr< T > GetSolverData ()
 
int num_vars () const
 Getter for number of variables in the optimization program. More...
 
const Eigen::VectorXd & initial_guess () const
 Getter for the initial guess. More...
 
const Eigen::VectorXd GetSolutionVectorValues () const
 Returns the solution in a flat Eigen::VectorXd. More...
 
int FindDecisionVariableIndex (const symbolic::Variable &var) const
 Returns the index of the decision variable. More...
 
int num_indeterminates () const
 Gets the solution of an Eigen matrix of decision variables. More...
 
size_t FindIndeterminateIndex (const symbolic::Variable &var) const
 Returns the index of the indeterminate. More...
 
template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value, Eigen::Matrix< double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > >::type GetSolution (const Eigen::MatrixBase< Derived > &var) const
 Gets the solution of an Eigen matrix of decision variables. More...
 
double GetSolution (const symbolic::Variable &var) const
 Gets the value of a single decision variable. More...
 
template<typename C >
Eigen::VectorXd EvalBindingAtSolution (const Binding< C > &binding) const
 Evaluate the constraint in the Binding at the solution value. More...
 
const VectorXDecisionVariabledecision_variables () const
 Getter for all decision variables in the program. More...
 
const symbolic::Variabledecision_variable (int i) const
 Getter for the decision variable with index i in the program. More...
 
const VectorXIndeterminateindeterminates () const
 Getter for all indeterminates in the program. More...
 
const symbolic::Variableindeterminate (int i) const
 Getter for the indeterminate with index i in the program. More...
 
 MathematicalProgram (const MathematicalProgram &)=delete
 
MathematicalProgramoperator= (const MathematicalProgram &)=delete
 
 MathematicalProgram (MathematicalProgram &&)=delete
 
MathematicalProgramoperator= (MathematicalProgram &&)=delete
 

Additional Inherited Members

- Public Types inherited from MathematicalProgram
using VarType = symbolic::Variable::Type
 
- Static Public Member Functions inherited from MathematicalProgram
template<typename F >
static std::shared_ptr< CostMakeCost (F &&f)
 Convert an input of type F to a FunctionCost object. More...
 
- Protected Member Functions inherited from DirectTrajectoryOptimization
 DirectTrajectoryOptimization (int num_inputs, int num_states, int num_time_samples, double trajectory_time_lower_bound, double trajectory_time_upper_bound)
 Construct a DirectTrajectoryOptimization object. More...
 
std::vector< doubleGetTimeVector () const
 Returns a vector containing the elapsed time at each knot point. More...
 
std::vector< Eigen::MatrixXd > GetInputVector () const
 Returns a vector containing the input values at each knot point. More...
 
std::vector< Eigen::MatrixXd > GetStateVector () const
 Returns a vector containing the state values at each knot point. More...
 
symbolic::Expression SubstitutePlaceholderVariables (const symbolic::Expression &e, int interval_index) const
 Replaces e.g. More...
 
int num_inputs () const
 
int num_states () const
 
int N () const
 
const solvers::VectorXDecisionVariableh_vars () const
 
const solvers::VectorXDecisionVariableu_vars () const
 
const solvers::VectorXDecisionVariablex_vars () const
 

Detailed Description

DircolTrajectoryOptimization implements a direct colocation approach to trajectory optimization, as described in C.

R. Hargraves and S. W. Paris. Direct trajectory optimization using nonlinear programming and collocation. J Guidance, 10(4):338-342, July-August 1987. It assumes a first-order hold on the input trajectory and a cubic spline representation of the state trajectory, and adds dynamic constraints (and running costs) to the midpoints as well as the knot points in order to achieve a 3rd order integration accuracy.

Parameters
systemA dynamical system to be used in the dynamic constraints. This system must implement DoToAutoDiffXd. 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 knot points in the trajectory.
trajectory_time_lower_boundConstrains the minimum duration for the entire trajectory. Required to prevent trivial solutions (like total time = 0).
trajectory_time_upper_boundConstrains the maximum duration for the entire trajectory. Required to prevent trivial solutions and solutions with unbounded/unreasonable numerical integration errors.

Constructor & Destructor Documentation

DircolTrajectoryOptimization ( const System< double > *  system,
const Context< double > &  context,
int  num_time_samples,
double  trajectory_time_lower_bound,
double  trajectory_time_upper_bound 
)

Here is the call graph for this function:

~DircolTrajectoryOptimization ( )
inlineoverride

Here is the call graph for this function:

Member Function Documentation

PiecewisePolynomialTrajectory ReconstructStateTrajectory ( ) const
overridevirtual

Get the state trajectory as a PiecewisePolynomialTrajectory.

Reimplemented from DirectTrajectoryOptimization.

Here is the call graph for this function:

Here is the caller graph for this function:


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