GCC Code Coverage Report

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