5 #ifndef __pinocchio_algorithm_contact_info_hpp__
6 #define __pinocchio_algorithm_contact_info_hpp__
26 template<ContactType contact_type>
53 template<
typename _Scalar>
56 template<
typename _Scalar>
62 template<
typename _Scalar>
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>();
107 template<
typename NewScalar,
typename Scalar,
int Options>
113 template<
typename _Scalar,
int _Options>
124 template<
typename _Scalar,
int _Options>
138 template<
typename _Scalar,
int _Options>
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
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;
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,
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,
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,
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,
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)
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
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
444 cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2);
450 Matrix36 getA1(
const RigidConstraintDataTpl<Scalar, Options> & cdata)
const
453 const SE3 & oMl = cdata.oMc1;
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();
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;
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();
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;
510 OutputMatrix &
res = _res.const_cast_derived();
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));
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();
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);
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);
658 std::invalid_argument,
"Contact6D in world frame is not managed");
665 assert(
false &&
"must never happened");
687 template<
typename NewScalar>
688 RigidConstraintModelTpl<NewScalar, Options>
cast()
const
690 typedef RigidConstraintModelTpl<NewScalar, Options> ReturnType;
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)
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;
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);
740 while (current1_id != current2_id)
742 if (current1_id > current2_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];
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");
770 while (current_id > 0)
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();
815 total_size += it->size();
823 template<
typename _Scalar,
int _Options>
825 RigidConstraintDataTpl : ConstraintDataBase<RigidConstraintDataTpl<_Scalar, _Options>>
827 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
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;
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())
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);
971 #endif // ifndef __pinocchio_algorithm_contact_info_hpp__