Merge pull request #12 from loco-3d/devel

Devel

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

@@ -10,19 +10,16 @@
 #include "multicontact-api/serialization/spatial.hpp"
 
 #include <curves/fwd.h>
-#include <curves/curve_abc.h>
 #include <curves/piecewise_curve.h>
+#include <curves/serialization/curves.hpp>
 #include <map>
 #include <vector>
 #include <set>
 #include <string>
 #include <sstream>
-#include <boost/shared_ptr.hpp>
-#include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/string.hpp>
 #include <boost/serialization/map.hpp>
 #include <boost/serialization/vector.hpp>
-#include <curves/serialization/registeration.hpp>
 
 namespace multicontact_api {
 namespace scenario {
@@ -45,9 +42,9 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
   typedef curves::curve_abc_t curve_t;
   // typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
   typedef curves::curve_SE3_t curve_SE3_t;
-  typedef boost::shared_ptr<curve_t> curve_ptr;
+  typedef curves::curve_ptr_t curve_ptr;
   // typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
-  typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr;
+  typedef curves::curve_SE3_ptr_t curve_SE3_ptr;
   typedef curves::piecewise3_t piecewise3_t;
   typedef curves::piecewise_t piecewise_t;
   typedef piecewise_t::t_time_t t_time_t;

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
@@ -818,6 +845,24 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
     return true;
   }
 
+  /**
+   * @brief haveContactModelDefined check that all the contact patch have a contact_model defined
+   * @return
+   */
+  bool haveContactModelDefined() const {
+    size_t i = 0;
+    for (const ContactPhase& phase : m_contact_phases) {
+      for (const std::string& eeName : phase.effectorsInContact()) {
+        if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type == ContactType::CONTACT_UNDEFINED) {
+          std::cout << "ContactModel not defined for phase " << i << " and effector " << eeName << std::endl;
+          return false;
+        }
+      }
+      ++i;
+    }
+    return true;
+  }
+
   /**
    * @brief haveZMPtrajectories check that all the contact phases have a zmp trajectory
    * @return
@@ -829,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;
@@ -1025,7 +1074,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
    */
   piecewise_SE3_t concatenateEffectorTrajectories(const std::string& eeName) const {
     piecewise_SE3_t res = piecewise_SE3_t();
-    transform_t last_placement;
+    transform_t last_placement, first_placement;
     // first find the first and last phase with a trajectory for this effector
     size_t first_phase = m_contact_phases.size();
     size_t last_phase = 0;
@@ -1034,9 +1083,20 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         last_phase = i;
         if (first_phase > i) {
           first_phase = i;
+          curve_SE3_ptr curve = m_contact_phases.at(i).effectorTrajectories().at(eeName);
+          first_placement = curve->operator()(curve->min());
         }
       }
     }
+    if(first_phase == m_contact_phases.size())
+      throw std::invalid_argument("The contact sequence doesn't have any phase with an effector trajectory"
+                                  " for the given effector name");
+    if(first_phase > 0){
+      // add a first constant phase at the initial placement
+      curve_SE3_ptr ptr_init(new SE3Curve_t(first_placement, first_placement, m_contact_phases.at(0).timeInitial(),
+                                       m_contact_phases.at(first_phase).timeInitial()));
+      res.add_curve_ptr(ptr_init);
+    }
     // loop over this phases to concatenate the trajectories
     for (size_t i = first_phase; i <= last_phase; ++i) {
       if (m_contact_phases.at(i).effectorHaveAtrajectory(eeName)) {
@@ -1049,6 +1109,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
         res.add_curve_ptr(ptr);
       }
     }
+    if(last_phase < m_contact_phases.size() - 1){
+      // add a last constant phase until the end of the contact sequence
+      curve_SE3_ptr ptr_final(new SE3Curve_t(last_placement, last_placement, m_contact_phases.at(last_phase).timeFinal(),
+                                       m_contact_phases.back().timeFinal()));
+      res.add_curve_ptr(ptr_final);
+    }
     return res;
   }
 

