pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
contact-info.hpp
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
16namespace pinocchio
17{
19 enum ContactType
20 {
21 CONTACT_3D = 0,
22 CONTACT_6D,
23 CONTACT_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>
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 BaumgarteCorrectorParametersTpl(int size = 6)
69 : Kp(size)
70 , Kd(size)
71 {
72 Kp.setZero();
73 Kd.setZero();
74 }
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
88 Vector6Max Kp;
89
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>
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
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
171 ContactType type;
172
174 JointIndex joint1_id;
175
177 JointIndex joint2_id;
178
180 SE3 joint1_placement;
181
183 SE3 joint2_placement;
184
186 ReferenceFrame reference_frame;
187
189 SE3 desired_contact_placement;
190
192 Motion desired_contact_velocity;
193
195 Motion desired_contact_acceleration;
196
198 BaumgarteCorrectorParameters corrector;
199
201 BooleanVector colwise_joint1_sparsity;
202
204 BooleanVector colwise_joint2_sparsity;
205
207 IndexVector joint1_span_indexes;
208
210 IndexVector joint2_span_indexes;
211
212 IndexVector loop_span_indexes;
213
215 int nv;
216
218 size_t depth_joint1, depth_joint2;
219
220 protected:
224 RigidConstraintModelTpl()
225 : nv(-1)
226 , depth_joint1(0)
227 , depth_joint2(0)
228 {
229 }
230
231 public:
244 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
245 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 , type(type)
255 , joint1_id(joint1_id)
256 , joint2_id(joint2_id)
257 , joint1_placement(joint1_placement)
258 , joint2_placement(joint2_placement)
259 , reference_frame(reference_frame)
260 , desired_contact_placement(SE3::Identity())
261 , desired_contact_velocity(Motion::Zero())
262 , desired_contact_acceleration(Motion::Zero())
263 , corrector(size())
264 , colwise_joint1_sparsity(model.nv)
265 , colwise_joint2_sparsity(model.nv)
266 , joint1_span_indexes((size_t)model.njoints)
267 , joint2_span_indexes((size_t)model.njoints)
268 , loop_span_indexes((size_t)model.nv)
269 {
270 init(model);
271 }
272
282 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
283 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 , type(type)
291 , joint1_id(joint1_id)
292 , joint2_id(0)
293 , joint1_placement(joint1_placement)
294 , joint2_placement(SE3::Identity())
295 , reference_frame(reference_frame)
296 , desired_contact_placement(SE3::Identity())
297 , desired_contact_velocity(Motion::Zero())
298 , desired_contact_acceleration(Motion::Zero())
299 , corrector(size())
300 , colwise_joint1_sparsity(model.nv)
301 , colwise_joint2_sparsity(model.nv)
302 , joint1_span_indexes((size_t)model.njoints)
303 , joint2_span_indexes((size_t)model.njoints)
304 , loop_span_indexes((size_t)model.nv)
305 {
306 init(model);
307 }
308
316 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
317 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 , type(type)
325 , joint1_id(joint1_id)
326 , joint2_id(joint2_id)
327 , joint1_placement(SE3::Identity())
328 , joint2_placement(SE3::Identity())
329 , reference_frame(reference_frame)
330 , desired_contact_placement(SE3::Identity())
331 , desired_contact_velocity(Motion::Zero())
332 , desired_contact_acceleration(Motion::Zero())
333 , corrector(size())
334 , colwise_joint1_sparsity(model.nv)
335 , colwise_joint2_sparsity(model.nv)
336 , joint1_span_indexes((size_t)model.njoints)
337 , joint2_span_indexes((size_t)model.njoints)
338 , loop_span_indexes((size_t)model.nv)
339 {
340 init(model);
341 }
342
352 template<int OtherOptions, template<typename, int> class JointCollectionTpl>
353 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 , type(type)
360 , joint1_id(joint1_id)
361 , joint2_id(0) // set to be the Universe
362 , joint1_placement(SE3::Identity())
363 , joint2_placement(SE3::Identity())
364 , reference_frame(reference_frame)
365 , desired_contact_placement(SE3::Identity())
366 , desired_contact_velocity(Motion::Zero())
367 , desired_contact_acceleration(Motion::Zero())
368 , corrector(size())
369 , colwise_joint1_sparsity(model.nv)
370 , colwise_joint2_sparsity(model.nv)
371 , joint1_span_indexes((size_t)model.njoints)
372 , joint2_span_indexes((size_t)model.njoints)
373 , loop_span_indexes((size_t)model.nv)
374 {
375 init(model);
376 }
377
381 ConstraintData createData() const
382 {
383 return ConstraintData(*this);
384 }
385
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
417 template<int OtherOptions>
418 bool operator!=(const RigidConstraintModelTpl<Scalar, OtherOptions> & other) const
419 {
420 return !(*this == other);
421 }
422
425 template<template<typename, int> class JointCollectionTpl>
426 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 if (joint1_id > 0)
434 cdata.oMc1 = data.oMi[joint1_id] * joint1_placement;
435 else
436 cdata.oMc1 = joint1_placement;
437
438 if (joint2_id > 0)
439 cdata.oMc2 = data.oMi[joint2_id] * joint2_placement;
440 else
441 cdata.oMc2 = joint2_placement;
442
443 // Compute relative placement
444 cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
445 }
446
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
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> class JointCollectionTpl>
500 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 OutputMatrix & res = _res.const_cast_derived();
510
511 PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), model.nv);
512 PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.cols(), res.cols());
513 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), size());
514 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 const Matrix36 A1 = getA1(cdata);
523 const Matrix36 A2 = getA2(cdata);
524 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
525 {
526 if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
527 continue;
528 Matrix36 A;
529 Vector3 AxSi;
530
531 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
532 const ConstColXpr Jcol = data.J.col(jj);
533
534 if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
535 {
536 A = A1 + A2;
537 AxSi.noalias() = A * Jcol;
538 }
539 else if (colwise_joint1_sparsity[jj])
540 AxSi.noalias() = A1 * Jcol;
541 else
542 AxSi.noalias() = A2 * Jcol;
543
544 res.noalias() += AxSi * mat.row(jj);
545 }
546 }
547
550 template<typename JacobianMatrix, template<typename, int> class JointCollectionTpl>
551 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 JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
559
560 const RigidConstraintModelTpl & cmodel = *this;
561
562 const SE3 & oMc1 = cdata.oMc1;
563 const SE3 & oMc2 = cdata.oMc2;
564 const SE3 & c1Mc2 = cdata.c1Mc2;
565
566 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
567 {
568
569 if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
570 {
571 const int sign = colwise_joint1_sparsity[jj] != colwise_joint2_sparsity[jj]
572 ? colwise_joint1_sparsity[jj] ? +1 : -1
573 : 0; // specific case for CONTACT_3D
574
575 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
576 const ConstColXpr Jcol = data.J.col(jj);
577 const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
578
579 switch (cmodel.type)
580 {
581 case CONTACT_3D: {
582 switch (cmodel.reference_frame)
583 {
584 case LOCAL: {
585 if (sign == 0)
586 {
587 const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations
588 const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
589 const typename Motion::Vector3 Jdiff_linear =
590 Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
591 jacobian_matrix.col(jj) = Jdiff_linear;
592 break;
593 }
594 else if (sign == 1)
595 {
596 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
597 jacobian_matrix.col(jj) = Jcol_local.linear();
598 break;
599 }
600 else // sign == -1
601 {
602 Motion Jcol_local(oMc2.actInv(Jcol_motion)); // TODO: simplify computations
603 Jcol_local.linear() =
604 c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
605 jacobian_matrix.col(jj) = -Jcol_local.linear();
606 break;
607 }
608 }
609 case LOCAL_WORLD_ALIGNED: {
610 if (sign == 0)
611 {
612 const typename Motion::Vector3 Jdiff_linear =
613 (oMc2.translation() - oMc1.translation()).cross(Jcol_motion.angular());
614 jacobian_matrix.col(jj) = Jdiff_linear;
615 break;
616 }
617 else
618 {
619 typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear());
620 if (sign == 1)
621 Jcol_local_world_aligned_linear -=
622 oMc1.translation().cross(Jcol_motion.angular());
623 else
624 Jcol_local_world_aligned_linear -=
625 oMc2.translation().cross(Jcol_motion.angular());
626 jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(sign);
627 break;
628 }
629 }
630 case WORLD: {
631 PINOCCHIO_THROW_PRETTY(
632 std::invalid_argument, "Contact3D in world frame is not managed");
633 }
634 }
635 break;
636 }
637
638 case CONTACT_6D: {
639 assert(
640 check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
641 switch (cmodel.reference_frame)
642 {
643 case LOCAL: {
644 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
645 jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(sign);
646 break;
647 }
648 case LOCAL_WORLD_ALIGNED: {
649 Motion Jcol_local_world_aligned(Jcol_motion);
650 Jcol_local_world_aligned.linear() -=
651 oMc1.translation().cross(Jcol_local_world_aligned.angular());
652 jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(sign);
653 break;
654 }
655 case WORLD: {
656 PINOCCHIO_THROW_PRETTY(
657 std::invalid_argument, "Contact6D in world frame is not managed");
658 }
659 }
660 break;
661 }
662
663 default:
664 assert(false && "must never happened");
665 break;
666 }
667 }
668 }
669 }
670
671 int size() const
672 {
673 switch (type)
674 {
675 case CONTACT_3D:
676 return contact_dim<CONTACT_3D>::value;
677 case CONTACT_6D:
678 return contact_dim<CONTACT_6D>::value;
679 default:
680 return contact_dim<CONTACT_UNDEFINED>::value;
681 }
682 return -1;
683 }
684
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 void init(const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
715 {
716 PINOCCHIO_CHECK_INPUT_ARGUMENT(
717 reference_frame == LOCAL || reference_frame == LOCAL_WORLD_ALIGNED,
718 "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
719 nv = model.nv;
720 depth_joint1 = static_cast<size_t>(model.supports[joint1_id].size());
721 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 colwise_joint1_sparsity.fill(default_sparsity_value);
727 colwise_joint2_sparsity.fill(default_sparsity_value);
728 joint1_span_indexes.reserve((size_t)model.njoints);
729 joint2_span_indexes.reserve((size_t)model.njoints);
730
731 JointIndex current1_id = 0;
732 if (joint1_id > 0)
733 current1_id = joint1_id;
734
735 JointIndex current2_id = 0;
736 if (joint2_id > 0)
737 current2_id = joint2_id;
738
739 while (current1_id != current2_id)
740 {
741 if (current1_id > current2_id)
742 {
743 const JointModel & joint1 = model.joints[current1_id];
744 const int j1nv = joint1.nv();
745 joint1_span_indexes.push_back((Eigen::DenseIndex)current1_id);
746 Eigen::DenseIndex current1_col_id = joint1.idx_v();
747 for (int k = 0; k < j1nv; ++k, ++current1_col_id)
748 {
749 colwise_joint1_sparsity[current1_col_id] = true;
750 }
751 current1_id = model.parents[current1_id];
752 }
753 else
754 {
755 const JointModel & joint2 = model.joints[current2_id];
756 const int j2nv = joint2.nv();
757 joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
758 Eigen::DenseIndex current2_col_id = joint2.idx_v();
759 for (int k = 0; k < j2nv; ++k, ++current2_col_id)
760 {
761 colwise_joint2_sparsity[current2_col_id] = true;
762 }
763 current2_id = model.parents[current2_id];
764 }
765 }
766 assert(current1_id == current2_id && "current1_id should be equal to current2_id");
767
768 if (type == CONTACT_3D)
769 {
770 JointIndex current_id = current1_id;
771 while (current_id > 0)
772 {
773 const JointModel & joint = model.joints[current_id];
774 const int jnv = joint.nv();
775 joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
776 joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
777 Eigen::DenseIndex current_row_id = joint.idx_v();
778 for (int k = 0; k < jnv; ++k, ++current_row_id)
779 {
780 colwise_joint1_sparsity[current_row_id] = true;
781 colwise_joint2_sparsity[current_row_id] = true;
782 }
783 current_id = model.parents[current_id];
784 }
785 }
786
787 std::rotate(
788 joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
789 std::rotate(
790 joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
791 Base::colwise_span_indexes.reserve((size_t)model.nv);
792 loop_span_indexes.reserve((size_t)model.nv);
793 for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
794 {
795 if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
796 {
797 colwise_span_indexes.push_back(col_id);
798 colwise_sparsity[col_id] = true;
799 }
800
801 if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
802 {
803 loop_span_indexes.push_back(col_id);
804 }
805 }
806 }
807 };
808
809 template<typename Scalar, int Options, class Allocator>
810 size_t getTotalConstraintSize(
811 const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
812 {
813 typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
814 size_t total_size = 0;
815 for (typename VectorType::const_iterator it = contact_models.begin();
816 it != contact_models.end(); ++it)
817 total_size += it->size();
818
819 return total_size;
820 }
821
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
851 Force contact_force;
852
854 SE3 oMc1;
855
857 SE3 oMc2;
858
860 SE3 c1Mc2;
861
863 Motion contact_placement_error;
864
866 Motion contact1_velocity;
867
869 Motion contact2_velocity;
870
872 Motion contact_velocity_error;
873
875 Motion contact_acceleration;
876
878 Motion contact_acceleration_desired;
879
881 Motion contact_acceleration_error;
882
885 Motion contact1_acceleration_drift;
886
889 Motion contact2_acceleration_drift;
890
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
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 explicit RigidConstraintDataTpl(const ContactModel & contact_model)
937 : contact_force(Force::Zero())
938 , oMc1(SE3::Identity())
939 , oMc2(SE3::Identity())
940 , c1Mc2(SE3::Identity())
941 , contact_placement_error(Motion::Zero())
942 , contact1_velocity(Motion::Zero())
943 , contact2_velocity(Motion::Zero())
944 , contact_velocity_error(Motion::Zero())
945 , contact_acceleration(Motion::Zero())
946 , contact_acceleration_desired(Motion::Zero())
947 , contact_acceleration_error(Motion::Zero())
948 , contact1_acceleration_drift(Motion::Zero())
949 , contact2_acceleration_drift(Motion::Zero())
950 , contact_acceleration_deviation(Motion::Zero())
951 , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
952 , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
953 , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
954 , dv1_dq(Matrix6x::Zero(6, contact_model.nv))
955 , da1_dq(Matrix6x::Zero(6, contact_model.nv))
956 , da1_dv(Matrix6x::Zero(6, contact_model.nv))
957 , da1_da(Matrix6x::Zero(6, contact_model.nv))
958 , dv2_dq(Matrix6x::Zero(6, contact_model.nv))
959 , da2_dq(Matrix6x::Zero(6, contact_model.nv))
960 , da2_dv(Matrix6x::Zero(6, contact_model.nv))
961 , da2_da(Matrix6x::Zero(6, contact_model.nv))
962 , dvc_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
963 , dac_dq(MatrixX::Zero(contact_model.size(), contact_model.nv))
964 , dac_dv(MatrixX::Zero(contact_model.size(), contact_model.nv))
965 , dac_da(MatrixX::Zero(contact_model.size(), contact_model.nv))
966 {
967 }
968
969 bool operator==(const RigidConstraintDataTpl & other) const
970 {
971 return contact_force == other.contact_force && oMc1 == other.oMc1 && oMc2 == other.oMc2
972 && c1Mc2 == other.c1Mc2 && contact_placement_error == other.contact_placement_error
973 && contact1_velocity == other.contact1_velocity
974 && contact2_velocity == other.contact2_velocity
975 && contact_velocity_error == other.contact_velocity_error
976 && contact_acceleration == other.contact_acceleration
977 && contact_acceleration_desired == other.contact_acceleration_desired
978 && contact_acceleration_error == other.contact_acceleration_error
979 && contact1_acceleration_drift == other.contact1_acceleration_drift
980 && contact2_acceleration_drift == other.contact2_acceleration_drift
981 && contact_acceleration_deviation == other.contact_acceleration_deviation
982 && extended_motion_propagators_joint1 == other.extended_motion_propagators_joint1
983 && lambdas_joint1 == other.lambdas_joint1
984 && extended_motion_propagators_joint2 == other.extended_motion_propagators_joint2
985 //
986 && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
987 && da1_da == other.da1_da
988 //
989 && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
990 && da2_da == other.da2_da
991 //
992 && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
993 && 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__
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition fwd.hpp:47
pinocchio::WORLD
@ WORLD
Definition fwd.hpp:48
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition fwd.hpp:52
pinocchio::LOCAL
@ LOCAL
Definition fwd.hpp:50
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::ContactType
ContactType
&#160;
Definition contact-info.hpp:20
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition contact-info.hpp:22
pinocchio::CONTACT_UNDEFINED
@ CONTACT_UNDEFINED
&#160;
Definition contact-info.hpp:23
pinocchio::sign
Scalar sign(const Scalar &t)
Returns the robust sign of t.
Definition sign.hpp:14
pinocchio::cross
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition skew.hpp:228
pinocchio::getTotalConstraintSize
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") RigidConstraintModelTpl size_t getTotalConstraintSize(const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Contact model structure containg all the info describing the rigid contact model.
Definition contact-info.hpp:810
pinocchio::BaumgarteCorrectorParametersTpl::Kp
Vector6Max Kp
Proportional corrector value.
Definition contact-info.hpp:88
pinocchio::BaumgarteCorrectorParametersTpl::Kd
Vector6Max Kd
Damping corrector value.
Definition contact-info.hpp:91
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition fwd.hpp:99
pinocchio::contact_dim
Definition contact-info.hpp:28
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72