Merge branch 'v2' into 'master'

V2

See merge request loco-3d/multicontact-api!11

CMakeLists.txt

@@ -15,6 +15,7 @@ INCLUDE(cmake/python.cmake)
 INCLUDE(cmake/ide.cmake)
 INCLUDE(cmake/test.cmake)
 
+SET(CMAKE_CXX_STANDARD 11)
 
 OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
@@ -34,11 +35,12 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
 ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
 ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
+ADD_REQUIRED_DEPENDENCY("curves >= 0.3")
 
 SET(BOOST_COMPONENTS unit_test_framework serialization)
 
 IF(BUILD_PYTHON_INTERFACE)
-  ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.3.1")
+  ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.6.12")
   SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
   FINDPYTHON()
   INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})

README.md

@@ -5,32 +5,261 @@
 
 
 This package is extracted from an original work of Justin Carpentier (jcarpent@laas.fr),
-with the goal to simplify the library and remove old dependencies.
+with the goal to simplify the library, make it more generic and remove old dependencies.
 
-This package installs a python module used to define, store and use ContactSequence objects. 
+This package provide C++ structures with python bindings used to define and store contact phases and contact sequences. 
 
-Basic usage of the ContactSequence object is to initialize either an empty sequence of given size : 
+# Dependencies
 
-``` Python
-cs = ContactSequenceHumanoid(num_contact_phases)
+* Eigen
+* [Pinocchio](https://github.com/stack-of-tasks/pinocchio)
+* [Curves](https://github.com/loco-3d/curves)
+* [Eigenpy](https://github.com/stack-of-tasks/eigenpy) (Only for python bindings)
+
+# Installation procedure
+
+## From binary
+This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots.org/robotpkg-wip.html)
+
+## From sources
+Install the required dependencies, eg. (choose for python version):
+
+```
+sudo apt-get install robotpkg-py35-pinocchio robotpkg-py35-curves
 ```
 
-Or load it from a xml file : 
+Clone the repository and build the package:
 
-``` Python
-cs = ContactSequenceHumanoid(0)
-cs.loadFromXML(filename, "ContactSequence") 
 ```
+git clone --recursive git@gepgitlab.laas.fr:loco-3d/multicontact-api.git
+cd multicontact-api && mkdir build && cd build
+cmake .. && make
+make test
+```
+
+# Usage
+
+## Main classes
+
+This package define 3 main classes representing physical concepts of legged locomotion. Contact Patch, Contact Phase and Contact Sequence.
+
+### Contact Patch
+
+A contact patch define the placement (in SE(3), in the world frame) of a contact as long as the friction coefficient for this contact. 
+
+Future work will also include the type of contact (eg planar, punctual, bilateral ...), a representation of the friction cone or additional information about the contact.
+
+### Contact Phase
+
+A contact phase is defined by a constant set of contact points.
+In the context of bipedal walking, two examples of contact phases are the single and double support phases.
+A contact phase store several data, many of which are optional. 
+
+It store the set of active contacts as a map<String, ContactPatch> with the effector name as Keys:
+
+```python
+cp = ContactPhase()
+p = SE3()
+p.setRandom()
+patchRF = ContactPatch(p,0.5) # create a new contact patch at the placement p with a friction coefficient of 0.5
+cp.addContact("right-feet",patchRF)
+# check if an effector is in contact:
+cp.isEffectorInContact("right-feet")
+# access to the contact patch from the effector name: 
+cp.contactPatch("right-feet")
+``` 
+
+A contact phase can be defined in a specific time interval: 
+
+```python
+cp = ContactPhase()
+cp.timeInitial = 1.
+cp.timeFinal =3.5
+``` 
+
+**Centroidal dynamic data**
+
+Optionnaly, a Contact Phase can store data related to the centroidal dynamic. It store the following initial and final values as public member:
+
+```c
+ /// \brief Initial position of the center of mass for this contact phase
+  point3_t m_c_init;
+  /// \brief Initial velocity of the center of mass for this contact phase
+  point3_t m_dc_init;
+  /// \brief Initial acceleration of the center of mass for this contact phase
+  point3_t m_ddc_init;
+  /// \brief Initial angular momentum for this contact phase
+  point3_t m_L_init;
+  /// \brief Initial angular momentum derivative for this contact phase
+  point3_t m_dL_init;
+  /// \brief Final position of the center of mass for this contact phase
+  point3_t m_c_final;
+  /// \brief Final velocity of the center of mass for this contact phase
+  point3_t m_dc_final;
+  /// \brief Final acceleration of the center of mass for this contact phase
+  point3_t m_ddc_final;
+  /// \brief Final angular momentum for this contact phase
+  point3_t m_L_final;
+  /// \brief Final angular momentum derivative for this contact phase
+  point3_t m_dL_final;
+``` 
+
+It also store centroidal trajectories, represented with objects from the [Curves](https://github.com/loco-3d/curves) library:
+
+
+```c
+/// \brief trajectory for the center of mass position
+  curve_ptr m_c;
+  /// \brief trajectory for the center of mass velocity
+  curve_ptr m_dc;
+  /// \brief trajectory for the center of mass acceleration
+  curve_ptr m_ddc;
+  /// \brief trajectory for the angular momentum
+  curve_ptr m_L;
+  /// \brief trajectory for the angular momentum derivative
+  curve_ptr m_dL;
+  /// \brief trajectory for the centroidal wrench
+  curve_ptr m_wrench;
+  /// \brief trajectory for the zmp
+  curve_ptr m_zmp;
+  /// \brief SE3 trajectory of the root of the robot
+  curve_SE3_ptr m_root;
+``` 
+
+**Wholebody data:**
+
+A Contact Phase can also store data related to the wholebody motion, it store the following initial and final wholebody configuration as public member: 
+
+```c
+  /// \brief Initial whole body configuration of this phase
+  pointX_t m_q_init;
+  /// \brief Final whole body configuration of this phase
+  pointX_t m_q_final;
+``` 
+
+And the following trajectories:
+
+```c
+/// \brief trajectory for the joint positions
+  curve_ptr m_q;
+  /// \brief trajectory for the joint velocities
+  curve_ptr m_dq;
+  /// \brief trajectory for the joint accelerations
+  curve_ptr m_ddq;
+  /// \brief trajectory for the joint torques
+  curve_ptr m_tau;
+``` 
+
+It can also store the contact forces and contact normal forces, in a map<String,curve_ptr> with the effector name as Key:
+
+
+```python
+fR = createRandomPiecewisePolynomial(12)
+cp.addContactForceTrajectory("right-feet",fR)
+# access the trajectory : 
+cp.contactForce("right-feet")
+# contact normal force :
+fnR = createRandomPiecewisePolynomial(1)
+cp.addContactNormalForceTrajectory("right-feet",fnR)
+# access the trajectory : 
+cp.contactNormalForce("right-feet")
+``` 
+
+And the effector trajectory for the swinging limbs, also in a map<String,curve_ptr> with the effector name as Key:
+
+```python
+# create a SE3 trajectory:
+init_pose = SE3.Identity()
+end_pose = SE3.Identity()
+init_pose.translation = array([0.2, -0.7, 0.6])
+end_pose.translation = array([3.6, -2.2, -0.9])
+init_pose.rotation = Quaternion.Identity().matrix()
+end_pose.rotation = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0).normalized().matrix()
+effL = SE3Curve(init_pose, end_pose, cp.timeInitial,cp.timeFinal)
+# add it to the contact phase:
+cp.addEffectorTrajectory("left-feet",effL)
+# access the trajectory : 
+cp.effectorTrajectory("left-feet")
+``` 
+
+### Contact Sequence
+
+As soon as a creation or a rupture of contact point occurs, the contact set is modified, defining a new contact phase.
+The concatenation of contact phases describes what we name a contact sequence, inside which all the contact phases have
+their own duration.
+
+A contact sequence is basically a Vector of Contact Phase, with several helper method which can be used to ease the creation of a Contact Sequence. 
+
+One can either create a Contact sequence with a know number of contact Phase and correctly set the members of the Contact Phases with: 
+
+```c
+ContactSequence cs = ContactSequence(3);
+ContactPhase cp0;
+/* set the contact phase members ... */
+cs.contactPhase(0) = cp0;
+// or:
+cs.contactPhase(1).m_c_init = Point3_t(1,0,0.7);
+cs.contactPhase(1).timeFinal(3.);
+// or : 
+ContactPhase& cp2 = cs.contactPhase(2);
+/* set the contact phase members ... */
+
+``` 
+
+Or simply append new Contact Phase at the end of the current Contact Sequence;
+
+```c
+ContactSequence cs; // create empty contact sequence
+ContactPhase cp0;
+/* set the contact phase members ... */
+cs.append(cp0);
+``` 
+
+**Helper methods to create contact sequence**
+
+Several helper methods have been added to the ContactSequence class to ease the contact creation process:
+
+* `breakContact(eeName, phaseDuration)` Add a new contactPhase at the end of the current ContactSequence, the new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact removed.
+
+* `createContact(eeName, contactPatch, phaseDuration)` Add a new contactPhase at the end of the current ContactSequence, the new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact added.
+
+* `moveEffectorToPlacement(eeName, placement, durationBreak, durationCreate)`  Add two new phases at the end of the current ContactSequence:
+  * it break the contact with eeName
+  * it create the contact with eeName at the given placement.
+
+
+* `moveEffectorOf(eeName, transform, durationBreak, durationCreate)` Similar to moveEffectorToPlacement but use a transform from the previous contact placement instead of a new placement. 
+
+
+
+**Helper methods to check a contact sequence**
+
+The ContactSequence class contains several methods to check if the sequence contains some of the optional data, and if they are consistents across all the contact phases. 
+This methods should be used in order to check if a ContactSequence object given as input to your algorithm have been correctly initialized with all the data that you are going to use in your algorithm. 
+It may also be used to check if your algorithm output consistent data. 
+
+Examples of such methods are `haveConsistentContacts` or `haveCentroidalTrajectories` which check that the (c, dc, ddc, L, dL) have been initialized, have the correct duration, 
+and that each trajectories of one phase correctly end at the same value as it begin in the next phase. 
+
+
+**Helper methods to access Data**
+
+The ContactSequence class also contains methods for easier access to the data contained in the ContactPhase vector. For example, `phaseAtTime` or `phaseIdAtTime` can be used to access a specific ContactPhase at a given time.
+`getAllEffectorsInContact` output all the effector used to create contact during the sequence. 
+
+Finally, methods exists to return the complete trajectory along the contact sequence, concatenating the trajectories of each phases (eg. `concatenateCtrajectories` return the complete c(t) trajectory for all the contact sequence).
+
+
+## Examples
+
+[Examples](examples/README.md) provide several serialized ContactSequence files with descriptions. 
+
+
+
+
+
+
+
 
-This ContactSequence object store a sequence of ContactPhases, defining a set of active contact with their placement. 
-A ContactPhase can also store centroidal data : state trajectory (c,dc,L) and control trajectory (ddc,dL). 
-This trajectories are stored as a set of discretized points, the time corresponding to each point can be found in 'time trajectory'.
 
-For example, to access the position of the center of mass during the first contact phase of a motion :  
 
-``` Python
-phase = cs.contact_phases[0]
-for k in range(len(phase.state_trajectory)):
-    print "c ("+str(phase.time_trajectory[k])+") = "+str(phase.state_trajectory[k][0:3])
-    
-```
\ No newline at end of file

bindings/python/CMakeLists.txt

@@ -11,16 +11,18 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   scenario/contact-phase.cpp
   scenario/contact-sequence.cpp
   scenario/enums.cpp
-  trajectories/trajectories.cpp
   )
 
 ADD_LIBRARY(${PY_NAME} SHARED ${${PROJECT_NAME}_PYTHON_SOURCES} ${${PROJECT_NAME}_PYTHON_HEADERS})
 ADD_SOURCE_GROUP(${PROJECT_NAME}_PYTHON_SOURCES)
 PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
 PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} pinocchio)
+PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} curves)
 TARGET_LINK_LIBRARIES(${PY_NAME} ${Boost_SERIALIZATION_LIBRARIES})
 TARGET_LINK_BOOST_PYTHON(${PY_NAME})
 SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PY_NAME})
+TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-conversion")
+TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-enum-compare")
 
 INSTALL(TARGETS ${PY_NAME} DESTINATION  ${${PY_NAME}_INSTALL_DIR})
 

bindings/python/module.cpp

@@ -6,7 +6,6 @@
 
 #include "multicontact-api/bindings/python/geometry/expose-geometry.hpp"
 #include "multicontact-api/bindings/python/scenario/expose-scenario.hpp"
-#include "multicontact-api/bindings/python/trajectories/expose-trajectories.hpp"
 
 namespace bp = boost::python;
 
@@ -16,5 +15,4 @@ BOOST_PYTHON_MODULE(libmulticontact_api) {
   using namespace multicontact_api::python;
   exposeGeometry();
   exposeScenario();
-  exposeTrajectories();
 }

bindings/python/scenario/contact-phase.cpp

@@ -3,13 +3,23 @@
 
 #include "multicontact-api/bindings/python/scenario/expose-scenario.hpp"
 #include "multicontact-api/bindings/python/scenario/contact-phase.hpp"
