crocoddyl  1.3.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 
21 template <typename _Scalar>
22 class ShootingProblemTpl {
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef ActionModelAbstractTpl<Scalar> ActionModelAbstract;
28  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
29  typedef MathBaseTpl<Scalar> MathBase;
30  typedef typename MathBase::VectorXs VectorXs;
31 
32  ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
33  boost::shared_ptr<ActionModelAbstract> terminal_model);
34  ~ShootingProblemTpl();
35 
36  Scalar calc(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
37  Scalar calcDiff(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
38  void rollout(const std::vector<VectorXs>& us, std::vector<VectorXs>& xs);
39  std::vector<VectorXs> rollout_us(const std::vector<VectorXs>& us);
40 
41  void updateModel(std::size_t i, boost::shared_ptr<ActionModelAbstract> model);
42 
43  const std::size_t& get_T() const;
44  const VectorXs& get_x0() const;
45  const std::vector<boost::shared_ptr<ActionModelAbstract> >& get_runningModels() const;
46  const boost::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
47  const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas() const;
48  const boost::shared_ptr<ActionDataAbstract>& get_terminalData() const;
49 
50  void set_x0(const VectorXs& x0_in);
51  void set_runningModels(const std::vector<boost::shared_ptr<ActionModelAbstract> >& models);
52  void set_terminalModel(boost::shared_ptr<ActionModelAbstract> model);
53 
54  protected:
55  Scalar cost_;
56  std::size_t T_;
57  VectorXs x0_;
58  boost::shared_ptr<ActionModelAbstract> terminal_model_;
59  boost::shared_ptr<ActionDataAbstract> terminal_data_;
60  std::vector<boost::shared_ptr<ActionModelAbstract> > running_models_;
61  std::vector<boost::shared_ptr<ActionDataAbstract> > running_datas_;
62 
63  private:
64  void allocateData();
65 };
66 
67 } // namespace crocoddyl
68 
69 /* --- Details -------------------------------------------------------------- */
70 /* --- Details -------------------------------------------------------------- */
71 /* --- Details -------------------------------------------------------------- */
72 #include "crocoddyl/core/optctrl/shooting.hxx"
73 
74 #endif // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_