crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
shooting.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
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 
109  std::vector<VectorXs> rollout_us(const std::vector<VectorXs>& us);
110 
119  void circularAppend(boost::shared_ptr<ActionModelAbstract> model, boost::shared_ptr<ActionDataAbstract> data);
120 
129  void circularAppend(boost::shared_ptr<ActionModelAbstract> model);
130 
138  void updateNode(std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
139  boost::shared_ptr<ActionDataAbstract> data);
140 
147  void updateModel(std::size_t i, boost::shared_ptr<ActionModelAbstract> model);
148 
152  const std::size_t& get_T() const;
153 
157  const VectorXs& get_x0() const;
158 
162  const std::vector<boost::shared_ptr<ActionModelAbstract> >& get_runningModels() const;
163 
167  const boost::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
168 
172  const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas() const;
173 
177  const boost::shared_ptr<ActionDataAbstract>& get_terminalData() const;
178 
182  void set_x0(const VectorXs& x0_in);
183 
187  void set_runningModels(const std::vector<boost::shared_ptr<ActionModelAbstract> >& models);
188 
192  void set_terminalModel(boost::shared_ptr<ActionModelAbstract> model);
193 
197  const std::size_t& get_nx() const;
198 
202  const std::size_t& get_ndx() const;
203 
207  const std::size_t& get_nu_max() const;
208 
209  protected:
210  Scalar cost_;
211  std::size_t T_;
212  VectorXs x0_;
213  boost::shared_ptr<ActionModelAbstract> terminal_model_;
214  boost::shared_ptr<ActionDataAbstract> terminal_data_;
215  std::vector<boost::shared_ptr<ActionModelAbstract> > running_models_;
216  std::vector<boost::shared_ptr<ActionDataAbstract> > running_datas_;
217  std::size_t nx_;
218  std::size_t ndx_;
219  std::size_t nu_max_;
220 
221  private:
222  void allocateData();
223 };
224 
225 } // namespace crocoddyl
226 
227 /* --- Details -------------------------------------------------------------- */
228 /* --- Details -------------------------------------------------------------- */
229 /* --- Details -------------------------------------------------------------- */
230 #include "crocoddyl/core/optctrl/shooting.hxx"
231 
232 #endif // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
std::vector< boost::shared_ptr< ActionDataAbstract > > running_datas_
Running action data.
Definition: shooting.hpp:216
std::size_t nx_
State dimension.
Definition: shooting.hpp:217
void updateNode(std::size_t i, boost::shared_ptr< ActionModelAbstract > model, boost::shared_ptr< ActionDataAbstract > data)
Update the model and data for a specific node.
VectorXs x0_
Initial state.
Definition: shooting.hpp:212
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:214
const std::vector< boost::shared_ptr< ActionDataAbstract > > & get_runningDatas() const
Return the running datas.
const std::size_t & get_T() const
Return the number of running nodes.
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:210
const std::size_t & get_nx() const
Return the dimension of the state tuple.
void updateModel(std::size_t i, boost::shared_ptr< ActionModelAbstract > model)
Update a model and allocated new data for a specific node.
std::size_t T_
number of running nodes
Definition: shooting.hpp:211
const boost::shared_ptr< ActionDataAbstract > & get_terminalData() const
Return the terminal data.
std::size_t nu_max_
Maximun control dimension.
Definition: shooting.hpp:219
std::size_t ndx_
State rate dimension.
Definition: shooting.hpp:218
const std::size_t & get_ndx() const
Return the dimension of the tangent space of the state manifold.
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_runningModels(const std::vector< boost::shared_ptr< ActionModelAbstract > > &models)
Modify the running models and allocate new data.
const std::size_t & get_nu_max() const
Return the maximun dimension of the control vector.
std::vector< boost::shared_ptr< ActionModelAbstract > > running_models_
Running action model.
Definition: shooting.hpp:215
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:213
const VectorXs & get_x0() const
Return the initial state.
void set_terminalModel(boost::shared_ptr< ActionModelAbstract > model)
Modify the terminal model and allocate new data.