Merge pull request #11 from pFernbach/topic/contact_model

Improve ContactModel

include/multicontact-api/CMakeLists.txt

@@ -15,7 +15,7 @@ SET(${PROJECT_NAME}_SERIALIZATION_HEADERS
   )
 
 SET(${PROJECT_NAME}_SCENARIO_HEADERS
-  scenario/contact-model-planar.hpp
+  scenario/contact-model.hpp
   scenario/contact-patch.hpp
   scenario/contact-phase.hpp
   scenario/contact-sequence.hpp
@@ -29,7 +29,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   bindings/python/geometry/linear-cone.hpp
   bindings/python/geometry/second-order-cone.hpp
   bindings/python/serialization/archive.hpp
-  bindings/python/scenario/contact-model-planar.hpp
+  bindings/python/scenario/contact-model.hpp
   bindings/python/scenario/contact-patch.hpp
   bindings/python/scenario/contact-phase.hpp
   bindings/python/scenario/contact-sequence.hpp

include/multicontact-api/bindings/python/scenario/contact-model-planar.hpp

-// Copyright (c) 2015-2018, CNRS
-// Authors: Justin Carpentier <jcarpent@laas.fr>
-
-#ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
-#define __multicontact_api_python_scenario_contact_model_planar_hpp__
-
-#include <string>
-
-#include "multicontact-api/scenario/contact-model-planar.hpp"
-#include "multicontact-api/bindings/python/serialization/archive.hpp"
-#include "multicontact-api/bindings/python/utils/printable.hpp"
-
-namespace multicontact_api {
-namespace python {
-
-namespace bp = boost::python;
-
-template <typename ContactModelPlanar>
-struct ContactModelPlanarPythonVisitor
-    : public boost::python::def_visitor<ContactModelPlanarPythonVisitor<ContactModelPlanar> > {
-  typedef typename ContactModelPlanar::Scalar Scalar;
-
-  template <class PyClass>
-  void visit(PyClass& cl) const {
-    cl.def(bp::init<>())
-        .def(bp::init<Scalar, Scalar>(bp::args("mu", "ZMP_radius")))
-        .def(bp::init<ContactModelPlanar>(bp::args("other"), "Copy contructor."))
-        .def_readwrite("mu", &ContactModelPlanar::m_mu, "Friction coefficient.")
-        .def_readwrite("ZMP_radius", &ContactModelPlanar::m_ZMP_radius, "Radius of the ZMP region.")
-
-        .def(bp::self == bp::self)
-        .def(bp::self != bp::self)
-        .def("copy", &copy, "Returns a copy of *this.");
-  }
-
-  static void expose(const std::string& class_name) {
-    std::string doc = "Contact Model Planar";
-    bp::class_<ContactModelPlanar>(class_name.c_str(), doc.c_str(), bp::no_init)
-        .def(ContactModelPlanarPythonVisitor<ContactModelPlanar>())
-        .def(SerializableVisitor<ContactModelPlanar>())
-        .def(PrintableVisitor<ContactModelPlanar>());
-  }
-
- private:
-  static ContactModelPlanar copy(const ContactModelPlanar& self) { return ContactModelPlanar(self); }
-};
-}  // namespace python
-}  // namespace multicontact_api
-
-#endif  // ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__

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

+// Copyright (c) 2015-2018, CNRS
+// Authors: Justin Carpentier <jcarpent@laas.fr>
+
+#ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__
+#define __multicontact_api_python_scenario_contact_model_planar_hpp__
+
+#include <string>
+#include "multicontact-api/scenario/fwd.hpp"
+#include "multicontact-api/scenario/contact-model.hpp"
+#include "multicontact-api/bindings/python/serialization/archive.hpp"
+#include "multicontact-api/bindings/python/utils/printable.hpp"
+#include <eigenpy/eigenpy.hpp>
+
+namespace multicontact_api {
+namespace python {
+
+namespace bp = boost::python;
+
+template <typename ContactModel>
+struct ContactModelPythonVisitor : public boost::python::def_visitor<ContactModelPythonVisitor<ContactModel> > {
+  typedef typename ContactModel::Scalar Scalar;
+  typedef scenario::ContactType ContactType;
+  typedef typename ContactModel::Matrix3X Matrix3X;
+  typedef typename ContactModel::Matrix6X Matrix6X;
+
+  template <class PyClass>
+  void visit(PyClass& cl) const {
+    cl.def(bp::init<>())
+        .def(bp::init<Scalar>(bp::args("mu")))
+        .def(bp::init<Scalar, ContactType>(bp::args("mu", "contact_type")))
+        .def(bp::init<ContactModel>(bp::args("other"), "Copy contructor."))
+        .def_readwrite("mu", &ContactModel::m_mu, "Friction coefficient.")
+        .def_readwrite("contact_type", &ContactModel::m_contact_type, "Enum that define the type of contact.")
+        .add_property("num_contact_points", &getNumContact, &setNumContact,
+                      "The number of contact points used to model this contact. \n"
+                      "Changing this value will clear the contact_points_positions matrix")
+        .add_property("contact_points_positions", &getContactPositions, &setContactPositions,
+                      "3xnum_contact_points matrix defining the contact points positions in the frame of the contact "
+                      "placement. \n"
+                      "num_contact_points is automatically updated to the number of columns of this matrix.")
+        .def("generatorMatrix", &ContactModel::generatorMatrix,
+             "generatorMatrix Return a 6x(num_contact_points*3) matrix"
+             "containing the generator used to compute contact forces.")
+        .def(bp::self == bp::self)
+        .def(bp::self != bp::self)
+        .def("copy", &copy, "Returns a copy of *this.");
+  }
+
+  static void expose(const std::string& class_name) {
+    std::string doc = "Contact Model";
+    bp::class_<ContactModel>(class_name.c_str(), doc.c_str(), bp::no_init)
+        .def(ContactModelPythonVisitor<ContactModel>())
+        .def(SerializableVisitor<ContactModel>())
+        .def(PrintableVisitor<ContactModel>());
+
+    ENABLE_SPECIFIC_MATRIX_TYPE(Matrix3X);
+    ENABLE_SPECIFIC_MATRIX_TYPE(Matrix6X);
+  }
+
+ private:
+  static ContactModel copy(const ContactModel& self) { return ContactModel(self); }
+  // define setter and getter
+  static size_t getNumContact(ContactModel& self) { return self.num_contact_points(); }
+  static void setNumContact(ContactModel& self, const size_t num) { self.num_contact_points(num); }
+  static Matrix3X getContactPositions(ContactModel& self) { return self.contact_points_positions(); }
+  static void setContactPositions(ContactModel& self, const Matrix3X& pos) { self.contact_points_positions(pos); }
+};
+}  // namespace python
+}  // namespace multicontact_api
+
+#endif  // ifndef __multicontact_api_python_scenario_contact_model_planar_hpp__

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

@@ -34,6 +34,7 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc
                       "Placement of the patch represented as a pinocchio SE3 object.")
         .add_property("friction", &getFriction, &setFriction,
                       "Friction coefficient between the robot and the environment for this contact.")
