GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-info.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 326 370 88.1%
Branches: 399 950 42.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 116 BaumgarteCorrectorParametersTpl(int size = 6)
69 116 : Kp(size)
70
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 116 , Kd(size)
71 {
72
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 116 Kp.setZero();
73
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 116 Kd.setZero();
74 116 }
75
76 1 bool operator==(const BaumgarteCorrectorParametersTpl & other) const
77 {
78
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 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 20 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 20 , type(type)
255 20 , joint1_id(joint1_id)
256 20 , joint2_id(joint2_id)
257
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , joint1_placement(joint1_placement)
258
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , joint2_placement(joint2_placement)
259 20 , reference_frame(reference_frame)
260
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_placement(SE3::Identity())
261
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_velocity(Motion::Zero())
262
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_acceleration(Motion::Zero())
263
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , corrector(size())
264
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , colwise_joint1_sparsity(model.nv)
265
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , colwise_joint2_sparsity(model.nv)
266
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , joint1_span_indexes((size_t)model.njoints)
267
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , joint2_span_indexes((size_t)model.njoints)
268
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 , loop_span_indexes((size_t)model.nv)
269 {
270
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 init(model);
271 20 }
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 17 RigidConstraintModelTpl(
284 const ContactType type,
285 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
286 const JointIndex joint1_id,
287 const SE3 & joint1_placement,
288
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 7 const ReferenceFrame & reference_frame = LOCAL)
289 : Base(model)
290 17 , type(type)
291 17 , joint1_id(joint1_id)
292 17 , joint2_id(0)
293
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , joint1_placement(joint1_placement)
294
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , joint2_placement(SE3::Identity())
295 17 , reference_frame(reference_frame)
296
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , desired_contact_placement(SE3::Identity())
297
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , desired_contact_velocity(Motion::Zero())
298
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , desired_contact_acceleration(Motion::Zero())
299
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 , corrector(size())
300
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , colwise_joint1_sparsity(model.nv)
301
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 , colwise_joint2_sparsity(model.nv)
302
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 , joint1_span_indexes((size_t)model.njoints)
303
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 , joint2_span_indexes((size_t)model.njoints)
304
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 34 , loop_span_indexes((size_t)model.nv)
305 {
306
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 init(model);
307 17 }
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 20 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 20 , type(type)
325 20 , joint1_id(joint1_id)
326 20 , joint2_id(joint2_id)
327
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , joint1_placement(SE3::Identity())
328
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , joint2_placement(SE3::Identity())
329 20 , reference_frame(reference_frame)
330
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_placement(SE3::Identity())
331
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_velocity(Motion::Zero())
332
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , desired_contact_acceleration(Motion::Zero())
333
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , corrector(size())
334
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , colwise_joint1_sparsity(model.nv)
335
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 , colwise_joint2_sparsity(model.nv)
336
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , joint1_span_indexes((size_t)model.njoints)
337
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 , joint2_span_indexes((size_t)model.njoints)
338
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 , loop_span_indexes((size_t)model.nv)
339 {
340
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 init(model);
341 20 }
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 59 RigidConstraintModelTpl(
354 const ContactType type,
355 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
356 const JointIndex joint1_id,
357
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const ReferenceFrame & reference_frame = LOCAL)
358 : Base(model)
359 59 , type(type)
360 59 , joint1_id(joint1_id)
361 59 , joint2_id(0) // set to be the Universe
362
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , joint1_placement(SE3::Identity())
363
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , joint2_placement(SE3::Identity())
364 59 , reference_frame(reference_frame)
365
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , desired_contact_placement(SE3::Identity())
366
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , desired_contact_velocity(Motion::Zero())
367
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , desired_contact_acceleration(Motion::Zero())
368
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 , corrector(size())
369
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , colwise_joint1_sparsity(model.nv)
370
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 , colwise_joint2_sparsity(model.nv)
371
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 , joint1_span_indexes((size_t)model.njoints)
372
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 , joint2_span_indexes((size_t)model.njoints)
373
1/2
✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
 118 , loop_span_indexes((size_t)model.nv)
374 {
375
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 init(model);
376 59 }
377
378 ///
379 /// \brief Create data storage associated to the constraint
380 ///
381 2 ConstraintData createData() const
382 {
383 2 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 1 bool operator==(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
396 {
397
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 return base() == other.base() && type == other.type && joint1_id == other.joint1_id
398
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 && joint2_id == other.joint2_id && joint1_placement == other.joint1_placement
399
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 && joint2_placement == other.joint2_placement
400
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 && reference_frame == other.reference_frame && corrector == other.corrector
401
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 && colwise_joint1_sparsity == other.colwise_joint1_sparsity
402
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 && colwise_joint2_sparsity == other.colwise_joint2_sparsity
403
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 && joint1_span_indexes == other.joint1_span_indexes
404
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 && joint2_span_indexes == other.joint2_span_indexes && nv == other.nv
405
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 && depth_joint1 == other.depth_joint1 && depth_joint2 == other.depth_joint2
406
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 && 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 3151 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 211 times.
 3151 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 211 cdata.oMc1 = joint1_placement;
437
438
2/2
✓ Branch 0 taken 739 times.
✓ Branch 1 taken 2412 times.
 3151 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 2412 cdata.oMc2 = joint2_placement;
442
443 // Compute relative placement
444
1/2
✓ Branch 2 taken 3151 times.
✗ Branch 3 not taken.
 3151 cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
445 3151 }
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 6 Matrix36 getA1(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
451 {
452
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Matrix36 res;
453 6 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
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 res.template leftCols<3>() = oMl.rotation().transpose();
461
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Vector3 v_tmp;
462
9/18
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(0, oMl.translation(), res.template rightCols<3>());
463
9/18
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(1, oMl.translation(), res.template rightCols<3>());
464
9/18
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(2, oMl.translation(), res.template rightCols<3>());
465
466 #undef PINOCCHIO_INTERNAL_COMPUTATION
467
468 12 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 6 Matrix36 getA2(const RigidConstraintDataTpl<Scalar, Options> & cdata) const
475 {
476
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Matrix36 res;
477 6 const SE3 & oM1 = cdata.oMc1;
478 6 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
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
 6 res.template leftCols<3>() = -oM1.rotation().transpose();
486
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Vector3 v_tmp;
487
10/20
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6 times.
✗ Branch 29 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(0, -oM2.translation(), res.template rightCols<3>());
488
10/20
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6 times.
✗ Branch 29 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(1, -oM2.translation(), res.template rightCols<3>());
489
10/20
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 6 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 6 times.
✗ Branch 29 not taken.
 6 PINOCCHIO_INTERNAL_COMPUTATION(2, -oM2.translation(), res.template rightCols<3>());
490
491 #undef PINOCCHIO_INTERNAL_COMPUTATION
492
493 12 return res;
494 }
495
496 template<
497 typename InputMatrix,
498 typename OutputMatrix,
499 template<typename, int> class JointCollectionTpl>
500 3 void jacobian_matrix_product(
501 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
502 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
503 RigidConstraintDataTpl<Scalar, Options> & cdata,
504 const Eigen::MatrixBase<InputMatrix> & mat,
505 const Eigen::MatrixBase<OutputMatrix> & _res) const
506 {
507 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
508 typedef typename Data::Vector3 Vector3;
509 3 OutputMatrix & res = _res.const_cast_derived();
510
511
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), model.nv);
512
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.cols(), res.cols());
513
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), size());
514
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 res.setZero();
515
516 // const Eigen::DenseIndex constraint_dim = size();
517 //
518 // const Eigen::DenseIndex
519 // complexity_strategy_1 = 6 * res.cols() * 36 + constraint_dim * 36 * res.cols(),
520 // complexity_strategy_2 = 36 * constraint_dim * 6 + constraint_dim * 36 * res.cols();
521
522
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const Matrix36 A1 = getA1(cdata);
523
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const Matrix36 A2 = getA2(cdata);
524
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
 99 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
525 {
526
8/10
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✓ Branch 4 taken 36 times.
✓ Branch 6 taken 60 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✓ Branch 9 taken 6 times.
✓ Branch 10 taken 54 times.
✓ Branch 11 taken 42 times.
 96 if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
527 54 continue;
528
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 Matrix36 A;
529
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 Vector3 AxSi;
530
531 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
532
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 const ConstColXpr Jcol = data.J.col(jj);
533
534
8/10
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 36 times.
✓ Branch 4 taken 6 times.
✓ Branch 6 taken 36 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✓ Branch 9 taken 30 times.
✓ Branch 10 taken 6 times.
✓ Branch 11 taken 36 times.
 42 if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
535 {
536
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 A = A1 + A2;
537
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 AxSi.noalias() = A * Jcol;
538 }
539
3/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 30 times.
✓ Branch 4 taken 6 times.
 36 else if (colwise_joint1_sparsity[jj])
540
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 30 AxSi.noalias() = A1 * Jcol;
541 else
542
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 AxSi.noalias() = A2 * Jcol;
543
544
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
 42 res.noalias() += AxSi * mat.row(jj);
545 }
546 3 }
547
548 ///  \brief Evaluate the Jacobian associated to the constraint at the given state stored in data
549 /// and cdata.  The results Jacobian is evaluated in the jacobian input/output matrix.
550 template<typename JacobianMatrix, template<typename, int> class JointCollectionTpl>
551 3151 void jacobian(
552 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
554 RigidConstraintDataTpl<Scalar, Options> & cdata,
555 const Eigen::MatrixBase<JacobianMatrix> & _jacobian_matrix) const
556 {
557 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
558 3151 JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
559
560 3151 const RigidConstraintModelTpl & cmodel = *this;
561
562 3151 const SE3 & oMc1 = cdata.oMc1;
563 3151 const SE3 & oMc2 = cdata.oMc2;
564 3151 const SE3 & c1Mc2 = cdata.c1Mc2;
565
566
2/2
✓ Branch 0 taken 74416 times.
✓ Branch 1 taken 3151 times.
 77567 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
567 {
568
569
6/6
✓ Branch 1 taken 46237 times.
✓ Branch 2 taken 28179 times.
✓ Branch 4 taken 5502 times.
✓ Branch 5 taken 40735 times.
✓ Branch 6 taken 33681 times.
✓ Branch 7 taken 40735 times.
 74416 if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
570 {
571
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]
572
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
573 : 0; // specific case for CONTACT_3D
574
575 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
576
1/2
✓ Branch 1 taken 33681 times.
✗ Branch 2 not taken.
 33681 const ConstColXpr Jcol = data.J.col(jj);
577
1/2
✓ Branch 1 taken 33681 times.
✗ Branch 2 not taken.
 33681 const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
578
579
2/3
✓ Branch 0 taken 20718 times.
✓ Branch 1 taken 12963 times.
✗ Branch 2 not taken.
 33681 switch (cmodel.type)
580 {
581 20718 case CONTACT_3D: {
582
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)
583 {
584 15544 case LOCAL: {
585
2/2
✓ Branch 0 taken 1378 times.
✓ Branch 1 taken 14166 times.
 15544 if (sign == 0)
586 {
587
1/2
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
 1378 const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations
588
1/2
✓ Branch 1 taken 1378 times.
✗ Branch 2 not taken.
 1378 const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
589
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 =
590
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();
591
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;
592 1378 break;
593 }
594
2/2
✓ Branch 0 taken 11770 times.
✓ Branch 1 taken 2396 times.
 14166 else if (sign == 1)
595 {
596
1/2
✓ Branch 1 taken 11770 times.
✗ Branch 2 not taken.
 11770 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
597
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();
598 11770 break;
599 }
600 else // sign == -1
601 {
602
1/2
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
 2396 Motion Jcol_local(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
603
2/4
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2396 times.
✗ Branch 5 not taken.
 2396 Jcol_local.linear() =
604
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
605
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();
606 2396 break;
607 }
608 }
609 5174 case LOCAL_WORLD_ALIGNED: {
610
2/2
✓ Branch 0 taken 792 times.
✓ Branch 1 taken 4382 times.
 5174 if (sign == 0)
611 {
612
1/2
✓ Branch 1 taken 792 times.
✗ Branch 2 not taken.
 792 const typename Motion::Vector3 Jdiff_linear =
613
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());
614
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;
615 792 break;
616 }
617 else
618 {
619
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());
620
2/2
✓ Branch 0 taken 3860 times.
✓ Branch 1 taken 522 times.
 4382 if (sign == 1)
621
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 -=
622
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());
623 else
624
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 -=
625
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());
626
3/8
✓ 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.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 4382 jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(sign);
627 4382 break;
628 }
629 }
630 ✗ case WORLD: {
631 ✗ PINOCCHIO_THROW_PRETTY(
632 std::invalid_argument, "Contact3D in world frame is not managed");
633 }
634 }
635 20718 break;
636 }
637
638 12963 case CONTACT_6D: {
639
2/4
✓ Branch 1 taken 12963 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12963 times.
✗ Branch 4 not taken.
 12963 assert(
640 check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
641
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)
642 {
643 8741 case LOCAL: {
644
1/2
✓ Branch 1 taken 8741 times.
✗ Branch 2 not taken.
 8741 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
645
4/10
✓ 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.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 8741 jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(sign);
646 8741 break;
647 }
648 4222 case LOCAL_WORLD_ALIGNED: {
649
1/2
✓ Branch 1 taken 4222 times.
✗ Branch 2 not taken.
 4222 Motion Jcol_local_world_aligned(Jcol_motion);
650
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() -=
651
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());
652
4/10
✓ 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.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 4222 jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(sign);
653 4222 break;
654 }
655 ✗ case WORLD: {
656 ✗ PINOCCHIO_THROW_PRETTY(
657 std::invalid_argument, "Contact6D in world frame is not managed");
658 }
659 }
660 12963 break;
661 }
662
663 ✗ default:
664 ✗ assert(false && "must never happened");
665 break;
666 }
667 }
668 }
669 3151 }
670
671 14599 int size() const
672 {
673
2/3
✓ Branch 0 taken 8591 times.
✓ Branch 1 taken 6008 times.
✗ Branch 2 not taken.
 14599 switch (type)
674 {
675 8591 case CONTACT_3D:
676 8591 return contact_dim<CONTACT_3D>::value;
677 6008 case CONTACT_6D:
678 6008 return contact_dim<CONTACT_6D>::value;
679 ✗ default:
680 ✗ return contact_dim<CONTACT_UNDEFINED>::value;
681 }
682 return -1;
683 }
684
685 /// \returns An expression of *this with the Scalar type casted to NewScalar.
686 template<typename NewScalar>
687 ✗ RigidConstraintModelTpl<NewScalar, Options> cast() const
688 {
689 typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
690 ✗ ReturnType res;
691 ✗ res.base() = base();
692 ✗ res.type = type;
693 ✗ res.joint1_id = joint1_id;
694 ✗ res.joint2_id = joint2_id;
695 ✗ res.joint1_placement = joint1_placement.template cast<NewScalar>();
696 ✗ res.joint2_placement = joint2_placement.template cast<NewScalar>();
697 ✗ res.reference_frame = reference_frame;
698 ✗ res.desired_contact_placement = desired_contact_placement.template cast<NewScalar>();
699 ✗ res.desired_contact_velocity = desired_contact_velocity.template cast<NewScalar>();
700 ✗ res.desired_contact_acceleration = desired_contact_acceleration.template cast<NewScalar>();
701 ✗ res.corrector = corrector.template cast<NewScalar>();
702 ✗ res.colwise_joint1_sparsity = colwise_joint1_sparsity;
703 ✗ res.colwise_joint2_sparsity = colwise_joint2_sparsity;
704 ✗ res.nv = nv;
705 ✗ res.depth_joint1 = depth_joint1;
706 ✗ res.depth_joint2 = depth_joint2;
707 ✗ res.loop_span_indexes = loop_span_indexes;
708
709 ✗ return res;
710 }
711
712 protected:
713 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
714 116 void init(const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
715 {
716
3/6
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 116 PINOCCHIO_CHECK_INPUT_ARGUMENT(
717 reference_frame == LOCAL || reference_frame == LOCAL_WORLD_ALIGNED,
718 "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
719 116 nv = model.nv;
720 116 depth_joint1 = static_cast<size_t>(model.supports[joint1_id].size());
721 116 depth_joint2 = static_cast<size_t>(model.supports[joint2_id].size());
722
723 typedef ModelTpl<Scalar, OtherOptions, JointCollectionTpl> Model;
724 typedef typename Model::JointModel JointModel;
725 static const bool default_sparsity_value = false;
726 116 colwise_joint1_sparsity.fill(default_sparsity_value);
727 116 colwise_joint2_sparsity.fill(default_sparsity_value);
728 116 joint1_span_indexes.reserve((size_t)model.njoints);
729 116 joint2_span_indexes.reserve((size_t)model.njoints);
730
731 116 JointIndex current1_id = 0;
732
2/2
✓ Branch 0 taken 95 times.
✓ Branch 1 taken 21 times.
 116 if (joint1_id > 0)
733 95 current1_id = joint1_id;
734
735 116 JointIndex current2_id = 0;
736
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 76 times.
 116 if (joint2_id > 0)
737 40 current2_id = joint2_id;
738
739
2/2
✓ Branch 0 taken 805 times.
✓ Branch 1 taken 116 times.
 921 while (current1_id != current2_id)
740 {
741
2/2
✓ Branch 0 taken 574 times.
✓ Branch 1 taken 231 times.
 805 if (current1_id > current2_id)
742 {
743 574 const JointModel & joint1 = model.joints[current1_id];
744 574 const int j1nv = joint1.nv();
745
1/2
✓ Branch 1 taken 574 times.
✗ Branch 2 not taken.
 574 joint1_span_indexes.push_back((Eigen::DenseIndex)current1_id);
746 574 Eigen::DenseIndex current1_col_id = joint1.idx_v();
747
2/2
✓ Branch 0 taken 933 times.
✓ Branch 1 taken 574 times.
 1507 for (int k = 0; k < j1nv; ++k, ++current1_col_id)
748 {
749 933 colwise_joint1_sparsity[current1_col_id] = true;
750 }
751 574 current1_id = model.parents[current1_id];
752 }
753 else
754 {
755 231 const JointModel & joint2 = model.joints[current2_id];
756 231 const int j2nv = joint2.nv();
757
1/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
758 231 Eigen::DenseIndex current2_col_id = joint2.idx_v();
759
2/2
✓ Branch 0 taken 311 times.
✓ Branch 1 taken 231 times.
 542 for (int k = 0; k < j2nv; ++k, ++current2_col_id)
760 {
761 311 colwise_joint2_sparsity[current2_col_id] = true;
762 }
763 231 current2_id = model.parents[current2_id];
764 }
765 }
766
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 116 times.
 116 assert(current1_id == current2_id && "current1_id should be equal to current2_id");
767
768
2/2
✓ Branch 0 taken 56 times.
✓ Branch 1 taken 60 times.
 116 if (type == CONTACT_3D)
769 {
770 56 JointIndex current_id = current1_id;
771
2/2
✓ Branch 0 taken 41 times.
✓ Branch 1 taken 56 times.
 97 while (current_id > 0)
772 {
773 41 const JointModel & joint = model.joints[current_id];
774 41 const int jnv = joint.nv();
775
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
776
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
777 41 Eigen::DenseIndex current_row_id = joint.idx_v();
778
2/2
✓ Branch 0 taken 106 times.
✓ Branch 1 taken 41 times.
 147 for (int k = 0; k < jnv; ++k, ++current_row_id)
779 {
780 106 colwise_joint1_sparsity[current_row_id] = true;
781 106 colwise_joint2_sparsity[current_row_id] = true;
782 }
783 41 current_id = model.parents[current_id];
784 }
785 }
786
787
2/4
✓ Branch 3 taken 116 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
 116 std::rotate(
788 joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
789
2/4
✓ Branch 3 taken 116 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
 116 std::rotate(
790 joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
791 116 Base::colwise_span_indexes.reserve((size_t)model.nv);
792 116 loop_span_indexes.reserve((size_t)model.nv);
793
2/2
✓ Branch 0 taken 3498 times.
✓ Branch 1 taken 116 times.
 3614 for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
794 {
795
8/10
✓ Branch 1 taken 3498 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2459 times.
✓ Branch 4 taken 1039 times.
✓ Branch 6 taken 2459 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 311 times.
✓ Branch 9 taken 2148 times.
✓ Branch 10 taken 1350 times.
✓ Branch 11 taken 2148 times.
 3498 if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
796 {
797
1/2
✓ Branch 1 taken 1350 times.
✗ Branch 2 not taken.
 1350 colwise_span_indexes.push_back(col_id);
798
1/2
✓ Branch 1 taken 1350 times.
✗ Branch 2 not taken.
 1350 colwise_sparsity[col_id] = true;
799 }
800
801
4/6
✓ Branch 1 taken 3498 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3498 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1244 times.
✓ Branch 7 taken 2254 times.
 3498 if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
802 {
803
1/2
✓ Branch 1 taken 1244 times.
✗ Branch 2 not taken.
 1244 loop_span_indexes.push_back(col_id);
804 }
805 }
806 116 }
807 };
808
809 template<typename Scalar, int Options, class Allocator>
810 8 size_t getTotalConstraintSize(
811 const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
812 {
813 typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
814 8 size_t total_size = 0;
815 8 for (typename VectorType::const_iterator it = contact_models.begin();
816
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 8 times.
 24 it != contact_models.end(); ++it)
817 16 total_size += it->size();
818
819 8 return total_size;
820 }
821
822 ///
823 ///  \brief Contact model structure containg all the info describing the rigid contact model
824 ///
825 template<typename _Scalar, int _Options>
826 struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
827 RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
828 {
829 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
830
831 typedef _Scalar Scalar;
832 enum
833 {
834 Options = _Options
835 };
836
837 typedef RigidConstraintModelTpl<Scalar, Options> ContactModel;
838 typedef RigidConstraintDataTpl ContactData;
839
840 typedef SE3Tpl<Scalar, Options> SE3;
841 typedef MotionTpl<Scalar, Options> Motion;
842 typedef ForceTpl<Scalar, Options> Force;
843 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
844 typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(Matrix6) VectorOfMatrix6;
845 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
846 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixX;
847
848 // data
849
850 /// \brief Resulting contact forces
851 Force contact_force;
852
853 /// \brief Placement of the constraint frame 1 with respect to the WORLD frame
854 SE3 oMc1;
855
856 /// \brief Placement of the constraint frame 2 with respect to the WORLD frame
857 SE3 oMc2;
858
859 /// \brief Relative displacement between the two frames
860 SE3 c1Mc2;
861
862 /// \brief Current contact placement error
863 Motion contact_placement_error;
864
865 /// \brief Current contact spatial velocity of the constraint 1
866 Motion contact1_velocity;
867
868 /// \brief Current contact spatial velocity of the constraint 2
869 Motion contact2_velocity;
870
871 /// \brief Current contact velocity error
872 Motion contact_velocity_error;
873
874 /// \brief Current contact spatial acceleration
875 Motion contact_acceleration;
876
877 /// \brief Contact spatial acceleration desired
878 Motion contact_acceleration_desired;
879
880 /// \brief Current contact spatial error (due to the integration step).
881 Motion contact_acceleration_error;
882
883 /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and
884 /// centrifugal effects) for the constraint frame 1.
885 Motion contact1_acceleration_drift;
886
887 /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and
888 /// centrifugal effects) for the constraint frame 2.
889 Motion contact2_acceleration_drift;
890
891 /// \brief Contact deviation from the reference acceleration (a.k.a the error)
892 Motion contact_acceleration_deviation;
893
894 VectorOfMatrix6 extended_motion_propagators_joint1;
895 VectorOfMatrix6 lambdas_joint1;
896 VectorOfMatrix6 extended_motion_propagators_joint2;
897
898 Matrix6x dv1_dq, da1_dq, da1_dv, da1_da;
899 Matrix6x dv2_dq, da2_dq, da2_dv, da2_da;
900 MatrixX dvc_dq, dac_dq, dac_dv, dac_da;
901
902 /// \brief Default constructor
903 RigidConstraintDataTpl()
904 : contact_force(Force::Zero())
905 , oMc1(SE3::Identity())
906 , oMc2(SE3::Identity())
907 , c1Mc2(SE3::Identity())
908 , contact_placement_error(Motion::Zero())
909 , contact1_velocity(Motion::Zero())
910 , contact2_velocity(Motion::Zero())
911 , contact_velocity_error(Motion::Zero())
912 , contact_acceleration(Motion::Zero())
913 , contact_acceleration_desired(Motion::Zero())
914 , contact_acceleration_error(Motion::Zero())
915 , contact1_acceleration_drift(Motion::Zero())
916 , contact2_acceleration_drift(Motion::Zero())
917 , contact_acceleration_deviation(Motion::Zero())
918 , extended_motion_propagators_joint1()
919 , lambdas_joint1()
920 , extended_motion_propagators_joint2()
921 , dv1_dq(6, 0)
922 , da1_dq(6, 0)
923 , da1_dv(6, 0)
924 , da1_da(6, 0)
925 , dv2_dq(6, 0)
926 , da2_dq(6, 0)
927 , da2_dv(6, 0)
928 , da2_da(6, 0)
929 , dvc_dq()
930 , dac_dq()
931 , dac_dv()
932 , dac_da()
933 {
934 }
935
936 121 explicit RigidConstraintDataTpl(const ContactModel & contact_model)
937 121 : contact_force(Force::Zero())
938
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , oMc1(SE3::Identity())
939
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , oMc2(SE3::Identity())
940
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , c1Mc2(SE3::Identity())
941
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_placement_error(Motion::Zero())
942
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact1_velocity(Motion::Zero())
943
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact2_velocity(Motion::Zero())
944
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_velocity_error(Motion::Zero())
945
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_acceleration(Motion::Zero())
946
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_acceleration_desired(Motion::Zero())
947
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_acceleration_error(Motion::Zero())
948
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact1_acceleration_drift(Motion::Zero())
949
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact2_acceleration_drift(Motion::Zero())
950
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 121 , contact_acceleration_deviation(Motion::Zero())
951
3/6
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 121 times.
✗ Branch 9 not taken.
 121 , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
952
3/6
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 121 times.
✗ Branch 9 not taken.
 121 , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
953
3/6
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 121 times.
✗ Branch 9 not taken.
 121 , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
954
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , dv1_dq(Matrix6x::Zero(6, contact_model.nv))
955
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da1_dq(Matrix6x::Zero(6, contact_model.nv))
956
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da1_dv(Matrix6x::Zero(6, contact_model.nv))
957
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da1_da(Matrix6x::Zero(6, contact_model.nv))
958
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , dv2_dq(Matrix6x::Zero(6, contact_model.nv))
959
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da2_dq(Matrix6x::Zero(6, contact_model.nv))
960
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da2_dv(Matrix6x::Zero(6, contact_model.nv))
961
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 121 , da2_da(Matrix6x::Zero(6, contact_model.nv))
962
2/4
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
 121 , dvc_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
963
2/4
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
 121 , dac_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
964
2/4
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
 121 , dac_dv(MatrixX::Zero(contact_model.size(), contact_model.nv))
965
2/4
✓ Branch 2 taken 121 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 121 times.
✗ Branch 6 not taken.
 121 , dac_da(MatrixX::Zero(contact_model.size(), contact_model.nv))
966 {
967 121 }
968
969 3 bool operator==(const RigidConstraintDataTpl & other) const
970 {
971
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 return contact_force == other.contact_force && oMc1 == other.oMc1 && oMc2 == other.oMc2
972
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 && c1Mc2 == other.c1Mc2 && contact_placement_error == other.contact_placement_error
973
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact1_velocity == other.contact1_velocity
974
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact2_velocity == other.contact2_velocity
975
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact_velocity_error == other.contact_velocity_error
976
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact_acceleration == other.contact_acceleration
977
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact_acceleration_desired == other.contact_acceleration_desired
978
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact_acceleration_error == other.contact_acceleration_error
979
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact1_acceleration_drift == other.contact1_acceleration_drift
980
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact2_acceleration_drift == other.contact2_acceleration_drift
981
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && contact_acceleration_deviation == other.contact_acceleration_deviation
982
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && extended_motion_propagators_joint1 == other.extended_motion_propagators_joint1
983
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && lambdas_joint1 == other.lambdas_joint1
984
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && extended_motion_propagators_joint2 == other.extended_motion_propagators_joint2
985 //
986
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
987
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && da1_da == other.da1_da
988 //
989
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
990
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 && da2_da == other.da2_da
991 //
992
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
993
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 && dac_da == other.dac_da;
994 }
995
996 ✗ bool operator!=(const RigidConstraintDataTpl & other) const
997 {
998 ✗ return !(*this == other);
999 }
1000 };
1001 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
1002
1003 } // namespace pinocchio
1004
1005 #endif // ifndef __pinocchio_algorithm_contact_info_hpp__
1006