crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-impulses.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_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
11 
12 #include <string>
13 #include <map>
14 #include <utility>
15 
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 #include "crocoddyl/multibody/impulse-base.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
23 struct ImpulseItemTpl {
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
28 
29  ImpulseItemTpl() {}
30  ImpulseItemTpl(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, bool active = true)
31  : name(name), impulse(impulse), active(active) {}
32 
33  std::string name;
34  boost::shared_ptr<ImpulseModelAbstract> impulse;
35  bool active;
36 };
37 
38 template <typename _Scalar>
39 class ImpulseModelMultipleTpl {
40  public:
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
43  typedef _Scalar Scalar;
44  typedef MathBaseTpl<Scalar> MathBase;
45  typedef StateMultibodyTpl<Scalar> StateMultibody;
46  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
47  typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
48  typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
49 
50  typedef ImpulseItemTpl<Scalar> ImpulseItem;
51 
52  typedef typename MathBase::Vector2s Vector2s;
53  typedef typename MathBase::Vector3s Vector3s;
54  typedef typename MathBase::VectorXs VectorXs;
55  typedef typename MathBase::MatrixXs MatrixXs;
56 
57  typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
58  typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
59  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
60 
61  explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
62  ~ImpulseModelMultipleTpl();
63 
64  void addImpulse(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, bool active = true);
65  void removeImpulse(const std::string& name);
66  void changeImpulseStatus(const std::string& name, bool active);
67 
68  void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
69  void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
70 
71  void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& vnext) const;
72  void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& force);
73  boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
74  void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& dvnext_dx) const;
75  void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& df_dx) const;
76 
77  const boost::shared_ptr<StateMultibody>& get_state() const;
78  const ImpulseModelContainer& get_impulses() const;
79  const std::size_t& get_ni() const;
80  const std::size_t& get_ni_total() const;
81  const std::vector<std::string>& get_active() const;
82  const std::vector<std::string>& get_inactive() const;
83  bool getImpulseStatus(const std::string& name) const;
84 
85  private:
86  boost::shared_ptr<StateMultibody> state_;
87  ImpulseModelContainer impulses_;
88  std::size_t ni_;
89  std::size_t ni_total_;
90  std::vector<std::string> active_;
91  std::vector<std::string> inactive_;
92 };
93 
94 template <typename _Scalar>
95 struct ImpulseDataMultipleTpl {
96  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
97 
98  typedef _Scalar Scalar;
99  typedef MathBaseTpl<Scalar> MathBase;
100  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
101  typedef ImpulseItemTpl<Scalar> ImpulseItem;
102  typedef typename MathBase::VectorXs VectorXs;
103  typedef typename MathBase::MatrixXs MatrixXs;
104 
105  template <template <typename Scalar> class Model>
106  ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
107  : Jc(model->get_ni_total(), model->get_state()->get_nv()),
108  dv0_dq(model->get_ni_total(), model->get_state()->get_nv()),
109  vnext(model->get_state()->get_nv()),
110  dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
111  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
112  Jc.setZero();
113  dv0_dq.setZero();
114  vnext.setZero();
115  dvnext_dx.setZero();
116  for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
117  it != model->get_impulses().end(); ++it) {
118  const boost::shared_ptr<ImpulseItem>& item = it->second;
119  impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
120  }
121  }
122 
123  MatrixXs Jc;
124  MatrixXs dv0_dq;
125  VectorXs vnext;
126  MatrixXs dvnext_dx;
127  typename ImpulseModelMultiple::ImpulseDataContainer impulses;
128  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
129 };
130 
131 } // namespace crocoddyl
132 
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 /* --- Details -------------------------------------------------------------- */
136 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
137 
138 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_