Drake
DirectTrajectoryOptimization Class Referenceabstract

DirectTrajectoryOptimization is an abstract class for direct method approaches to trajectory optimization. More...

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

Inheritance diagram for DirectTrajectoryOptimization:
Collaboration diagram for DirectTrajectoryOptimization:

Public Member Functions

 ~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...
 
virtual PiecewisePolynomialTrajectory ReconstructStateTrajectory () const
 Get the state trajectory as a PiecewisePolynomialTrajectory. More...
 
Does not allow copy, move, or assignment
 DirectTrajectoryOptimization (const DirectTrajectoryOptimization &)=delete
 
DirectTrajectoryOptimizationoperator= (const DirectTrajectoryOptimization &)=delete
 
 DirectTrajectoryOptimization (DirectTrajectoryOptimization &&)=delete
 
DirectTrajectoryOptimizationoperator= (DirectTrajectoryOptimization &&)=delete
 
- Public Member Functions inherited from MathematicalProgram
 MathematicalProgram ()
 
virtual ~MathematicalProgram ()
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewVariables (VarType type, const std::array< std::string, rows *cols > &names)
 Adds new variables to MathematicalProgram. More...
 
template<int rows>
VectorDecisionVariable< rows > NewVariables (VarType type, const std::array< std::string, rows > &names)
 Adds column vector variables to the optimization program. More...
 
template<int rows>
MatrixDecisionVariable< rows, rows > NewSymmetricVariables (VarType type, const std::array< std::string, rows *(rows+1)/2 > &names)
 Adds symmetric matrix variables to optimization program. More...
 
VectorXDecisionVariable NewContinuousVariables (int rows, const std::vector< std::string > &names)
 Adds continuous variables to this MathematicalProgram. More...
 
VectorXDecisionVariable NewContinuousVariables (int rows, const std::string &name="x")
 Adds continuous variables to this MathematicalProgram, with default name "x". More...
 
