GCC Code Coverage Report

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