Crocoddyl
 
Loading...
Searching...
No Matches
multiple-contacts.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
11#define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
12
13#include "crocoddyl/multibody/contact-base.hpp"
14#include "crocoddyl/multibody/fwd.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22 typedef _Scalar Scalar;
24
26 ContactItemTpl(const std::string& name,
27 std::shared_ptr<ContactModelAbstract> contact,
28 const bool active = true)
29 : name(name), contact(contact), active(active) {}
30
31 template <typename NewScalar>
32 ContactItemTpl<NewScalar> cast() const {
33 typedef ContactItemTpl<NewScalar> ReturnType;
34 ReturnType ret(name, contact->template cast<NewScalar>(), active);
35 return ret;
36 }
37
41 friend std::ostream& operator<<(std::ostream& os,
42 const ContactItemTpl<Scalar>& model) {
43 os << "{" << *model.contact << "}";
44 return os;
45 }
46
47 std::string name;
48 std::shared_ptr<ContactModelAbstract> contact;
49 bool active;
50};
51
60template <typename _Scalar>
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
71
73
74 typedef typename MathBase::Vector2s Vector2s;
75 typedef typename MathBase::Vector3s Vector3s;
76 typedef typename MathBase::VectorXs VectorXs;
77 typedef typename MathBase::MatrixXs MatrixXs;
78
79 typedef std::map<std::string, std::shared_ptr<ContactItem> >
80 ContactModelContainer;
81 typedef std::map<std::string, std::shared_ptr<ContactDataAbstract> >
82 ContactDataContainer;
83 typedef typename pinocchio::container::aligned_vector<
84 pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
85
92 ContactModelMultipleTpl(std::shared_ptr<StateMultibody> state,
93 const std::size_t nu);
94
100 ContactModelMultipleTpl(std::shared_ptr<StateMultibody> state);
102
112 void addContact(const std::string& name,
113 std::shared_ptr<ContactModelAbstract> contact,
114 const bool active = true);
115
121 void removeContact(const std::string& name);
122
129 void changeContactStatus(const std::string& name, const bool active);
130
137 void calc(const std::shared_ptr<ContactDataMultiple>& data,
138 const Eigen::Ref<const VectorXs>& x);
139
146 void calcDiff(const std::shared_ptr<ContactDataMultiple>& data,
147 const Eigen::Ref<const VectorXs>& x);
148
156 void updateAcceleration(const std::shared_ptr<ContactDataMultiple>& data,
157 const VectorXs& dv) const;
158
166 void updateForce(const std::shared_ptr<ContactDataMultiple>& data,
167 const VectorXs& force);
168
178 void updateAccelerationDiff(const std::shared_ptr<ContactDataMultiple>& data,
179 const MatrixXs& ddv_dx) const;
180
192 void updateForceDiff(const std::shared_ptr<ContactDataMultiple>& data,
193 const MatrixXs& df_dx, const MatrixXs& df_du) const;
194
205 void updateRneaDiff(const std::shared_ptr<ContactDataMultiple>& data,
206 pinocchio::DataTpl<Scalar>& pinocchio) const;
207
214 std::shared_ptr<ContactDataMultiple> createData(
215 pinocchio::DataTpl<Scalar>* const data);
216
226 template <typename NewScalar>
228
232 const std::shared_ptr<StateMultibody>& get_state() const;
233
237 const ContactModelContainer& get_contacts() const;
238
242 std::size_t get_nc() const;
243
247 std::size_t get_nc_total() const;
248
252 std::size_t get_nu() const;
253
257 const std::set<std::string>& get_active_set() const;
258
262 const std::set<std::string>& get_inactive_set() const;
263
267 bool getContactStatus(const std::string& name) const;
268
275
282 void setComputeAllContacts(const bool status);
283
287 template <class Scalar>
288 friend std::ostream& operator<<(std::ostream& os,
290
291 private:
292 std::shared_ptr<StateMultibody> state_;
293 ContactModelContainer contacts_;
294 std::size_t nc_;
295 std::size_t nc_total_;
296 std::size_t nu_;
297 std::set<std::string> active_set_;
298 std::set<std::string> inactive_set_;
299 bool compute_all_contacts_;
300};
301
307template <typename _Scalar>
309 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
310
311 typedef _Scalar Scalar;
315 typedef typename MathBase::VectorXs VectorXs;
316 typedef typename MathBase::MatrixXs MatrixXs;
317
324 template <template <typename Scalar> class Model>
325 ContactDataMultipleTpl(Model<Scalar>* const model,
326 pinocchio::DataTpl<Scalar>* const data)
327 : Jc(model->get_nc_total(), model->get_state()->get_nv()),
328 a0(model->get_nc_total()),
329 da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
330 dv(model->get_state()->get_nv()),
331 ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
332 fext(model->get_state()->get_pinocchio()->njoints,
333 pinocchio::ForceTpl<Scalar>::Zero()) {
334 Jc.setZero();
335 a0.setZero();
336 da0_dx.setZero();
337 dv.setZero();
338 ddv_dx.setZero();
339 for (typename ContactModelMultiple::ContactModelContainer::const_iterator
340 it = model->get_contacts().begin();
341 it != model->get_contacts().end(); ++it) {
342 const std::shared_ptr<ContactItem>& item = it->second;
343 contacts.insert(
344 std::make_pair(item->name, item->contact->createData(data)));
345 }
346 }
347
348 MatrixXs Jc;
351 VectorXs a0;
354 MatrixXs
359 VectorXs dv;
361 MatrixXs
366 typename ContactModelMultiple::ContactDataContainer
368 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
370};
371
372} // namespace crocoddyl
373
374/* --- Details -------------------------------------------------------------- */
375/* --- Details -------------------------------------------------------------- */
376/* --- Details -------------------------------------------------------------- */
377#include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
378
379CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactItemTpl)
380CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModelMultipleTpl)
381CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactDataMultipleTpl)
382
383#endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
Define a stack of contact models.
void updateRneaDiff(const std::shared_ptr< ContactDataMultiple > &data, pinocchio::DataTpl< Scalar > &pinocchio) const
Update the RNEA derivatives dtau_dq by adding the skew term (necessary for contacts expressed in LOCA...
friend std::ostream & operator<<(std::ostream &os, const ContactModelMultipleTpl< Scalar > &model)
Print information on the contact models.
std::size_t get_nc() const
Return the dimension of active contacts.
void updateAcceleration(const std::shared_ptr< ContactDataMultiple > &data, const VectorXs &dv) const
Update the constrained system acceleration.
void setComputeAllContacts(const bool status)
Set the tyoe of contact computation: all or active contacts.
bool getComputeAllContacts() const
Return the type of contact computation.
const std::set< std::string > & get_active_set() const
Return the names of the set of active contacts.
const std::set< std::string > & get_inactive_set() const
Return the names of the set of inactive contacts.
const ContactModelContainer & get_contacts() const
Return the contact models.
void calc(const std::shared_ptr< ContactDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the contact Jacobian and contact acceleration.
ContactModelMultipleTpl(std::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the multi-contact model.
void updateAccelerationDiff(const std::shared_ptr< ContactDataMultiple > &data, const MatrixXs &ddv_dx) const
Update the Jacobian of the constrained system acceleration.
void addContact(const std::string &name, std::shared_ptr< ContactModelAbstract > contact, const bool active=true)
Add contact item.
void updateForceDiff(const std::shared_ptr< ContactDataMultiple > &data, const MatrixXs &df_dx, const MatrixXs &df_du) const
Update the Jacobian of the spatial force defined in frame coordinate.
void changeContactStatus(const std::string &name, const bool active)
Change the contact status.
void removeContact(const std::string &name)
Remove contact item.
bool getContactStatus(const std::string &name) const
Return the status of a given contact name.
std::size_t get_nc_total() const
Return the dimension of all contacts.
ContactModelMultipleTpl< NewScalar > cast() const
Cast the multi-contact model to a different scalar type.
const std::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
std::shared_ptr< ContactDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-contact data.
void updateForce(const std::shared_ptr< ContactDataMultiple > &data, const VectorXs &force)
Update the spatial force defined in frame coordinate.
void calcDiff(const std::shared_ptr< ContactDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
ContactModelMultipleTpl(std::shared_ptr< StateMultibody > state)
Initialize the multi-contact model.
std::size_t get_nu() const
Return the dimension of control vector.
State multibody representation.
Definition multibody.hpp:34
Define the multi-contact data.
ContactModelMultiple::ContactDataContainer contacts
Stack of contact data.
ContactDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-contact data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
friend std::ostream & operator<<(std::ostream &os, const ContactItemTpl< Scalar > &model)
Print information on the contact item.