GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-3d.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 38 39 97.4%
Branches: 66 134 49.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University f_world 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_CONTACTS_CONTACT_3D_HPP_
11 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
12
13 #include <pinocchio/algorithm/frames.hpp>
14 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
15 #include <pinocchio/multibody/data.hpp>
16 #include <pinocchio/spatial/motion.hpp>
17
18 #include "crocoddyl/core/utils/deprecate.hpp"
19 #include "crocoddyl/multibody/contact-base.hpp"
20 #include "crocoddyl/multibody/fwd.hpp"
21
22 namespace crocoddyl {
23
24 template <typename _Scalar>
25 class ContactModel3DTpl : public ContactModelAbstractTpl<_Scalar> {
26 public:
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 ✗ CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel3DTpl)
29
30 typedef _Scalar Scalar;
31 typedef MathBaseTpl<Scalar> MathBase;
32 typedef ContactModelAbstractTpl<Scalar> Base;
33 typedef ContactData3DTpl<Scalar> Data;
34 typedef StateMultibodyTpl<Scalar> StateMultibody;
35 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
36 typedef typename MathBase::Matrix3s Matrix3s;
37 typedef typename MathBase::Vector2s Vector2s;
38 typedef typename MathBase::Vector3s Vector3s;
39 typedef typename MathBase::VectorXs VectorXs;
40
41 /**
42 * @brief Initialize the 3d contact model
43 *
44 * To learn more about the computation of the contact derivatives in different
45 * frames see
46 * S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
47 * Frames: Application to Polishing with Talos, ICHR 2022
48 *
49 * @param[in] state State of the multibody system
50 * @param[in] id Reference frame id of the contact
51 * @param[in] xref Contact position used for the Baumgarte stabilization
52 * @param[in] type Type of contact
53 * @param[in] nu Dimension of the control vector
54 * @param[in] gains Baumgarte stabilization gains
55 */
56 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
57 const pinocchio::FrameIndex id, const Vector3s& xref,
58 const pinocchio::ReferenceFrame type, const std::size_t nu,
59 const Vector2s& gains = Vector2s::Zero());
60
61 /**
62 * @brief Initialize the 3d contact model
63 *
64 * The default `nu` is obtained from `StateAbstractTpl::get_nv()`. To learn
65 * more about the computation of the contact derivatives in different frames
66 * see
67 * S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
68 * Frames: Application to Polishing with Talos, ICHR 2022
69 *
70 * @param[in] state State of the multibody system
71 * @param[in] id Reference frame id of the contact
72 * @param[in] xref Contact position used for the Baumgarte stabilization
73 * @param[in] type Type of contact
74 * @param[in] gains Baumgarte stabilization gains
75 */
76 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
77 const pinocchio::FrameIndex id, const Vector3s& xref,
78 const pinocchio::ReferenceFrame type,
79 const Vector2s& gains = Vector2s::Zero());
80
81 DEPRECATED(
82 "Use constructor that passes the type type of contact, this assumes is "
83 "pinocchio::LOCAL",
84 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
85 const pinocchio::FrameIndex id, const Vector3s& xref,
86 const std::size_t nu,
87 const Vector2s& gains = Vector2s::Zero());)
88 DEPRECATED(
89 "Use constructor that passes the type type of contact, this assumes is "
90 "pinocchio::LOCAL",
91 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
92 const pinocchio::FrameIndex id, const Vector3s& xref,
93 const Vector2s& gains = Vector2s::Zero());)
94 2378 virtual ~ContactModel3DTpl() = default;
95
96 /**
97 * @brief Compute the 3d contact Jacobian and drift
98 *
99 * @param[in] data 3d contact data
100 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
101 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
102 */
103 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
104 const Eigen::Ref<const VectorXs>& x) override;
105
106 /**
107 * @brief Compute the derivatives of the 3d contact holonomic constraint
108 *
109 * @param[in] data 3d contact data
110 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
111 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
112 */
113 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
114 const Eigen::Ref<const VectorXs>& x) override;
115
116 /**
117 * @brief Convert the force into a stack of spatial forces
118 *
119 * @param[in] data 3d contact data
120 * @param[in] force 3d force
121 */
122 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
123 const VectorXs& force) override;
124
125 /**
126 * @brief Create the 3d contact data
127 */
128 virtual std::shared_ptr<ContactDataAbstract> createData(
129 pinocchio::DataTpl<Scalar>* const data) override;
130
131 /**
132 * @brief Cast the contact-3d model to a different scalar type.
133 *
134 * It is useful for operations requiring different precision or scalar types.
135 *
136 * @tparam NewScalar The new scalar type to cast to.
137 * @return ContactModel3DTpl<NewScalar> A contact model with the
138 * new scalar type.
139 */
140 template <typename NewScalar>
141 ContactModel3DTpl<NewScalar> cast() const;
142
143 /**
144 * @brief Return the reference frame translation
145 */
146 const Vector3s& get_reference() const;
147
148 /**
149 * @brief Return the Baumgarte stabilization gains
150 */
151 const Vector2s& get_gains() const;
152
153 /**
154 * @brief Modify the reference frame translation
155 */
156 void set_reference(const Vector3s& reference);
157
158 /**
159 * @brief Print relevant information of the 3d contact model
160 *
161 * @param[out] os Output stream object
162 */
163 virtual void print(std::ostream& os) const override;
164
165 protected:
166 using Base::id_;
167 using Base::nc_;
168 using Base::nu_;
169 using Base::state_;
170 using Base::type_;
171
172 private:
173 Vector3s xref_; //!< Contact position used for the Baumgarte stabilization
174 Vector2s gains_; //!< Baumgarte stabilization gains
175 };
176
177 template <typename _Scalar>
178 struct ContactData3DTpl : public ContactDataAbstractTpl<_Scalar> {
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180
181 typedef _Scalar Scalar;
182 typedef MathBaseTpl<Scalar> MathBase;
183 typedef ContactDataAbstractTpl<Scalar> Base;
184 typedef typename MathBase::Vector3s Vector3s;
185 typedef typename MathBase::Matrix3s Matrix3s;
186 typedef typename MathBase::Matrix3xs Matrix3xs;
187 typedef typename MathBase::Matrix6xs Matrix6xs;
188 typedef typename pinocchio::MotionTpl<Scalar> Motion;
189 typedef typename pinocchio::ForceTpl<Scalar> Force;
190
191 template <template <typename Scalar> class Model>
192 92612 ContactData3DTpl(Model<Scalar>* const model,
193 pinocchio::DataTpl<Scalar>* const data)
194 : Base(model, data),
195 92612 v(Motion::Zero()),
196
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 f_local(Force::Zero()),
197
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 da0_local_dx(3, model->get_state()->get_ndx()),
198
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 fJf(6, model->get_state()->get_nv()),
199
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 v_partial_dq(6, model->get_state()->get_nv()),
200
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 a_partial_dq(6, model->get_state()->get_nv()),
201
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 a_partial_dv(6, model->get_state()->get_nv()),
202
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 a_partial_da(6, model->get_state()->get_nv()),
203
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 fXjdv_dq(6, model->get_state()->get_nv()),
204
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 fXjda_dq(6, model->get_state()->get_nv()),
205
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
 92612 fXjda_dv(6, model->get_state()->get_nv()),
206
13/26
✓ Branch 2 taken 92586 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92586 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92586 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92586 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 92586 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 92586 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 92586 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 92586 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 92586 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 92586 times.
✗ Branch 33 not taken.
✓ Branch 36 taken 92586 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 92586 times.
✗ Branch 40 not taken.
 277836 fJf_df(3, model->get_state()->get_nv()) {
207
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 frame = model->get_id();
208
3/6
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92586 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 92586 times.
✗ Branch 11 not taken.
 92612 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
209
2/4
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92586 times.
✗ Branch 5 not taken.
 92612 fXj = jMf.inverse().toActionMatrix();
210
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a0_local.setZero();
211
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 dp.setZero();
212
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 dp_local.setZero();
213
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 da0_local_dx.setZero();
214
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 fJf.setZero();
215
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 v_partial_dq.setZero();
216
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a_partial_dq.setZero();
217
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a_partial_dv.setZero();
218
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a_partial_da.setZero();
219
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 vv_skew.setZero();
220
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 vw_skew.setZero();
221
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a0_skew.setZero();
222
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 a0_world_skew.setZero();
223
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 dp_skew.setZero();
224
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 f_skew.setZero();
225
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 fXjdv_dq.setZero();
226
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 fXjda_dq.setZero();
227
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 fXjda_dv.setZero();
228
1/2
✓ Branch 1 taken 92586 times.
✗ Branch 2 not taken.
 92612 fJf_df.setZero();
229 92612 }
230 185148 virtual ~ContactData3DTpl() = default;
231
232 using Base::a0;
233 using Base::da0_dx;
234 using Base::df_du;
235 using Base::df_dx;
236 using Base::f;
237 using Base::frame;
238 using Base::fXj;
239 using Base::Jc;
240 using Base::jMf;
241 using Base::pinocchio;
242
243 Motion v;
244 Vector3s a0_local;
245 Vector3s dp;
246 Vector3s dp_local;
247 Force f_local;
248 Matrix3xs da0_local_dx;
249 Matrix6xs fJf;
250 Matrix6xs v_partial_dq;
251 Matrix6xs a_partial_dq;
252 Matrix6xs a_partial_dv;
253 Matrix6xs a_partial_da;
254 Matrix3s vv_skew;
255 Matrix3s vw_skew;
256 Matrix3s a0_skew;
257 Matrix3s a0_world_skew;
258 Matrix3s dp_skew;
259 Matrix3s f_skew;
260 Matrix6xs fXjdv_dq;
261 Matrix6xs fXjda_dq;
262 Matrix6xs fXjda_dv;
263 Matrix3xs fJf_df;
264 };
265
266 } // namespace crocoddyl
267
268 /* --- Details -------------------------------------------------------------- */
269 /* --- Details -------------------------------------------------------------- */
270 /* --- Details -------------------------------------------------------------- */
271 #include "crocoddyl/multibody/contacts/contact-3d.hxx"
272
273 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel3DTpl)
274 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData3DTpl)
275
276 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
277