GCC Code Coverage Report

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