MatrixXDecisionVariable NewContinuousVariables (int rows, int cols, const std::vector< std::string > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
MatrixXDecisionVariable NewContinuousVariables (int rows, int cols, const std::string &name="X")
 Adds continuous variables to this MathematicalProgram, with default name "X". More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewContinuousVariables (const std::array< std::string, rows *cols > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int rows, int cols>
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>
VectorDecisionVariable< rows > NewContinuousVariables (const std::array< std::string, rows > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int rows>
VectorDecisionVariable< rows > NewContinuousVariables (const std::string &name="x")
 Adds continuous variables to the program. More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewBinaryVariables (const std::array< std::string, rows *cols > &names)
 Adds binary variables, appending them to an internal vector of any existing vars. More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewBinaryVariables (const std::string &name="b")
 Adds a matrix of binary variables into the optimization program. More...
 
template<int rows>
VectorDecisionVariable< rows > NewBinaryVariables (const std::array< std::string, rows > &names)
 Adds vector of binary variables into the optimization program. More...
 
template<int rows>
VectorDecisionVariable< rows > NewBinaryVariables (const std::string &name="b")
 Adds vector of binary variables into the optimization program. More...
 
MatrixXDecisionVariable NewBinaryVariables (int rows, int cols, const std::vector< std::string > &names)
 Adds binary variables, appending them to an internal vector of any existing vars. More...
 
MatrixXDecisionVariable NewBinaryVariables (int rows, int cols, const std::string &name="b")
 Adds binary variables to this MathematicalProgram, with default name "b". More...
 
VectorXDecisionVariable NewBinaryVariables (int rows, const std::string &name="b")
 Adds binary variables to this MathematicalProgram. More...
 
MatrixXDecisionVariable NewSymmetricContinuousVariables (int rows, const std::vector< std::string > &names)
 Adds a symmetric matrix as decision 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...
 
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...
 
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 (SolverType solver_type, const std::string &solver_option, double option_value)
 Set an option for a particular solver. More...
 
void SetSolverOption (SolverType solver_type, const std::string &solver_option, int option_value)
 
void SetSolverOption (SolverType solver_type, const std::string &solver_option, const std::string &option_value)
 
const std::map< std::string, double > & GetSolverOptionsDouble (SolverType solver_type)
 
const std::map< std::string, int > & GetSolverOptionsInt (SolverType solver_type)
 
const std::map< std::string, std::string > & GetSolverOptionsStr (SolverType solver_type)
 
void GetSolverResult (SolverType *solver_type, int *solver_result) const
 Get the name and result code of the particular solver which was used to solve this MathematicalProgram. More...
 
void SetSolverResult (SolverType solver_type, int solver_result)
 
double GetOptimalCost () const
 Getter for optimal cost at the solution. More...
 
void SetOptimalCost (double optimal_cost)
 
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...
 
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...
 
 MathematicalProgram (const MathematicalProgram &)=delete
 
MathematicalProgramoperator= (const MathematicalProgram &)=delete
 
 MathematicalProgram (MathematicalProgram &&)=delete
 
MathematicalProgramoperator= (MathematicalProgram &&)=delete
 

Protected Member Functions

 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
 

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

Detailed Description

DirectTrajectoryOptimization is an abstract class for direct method approaches to trajectory optimization.

Subclasses must implement the abstract method: DoAddRunningCost() and should add any dynamic constraints in their constructor.

This class assumes that there are a fixed number (N) time steps/samples, and that the trajectory is discretized into timesteps h (N-1 of these), state x (N of these), and control input u (N of these).

To maintain nominal sparsity in the optimization programs, this implementation assumes that all constraints and costs are time-invariant.

Constructor & Destructor Documentation

Here is the caller graph for this function:

~DirectTrajectoryOptimization ( )
inlineoverride
DirectTrajectoryOptimization ( int  num_inputs,
int  num_states,
int  num_time_samples,
double  trajectory_time_lower_bound,
double  trajectory_time_upper_bound 
)
protected

Construct a DirectTrajectoryOptimization object.

The dimensions of the trajectory are established at construction, though other parameters (costs, bounds, constraints, etc) can be set before calling SolveTraj.

Parameters
num_inputsNumber of inputs at each sample point.
num_statesNumber of states at each sample point.
num_time_samplesNumber of time samples.
trajectory_time_lower_boundBound on total time for trajectory.
trajectory_time_upper_boundBound on total time for trajectory.

Here is the call graph for this function:

Member Function Documentation

void AddFinalCost ( std::shared_ptr< solvers::Cost cost)

Add a cost to the final state and total time.

Parameters
costA cost which expects total time as the first element of x when Eval is invoked, followed by the final state (num_states additional elements).

Here is the call graph for this function:

Here is the caller graph for this function:

void AddFinalCost ( const symbolic::Expression e)
inline

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.

Here is the call graph for this function:

void AddFinalCost ( const Eigen::Ref< const MatrixX< symbolic::Expression >> &  e)
inline

Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations.

Note: Derived classes will need to type using DirectTrajectoryOptimization::AddFinalCost; to "unhide" this method.

Here is the call graph for this function:

std::enable_if<solvers::detail::is_cost_functor_candidate<F>::value, std::shared_ptr<solvers::Cost> >::type AddFinalCostFunc ( F &&  f)
inline

Add a cost to the final state and total time.

Parameters
fA callable which meets the requirments of MathematicalProgram::AddCost().

Here is the call graph for this function:

Here is the caller graph for this function:

void AddInputBounds ( const Eigen::VectorXd &  lower_bound,
const Eigen::VectorXd &  upper_bound 
)

Add upper and lower bounds on the input values.

Calling this function multiple times will add additional bounds on the input rather than resetting any bounds from previous invocations.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddInputConstraint ( std::shared_ptr< ConstraintT >  constraint,
const std::vector< int > &  time_indices 
)
inline

Add a constraint on the input at the specified time indices.

Parameters
constraintThe constraint to be applied.
time_indicesApply the constraints only at these time indices (zero offset).

Here is the call graph for this function:

void AddRunningCost ( const symbolic::Expression g)
inline

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.

The particular integration scheme is determined by the derived class implementation.

Here is the caller graph for this function:

void AddRunningCost ( const Eigen::MatrixBase< Derived > &  g)
inline

Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations.

void AddRunningCost ( std::shared_ptr< solvers::Cost cost)
inline

Here is the call graph for this function:

std::enable_if<solvers::detail::is_cost_functor_candidate<F>::value, std::shared_ptr<solvers::Cost> >::type AddRunningCostFunc ( F &&  f)
inline

Adds an integrated cost to all time steps.

Parameters
fA callable which meets the requirments of MathematicalProgram::AddCost().

Here is the call graph for this function:

Here is the caller graph for this function:

void AddStateConstraint ( std::shared_ptr< ConstraintT >  constraint,
const std::vector< int > &  time_indices 
)
inline

Add a constraint on the state at the specified time indices.

Parameters
constraintThe constraint to be applied.
time_indicesApply the constraints only at these time indices (zero offset).

Here is the call graph for this function:

Here is the caller graph for this function:

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

Parameters
lower_boundA vector of lower bounds.
upper_boundA vector of upper bounds.
interval_indicesA vector of interval indices.

Here is the call graph for this function:

Here is the caller graph for this function:

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

Parameters
lower_boundA vector of lower bounds.
upper_boundA vector of upper bounds.

Here is the call graph for this function:

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.

Parameters
lower_boundA scalar double lower bound.
upper_boundA scalar double upper bound.

Here is the call graph for this function:

Eigen::VectorBlock<const solvers::VectorXDecisionVariable> final_state ( ) const
inline

Returns the decision variables associated with the state, x, at the final time index.

Here is the call graph for this function:

Here is the caller graph for this function:

std::vector< Eigen::MatrixXd > GetInputVector ( ) const
protected

Returns a vector containing the input values at each knot point.

Here is the call graph for this function:

Here is the caller graph for this function:

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.

Output matrices contain one set of input/state per column, and the number of columns is equal to the number of time samples. times will be populated with the times corresponding to each column.

Here is the call graph for this function:

Here is the caller graph for this function:

std::vector< Eigen::MatrixXd > GetStateVector ( ) const
protected

Returns a vector containing the state values at each knot point.

Here is the call graph for this function:

Here is the caller graph for this function:

std::vector< double > GetTimeVector ( ) const
protected

Returns a vector containing the elapsed time at each knot point.

Here is the call graph for this function:

Here is the caller graph for this function:

const solvers::VectorXDecisionVariable& h_vars ( ) const
inlineprotected

Here is the caller graph for this function:

Eigen::VectorBlock<const solvers::VectorXDecisionVariable> initial_state ( ) const
inline

Returns the decision variables associated with the state, x, at the initial time index.

Here is the call graph for this function:

Here is the caller graph for this function:

const solvers::VectorXDecisionVariable& input ( ) const
inline

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.

Here is the caller graph for this function:

Eigen::VectorBlock<const solvers::VectorXDecisionVariable> input ( int  index) const
inline

Returns the decision variables associated with the input, u, at time index index.

int N ( ) const
inlineprotected

Here is the caller graph for this function:

int num_inputs ( ) const
inlineprotected

Here is the caller graph for this function:

int num_states ( ) const
inlineprotected

Here is the caller graph for this function:

PiecewisePolynomialTrajectory ReconstructInputTrajectory ( ) const

Get the input trajectory as a PiecewisePolynomialTrajectory.

Here is the call graph for this function:

Here is the caller graph for this function:

PiecewisePolynomialTrajectory ReconstructStateTrajectory ( ) const
virtual

Get the state trajectory as a PiecewisePolynomialTrajectory.

Reimplemented in DircolTrajectoryOptimization.

Here is the call graph for this function:

Here is the caller graph for this function:

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.

Parameters
timespan_initThe initial guess for the timespan of the resulting trajectory.
traj_init_uInitial 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).
traj_init_xInitial 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).

Here is the call graph for this function:

Here is the caller graph for this function:

const solvers::VectorXDecisionVariable& state ( ) const
inline

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.

Here is the caller graph for this function:

Eigen::VectorBlock<const solvers::VectorXDecisionVariable> state ( int  index) const
inline

Returns the decision variables associated with the state, x, at time index index.

symbolic::Expression SubstitutePlaceholderVariables ( const symbolic::Expression e,
int  interval_index 
) const
protected

Replaces e.g.

placeholder_x_var_ with x_vars_ at time interval interval_index, for all placeholder variables.

Here is the call graph for this function:

Here is the caller graph for this function:

const solvers::VectorDecisionVariable<1>& time ( ) const
inline

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.

Here is the caller graph for this function:

const solvers::VectorDecisionVariable<1> timestep ( int  index) const
inline

Returns the decision variable associated with the timestep, h, at time index index.

const solvers::VectorXDecisionVariable& u_vars ( ) const
inlineprotected

Here is the caller graph for this function:

const solvers::VectorXDecisionVariable& x_vars ( ) const
inlineprotected

Here is the caller graph for this function:


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