GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-cop-position.hpp
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 43 53 81.1%
Branches: 25 98 25.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2024, University of Duisburg-Essen,
5 // University of Edinburgh, 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_COP_POSITION_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_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-3d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
18 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19 #include "crocoddyl/multibody/cop-support.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 Center of pressure residual function
33 *
34 * It builds a residual function that bounds the center of pressure (CoP) for
35 * one contact surface to lie inside a certain geometric area defined around the
36 * reference contact frame. The residual residual vector is described as
37 * \f$\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\geq\mathbf{0}\f$, where \f[
38 * \mathbf{A}=
39 * \begin{bmatrix} 0 & 0 & X/2 & 0 & -1 & 0 \\ 0 & 0 & X/2 & 0 & 1 & 0 \\ 0
40 * & 0 & Y/2 & 1 & 0 & 0 \\ 0 & 0 & Y/2 & -1 & 0 & 0 \end{bmatrix} \f] is the
41 * inequality matrix and \f$\boldsymbol{\lambda}\f$ is the reference spatial
42 * contact force in the frame coordinate. The CoP lies inside the convex hull of
43 * the foot, see eq.(18-19) of
44 * https://hal.archives-ouvertes.fr/hal-02108449/document is we have:
45 * \f[
46 * \begin{align}\begin{split}\tau^x &\leq
47 * Yf^z \\-\tau^x &\leq Yf^z \\\tau^y &\leq Yf^z \\-\tau^y &\leq Yf^z
48 * \end{split}\end{align}
49 * \f]
50 * with \f$\boldsymbol{\lambda}= \begin{bmatrix}f^x & f^y & f^z & \tau^x &
51 * \tau^y & \tau^z \end{bmatrix}^T\f$.
52 *
53 * The residual is computed, from the residual vector \f$\mathbf{r}\f$, through
54 * an user defined activation model. Additionally, the contact frame id, the
55 * desired support region for the CoP and the inequality matrix are handled
56 * within `CoPSupportTpl`. The force vector \f$\boldsymbol{\lambda}\f$ and its
57 * derivatives are computed by `DifferentialActionModelContactFwdDynamicsTpl` or
58 * `ActionModelImpulseFwdDynamicTpl`. These values are stored in a shared data
59 * (i.e., `DataCollectorContactTpl` or `DataCollectorImpulseTpl`). Note that
60 * this residual function cannot be used with other action models.
61 *
62 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`,
63 * `DifferentialActionModelContactFwdDynamicsTpl`,
64 * `ActionModelImpulseFwdDynamicTpl`, `DataCollectorForceTpl`
65 */
66 template <typename _Scalar>
67 class ResidualModelContactCoPPositionTpl
68 : public ResidualModelAbstractTpl<_Scalar> {
69 public:
70 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71
72 typedef _Scalar Scalar;
73 typedef MathBaseTpl<Scalar> MathBase;
74 typedef ResidualModelAbstractTpl<Scalar> Base;
75 typedef ResidualDataContactCoPPositionTpl<Scalar> Data;
76 typedef StateMultibodyTpl<Scalar> StateMultibody;
77 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
78 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
79 typedef CoPSupportTpl<Scalar> CoPSupport;
80 typedef typename MathBase::VectorXs VectorXs;
81 typedef typename MathBase::MatrixXs MatrixXs;
82 typedef typename MathBase::Matrix46s Matrix46;
83
84 /**
85 * @brief Initialize the contact CoP residual model
86 *
87 * Note that for the inverse-dynamic cases, the control vector contains the
88 * generalized accelerations, torques, and all the contact forces.
89 *
90 * @param[in] state State of the multibody system
91 * @param[in] id Reference frame id
92 * @param[in] cref Reference support region of the CoP
93 * @param[in] nu Dimension of control vector
94 * @param[in] fwddyn Indicates that we have a forward dynamics problem (true)
95 * or inverse dynamics (false)
96 */
97 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
98 const pinocchio::FrameIndex id,
99 const CoPSupport& cref,
100 const std::size_t nu,
101 const bool fwddyn = true);
102
103 /**
104 * @brief Initialize the contact CoP residual model
105 *
106 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`. Note
107 * that this constructor can be used for forward-dynamics cases only.
108 *
109 * @param[in] state State of the multibody system
110 * @param[in] id Reference frame id
111 * @param[in] cref Reference support region of the CoP
112 */
113 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
114 const pinocchio::FrameIndex id,
115 const CoPSupport& cref);
116 virtual ~ResidualModelContactCoPPositionTpl();
117
118 /**
119 * @brief Compute the contact CoP residual
120 *
121 * The CoP residual is computed based on the \f$\mathbf{A}\f$ matrix, the
122 * force vector is computed by `DifferentialActionModelContactFwdDynamicsTpl`
123 * or `ActionModelImpulseFwdDynamicTpl` which is stored in
124 * `DataCollectorContactTpl` or `DataCollectorImpulseTpl`.
125 *
126 * @param[in] data Contact CoP data
127 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
128 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
129 */
130 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
131 const Eigen::Ref<const VectorXs>& x,
132 const Eigen::Ref<const VectorXs>& u);
133
134 /**
135 * @brief Compute the residual vector for nodes that depends only on the state
136 *
137 * It updates the residual vector based on the state only (i.e., it ignores
138 * the contact forces). This function is used in the terminal nodes of an
139 * optimal control problem.
140 *
141 * @param[in] data Residual data
142 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
143 */
144 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
145 const Eigen::Ref<const VectorXs>& x);
146
147 /**
148 * @brief Compute the Jacobians of the contact CoP residual
149 *
150 * The CoP residual is computed based on the \f$\mathbf{A}\f$ matrix, the
151 * force vector is computed by `DifferentialActionModelContactFwdDynamicsTpl`
152 * or `ActionModelImpulseFwdDynamicTpl` which is stored in
153 * `DataCollectorContactTpl` or `DataCollectorImpulseTpl`.
154 *
155 * @param[in] data Contact CoP data
156 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
157 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
158 */
159 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x,
161 const Eigen::Ref<const VectorXs>& u);
162
163 /**
164 * @brief Compute the Jacobian of the residual functions with respect to the
165 * state only
166 *
167 * It updates the Jacobian of the residual function based on the state only
168 * (i.e., it ignores the contact forces). This function is used in the
169 * terminal nodes of an optimal control problem.
170 *
171 * @param[in] data Residual data
172 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
173 */
174 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
175 const Eigen::Ref<const VectorXs>& x);
176
177 /**
178 * @brief Create the contact CoP residual data
179 *
180 * Each residual model has its own data that needs to be allocated.
181 * This function returns the allocated data for a predefined residual.
182 *
183 * @param[in] data Shared data (it should be of type
184 * `DataCollectorContactTpl`)
185 * @return the residual data.
186 */
187 virtual std::shared_ptr<ResidualDataAbstract> createData(
188 DataCollectorAbstract* const data);
189
190 /**
191 * @brief Update the Jacobians of the contact friction cone residual
192 *
193 * @param[in] data Contact friction cone residual data
194 */
195 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
196
197 /**
198 * @brief Indicates if we are using the forward-dynamics (true) or
199 * inverse-dynamics (false)
200 */
201 bool is_fwddyn() const;
202
203 /**
204 * @brief Return the reference frame id
205 */
206 pinocchio::FrameIndex get_id() const;
207
208 /**
209 * @brief Return the reference support region of CoP
210 */
211 const CoPSupport& 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(pinocchio::FrameIndex id);)
218
219 /**
220 * @brief Modify the reference support region of CoP
221 */
222 void set_reference(const CoPSupport& reference);
223
224 /**
225 * @brief Print relevant information of the cop-position residual
226 *
227 * @param[out] os Output stream object
228 */
229 virtual void print(std::ostream& os) const;
230
231 protected:
232 using Base::nu_;
233 using Base::state_;
234
235 private:
236 bool fwddyn_; //!< Indicates if we are using this function for forward
237 //!< dynamics
238 bool update_jacobians_; //!< Indicates if we need to update the Jacobians
239 //!< (used for inverse dynamics case)
240 pinocchio::FrameIndex id_; //!< Reference frame id
241 CoPSupport cref_; //!< Reference support region of CoP
242 };
243
244 template <typename _Scalar>
245 struct ResidualDataContactCoPPositionTpl
246 : public ResidualDataAbstractTpl<_Scalar> {
247 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
248
249 typedef _Scalar Scalar;
250 typedef MathBaseTpl<Scalar> MathBase;
251 typedef ResidualDataAbstractTpl<Scalar> Base;
252 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
253 typedef StateMultibodyTpl<Scalar> StateMultibody;
254 typedef typename MathBase::MatrixXs MatrixXs;
255
256 template <template <typename Scalar> class Model>
257 3418 ResidualDataContactCoPPositionTpl(Model<Scalar>* const model,
258 DataCollectorAbstract* const data)
259 3418 : Base(model, data) {
260 // Check that proper shared data has been passed
261 3418 bool is_contact = true;
262 3418 DataCollectorContactTpl<Scalar>* d1 =
263
1/2
✓ Branch 0 taken 3418 times.
✗ Branch 1 not taken.
 3418 dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
264 3418 DataCollectorImpulseTpl<Scalar>* d2 =
265
1/2
✓ Branch 0 taken 3418 times.
✗ Branch 1 not taken.
 3418 dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
266
3/4
✓ Branch 0 taken 1692 times.
✓ Branch 1 taken 1726 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1692 times.
 3418 if (d1 == NULL && d2 == NULL) {
267 ✗ throw_pretty(
268 "Invalid argument: the shared data should be derived from "
269 "DataCollectorContact or DataCollectorImpulse");
270 }
271
2/2
✓ Branch 0 taken 1692 times.
✓ Branch 1 taken 1726 times.
 3418 if (d2 != NULL) {
272 1692 is_contact = false;
273 }
274
275 // Avoids data casting at runtime
276 3418 const pinocchio::FrameIndex id = model->get_id();
277 3418 const std::shared_ptr<StateMultibody>& state =
278 std::static_pointer_cast<StateMultibody>(model->get_state());
279
1/2
✓ Branch 5 taken 3418 times.
✗ Branch 6 not taken.
 3418 std::string frame_name = state->get_pinocchio()->frames[id].name;
280 3418 bool found_contact = false;
281
2/2
✓ Branch 0 taken 1726 times.
✓ Branch 1 taken 1692 times.
 3418 if (is_contact) {
282 1726 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
283 1726 d1->contacts->contacts.begin();
284
1/2
✓ Branch 4 taken 2588 times.
✗ Branch 5 not taken.
 2588 it != d1->contacts->contacts.end(); ++it) {
285
2/2
✓ Branch 2 taken 1726 times.
✓ Branch 3 taken 862 times.
 2588 if (it->second->frame == id) {
286 1726 ContactData3DTpl<Scalar>* d3d =
287
1/2
✓ Branch 2 taken 1726 times.
✗ Branch 3 not taken.
 1726 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
288
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1726 times.
 1726 if (d3d != NULL) {
289 ✗ found_contact = true;
290 ✗ contact = it->second;
291 ✗ throw_pretty(
292 "Domain error: there isn't defined at least a 6d contact for " +
293 frame_name);
294 break;
295 }
296 1726 ContactData6DTpl<Scalar>* d6d =
297
1/2
✓ Branch 2 taken 1726 times.
✗ Branch 3 not taken.
 1726 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
298
1/2
✓ Branch 0 taken 1726 times.
✗ Branch 1 not taken.
 1726 if (d6d != NULL) {
299 1726 found_contact = true;
300 1726 contact = it->second;
301 1726 break;
302 }
303 ✗ throw_pretty(
304 "Domain error: there isn't defined at least a 6d contact for " +
305 frame_name);
306 break;
307 }
308 }
309 } else {
310 1692 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
311 1692 d2->impulses->impulses.begin();
312
1/2
✓ Branch 4 taken 2772 times.
✗ Branch 5 not taken.
 2772 it != d2->impulses->impulses.end(); ++it) {
313
2/2
✓ Branch 2 taken 1692 times.
✓ Branch 3 taken 1080 times.
 2772 if (it->second->frame == id) {
314 1692 ImpulseData3DTpl<Scalar>* d3d =
315
1/2
✓ Branch 2 taken 1692 times.
✗ Branch 3 not taken.
 1692 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
316
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1692 times.
 1692 if (d3d != NULL) {
317 ✗ found_contact = true;
318 ✗ contact = it->second;
319 ✗ throw_pretty(
320 "Domain error: there isn't defined at least a 6d contact for " +
321 frame_name);
322 break;
323 }
324 1692 ImpulseData6DTpl<Scalar>* d6d =
325
1/2
✓ Branch 2 taken 1692 times.
✗ Branch 3 not taken.
 1692 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
326
1/2
✓ Branch 0 taken 1692 times.
✗ Branch 1 not taken.
 1692 if (d6d != NULL) {
327 1692 found_contact = true;
328 1692 contact = it->second;
329 1692 break;
330 }
331 ✗ throw_pretty(
332 "Domain error: there isn't defined at least a 6d contact for " +
333 frame_name);
334 break;
335 }
336 }
337 }
338
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3418 times.
 3418 if (!found_contact) {
339 ✗ throw_pretty("Domain error: there isn't defined contact data for " +
340 frame_name);
341 }
342 3418 }
343
344 pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
345 std::shared_ptr<ForceDataAbstractTpl<Scalar> > contact; //!< Contact force
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-cop-position.hxx"
358
359 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
360