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 |