GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-2d.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 26 26 100.0%
Branches: 31 62 50.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
11
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14 #include <pinocchio/multibody/data.hpp>
15 #include <pinocchio/spatial/motion.hpp>
16
17 #include "crocoddyl/core/utils/exception.hpp"
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
28 typedef _Scalar Scalar;
29 typedef MathBaseTpl<Scalar> MathBase;
30 typedef ContactModelAbstractTpl<Scalar> Base;
31 typedef ContactData2DTpl<Scalar> Data;
32 typedef StateMultibodyTpl<Scalar> StateMultibody;
33 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
34 typedef typename MathBase::Vector2s Vector2s;
35 typedef typename MathBase::Vector3s Vector3s;
36 typedef typename MathBase::VectorXs VectorXs;
37 typedef typename MathBase::Matrix3s Matrix3s;
38
39 /**
40 * @brief Initialize the 2d contact model
41 *
42 * @param[in] state State of the multibody system
43 * @param[in] id Reference frame id of the contact
44 * @param[in] xref Contact position used for the Baumgarte stabilization
45 * @param[in] nu Dimension of the control vector
46 * @param[in] gains Baumgarte stabilization gains
47 */
48 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state,
49 const pinocchio::FrameIndex id, const Vector2s& xref,
50 const std::size_t nu,
51 const Vector2s& gains = Vector2s::Zero());
52
53 /**
54 * @brief Initialize the 2d contact model
55 *
56 * The default `nu` is obtained from `StateAbstractTpl::get_nv()`.
57 *
58 * @param[in] state State of the multibody system
59 * @param[in] id Reference frame id of the contact
60 * @param[in] xref Contact position used for the Baumgarte stabilization
61 * @param[in] gains Baumgarte stabilization gains
62 */
63 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state,
64 const pinocchio::FrameIndex id, const Vector2s& xref,
65 const Vector2s& gains = Vector2s::Zero());
66 virtual ~ContactModel2DTpl();
67
68 /**
69 * @brief Compute the 2d contact Jacobian and drift
70 *
71 * @param[in] data 2d contact data
72 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
73 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
74 */
75 virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x);
77
78 /**
79 * @brief Compute the derivatives of the 2d contact holonomic constraint
80 *
81 * @param[in] data 2d contact data
82 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
83 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
84 */
85 virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
86 const Eigen::Ref<const VectorXs>& x);
87
88 /**
89 * @brief Convert the force into a stack of spatial forces
90 *
91 * @param[in] data 2d contact data
92 * @param[in] force 2d force
93 */
94 virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data,
95 const VectorXs& force);
96
97 /**
98 * @brief Create the 2d contact data
99 */
100 virtual boost::shared_ptr<ContactDataAbstract> createData(
101 pinocchio::DataTpl<Scalar>* const data);
102
103 /**
104 * @brief Return the reference frame translation
105 */
106 const Vector2s& get_reference() const;
107
108 /**
109 * @brief Create the 2d contact data
110 */
111 const Vector2s& get_gains() const;
112
113 /**
114 * @brief Modify the reference frame translation
115 */
116 void set_reference(const Vector2s& reference);
117
118 /**
119 * @brief Print relevant information of the 2d contact model
120 *
121 * @param[out] os Output stream object
122 */
123 virtual void print(std::ostream& os) const;
124
125 protected:
126 using Base::id_;
127 using Base::nc_;
128 using Base::nu_;
129 using Base::state_;
130
131 private:
132 Vector2s xref_; //!< Contact position used for the Baumgarte stabilization
133 Vector2s gains_; //!< Baumgarte stabilization gains
134 };
135
136 template <typename _Scalar>
137 struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139
140 typedef _Scalar Scalar;
141 typedef MathBaseTpl<Scalar> MathBase;
142 typedef ContactDataAbstractTpl<Scalar> Base;
143 typedef typename MathBase::Matrix2s Matrix2s;
144 typedef typename MathBase::Matrix3s Matrix3s;
145 typedef typename MathBase::Matrix6xs Matrix6xs;
146 typedef typename MathBase::Vector3s Vector3s;
147
148 template <template <typename Scalar> class Model>
149 6912 ContactData2DTpl(Model<Scalar>* const model,
150 pinocchio::DataTpl<Scalar>* const data)
151 : Base(model, data),
152
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 fJf(6, model->get_state()->get_nv()),
153
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 v_partial_dq(6, model->get_state()->get_nv()),
154
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 a_partial_dq(6, model->get_state()->get_nv()),
155
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 a_partial_dv(6, model->get_state()->get_nv()),
156
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 a_partial_da(6, model->get_state()->get_nv()),
157
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 fXjdv_dq(6, model->get_state()->get_nv()),
158
1/2
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 fXjda_dq(6, model->get_state()->get_nv()),
159
8/16
✓ Branch 2 taken 6912 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6912 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6912 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6912 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6912 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6912 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 6912 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 6912 times.
✗ Branch 27 not taken.
 13824 fXjda_dv(6, model->get_state()->get_nv()) {
160 6912 frame = model->get_id();
161
1/2
✓ Branch 6 taken 6912 times.
✗ Branch 7 not taken.
 6912 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
162
2/4
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6912 times.
✗ Branch 5 not taken.
 6912 fXj = jMf.inverse().toActionMatrix();
163
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 fJf.setZero();
164
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 v_partial_dq.setZero();
165
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 a_partial_dq.setZero();
166
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 a_partial_dv.setZero();
167
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 a_partial_da.setZero();
168
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 fXjdv_dq.setZero();
169
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 fXjda_dq.setZero();
170
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 fXjda_dv.setZero();
171
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 vv.setZero();
172
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 vw.setZero();
173
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 vv_skew.setZero();
174
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 vw_skew.setZero();
175
1/2
✓ Branch 1 taken 6912 times.
✗ Branch 2 not taken.
 6912 oRf.setZero();
176 6912 }
177
178 using Base::a0;
179 using Base::da0_dx;
180 using Base::df_du;
181 using Base::df_dx;
182 using Base::f;
183 using Base::frame;
184 using Base::fXj;
185 using Base::Jc;
186 using Base::jMf;
187 using Base::pinocchio;
188
189 pinocchio::MotionTpl<Scalar> v;
190 pinocchio::MotionTpl<Scalar> a;
191 Matrix6xs fJf;
192 Matrix6xs v_partial_dq;
193 Matrix6xs a_partial_dq;
194 Matrix6xs a_partial_dv;
195 Matrix6xs a_partial_da;
196 Matrix6xs fXjdv_dq;
197 Matrix6xs fXjda_dq;
198 Matrix6xs fXjda_dv;
199 Vector3s vv;
200 Vector3s vw;
201 Matrix3s vv_skew;
202 Matrix3s vw_skew;
203 Matrix2s oRf;
204 };
205
206 } // namespace crocoddyl
207
208 /* --- Details -------------------------------------------------------------- */
209 /* --- Details -------------------------------------------------------------- */
210 /* --- Details -------------------------------------------------------------- */
211 #include "crocoddyl/multibody/contacts/contact-2d.hxx"
212
213 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
214