GCC Code Coverage Report

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