GCC Code Coverage Report

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