crocoddyl  1.5.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 
45 template <typename _Scalar>
46 class ImpulseModelMultipleTpl {
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef _Scalar Scalar;
51  typedef MathBaseTpl<Scalar> MathBase;
52  typedef StateMultibodyTpl<Scalar> StateMultibody;
53  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
54  typedef ImpulseDataMultipleTpl<Scalar> ImpulseDataMultiple;
55  typedef ImpulseModelAbstractTpl<Scalar> ImpulseModelAbstract;
56 
57  typedef ImpulseItemTpl<Scalar> ImpulseItem;
58 
59  typedef typename MathBase::Vector2s Vector2s;
60  typedef typename MathBase::Vector3s Vector3s;
61  typedef typename MathBase::VectorXs VectorXs;
62  typedef typename MathBase::MatrixXs MatrixXs;
63 
64  typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
65  typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
66  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
67 
73  explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
74  ~ImpulseModelMultipleTpl();
75 
85  void addImpulse(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, bool active = true);
86 
92  void removeImpulse(const std::string& name);
93 
100  void changeImpulseStatus(const std::string& name, bool active);
101 
108  void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
109 
116  void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
117 
124  void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& vnext) const;
125 
133  void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& impulse);
134 
142  void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& dvnext_dx) const;
143 
151  void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& df_dx) const;
152 
159  boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
160 
164  const boost::shared_ptr<StateMultibody>& get_state() const;
165 
169  const ImpulseModelContainer& get_impulses() const;
170 
174  const std::size_t& get_ni() const;
175 
179  const std::size_t& get_ni_total() const;
180 
184  const std::vector<std::string>& get_active() const;
185 
189  const std::vector<std::string>& get_inactive() const;
190 
194  bool getImpulseStatus(const std::string& name) const;
195 
196  private:
197  boost::shared_ptr<StateMultibody> state_;
198  ImpulseModelContainer impulses_;
199  std::size_t ni_;
200  std::size_t ni_total_;
201  std::vector<std::string> active_;
202  std::vector<std::string> inactive_;
203 };
204 
210 template <typename _Scalar>
211 struct ImpulseDataMultipleTpl {
212  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
213 
214  typedef _Scalar Scalar;
215  typedef MathBaseTpl<Scalar> MathBase;
216  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
217  typedef ImpulseItemTpl<Scalar> ImpulseItem;
218  typedef typename MathBase::VectorXs VectorXs;
219  typedef typename MathBase::MatrixXs MatrixXs;
220 
227  template <template <typename Scalar> class Model>
228  ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
229  : Jc(model->get_ni_total(), model->get_state()->get_nv()),
230  dv0_dq(model->get_ni_total(), model->get_state()->get_nv()),
231  vnext(model->get_state()->get_nv()),
232  dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
233  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
234  Jc.setZero();
235  dv0_dq.setZero();
236  vnext.setZero();
237  dvnext_dx.setZero();
238  for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
239  it != model->get_impulses().end(); ++it) {
240  const boost::shared_ptr<ImpulseItem>& item = it->second;
241  impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
242  }
243  }
244 
245  MatrixXs Jc;
246  MatrixXs
249  VectorXs vnext;
252  MatrixXs dvnext_dx;
254  typename ImpulseModelMultiple::ImpulseDataContainer impulses;
256  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
258 };
259 
260 } // namespace crocoddyl
261 
262 /* --- Details -------------------------------------------------------------- */
263 /* --- Details -------------------------------------------------------------- */
264 /* --- Details -------------------------------------------------------------- */
265 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
266 
267 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
ImpulseDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-impulse data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.