haveEffectorTrajectories now take as input the precision treshold used in isApprox()

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

@@ -24,6 +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)
 
   template <class PyClass>
   void visit(PyClass& cl) const {
@@ -128,12 +129,14 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
              "Also check that the time interval of this trajectories matches the one of the phase.\n"
              "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()),
              "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."))
         .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

@@ -591,7 +591,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
    * If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact placement.
    * @return
    */
-  bool haveEffectorsTrajectories() const{
+  bool haveEffectorsTrajectories( const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const{
     if(!haveTimings())
       return false;
     for(size_t i = 0 ; i < m_contact_phases.size() -1 ; ++i){
@@ -610,14 +610,14 @@ 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())){
+        if(!pMax.isApprox(m_contact_phases.at(i+1).contactPatches().at(eeName).placement(), prec)){
           std::cout<<"Effector trajectory for "<<eeName
                   << " do not end at it's contact placement in the next phase, for phase "<<i<<std::endl;
           return false;
         }
         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())){
+          if(!pMin.isApprox(m_contact_phases.at(i-1).contactPatches().at(eeName).placement(), prec)){
             std::cout<<"Effector trajectory for "<<eeName
                     << " do not start at it's contact placement in the previous phase, for phase "<<i<<std::endl;
             return false;