Revert "remove examples, ref. loco-3d#26"

This reverts commit da305da.
nim65s · Jan 25, 2022 · a557abe · a557abe
1 parent edcada1
commit a557abe
Show file tree

Hide file tree

Showing 37 changed files with 830 additions and 0 deletions.
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/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/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)