+        .def_readwrite("contact_model", &ContactPatch::m_contact_model, "The contact model defining this contact.")
         .def(bp::self == bp::self)
         .def(bp::self != bp::self)
         .def("copy", &copy, "Returns a copy of *this.");

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

@@ -160,6 +160,8 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
         .def("haveFriction", &CS::haveFriction,
              "check that all the contact patch used in the sequence have"
              "a friction coefficient initialized.")
+        .def("haveContactModelDefined", &CS::haveContactModelDefined,
+             "haveContactModelDefined check that all the contact patch have a contact_model defined")
         .def("haveZMPtrajectories", &CS::haveZMPtrajectories,
              "check that all the contact phases have a ZMP trajectory.")
         .def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
@@ -260,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
   static bp::list getAllEffectorsInContactAsList(CS& self) {
     return toPythonList<std::string>(self.getAllEffectorsInContact());
   }
-
 };
 }  // namespace python
 }  // namespace multicontact_api

include/multicontact-api/bindings/python/serialization/archive.hpp

@@ -13,22 +13,20 @@ namespace bp = boost::python;
 
 template <typename Derived>
 struct cs_pickle_suite : bp::pickle_suite {
+  static bp::object getstate(const Derived& cs) {
+    std::ostringstream os;
+    boost::archive::text_oarchive oa(os);
+    oa << cs;
+    return bp::str(os.str());
+  }
 
-    static bp::object getstate (const Derived& cs) {
-        std::ostringstream os;
-        boost::archive::text_oarchive oa(os);
-        oa << cs;
-        return bp::str(os.str());
-    }
-
-    static void
-    setstate(Derived& cs, bp::object entries) {
-        bp::str s = bp::extract<bp::str> (entries)();
-        std::string st = bp::extract<std::string> (s)();
-        std::istringstream is (st);
-        boost::archive::text_iarchive ia (is);
-        ia >> cs;
-    }
+  static void setstate(Derived& cs, bp::object entries) {
+    bp::str s = bp::extract<bp::str>(entries)();
+    std::string st = bp::extract<std::string>(s)();
+    std::istringstream is(st);
+    boost::archive::text_iarchive ia(is);
+    ia >> cs;
+  }
 };
 
 template <typename Derived>

