Merge pull request #10 from pFernbach/topic/fix_checks

fix contactSequence check methods

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

@@ -415,6 +415,18 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         std::cout << "CoM acceleration trajectory not defined for phase : " << i << std::endl;
         return false;
       }
+      if (phase.m_c->dim() != 3) {
+        std::cout << "CoM trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
+      if (phase.m_dc->dim() != 3) {
+        std::cout << "CoM velocity trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
+      if (phase.m_ddc->dim() != 3) {
+        std::cout << "CoM acceleration trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
       if (phase.m_c->min() != phase.timeInitial()) {
         std::cout << "CoM trajectory do not start at t_init for phase : " << i << std::endl;
         return false;
@@ -508,6 +520,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         std::cout << "AM velocity trajectory not defined for phase : " << i << std::endl;
         return false;
       }
+      if (phase.m_L->dim() != 3) {
+        std::cout << "AM trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
+      if (phase.m_dL->dim() != 3) {
+        std::cout << "AM derivative trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
       if (phase.m_L->min() != phase.timeInitial()) {
         std::cout << "AM trajectory do not start at t_init for phase : " << i << std::endl;
         return false;
@@ -582,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))) {
@@ -601,7 +622,10 @@ 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
@@ -612,7 +636,10 @@ 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
@@ -847,6 +874,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         std::cout << "ZMP trajectory not defined for phase : " << i << std::endl;
         return false;
       }
+      if (phase.m_zmp->dim() != 3) {
+        std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i << std::endl;
+        return false;
+      }
       if (phase.m_zmp->min() != phase.timeInitial()) {
         std::cout << "ZMP trajectory do not start at t_init for phase : " << i << std::endl;
         return false;

unittest/examples.cpp

@@ -80,6 +80,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
   BOOST_CHECK(cs.haveCentroidalValues());
   BOOST_CHECK(cs.haveCentroidalTrajectories());
   BOOST_CHECK(cs.haveEffectorsTrajectories());
+  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false));
 }
 
 BOOST_AUTO_TEST_CASE(step_in_place_WB) {

unittest/python/serialization_examples.py

@@ -149,6 +149,7 @@ class ExamplesSerialization(unittest.TestCase):
         self.assertTrue(cs.haveCentroidalValues())
         self.assertTrue(cs.haveCentroidalTrajectories())
         self.assertTrue(cs.haveEffectorsTrajectories())
+        self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
         checkCS(self, cs, root=True, effector=True, wholeBody=False)
 
     def test_step_in_place_WB(self):

unittest/scenario.cpp

@@ -1974,4 +1974,101 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
   BOOST_CHECK_THROW(cs1.phaseAtTime(10.), std::invalid_argument);
 }
 
+BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory) {
+  ContactSequence cs1 = ContactSequence(0);
+  ContactPhase cp0 = ContactPhase(0, 2);
+  cp0.m_c = buildRandomPolynomial3D(0, 2);
+  cp0.m_c_init = cp0.m_c->operator()(0);
+  cp0.m_c_final = cp0.m_c->operator()(2);
+  cp0.m_dc = buildRandomPolynomial3D(0, 2);
+  cp0.m_dc_init = cp0.m_dc->operator()(0);
+  cp0.m_dc_final = cp0.m_dc->operator()(2);
+  cp0.m_ddc = buildRandomPolynomial3D(0, 2);
+  cp0.m_ddc_init = cp0.m_ddc->operator()(0);
+  cp0.m_ddc_final = cp0.m_ddc->operator()(2);
+  cp0.m_L = buildRandomPolynomial3D(0, 2);
+  cp0.m_L_init = cp0.m_L->operator()(0);
+  cp0.m_L_final = cp0.m_L->operator()(2);
+  cp0.m_dL = buildRandomPolynomial3D(0, 2);
+  cp0.m_dL_init = cp0.m_dL->operator()(0);
+  cp0.m_dL_final = cp0.m_dL->operator()(2);
+  cp0.m_zmp = buildRandomPolynomial3D(0, 2);
+
+  cs1.append(cp0);
+  BOOST_CHECK(cs1.haveCOMtrajectories());
+  BOOST_CHECK(cs1.haveAMtrajectories());
+  BOOST_CHECK(cs1.haveZMPtrajectories());
+}
+
+BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory_wrong_dimension) {
+  ContactSequence cs1 = ContactSequence(0);
+  ContactPhase cp0 = ContactPhase(0, 2);
+  cp0.m_c = buildRandomPolynomial3D(0, 2);
+  cp0.m_c_init = cp0.m_c->operator()(0);
+  cp0.m_c_final = cp0.m_c->operator()(2);
+  cp0.m_dc = buildRandomPolynomial12D(0, 2);
+  cp0.m_dc_init = cp0.m_dc->operator()(0).head<3>();
+  cp0.m_dc_final = cp0.m_dc->operator()(2).head<3>();
+  cp0.m_ddc = buildRandomPolynomial3D(0, 2);
+  cp0.m_ddc_init = cp0.m_ddc->operator()(0);
+  cp0.m_ddc_final = cp0.m_ddc->operator()(2);
+  cp0.m_L = buildRandomPolynomial12D(0, 2);
+  cp0.m_L_init = cp0.m_L->operator()(0).head<3>();
+  cp0.m_L_final = cp0.m_L->operator()(2).head<3>();
+  cp0.m_dL = buildRandomPolynomial3D(0, 2);
+  cp0.m_dL_init = cp0.m_dL->operator()(0);
+  cp0.m_dL_final = cp0.m_dL->operator()(2);
+  cp0.m_zmp = buildRandomPolynomial12D(0, 2);
+
+  cs1.append(cp0);
+  BOOST_CHECK(!cs1.haveCOMtrajectories());
+  BOOST_CHECK(!cs1.haveAMtrajectories());
+  BOOST_CHECK(!cs1.haveZMPtrajectories());
+}
+
+BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory) {
+  ContactSequence cs1 = ContactSequence(0);
+  ContactPhase cp0 = ContactPhase(0, 1);
+  ContactPhase cp1 = ContactPhase(1, 2);
+  ContactPhase cp2 = ContactPhase(2, 3);
+
+  cp1.addEffectorTrajectory("left_foot", buildRandomSE3LinearTraj(1, 2));
+
+  cp0.addContact("left_foot", ContactPatch(SE3(cp1.effectorTrajectories("left_foot")->operator()(1.).matrix())));
+  cp2.addContact("left_foot", ContactPatch(SE3(cp1.effectorTrajectories("left_foot")->operator()(2.).matrix())));
+
+  cs1.append(cp0);
+  cs1.append(cp1);
+  cs1.append(cp2);
+  BOOST_CHECK(cs1.haveEffectorsTrajectories());
+  BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3));
+  BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3, true));
+}
+
+BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation) {
+  ContactSequence cs1 = ContactSequence(0);
+  ContactPhase cp0 = ContactPhase(0, 1);
+  ContactPhase cp1 = ContactPhase(1, 2);
+  ContactPhase cp2 = ContactPhase(2, 3);
+
+  cp1.addEffectorTrajectory("left_foot", buildRandomSE3LinearTraj(1, 2));
+  SE3 p0(cp1.effectorTrajectories("left_foot")->operator()(1.).matrix());
+  SE3 p1(cp1.effectorTrajectories("left_foot")->operator()(2.).matrix());
+  SE3 p0_r = SE3::Identity().setRandom();
+  SE3 p1_r = SE3::Identity().setRandom();
+  p0.rotation() = p0_r.rotation();
+  p1.rotation() = p1_r.rotation();
+
+  cp0.addContact("left_foot", ContactPatch(p0));
+  cp2.addContact("left_foot", ContactPatch(p1));
+
+  cs1.append(cp0);
+  cs1.append(cp1);
+  cs1.append(cp2);
+  BOOST_CHECK(!cs1.haveEffectorsTrajectories());
+  BOOST_CHECK(!cs1.haveEffectorsTrajectories(1e-6));
+  BOOST_CHECK(!cs1.haveEffectorsTrajectories(1e-6, true));
+  BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-6, false));
+}
+
 BOOST_AUTO_TEST_SUITE_END()