ContactSequence::haveEffectorTrajectory take an additionnal argument to ignore rotation

include/multicontact-api/bindings/python/scenario/contact-sequence.hpp

@@ -24,7 +24,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
   BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads, CS::createContact, 2, 3)
   BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorToPlacement_overloads, CS::moveEffectorToPlacement, 2, 4)
   BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorOf_overloads, CS::moveEffectorOf, 2, 4)
-  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 1)
+  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 2)
 
   template <class PyClass>
   void visit(PyClass& cl) const {
@@ -130,14 +130,16 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
              "and that the trajectories start and end and the correct values defined in each phase.")
         .def("haveEffectorsTrajectories", &CS::haveEffectorsTrajectories,
              cs_haveEffectorTrajectories_overloads(
-                 (bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision()),
+                 (bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision(),
+                  bp::args("use_rotation") = true),
                  "check that for each phase preceeding a contact creation,"
                  "an SE3 trajectory is defined for the effector that will be in contact.\n"
                  "Also check that this trajectory is defined on the time-interval of the phase.\n"
                  "Also check that the trajectory correctly end at the placement defined for the contact in the next "
                  "phase.\n"
                  "If this effector was in contact in the previous phase,"
-                 "it check that the trajectory start at the previous contact placement."))
+                 "it check that the trajectory start at the previous contact placement.\n"
+                 "If use_rotation == false, only the translation part of the transforms are compared. "))
         .def("haveJointsTrajectories", &CS::haveJointsTrajectories,
              "Check that a q trajectory is defined for each phases.\n"
              "Also check that the time interval of this trajectories matches the one of the phase.\n"

include/multicontact-api/scenario/contact-sequence.hpp

@@ -602,7 +602,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
    * placement.
    * @return
    */
-  bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
+  bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
+                                 const bool use_rotation = true) const {
     if (!haveTimings()) return false;
     for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
       for (std::string eeName : m_contact_phases.at(i).getContactsCreated(m_contact_phases.at(i + 1))) {
@@ -621,7 +622,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
           return false;
         }
         ContactPatch::SE3 pMax = ContactPatch::SE3((*traj)(traj->max()).matrix());
-        if (!pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec)) {
+        if ((use_rotation && ! pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec))
+            || (! pMax.translation().isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement().translation(), prec))) {
           std::cout << "Effector trajectory for " << eeName
                     << " do not end at it's contact placement in the next phase, for phase " << i << std::endl;
           std::cout << "Last point : " << std::endl
@@ -632,7 +634,9 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         }
         if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
           ContactPatch::SE3 pMin = ContactPatch::SE3((*traj)(traj->min()).matrix());
-          if (!pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec)) {
+          if ((use_rotation && ! pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec))
+              || (! pMin.translation().isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement().translation(), prec)))
+          {
             std::cout << "Effector trajectory for " << eeName
                       << " do not start at it's contact placement in the previous phase, for phase " << i << std::endl;
             std::cout << "First point : " << std::endl