crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
multiple-contacts.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_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, bool active = true)
31  : name(name), contact(contact), active(active) {}
32 
33  std::string name;
34  boost::shared_ptr<ContactModelAbstract> contact;
35  bool active;
36 };
37 
45 template <typename _Scalar>
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef _Scalar Scalar;
56 
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<ContactItem> > ContactModelContainer;
65  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
66  typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
67 
74  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu);
75 
81  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
83 
93  void addContact(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, bool active = true);
94 
100  void removeContact(const std::string& name);
101 
108  void changeContactStatus(const std::string& name, bool active);
109 
116  void calc(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
117 
124  void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
125 
132  void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& dv) const;
133 
141  void updateForce(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& force);
142 
150  void updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& ddv_dx) const;
151 
161  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
162  const MatrixXs& df_du) const;
163 
170  boost::shared_ptr<ContactDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
171 
175  const boost::shared_ptr<StateMultibody>& get_state() const;
176 
180  const ContactModelContainer& get_contacts() const;
181 
185  const std::size_t& get_nc() const;
186 
190  const std::size_t& get_nc_total() const;
191 
195  const std::size_t& get_nu() const;
196 
200  const std::vector<std::string>& get_active() const;
201 
205  const std::vector<std::string>& get_inactive() const;
206 
210  bool getContactStatus(const std::string& name) const;
211 
212  private:
213  boost::shared_ptr<StateMultibody> state_;
214  ContactModelContainer contacts_;
215  std::size_t nc_;
216  std::size_t nc_total_;
217  std::size_t nu_;
218  std::vector<std::string> active_;
219  std::vector<std::string> inactive_;
220 };
221 
227 template <typename _Scalar>
229  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 
231  typedef _Scalar Scalar;
235  typedef typename MathBase::VectorXs VectorXs;
236  typedef typename MathBase::MatrixXs MatrixXs;
237 
244  template <template <typename Scalar> class Model>
245  ContactDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
246  : Jc(model->get_nc_total(), model->get_state()->get_nv()),
247  a0(model->get_nc_total()),
248  da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
249  dv(model->get_state()->get_nv()),
250  ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
251  fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
252  Jc.setZero();
253  a0.setZero();
254  da0_dx.setZero();
255  dv.setZero();
256  ddv_dx.setZero();
257  for (typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
258  it != model->get_contacts().end(); ++it) {
259  const boost::shared_ptr<ContactItem>& item = it->second;
260  contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
261  }
262  }
263 
264  MatrixXs Jc;
265  VectorXs a0;
267  MatrixXs
271  VectorXs
274  dv;
275  MatrixXs ddv_dx;
276  typename ContactModelMultiple::ContactDataContainer contacts;
278  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
280 };
281 
282 } // namespace crocoddyl
283 
284 /* --- Details -------------------------------------------------------------- */
285 /* --- Details -------------------------------------------------------------- */
286 /* --- Details -------------------------------------------------------------- */
287 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
288 
289 #endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
Define a stack of contact models.
VectorXs dv
Constrained system acceleration in generalized coordinates .
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.