-#include "multicontact-api/bindings/python/scenario/contact-phase-humanoid.hpp"
+
+// required because of the serialization of the curves pointer :
+#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>
 
 namespace multicontact_api {
 namespace python {
 void exposeContactPhase() {
-  ContactPhasePythonVisitor<multicontact_api::scenario::ContactPhase4>::expose("ContactPhase4");
-  ContactPhaseHumanoidPythonVisitor<multicontact_api::scenario::ContactPhaseHumanoid>::expose("ContactPhaseHumanoid");
+  ContactPhasePythonVisitor<multicontact_api::scenario::ContactPhase>::expose("ContactPhase");
 }
 }  // namespace python
+
+// namespace python
 }  // namespace multicontact_api

bindings/python/scenario/contact-sequence.cpp

@@ -3,16 +3,21 @@
 //
 #include "multicontact-api/bindings/python/scenario/expose-scenario.hpp"
 #include "multicontact-api/bindings/python/scenario/contact-sequence.hpp"
-#include "multicontact-api/bindings/python/scenario/contact-phase-humanoid.hpp"
-#include "multicontact-api/bindings/python/scenario/ms-interval.hpp"
+
+// required because of the serialization of the curves pointer :
+#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>
 
 namespace multicontact_api {
 namespace python {
 void exposeContactSequence() {
-  //      ContactSequencePythonVisitor<multicontact_api::scenario::ContactSequence4>::expose("ContactSequence4");
-  ContactSequencePythonVisitor<multicontact_api::scenario::ContactSequenceHumanoid>::expose("ContactSequenceHumanoid");
-  MSIntervalPythonVisitor<multicontact_api::scenario::ContactSequenceHumanoid::MSIntervalData>::expose(
-      "MSIntervalData");
+  ContactSequencePythonVisitor<multicontact_api::scenario::ContactSequence>::expose("ContactSequence");
 }
 }  // namespace python
 }  // namespace multicontact_api

