GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-2d.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 28 29 96.6%
Branches: 50 102 49.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_2D_HPP_
11 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_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/multibody/contact-base.hpp"
19 #include "crocoddyl/multibody/fwd.hpp"
20
21 namespace crocoddyl {
22
23 template <typename _Scalar>
24 class ContactModel2DTpl : public ContactModelAbstractTpl<_Scalar> {
25 public:
26 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 ✗ CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel2DTpl)
28
29 typedef _Scalar Scalar;
30 typedef MathBaseTpl<Scalar> MathBase;
31 typedef ContactModelAbstractTpl<Scalar> Base;
32 typedef ContactData2DTpl<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 2d contact model
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] nu Dimension of the control vector
47 * @param[in] gains Baumgarte stabilization gains
48 */
49 ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
50 const pinocchio::FrameIndex id, const Vector2s& xref,
51 const std::size_t nu,
52 const Vector2s& gains = Vector2s::Zero());
53
54 /**
55 * @brief Initialize the 2d contact model
56 *
57 * The default `nu` is obtained from `StateAbstractTpl::get_nv()`.
58 *
59 * @param[in] state State of the multibody system
60 * @param[in] id Reference frame id of the contact
61 * @param[in] xref Contact position used for the Baumgarte stabilization
62 * @param[in] gains Baumgarte stabilization gains
63 */
64 ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
65 const pinocchio::FrameIndex id, const Vector2s& xref,
66 const Vector2s& gains = Vector2s::Zero());
67 270 virtual ~ContactModel2DTpl() = default;
68
69 /**
70 * @brief Compute the 2d contact Jacobian and drift
71 *
72 * @param[in] data 2d contact data
73 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
74 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
75 */
76 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
77 const Eigen::Ref<const VectorXs>& x) override;
78
79 /**
80 * @brief Compute the derivatives of the 2d contact holonomic constraint
81 *
82 * @param[in] data 2d contact data
83 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
84 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
85 */
86 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x) override;
88
89 /**
90 * @brief Convert the force into a stack of spatial forces
91 *
92 * @param[in] data 2d contact data
93 * @param[in] force 2d force
94 */
95 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
96 const VectorXs& force) override;
97
98 /**
99 * @brief Create the 2d contact data
100 */
101 virtual std::shared_ptr<ContactDataAbstract> createData(
102 pinocchio::DataTpl<Scalar>* const data) override;
103
104 /**
105 * @brief Cast the contact-2d model to a different scalar type.
106 *
107 * It is useful for operations requiring different precision or scalar types.
108 *
109 * @tparam NewScalar The new scalar type to cast to.
110 * @return ContactModel2DTpl<NewScalar> A contact model with the
111 * new scalar type.
112 */
113 template <typename NewScalar>
114 ContactModel2DTpl<NewScalar> cast() const;
115
116 /**
117 * @brief Return the reference frame translation
118 */
119 const Vector2s& get_reference() const;
120
121 /**
122 * @brief Create the 2d contact data
123 */
124 const Vector2s& get_gains() const;
125
126 /**
127 * @brief Modify the reference frame translation
128 */
129 void set_reference(const Vector2s& reference);
130
131 /**
132 * @brief Print relevant information of the 2d contact model
133 *
134 * @param[out] os Output stream object
135 */
136 virtual void print(std::ostream& os) const override;
137
138 protected:
139 using Base::id_;
140 using Base::nc_;
141 using Base::nu_;
142 using Base::state_;
143
144 private:
145 Vector2s xref_; //!< Contact position used for the Baumgarte stabilization
146 Vector2s gains_; //!< Baumgarte stabilization gains
147 };
148
149 template <typename _Scalar>
150 struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
151 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
152
153 typedef _Scalar Scalar;
154 typedef MathBaseTpl<Scalar> MathBase;
155 typedef ContactDataAbstractTpl<Scalar> Base;
156 typedef typename MathBase::Matrix2s Matrix2s;
157 typedef typename MathBase::Matrix3s Matrix3s;
158 typedef typename MathBase::Matrix6xs Matrix6xs;
159 typedef typename MathBase::Vector3s Vector3s;
160
161 template <template <typename Scalar> class Model>
162 6907 ContactData2DTpl(Model<Scalar>* const model,
163 pinocchio::DataTpl<Scalar>* const data)
164 : Base(model, data),
165
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 fJf(6, model->get_state()->get_nv()),
166
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 v_partial_dq(6, model->get_state()->get_nv()),
167
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 a_partial_dq(6, model->get_state()->get_nv()),
168
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 a_partial_dv(6, model->get_state()->get_nv()),
169
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 a_partial_da(6, model->get_state()->get_nv()),
170
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 fXjdv_dq(6, model->get_state()->get_nv()),
171
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
 6907 fXjda_dq(6, model->get_state()->get_nv()),
172
10/20
✓ Branch 2 taken 6905 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6905 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 6905 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6905 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6905 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6905 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6905 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6905 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6905 times.
✗ Branch 31 not taken.
 13814 fXjda_dv(6, model->get_state()->get_nv()) {
173
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 frame = model->get_id();
174
3/6
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6905 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 6905 times.
✗ Branch 11 not taken.
 6907 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
175
2/4
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6905 times.
✗ Branch 5 not taken.
 6907 fXj = jMf.inverse().toActionMatrix();
176
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 fJf.setZero();
177
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 v_partial_dq.setZero();
178
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 a_partial_dq.setZero();
179
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 a_partial_dv.setZero();
180
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 a_partial_da.setZero();
181
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 fXjdv_dq.setZero();
182
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 fXjda_dq.setZero();
183
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 fXjda_dv.setZero();
184
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 vv.setZero();
185
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 vw.setZero();
186
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 vv_skew.setZero();
187
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 vw_skew.setZero();
188
1/2
✓ Branch 1 taken 6905 times.
✗ Branch 2 not taken.
 6907 oRf.setZero();
189 6907 }
190 13810 virtual ~ContactData2DTpl() = default;
191
192 using Base::a0;
193 using Base::da0_dx;
194 using Base::df_du;
195 using Base::df_dx;
196 using Base::f;
197 using Base::frame;
198 using Base::fXj;
199 using Base::Jc;
200 using Base::jMf;
201 using Base::pinocchio;
202
203 pinocchio::MotionTpl<Scalar> v;
204 pinocchio::MotionTpl<Scalar> a;
205 Matrix6xs fJf;
206 Matrix6xs v_partial_dq;
207 Matrix6xs a_partial_dq;
208 Matrix6xs a_partial_dv;
209 Matrix6xs a_partial_da;
210 Matrix6xs fXjdv_dq;
211 Matrix6xs fXjda_dq;
212 Matrix6xs fXjda_dv;
213 Vector3s vv;
214 Vector3s vw;
215 Matrix3s vv_skew;
216 Matrix3s vw_skew;
217 Matrix2s oRf;
218 };
219
220 } // namespace crocoddyl
221
222 /* --- Details -------------------------------------------------------------- */
223 /* --- Details -------------------------------------------------------------- */
224 /* --- Details -------------------------------------------------------------- */
225 #include "crocoddyl/multibody/contacts/contact-2d.hxx"
226
227 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel2DTpl)
228 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData2DTpl)
229
230 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
231