crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-contacts.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_CONTACTS_MULTIPLE_CONTACTS_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_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/contact-base.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
28 
29  ContactItemTpl() {}
30  ContactItemTpl(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, const bool active = true)
31  : name(name), contact(contact), active(active) {}
32 
36  template <class Scalar>
37  friend std::ostream& operator<<(std::ostream& os, const ContactItemTpl<Scalar>& model) {
38  os << "{" << *model.contact << "}";
39  return os;
40  }
41 
42  std::string name;
43  boost::shared_ptr<ContactModelAbstract> contact;
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<ContactItem> > ContactModelContainer;
74  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
75  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
76 
83  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
84 
90  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
92 
102  void addContact(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, const bool active = true);
103 
109  void removeContact(const std::string& name);
110 
117  void changeContactStatus(const std::string& name, const bool active);
118 
125  void calc(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
126 
133  void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
134 
141  void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& dv) const;
142 
150  void updateForce(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& force);
151 
159  void updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& ddv_dx) const;
160 
170  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
171  const MatrixXs& df_du) const;
172 
179  boost::shared_ptr<ContactDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
180 
184  const boost::shared_ptr<StateMultibody>& get_state() const;
185 
189  const ContactModelContainer& get_contacts() const;
190 
194  std::size_t get_nc() const;
195 
199  std::size_t get_nc_total() const;
200 
204  std::size_t get_nu() const;
205 
209  const std::vector<std::string>& get_active() const;
210 
214  const std::vector<std::string>& get_inactive() const;
215 
219  bool getContactStatus(const std::string& name) const;
220 
224  template <class Scalar>
225  friend std::ostream& operator<<(std::ostream& os, const ContactModelMultipleTpl<Scalar>& model);
226 
227  private:
228  boost::shared_ptr<StateMultibody> state_;
229  ContactModelContainer contacts_;
230  std::size_t nc_;
231  std::size_t nc_total_;
232  std::size_t nu_;
233  std::vector<std::string> active_;
234  std::vector<std::string> inactive_;
235 };
236 
242 template <typename _Scalar>
244  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
245 
246  typedef _Scalar Scalar;
250  typedef typename MathBase::VectorXs VectorXs;
251  typedef typename MathBase::MatrixXs MatrixXs;
252 
259  template <template <typename Scalar> class Model>
260  ContactDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
261  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
262  a0(model->get_nc_total()),
263  da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
264  dv(model->get_state()->get_nv()),
265  ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
266  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
267  Jc.setZero();
268  a0.setZero();
269  da0_dx.setZero();
270  dv.setZero();
271  ddv_dx.setZero();
272  for (typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
273  it != model->get_contacts().end(); ++it) {
274  const boost::shared_ptr<ContactItem>& item = it->second;
275  contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
276  }
277  }
278 
279  MatrixXs Jc;
280  VectorXs a0;
282  MatrixXs
286  VectorXs
289  dv;
290  MatrixXs ddv_dx;
291  typename ContactModelMultiple::ContactDataContainer contacts;
293  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
295 };
296 
297 } // namespace crocoddyl
298 
299 /* --- Details -------------------------------------------------------------- */
300 /* --- Details -------------------------------------------------------------- */
301 /* --- Details -------------------------------------------------------------- */
302 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
303 
304 #endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
Define a stack of contact models.
VectorXs dv
Constrained system acceleration in generalized coordinates .
State multibody representation.
Definition: fwd.hpp:300
Define the multi-contact data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
ContactDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-contact data.