include/multicontact-api/scenario/contact-model-planar.hpp

-// Copyright (c) 2015-2018, CNRS
-// Authors: Justin Carpentier <jcarpent@laas.fr>
-
-#ifndef __multicontact_api_scenario_contact_model_planar_hpp__
-#define __multicontact_api_scenario_contact_model_planar_hpp__
-
-#include "multicontact-api/scenario/fwd.hpp"
-#include "multicontact-api/serialization/archive.hpp"
-
-#include <iostream>
-#include <Eigen/Dense>
-
-namespace multicontact_api {
-namespace scenario {
-
-template <typename _Scalar>
-struct ContactModelPlanarTpl : public serialization::Serializable<ContactModelPlanarTpl<_Scalar> > {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  typedef _Scalar Scalar;
-
-  /// \brief Default constructor.
-  ContactModelPlanarTpl() : m_mu(-1.), m_ZMP_radius(-1.) {}
-
-  /// \brief Default constructor.
-  ContactModelPlanarTpl(const Scalar mu, const Scalar ZMP_radius) : m_mu(mu), m_ZMP_radius(ZMP_radius) {}
-
-  /// \brief Copy constructor
-  template <typename S2>
-  explicit ContactModelPlanarTpl(const ContactModelPlanarTpl<S2>& other)
-      : m_mu(other.mu), m_ZMP_radius(other.ZMP_radius) {}
-
-  template <typename S2>
-  bool operator==(const ContactModelPlanarTpl<S2>& other) const {
-    return m_mu == other.m_mu && m_ZMP_radius == other.m_ZMP_radius;
-  }
-
-  template <typename S2>
-  bool operator!=(const ContactModelPlanarTpl<S2>& other) const {
-    return !(*this == other);
-  }
-
-  void disp(std::ostream& os) const {
-    os << "mu: " << m_mu << std::endl << "ZMP radius: " << m_ZMP_radius << std::endl;
-  }
-
-  template <typename S2>
-  friend std::ostream& operator<<(std::ostream& os, const ContactModelPlanarTpl<S2>& cp) {
-    cp.disp(os);
-    return os;
-  }
-
-  /// \brief Friction coefficient.
-  Scalar m_mu;
-  /// \brief ZMP radius.
-  Scalar m_ZMP_radius;
-
- private:
-  // Serialization of the class
-  friend class boost::serialization::access;
-
-  template <class Archive>
-  void save(Archive& ar, const unsigned int /*version*/) const {
-    ar& boost::serialization::make_nvp("mu", m_mu);
-    ar& boost::serialization::make_nvp("ZMP_radius", m_ZMP_radius);
-  }
-
-  template <class Archive>
-  void load(Archive& ar, const unsigned int /*version*/) {
-    ar >> boost::serialization::make_nvp("mu", m_mu);
-    ar >> boost::serialization::make_nvp("ZMP_radius", m_ZMP_radius);
-  }
-
-  BOOST_SERIALIZATION_SPLIT_MEMBER()
-};
-}  // namespace scenario
-}  // namespace multicontact_api
-
-#endif  // ifndef __multicontact_api_scenario_contact_model_planar_hpp__

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