include/multicontact-api/scenario/fwd.hpp

@@ -26,9 +26,10 @@ struct ContactSequenceTpl;
 typedef ContactSequenceTpl<ContactPhase> ContactSequence;
 
 template <typename Scalar>
-struct ContactModelPlanarTpl;
-typedef ContactModelPlanarTpl<double> ContactModelPlanar;
+struct ContactModelTpl;
+typedef ContactModelTpl<double> ContactModel;
 
+enum ContactType { CONTACT_UNDEFINED, CONTACT_PLANAR, CONTACT_POINT };
 enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
 
 }  // namespace scenario

unittest/CMakeLists.txt

-# Copyright (c) 2015-2018, CNRS
-# Authors: Justin Carpentier <jcarpent@laas.fr>
+# Copyright (c) 2015-2020, CNRS
+# Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel
 
 ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK)
 
@@ -11,11 +11,9 @@ SET(${PROJECT_NAME}_TESTS
 
 FOREACH(TEST ${${PROJECT_NAME}_TESTS})
   ADD_UNIT_TEST(${TEST} ${TEST})
-  TARGET_LINK_LIBRARIES(${TEST} ${Boost_LIBRARIES})
-  PKG_CONFIG_USE_DEPENDENCY(${TEST} eigen3)
-  PKG_CONFIG_USE_DEPENDENCY(${TEST} pinocchio)
-  PKG_CONFIG_USE_DEPENDENCY(${TEST} curves)
+  TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} ${Boost_LIBRARIES})
 ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS})
