crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-impulses.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, 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 <set>
15 #include <utility>
16 
17 #include "crocoddyl/multibody/fwd.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 #include "crocoddyl/multibody/impulse-base.hpp"
20 
21 namespace crocoddyl {
22 
23 template <typename _Scalar>
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef _Scalar Scalar;
29 
30  ImpulseItemTpl() {}
31  ImpulseItemTpl(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true)
32  : name(name), impulse(impulse), active(active) {}
33 
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;
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::set<std::string>& get_active_set() const;
196 
200  const std::set<std::string>& get_inactive_set() const;
201 
202  DEPRECATED("get_active() is deprecated and will be replaced with get_active_set()",
203  const std::vector<std::string>& get_active() {
204  active_.clear();
205  active_.reserve(active_set_.size());
206  for (const auto& contact : active_set_) {
207  active_.push_back(contact);
208  }
209  return active_;
210  };)
211 
212  DEPRECATED("get_inactive() is deprecated and will be replaced with get_inactive_set()",
213  const std::vector<std::string>& get_inactive() {
214  inactive_.clear();
215  inactive_.reserve(inactive_set_.size());
216  for (const auto& contact : inactive_set_) {
217  inactive_.push_back(contact);
218  }
219  return inactive_;
220  };)
221 
225  bool getImpulseStatus(const std::string& name) const;
226 
230  template <class Scalar>
231  friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
232 
233  private:
234  boost::shared_ptr<StateMultibody> state_;
235  ImpulseModelContainer impulses_;
236  std::size_t nc_;
237  std::size_t nc_total_;
238  std::set<std::string> active_set_;
239  std::set<std::string> inactive_set_;
240 
241  // Vector variants. These are to maintain the API compatibility for the deprecated syntax.
242  // These will be removed in future versions along with get_active() / get_inactive()
243  std::vector<std::string> active_;
244  std::vector<std::string> inactive_;
245 };
246 
252 template <typename _Scalar>
254  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
255 
256  typedef _Scalar Scalar;
260  typedef typename MathBase::VectorXs VectorXs;
261  typedef typename MathBase::MatrixXs MatrixXs;
262 
269  template <template <typename Scalar> class Model>
270  ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
271  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
272  dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
273  vnext(model->get_state()->get_nv()),
274  dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
275  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
276  Jc.setZero();
277  dv0_dq.setZero();
278  vnext.setZero();
279  dvnext_dx.setZero();
280  for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
281  it != model->get_impulses().end(); ++it) {
282  const boost::shared_ptr<ImpulseItem>& item = it->second;
283  impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
284  }
285  }
286 
287  MatrixXs Jc;
289  MatrixXs
293  VectorXs vnext;
295  MatrixXs dvnext_dx;
297  typename ImpulseModelMultiple::ImpulseDataContainer impulses;
298  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
300 };
301 
302 } // namespace crocoddyl
303 
304 /* --- Details -------------------------------------------------------------- */
305 /* --- Details -------------------------------------------------------------- */
306 /* --- Details -------------------------------------------------------------- */
307 #include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
308 
309 #endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
Define a stack of impulse models.
void updateForceDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &df_dx) const
Update the Jacobian of the spatial impulse defined in frame coordinate.
std::size_t get_nc() const
Return the dimension of active impulses.
const ImpulseModelContainer & get_impulses() const
Return the impulse models.
void addImpulse(const std::string &name, boost::shared_ptr< ImpulseModelAbstract > impulse, const bool active=true)
Add impulse item.
void calcDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
void updateVelocity(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &vnext) const
Update the system velocity after impulse.
DEPRECATED("Use get_nc().", std::size_t get_ni() const ;) std DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const ;) const std const std::set< std::string > & get_inactive_set() const
Return the dimension of all impulses.
void updateForce(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &impulse)
Update the spatial impulse defined in frame coordinate.
void calc(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total contact Jacobian and contact acceleration.
void updateVelocityDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &dvnext_dx) const
Update the Jacobian of the system velocity after impulse.
const boost::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
friend std::ostream & operator<<(std::ostream &os, const ImpulseModelMultipleTpl< Scalar > &model)
Print information on the impulse models.
ImpulseModelMultipleTpl(boost::shared_ptr< StateMultibody > state)
Initialize the multi-impulse model.
bool getImpulseStatus(const std::string &name) const
Return the status of a given impulse name.
void changeImpulseStatus(const std::string &name, const bool active)
Change the impulse status.
boost::shared_ptr< ImpulseDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-impulse data.
void removeImpulse(const std::string &name)
Remove impulse item.
State multibody representation.
Definition: multibody.hpp:31
Define the multi-impulse data.
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.
ImpulseModelMultiple::ImpulseDataContainer impulses
Stack of impulse data.
friend std::ostream & operator<<(std::ostream &os, const ImpulseItemTpl< Scalar > &model)
Print information on the impulse item.