bindings/python/scenario/enums.cpp

@@ -3,8 +3,7 @@
 //
 #include "multicontact-api/scenario/fwd.hpp"
 #include "multicontact-api/bindings/python/scenario/expose-scenario.hpp"
-#include "multicontact-api/bindings/python/scenario/enums.hpp"
-
+#include <boost/python.hpp>
 #include <boost/python/enum.hpp>
 
 namespace multicontact_api {
@@ -13,16 +12,6 @@ namespace bp = boost::python;
 
 using namespace multicontact_api::scenario;
 
-void exposeEnumHumanoidPhaseType() {
-  bp::enum_<HumanoidPhaseType>("HumanoidPhaseType")
-      .value("SINGLE_SUPPORT", SINGLE_SUPPORT)
-      .value("DOUBLE_SUPPORT", DOUBLE_SUPPORT)
-      .value("TRIPLE_SUPPORT", TRIPLE_SUPPORT)
-      .value("QUADRUPLE_SUPPORT", QUADRUPLE_SUPPORT)
-      .value("NO_SUPPORT", NO_SUPPORT)
-      .value("HUMANOID_PHASE_UNDEFINED", HUMANOID_PHASE_UNDEFINED);
-}
-
 void exposeEnumConicType() {
   bp::enum_<ConicType>("ConicType")
       .value("CONIC_SOWC", CONIC_SOWC)
@@ -30,9 +19,6 @@ void exposeEnumConicType() {
       .value("CONIC_UNDEFINED", CONIC_UNDEFINED);
 }
 
-void exposeScenarioEnums() {
-  exposeEnumHumanoidPhaseType();
-  exposeEnumConicType();
-}
+void exposeScenarioEnums() { exposeEnumConicType(); }
 }  // namespace python
 }  // namespace multicontact_api

