GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-1d.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 37 37 100.0%
Branches: 45 90 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2023, 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/core/utils/exception.hpp"
19 #include "crocoddyl/multibody/contact-base.hpp"
20 #include "crocoddyl/multibody/fwd.hpp"
21
22 namespace crocoddyl {
23
24 template <typename _Scalar>
25 class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
26 public:
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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(boost::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(boost::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(boost::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(boost::shared_ptr<StateMultibody> state,
93 const pinocchio::FrameIndex id, const Scalar xref,
94 const Vector2s& gains = Vector2s::Zero());)
95 virtual ~ContactModel1DTpl();
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 boost::shared_ptr<ContactDataAbstract>& data,
105 const Eigen::Ref<const VectorXs>& x);
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 boost::shared_ptr<ContactDataAbstract>& data,
115 const Eigen::Ref<const VectorXs>& x);
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 boost::shared_ptr<ContactDataAbstract>& data,
124 const VectorXs& force);
125
126 /**
127 * @brief Create the 1d contact data
128 */
129 virtual boost::shared_ptr<ContactDataAbstract> createData(
130 pinocchio::DataTpl<Scalar>* const data);
131
132 /**
133 * @brief Return the reference frame translation
134 */
135 const Scalar get_reference() const;
136
137 /**
138 * @brief Create the 1d contact data
139 */
140 const Vector2s& get_gains() const;
141
142 /**
143 * @brief Return the rotation of the reference frames's z axis
144 */
145 const Matrix3s& get_axis_rotation() const;
146
147 /**
148 * @brief Modify the reference frame translation
149 */
150 void set_reference(const Scalar reference);
151
152 /**
153 * @brief Modify the rotation of the reference frames's z axis
154 */
155 void set_axis_rotation(const Matrix3s& rotation);
156
157 /**
158 * @brief Print relevant information of the 1d contact model
159 *
160 * @param[out] os Output stream object
161 */
162 virtual void print(std::ostream& os) const;
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 Scalar xref_; //!< Contact position used for the Baumgarte stabilization
173 Matrix3s Raxis_; //!< Rotation of the reference frame's z-axis
174 Vector2s gains_; //!< Baumgarte stabilization gains
175 };
176
177 template <typename _Scalar>
178 struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180
181 typedef _Scalar Scalar;
182 typedef MathBaseTpl<Scalar> MathBase;
183 typedef ContactDataAbstractTpl<Scalar> Base;
184 typedef typename MathBase::Matrix2s Matrix2s;
185 typedef typename MathBase::Matrix3s Matrix3s;
186 typedef typename MathBase::Matrix3xs Matrix3xs;
187 typedef typename MathBase::Matrix6xs Matrix6xs;
188 typedef typename MathBase::Vector3s Vector3s;
189 typedef typename pinocchio::MotionTpl<Scalar> Motion;
190 typedef typename pinocchio::ForceTpl<Scalar> Force;
191
192 template <template <typename Scalar> class Model>
193 734 ContactData1DTpl(Model<Scalar>* const model,
194 pinocchio::DataTpl<Scalar>* const data)
195 : Base(model, data),
196 734 v(Motion::Zero()),
197
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 f_local(Force::Zero()),
198
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 da0_local_dx(3, model->get_state()->get_ndx()),
199
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 fJf(6, model->get_state()->get_nv()),
200
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 v_partial_dq(6, model->get_state()->get_nv()),
201
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 a_partial_dq(6, model->get_state()->get_nv()),
202
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 a_partial_dv(6, model->get_state()->get_nv()),
203
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 a_partial_da(6, model->get_state()->get_nv()),
204
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 fXjdv_dq(6, model->get_state()->get_nv()),
205
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 fXjda_dq(6, model->get_state()->get_nv()),
206
1/2
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 fXjda_dv(6, model->get_state()->get_nv()),
207
12/24
✓ Branch 2 taken 734 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 734 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 734 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 734 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 734 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 734 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 734 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 734 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 734 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 734 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 734 times.
✗ Branch 33 not taken.
✓ Branch 38 taken 734 times.
✗ Branch 39 not taken.
 2202 fJf_df(3, model->get_state()->get_nv()) {
208 734 frame = model->get_id();
209
1/2
✓ Branch 6 taken 734 times.
✗ Branch 7 not taken.
 734 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
210
2/4
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 734 times.
✗ Branch 5 not taken.
 734 fXj = jMf.inverse().toActionMatrix();
211
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a0_local.setZero();
212
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 dp.setZero();
213
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 dp_local.setZero();
214
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 da0_local_dx.setZero();
215
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 fJf.setZero();
216
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 v_partial_dq.setZero();
217
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a_partial_dq.setZero();
218
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a_partial_dv.setZero();
219
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a_partial_da.setZero();
220
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 vv_skew.setZero();
221
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 vw_skew.setZero();
222
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a0_skew.setZero();
223
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 a0_world_skew.setZero();
224
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 dp_skew.setZero();
225
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 f_skew.setZero();
226
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 fXjdv_dq.setZero();
227
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 fXjda_dq.setZero();
228
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 fXjda_dv.setZero();
229
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 oRf.setZero();
230
1/2
✓ Branch 1 taken 734 times.
✗ Branch 2 not taken.
 734 fJf_df.setZero();
231 734 }
232
233 using Base::a0;
234 using Base::da0_dx;
235 using Base::df_du;
236 using Base::df_dx;
237 using Base::f;
238 using Base::frame;
239 using Base::fXj;
240 using Base::Jc;
241 using Base::jMf;
242 using Base::pinocchio;
243
244 Motion v;
245 Vector3s a0_local;
246 Vector3s dp;
247 Vector3s dp_local;
248 Force f_local;
249 Matrix3xs da0_local_dx;
250 Matrix6xs fJf;
251 Matrix6xs v_partial_dq;
252 Matrix6xs a_partial_dq;
253 Matrix6xs a_partial_dv;
254 Matrix6xs a_partial_da;
255 Matrix3s vv_skew;
256 Matrix3s vw_skew;
257 Matrix3s a0_skew;
258 Matrix3s a0_world_skew;
259 Matrix3s dp_skew;
260 Matrix3s f_skew;
261 Matrix6xs fXjdv_dq;
262 Matrix6xs fXjda_dq;
263 Matrix6xs fXjda_dv;
264 Matrix2s oRf;
265 Matrix3xs fJf_df;
266 };
267
268 } // namespace crocoddyl
269
270 /* --- Details -------------------------------------------------------------- */
271 /* --- Details -------------------------------------------------------------- */
272 /* --- Details -------------------------------------------------------------- */
273 #include "crocoddyl/multibody/contacts/contact-1d.hxx"
274
275 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_
276