GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-force.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 57 65 87.7%
Branches: 29 82 35.4%
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_FORCE_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_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-1d.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/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 Define a contact force residual function
33 *
34 * This residual function is defined as
35 * \f$\mathbf{r}=\boldsymbol{\lambda}-\boldsymbol{\lambda}^*\f$, where
36 * \f$\boldsymbol{\lambda}, \boldsymbol{\lambda}^*\f$ are the current and
37 * reference spatial forces, respectively. The current spatial forces
38 * \f$\boldsymbol{\lambda}\in\mathbb{R}^{nc}\f$ is computed by
39 * `DifferentialActionModelContactFwdDynamicsTpl` or
40 * `ActionModelImpulseFwdDynamicTpl`, with `nc` as the dimension of the contact.
41 *
42 * Both residual and residual Jacobians are computed analytically, where the
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 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`,
52 * `DifferentialActionModelContactFwdDynamicsTpl`,
53 * `ActionModelImpulseFwdDynamicTpl`, `DataCollectorContactTpl`,
54 * `DataCollectorImpulseTpl`
55 */
56 template <typename _Scalar>
57 class ResidualModelContactForceTpl : public ResidualModelAbstractTpl<_Scalar> {
58 public:
59 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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(boost::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(boost::shared_ptr<StateMultibody> state,
105 const pinocchio::FrameIndex id,
106 const Force& fref, const std::size_t nc);
107 virtual ~ResidualModelContactForceTpl();
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 boost::shared_ptr<ResidualDataAbstract>& data,
122 const Eigen::Ref<const VectorXs>& x,
123 const Eigen::Ref<const VectorXs>& u);
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 boost::shared_ptr<ResidualDataAbstract>& data,
136 const Eigen::Ref<const VectorXs>& x);
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 boost::shared_ptr<ResidualDataAbstract>& data,
151 const Eigen::Ref<const VectorXs>& x,
152 const Eigen::Ref<const VectorXs>& u);
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 boost::shared_ptr<ResidualDataAbstract>& data,
166 const Eigen::Ref<const VectorXs>& x);
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 boost::shared_ptr<ResidualDataAbstract> createData(
175 DataCollectorAbstract* const data);
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 boost::shared_ptr<ResidualDataAbstract>& data);
183
184 /**
185 * @brief Indicates if we are using the forward-dynamics (true) or
186 * inverse-dynamics (false)
187 */
188 bool is_fwddyn() const;
189
190 /**
191 * @brief Return the reference frame id
192 */
193 pinocchio::FrameIndex get_id() const;
194
195 /**
196 * @brief Return the reference spatial contact force in the contact
197 * coordinates
198 */
199 const Force& get_reference() const;
200
201 /**
202 * @brief Modify the reference frame id
203 */
204 DEPRECATED("Do not use set_id, instead create a new model",
205 void set_id(const pinocchio::FrameIndex id);)
206
207 /**
208 * @brief Modify the reference spatial contact force in the contact
209 * coordinates
210 */
211 void set_reference(const Force& reference);
212
213 /**
214 * @brief Print relevant information of the contact-force residual
215 *
216 * @param[out] os Output stream object
217 */
218 virtual void print(std::ostream& os) const;
219
220 protected:
221 using Base::nr_;
222 using Base::nu_;
223 using Base::state_;
224
225 private:
226 bool fwddyn_; //!< Indicates if we are using this function for forward
227 //!< dynamics
228 bool update_jacobians_; //!< Indicates if we need to update the Jacobians
229 //!< (used for inverse dynamics case)
230 pinocchio::FrameIndex id_; //!< Reference frame id
231 Force fref_; //!< Reference spatial contact force in the contact coordinates
232 };
233
234 template <typename _Scalar>
235 struct ResidualDataContactForceTpl : 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 pinocchio::ForceTpl<Scalar> Force;
245 typedef StateMultibodyTpl<Scalar> StateMultibody;
246 typedef typename MathBase::MatrixXs MatrixXs;
247
248 template <template <typename Scalar> class Model>
249 161402 ResidualDataContactForceTpl(Model<Scalar>* const model,
250 DataCollectorAbstract* const data)
251 161402 : Base(model, data) {
252 161402 contact_type = ContactUndefined;
253
254 // Check that proper shared data has been passed
255 161402 bool is_contact = true;
256 161402 DataCollectorContactTpl<Scalar>* d1 =
257
1/2
✓ Branch 0 taken 161402 times.
✗ Branch 1 not taken.
 161402 dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
258 161402 DataCollectorImpulseTpl<Scalar>* d2 =
259
1/2
✓ Branch 0 taken 161402 times.
✗ Branch 1 not taken.
 161402 dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
260
3/4
✓ Branch 0 taken 11434 times.
✓ Branch 1 taken 149968 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11434 times.
 161402 if (d1 == NULL && d2 == NULL) {
261 ✗ throw_pretty(
262 "Invalid argument: the shared data should be derived from "
263 "DataCollectorContact or DataCollectorImpulse");
264 }
265
2/2
✓ Branch 0 taken 11434 times.
✓ Branch 1 taken 149968 times.
 161402 if (d2 != NULL) {
266 11434 is_contact = false;
267 }
268
269 // Avoids data casting at runtime
270 161402 const pinocchio::FrameIndex id = model->get_id();
271 161402 const boost::shared_ptr<StateMultibody>& state =
272 boost::static_pointer_cast<StateMultibody>(model->get_state());
273
1/2
✓ Branch 5 taken 161402 times.
✗ Branch 6 not taken.
 161402 std::string frame_name = state->get_pinocchio()->frames[id].name;
274 161402 bool found_contact = false;
275
2/2
✓ Branch 0 taken 149968 times.
✓ Branch 1 taken 11434 times.
 161402 if (is_contact) {
276 149968 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
277 149968 d1->contacts->contacts.begin();
278
1/2
✓ Branch 4 taken 297078 times.
✗ Branch 5 not taken.
 297078 it != d1->contacts->contacts.end(); ++it) {
279
2/2
✓ Branch 2 taken 149968 times.
✓ Branch 3 taken 147110 times.
 297078 if (it->second->frame == id) {
280 149968 ContactData1DTpl<Scalar>* d1d =
281
1/2
✓ Branch 2 taken 149968 times.
✗ Branch 3 not taken.
 149968 dynamic_cast<ContactData1DTpl<Scalar>*>(it->second.get());
282
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 149968 times.
 149968 if (d1d != NULL) {
283 ✗ contact_type = Contact1D;
284 ✗ found_contact = true;
285 ✗ contact = it->second;
286 ✗ break;
287 }
288 149968 ContactData3DTpl<Scalar>* d3d =
289
1/2
✓ Branch 2 taken 149968 times.
✗ Branch 3 not taken.
 149968 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
290
2/2
✓ Branch 0 taken 92794 times.
✓ Branch 1 taken 57174 times.
 149968 if (d3d != NULL) {
291 92794 contact_type = Contact3D;
292 92794 found_contact = true;
293 92794 contact = it->second;
294 92794 break;
295 }
296 57174 ContactData6DTpl<Scalar>* d6d =
297
1/2
✓ Branch 2 taken 57174 times.
✗ Branch 3 not taken.
 57174 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
298
1/2
✓ Branch 0 taken 57174 times.
✗ Branch 1 not taken.
 57174 if (d6d != NULL) {
299 57174 contact_type = Contact6D;
300 57174 found_contact = true;
301 57174 contact = it->second;
302 57174 break;
303 }
304 ✗ throw_pretty(
305 "Domain error: there isn't defined at least a 3d contact for " +
306 frame_name);
307 break;
308 }
309 }
310 } else {
311 11434 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
312 11434 d2->impulses->impulses.begin();
313
1/2
✓ Branch 4 taken 22087 times.
✗ Branch 5 not taken.
 22087 it != d2->impulses->impulses.end(); ++it) {
314
2/2
✓ Branch 2 taken 11434 times.
✓ Branch 3 taken 10653 times.
 22087 if (it->second->frame == id) {
315 11434 ImpulseData3DTpl<Scalar>* d3d =
316
1/2
✓ Branch 2 taken 11434 times.
✗ Branch 3 not taken.
 11434 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
317
2/2
✓ Branch 0 taken 4360 times.
✓ Branch 1 taken 7074 times.
 11434 if (d3d != NULL) {
318 4360 contact_type = Contact3D;
319 4360 found_contact = true;
320 4360 contact = it->second;
321 4360 break;
322 }
323 7074 ImpulseData6DTpl<Scalar>* d6d =
324
1/2
✓ Branch 2 taken 7074 times.
✗ Branch 3 not taken.
 7074 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
325
1/2
✓ Branch 0 taken 7074 times.
✗ Branch 1 not taken.
 7074 if (d6d != NULL) {
326 7074 contact_type = Contact6D;
327 7074 found_contact = true;
328 7074 contact = it->second;
329 7074 break;
330 }
331 ✗ throw_pretty(
332 "Domain error: there isn't defined at least a 3d impulse for " +
333 frame_name);
334 break;
335 }
336 }
337 }
338
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 161402 times.
 161402 if (!found_contact) {
339 ✗ throw_pretty(
340 "Domain error: there isn't defined contact/impulse data for " +
341 frame_name);
342 }
343 161402 }
344
345 boost::shared_ptr<ForceDataAbstractTpl<Scalar> >
346 contact; //!< Contact force data
347 ContactType contact_type; //!< Type of contact (3D / 6D)
348 using Base::r;
349 using Base::Ru;
350 using Base::Rx;
351 using Base::shared;
352 };
353
354 } // namespace crocoddyl
355
356 /* --- Details -------------------------------------------------------------- */
357 /* --- Details -------------------------------------------------------------- */
358 /* --- Details -------------------------------------------------------------- */
359 #include "crocoddyl/multibody/residuals/contact-force.hxx"
360
361 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
362