Drake
direct_trajectory_optimization.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 #include <vector>
6 
7 #include <Eigen/Core>
8 
11 #include "drake/common/symbolic.h"
17 
18 namespace drake {
19 namespace systems {
20 
38  public:
40 
42 
46  DRAKE_DEMAND(index >= 0 && index < N_);
47  return h_vars_.segment<1>(index);
48  }
49 
57  return placeholder_t_var_;
58  }
59 
67  return placeholder_x_vars_;
68  }
69 
77  return placeholder_u_vars_;
78  }
79 
82  Eigen::VectorBlock<const solvers::VectorXDecisionVariable> state(
83  int index) const {
84  DRAKE_DEMAND(index >= 0 && index < N_);
85  return x_vars_.segment(index * num_states_, num_states_);
86  }
87 
90  Eigen::VectorBlock<const solvers::VectorXDecisionVariable> initial_state()
91  const {
92  return state(0);
93  }
94 
97  Eigen::VectorBlock<const solvers::VectorXDecisionVariable> final_state()
98  const {
99  return state(N_ - 1);
100  }
101 
104  Eigen::VectorBlock<const solvers::VectorXDecisionVariable> input(
105  int index) const {
106  DRAKE_DEMAND(index >= 0 && index < N_);
107  return u_vars_.segment(index * num_inputs_, num_inputs_);
108  }
109 
116  void AddRunningCost(const symbolic::Expression& g) { DoAddRunningCost(g); }
117 
120  template <typename Derived>
121  void AddRunningCost(const Eigen::MatrixBase<Derived>& g) {
122  DRAKE_DEMAND(g.rows() == 1 && g.cols() == 1);
123  DoAddRunningCost(g(0, 0));
124  }
125 
132  template <typename F>
134  std::shared_ptr<solvers::Cost>>::type
136  auto c = solvers::MakeFunctionCost(std::forward<F>(f));
137  AddRunningCost(c);
138  return c;
139  }
140 
141  // TODO(russt): Remove the methods below that add constraints without
142  // requesting the variable list to be passed in. We should force the user
143  // to specify the variables to provide clarity and robustness.
144  // The "add to a list of times" utilities are still useful... we can replace
145  // them with methods of the form AddCost(..., MatrixXDecisionVariable), which
146  // adds the same constraint to each column of the variable matrix.
147 
148  // Adds an integrated cost to all time steps.
149  //
150  // @param constraint A constraint which expects a timestep, state, and input
151  // as the elements of x when Eval is invoked.
152  void AddRunningCost(std::shared_ptr<solvers::Cost> cost) {
153  DoAddRunningCost(cost);
154  }
155 
161  void AddInputBounds(const Eigen::VectorXd& lower_bound,
162  const Eigen::VectorXd& upper_bound);
163 
172  template <typename ConstraintT>
173  void AddInputConstraint(std::shared_ptr<ConstraintT> constraint,
174  const std::vector<int>& time_indices) {
175  for (const int i : time_indices) {
176  DRAKE_ASSERT(i < N_);
177  AddConstraint(constraint, u_vars_.segment(i * num_inputs_, num_inputs_));
178  }
179  }
180 
189  template <typename ConstraintT>
190  void AddStateConstraint(std::shared_ptr<ConstraintT> constraint,
191  const std::vector<int>& time_indices) {
192  for (const int i : time_indices) {
193  DRAKE_ASSERT(i < N_);
194  AddConstraint(constraint, x_vars_.segment(i * num_states_, num_states_));
195  }
196  }
197 
208  void AddTimeIntervalBounds(const Eigen::VectorXd& lower_bound,
209  const Eigen::VectorXd& upper_bound,
210  const std::vector<int>& interval_indices);
211 
221  void AddTimeIntervalBounds(const Eigen::VectorXd& lower_bound,
222  const Eigen::VectorXd& upper_bound);
223 
231  void AddTimeIntervalBounds(double lower_bound, double upper_bound);
232 
240  void AddFinalCost(std::shared_ptr<solvers::Cost> cost);
241 
249  }
250 
256  void AddFinalCost(const Eigen::Ref<const MatrixX<symbolic::Expression>>& e) {
257  DRAKE_DEMAND(e.rows() == 1 && e.cols() == 1);
258  AddFinalCost(e(0, 0));
259  }
260 
267  template <typename F>
269  std::shared_ptr<solvers::Cost>>::type
271  auto c = solvers::MakeFunctionCost(std::forward<F>(f));
272  AddFinalCost(c);
273  return c;
274  }
275 
291  double timespan_init, const PiecewisePolynomial<double>& traj_init_u,
292  const PiecewisePolynomial<double>& traj_init_x);
293  // TODO(Lucy-tri) If timespan_init has any relationship to
294  // trajectory_time_{lower,upper}_bound, then add doc and asserts.
295 
303  void GetResultSamples(Eigen::MatrixXd* inputs, Eigen::MatrixXd* states,
304  std::vector<double>* times) const;
305 
310 
315 
316  protected:
331  // TODO(russt): update comment above. Note that system/context are cloned
332  // internally... must use accessors to make any changes to the system.
334  int num_time_samples,
335  double trajectory_time_lower_bound,
336  double trajectory_time_upper_bound);
337  // TODO(Lucy-tri) add param to indicate whether time steps are constant or
338  // independent, and modify implementation to handle.
339 
343  std::vector<double> GetTimeVector() const;
344 
348  std::vector<Eigen::MatrixXd> GetInputVector() const;
349 
353  std::vector<Eigen::MatrixXd> GetStateVector() const;
354 
360  const symbolic::Expression& e, int interval_index) const;
361 
362  int num_inputs() const { return num_inputs_; }
363  int num_states() const { return num_states_; }
364  int N() const { return N_; }
365  const solvers::VectorXDecisionVariable& h_vars() const { return h_vars_; }
366  const solvers::VectorXDecisionVariable& u_vars() const { return u_vars_; }
367  const solvers::VectorXDecisionVariable& x_vars() const { return x_vars_; }
368 
369  private:
384  void GetInitialVars(double timespan_init_in,
385  const PiecewisePolynomial<double>& traj_init_u,
386  const PiecewisePolynomial<double>& traj_init_x);
387 
388  virtual void DoAddRunningCost(const symbolic::Expression& g) = 0;
389 
390  virtual void DoAddRunningCost(std::shared_ptr<solvers::Cost> cost) = 0;
391 
392  const int num_inputs_{};
393  const int num_states_{};
394  const int N_{}; // Number of time samples
395 
396  solvers::VectorXDecisionVariable h_vars_; // Time deltas between each
397  // input/state sample.
400 
401  // See description of the public time(), state(), and input() accessor methods
402  // for details about the placeholder variables.
403  solvers::VectorDecisionVariable<1> placeholder_t_var_;
404  solvers::VectorXDecisionVariable placeholder_x_vars_;
405  solvers::VectorXDecisionVariable placeholder_u_vars_;
406 };
407 
408 } // namespace systems
409 } // namespace drake
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.
Definition: direct_trajectory_optimization.h:135
std::vector< snopt::doublereal > F
Definition: snopt_solver.cc:57
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 l...
Definition: direct_trajectory_optimization.h:256
A PiecewisePolynomialTrajectory is a Trajectory that is represented by (implemented in terms of) a Pi...
Definition: piecewise_polynomial_trajectory.h:17
int g
Definition: rgbd_camera.cc:90
const solvers::VectorXDecisionVariable & state() const
Returns placeholder decision variables (not actually declared as decision variables in the Mathematic...
Definition: direct_trajectory_optimization.h:66
int num_states() const
Definition: direct_trajectory_optimization.h:363
int num_inputs() const
Definition: direct_trajectory_optimization.h:362
virtual PiecewisePolynomialTrajectory ReconstructStateTrajectory() const
Get the state trajectory as a PiecewisePolynomialTrajectory.
Definition: direct_trajectory_optimization.cc:274
NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseK...
Definition: automotive_demo.cc:88
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.
Definition: direct_trajectory_optimization.cc:177
DirectTrajectoryOptimization(const DirectTrajectoryOptimization &)=delete
~DirectTrajectoryOptimization() override
Definition: direct_trajectory_optimization.h:41
void AddRunningCost(const Eigen::MatrixBase< Derived > &g)
Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic l...
Definition: direct_trajectory_optimization.h:121
void AddInputBounds(const Eigen::VectorXd &lower_bound, const Eigen::VectorXd &upper_bound)
Add upper and lower bounds on the input values.
Definition: direct_trajectory_optimization.cc:57
symbolic::Expression SubstitutePlaceholderVariables(const symbolic::Expression &e, int interval_index) const
Replaces e.g.
Definition: direct_trajectory_optimization.cc:228
std::vector< double > GetTimeVector() const
Returns a vector containing the elapsed time at each knot point.
Definition: direct_trajectory_optimization.cc:190
DirectTrajectoryOptimization is an abstract class for direct method approaches to trajectory optimiza...
Definition: direct_trajectory_optimization.h:37
void AddRunningCost(std::shared_ptr< solvers::Cost > cost)
Definition: direct_trajectory_optimization.h:152
const Constraint * constraint
Definition: nlopt_solver.cc:102
Eigen::VectorBlock< const solvers::VectorXDecisionVariable > input(int index) const
Returns the decision variables associated with the input, u, at time index index. ...
Definition: direct_trajectory_optimization.h:104
const solvers::VectorDecisionVariable< 1 > timestep(int index) const
Returns the decision variable associated with the timestep, h, at time index index.
Definition: direct_trajectory_optimization.h:45
const solvers::VectorXDecisionVariable & x_vars() const
Definition: direct_trajectory_optimization.h:367
void AddStateConstraint(std::shared_ptr< ConstraintT > constraint, const std::vector< int > &time_indices)
Add a constraint on the state at the specified time indices.
Definition: direct_trajectory_optimization.h:190
const solvers::VectorXDecisionVariable & input() const
Returns placeholder decision variables (not actually declared as decision variables in the Mathematic...
Definition: direct_trajectory_optimization.h:76
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:39
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:79
int value
Definition: copyable_unique_ptr_test.cc:61
Binding< Cost > AddCost(const Binding< Cost > &binding)
Adds a generic cost to the optimization program.
Definition: mathematical_program.cc:218
Provides Drake&#39;s assertion implementation.
void AddFinalCost(std::shared_ptr< solvers::Cost > cost)
Add a cost to the final state and total time.
Definition: direct_trajectory_optimization.cc:139
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:47
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.
Definition: direct_trajectory_optimization.cc:246
int N() const
Definition: direct_trajectory_optimization.h:364
SolutionResult
Definition: mathematical_program_solver_interface.h:13
std::vector< Eigen::MatrixXd > GetInputVector() const
Returns a vector containing the input values at each knot point.
Definition: direct_trajectory_optimization.cc:201
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]) <= up...
Definition: direct_trajectory_optimization.cc:76
Binding< Constraint > AddConstraint(const Binding< Constraint > &binding)
Adds a generic constraint to the program.
Definition: mathematical_program.cc:298
Represents a symbolic form of an expression.
Definition: symbolic_expression.h:171
std::vector< Eigen::MatrixXd > GetStateVector() const
Returns a vector containing the state values at each knot point.
Definition: direct_trajectory_optimization.cc:214
Eigen::VectorBlock< const solvers::VectorXDecisionVariable > state(int index) const
Returns the decision variables associated with the state, x, at time index index. ...
Definition: direct_trajectory_optimization.h:82
VectorDecisionVariable< Eigen::Dynamic > VectorXDecisionVariable
Definition: decision_variable.h:17
void AddRunningCost(const symbolic::Expression &g)
Adds an integrated cost to all time steps, of the form where any instances of time(), state(), and/or input() placeholder variables are substituted with the relevant variables for each current time index.
Definition: direct_trajectory_optimization.h:116
Eigen::VectorBlock< const solvers::VectorXDecisionVariable > initial_state() const
Returns the decision variables associated with the state, x, at the initial time index.
Definition: direct_trajectory_optimization.h:90
const solvers::VectorXDecisionVariable & h_vars() const
Definition: direct_trajectory_optimization.h:365
const solvers::VectorXDecisionVariable & u_vars() const
Definition: direct_trajectory_optimization.h:366
MatrixDecisionVariable< rows, 1 > VectorDecisionVariable
Definition: decision_variable.h:14
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.
Definition: direct_trajectory_optimization.h:270
void AddFinalCost(const symbolic::Expression &e)
Adds a cost to the final time, of the form where any instances of time(), state(), and/or input() placeholder variables are substituted with the relevant variables for each current time index.
Definition: direct_trajectory_optimization.h:247
Eigen::VectorBlock< const solvers::VectorXDecisionVariable > final_state() const
Returns the decision variables associated with the state, x, at the final time index.
Definition: direct_trajectory_optimization.h:97
const solvers::VectorDecisionVariable< 1 > & time() const
Returns a placeholder decision variable (not actually declared as a decision variable in the Mathemat...
Definition: direct_trajectory_optimization.h:56
void AddInputConstraint(std::shared_ptr< ConstraintT > constraint, const std::vector< int > &time_indices)
Add a constraint on the input at the specified time indices.
Definition: direct_trajectory_optimization.h:173
Provides public header files of Drake&#39;s symbolic library.
Definition: mathematical_program.h:266
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:35
PiecewisePolynomialTrajectory ReconstructInputTrajectory() const
Get the input trajectory as a PiecewisePolynomialTrajectory.
Definition: direct_trajectory_optimization.cc:267
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
std::shared_ptr< Cost > MakeFunctionCost(F &&f)
Converts an input of type F to a FunctionCost object.
Definition: cost.h:321