+// Copyright (c) 2015-2018, CNRS
+// Authors: Justin Carpentier <jcarpent@laas.fr>
+
+#ifndef __multicontact_api_scenario_contact_model_planar_hpp__
+#define __multicontact_api_scenario_contact_model_planar_hpp__
+
+#include "multicontact-api/scenario/fwd.hpp"
+#include "multicontact-api/serialization/archive.hpp"
+#include "multicontact-api/serialization/eigen-matrix.hpp"
+#include <iostream>
+#include <Eigen/Dense>
+#include <pinocchio/spatial/skew.hpp>
+
+namespace multicontact_api {
+namespace scenario {
+
+template <typename _Scalar>
+struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Scalar> > {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef _Scalar Scalar;
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
+  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6X;
+
+  /// \brief Default constructor.
+  ContactModelTpl()
+      : m_mu(-1.),
+        m_contact_type(ContactType::CONTACT_UNDEFINED),
+        m_num_contact_points(1),
+        m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
+
+  /// \brief Constructor with friction
+  ContactModelTpl(const Scalar mu)
+      : m_mu(mu),
+        m_contact_type(ContactType::CONTACT_UNDEFINED),
+        m_num_contact_points(1),
+        m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
+
+  /// \brief Full constructor
+  ContactModelTpl(const Scalar mu, const ContactType contact_type)
+      : m_mu(mu),
+        m_contact_type(contact_type),
+        m_num_contact_points(1),
+        m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
+
+  /// \brief Copy constructor
+  template <typename S2>
+  explicit ContactModelTpl(const ContactModelTpl<S2>& other)
+      : m_mu(other.mu),
+        m_contact_type(other.m_contact_type),
+        m_contact_points_positions(other.m_contact_points_positions),
+        m_num_contact_points(other.m_num_contact_points) {}
+
+  template <typename S2>
+  bool operator==(const ContactModelTpl<S2>& other) const {
+    return m_mu == other.m_mu && m_contact_type == other.m_contact_type &&
+           m_num_contact_points == other.m_num_contact_points &&
+           m_contact_points_positions == other.m_contact_points_positions;
+  }
+
+  template <typename S2>
+  bool operator!=(const ContactModelTpl<S2>& other) const {
+    return !(*this == other);
+  }
+
+  void disp(std::ostream& os) const {
+    os << "ContactType: " << m_contact_type << ", mu: " << m_mu << std::endl
+       << "Number of contact points: " << m_num_contact_points << ", positions: " << std::endl
+       << m_contact_points_positions << std::endl;
+  }
+
+  template <typename S2>
+  friend std::ostream& operator<<(std::ostream& os, const ContactModelTpl<S2>& cp) {
+    cp.disp(os);
+    return os;
+  }
+
+  size_t num_contact_points() const { return m_num_contact_points; }
+
+  void num_contact_points(const size_t num) {
+    m_num_contact_points = num;
+    m_contact_points_positions = Matrix3X::Zero(3, num);
+  }
+
+  Matrix3X contact_points_positions() const { return m_contact_points_positions; }
+
+  void contact_points_positions(const Matrix3X& positions) {
+    m_contact_points_positions = positions;
+    m_num_contact_points = positions.cols();
+  }
+
+  /**
+   * @brief generatorMatrix Return a 6x(num_contact_points*3) matrix
+   * containing the generator used to compute contact forces.
+   * @return
+   */
+  Matrix6X generatorMatrix() const {
+    Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
+    for(size_t i=0; i<m_num_contact_points; i++)
+    {
+      gen.block(0, i*3, 3, 3) = Matrix3::Identity();
+      gen.block(3, i*3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
+    }
+    return gen;
+  }
+
+  /// \brief Friction coefficient.
+  Scalar m_mu;
+  /// \brief ZMP radius.
+  ContactType m_contact_type;
+
+ private:
+  /// \brief The number of contact points used to model this contact
+  size_t m_num_contact_points;
+  /// \brief 3xnum_contact_points matrix defining the contact points positions in the frame of the contact placement
+  Matrix3X m_contact_points_positions;
+
+  // Serialization of the class
+  friend class boost::serialization::access;
+
+  template <class Archive>
+  void save(Archive& ar, const unsigned int /*version*/) const {
+    ar& boost::serialization::make_nvp("mu", m_mu);
+    ar& boost::serialization::make_nvp("contact_type", m_contact_type);
+    ar& boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
+    ar& boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
+  }
+
+  template <class Archive>
+  void load(Archive& ar, const unsigned int /*version*/) {
+    ar >> boost::serialization::make_nvp("mu", m_mu);
+    ar >> boost::serialization::make_nvp("contact_type", m_contact_type);
+    ar >> boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
+    ar >> boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
+  }
+
+  BOOST_SERIALIZATION_SPLIT_MEMBER()
+};
+}  // namespace scenario
+}  // namespace multicontact_api
+
+#endif  // ifndef __multicontact_api_scenario_contact_model_planar_hpp__

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

@@ -3,6 +3,7 @@
 #define __multicontact_api_scenario_contact_patch_hpp__
 
 #include "multicontact-api/scenario/fwd.hpp"
+#include "multicontact-api/scenario/contact-model.hpp"
 #include <pinocchio/spatial/se3.hpp>
 #include "multicontact-api/serialization/archive.hpp"
 #include "multicontact-api/serialization/spatial.hpp"
@@ -15,29 +16,31 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   typedef _Scalar Scalar;
+  typedef ContactModelTpl<_Scalar> ContactModel;
   typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
 
   /// \brief Default constructor.
-  ContactPatchTpl() : m_placement(SE3::Identity()), m_mu(-1.) {}
+  ContactPatchTpl() : m_contact_model(), m_placement(SE3::Identity()) {}
 
   /// \brief Init contact patch from a given placement.
-  explicit ContactPatchTpl(const SE3& placement) : m_placement(placement), m_mu(-1.) {}
+  explicit ContactPatchTpl(const SE3& placement) : m_contact_model(), m_placement(placement) {}
 
   /// \brief Init contact patch from a given placement and a friction coefficient
-  ContactPatchTpl(const SE3& placement, const Scalar mu) : m_placement(placement), m_mu(mu) {}
+  ContactPatchTpl(const SE3& placement, const Scalar mu) : m_contact_model(mu), m_placement(placement) {}
 
   /// \brief Copy constructor
-  ContactPatchTpl(const ContactPatchTpl& other) : m_placement(other.m_placement), m_mu(other.m_mu) {}
+  ContactPatchTpl(const ContactPatchTpl& other)
+      : m_contact_model(other.m_contact_model), m_placement(other.m_placement) {}
 
   const SE3& placement() const { return m_placement; }
   SE3& placement() { return m_placement; }
 
-  const Scalar& friction() const { return m_mu; }
-  Scalar& friction() { return m_mu; }
+  const Scalar& friction() const { return m_contact_model.m_mu; }
+  Scalar& friction() { return m_contact_model.m_mu; }
 
   template <typename S2>
   bool operator==(const ContactPatchTpl<S2>& other) const {
-    return m_placement == other.m_placement && m_mu == other.m_mu;
+    return m_placement == other.m_placement && m_contact_model == other.m_contact_model;
   }
 
   template <typename S2>
@@ -46,7 +49,7 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
   }
 
   void disp(std::ostream& os) const {
-    os << "Placement:\n" << m_placement << std::endl << "Friction coefficient : " << m_mu << std::endl;
+    os << "Placement:\n" << m_placement << std::endl << "ContactModel : " << m_contact_model << std::endl;
   }
 
   template <typename S2>
