GCC Code Coverage Report

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