crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-impulses.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, 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, const bool active = true)
31  : name(name), impulse(impulse), active(active) {}
32 
36  template <class Scalar>
37  friend std::ostream& operator<<(std::ostream& os, const ImpulseItemTpl<Scalar>& model) {
38  os << "{" << *model.impulse << "}";
39  return os;
40  }
41 
42  std::string name;
43  boost::shared_ptr<ImpulseModelAbstract> impulse;
44  bool active;
45 };
46 
54 template <typename _Scalar>
56  public:
57  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 
59  typedef _Scalar Scalar;
60  typedef MathBaseTpl<Scalar> MathBase;
65 
67 
68  typedef typename MathBase::Vector2s Vector2s;
69  typedef typename MathBase::Vector3s Vector3s;
70  typedef typename MathBase::VectorXs VectorXs;
71  typedef typename MathBase::MatrixXs MatrixXs;
72 
73  typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
74  typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
75  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
76 
82  explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
84 
94  void addImpulse(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true);
95 
101  void removeImpulse(const std::string& name);
102 
109  void changeImpulseStatus(const std::string& name, const bool active);
110 
117  void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
118 
125  void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
126 
133  void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& vnext) const;
134 
142  void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& impulse);
143 
151  void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& dvnext_dx) const;
152 
160  void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& df_dx) const;
161 
168  boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
169 
173  const boost::shared_ptr<StateMultibody>& get_state() const;
174 
178  const ImpulseModelContainer& get_impulses() const;
179 
183  std::size_t get_nc() const;
184  DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
185 
189  std::size_t get_nc_total() const;
190  DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const;)
191 
195  const std::vector<std::string>& get_active() const;
196 
200  const std::vector<std::string>& get_inactive() const;
201 
205  bool getImpulseStatus(const std::string& name) const;
206 
210  template <class Scalar>
211  friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
212 
213  private:
214  boost::shared_ptr<StateMultibody> state_;
215  ImpulseModelContainer impulses_;
216  std::size_t nc_;
217  std::size_t nc_total_;
218  std::vector<std::string> active_;
219  std::vector<std::string> inactive_;
220 };
221 
227 template <typename _Scalar>
228 struct ImpulseDataMultipleTpl {
229  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 
231  typedef _Scalar Scalar;
232  typedef MathBaseTpl<Scalar> MathBase;
235  typedef typename MathBase::VectorXs VectorXs;
236  typedef typename MathBase::MatrixXs MatrixXs;
237 
244  template <template <typename Scalar> class Model>
245  ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
246  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
247  dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
248  vnext(model->get_state()->get_nv()),
249  dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
250  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
251  Jc.setZero();
252  dv0_dq.setZero();
253  vnext.setZero();
254  dvnext_dx.setZero();
255  for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
256  it != model->get_impulses().end(); ++it) {
257  const boost::shared_ptr<ImpulseItem>& item = it->second;
258  impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
259  }
260  }
261 
262  MatrixXs Jc;
263  MatrixXs
266  VectorXs vnext;
269  MatrixXs dvnext_dx;
271  typename ImpulseModelMultiple::ImpulseDataContainer impulses;
273  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
275 };
276 
277 } // namespace crocoddyl
278 
279 /* --- Details -------------------------------------------------------------- */
280 /* --- Details -------------------------------------------------------------- */
281 /* --- Details -------------------------------------------------------------- */
282 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
283 
284 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
Define the multi-impulse data.
Definition: fwd.hpp:340
State multibody representation.
Definition: fwd.hpp:300
Define a stack of impulse models.
Definition: fwd.hpp:338
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.