bindings/python/trajectories/trajectories.cpp

-// Copyright (c) 2015-2018, CNRS
-// Authors: Justin Carpentier <jcarpent@laas.fr>
-
-#include "multicontact-api/bindings/python/trajectories/cubic-hermite-spline.hpp"
-
-namespace multicontact_api {
-namespace python {
-void exposeTrajectories() {
-  using namespace trajectories;
-  typedef CubicHermiteSplineTpl<double, 3> CubicHermiteSpline3;
-  typedef CubicHermiteSplineTpl<double, Eigen::Dynamic> CubicHermiteSpline;
-
-  CubicHermiteSplinePythonVisitor<CubicHermiteSpline3>::expose("CubicHermiteSpline3");
-  CubicHermiteSplinePythonVisitor<CubicHermiteSpline>::expose("CubicHermiteSpline");
-  exposeSplineAlgos();
-}
-}  // namespace python
-}  // namespace multicontact_api

cmake @ 61344038

-Subproject commit a3ab52d6525aa6350b657068fddb41665aec4e08
+Subproject commit 61344038b1352d5a8de1e20db710c83be805d2eb

examples/README.md

+This folder contains several serialized ContactSequence objects for different scenarios. They are all made for the Talos humanoid robot, and a simple environment with a flat floor at z=0.
+
+All this file have been generated with the [multicontact-locomotion-planning](https://github.com/loco-3d/multicontact-locomotion-planning) framework.
+
+## Loading the files:
+
+### C++
+
+```c
+#include "multicontact-api/scenario/contact-sequence.hpp"
+
+
+ContactSequence cs;
+cs.loadFromBinary(filename);
+```
+
+### Python
+
+
+```python
+from multicontact_api import ContactSequence
+
+cs = ContactSequence()
+cs.loadFromBinary(filename)
+```
+
+## Suffix notation
+
+For the same scenario, several files may exist with different Suffixes, here is the meaning of this Suffixes:
+
+* No Suffix: only contains the contacts placements, the initial and final CoM position and the initial and final wholeBody configuration. 
+* _COM Suffix: also contains the phases duration and the centroidal trajectories (c, dc, ddc, L, dL)
+* _REF Suffix: also contains the end-effector trajectories for each swing phases
+* _WB Suffix: also contains all the WholeBody data (q, dq, ddq, tau, contact forces). 
+Note that the centroidal and end-effector trajectories contained in this file are not exactly the same as the ones in the _REF file, they are computed from the wholeBody motion.
+
+## Scenarios
+
+### com_motion_above_feet
+
+Contact sequence with only one Contact Phase:
+
+Starting from the reference configuration with both feet in contacts, the CoM is moved above the right feet (in 2 seconds) then above the left feet (in 3 seconds), then go back to the reference position (in 2 seconds).
+
+![com_motion_above_feet motion.](videos/com_motion_above_feet.gif "com_motion_above_feet motion.")
+
+
+### step_in_place_quasistatic
+
+Starting from the reference configuration with both feet in contact with the ground, the Robot do 2 steps in place with each feet (starting with the right one). 
+The Centroidal motion is quasi-static : the CoM only move during the double support phase (duration 2s) and is fixed during the single support phase (duration 1.5s).
+
+![step_in_place_quasistatic motion.](videos/step_in_place_quasistatic.gif "step_in_place_quasistatic motion.")
+
+### step_in_place
+
+Similar to above exept that the motion is not quasi-static and the double support duration is 0.2 seconds and the single support last 1 seconds.
+
+![step_in_place motion.](videos/step_in_place.gif "step_in_place motion.")
+
+### walk_20cm
+
+Walk 1 meter forward with 20cm steps, starting with the right foot. The first and last steps are only 10cm long. Double support duration 0.2seconds, single support duration 1.2seconds.
+
+![walk_20cm motion.](videos/walk_20cm.gif "walk_20cm motion.")
+
+### walk_20cm_quasistatic
+
+Similar to above exept that the motion is quasistatic and the CoM only move during the double support phases. Double support duration 2 seconds, single support duration 2 seconds.
+
+
+![walk_20cm_quasistatic motion.](videos/walk_20cm_quasistatic.gif "walk_20cm_quasistatic motion.")
+