GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/multiple-contacts.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 28 0.0%
Branches: 0 64 0.0%
Line Branch Exec Source
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.
8 ///////////////////////////////////////////////////////////////////////////////
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
16 namespace crocoddyl {
17
18 template <typename _Scalar>
19 struct ContactItemTpl {
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22 typedef _Scalar Scalar;
23 typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;
24
25 ✗ ContactItemTpl() {}
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
38 /**
39 * @brief Print information on the contact item
40 */
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
52 /**
53 * @brief Define a stack of contact models
54 *
55 * The contact models can be defined with active and inactive status. The idea
56 * behind this design choice is to be able to create a mechanism that allocates
57 * the entire data needed for the computations. Then, there are designed
58 * routines that update the only active contacts.
59 */
60 template <typename _Scalar>
61 class ContactModelMultipleTpl {
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
66 typedef MathBaseTpl<Scalar> MathBase;
67 typedef StateMultibodyTpl<Scalar> StateMultibody;
68 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
69 typedef ContactDataMultipleTpl<Scalar> ContactDataMultiple;
70 typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;
71
72 typedef ContactItemTpl<Scalar> ContactItem;
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
86 /**
87 * @brief Initialize the multi-contact model
88 *
89 * @param[in] state Multibody state
90 * @param[in] nu Dimension of control vector
91 */
92 ContactModelMultipleTpl(std::shared_ptr<StateMultibody> state,
93 const std::size_t nu);
94
95 /**
96 * @brief Initialize the multi-contact model
97 *
98 * @param[in] state Multibody state
99 */
100 ContactModelMultipleTpl(std::shared_ptr<StateMultibody> state);
101 ~ContactModelMultipleTpl();
102
103 /**
104 * @brief Add contact item
105 *
106 * Note that the memory is allocated for inactive contacts as well.
107 *
108 * @param[in] name Contact name
109 * @param[in] contact Contact model
110 * @param[in] active Contact status (active by default)
111 */
112 void addContact(const std::string& name,
113 std::shared_ptr<ContactModelAbstract> contact,
114 const bool active = true);
115
116 /**
117 * @brief Remove contact item
118 *
119 * @param[in] name Contact name
120 */
121 void removeContact(const std::string& name);
122
123 /**
124 * @brief Change the contact status
125 *
126 * @param[in] name Contact name
127 * @param[in] active Contact status (True for active)
128 */
129 void changeContactStatus(const std::string& name, const bool active);
130
131 /**
132 * @brief Compute the contact Jacobian and contact acceleration
133 *
134 * @param[in] data Multi-contact data
135 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
136 */
137 void calc(const std::shared_ptr<ContactDataMultiple>& data,
138 const Eigen::Ref<const VectorXs>& x);
139
140 /**
141 * @brief Compute the derivatives of the contact holonomic constraint
142 *
143 * @param[in] data Multi-contact data
144 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
145 */
146 void calcDiff(const std::shared_ptr<ContactDataMultiple>& data,
147 const Eigen::Ref<const VectorXs>& x);
148
149 /**
150 * @brief Update the constrained system acceleration
151 *
152 * @param[in] data Multi-contact data
153 * @param[in] dv Constrained system acceleration
154 * \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
155 */
156 void updateAcceleration(const std::shared_ptr<ContactDataMultiple>& data,
157 const VectorXs& dv) const;
158
159 /**
160 * @brief Update the spatial force defined in frame coordinate
161 *
162 * @param[in] data Multi-contact data
163 * @param[in] force Spatial force defined in frame coordinate
164 * \f${}^o\underline{\boldsymbol{\lambda}}_c\in\mathbb{R}^{nc}\f$
165 */
166 void updateForce(const std::shared_ptr<ContactDataMultiple>& data,
167 const VectorXs& force);
168
169 /**
170 * @brief Update the Jacobian of the constrained system acceleration
171 *
172 * @param[in] data Multi-contact data
173 * @param[in] ddv_dx Jacobian of the system acceleration in generalized
174 * coordinates
175 * \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
176 * ndx}\f$
177 */
178 void updateAccelerationDiff(const std::shared_ptr<ContactDataMultiple>& data,
179 const MatrixXs& ddv_dx) const;
180
181 /**
182 * @brief Update the Jacobian of the spatial force defined in frame coordinate
183 *
184 * @param[in] data Multi-contact data
185 * @param[in] df_dx Jacobian of the spatial impulse defined in frame
186 * coordinate
187 * \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{x}}\in\mathbb{R}^{nc\times{ndx}}\f$
188 * @param[in] df_du Jacobian of the spatial impulse defined in frame
189 * coordinate
190 * \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{u}}\in\mathbb{R}^{nc\times{nu}}\f$
191 */
192 void updateForceDiff(const std::shared_ptr<ContactDataMultiple>& data,
193 const MatrixXs& df_dx, const MatrixXs& df_du) const;
194
195 /**
196 * @brief Update the RNEA derivatives dtau_dq by adding the skew term
197 * (necessary for contacts expressed in LOCAL_WORLD_ALIGNED / WORLD)
198 *
199 * To learn more about the computation of the contact derivatives in different
200 * frames see https://hal.science/hal-03758989/document.
201 *
202 * @param[in] data Multi-contact data
203 * @param[in] pinocchio Pinocchio data
204 */
205 void updateRneaDiff(const std::shared_ptr<ContactDataMultiple>& data,
206 pinocchio::DataTpl<Scalar>& pinocchio) const;
207
208 /**
209 * @brief Create the multi-contact data
210 *
211 * @param[in] data Pinocchio data
212 * @return the multi-contact data.
213 */
214 std::shared_ptr<ContactDataMultiple> createData(
215 pinocchio::DataTpl<Scalar>* const data);
216
217 /**
218 * @brief Cast the multi-contact model to a different scalar type.
219 *
220 * It is useful for operations requiring different precision or scalar types.
221 *
222 * @tparam NewScalar The new scalar type to cast to.
223 * @return ContactModelMultipleTpl<NewScalar> A multi-contact model with the
224 * new scalar type.
225 */
226 template <typename NewScalar>
227 ContactModelMultipleTpl<NewScalar> cast() const;
228
229 /**
230 * @brief Return the multibody state
231 */
232 const std::shared_ptr<StateMultibody>& get_state() const;
233
234 /**
235 * @brief Return the contact models
236 */
237 const ContactModelContainer& get_contacts() const;
238
239 /**
240 * @brief Return the dimension of active contacts
241 */
242 std::size_t get_nc() const;
243
244 /**
245 * @brief Return the dimension of all contacts
246 */
247 std::size_t get_nc_total() const;
248
249 /**
250 * @brief Return the dimension of control vector
251 */
252 std::size_t get_nu() const;
253
254 /**
255 * @brief Return the names of the set of active contacts
256 */
257 const std::set<std::string>& get_active_set() const;
258
259 /**
260 * @brief Return the names of the set of inactive contacts
261 */
262 const std::set<std::string>& get_inactive_set() const;
263
264 /**
265 * @brief Return the status of a given contact name
266 */
267 bool getContactStatus(const std::string& name) const;
268
269 /**
270 * @brief Return the type of contact computation
271 *
272 * True for all contacts, otherwise false for active contacts
273 */
274 bool getComputeAllContacts() const;
275
276 /**
277 * @brief Set the tyoe of contact computation: all or active contacts
278 *
279 * @param status Type of contact computation (true for all contacts and false
280 * for active contacts)
281 */
282 void setComputeAllContacts(const bool status);
283
284 /**
285 * @brief Print information on the contact models
286 */
287 template <class Scalar>
288 friend std::ostream& operator<<(std::ostream& os,
289 const ContactModelMultipleTpl<Scalar>& model);
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
302 /**
303 * @brief Define the multi-contact data
304 *
305 * \sa ContactModelMultipleTpl
306 */
307 template <typename _Scalar>
308 struct ContactDataMultipleTpl {
309 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
310
311 typedef _Scalar Scalar;
312 typedef MathBaseTpl<Scalar> MathBase;
313 typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
314 typedef ContactItemTpl<Scalar> ContactItem;
315 typedef typename MathBase::VectorXs VectorXs;
316 typedef typename MathBase::MatrixXs MatrixXs;
317
318 /**
319 * @brief Initialized a multi-contact data
320 *
321 * @param[in] model Multi-contact model
322 * @param[in] data Pinocchio data
323 */
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; //!< Contact Jacobian in frame coordinate
349 //!< \f$\mathbf{J}_c\in\mathbb{R}^{nc_{total}\times{nv}}\f$
350 //!< (memory defined for active and inactive contacts)
351 VectorXs a0; //!< Desired spatial contact acceleration in frame coordinate
352 //!< \f$\underline{\mathbf{a}}_0\in\mathbb{R}^{nc_{total}}\f$
353 //!< (memory defined for active and inactive contacts)
354 MatrixXs
355 da0_dx; //!< Jacobian of the desired spatial contact acceleration in
356 //!< frame coordinate
357 //!< \f$\frac{\partial\underline{\mathbf{a}}_0}{\partial\mathbf{x}}\in\mathbb{R}^{nc_{total}\times{ndx}}\f$
358 //!< (memory defined for active and inactive contacts)
359 VectorXs dv; //!< Constrained system acceleration in generalized coordinates
360 //!< \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
361 MatrixXs
362 ddv_dx; //!< Jacobian of the system acceleration in generalized
363 //!< coordinates
364 //!< \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
365 //!< ndx}\f$
366 typename ContactModelMultiple::ContactDataContainer
367 contacts; //!< Stack of contact data
368 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
369 fext; //!< External spatial forces in body coordinates
370 };
371
372 } // namespace crocoddyl
373
374 /* --- Details -------------------------------------------------------------- */
375 /* --- Details -------------------------------------------------------------- */
376 /* --- Details -------------------------------------------------------------- */
377 #include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
378
379 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactItemTpl)
380 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModelMultipleTpl)
381 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactDataMultipleTpl)
382
383 #endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
384