+
 TARGET_COMPILE_DEFINITIONS(examples PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
 
 IF(BUILD_PYTHON_INTERFACE)

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/scenario.py

@@ -9,9 +9,9 @@ from curves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial
 from numpy import array, array_equal, isclose, random
 
 import pinocchio as pin
-from multicontact_api import ContactModelPlanar, ContactPatch, ContactPhase, ContactSequence
+from multicontact_api import ContactModel, ContactPatch, ContactPhase, ContactSequence, ContactType
 from pinocchio import SE3, Quaternion
-
+import pickle
 pin.switchToNumpyArray()
 
 
@@ -149,57 +149,112 @@ def buildRandomContactPhase(min=-1, max=-1):
 
 
 class ContactModelTest(unittest.TestCase):
-    def test_contact_model_planar(self):
+    def test_contact_model(self):
         mu = 0.3
-        zmp_radius = 0.01
+        # default constructor
+        mp = ContactModel()
+        self.assertEqual(mp.mu, -1.)
+        self.assertEqual(mp.contact_type, ContactType.CONTACT_UNDEFINED)
+        self.assertEqual(mp.num_contact_points, 1)
+        self.assertEqual(len(mp.contact_points_positions.shape), 1)
+        self.assertEqual(mp.contact_points_positions.shape[0], 3)
+        self.assertTrue(not mp.contact_points_positions.any())
+
+        # constructor with friction
+        mp_mu = ContactModel(mu)
+        self.assertEqual(mp_mu.mu, mu)
+        self.assertEqual(mp_mu.contact_type, ContactType.CONTACT_UNDEFINED)
+        self.assertEqual(mp.num_contact_points, 1)
+        self.assertEqual(len(mp.contact_points_positions.shape), 1)
+        self.assertEqual(mp.contact_points_positions.shape[0], 3)
+        self.assertTrue(not mp.contact_points_positions.any())
+
         # constructor with both values
-        mp1 = ContactModelPlanar(mu, zmp_radius)
+        mp1 = ContactModel(mu, ContactType.CONTACT_PLANAR)
         # test getter bindings
         self.assertEqual(mp1.mu, mu)
-        self.assertEqual(mp1.ZMP_radius, zmp_radius)
+        self.assertEqual(mp1.contact_type, ContactType.CONTACT_PLANAR)
+        self.assertEqual(mp.num_contact_points, 1)
+        self.assertEqual(len(mp.contact_points_positions.shape), 1)
+        self.assertEqual(mp.contact_points_positions.shape[0], 3)
+        self.assertTrue(not mp.contact_points_positions.any())
 
         # copy constructor :
-        mp2 = ContactModelPlanar(mp1)
+        mp2 = ContactModel(mp1)
         self.assertEqual(mp2.mu, mu)
-        self.assertEqual(mp2.ZMP_radius, zmp_radius)
+        self.assertEqual(mp2.contact_type, ContactType.CONTACT_PLANAR)
+        self.assertEqual(mp.num_contact_points, 1)
+        self.assertEqual(len(mp.contact_points_positions.shape), 1)
+        self.assertEqual(mp.contact_points_positions.shape[0], 3)
+        self.assertTrue(not mp.contact_points_positions.any())
 
         # test operator ==
         self.assertTrue(mp1 == mp2)
         mp1.mu = 0.5
         self.assertTrue(mp1 != mp2)
 
+    def test_contact_model_contact_points(self):
+        mp1 = ContactModel(0.5, ContactType.CONTACT_PLANAR)
+        mp1.num_contact_points = 4
+        self.assertEqual(mp1.num_contact_points, 4)
+        self.assertEqual(mp1.contact_points_positions.shape[0], 3)
+        self.assertEqual(mp1.contact_points_positions.shape[1], 4)
+        self.assertTrue(not mp1.contact_points_positions.any())
+
+        pos = np.random.rand(3, 5)
+        mp1.contact_points_positions = pos
+        self.assertEqual(mp1.num_contact_points, 5)
+        self.assertEqual(mp1.contact_points_positions.shape[0], 3)
+        self.assertEqual(mp1.contact_points_positions.shape[1], 5)
+        self.assertTrue(isclose(mp1.contact_points_positions, pos).all())
+
+        generators = mp1.generatorMatrix()
+        self.assertEqual(generators.shape[0], 6)
+        self.assertEqual(generators.shape[1], 5*3)
+
+        mp1.num_contact_points = 2
+        self.assertEqual(mp1.num_contact_points, 2)
+        self.assertEqual(mp1.contact_points_positions.shape[0], 3)
+        self.assertEqual(mp1.contact_points_positions.shape[1], 2)
+        self.assertTrue(not mp1.contact_points_positions.any())
+
     def test_contact_model_serialization_default(self):
-        mp1 = ContactModelPlanar()
+        mp1 = ContactModel()
         mp1.saveAsText("mp_test.txt")
-        mp_txt = ContactModelPlanar()
+        mp_txt = ContactModel()
         mp_txt.loadFromText("mp_test.txt")
         self.assertEqual(mp1, mp_txt)
         mp1.saveAsBinary("mp_test")
-        mp_bin = ContactModelPlanar()
+        mp_bin = ContactModel()
         mp_bin.loadFromBinary("mp_test")
         self.assertEqual(mp1, mp_bin)
         mp1.saveAsXML("mp_test.xml", 'ContactModel')
-        mp_xml = ContactModelPlanar()
+        mp_xml = ContactModel()
         mp_xml.loadFromXML("mp_test.xml", 'ContactPatch')
         self.assertEqual(mp1, mp_xml)
+        mp_pickled = pickle.dumps(mp1)
+        mp_from_pickle = pickle.loads(mp_pickled)
+        self.assertEqual(mp1, mp_from_pickle)
 
     def test_contact_model_serialization_full(self):
         mu = 0.3
-        zmp_radius = 0.01
         # constructor with both values
-        mp1 = ContactModelPlanar(mu, zmp_radius)
+        mp1 = ContactModel(mu, ContactType.CONTACT_PLANAR)
         mp1.saveAsText("mp_test.txt")
-        mp_txt = ContactModelPlanar()
+        mp_txt = ContactModel()
         mp_txt.loadFromText("mp_test.txt")
         self.assertEqual(mp1, mp_txt)
         mp1.saveAsBinary("mp_test")
-        mp_bin = ContactModelPlanar()
+        mp_bin = ContactModel()
         mp_bin.loadFromBinary("mp_test")
         self.assertEqual(mp1, mp_bin)
         mp1.saveAsXML("mp_test.xml", 'ContactModel')
-        mp_xml = ContactModelPlanar()
+        mp_xml = ContactModel()
         mp_xml.loadFromXML("mp_test.xml", 'ContactPatch')
         self.assertEqual(mp1, mp_xml)
+        mp_pickled = pickle.dumps(mp1)
+        mp_from_pickle = pickle.loads(mp_pickled)
+        self.assertEqual(mp1, mp_from_pickle)
 
 
 class ContactPatchTest(unittest.TestCase):
@@ -278,6 +333,9 @@ class ContactPatchTest(unittest.TestCase):
         cp_xml = ContactPatch()
         cp_xml.loadFromXML("cp_test.xml", 'ContactPatch')
         self.assertEqual(cp1, cp_xml)
+        cp_pickled = pickle.dumps(cp1)
+        cp_from_pickle = pickle.loads(cp_pickled)
+        self.assertEqual(cp1, cp_from_pickle)
 
     def test_serialization_full(self):
         p = SE3()
@@ -295,6 +353,32 @@ class ContactPatchTest(unittest.TestCase):
         cp_xml = ContactPatch()
         cp_xml.loadFromXML("cp_test.xml", 'ContactPatch')
         self.assertEqual(cp1, cp_xml)
+        cp_pickled = pickle.dumps(cp1)
+        cp_from_pickle = pickle.loads(cp_pickled)
+        self.assertEqual(cp1, cp_from_pickle)
+
+    def test_contact_patch_model_accessor(self):
+        p = SE3()
+        p.setRandom()
+        cp1 = ContactPatch(p, 0.9)
+        cm = cp1.contact_model
+        self.assertEqual(cm.mu, 0.9)
+        cm.mu = 0.5
+        self.assertEqual(cp1.friction, 0.5)
+
+        cp1.contact_model.contact_type = ContactType.CONTACT_PLANAR
+        self.assertEqual(cp1.contact_model.contact_type, ContactType.CONTACT_PLANAR)
+
+        cp1.friction = 2
+        self.assertEqual(cp1.contact_model.mu, 2)
+        self.assertEqual(cm.mu, 2)
+
+        pos = np.random.rand(3, 4)
+        cp1.contact_model.contact_points_positions = pos
+        self.assertEqual(cp1.contact_model.num_contact_points, 4)
+        self.assertEqual(cp1.contact_model.contact_points_positions.shape[0], 3)
+        self.assertEqual(cp1.contact_model.contact_points_positions.shape[1], 4)
+        self.assertTrue(isclose(cp1.contact_model.contact_points_positions, pos).all())
 
 
 class ContactPhaseTest(unittest.TestCase):
@@ -1144,6 +1228,9 @@ class ContactPhaseTest(unittest.TestCase):
         cp_xml = ContactPhase()
         cp_xml.loadFromXML("cp_test.xml", 'ContactPhase')
         self.assertEqual(cp1, cp_xml)
+        cp_pickled = pickle.dumps(cp1)
+        cp_from_pickle = pickle.loads(cp_pickled)
+        self.assertEqual(cp1, cp_from_pickle)
 
     def test_contact_phase_serialization_full(self):
         cp1 = buildRandomContactPhase(0., 2.)
@@ -1160,6 +1247,9 @@ class ContactPhaseTest(unittest.TestCase):
         cp_xml.loadFromXML("cp_test_full.xml", 'ContactPhase')
         self.assertEqual(cp1, cp_xml)
         # TODO : check serialization from another file
+        cp_pickled = pickle.dumps(cp1)
+        cp_from_pickle = pickle.loads(cp_pickled)
+        self.assertEqual(cp1, cp_from_pickle)
 
     def test_contact_phase_contacts_variation(self):
         # # contacts repositioned :
@@ -1591,6 +1681,41 @@ class ContactSequenceTest(unittest.TestCase):
         consistent = cs4.haveTimings()
         self.assertFalse(consistent)
 
+    def test_contact_sequence_have_contact_model(self):
+        cs1 = ContactSequence(0)
+        cp0 = buildRandomContactPhase(0, 2)
+        cp1 = buildRandomContactPhase(2, 4.)
+        cs1.append(cp0)
+        cs1.append(cp1)
+        self.assertFalse(cs1.haveContactModelDefined())
+
+        mp1 = ContactModel(0.5, ContactType.CONTACT_PLANAR)
+        pos = np.random.rand(3, 5)
+        mp1.contact_points_positions = pos
+        mp2 = ContactModel(1., ContactType.CONTACT_POINT)
+        pos = np.random.rand(3, 5)
+        mp1.contact_points_positions = pos
+
+        cs1.contactPhases[0].contactPatch("right-leg").contact_model = mp1
+        cs1.contactPhases[0].contactPatch("left-leg").contact_model = mp2
+        cs1.contactPhases[1].contactPatch("right-leg").contact_model = mp1
+        cs1.contactPhases[1].contactPatch("left-leg").contact_model = mp2
+        self.assertTrue(cs1.haveContactModelDefined())
+
+        cp2 = buildRandomContactPhase(6., 8.)
+        cs1.append(cp2)
+        self.assertFalse(cs1.haveContactModelDefined())
+        mp3 = ContactModel(0.2)
+        cs1.contactPhases[2].contactPatch("right-leg").contact_model = mp3
+        cs1.contactPhases[2].contactPatch("left-leg").contact_model = mp2
+        self.assertFalse(cs1.haveContactModelDefined())
+
+        mp3.contact_type = ContactType.CONTACT_PLANAR  # do not change the contact model already in the seqence
+        self.assertFalse(cs1.haveContactModelDefined())
+
+        cs1.contactPhases[2].contactPatch("right-leg").contact_model.contact_type = ContactType.CONTACT_PLANAR
+        self.assertTrue(cs1.haveContactModelDefined())
+
     def test_contact_sequence_concatenate_config_traj(self):
         cs1 = ContactSequence(0)
         cp0 = buildRandomContactPhase(0, 2)
@@ -1741,6 +1866,15 @@ class ContactSequenceTest(unittest.TestCase):
         with self.assertRaises(ValueError):
             cs1.phaseAtTime(10.)
 
+    def test_pickle_contact_sequence(self):
+        cs = ContactSequence()
+        for i in range(10):
+            cp = buildRandomContactPhase(0., 2.)
+            cs.append(cp)
+        cs_pickled = pickle.dumps(cs)
+        cs_from_pickle = pickle.loads(cs_pickled)
+        self.assertEqual(cs_from_pickle, cs)
+
 
 if __name__ == '__main__':
     unittest.main()

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/python/trivial.py

@@ -6,7 +6,7 @@ import multicontact_api
 class TrivialTest(unittest.TestCase):
     """ A test written by someone who has no idea what this software is about"""
     def test_trivial(self):
-        comopla = multicontact_api.ContactModelPlanar()
+        comopla = multicontact_api.ContactModel()
         epsilon = 0.00001
         value_wanted = -1.0
         self.assertTrue((comopla.mu - value_wanted) < epsilon)

unittest/scenario.cpp

-// Copyright (c) 2015-2018, CNRS
-// Authors: Justin Carpentier <jcarpent@laas.fr>
+// Copyright (c) 2019-2020, CNRS
+// Authors: Pierre Fernbach <pierre.fernbach@laas.fr>,
 
 #include <iostream>
 
@@ -7,19 +7,16 @@
 #include <boost/test/unit_test.hpp>
 #include <boost/utility/binary.hpp>
 
-#include "multicontact-api/scenario/contact-model-planar.hpp"
+#include "multicontact-api/scenario/contact-model.hpp"
 #include "multicontact-api/scenario/contact-patch.hpp"
 #include "multicontact-api/scenario/contact-phase.hpp"
 #include "multicontact-api/scenario/contact-sequence.hpp"
 
 #include <curves/fwd.h>
-#include <curves/so3_linear.h>
 #include <curves/se3_curve.h>
 #include <curves/polynomial.h>
 #include <curves/bezier_curve.h>
 #include <curves/piecewise_curve.h>
-#include <curves/exact_cubic.h>
-#include <curves/cubic_hermite_spline.h>
 
 typedef Eigen::Matrix<double, 1, 1> point1_t;
 using curves::point3_t;
@@ -36,21 +33,9 @@ using curves::t_point3_t;
 using curves::t_pointX_t;
 using namespace multicontact_api::scenario;
 typedef ContactSequence::ContactPhaseVector ContactPhaseVector;
+typedef ContactModel::Matrix3X Matrix3X;
+typedef ContactModel::Matrix6X Matrix6X;
 
-template <typename Scalar>
-struct ATpl {
-  typedef pinocchio::SE3Tpl<Scalar> SE3;
-
-  explicit ATpl() : data() {}
-  explicit ATpl(const ATpl& other) : data(other.data){};
-
-  bool operator==(const ATpl& other) { return data == other.data; }
-
- protected:
-  SE3 data;
-};
-
-typedef ATpl<double> Ad;
 typedef pinocchio::SE3Tpl<double> SE3;
 
 curve_ptr_t buildPiecewisePolynomialC2() {
@@ -356,19 +341,107 @@ void explicitContactPhaseAssertEqual(ContactPhase& cp1, ContactPhase& cp2) {
 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
 BOOST_AUTO_TEST_CASE(contact_model) {
-  const double mu = 0.3;
-  const double ZMP_radius = 0.01;
+  ContactModel mp;
+  BOOST_CHECK(mp.m_mu == -1.);
+  BOOST_CHECK(mp.m_contact_type == ContactType::CONTACT_UNDEFINED);
+  BOOST_CHECK(mp.num_contact_points() == 1);
+  BOOST_CHECK(mp.contact_points_positions().cols() == 1);
+  BOOST_CHECK(mp.contact_points_positions().isZero());
 
-  ContactModelPlanar mp1(mu, ZMP_radius);
-  ContactModelPlanar mp2(mp1);
+  const double mu = 0.3;
+  ContactModel mp_mu(mu);
+  BOOST_CHECK(mp_mu.m_mu == mu);
+  BOOST_CHECK(mp_mu.m_contact_type == ContactType::CONTACT_UNDEFINED);
+  BOOST_CHECK(mp_mu.num_contact_points() == 1);
+  BOOST_CHECK(mp_mu.contact_points_positions().cols() == 1);
+  BOOST_CHECK(mp_mu.contact_points_positions().isZero());
+
+  ContactModel mp1(mu, ContactType::CONTACT_PLANAR);
   BOOST_CHECK(mp1.m_mu == mu);
-  BOOST_CHECK(mp1.m_ZMP_radius == ZMP_radius);
+  BOOST_CHECK(mp1.m_contact_type == ContactType::CONTACT_PLANAR);
+  BOOST_CHECK(mp1.num_contact_points() == 1);
+  BOOST_CHECK(mp1.contact_points_positions().cols() == 1);
+  BOOST_CHECK(mp1.contact_points_positions().isZero());
+
+  ContactModel mp2(mp1);
+  BOOST_CHECK(mp2.m_mu == mu);
+  BOOST_CHECK(mp2.m_contact_type == ContactType::CONTACT_PLANAR);
+  BOOST_CHECK(mp2.num_contact_points() == 1);
+  BOOST_CHECK(mp2.contact_points_positions().cols() == 1);
+  BOOST_CHECK(mp2.contact_points_positions().isZero());
+}
+
+BOOST_AUTO_TEST_CASE(contact_model_points_positions) {
+  const double mu = 0.3;
+  ContactModel mp(mu, ContactType::CONTACT_PLANAR);
+