GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-info.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 241 363 66.4%
Branches: 219 876 25.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2023 INRIA CNRS
3 //
4
5 #ifndef __pinocchio_algorithm_contact_info_hpp__
6 #define __pinocchio_algorithm_contact_info_hpp__
7
8 #include <algorithm>
9
10 #include "pinocchio/multibody/model.hpp"
11 #include "pinocchio/algorithm/fwd.hpp"
12 #include "pinocchio/algorithm/constraints/fwd.hpp"
13 #include "pinocchio/algorithm/constraints/constraint-model-base.hpp"
14 #include "pinocchio/algorithm/constraints/constraint-data-base.hpp"
15
16 namespace pinocchio
17 {
18 ///  \brief Type of contact
19 enum ContactType
20 {
21 CONTACT_3D = 0, /// \brief Point contact model
22 CONTACT_6D, ///  \brief Frame contact model
23 CONTACT_UNDEFINED ///  \brief The default contact is undefined
24 };
25
26 template<ContactType contact_type>
27 struct contact_dim
28 {
29 enum
30 {
31 value = 0
32 };
33 };
34
35 template<>
36 struct contact_dim<CONTACT_3D>
37 {
38 enum
39 {
40 value = 3
41 };
42 };
43
44 template<>
45 struct contact_dim<CONTACT_6D>
46 {
47 enum
48 {
49 value = 6
50 };
51 };
52
53 template<typename _Scalar>
54 struct BaumgarteCorrectorParametersTpl;
55
56 template<typename _Scalar>
57 struct traits<BaumgarteCorrectorParametersTpl<_Scalar>>
58 {
59 typedef _Scalar Scalar;
60 };
61
62 template<typename _Scalar>
63 struct BaumgarteCorrectorParametersTpl : NumericalBase<BaumgarteCorrectorParametersTpl<_Scalar>>
64 {
65 typedef _Scalar Scalar;
66 typedef Eigen::Matrix<Scalar, -1, 1, Eigen::ColMajor, 6> Vector6Max;
67
68 69 BaumgarteCorrectorParametersTpl(int size = 6)
69 69 : Kp(size)
70 69 , Kd(size)
71 {
72 69 Kp.setZero();
73 69 Kd.setZero();
74 69 }
75
76 ✗ bool operator==(const BaumgarteCorrectorParametersTpl & other) const
77 {
78 ✗ return Kp == other.Kp && Kd == other.Kd;
79 }
80
81 ✗ bool operator!=(const BaumgarteCorrectorParametersTpl & other) const
82 {
83 ✗ return !(*this == other);
84 }
85
86 // parameters
87 /// \brief Proportional corrector value.
88 Vector6Max Kp;
89
90 /// \brief Damping corrector value.
91 Vector6Max Kd;
92
93 template<typename NewScalar>
94 ✗ BaumgarteCorrectorParametersTpl<NewScalar> cast() const
95 {
96 typedef BaumgarteCorrectorParametersTpl<NewScalar> ReturnType;
97 ✗ ReturnType res;
98 ✗ res.Kp = Kp.template cast<NewScalar>();
99 ✗ res.Kd = Kd.template cast<NewScalar>();
100 ✗ return res;
101 }
102 };
103
104 // TODO Remove when API is stabilized
105 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
106 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
107 template<typename NewScalar, typename Scalar, int Options>
108 struct CastType<NewScalar, RigidConstraintModelTpl<Scalar, Options>>
109 {
110 typedef RigidConstraintModelTpl<NewScalar, Options> type;
111 };
112
113 template<typename _Scalar, int _Options>
114 struct traits<RigidConstraintModelTpl<_Scalar, _Options>>
115 {
116 typedef _Scalar Scalar;
117 enum
118 {
119 Options = _Options
120 };
121 typedef RigidConstraintDataTpl<Scalar, Options> ConstraintData;
122 };
123
124 template<typename _Scalar, int _Options>
125 struct traits<RigidConstraintDataTpl<_Scalar, _Options>>
126 {
127 typedef _Scalar Scalar;
128 enum
129 {
130 Options = _Options
131 };
132 typedef RigidConstraintModelTpl<Scalar, Options> ConstraintModel;
133 };
134
135 ///
136 ///  \brief Contact model structure containg all the info describing the rigid contact model
137 ///
138 template<typename _Scalar, int _Options>
139 struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
140 RigidConstraintModelTpl : ConstraintModelBase<RigidConstraintModelTpl<_Scalar, _Options>>
141 {
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143
144 typedef _Scalar Scalar;
145 enum
146 {
147 Options = _Options
148 };
149 typedef ConstraintModelBase<RigidConstraintModelTpl<_Scalar, _Options>> Base;
150
151 template<typename NewScalar, int NewOptions>
152 friend struct RigidConstraintModelTpl;
153
154 using Base::base;
155 using Base::colwise_span_indexes;
156 using Base::colwise_sparsity;
157
158 typedef RigidConstraintModelTpl ContactModel;
159 typedef RigidConstraintDataTpl<Scalar, Options> ContactData;
160 typedef RigidConstraintDataTpl<Scalar, Options> ConstraintData;
161
162 typedef SE3Tpl<Scalar, Options> SE3;
163 typedef MotionTpl<Scalar, Options> Motion;
164 typedef ForceTpl<Scalar, Options> Force;
165 typedef BaumgarteCorrectorParametersTpl<Scalar> BaumgarteCorrectorParameters;
166 typedef typename Base::BooleanVector BooleanVector;
167 typedef typename Base::IndexVector IndexVector;
168 typedef Eigen::Matrix<Scalar, 3, 6, Options> Matrix36;
169
170 ///  \brief Type of the contact.
171 ContactType type;
172
173 /// \brief Index of the first joint in the model tree
174 JointIndex joint1_id;
175
176 /// \brief Index of the second joint in the model tree
177 JointIndex joint2_id;
178
179 /// \brief Relative placement with respect to the frame of joint1.
180 SE3 joint1_placement;
181
182 /// \brief Relative placement with respect to the frame of joint2.
183 SE3 joint2_placement;
184
185 /// \brief Reference frame where the constraint is expressed (LOCAL_WORLD_ALIGNED or LOCAL)
186 ReferenceFrame reference_frame;
187
188 /// \brief Desired contact placement
189 SE3 desired_contact_placement;
190
191 /// \brief Desired contact spatial velocity
192 Motion desired_contact_velocity;
193
194 /// \brief Desired contact spatial acceleration
195 Motion desired_contact_acceleration;
196
197 ///  \brief Corrector parameters
198 BaumgarteCorrectorParameters corrector;
199
200 /// \brief Colwise sparsity pattern associated with joint 1.
201 BooleanVector colwise_joint1_sparsity;
202
203 /// \brief Colwise sparsity pattern associated with joint 2.
204 BooleanVector colwise_joint2_sparsity;
205
206 /// \brief Jointwise span indexes associated with joint 1.
207 IndexVector joint1_span_indexes;
208
209 /// \brief Jointwise span indexes associated with joint 2.
210 IndexVector joint2_span_indexes;
211
212 IndexVector loop_span_indexes;
213
214 /// \brief Dimensions of the models
215 int nv;
216
217 ///  \brief Depth of the kinematic tree for joint1 and joint2
218 size_t depth_joint1, depth_joint2;
219
220 protected:
221 ///
222 ///  \brief Default constructor
223 ///
224 ✗ RigidConstraintModelTpl()
225 ✗ : nv(-1)
226 ✗ , depth_joint1(0)
227 ✗ , depth_joint2(0)
228 {
229 }
230
231 public:
232 ///
233 ///  \brief Contructor with from a given type, joint indexes and placements.
234 ///
235 /// \param[in] type Type of the contact.
236 /// \param[in] model Model associated to the constraint.
237 /// \param[in] joint1_id Index of the joint 1 in the model tree.
238 /// \param[in] joint2_id Index of the joint 2 in the model tree.
239 /// \param[in] joint1_placement Placement of the constraint w.r.t the frame of joint1.
240 /// \param[in] joint2_placement Placement of the constraint w.r.t the frame of joint2.
241 /// \param[in] reference_frame Reference frame in which the constraints quantities are
242 /// expressed.
243 ///
244 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
245 8 RigidConstraintModelTpl(
246 const ContactType type,
247 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
248 const JointIndex joint1_id,
249 const SE3 & joint1_placement,
250 const JointIndex joint2_id,
251 const SE3 & joint2_placement,
252 ✗ const ReferenceFrame & reference_frame = LOCAL)
253 : Base(model)
254 8 , type(type)
255 8 , joint1_id(joint1_id)
256 8 , joint2_id(joint2_id)
257
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , joint1_placement(joint1_placement)
258
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , joint2_placement(joint2_placement)
259 8 , reference_frame(reference_frame)
260
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_placement(SE3::Identity())
261
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_velocity(Motion::Zero())
262
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_acceleration(Motion::Zero())
263
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , corrector(size())
264
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , colwise_joint1_sparsity(model.nv)
265
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , colwise_joint2_sparsity(model.nv)
266
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , joint1_span_indexes((size_t)model.njoints)
267
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , joint2_span_indexes((size_t)model.njoints)
268
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 , loop_span_indexes((size_t)model.nv)
269 {
270
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 init(model);
271 8 }
272
273 ///
274 ///  \brief Contructor with from a given type, joint1_id and placement.
275 ///
276 /// \param[in] type Type of the contact.
277 /// \param[in] joint1_id Index of the joint 1 in the model tree.
278 /// \param[in] joint1_placement Placement of the constraint w.r.t the frame of joint1.
279 /// \param[in] reference_frame Reference frame in which the constraints quantities are
280 /// expressed.
281 ///
282 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
283 4 RigidConstraintModelTpl(
284 const ContactType type,
285 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
286 const JointIndex joint1_id,
287 const SE3 & joint1_placement,
288 ✗ const ReferenceFrame & reference_frame = LOCAL)
289 : Base(model)
290 4 , type(type)
291 4 , joint1_id(joint1_id)
292 4 , joint2_id(0)
293
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , joint1_placement(joint1_placement)
294
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , joint2_placement(SE3::Identity())
295 4 , reference_frame(reference_frame)
296
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , desired_contact_placement(SE3::Identity())
297
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , desired_contact_velocity(Motion::Zero())
298
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , desired_contact_acceleration(Motion::Zero())
299
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 , corrector(size())
300
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , colwise_joint1_sparsity(model.nv)
301
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 , colwise_joint2_sparsity(model.nv)
302
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 , joint1_span_indexes((size_t)model.njoints)
303
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 , joint2_span_indexes((size_t)model.njoints)
304
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 , loop_span_indexes((size_t)model.nv)
305 {
306
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 init(model);
307 4 }
308
309 ///
310 ///  \brief Contructor with from a given type and the joint ids.
311 ///
312 /// \param[in] type Type of the contact.
313 /// \param[in] joint1_id Index of the joint 1 in the model tree.
314 /// \param[in] joint2_id Index of the joint 2 in the model tree.
315 ///
316 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
317 8 RigidConstraintModelTpl(
318 const ContactType type,
319 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
320 const JointIndex joint1_id,
321 const JointIndex joint2_id,
322 const ReferenceFrame & reference_frame = LOCAL)
323 : Base(model)
324 8 , type(type)
325 8 , joint1_id(joint1_id)
326 8 , joint2_id(joint2_id)
327
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , joint1_placement(SE3::Identity())
328
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , joint2_placement(SE3::Identity())
329 8 , reference_frame(reference_frame)
330
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_placement(SE3::Identity())
331
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_velocity(Motion::Zero())
332
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , desired_contact_acceleration(Motion::Zero())
333
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , corrector(size())
334
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , colwise_joint1_sparsity(model.nv)
335
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 , colwise_joint2_sparsity(model.nv)
336
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , joint1_span_indexes((size_t)model.njoints)
337
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 , joint2_span_indexes((size_t)model.njoints)
338
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 , loop_span_indexes((size_t)model.nv)
339 {
340
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 init(model);
341 8 }
342
343 ///
344 ///  \brief Contructor with from a given type and .
345 ///
346 /// \param[in] type Type of the contact.
347 /// \param[in] joint1_id Index of the joint 1 in the model tree.
348 ///
349 /// \remarks The second joint id (joint2_id) is set to be 0 (corresponding to the index of the
350 /// universe).
351 ///
352 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
353 49 RigidConstraintModelTpl(
354 const ContactType type,
355 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
356 const JointIndex joint1_id,
357 ✗ const ReferenceFrame & reference_frame = LOCAL)
358 : Base(model)
359 49 , type(type)
360 49 , joint1_id(joint1_id)
361 49 , joint2_id(0) // set to be the Universe
362
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , joint1_placement(SE3::Identity())
363
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , joint2_placement(SE3::Identity())
364 49 , reference_frame(reference_frame)
365
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , desired_contact_placement(SE3::Identity())
366
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , desired_contact_velocity(Motion::Zero())
367
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , desired_contact_acceleration(Motion::Zero())
368
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 , corrector(size())
369
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , colwise_joint1_sparsity(model.nv)
370
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 , colwise_joint2_sparsity(model.nv)
371
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 , joint1_span_indexes((size_t)model.njoints)
372
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 , joint2_span_indexes((size_t)model.njoints)
373
1/2
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
 98 , loop_span_indexes((size_t)model.nv)
374 {
375
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 init(model);
376 49 }
377
378 ///
379 /// \brief Create data storage associated to the constraint
380 ///
381 ✗ ConstraintData createData() const
382 {
383 ✗ return ConstraintData(*this);
384 }
385
386 ///
387 ///  \brief Comparison operator
388 ///
389 /// \param[in] other Other RigidConstraintModelTpl to compare with.
390 ///
391 /// \returns true if the two *this is equal to other (type, joint1_id and placement attributs
392 /// must be the same).
393 ///
394 template<int OtherOptions>
395 ✗ bool operator==(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
396 {
397 ✗ return base() == other.base() && type == other.type && joint1_id == other.joint1_id
398 ✗ && joint2_id == other.joint2_id && joint1_placement == other.joint1_placement
399 ✗ && joint2_placement == other.joint2_placement
400 ✗ && reference_frame == other.reference_frame && corrector == other.corrector
401 ✗ && colwise_joint1_sparsity == other.colwise_joint1_sparsity
402 ✗ && colwise_joint2_sparsity == other.colwise_joint2_sparsity
403 ✗ && joint1_span_indexes == other.joint1_span_indexes
404 ✗ && joint2_span_indexes == other.joint2_span_indexes && nv == other.nv
405 ✗ && depth_joint1 == other.depth_joint1 && depth_joint2 == other.depth_joint2
406 ✗ && loop_span_indexes == other.loop_span_indexes;
407 }
408
409 ///
410 ///  \brief Oposite of the comparison operator.
411 ///
412 /// \param[in] other Other RigidConstraintModelTpl to compare with.
413 ///
414 /// \returns false if the two *this is not equal to other (at least type, joint1_id or placement
415 /// attributs is different).
416 ///
417 template<int OtherOptions>
418 ✗ bool operator!=(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
419 {
420 ✗ return !(*this == other);
421 }
422
423 /// \brief Evaluate the constraint values at the current state given by data and store the
424 /// results in cdata.
425 template<template<typename, int> class JointCollectionTpl>
426 3149 void calc(
427 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
428 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
429 RigidConstraintDataTpl<Scalar, Options> & cdata) const
430 {
431 PINOCCHIO_UNUSED_VARIABLE(model);
432
433
2/2
✓ Branch 0 taken 2940 times.
✓ Branch 1 taken 209 times.
 3149 if (joint1_id > 0)
434
1/2
✓ Branch 3 taken 2940 times.
✗ Branch 4 not taken.
 2940 cdata.oMc1 = data.oMi[joint1_id] * joint1_placement;
435 else
436 209 cdata.oMc1 = joint1_placement;
437
438
2/2
✓ Branch 0 taken 739 times.
✓ Branch 1 taken 2410 times.
 3149 if (joint2_id > 0)
439
1/2
✓ Branch 3 taken 739 times.
✗ Branch 4 not taken.
 739 cdata.oMc2 = data.oMi[joint2_id] * joint2_placement;
440 else
441 2410 cdata.oMc2 = joint2_placement;
442
443 // Compute relative placement
444
1/2
✓ Branch 2 taken 3149 times.
✗ Branch 3 not taken.
 3149 cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
445 3149 }
446
447 /// \brief Returns the constraint projector associated with joint 1.
448 /// This matrix transforms a spatial velocity expressed at the origin to the first component of
449 /// the constraint associated with joint 1.
450 ✗ Matrix36 getA1(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
451 {
452 ✗ Matrix36 res;
453 ✗ const SE3 & oMl = cdata.oMc1;
454 typedef typename SE3::Vector3 Vector3;
455
456 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, res) \
457 CartesianAxis<axis_id>::cross(v3_in, v_tmp); \
458 res.col(axis_id).noalias() = oMl.rotation().transpose() * v_tmp;
459
460 ✗ res.template leftCols<3>() = oMl.rotation().transpose();
461 ✗ Vector3 v_tmp;
462 ✗ PINOCCHIO_INTERNAL_COMPUTATION(0, oMl.translation(), res.template rightCols<3>());
463 ✗ PINOCCHIO_INTERNAL_COMPUTATION(1, oMl.translation(), res.template rightCols<3>());
464 ✗ PINOCCHIO_INTERNAL_COMPUTATION(2, oMl.translation(), res.template rightCols<3>());
465
466 #undef PINOCCHIO_INTERNAL_COMPUTATION
467
468 ✗ return res;
469 }
470
471 /// \brief Returns the constraint projector associated with joint 2.
472 /// This matrix transforms a spatial velocity expressed at the origin to the first component of
473 /// the constraint associated with joint 2.
474 ✗ Matrix36 getA2(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
475 {
476 ✗ Matrix36 res;
477 ✗ const SE3 & oM1 = cdata.oMc1;
478 ✗ const SE3 & oM2 = cdata.oMc2;
479 typedef typename SE3::Vector3 Vector3;
480
481 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, res) \
482 CartesianAxis<axis_id>::cross(v3_in, v_tmp); \
483 res.col(axis_id).noalias() = oM1.rotation().transpose() * v_tmp;
484
485 ✗ res.template leftCols<3>() = -oM1.rotation().transpose();
486 ✗ Vector3 v_tmp;
487 ✗ PINOCCHIO_INTERNAL_COMPUTATION(0, -oM2.translation(), res.template rightCols<3>());
488 ✗ PINOCCHIO_INTERNAL_COMPUTATION(1, -oM2.translation(), res.template rightCols<3>());
489 ✗ PINOCCHIO_INTERNAL_COMPUTATION(2, -oM2.translation(), res.template rightCols<3>());
490
491 #undef PINOCCHIO_INTERNAL_COMPUTATION
492
493 ✗ return res;
494 }
495
496 template<
497 typename InputMatrix,
498 typename OutputMatrix,
499 template<typename, int>
500 class JointCollectionTpl>
501 ✗ void jacobian_matrix_product(
502 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
503 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
504 RigidConstraintDataTpl<Scalar, Options> & cdata,
505 const Eigen::MatrixBase<InputMatrix> & mat,
506 const Eigen::MatrixBase<OutputMatrix> & _res) const
507 {
508 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
509 typedef typename Data::Vector3 Vector3;
510 ✗ OutputMatrix & res = _res.const_cast_derived();
511
512 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), model.nv);
513 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.cols(), res.cols());
514 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), size());
515 ✗ res.setZero();
516
517 // const Eigen::DenseIndex constraint_dim = size();
518 //
519 // const Eigen::DenseIndex
520 // complexity_strategy_1 = 6 * res.cols() * 36 + constraint_dim * 36 * res.cols(),
521 // complexity_strategy_2 = 36 * constraint_dim * 6 + constraint_dim * 36 * res.cols();
522
523 ✗ const Matrix36 A1 = getA1(cdata);
524 ✗ const Matrix36 A2 = getA2(cdata);
525 ✗ for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
526 {
527 ✗ if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
528 ✗ continue;
529 ✗ Matrix36 A;
530 ✗ Vector3 AxSi;
531
532 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
533 ✗ const ConstColXpr Jcol = data.J.col(jj);
534
535 ✗ if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
536 {
537 ✗ A = A1 + A2;
538 ✗ AxSi.noalias() = A * Jcol;
539 }
540 ✗ else if (colwise_joint1_sparsity[jj])
541 ✗ AxSi.noalias() = A1 * Jcol;
542 else
543 ✗ AxSi.noalias() = A2 * Jcol;
544
545 ✗ res.noalias() += AxSi * mat.row(jj);
546 }
547 }
548
549 ///  \brief Evaluate the Jacobian associated to the constraint at the given state stored in data
550 /// and cdata.  The results Jacobian is evaluated in the jacobian input/output matrix.
551 template<typename JacobianMatrix, template<typename, int> class JointCollectionTpl>
552 3149 void jacobian(
553 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
554 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
555 RigidConstraintDataTpl<Scalar, Options> & cdata,
556 const Eigen::MatrixBase<JacobianMatrix> & _jacobian_matrix) const
557 {
558 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
559 3149 JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
560
561 3149 const RigidConstraintModelTpl & cmodel = *this;
562
563 3149 const SE3 & oMc1 = cdata.oMc1;
564 3149 const SE3 & oMc2 = cdata.oMc2;
565 3149 const SE3 & c1Mc2 = cdata.c1Mc2;
566
567
2/2
✓ Branch 0 taken 74352 times.
✓ Branch 1 taken 3149 times.
 77501 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
568 {
569
570
6/6
✓ Branch 1 taken 46173 times.
✓ Branch 2 taken 28179 times.
✓ Branch 4 taken 5502 times.
✓ Branch 5 taken 40671 times.
✓ Branch 6 taken 33681 times.
✓ Branch 7 taken 40671 times.
 74352 if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
571 {
572
2/4
✓ Branch 1 taken 33681 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33681 times.
✗ Branch 5 not taken.
 33681 const int sign = colwise_joint1_sparsity[jj] != colwise_joint2_sparsity[jj]
573
5/6
✓ Branch 0 taken 31511 times.
✓ Branch 1 taken 2170 times.
✓ Branch 3 taken 31511 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 26009 times.
✓ Branch 6 taken 5502 times.
 33681 ? colwise_joint1_sparsity[jj] ? +1 : -1
574 : 0; // specific case for CONTACT_3D
575
576 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
577
1/2
✓ Branch 1 taken 33681 times.
✗ Branch 2 not taken.
 33681 const ConstColXpr Jcol = data.J.col(jj);
578
1/2
✓ Branch 1 taken 33681 times.
✗ Branch 2 not taken.
 33681 const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
579
580
2/3
✓ Branch 0 taken 20718 times.
✓ Branch 1 taken 12963 times.
✗ Branch 2 not taken.
 33681 switch (cmodel.type)
581 {
582 20718 case CONTACT_3D: {
583
2/4
✓ Branch 0 taken 15544 times.
✓ Branch 1 taken 5174 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 20718 switch (cmodel.reference_frame)
584 {
585 15544 case LOCAL: {
586
2/2
✓ Branch 0 taken 1378 times.
✓ Branch 1 taken 14166 times.
 15544 if (sign == 0)
587 {
588
1/2
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
 1378 const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations
589
1/2
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
 1378 const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
590
2/4
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1378 times.
✗ Branch 5 not taken.
 1378 const typename Motion::Vector3 Jdiff_linear =
591
4/8
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1378 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1378 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1378 times.
✗ Branch 11 not taken.
 1378 Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
592
2/4
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1378 times.
✗ Branch 5 not taken.
 1378 jacobian_matrix.col(jj) = Jdiff_linear;
593 1378 break;
594 }
595
2/2
✓ Branch 0 taken 11770 times.
✓ Branch 1 taken 2396 times.
 14166 else if (sign == 1)
596 {
597
1/2
✓ Branch 1 taken 11770 times.
✗ Branch 2 not taken.
 11770 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
598
3/6
✓ Branch 1 taken 11770 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11770 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11770 times.
✗ Branch 8 not taken.
 11770 jacobian_matrix.col(jj) = Jcol_local.linear();
599 11770 break;
600 }
601 else // sign == -1
602 {
603
1/2
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
 2396 Motion Jcol_local(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
604
2/4
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2396 times.
✗ Branch 5 not taken.
 2396 Jcol_local.linear() =
605
3/6
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2396 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2396 times.
✗ Branch 8 not taken.
 2396 c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
606
4/8
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2396 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2396 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2396 times.
✗ Branch 11 not taken.
 2396 jacobian_matrix.col(jj) = -Jcol_local.linear();
607 2396 break;
608 }
609 }
610 5174 case LOCAL_WORLD_ALIGNED: {
611
2/2
✓ Branch 0 taken 792 times.
✓ Branch 1 taken 4382 times.
 5174 if (sign == 0)
612 {
613
1/2
✓ Branch 1 taken 792 times.
✗ Branch 2 not taken.
 792 const typename Motion::Vector3 Jdiff_linear =
614
4/8
✓ Branch 1 taken 792 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 792 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 792 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 792 times.
✗ Branch 11 not taken.
 792 (oMc2.translation() - oMc1.translation()).cross(Jcol_motion.angular());
615
2/4
✓ Branch 1 taken 792 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 792 times.
✗ Branch 5 not taken.
 792 jacobian_matrix.col(jj) = Jdiff_linear;
616 792 break;
617 }
618 else
619 {
620
2/4
✓ Branch 1 taken 4382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4382 times.
✗ Branch 5 not taken.
 4382 typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear());
621
2/2
✓ Branch 0 taken 3860 times.
✓ Branch 1 taken 522 times.
 4382 if (sign == 1)
622
2/4
✓ Branch 1 taken 3860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3860 times.
✗ Branch 5 not taken.
 3860 Jcol_local_world_aligned_linear -=
623
2/4
✓ Branch 1 taken 3860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3860 times.
✗ Branch 5 not taken.
 7720 oMc1.translation().cross(Jcol_motion.angular());
624 else
625
2/4
✓ Branch 1 taken 522 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 522 times.
✗ Branch 5 not taken.
 522 Jcol_local_world_aligned_linear -=
626
2/4
✓ Branch 1 taken 522 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 522 times.
✗ Branch 5 not taken.
 1044 oMc2.translation().cross(Jcol_motion.angular());
627
3/6
✓ Branch 1 taken 4382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4382 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4382 times.
✗ Branch 8 not taken.
 4382 jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(sign);
628 4382 break;
629 }
630 }
631 ✗ case WORLD: {
632 ✗ PINOCCHIO_THROW_PRETTY(
633 std::invalid_argument, "Contact3D in world frame is not managed");
634 }
635 }
636 20718 break;
637 }
638
639 12963 case CONTACT_6D: {
640
2/4
✓ Branch 1 taken 12963 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12963 times.
✗ Branch 4 not taken.
 12963 assert(
641 check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
642
2/4
✓ Branch 0 taken 8741 times.
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 12963 switch (cmodel.reference_frame)
643 {
644 8741 case LOCAL: {
645
1/2
✓ Branch 1 taken 8741 times.
✗ Branch 2 not taken.
 8741 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
646
4/8
✓ Branch 1 taken 8741 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8741 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8741 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8741 times.
✗ Branch 11 not taken.
 8741 jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(sign);
647 8741 break;
648 }
649 4222 case LOCAL_WORLD_ALIGNED: {
650
1/2
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
 4222 Motion Jcol_local_world_aligned(Jcol_motion);
651
3/6
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4222 times.
✗ Branch 8 not taken.
 4222 Jcol_local_world_aligned.linear() -=
652
2/4
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4222 times.
✗ Branch 5 not taken.
 4222 oMc1.translation().cross(Jcol_local_world_aligned.angular());
653
4/8
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4222 times.
✗ Branch 11 not taken.
 4222 jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(sign);
654 4222 break;
655 }
656 ✗ case WORLD: {
657 ✗ PINOCCHIO_THROW_PRETTY(
658 std::invalid_argument, "Contact6D in world frame is not managed");
659 }
660 }
661 12963 break;
662 }
663
664 ✗ default:
665 ✗ assert(false && "must never happened");
666 break;
667 }
668 }
669 }
670 3149 }
671
672 14024 int size() const
673 {
674
2/3
✓ Branch 0 taken 8370 times.
✓ Branch 1 taken 5654 times.
✗ Branch 2 not taken.
 14024 switch (type)
675 {
676 8370 case CONTACT_3D:
677 8370 return contact_dim<CONTACT_3D>::value;
678 5654 case CONTACT_6D:
679 5654 return contact_dim<CONTACT_6D>::value;
680 ✗ default:
681 ✗ return contact_dim<CONTACT_UNDEFINED>::value;
682 }
683 return -1;
684 }
685
686 /// \returns An expression of *this with the Scalar type casted to NewScalar.
687 template<typename NewScalar>
688 ✗ RigidConstraintModelTpl<NewScalar, Options> cast() const
689 {
690 typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
691 ✗ ReturnType res;
692 ✗ res.base() = base();
693 ✗ res.type = type;
694 ✗ res.joint1_id = joint1_id;
695 ✗ res.joint2_id = joint2_id;
696 ✗ res.joint1_placement = joint1_placement.template cast<NewScalar>();
697 ✗ res.joint2_placement = joint2_placement.template cast<NewScalar>();
698 ✗ res.reference_frame = reference_frame;
699 ✗ res.desired_contact_placement = desired_contact_placement.template cast<NewScalar>();
700 ✗ res.desired_contact_velocity = desired_contact_velocity.template cast<NewScalar>();
701 ✗ res.desired_contact_acceleration = desired_contact_acceleration.template cast<NewScalar>();
702 ✗ res.corrector = corrector.template cast<NewScalar>();
703 ✗ res.colwise_joint1_sparsity = colwise_joint1_sparsity;
704 ✗ res.colwise_joint2_sparsity = colwise_joint2_sparsity;
705 ✗ res.nv = nv;
706 ✗ res.depth_joint1 = depth_joint1;
707 ✗ res.depth_joint2 = depth_joint2;
708 ✗ res.loop_span_indexes = loop_span_indexes;
709
710 ✗ return res;
711 }
712
713 protected:
714 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
715 69 void init(const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
716 {
717
3/6
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 69 PINOCCHIO_CHECK_INPUT_ARGUMENT(
718 reference_frame == LOCAL || reference_frame == LOCAL_WORLD_ALIGNED,
719 "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
720 69 nv = model.nv;
721 69 depth_joint1 = static_cast<size_t>(model.supports[joint1_id].size());
722 69 depth_joint2 = static_cast<size_t>(model.supports[joint2_id].size());
723
724 typedef ModelTpl<Scalar, OtherOptions, JointCollectionTpl> Model;
725 typedef typename Model::JointModel JointModel;
726 static const bool default_sparsity_value = false;
727 69 colwise_joint1_sparsity.fill(default_sparsity_value);
728 69 colwise_joint2_sparsity.fill(default_sparsity_value);
729 69 joint1_span_indexes.reserve((size_t)model.njoints);
730 69 joint2_span_indexes.reserve((size_t)model.njoints);
731
732 69 JointIndex current1_id = 0;
733
2/2
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 8 times.
 69 if (joint1_id > 0)
734 61 current1_id = joint1_id;
735
736 69 JointIndex current2_id = 0;
737
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 53 times.
 69 if (joint2_id > 0)
738 16 current2_id = joint2_id;
739
740
2/2
✓ Branch 0 taken 499 times.
✓ Branch 1 taken 69 times.
 568 while (current1_id != current2_id)
741 {
742
2/2
✓ Branch 0 taken 394 times.
✓ Branch 1 taken 105 times.
 499 if (current1_id > current2_id)
743 {
744 394 const JointModel & joint1 = model.joints[current1_id];
745
1/2
✓ Branch 1 taken 394 times.
✗ Branch 2 not taken.
 394 joint1_span_indexes.push_back((Eigen::DenseIndex)current1_id);
746 394 Eigen::DenseIndex current1_col_id = joint1.idx_v();
747
2/2
✓ Branch 1 taken 659 times.
✓ Branch 2 taken 394 times.
 1053 for (int k = 0; k < joint1.nv(); ++k, ++current1_col_id)
748 {
749 659 colwise_joint1_sparsity[current1_col_id] = true;
750 }
751 394 current1_id = model.parents[current1_id];
752 }
753 else
754 {
755 105 const JointModel & joint2 = model.joints[current2_id];
756
1/2
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
 105 joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
757 105 Eigen::DenseIndex current2_col_id = joint2.idx_v();
758
2/2
✓ Branch 1 taken 145 times.
✓ Branch 2 taken 105 times.
 250 for (int k = 0; k < joint2.nv(); ++k, ++current2_col_id)
759 {
760 145 colwise_joint2_sparsity[current2_col_id] = true;
761 }
762 105 current2_id = model.parents[current2_id];
763 }
764 }
765
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 69 times.
 69 assert(current1_id == current2_id && "current1_id should be equal to current2_id");
766
767
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 39 times.
 69 if (type == CONTACT_3D)
768 {
769 30 JointIndex current_id = current1_id;
770
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 30 times.
 37 while (current_id > 0)
771 {
772 7 const JointModel & joint = model.joints[current_id];
773
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
774
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
775 7 Eigen::DenseIndex current_row_id = joint.idx_v();
776
2/2
✓ Branch 1 taken 22 times.
✓ Branch 2 taken 7 times.
 29 for (int k = 0; k < joint.nv(); ++k, ++current_row_id)
777 {
778 22 colwise_joint1_sparsity[current_row_id] = true;
779 22 colwise_joint2_sparsity[current_row_id] = true;
780 }
781 7 current_id = model.parents[current_id];
782 }
783 }
784
785
2/4
✓ Branch 3 taken 69 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 69 std::rotate(
786 joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
787
2/4
✓ Branch 3 taken 69 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 69 std::rotate(
788 joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
789 69 Base::colwise_span_indexes.reserve((size_t)model.nv);
790 69 loop_span_indexes.reserve((size_t)model.nv);
791
2/2
✓ Branch 0 taken 2078 times.
✓ Branch 1 taken 69 times.
 2147 for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
792 {
793
8/10
✓ Branch 1 taken 2078 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1397 times.
✓ Branch 4 taken 681 times.
✓ Branch 6 taken 1397 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 145 times.
✓ Branch 9 taken 1252 times.
✓ Branch 10 taken 826 times.
✓ Branch 11 taken 1252 times.
 2078 if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
794 {
795
1/2
✓ Branch 1 taken 826 times.
✗ Branch 2 not taken.
 826 colwise_span_indexes.push_back(col_id);
796
1/2
✓ Branch 1 taken 826 times.
✗ Branch 2 not taken.
 826 colwise_sparsity[col_id] = true;
797 }
798
799
4/6
✓ Branch 1 taken 2078 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2078 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 804 times.
✓ Branch 7 taken 1274 times.
 2078 if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
800 {
801
1/2
✓ Branch 1 taken 804 times.
✗ Branch 2 not taken.
 804 loop_span_indexes.push_back(col_id);
802 }
803 }
804 69 }
805 };
806
807 template<typename Scalar, int Options, class Allocator>
808 2 size_t getTotalConstraintSize(
809 const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
810 {
811 typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
812 2 size_t total_size = 0;
813 2 for (typename VectorType::const_iterator it = contact_models.begin();
814
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 2 times.
 6 it != contact_models.end(); ++it)
815 4 total_size += it->size();
816
817 2 return total_size;
818 }
819
820 ///
821 ///  \brief Contact model structure containg all the info describing the rigid contact model
822 ///
823 template<typename _Scalar, int _Options>
824 struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
825 RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
826 {
827 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
828
829 typedef _Scalar Scalar;
830 enum
831 {
832 Options = _Options
833 };
834
835 typedef RigidConstraintModelTpl<Scalar, Options> ContactModel;
836 typedef RigidConstraintDataTpl ContactData;
837
838 typedef SE3Tpl<Scalar, Options> SE3;
839 typedef MotionTpl<Scalar, Options> Motion;
840 typedef ForceTpl<Scalar, Options> Force;
841 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
842 typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(Matrix6) VectorOfMatrix6;
843 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
844 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixX;
845
846 // data
847
848 /// \brief Resulting contact forces
849 Force contact_force;
850
851 /// \brief Placement of the constraint frame 1 with respect to the WORLD frame
852 SE3 oMc1;
853
854 /// \brief Placement of the constraint frame 2 with respect to the WORLD frame
855 SE3 oMc2;
856
857 /// \brief Relative displacement between the two frames
858 SE3 c1Mc2;
859
860 /// \brief Current contact placement error
861 Motion contact_placement_error;
862
863 /// \brief Current contact spatial velocity of the constraint 1
864 Motion contact1_velocity;
865
866 /// \brief Current contact spatial velocity of the constraint 2
867 Motion contact2_velocity;
868
869 /// \brief Current contact velocity error
870 Motion contact_velocity_error;
871
872 /// \brief Current contact spatial acceleration
873 Motion contact_acceleration;
874
875 /// \brief Contact spatial acceleration desired
876 Motion contact_acceleration_desired;
877
878 /// \brief Current contact spatial error (due to the integration step).
879 Motion contact_acceleration_error;
880
881 /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and
882 /// centrifugal effects) for the constraint frame 1.
883 Motion contact1_acceleration_drift;
884
885 /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and
886 /// centrifugal effects) for the constraint frame 2.
887 Motion contact2_acceleration_drift;
888
889 /// \brief Contact deviation from the reference acceleration (a.k.a the error)
890 Motion contact_acceleration_deviation;
891
892 VectorOfMatrix6 extended_motion_propagators_joint1;
893 VectorOfMatrix6 lambdas_joint1;
894 VectorOfMatrix6 extended_motion_propagators_joint2;
895
896 Matrix6x dv1_dq, da1_dq, da1_dv, da1_da;
897 Matrix6x dv2_dq, da2_dq, da2_dv, da2_da;
898 MatrixX dvc_dq, dac_dq, dac_dv, dac_da;
899
900 /// \brief Default constructor
901 RigidConstraintDataTpl()
902 {
903 }
904
905 81 explicit RigidConstraintDataTpl(const ContactModel & contact_model)
906 81 : contact_force(Force::Zero())
907 81 , contact_placement_error(Motion::Zero())
908 81 , contact1_velocity(Motion::Zero())
909 81 , contact2_velocity(Motion::Zero())
910 81 , contact_velocity_error(Motion::Zero())
911 81 , contact_acceleration(Motion::Zero())
912 81 , contact_acceleration_desired(Motion::Zero())
913 81 , contact_acceleration_error(Motion::Zero())
914 81 , contact1_acceleration_drift(Motion::Zero())
915 81 , contact2_acceleration_drift(Motion::Zero())
916 81 , contact_acceleration_deviation(Motion::Zero())
917
3/6
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
 81 , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
918
3/6
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
 81 , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
919
3/6
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
 81 , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
920
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , dv1_dq(6, contact_model.nv)
921
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da1_dq(6, contact_model.nv)
922
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da1_dv(6, contact_model.nv)
923
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da1_da(6, contact_model.nv)
924
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , dv2_dq(6, contact_model.nv)
925
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da2_dq(6, contact_model.nv)
926
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da2_dv(6, contact_model.nv)
927
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 , da2_da(6, contact_model.nv)
928
1/2
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
 81 , dvc_dq(contact_model.size(), contact_model.nv)
929
1/2
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
 81 , dac_dq(contact_model.size(), contact_model.nv)
930
1/2
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
 81 , dac_dv(contact_model.size(), contact_model.nv)
931
1/2
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 162 , dac_da(contact_model.size(), contact_model.nv)
932 {
933 81 }
934
935 ✗ bool operator==(const RigidConstraintDataTpl & other) const
936 {
937 ✗ return contact_force == other.contact_force && oMc1 == other.oMc1 && oMc2 == other.oMc2
938 ✗ && c1Mc2 == other.c1Mc2 && contact_placement_error == other.contact_placement_error
939 ✗ && contact1_velocity == other.contact1_velocity
940 ✗ && contact2_velocity == other.contact2_velocity
941 ✗ && contact_velocity_error == other.contact_velocity_error
942 ✗ && contact_acceleration == other.contact_acceleration
943 ✗ && contact_acceleration_desired == other.contact_acceleration_desired
944 ✗ && contact_acceleration_error == other.contact_acceleration_error
945 ✗ && contact1_acceleration_drift == other.contact1_acceleration_drift
946 ✗ && contact2_acceleration_drift == other.contact2_acceleration_drift
947 ✗ && contact_acceleration_deviation == other.contact_acceleration_deviation
948 ✗ && extended_motion_propagators_joint1 == other.extended_motion_propagators_joint1
949 ✗ && lambdas_joint1 == other.lambdas_joint1
950 ✗ && extended_motion_propagators_joint2 == other.extended_motion_propagators_joint2
951 //
952 ✗ && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
953 ✗ && da1_da == other.da1_da
954 //
955 ✗ && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
956 ✗ && da2_da == other.da2_da
957 //
958 ✗ && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
959 ✗ && dac_da == other.dac_da;
960 }
961
962 ✗ bool operator!=(const RigidConstraintDataTpl & other) const
963 {
964 ✗ return !(*this == other);
965 }
966 };
967 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
968
969 } // namespace pinocchio
970
971 #endif // ifndef __pinocchio_algorithm_contact_info_hpp__
972