GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-6d.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 33 0.0%
Branches: 0 108 0.0%
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 "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 ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 ✗ CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel6DTpl)
24
25 typedef _Scalar Scalar;
26 typedef MathBaseTpl<Scalar> MathBase;
27 typedef ContactModelAbstractTpl<Scalar> Base;
28 typedef ContactData6DTpl<Scalar> Data;
29 typedef StateMultibodyTpl<Scalar> StateMultibody;
30 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
31 typedef pinocchio::SE3Tpl<Scalar> SE3;
32 typedef typename MathBase::Vector2s Vector2s;
33 typedef typename MathBase::Vector3s Vector3s;
34 typedef typename MathBase::VectorXs VectorXs;
35 typedef typename MathBase::Matrix3s Matrix3s;
36
37 /**
38 * @brief Initialize the 6d contact model
39 *
40 * To learn more about the computation of the contact derivatives in different
41 * frames see
42 * S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
43 * Frames: Application to Polishing with Talos, ICHR 2022
44 *
45 * @param[in] state State of the multibody system
46 * @param[in] id Reference frame id of the contact
47 * @param[in] pref Contact placement used for the Baumgarte stabilization
48 * @param[in] type Type of contact
49 * @param[in] nu Dimension of the control vector
50 * @param[in] gains Baumgarte stabilization gains
51 */
52 ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
53 const pinocchio::FrameIndex id, const SE3& pref,
54 const pinocchio::ReferenceFrame type, const std::size_t nu,
55 const Vector2s& gains = Vector2s::Zero());
56
57 /**
58 * @brief Initialize the 6d 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] pref Contact placement used for the Baumgarte stabilization
69 * @param[in] type Type of contact
70 * @param[in] gains Baumgarte stabilization gains
71 */
72 ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
73 const pinocchio::FrameIndex id, const SE3& pref,
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 ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
81 const pinocchio::FrameIndex id, const SE3& pref,
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 ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
88 const pinocchio::FrameIndex id, const SE3& pref,
89 const Vector2s& gains = Vector2s::Zero());)
90 ✗ virtual ~ContactModel6DTpl() = default;
91
92 /**
93 * @brief Compute the 3d contact Jacobian and drift
94 *
95 * @param[in] data 3d 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 6d contact holonomic constraint
104 *
105 * @param[in] data 6d 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 6d contact data
116 * @param[in] force 6d force
117 */
118 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
119 const VectorXs& force) override;
120
121 /**
122 * @brief Create the 6d contact data
123 */
124 virtual std::shared_ptr<ContactDataAbstract> createData(
125 pinocchio::DataTpl<Scalar>* const data) override;
126
127 /**
128 * @brief Cast the contact-6d 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 ContactModel6DTpl<NewScalar> A contact model with the
134 * new scalar type.
135 */
136 template <typename NewScalar>
137 ContactModel6DTpl<NewScalar> cast() const;
138
139 /**
140 * @brief Return the reference frame placement
141 */
142 const SE3& get_reference() const;
143
144 /**
145 * @brief Return the Baumgarte stabilization gains
146 */
147 const Vector2s& get_gains() const;
148
149 /**
150 * @brief Modify the reference frame placement
151 */
152 void set_reference(const SE3& reference);
153
154 /**
155 * @brief Print relevant information of the 6d contact model
156 *
157 * @param[out] os Output stream object
158 */
159 virtual void print(std::ostream& os) const override;
160
161 protected:
162 using Base::id_;
163 using Base::nc_;
164 using Base::nu_;
165 using Base::state_;
166 using Base::type_;
167
168 private:
169 SE3 pref_; //!< Contact placement used for the Baumgarte stabilization
170 Vector2s gains_; //!< Baumgarte stabilization gains
171 };
172
173 template <typename _Scalar>
174 struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
175 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
176
177 typedef _Scalar Scalar;
178 typedef MathBaseTpl<Scalar> MathBase;
179 typedef ContactDataAbstractTpl<Scalar> Base;
180 typedef typename MathBase::Matrix3s Matrix3s;
181 typedef typename MathBase::Matrix6xs Matrix6xs;
182 typedef typename MathBase::Matrix6s Matrix6s;
183 typedef typename MathBase::MatrixXs MatrixXs;
184 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
185 typedef typename pinocchio::MotionTpl<Scalar> Motion;
186 typedef typename pinocchio::ForceTpl<Scalar> Force;
187
188 template <template <typename Scalar> class Model>
189 ✗ ContactData6DTpl(Model<Scalar>* const model,
190 pinocchio::DataTpl<Scalar>* const data)
191 : Base(model, data),
192 ✗ rMf(SE3::Identity()),
193 ✗ lwaMl(SE3::Identity()),
194 ✗ v(Motion::Zero()),
195 ✗ a0_local(Motion::Zero()),
196 ✗ f_local(Force::Zero()),
197 ✗ da0_local_dx(6, model->get_state()->get_ndx()),
198 ✗ fJf(6, model->get_state()->get_nv()),
199 ✗ v_partial_dq(6, model->get_state()->get_nv()),
200 ✗ a_partial_dq(6, model->get_state()->get_nv()),
201 ✗ a_partial_dv(6, model->get_state()->get_nv()),
202 ✗ a_partial_da(6, model->get_state()->get_nv()),
203 ✗ fJf_df(6, model->get_state()->get_nv()) {
204 ✗ frame = model->get_id();
205 ✗ jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
206 ✗ fXj = jMf.inverse().toActionMatrix();
207 ✗ da0_local_dx.setZero();
208 ✗ fJf.setZero();
209 ✗ v_partial_dq.setZero();
210 ✗ a_partial_dq.setZero();
211 ✗ a_partial_dv.setZero();
212 ✗ a_partial_da.setZero();
213 ✗ av_world_skew.setZero();
214 ✗ aw_world_skew.setZero();
215 ✗ av_skew.setZero();
216 ✗ aw_skew.setZero();
217 ✗ fv_skew.setZero();
218 ✗ fw_skew.setZero();
219 ✗ rMf_Jlog6.setZero();
220 ✗ fJf_df.setZero();
221 }
222 ✗ virtual ~ContactData6DTpl() = default;
223
224 using Base::a0;
225 using Base::da0_dx;
226 using Base::df_du;
227 using Base::df_dx;
228 using Base::f;
229 using Base::frame;
230 using Base::fXj;
231 using Base::Jc;
232 using Base::jMf;
233 using Base::pinocchio;
234
235 SE3 rMf;
236 SE3 lwaMl;
237 Motion v;
238 Motion a0_local;
239 Force f_local;
240 Matrix6xs da0_local_dx;
241 MatrixXs fJf;
242 Matrix6xs v_partial_dq;
243 Matrix6xs a_partial_dq;
244 Matrix6xs a_partial_dv;
245 Matrix6xs a_partial_da;
246 Matrix3s av_world_skew;
247 Matrix3s aw_world_skew;
248 Matrix3s av_skew;
249 Matrix3s aw_skew;
250 Matrix3s fv_skew;
251 Matrix3s fw_skew;
252 Matrix6s rMf_Jlog6;
253 MatrixXs fJf_df;
254 };
255
256 } // namespace crocoddyl
257 /* --- Details -------------------------------------------------------------- */
258 /* --- Details -------------------------------------------------------------- */
259 /* --- Details -------------------------------------------------------------- */
260 #include "crocoddyl/multibody/contacts/contact-6d.hxx"
261
262 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel6DTpl)
263 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData6DTpl)
264
265 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
266