GCC Code Coverage Report

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