GCC Code Coverage Report

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