@@ -55,11 +58,12 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
     return os;
   }
 
+  /// \brief Contact model of this contact
+  ContactModel m_contact_model;
+
  protected:
   /// \brief Placement of the contact patch
   SE3 m_placement;
-  /// \brief friction coefficient for this contact
-  Scalar m_mu;
 
  private:
   // Serialization of the class
@@ -68,13 +72,13 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
   template <class Archive>
   void save(Archive& ar, const unsigned int /*version*/) const {
     ar& boost::serialization::make_nvp("placement", m_placement);
-    ar& boost::serialization::make_nvp("mu", m_mu);
+    ar& boost::serialization::make_nvp("contact_model", m_contact_model);
   }
 
   template <class Archive>
   void load(Archive& ar, const unsigned int /*version*/) {
     ar >> boost::serialization::make_nvp("placement", m_placement);
-    ar >> boost::serialization::make_nvp("mu", m_mu);
+    ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
   }
 
   BOOST_SERIALIZATION_SPLIT_MEMBER()  // why is it required ? using only serialize() lead to compilation error,

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

@@ -818,6 +818,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

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/examples.cpp

@@ -143,14 +143,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
 BOOST_AUTO_TEST_CASE(walk_20cm) {
   ContactSequence cs;
   cs.loadFromBinary(path + "walk_20cm.cs");
-  BOOST_CHECK_EQUAL(cs.size(), 15);
+  BOOST_CHECK_EQUAL(cs.size(), 23);
   BOOST_CHECK(cs.haveConsistentContacts());
 }
 
 BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
   ContactSequence cs;
   cs.loadFromBinary(path + "walk_20cm_COM.cs");
-  BOOST_CHECK_EQUAL(cs.size(), 15);
+  BOOST_CHECK_EQUAL(cs.size(), 23);
   BOOST_CHECK(cs.haveConsistentContacts());
   BOOST_CHECK(cs.haveTimings());
   BOOST_CHECK(cs.haveCentroidalValues());
@@ -160,18 +160,18 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {