crocoddyl  1.8.1
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  friend std::ostream& operator<<(std::ostream& os, const ContactItemTpl<Scalar>& model) {
37  os << "{" << *model.contact << "}";
38  return os;
39  }
40 
41  std::string name;
42  boost::shared_ptr<ContactModelAbstract> contact;
43  bool active;
44 };
45 
53 template <typename _Scalar>
55  public:
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
64 
66 
67  typedef typename MathBase::Vector2s Vector2s;
68  typedef typename MathBase::Vector3s Vector3s;
69  typedef typename MathBase::VectorXs VectorXs;
70  typedef typename MathBase::MatrixXs MatrixXs;
71 
72  typedef std::map<std::string, boost::shared_ptr<ContactItem> > ContactModelContainer;
73  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
74  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
75 
82  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
83 
89  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
91 
101  void addContact(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, const bool active = true);
102 
108  void removeContact(const std::string& name);
109 
116  void changeContactStatus(const std::string& name, const bool active);
117 
124  void calc(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
125 
132  void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
133 
140  void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& dv) const;
141 
149  void updateForce(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& force);
150 
158  void updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& ddv_dx) const;
159 
169  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
170  const MatrixXs& df_du) const;
171 
178  boost::shared_ptr<ContactDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
179 
183  const boost::shared_ptr<StateMultibody>& get_state() const;
184 
188  const ContactModelContainer& get_contacts() const;
189 
193  std::size_t get_nc() const;
194 
198  std::size_t get_nc_total() const;
199 
203  std::size_t get_nu() const;
204 
208  const std::vector<std::string>& get_active() const;
209 
213  const std::vector<std::string>& get_inactive() const;
214 
218  bool getContactStatus(const std::string& name) const;
219 
223  template <class Scalar>
224  friend std::ostream& operator<<(std::ostream& os, const ContactModelMultipleTpl<Scalar>& model);
225 
226  private:
227  boost::shared_ptr<StateMultibody> state_;
228  ContactModelContainer contacts_;
229  std::size_t nc_;
230  std::size_t nc_total_;
231  std::size_t nu_;
232  std::vector<std::string> active_;
233  std::vector<std::string> inactive_;
234 };
235 
241 template <typename _Scalar>
243  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
244 
245  typedef _Scalar Scalar;
249  typedef typename MathBase::VectorXs VectorXs;
250  typedef typename MathBase::MatrixXs MatrixXs;
251 
258  template <template <typename Scalar> class Model>
259  ContactDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
260  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
261  a0(model->get_nc_total()),
262  da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
263  dv(model->get_state()->get_nv()),
264  ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
265  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
266  Jc.setZero();
267  a0.setZero();
268  da0_dx.setZero();
269  dv.setZero();
270  ddv_dx.setZero();
271  for (typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
272  it != model->get_contacts().end(); ++it) {
273  const boost::shared_ptr<ContactItem>& item = it->second;
274  contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
275  }
276  }
277 
278  MatrixXs Jc;
279  VectorXs a0;
281  MatrixXs
285  VectorXs
288  dv;
289  MatrixXs ddv_dx;
290  typename ContactModelMultiple::ContactDataContainer contacts;
292  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
294 };
295 
296 } // namespace crocoddyl
297 
298 /* --- Details -------------------------------------------------------------- */
299 /* --- Details -------------------------------------------------------------- */
300 /* --- Details -------------------------------------------------------------- */
301 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
302 
303 #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.