loco-3d · nim65s · Jan 21, 2022 · Jan 21, 2022 · Jan 21, 2022 · Jan 25, 2022
diff --git a/README.md b/README.md
@@ -249,6 +249,7 @@ The ContactSequence class also contains methods for easier access to the data co
 
 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).
 
-## Serialization
 
-All classes have Boost Serialization features. This is intended for data transfert between processes, and not long-term storage.
+## Examples
+
+[Examples](examples/README.md) provide several serialized ContactSequence files with descriptions.
diff --git a/examples/README.md b/examples/README.md
@@ -0,0 +1,87 @@
+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)
+```
+
+## Display the motion in gepetto-gui
+
+A script is provided to load a motion and display it in gepetto-gui, this script require pinocchio (with python bindings) and gepetto-gui. 
+
+```
+python3 display_gepetto_gui.py CS_WB_NAME
+```
+
+Optionally, you can specify an environment to load in the viewer (the default is a flat floor at z=0)
+
+```
+python3 display_gepetto_gui.py CS_WB_NAME --env_name  multicontact/plateforme_surfaces
+```
+
+## 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.")
+
diff --git a/examples/com_motion_above_feet_COM.cs b/examples/com_motion_above_feet_COM.cs
diff --git a/examples/com_motion_above_feet_REF.cs b/examples/com_motion_above_feet_REF.cs
diff --git a/examples/com_motion_above_feet_WB.cs b/examples/com_motion_above_feet_WB.cs
diff --git a/examples/display_gepetto_gui.py b/examples/display_gepetto_gui.py
@@ -0,0 +1,80 @@
+import argparse
+import atexit
+import os
+import subprocess
+import time
+
+import ndcurves
+import gepetto.corbaserver
+from multicontact_api import ContactSequence
+from rospkg import RosPack
+
+import pinocchio as pin
+
+# Define robot model
+robot_package_name = "talos_data"
+urdf_name = "talos_reduced"
+# Define environment
+env_package_name = "hpp_environments"
+env_name = "multicontact/ground"  # default value, may be defined with argument
+scene_name = "world"
+# timestep used to display the configurations
+DT_DISPLAY = 0.04  # 25 fps
+
+
+def display_wb(robot, q_t):
+    t = q_t.min()
+    while t <= q_t.max():
+        t_start = time.time()
+        robot.display(q_t(t))
+        t += DT_DISPLAY
+        elapsed = time.time() - t_start
+        if elapsed > DT_DISPLAY:
+            print("Warning : display not real time ! choose a greater time step for the display.")
+        else:
+            time.sleep(DT_DISPLAY - elapsed)
+    # display last config if the total duration is not a multiple of the dt
+    robot.display(q_t(q_t.max()))
+
+
+if __name__ == '__main__':
+
+    # Get cs_name from the arguments:
+    parser = argparse.ArgumentParser(
+        description="Load a ContactSequence and display the joint-trajectory in gepetto-gui")
+    parser.add_argument('cs_name', type=str, help="The name of the serialized ContactSequence file")
+    parser.add_argument('--env_name', type=str, help="The name of environment urdf file in hpp_environments")
+    args = parser.parse_args()
+    cs_name = args.cs_name
+    if args.env_name:
+        env_name = args.env_name
+
+    # Start the gepetto-gui background process
+    subprocess.run(["killall", "gepetto-gui"])
+    process_viewer = subprocess.Popen("gepetto-gui",
+                                      stdout=subprocess.PIPE,
+                                      stderr=subprocess.DEVNULL,
+                                      preexec_fn=os.setpgrp)
+    atexit.register(process_viewer.kill)
+
+    # Load robot model in pinocchio
+    rp = RosPack()
+    urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf'
+    robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer())
+    robot.initViewer(loadModel=True)
+    robot.displayCollisions(False)
+    robot.displayVisuals(True)
+
+    # Load environment model
+    cl = gepetto.corbaserver.Client()
+    gui = cl.gui
+    env_package_path = rp.get_path(env_package_name)
+    env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf'
+    gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True)
+
+    # Load the motion from the multicontact-api file
+    cs = ContactSequence()
+    cs.loadFromBinary(cs_name)
+    assert cs.haveJointsTrajectories(), "The file loaded do not have joint trajectories stored."
+    q_t = cs.concatenateQtrajectories()
+    display_wb(robot, q_t)
diff --git a/examples/platforms.cs b/examples/platforms.cs
diff --git a/examples/platforms_COM.cs b/examples/platforms_COM.cs
diff --git a/examples/platforms_REF.cs b/examples/platforms_REF.cs
diff --git a/examples/platforms_WB.cs b/examples/platforms_WB.cs
diff --git a/examples/previous_versions/api_0.cs b/examples/previous_versions/api_0.cs
diff --git a/examples/previous_versions/api_1.cs b/examples/previous_versions/api_1.cs
diff --git a/examples/step_in_place.cs b/examples/step_in_place.cs
diff --git a/examples/step_in_place_COM.cs b/examples/step_in_place_COM.cs
diff --git a/examples/step_in_place_REF.cs b/examples/step_in_place_REF.cs
diff --git a/examples/step_in_place_WB.cs b/examples/step_in_place_WB.cs
diff --git a/examples/step_in_place_quasistatic.cs b/examples/step_in_place_quasistatic.cs
diff --git a/examples/step_in_place_quasistatic_COM.cs b/examples/step_in_place_quasistatic_COM.cs
diff --git a/examples/step_in_place_quasistatic_REF.cs b/examples/step_in_place_quasistatic_REF.cs
diff --git a/examples/step_in_place_quasistatic_WB.cs b/examples/step_in_place_quasistatic_WB.cs
diff --git a/examples/update_xml.py b/examples/update_xml.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+"""
+convert objects xml-exported with pinocchio < 2.6.0
+into objects xml-importables with pinocchio >= 2.6.0
+then import that from multicontact-api
+and re-export as binary
+"""
+
+from pathlib import Path
+
+import multicontact_api
+
+DEL_BOTH = [
+    'c_init',
+    'dc_init',
+    'ddc_init',
+    'L_init',
+    'dL_init',
+    'c_final',
+    'dc_final',
+    'ddc_final',
+    'L_final',
+    'dL_final',
+]
+
+DEL_COLS = DEL_BOTH + [
+    'q_init',
+    'q_final',
+]
+
+DEL_ROWS = DEL_BOTH + ['contact_points_positions']
+
+
+def update_xml(f: Path) -> Path:
+    prev = ""
+    updated = f.parent / f"updated_{f.name}"
+    with f.open() as f_in, updated.open("w") as f_out:
+        for line in f_in:
+            if line.strip().startswith("<rows>"):
+                if prev in DEL_ROWS:
+                    continue
+            elif line.strip().startswith("<cols>"):
+                if prev in DEL_COLS:
+                    continue
+            else:
+                if strip := line.strip(" \t\n<>"):
+                    prev = strip.split()[0]
+            print(line, end="", file=f_out)
+    return updated
+
+
+def xml_to_bin(f: Path) -> Path:
+    updated = f.parent / f"{f.stem}.cs"
+    cs = multicontact_api.ContactSequence()
+    cs.loadFromXML(str(f), "nimp")
+    cs.saveAsBinary(str(updated))
+    return updated
+
+
+if __name__ == '__main__':
+    for f in Path().glob("*.xml"):
+        print(f"updtaing {f}...")
+        try:
+            new_xml = update_xml(f)
+            new_bin = xml_to_bin(new_xml)
+            print(f"{f} updated into {new_bin}")
+        except RuntimeError as e:
+            print(f"ERROR on {f}: {e}")
diff --git a/examples/videos/com_motion_above_feet.gif b/examples/videos/com_motion_above_feet.gif
diff --git a/examples/videos/step_in_place.gif b/examples/videos/step_in_place.gif
diff --git a/examples/videos/step_in_place_quasistatic.gif b/examples/videos/step_in_place_quasistatic.gif
diff --git a/examples/videos/walk_20cm.gif b/examples/videos/walk_20cm.gif
diff --git a/examples/videos/walk_20cm_quasistatic.gif b/examples/videos/walk_20cm_quasistatic.gif
diff --git a/examples/walk_20cm.cs b/examples/walk_20cm.cs
diff --git a/examples/walk_20cm_COM.cs b/examples/walk_20cm_COM.cs
diff --git a/examples/walk_20cm_REF.cs b/examples/walk_20cm_REF.cs
diff --git a/examples/walk_20cm_WB.cs b/examples/walk_20cm_WB.cs
diff --git a/examples/walk_20cm_quasistatic.cs b/examples/walk_20cm_quasistatic.cs
diff --git a/examples/walk_20cm_quasistatic_COM.cs b/examples/walk_20cm_quasistatic_COM.cs
diff --git a/examples/walk_20cm_quasistatic_REF.cs b/examples/walk_20cm_quasistatic_REF.cs
diff --git a/examples/walk_20cm_quasistatic_WB.cs b/examples/walk_20cm_quasistatic_WB.cs
diff --git a/include/multicontact-api/serialization/eigen-matrix.hpp b/include/multicontact-api/serialization/eigen-matrix.hpp
@@ -28,6 +28,17 @@
 #ifndef EIGEN_BOOST_SERIALIZATION
 #define EIGEN_BOOST_SERIALIZATION
 
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+#include <pinocchio/config.hpp>
+#if PINOCCHIO_VERSION_AT_LEAST(2, 6, 0)
+#define CURVES_WITH_PINOCCHIO_260
+#endif
+#endif
+
+#ifdef CURVES_WITH_PINOCCHIO_260
+#include <pinocchio/serialization/eigen.hpp>
+#else
+
 #include <Eigen/Dense>
 #include <boost/serialization/split_free.hpp>
 #include <boost/serialization/vector.hpp>
@@ -69,4 +80,8 @@ void serialize(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxR
 }  // namespace serialization
 }  // namespace boost
 
+#endif
+
+#undef CURVES_WITH_PINOCCHIO_260
+
 #endif  // ifndef __multicontact_api_serialization_eigen_matrix_hpp__
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
@@ -6,13 +6,18 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK)
 SET(${PROJECT_NAME}_TESTS
   geometry
   scenario
+  examples
+  serialization_versionning
   )
 
 FOREACH(TEST ${${PROJECT_NAME}_TESTS})
   ADD_UNIT_TEST(${TEST} "${TEST}.cpp")
   TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} Boost::unit_test_framework)
 ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS})
 
+TARGET_COMPILE_DEFINITIONS(examples PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
+TARGET_COMPILE_DEFINITIONS(serialization_versionning PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../examples/")
+
 IF(BUILD_PYTHON_INTERFACE)
   ADD_SUBDIRECTORY(python)
 ENDIF(BUILD_PYTHON_INTERFACE)