GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-friction-cone.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 61 65 93.8%
Branches: 30 82 36.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, 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/core/utils/exception.hpp"
15 #include "crocoddyl/multibody/contact-base.hpp"
16 #include "crocoddyl/multibody/contacts/contact-2d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
18 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
19 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
20 #include "crocoddyl/multibody/data/contacts.hpp"
21 #include "crocoddyl/multibody/data/impulses.hpp"
22 #include "crocoddyl/multibody/friction-cone.hpp"
23 #include "crocoddyl/multibody/fwd.hpp"
24 #include "crocoddyl/multibody/impulse-base.hpp"
25 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
26 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
27 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
28 #include "crocoddyl/multibody/states/multibody.hpp"
29
30 namespace crocoddyl {
31
32 /**
33 * @brief Contact friction cone residual
34 *
35 * This residual function is defined as
36 * \f$\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\f$, where
37 * \f$\mathbf{A}\in~\mathbb{R}^{nr\times nc}\f$ describes the linearized
38 * friction cone, \f$\boldsymbol{\lambda}\in~\mathbb{R}^{nc}\f$ is the spatial
39 * contact forces computed by `DifferentialActionModelContactFwdDynamicsTpl`,
40 * and `nr`, `nc` are the number of cone facets and dimension of the contact,
41 * respectively.
42 *
43 * Both residual and residual Jacobians are computed analytically, where th
44 * force vector \f$\boldsymbol{\lambda}\f$ and its Jacobians
45 * \f$\left(\frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{x}},
46 * \frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{u}}\right)\f$ are
47 * computed by `DifferentialActionModelContactFwdDynamicsTpl` or
48 * `ActionModelImpulseFwdDynamicTpl`. These values are stored in a shared data
49 * (i.e. `DataCollectorContactTpl` or `DataCollectorImpulseTpl`). Note that
50 * this residual function cannot be used with other action models.
51 *
52 * As described in `ResidualModelAbstractTpl()`, the residual value and its
53 * derivatives are calculated by `calc` and `calcDiff`, respectively.
54 *
55 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`,
56 * `DifferentialActionModelContactFwdDynamicsTpl`,
57 * `ActionModelImpulseFwdDynamicTpl`, `DataCollectorForceTpl`
58 */
59 template <typename _Scalar>
60 class ResidualModelContactFrictionConeTpl
61 : public ResidualModelAbstractTpl<_Scalar> {
62 public:
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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(boost::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(boost::shared_ptr<StateMultibody> state,
107 const pinocchio::FrameIndex id,
108 const FrictionCone& fref);
109 virtual ~ResidualModelContactFrictionConeTpl();
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 boost::shared_ptr<ResidualDataAbstract>& data,
119 const Eigen::Ref<const VectorXs>& x,
120 const Eigen::Ref<const VectorXs>& u);
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 boost::shared_ptr<ResidualDataAbstract>& data,
133 const Eigen::Ref<const VectorXs>& x);
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 boost::shared_ptr<ResidualDataAbstract>& data,
143 const Eigen::Ref<const VectorXs>& x,
144 const Eigen::Ref<const VectorXs>& u);
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 boost::shared_ptr<ResidualDataAbstract>& data,
158 const Eigen::Ref<const VectorXs>& x);
159
160 /**
161 * @brief Create the contact friction cone residual data
162 */
163 virtual boost::shared_ptr<ResidualDataAbstract> createData(
164 DataCollectorAbstract* const data);
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 boost::shared_ptr<ResidualDataAbstract>& data);
172
173 /**
174 * @brief Indicates if we are using the forward-dynamics (true) or
175 * inverse-dynamics (false)
176 */
177 bool is_fwddyn() const;
178
179 /**
180 * @brief Return the reference frame id
181 */
182 pinocchio::FrameIndex get_id() const;
183
184 /**
185 * @brief Return the reference contact friction cone
186 */
187 const FrictionCone& get_reference() const;
188
189 /**
190 * @brief Modify the reference frame id
191 */
192 DEPRECATED("Do not use set_id, instead create a new model",
193 void set_id(const pinocchio::FrameIndex id);)
194
195 /**
196 * @brief Modify the reference contact friction cone
197 */
198 void set_reference(const FrictionCone& reference);
199
200 /**
201 * @brief Print relevant information of the contact-friction-cone residual
202 *
203 * @param[out] os Output stream object
204 */
205 virtual void print(std::ostream& os) const;
206
207 protected:
208 using Base::nu_;
209 using Base::state_;
210
211 private:
212 bool fwddyn_; //!< Indicates if we are using this function for forward
213 //!< dynamics
214 bool update_jacobians_; //!< Indicates if we need to update the Jacobians
215 //!< (used for inverse dynamics case)
216 pinocchio::FrameIndex id_; //!< Reference frame id
217 FrictionCone fref_; //!< Reference contact friction cone
218 };
219
220 template <typename _Scalar>
221 struct ResidualDataContactFrictionConeTpl
222 : public ResidualDataAbstractTpl<_Scalar> {
223 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
224
225 typedef _Scalar Scalar;
226 typedef MathBaseTpl<Scalar> MathBase;
227 typedef ResidualDataAbstractTpl<Scalar> Base;
228 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
229 typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
230 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
231 typedef StateMultibodyTpl<Scalar> StateMultibody;
232 typedef typename MathBase::MatrixXs MatrixXs;
233
234 template <template <typename Scalar> class Model>
235 72435 ResidualDataContactFrictionConeTpl(Model<Scalar>* const model,
236 DataCollectorAbstract* const data)
237 72435 : Base(model, data) {
238 72435 contact_type = ContactUndefined;
239 // Check that proper shared data has been passed
240 72435 bool is_contact = true;
241 72435 DataCollectorContactTpl<Scalar>* d1 =
242
1/2
✓ Branch 0 taken 72435 times.
✗ Branch 1 not taken.
 72435 dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
243 72435 DataCollectorImpulseTpl<Scalar>* d2 =
244
1/2
✓ Branch 0 taken 72435 times.
✗ Branch 1 not taken.
 72435 dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
245
3/4
✓ Branch 0 taken 8743 times.
✓ Branch 1 taken 63692 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8743 times.
 72435 if (d1 == NULL && d2 == NULL) {
246 ✗ throw_pretty(
247 "Invalid argument: the shared data should be derived from "
248 "DataCollectorContact or DataCollectorImpulse");
249 }
250
2/2
✓ Branch 0 taken 8743 times.
✓ Branch 1 taken 63692 times.
 72435 if (d2 != NULL) {
251 8743 is_contact = false;
252 }
253
254 // Avoids data casting at runtime
255 72435 const pinocchio::FrameIndex id = model->get_id();
256 72435 const boost::shared_ptr<StateMultibody>& state =
257 boost::static_pointer_cast<StateMultibody>(model->get_state());
258
1/2
✓ Branch 5 taken 72435 times.
✗ Branch 6 not taken.
 72435 std::string frame_name = state->get_pinocchio()->frames[id].name;
259 72435 bool found_contact = false;
260
2/2
✓ Branch 0 taken 63692 times.
✓ Branch 1 taken 8743 times.
 72435 if (is_contact) {
261 63692 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
262 63692 d1->contacts->contacts.begin();
263
1/2
✓ Branch 4 taken 122166 times.
✗ Branch 5 not taken.
 122166 it != d1->contacts->contacts.end(); ++it) {
264
2/2
✓ Branch 2 taken 63692 times.
✓ Branch 3 taken 58474 times.
 122166 if (it->second->frame == id) {
265 63692 ContactData2DTpl<Scalar>* d2d =
266
1/2
✓ Branch 2 taken 63692 times.
✗ Branch 3 not taken.
 63692 dynamic_cast<ContactData2DTpl<Scalar>*>(it->second.get());
267
2/2
✓ Branch 0 taken 3332 times.
✓ Branch 1 taken 60360 times.
 63692 if (d2d != NULL) {
268 3332 contact_type = Contact2D;
269 3332 found_contact = true;
270 3332 contact = it->second;
271 3332 break;
272 }
273 60360 ContactData3DTpl<Scalar>* d3d =
274
1/2
✓ Branch 2 taken 60360 times.
✗ Branch 3 not taken.
 60360 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
275
2/2
✓ Branch 0 taken 45222 times.
✓ Branch 1 taken 15138 times.
 60360 if (d3d != NULL) {
276 45222 contact_type = Contact3D;
277 45222 found_contact = true;
278 45222 contact = it->second;
279 45222 break;
280 }
281 15138 ContactData6DTpl<Scalar>* d6d =
282
1/2
✓ Branch 2 taken 15138 times.
✗ Branch 3 not taken.
 15138 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
283
1/2
✓ Branch 0 taken 15138 times.
✗ Branch 1 not taken.
 15138 if (d6d != NULL) {
284 15138 contact_type = Contact6D;
285 15138 found_contact = true;
286 15138 contact = it->second;
287 15138 break;
288 }
289 ✗ throw_pretty(
290 "Domain error: there isn't defined at least a 2d contact for " +
291 frame_name);
292 break;
293 }
294 }
295 } else {
296 8743 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
297 8743 d2->impulses->impulses.begin();
298
1/2
✓ Branch 4 taken 16705 times.
✗ Branch 5 not taken.
 16705 it != d2->impulses->impulses.end(); ++it) {
299
2/2
✓ Branch 2 taken 8743 times.
✓ Branch 3 taken 7962 times.
 16705 if (it->second->frame == id) {
300 8743 ImpulseData3DTpl<Scalar>* d3d =
301
1/2
✓ Branch 2 taken 8743 times.
✗ Branch 3 not taken.
 8743 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
302
2/2
✓ Branch 0 taken 4360 times.
✓ Branch 1 taken 4383 times.
 8743 if (d3d != NULL) {
303 4360 contact_type = Contact3D;
304 4360 found_contact = true;
305 4360 contact = it->second;
306 4360 break;
307 }
308 4383 ImpulseData6DTpl<Scalar>* d6d =
309
1/2
✓ Branch 2 taken 4383 times.
✗ Branch 3 not taken.
 4383 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
310
1/2
✓ Branch 0 taken 4383 times.
✗ Branch 1 not taken.
 4383 if (d6d != NULL) {
311 4383 contact_type = Contact6D;
312 4383 found_contact = true;
313 4383 contact = it->second;
314 4383 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 }
323
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 72435 times.
 72435 if (!found_contact) {
324 ✗ throw_pretty("Domain error: there isn't defined contact data for " +
325 frame_name);
326 }
327 72435 }
328
329 boost::shared_ptr<ForceDataAbstractTpl<Scalar> >
330 contact; //!< Contact force data
331 ContactType contact_type; //!< Type of contact (2D / 3D / 6D)
332 using Base::r;
333 using Base::Ru;
334 using Base::Rx;
335 using Base::shared;
336 };
337
338 } // namespace crocoddyl
339
340 /* --- Details -------------------------------------------------------------- */
341 /* --- Details -------------------------------------------------------------- */
342 /* --- Details -------------------------------------------------------------- */
343 #include "crocoddyl/multibody/residuals/contact-friction-cone.hxx"
344
345 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
346