GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-force.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 59 68 86.8%
Branches: 32 90 35.6%
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_RESIDUALS_CONTACT_FORCE_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
12
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/multibody/contact-base.hpp"
15 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
16 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
18 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19 #include "crocoddyl/multibody/data/contacts.hpp"
20 #include "crocoddyl/multibody/data/impulses.hpp"
21 #include "crocoddyl/multibody/fwd.hpp"
22 #include "crocoddyl/multibody/impulse-base.hpp"
23 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
24 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
25 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
26 #include "crocoddyl/multibody/states/multibody.hpp"
27
28 namespace crocoddyl {
29
30 /**
31 * @brief Define a contact force residual function
32 *
33 * This residual function is defined as
34 * \f$\mathbf{r}=\boldsymbol{\lambda}-\boldsymbol{\lambda}^*\f$, where
35 * \f$\boldsymbol{\lambda}, \boldsymbol{\lambda}^*\f$ are the current and
36 * reference spatial forces, respectively. The current spatial forces
37 * \f$\boldsymbol{\lambda}\in\mathbb{R}^{nc}\f$ is computed by
38 * `DifferentialActionModelContactFwdDynamicsTpl` or
39 * `ActionModelImpulseFwdDynamicTpl`, with `nc` as the dimension of the contact.
40 *
41 * Both residual and residual Jacobians are computed analytically, where the
42 * force vector \f$\boldsymbol{\lambda}\f$ and its Jacobians
43 * \f$\left(\frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{x}},
44 * \frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{u}}\right)\f$ are
45 * computed by `DifferentialActionModelContactFwdDynamicsTpl` or
46 * `ActionModelImpulseFwdDynamicTpl`. These values are stored in a shared data
47 * (i.e., `DataCollectorContactTpl` or `DataCollectorImpulseTpl`). Note that
48 * this residual function cannot be used with other action models.
49 *
50 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`,
51 * `DifferentialActionModelContactFwdDynamicsTpl`,
52 * `ActionModelImpulseFwdDynamicTpl`, `DataCollectorContactTpl`,
53 * `DataCollectorImpulseTpl`
54 */
55 template <typename _Scalar>
56 class ResidualModelContactForceTpl : public ResidualModelAbstractTpl<_Scalar> {
57 public:
58 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelContactForceTpl)
60
61 typedef _Scalar Scalar;
62 typedef MathBaseTpl<Scalar> MathBase;
63 typedef ResidualModelAbstractTpl<Scalar> Base;
64 typedef ResidualDataContactForceTpl<Scalar> Data;
65 typedef StateMultibodyTpl<Scalar> StateMultibody;
66 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
67 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
68 typedef pinocchio::ForceTpl<Scalar> Force;
69 typedef typename MathBase::VectorXs VectorXs;
70 typedef typename MathBase::MatrixXs MatrixXs;
71
72 /**
73 * @brief Initialize the contact force residual model
74 *
75 * Note that for the inverse-dynamic cases, the control vector contains the
76 * generalized accelerations, torques, and all the contact forces.
77 *
78 * @param[in] state Multibody state
79 * @param[in] id Reference frame id
80 * @param[in] fref Reference spatial contact force in the contact
81 * coordinates
82 * @param[in] nc Dimension of the contact force (nc <= 6)
83 * @param[in] nu Dimension of control vector
84 * @param[in] fwddyn Indicates that we have a forward dynamics problem (true)
85 * or inverse dynamics (false)
86 */
87 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
88 const pinocchio::FrameIndex id,
89 const Force& fref, const std::size_t nc,
90 const std::size_t nu, const bool fwddyn = true);
91
92 /**
93 * @brief Initialize the contact force residual model
94 *
95 * The default `nu` is obtained from `StateAbstractTpl::get_nv()`. Note that
96 * this constructor can be used for forward-dynamics cases only.
97 *
98 * @param[in] state Multibody state
99 * @param[in] id Reference frame id
100 * @param[in] fref Reference spatial contact force in the contact
101 * coordinates
102 * @param[in] nc Dimension of the contact force (nc <= 6)
103 */
104 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
105 const pinocchio::FrameIndex id,
106 const Force& fref, const std::size_t nc);
107 3354 virtual ~ResidualModelContactForceTpl() = default;
108
109 /**
110 * @brief Compute the contact force residual
111 *
112 * The CoP residual is computed based on the \f$\mathbf{A}\f$ matrix, the
113 * force vector is computed by `DifferentialActionModelContactFwdDynamicsTpl`
114 * or `ActionModelImpulseFwdDynamicTpl` which is stored in
115 * `DataCollectorContactTpl` or `DataCollectorImpulseTpl`.
116 *
117 * @param[in] data Contact force data
118 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
119 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
120 */
121 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
122 const Eigen::Ref<const VectorXs>& x,
123 const Eigen::Ref<const VectorXs>& u) override;
124
125 /**
126 * @brief Compute the residual vector for nodes that depends only on the state
127 *
128 * It updates the residual vector based on the state only (i.e., it ignores
129 * the contact forces). This function is used in the terminal nodes of an
130 * optimal control problem.
131 *
132 * @param[in] data Residual data
133 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
134 */
135 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
136 const Eigen::Ref<const VectorXs>& x) override;
137
138 /**
139 * @brief Compute the derivatives of the contact force residual
140 *
141 * The CoP residual is computed based on the \f$\mathbf{A}\f$ matrix, the
142 * force vector is computed by `DifferentialActionModelContactFwdDynamicsTpl`
143 * or `ActionModelImpulseFwdDynamicTpl` which is stored in
144 * `DataCollectorContactTpl` or `DataCollectorImpulseTpl`.
145 *
146 * @param[in] data Contact force data
147 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
148 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
149 */
150 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
151 const Eigen::Ref<const VectorXs>& x,
152 const Eigen::Ref<const VectorXs>& u) override;
153
154 /**
155 * @brief Compute the Jacobian of the residual functions with respect to the
156 * state only
157 *
158 * It updates the Jacobian of the residual function based on the state only
159 * (i.e., it ignores the contact forces). This function is used in the
160 * terminal nodes of an optimal control problem.
161 *
162 * @param[in] data Residual data
163 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
164 */
165 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
166 const Eigen::Ref<const VectorXs>& x) override;
167
168 /**
169 * @brief Create the contact force residual data
170 *
171 * @param[in] data shared data (it should be of type DataCollectorContactTpl)
172 * @return the residual data.
173 */
174 virtual std::shared_ptr<ResidualDataAbstract> createData(
175 DataCollectorAbstract* const data) override;
176
177 /**
178 * @brief Update the Jacobians of the contact friction cone residual
179 *
180 * @param[in] data Contact friction cone residual data
181 */
182 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
183
184 /**
185 * @brief Cast the contact-force residual model to a different scalar type.
186 *
187 * It is useful for operations requiring different precision or scalar types.
188 *
189 * @tparam NewScalar The new scalar type to cast to.
190 * @return ResidualModelContactForceTpl<NewScalar> A residual model with the
191 * new scalar type.
192 */
193 template <typename NewScalar>
194 ResidualModelContactForceTpl<NewScalar> cast() const;
195
196 /**
197 * @brief Indicates if we are using the forward-dynamics (true) or
198 * inverse-dynamics (false)
199 */
200 bool is_fwddyn() const;
201
202 /**
203 * @brief Return the reference frame id
204 */
205 pinocchio::FrameIndex get_id() const;
206
207 /**
208 * @brief Return the reference spatial contact force in the contact
209 * coordinates
210 */
211 const Force& get_reference() const;
212
213 /**
214 * @brief Modify the reference frame id
215 */
216 DEPRECATED("Do not use set_id, instead create a new model",
217 void set_id(const pinocchio::FrameIndex id);)
218
219 /**
220 * @brief Modify the reference spatial contact force in the contact
221 * coordinates
222 */
223 void set_reference(const Force& reference);
224
225 /**
226 * @brief Print relevant information of the contact-force residual
227 *
228 * @param[out] os Output stream object
229 */
230 virtual void print(std::ostream& os) const override;
231
232 protected:
233 using Base::nr_;
234 using Base::nu_;
235 using Base::state_;
236
237 private:
238 bool fwddyn_; //!< Indicates if we are using this function for forward
239 //!< dynamics
240 bool update_jacobians_; //!< Indicates if we need to update the Jacobians
241 //!< (used for inverse dynamics case)
242 pinocchio::FrameIndex id_; //!< Reference frame id
243 Force fref_; //!< Reference spatial contact force in the contact coordinates
244 };
245
246 template <typename _Scalar>
247 struct ResidualDataContactForceTpl : public ResidualDataAbstractTpl<_Scalar> {
248 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
249
250 typedef _Scalar Scalar;
251 typedef MathBaseTpl<Scalar> MathBase;
252 typedef ResidualDataAbstractTpl<Scalar> Base;
253 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
254 typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
255 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
256 typedef pinocchio::ForceTpl<Scalar> Force;
257 typedef StateMultibodyTpl<Scalar> StateMultibody;
258 typedef typename MathBase::MatrixXs MatrixXs;
259
260 template <template <typename Scalar> class Model>
261 161404 ResidualDataContactForceTpl(Model<Scalar>* const model,
262 DataCollectorAbstract* const data)
263 161404 : Base(model, data) {
264 161404 contact_type = ContactUndefined;
265
266 // Check that proper shared data has been passed
267 161404 bool is_contact = true;
268 161404 DataCollectorContactTpl<Scalar>* d1 =
269
1/2
✓ Branch 0 taken 161402 times.
✗ Branch 1 not taken.
 161404 dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
270 161404 DataCollectorImpulseTpl<Scalar>* d2 =
271
1/2
✓ Branch 0 taken 161402 times.
✗ Branch 1 not taken.
 161404 dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
272
3/4
✓ Branch 0 taken 11434 times.
✓ Branch 1 taken 149968 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11434 times.
 161404 if (d1 == NULL && d2 == NULL) {
273 ✗ throw_pretty(
274 "Invalid argument: the shared data should be derived from "
275 "DataCollectorContact or DataCollectorImpulse");
276 }
277
2/2
✓ Branch 0 taken 11434 times.
✓ Branch 1 taken 149968 times.
 161404 if (d2 != NULL) {
278 11434 is_contact = false;
279 }
280
281 // Avoids data casting at runtime
282
1/2
✓ Branch 1 taken 161402 times.
✗ Branch 2 not taken.
 161404 const pinocchio::FrameIndex id = model->get_id();
283
1/2
✓ Branch 1 taken 161402 times.
✗ Branch 2 not taken.
 161404 const std::shared_ptr<StateMultibody>& state =
284 std::static_pointer_cast<StateMultibody>(model->get_state());
285
2/4
✓ Branch 2 taken 161402 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 161402 times.
✗ Branch 8 not taken.
 161404 std::string frame_name = state->get_pinocchio()->frames[id].name;
286 161404 bool found_contact = false;
287
2/2
✓ Branch 0 taken 149968 times.
✓ Branch 1 taken 11434 times.
 161404 if (is_contact) {
288 149970 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
289 149970 d1->contacts->contacts.begin();
290
1/2
✓ Branch 4 taken 297078 times.
✗ Branch 5 not taken.
 297080 it != d1->contacts->contacts.end(); ++it) {
291
2/2
✓ Branch 2 taken 149968 times.
✓ Branch 3 taken 147110 times.
 297080 if (it->second->frame == id) {
292 149970 ContactData1DTpl<Scalar>* d1d =
293
1/2
✓ Branch 2 taken 149968 times.
✗ Branch 3 not taken.
 149970 dynamic_cast<ContactData1DTpl<Scalar>*>(it->second.get());
294
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 149968 times.
 149970 if (d1d != NULL) {
295 ✗ contact_type = Contact1D;
296 ✗ found_contact = true;
297 ✗ contact = it->second;
298 ✗ break;
299 }
300 149970 ContactData3DTpl<Scalar>* d3d =
301
1/2
✓ Branch 2 taken 149968 times.
✗ Branch 3 not taken.
 149970 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
302
2/2
✓ Branch 0 taken 92794 times.
✓ Branch 1 taken 57174 times.
 149970 if (d3d != NULL) {
303 92794 contact_type = Contact3D;
304 92794 found_contact = true;
305 92794 contact = it->second;
306 92794 break;
307 }
308 57176 ContactData6DTpl<Scalar>* d6d =
309
1/2
✓ Branch 2 taken 57174 times.
✗ Branch 3 not taken.
 57176 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
310
1/2
✓ Branch 0 taken 57174 times.
✗ Branch 1 not taken.
 57176 if (d6d != NULL) {
311 57176 contact_type = Contact6D;
312 57176 found_contact = true;
313 57176 contact = it->second;
314 57176 break;
315 }
316 ✗ throw_pretty(
317 "Domain error: there isn't defined at least a 3d contact for " +
318 frame_name);
319 break;
320 }
321 }
322 } else {
323 11434 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
324 11434 d2->impulses->impulses.begin();
325
1/2
✓ Branch 4 taken 22087 times.
✗ Branch 5 not taken.
 22087 it != d2->impulses->impulses.end(); ++it) {
326
2/2
✓ Branch 2 taken 11434 times.
✓ Branch 3 taken 10653 times.
 22087 if (it->second->frame == id) {
327 11434 ImpulseData3DTpl<Scalar>* d3d =
328
1/2
✓ Branch 2 taken 11434 times.
✗ Branch 3 not taken.
 11434 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
329
2/2
✓ Branch 0 taken 4360 times.
✓ Branch 1 taken 7074 times.
 11434 if (d3d != NULL) {
330 4360 contact_type = Contact3D;
331 4360 found_contact = true;
332 4360 contact = it->second;
333 4360 break;
334 }
335 7074 ImpulseData6DTpl<Scalar>* d6d =
336
1/2
✓ Branch 2 taken 7074 times.
✗ Branch 3 not taken.
 7074 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
337
1/2
✓ Branch 0 taken 7074 times.
✗ Branch 1 not taken.
 7074 if (d6d != NULL) {
338 7074 contact_type = Contact6D;
339 7074 found_contact = true;
340 7074 contact = it->second;
341 7074 break;
342 }
343 ✗ throw_pretty(
344 "Domain error: there isn't defined at least a 3d impulse for " +
345 frame_name);
346 break;
347 }
348 }
349 }
350
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 161402 times.
 161404 if (!found_contact) {
351 ✗ throw_pretty(
352 "Domain error: there isn't defined contact/impulse data for " +
353 frame_name);
354 }
355 161404 }
356 322804 virtual ~ResidualDataContactForceTpl() = default;
357
358 std::shared_ptr<ForceDataAbstractTpl<Scalar> >
359 contact; //!< Contact force data
360 ContactType contact_type; //!< Type of contact (3D / 6D)
361 using Base::r;
362 using Base::Ru;
363 using Base::Rx;
364 using Base::shared;
365 };
366
367 } // namespace crocoddyl
368
369 /* --- Details -------------------------------------------------------------- */
370 /* --- Details -------------------------------------------------------------- */
371 /* --- Details -------------------------------------------------------------- */
372 #include "crocoddyl/multibody/residuals/contact-force.hxx"
373
374 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelContactForceTpl)
375 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataContactForceTpl)
376
377 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
378