crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
shooting.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
10 #define CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
11 
12 #include <stdexcept>
13 #include <vector>
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 #include "crocoddyl/core/action-base.hpp"
17 #include "crocoddyl/core/utils/to-string.hpp"
18 
19 namespace crocoddyl {
20 
30 template <typename _Scalar>
31 class ShootingProblemTpl {
32  public:
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 
35  typedef _Scalar Scalar;
36  typedef ActionModelAbstractTpl<Scalar> ActionModelAbstract;
37  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
38  typedef MathBaseTpl<Scalar> MathBase;
39  typedef typename MathBase::VectorXs VectorXs;
40 
48  ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
49  boost::shared_ptr<ActionModelAbstract> terminal_model);
50 
60  ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
61  boost::shared_ptr<ActionModelAbstract> terminal_model,
62  const std::vector<boost::shared_ptr<ActionDataAbstract> >& running_datas,
63  boost::shared_ptr<ActionDataAbstract> terminal_data);
67  ShootingProblemTpl(const ShootingProblemTpl<Scalar>& problem);
69 
80  Scalar calc(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
81 
94  Scalar calcDiff(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
95 
102  void rollout(const std::vector<VectorXs>& us, std::vector<VectorXs>& xs);
103 
110  std::vector<VectorXs> rollout_us(const std::vector<VectorXs>& us);
111 
118  void quasiStatic(std::vector<VectorXs>& us, const std::vector<VectorXs>& xs);
119 
126  std::vector<VectorXs> quasiStatic_xs(const std::vector<VectorXs>& xs);
127 
136  void circularAppend(boost::shared_ptr<ActionModelAbstract> model, boost::shared_ptr<ActionDataAbstract> data);
137 
146  void circularAppend(boost::shared_ptr<ActionModelAbstract> model);
147 
155  void updateNode(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
156  boost::shared_ptr<ActionDataAbstract> data);
157 
164  void updateModel(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model);
165 
169  std::size_t get_T() const;
170 
174  const VectorXs& get_x0() const;
175 
179  const std::vector<boost::shared_ptr<ActionModelAbstract> >& get_runningModels() const;
180 
184  const boost::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
185 
189  const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas() const;
190 
194  const boost::shared_ptr<ActionDataAbstract>& get_terminalData() const;
195 
199  void set_x0(const VectorXs& x0_in);
200 
204  void set_runningModels(const std::vector<boost::shared_ptr<ActionModelAbstract> >& models);
205 
209  void set_terminalModel(boost::shared_ptr<ActionModelAbstract> model);
210 
216  void set_nthreads(const int nthreads);
217 
221  std::size_t get_nx() const;
222 
226  std::size_t get_ndx() const;
227 
231  std::size_t get_nu_max() const;
232 
236  std::size_t get_nthreads() const;
237 
241  template <class Scalar>
242  friend std::ostream& operator<<(std::ostream& os, const ShootingProblemTpl<Scalar>& problem);
243 
244  protected:
245  Scalar cost_;
246  std::size_t T_;
247  VectorXs x0_;
248  boost::shared_ptr<ActionModelAbstract> terminal_model_;
249  boost::shared_ptr<ActionDataAbstract> terminal_data_;
250  std::vector<boost::shared_ptr<ActionModelAbstract> > running_models_;
251  std::vector<boost::shared_ptr<ActionDataAbstract> > running_datas_;
252  std::size_t nx_;
253  std::size_t ndx_;
254  std::size_t nu_max_;
255  std::size_t nthreads_;
256 
257  private:
258  void allocateData();
259 };
260 
261 } // namespace crocoddyl
262 
263 /* --- Details -------------------------------------------------------------- */
264 /* --- Details -------------------------------------------------------------- */
265 /* --- Details -------------------------------------------------------------- */
266 #include "crocoddyl/core/optctrl/shooting.hxx"
267 
268 #endif // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
std::vector< boost::shared_ptr< ActionDataAbstract > > running_datas_
Running action data.
Definition: shooting.hpp:251
std::size_t nx_
State dimension.
Definition: shooting.hpp:252
std::size_t get_ndx() const
Return the dimension of the tangent space of the state manifold.
std::size_t nthreads_
Number of threach launch by the multi-threading application.
Definition: shooting.hpp:255
VectorXs x0_
Initial state.
Definition: shooting.hpp:247
std::vector< VectorXs > quasiStatic_xs(const std::vector< VectorXs > &xs)
void updateNode(const std::size_t i, boost::shared_ptr< ActionModelAbstract > model, boost::shared_ptr< ActionDataAbstract > data)
Update the model and data for a specific node.
void rollout(const std::vector< VectorXs > &us, std::vector< VectorXs > &xs)
Integrate the dynamics given a control sequence.
boost::shared_ptr< ActionDataAbstract > terminal_data_
Terminal action data.
Definition: shooting.hpp:249
const std::vector< boost::shared_ptr< ActionDataAbstract > > & get_runningDatas() const
Return the running datas.
void set_x0(const VectorXs &x0_in)
Modify the initial state.
void circularAppend(boost::shared_ptr< ActionModelAbstract > model, boost::shared_ptr< ActionDataAbstract > data)
Circular append of the model and data onto the end running node.
Scalar cost_
Total cost.
Definition: shooting.hpp:245
std::size_t T_
number of running nodes
Definition: shooting.hpp:246
const boost::shared_ptr< ActionDataAbstract > & get_terminalData() const
Return the terminal data.
std::size_t nu_max_
Maximum control dimension.
Definition: shooting.hpp:254
std::size_t get_nu_max() const
Return the maximum dimension of the control vector.
std::size_t get_nthreads() const
Return the number of threads.
std::size_t ndx_
State rate dimension.
Definition: shooting.hpp:253
const std::vector< boost::shared_ptr< ActionModelAbstract > > & get_runningModels() const
Return the running models.
Scalar calc(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us)
Compute the cost and the next states.
void set_nthreads(const int nthreads)
Modify the number of threads using with multithreading support.
void set_runningModels(const std::vector< boost::shared_ptr< ActionModelAbstract > > &models)
Modify the running models and allocate new data.
void updateModel(const std::size_t i, boost::shared_ptr< ActionModelAbstract > model)
Update a model and allocated new data for a specific node.
std::vector< boost::shared_ptr< ActionModelAbstract > > running_models_
Running action model.
Definition: shooting.hpp:250
Scalar calcDiff(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us)
Compute the derivatives of the cost and dynamics.
const boost::shared_ptr< ActionModelAbstract > & get_terminalModel() const
Return the terminal model.
ShootingProblemTpl(const VectorXs &x0, const std::vector< boost::shared_ptr< ActionModelAbstract > > &running_models, boost::shared_ptr< ActionModelAbstract > terminal_model)
Initialize the shooting problem and allocate its data.
std::vector< VectorXs > rollout_us(const std::vector< VectorXs > &us)
boost::shared_ptr< ActionModelAbstract > terminal_model_
Terminal action model.
Definition: shooting.hpp:248
const VectorXs & get_x0() const
Return the initial state.
std::size_t get_nx() const
Return the dimension of the state tuple.
std::size_t get_T() const
Return the number of running nodes.
void quasiStatic(std::vector< VectorXs > &us, const std::vector< VectorXs > &xs)
Compute the quasic static commands given a state trajectory.
void set_terminalModel(boost::shared_ptr< ActionModelAbstract > model)
Modify the terminal model and allocate new data.