5 #ifndef __pinocchio_algorithm_contact_info_hpp__
6 #define __pinocchio_algorithm_contact_info_hpp__
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"
26 template<ContactType contact_type>
53 template<
typename _Scalar>
56 template<
typename _Scalar>
59 typedef _Scalar Scalar;
62 template<
typename _Scalar>
65 typedef _Scalar Scalar;
66 typedef Eigen::Matrix<Scalar, -1, 1, Eigen::ColMajor, 6> Vector6Max;
78 return Kp == other.
Kp &&
Kd == other.
Kd;
83 return !(*
this == other);
93 template<
typename NewScalar>
98 res.
Kp =
Kp.template cast<NewScalar>();
99 res.Kd =
Kd.template cast<NewScalar>();
105 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
106 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
107 template<
typename NewScalar,
typename Scalar,
int Options>
113 template<
typename _Scalar,
int _Options>
116 typedef _Scalar Scalar;
124 template<
typename _Scalar,
int _Options>
127 typedef _Scalar Scalar;
138 template<
typename _Scalar,
int _Options>
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
144 typedef _Scalar Scalar;
149 typedef ConstraintModelBase<RigidConstraintModelTpl<_Scalar, _Options>> Base;
151 template<
typename NewScalar,
int NewOptions>
152 friend struct RigidConstraintModelTpl;
155 using Base::colwise_span_indexes;
156 using Base::colwise_sparsity;
158 typedef RigidConstraintModelTpl ContactModel;
159 typedef RigidConstraintDataTpl<Scalar, Options> ContactData;
160 typedef RigidConstraintDataTpl<Scalar, Options> ConstraintData;
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;
174 JointIndex joint1_id;
177 JointIndex joint2_id;
180 SE3 joint1_placement;
183 SE3 joint2_placement;
189 SE3 desired_contact_placement;
192 Motion desired_contact_velocity;
195 Motion desired_contact_acceleration;
198 BaumgarteCorrectorParameters corrector;
201 BooleanVector colwise_joint1_sparsity;
204 BooleanVector colwise_joint2_sparsity;
207 IndexVector joint1_span_indexes;
210 IndexVector joint2_span_indexes;
212 IndexVector loop_span_indexes;
218 size_t depth_joint1, depth_joint2;
224 RigidConstraintModelTpl()
244 template<
int OtherOptions,
template<
typename,
int>
class JointCollectionTpl>
245 RigidConstraintModelTpl(
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,
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())
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)
282 template<
int OtherOptions,
template<
typename,
int>
class JointCollectionTpl>
283 RigidConstraintModelTpl(
285 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
286 const JointIndex joint1_id,
287 const SE3 & joint1_placement,
291 , joint1_id(joint1_id)
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())
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)
316 template<
int OtherOptions,
template<
typename,
int>
class JointCollectionTpl>
317 RigidConstraintModelTpl(
319 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
320 const JointIndex joint1_id,
321 const JointIndex joint2_id,
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())
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)
352 template<
int OtherOptions,
template<
typename,
int>
class JointCollectionTpl>
353 RigidConstraintModelTpl(
355 const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model,
356 const JointIndex joint1_id,
360 , joint1_id(joint1_id)
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())
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)
381 ConstraintData createData()
const
383 return ConstraintData(*
this);
394 template<
int OtherOptions>
395 bool operator==(
const RigidConstraintModelTpl<Scalar, OtherOptions> & other)
const
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;
417 template<
int OtherOptions>
418 bool operator!=(
const RigidConstraintModelTpl<Scalar, OtherOptions> & other)
const
420 return !(*
this == other);
425 template<
template<
typename,
int>
class JointCollectionTpl>
427 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
428 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
429 RigidConstraintDataTpl<Scalar, Options> & cdata)
const
431 PINOCCHIO_UNUSED_VARIABLE(model);
434 cdata.oMc1 = data.oMi[joint1_id] * joint1_placement;
436 cdata.oMc1 = joint1_placement;
439 cdata.oMc2 = data.oMi[joint2_id] * joint2_placement;
441 cdata.oMc2 = joint2_placement;
444 cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
450 Matrix36 getA1(
const RigidConstraintDataTpl<Scalar, Options> & cdata)
const
453 const SE3 & oMl = cdata.oMc1;
454 typedef typename SE3::Vector3 Vector3;
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;
460 res.template leftCols<3>() = oMl.rotation().transpose();
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>());
466 #undef PINOCCHIO_INTERNAL_COMPUTATION
474 Matrix36 getA2(
const RigidConstraintDataTpl<Scalar, Options> & cdata)
const
477 const SE3 & oM1 = cdata.oMc1;
478 const SE3 & oM2 = cdata.oMc2;
479 typedef typename SE3::Vector3 Vector3;
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;
485 res.template leftCols<3>() = -oM1.rotation().transpose();
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>());
491 #undef PINOCCHIO_INTERNAL_COMPUTATION
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
508 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
509 typedef typename Data::Vector3 Vector3;
510 OutputMatrix & res = _res.const_cast_derived();
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());
523 const Matrix36 A1 = getA1(cdata);
524 const Matrix36 A2 = getA2(cdata);
525 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
527 if (!(colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj]))
532 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
533 const ConstColXpr Jcol = data.J.col(jj);
535 if (colwise_joint1_sparsity[jj] && colwise_joint2_sparsity[jj])
538 AxSi.noalias() = A * Jcol;
540 else if (colwise_joint1_sparsity[jj])
541 AxSi.noalias() = A1 * Jcol;
543 AxSi.noalias() = A2 * Jcol;
545 res.noalias() += AxSi * mat.row(jj);
551 template<
typename JacobianMatrix,
template<
typename,
int>
class JointCollectionTpl>
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
558 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
559 JacobianMatrix & jacobian_matrix = _jacobian_matrix.const_cast_derived();
561 const RigidConstraintModelTpl & cmodel = *
this;
563 const SE3 & oMc1 = cdata.oMc1;
564 const SE3 & oMc2 = cdata.oMc2;
565 const SE3 & c1Mc2 = cdata.c1Mc2;
567 for (Eigen::DenseIndex jj = 0; jj < model.nv; ++jj)
570 if (colwise_joint1_sparsity[jj] || colwise_joint2_sparsity[jj])
572 const int sign = colwise_joint1_sparsity[jj] != colwise_joint2_sparsity[jj]
573 ? colwise_joint1_sparsity[jj] ? +1 : -1
576 typedef typename Data::Matrix6x::ConstColXpr ConstColXpr;
577 const ConstColXpr Jcol = data.J.col(jj);
578 const MotionRef<const ConstColXpr> Jcol_motion(Jcol);
583 switch (cmodel.reference_frame)
588 const Motion Jcol_local1(oMc1.actInv(Jcol_motion));
589 const Motion Jcol_local2(oMc2.actInv(Jcol_motion));
590 const typename Motion::Vector3 Jdiff_linear =
591 Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
592 jacobian_matrix.col(jj) = Jdiff_linear;
597 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
598 jacobian_matrix.col(jj) = Jcol_local.linear();
603 Motion Jcol_local(oMc2.actInv(Jcol_motion));
604 Jcol_local.linear() =
605 c1Mc2.rotation() * Jcol_local.linear();
606 jacobian_matrix.col(jj) = -Jcol_local.linear();
613 const typename Motion::Vector3 Jdiff_linear =
614 (oMc2.translation() - oMc1.translation()).
cross(Jcol_motion.angular());
615 jacobian_matrix.col(jj) = Jdiff_linear;
620 typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear());
622 Jcol_local_world_aligned_linear -=
623 oMc1.translation().cross(Jcol_motion.angular());
625 Jcol_local_world_aligned_linear -=
626 oMc2.translation().cross(Jcol_motion.angular());
627 jacobian_matrix.col(jj) = Jcol_local_world_aligned_linear * Scalar(
sign);
632 PINOCCHIO_THROW_PRETTY(
633 std::invalid_argument,
"Contact3D in world frame is not managed");
641 check_expression_if_real<Scalar>(
sign != 0) &&
"sign should be equal to +1 or -1.");
642 switch (cmodel.reference_frame)
645 const Motion Jcol_local(oMc1.actInv(Jcol_motion));
646 jacobian_matrix.col(jj) = Jcol_local.toVector() * Scalar(
sign);
650 Motion Jcol_local_world_aligned(Jcol_motion);
651 Jcol_local_world_aligned.linear() -=
652 oMc1.translation().cross(Jcol_local_world_aligned.angular());
653 jacobian_matrix.col(jj) = Jcol_local_world_aligned.toVector() * Scalar(
sign);
657 PINOCCHIO_THROW_PRETTY(
658 std::invalid_argument,
"Contact6D in world frame is not managed");
665 assert(
false &&
"must never happened");
677 return contact_dim<CONTACT_3D>::value;
679 return contact_dim<CONTACT_6D>::value;
681 return contact_dim<CONTACT_UNDEFINED>::value;
687 template<
typename NewScalar>
688 RigidConstraintModelTpl<NewScalar, Options> cast()
const
690 typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
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;
706 res.depth_joint1 = depth_joint1;
707 res.depth_joint2 = depth_joint2;
708 res.loop_span_indexes = loop_span_indexes;
714 template<
int OtherOptions,
template<
typename,
int>
class JointCollectionTpl>
715 void init(
const ModelTpl<Scalar, OtherOptions, JointCollectionTpl> & model)
717 PINOCCHIO_CHECK_INPUT_ARGUMENT(
719 "reference_frame should be LOCAL or LOCAL_WORLD_ALIGNED");
721 depth_joint1 =
static_cast<size_t>(model.supports[joint1_id].size());
722 depth_joint2 =
static_cast<size_t>(model.supports[joint2_id].size());
724 typedef ModelTpl<Scalar, OtherOptions, JointCollectionTpl> Model;
725 typedef typename Model::JointModel JointModel;
726 static const bool default_sparsity_value =
false;
727 colwise_joint1_sparsity.fill(default_sparsity_value);
728 colwise_joint2_sparsity.fill(default_sparsity_value);
729 joint1_span_indexes.reserve((
size_t)model.njoints);
730 joint2_span_indexes.reserve((
size_t)model.njoints);
732 JointIndex current1_id = 0;
734 current1_id = joint1_id;
736 JointIndex current2_id = 0;
738 current2_id = joint2_id;
740 while (current1_id != current2_id)
742 if (current1_id > current2_id)
744 const JointModel & joint1 = model.joints[current1_id];
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 < joint1.nv(); ++k, ++current1_col_id)
749 colwise_joint1_sparsity[current1_col_id] =
true;
751 current1_id = model.parents[current1_id];
755 const JointModel & joint2 = model.joints[current2_id];
756 joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
757 Eigen::DenseIndex current2_col_id = joint2.idx_v();
758 for (
int k = 0; k < joint2.nv(); ++k, ++current2_col_id)
760 colwise_joint2_sparsity[current2_col_id] =
true;
762 current2_id = model.parents[current2_id];
765 assert(current1_id == current2_id &&
"current1_id should be equal to current2_id");
767 if (type == CONTACT_3D)
769 JointIndex current_id = current1_id;
770 while (current_id > 0)
772 const JointModel & joint = model.joints[current_id];
773 joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
774 joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
775 Eigen::DenseIndex current_row_id = joint.idx_v();
776 for (
int k = 0; k < joint.nv(); ++k, ++current_row_id)
778 colwise_joint1_sparsity[current_row_id] =
true;
779 colwise_joint2_sparsity[current_row_id] =
true;
781 current_id = model.parents[current_id];
786 joint1_span_indexes.rbegin(), joint1_span_indexes.rbegin() + 1, joint1_span_indexes.rend());
788 joint2_span_indexes.rbegin(), joint2_span_indexes.rbegin() + 1, joint2_span_indexes.rend());
789 Base::colwise_span_indexes.reserve((
size_t)model.nv);
790 loop_span_indexes.reserve((
size_t)model.nv);
791 for (Eigen::DenseIndex col_id = 0; col_id < model.nv; ++col_id)
793 if (colwise_joint1_sparsity[col_id] || colwise_joint2_sparsity[col_id])
795 colwise_span_indexes.push_back(col_id);
796 colwise_sparsity[col_id] =
true;
799 if (colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id])
801 loop_span_indexes.push_back(col_id);
807 template<
typename Scalar,
int Options,
class Allocator>
811 typedef std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> VectorType;
812 size_t total_size = 0;
813 for (
typename VectorType::const_iterator it = contact_models.begin();
814 it != contact_models.end(); ++it)
815 total_size += it->size();
823 template<
typename _Scalar,
int _Options>
825 RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
827 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
829 typedef _Scalar Scalar;
835 typedef RigidConstraintModelTpl<Scalar, Options> ContactModel;
836 typedef RigidConstraintDataTpl ContactData;
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;
861 Motion contact_placement_error;
864 Motion contact1_velocity;
867 Motion contact2_velocity;
870 Motion contact_velocity_error;
873 Motion contact_acceleration;
876 Motion contact_acceleration_desired;
879 Motion contact_acceleration_error;
883 Motion contact1_acceleration_drift;
887 Motion contact2_acceleration_drift;
890 Motion contact_acceleration_deviation;
892 VectorOfMatrix6 extended_motion_propagators_joint1;
893 VectorOfMatrix6 lambdas_joint1;
894 VectorOfMatrix6 extended_motion_propagators_joint2;
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;
901 RigidConstraintDataTpl()
905 explicit RigidConstraintDataTpl(
const ContactModel & contact_model)
906 : contact_force(Force::Zero())
907 , contact_placement_error(Motion::Zero())
908 , contact1_velocity(Motion::Zero())
909 , contact2_velocity(Motion::Zero())
910 , contact_velocity_error(Motion::Zero())
911 , contact_acceleration(Motion::Zero())
912 , contact_acceleration_desired(Motion::Zero())
913 , contact_acceleration_error(Motion::Zero())
914 , contact1_acceleration_drift(Motion::Zero())
915 , contact2_acceleration_drift(Motion::Zero())
916 , contact_acceleration_deviation(Motion::Zero())
917 , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero())
918 , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero())
919 , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero())
920 , dv1_dq(6, contact_model.
nv)
921 , da1_dq(6, contact_model.
nv)
922 , da1_dv(6, contact_model.
nv)
923 , da1_da(6, contact_model.
nv)
924 , dv2_dq(6, contact_model.
nv)
925 , da2_dq(6, contact_model.
nv)
926 , da2_dv(6, contact_model.
nv)
927 , da2_da(6, contact_model.
nv)
928 , dvc_dq(contact_model.size(), contact_model.
nv)
929 , dac_dq(contact_model.size(), contact_model.
nv)
930 , dac_dv(contact_model.size(), contact_model.
nv)
931 , dac_da(contact_model.size(), contact_model.
nv)
935 bool operator==(
const RigidConstraintDataTpl & other)
const
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
952 && dv1_dq == other.dv1_dq && da1_dq == other.da1_dq && da1_dv == other.da1_dv
953 && da1_da == other.da1_da
955 && dv2_dq == other.dv2_dq && da2_dq == other.da2_dq && da2_dv == other.da2_dv
956 && da2_da == other.da2_da
958 && dvc_dq == other.dvc_dq && dac_dq == other.dac_dq && dac_dv == other.dac_dv
959 && dac_da == other.dac_da;
962 bool operator!=(
const RigidConstraintDataTpl & other)
const
964 return !(*
this == other);
967 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Main pinocchio namespace.
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") ContactCholeskyDecompositionTpl
Contact Cholesky decomposition structure. This structure allows to compute in a efficient and parsimo...
@ CONTACT_6D
Point contact model.
@ CONTACT_UNDEFINED
 
Scalar sign(const Scalar &t)
Returns the robust sign of t.
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
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.
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.
Vector6Max Kp
Proportional corrector value.
Vector6Max Kd
Damping corrector value.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Common traits structure to fully define base classes for CRTP.