diff --git a/examples/README.md b/examples/README.md deleted file mode 100644 index 00c4dcc..0000000 --- a/examples/README.md +++ /dev/null @@ -1,87 +0,0 @@ -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 deleted file mode 100644 index 177f67b..0000000 Binary files a/examples/com_motion_above_feet_COM.cs and /dev/null differ diff --git a/examples/com_motion_above_feet_REF.cs b/examples/com_motion_above_feet_REF.cs deleted file mode 100644 index cdaf690..0000000 Binary files a/examples/com_motion_above_feet_REF.cs and /dev/null differ diff --git a/examples/com_motion_above_feet_WB.cs b/examples/com_motion_above_feet_WB.cs deleted file mode 100644 index b856bc9..0000000 Binary files a/examples/com_motion_above_feet_WB.cs and /dev/null differ diff --git a/examples/display_gepetto_gui.py b/examples/display_gepetto_gui.py deleted file mode 100644 index cb259e4..0000000 --- a/examples/display_gepetto_gui.py +++ /dev/null @@ -1,80 +0,0 @@ -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 deleted file mode 100644 index ceafa3d..0000000 Binary files a/examples/platforms.cs and /dev/null differ diff --git a/examples/platforms_COM.cs b/examples/platforms_COM.cs deleted file mode 100644 index c1fe8ca..0000000 Binary files a/examples/platforms_COM.cs and /dev/null differ diff --git a/examples/platforms_REF.cs b/examples/platforms_REF.cs deleted file mode 100644 index 28859bf..0000000 Binary files a/examples/platforms_REF.cs and /dev/null differ diff --git a/examples/platforms_WB.cs b/examples/platforms_WB.cs deleted file mode 100644 index 2e9ad96..0000000 Binary files a/examples/platforms_WB.cs and /dev/null differ diff --git a/examples/previous_versions/api_0.cs b/examples/previous_versions/api_0.cs deleted file mode 100644 index 6ec4bd6..0000000 Binary files a/examples/previous_versions/api_0.cs and /dev/null differ diff --git a/examples/previous_versions/api_1.cs b/examples/previous_versions/api_1.cs deleted file mode 100644 index 7f2e825..0000000 Binary files a/examples/previous_versions/api_1.cs and /dev/null differ diff --git a/examples/step_in_place.cs b/examples/step_in_place.cs deleted file mode 100644 index b4bc810..0000000 Binary files a/examples/step_in_place.cs and /dev/null differ diff --git a/examples/step_in_place_COM.cs b/examples/step_in_place_COM.cs deleted file mode 100644 index 5a08ab8..0000000 Binary files a/examples/step_in_place_COM.cs and /dev/null differ diff --git a/examples/step_in_place_REF.cs b/examples/step_in_place_REF.cs deleted file mode 100644 index 23ce1cf..0000000 Binary files a/examples/step_in_place_REF.cs and /dev/null differ diff --git a/examples/step_in_place_WB.cs b/examples/step_in_place_WB.cs deleted file mode 100644 index b700d1e..0000000 Binary files a/examples/step_in_place_WB.cs and /dev/null differ diff --git a/examples/step_in_place_quasistatic.cs b/examples/step_in_place_quasistatic.cs deleted file mode 100644 index 8f462e6..0000000 Binary files a/examples/step_in_place_quasistatic.cs and /dev/null differ diff --git a/examples/step_in_place_quasistatic_COM.cs b/examples/step_in_place_quasistatic_COM.cs deleted file mode 100644 index 99d2d1b..0000000 Binary files a/examples/step_in_place_quasistatic_COM.cs and /dev/null differ diff --git a/examples/step_in_place_quasistatic_REF.cs b/examples/step_in_place_quasistatic_REF.cs deleted file mode 100644 index c95646a..0000000 Binary files a/examples/step_in_place_quasistatic_REF.cs and /dev/null differ diff --git a/examples/step_in_place_quasistatic_WB.cs b/examples/step_in_place_quasistatic_WB.cs deleted file mode 100644 index 5339b4f..0000000 Binary files a/examples/step_in_place_quasistatic_WB.cs and /dev/null differ diff --git a/examples/videos/com_motion_above_feet.gif b/examples/videos/com_motion_above_feet.gif deleted file mode 100644 index 8abc368..0000000 Binary files a/examples/videos/com_motion_above_feet.gif and /dev/null differ diff --git a/examples/videos/step_in_place.gif b/examples/videos/step_in_place.gif deleted file mode 100644 index b97cd00..0000000 Binary files a/examples/videos/step_in_place.gif and /dev/null differ diff --git a/examples/videos/step_in_place_quasistatic.gif b/examples/videos/step_in_place_quasistatic.gif deleted file mode 100644 index e097556..0000000 Binary files a/examples/videos/step_in_place_quasistatic.gif and /dev/null differ diff --git a/examples/videos/walk_20cm.gif b/examples/videos/walk_20cm.gif deleted file mode 100644 index 6458a4b..0000000 Binary files a/examples/videos/walk_20cm.gif and /dev/null differ diff --git a/examples/videos/walk_20cm_quasistatic.gif b/examples/videos/walk_20cm_quasistatic.gif deleted file mode 100644 index e6ea656..0000000 Binary files a/examples/videos/walk_20cm_quasistatic.gif and /dev/null differ diff --git a/examples/walk_20cm.cs b/examples/walk_20cm.cs deleted file mode 100644 index 4398c23..0000000 Binary files a/examples/walk_20cm.cs and /dev/null differ diff --git a/examples/walk_20cm_COM.cs b/examples/walk_20cm_COM.cs deleted file mode 100644 index ade170f..0000000 Binary files a/examples/walk_20cm_COM.cs and /dev/null differ diff --git a/examples/walk_20cm_REF.cs b/examples/walk_20cm_REF.cs deleted file mode 100644 index c654049..0000000 Binary files a/examples/walk_20cm_REF.cs and /dev/null differ diff --git a/examples/walk_20cm_WB.cs b/examples/walk_20cm_WB.cs deleted file mode 100644 index 4e1817a..0000000 Binary files a/examples/walk_20cm_WB.cs and /dev/null differ diff --git a/examples/walk_20cm_quasistatic.cs b/examples/walk_20cm_quasistatic.cs deleted file mode 100644 index 4398c23..0000000 Binary files a/examples/walk_20cm_quasistatic.cs and /dev/null differ diff --git a/examples/walk_20cm_quasistatic_COM.cs b/examples/walk_20cm_quasistatic_COM.cs deleted file mode 100644 index 34dc59f..0000000 Binary files a/examples/walk_20cm_quasistatic_COM.cs and /dev/null differ diff --git a/examples/walk_20cm_quasistatic_REF.cs b/examples/walk_20cm_quasistatic_REF.cs deleted file mode 100644 index af4440f..0000000 Binary files a/examples/walk_20cm_quasistatic_REF.cs and /dev/null differ diff --git a/examples/walk_20cm_quasistatic_WB.cs b/examples/walk_20cm_quasistatic_WB.cs deleted file mode 100644 index 5433484..0000000 Binary files a/examples/walk_20cm_quasistatic_WB.cs and /dev/null differ diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 6069116..c17819e 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -6,8 +6,6 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK) SET(${PROJECT_NAME}_TESTS geometry scenario - examples - serialization_versionning ) FOREACH(TEST ${${PROJECT_NAME}_TESTS}) @@ -15,9 +13,6 @@ FOREACH(TEST ${${PROJECT_NAME}_TESTS}) 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) diff --git a/unittest/examples.cpp b/unittest/examples.cpp deleted file mode 100644 index 37698ca..0000000 --- a/unittest/examples.cpp +++ /dev/null @@ -1,262 +0,0 @@ -// Copyright (c) 2019-2020, CNRS -// Authors: Pierre Fernbach , - -#include - -#define BOOST_TEST_MODULE StatsTests -#include -#include - -#include "multicontact-api/scenario/contact-sequence.hpp" -#include "multicontact-api/scenario/fwd.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * This unit test try to deserialize the ContactSequences in the examples folder - * and check if they have the given data set. - * If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with - * serialized objects The objects need to be re-generated. - */ - -using namespace multicontact_api::scenario; - -const std::string path = TEST_DATA_PATH; -BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) - -BOOST_AUTO_TEST_CASE(com_motion_above_feet_COM) { - ContactSequence cs; - cs.loadFromBinary(path + "com_motion_above_feet_COM.cs"); - BOOST_CHECK_EQUAL(cs.size(), 1); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); -} - -BOOST_AUTO_TEST_CASE(com_motion_above_feet_WB) { - ContactSequence cs; - cs.loadFromBinary(path + "com_motion_above_feet_WB.cs"); - BOOST_CHECK_EQUAL(cs.size(), 1); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); -} - -BOOST_AUTO_TEST_CASE(step_in_place) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_COM) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_COM.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_REF) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_REF.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false)); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_WB) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_WB.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_quasistatic) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_quasistatic.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_quasistatic_COM.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_quasistatic_REF.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) { - ContactSequence cs; - cs.loadFromBinary(path + "step_in_place_quasistatic_WB.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_COM) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_COM.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_REF) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_REF.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_WB) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_WB.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_quasistatic.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_quasistatic_COM.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(!cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_quasistatic_REF.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) { - ContactSequence cs; - cs.loadFromBinary(path + "walk_20cm_quasistatic_WB.cs"); - BOOST_CHECK_EQUAL(cs.size(), 23); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt index a0d4851..bd86ead 100644 --- a/unittest/python/CMakeLists.txt +++ b/unittest/python/CMakeLists.txt @@ -2,7 +2,6 @@ SET(${PROJECT_NAME}_PYTHON_TESTS trivial geometry scenario - #serialization_examples ) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) diff --git a/unittest/python/serialization_examples.py b/unittest/python/serialization_examples.py deleted file mode 100644 index 956f4a4..0000000 --- a/unittest/python/serialization_examples.py +++ /dev/null @@ -1,330 +0,0 @@ -import pathlib -import unittest - -import ndcurves # noqa: requiered to get C++ type exposition - -import pinocchio as pin -from multicontact_api import ContactSequence - -pin.switchToNumpyArray() - -PATH = (pathlib.Path(__file__).parent.parent.parent / 'examples').absolute() -print("PATH : ", PATH) - - -def assertTrajNotNone(testCase, phase, root, wholeBody): - testCase.assertIsNotNone(phase.c_t) - testCase.assertIsNotNone(phase.dc_t) - testCase.assertIsNotNone(phase.ddc_t) - testCase.assertIsNotNone(phase.L_t) - testCase.assertIsNotNone(phase.dL_t) - if root: - testCase.assertIsNotNone(phase.root_t) - if wholeBody: - testCase.assertIsNotNone(phase.q_t) - testCase.assertIsNotNone(phase.dq_t) - testCase.assertIsNotNone(phase.ddq_t) - testCase.assertIsNotNone(phase.tau_t) - - -def testTrajMinMax(testCase, phase, root, wholeBody): - testCase.assertTrue(phase.c_t.min() >= 0.) - testCase.assertTrue(phase.dc_t.min() >= 0.) - testCase.assertTrue(phase.ddc_t.min() >= 0.) - testCase.assertTrue(phase.L_t.min() >= 0.) - testCase.assertTrue(phase.dL_t.min() >= 0.) - testCase.assertTrue(phase.c_t.max() >= 0.) - testCase.assertTrue(phase.dc_t.max() >= 0.) - testCase.assertTrue(phase.ddc_t.max() >= 0.) - testCase.assertTrue(phase.L_t.max() >= 0.) - testCase.assertTrue(phase.dL_t.max() >= 0.) - if root: - testCase.assertTrue(phase.root_t.min() >= 0.) - testCase.assertTrue(phase.root_t.max() >= 0.) - if wholeBody: - testCase.assertTrue(phase.q_t.max() >= 0.) - testCase.assertTrue(phase.dq_t.max() >= 0.) - testCase.assertTrue(phase.ddq_t.max() >= 0.) - testCase.assertTrue(phase.tau_t.max() >= 0.) - testCase.assertTrue(phase.q_t.min() >= 0.) - testCase.assertTrue(phase.dq_t.min() >= 0.) - testCase.assertTrue(phase.ddq_t.min() >= 0.) - testCase.assertTrue(phase.tau_t.min() >= 0.) - - -def testCallTraj(testCase, phase, root, quasistatic, wholeBody): - testCase.assertTrue(phase.c_t((phase.c_t.max() + phase.c_t.min()) / 2.).any()) - if not quasistatic: - testCase.assertTrue(phase.dc_t((phase.dc_t.max() + phase.dc_t.min()) / 2.).any()) - testCase.assertTrue(phase.ddc_t((phase.ddc_t.max() + phase.ddc_t.min()) / 2.).any()) - # testCase.assertTrue(phase.L_t((phase.L_t.max() + phase.L_t.min()) / 2.).any()) - # testCase.assertTrue(phase.dL_t((phase.dL_t.max() + phase.dL_t.min()) / 2.).any()) - if root: - testCase.assertTrue(phase.root_t.max() >= 0.) - if wholeBody: - testCase.assertTrue(phase.q_t((phase.q_t.max() + phase.q_t.min()) / 2.).any()) - testCase.assertTrue(phase.dq_t((phase.dq_t.max() + phase.dq_t.min()) / 2.).any()) - testCase.assertTrue(phase.ddq_t((phase.ddq_t.max() + phase.ddq_t.min()) / 2.).any()) - testCase.assertTrue(phase.tau_t((phase.tau_t.max() + phase.tau_t.min()) / 2.).any()) - - -def testEffectorTraj(testCase, phase): - for eeName, traj in phase.effectorTrajectories().items(): - testCase.assertIsNotNone(traj) - testCase.assertTrue(traj.min() >= 0.) - testCase.assertTrue(traj.max() >= 0.) - testCase.assertTrue(traj((traj.min() + traj.max()) / 2.).any()) - - -def testContactForce(testCase, phase): - for eeName, traj in phase.contactForces().items(): - testCase.assertIsNotNone(traj) - testCase.assertTrue(traj.min() >= 0.) - testCase.assertTrue(traj.max() >= 0.) - testCase.assertTrue(traj((traj.min() + traj.max()) / 2.).any()) - - -def checkPhase(testCase, phase, root=False, quasistatic=False, effector=False, wholeBody=False): - assertTrajNotNone(testCase, phase, root, wholeBody) - testTrajMinMax(testCase, phase, root, wholeBody) - testCallTraj(testCase, phase, root, quasistatic, wholeBody) - if effector: - testEffectorTraj(testCase, phase) - if wholeBody: - testContactForce(testCase, phase) - - -def checkCS(testCase, cs, root=False, quasistatic=False, effector=False, wholeBody=False): - for phase in cs.contactPhases: - checkPhase(testCase, phase, root, quasistatic, effector, wholeBody) - - -class ExamplesSerialization(unittest.TestCase): - def test_com_motion_above_feet_COM(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs")) - self.assertEqual(cs.size(), 1) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - checkCS(self, cs) - - def test_com_motion_above_feet_WB(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs")) - self.assertEqual(cs.size(), 1) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveJointsTrajectories()) - self.assertTrue(cs.haveJointsDerivativesTrajectories()) - self.assertTrue(cs.haveContactForcesTrajectories()) - self.assertTrue(cs.haveZMPtrajectories()) - checkCS(self, cs, wholeBody=True) - - def test_step_in_place(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - - def test_step_in_place_COM(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_COM.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - checkCS(self, cs, effector=False, wholeBody=False) - - def test_step_in_place_REF(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_REF.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False)) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, root=True, effector=True, wholeBody=False) - - def test_step_in_place_WB(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_WB.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) - self.assertTrue(cs.haveJointsTrajectories()) - self.assertTrue(cs.haveJointsDerivativesTrajectories()) - self.assertTrue(cs.haveContactForcesTrajectories()) - self.assertTrue(cs.haveZMPtrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, effector=True, wholeBody=True) - - def test_step_in_place_quasistatic(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_quasistatic.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - - def test_step_in_place_quasistatic_COM(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_COM.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False) - - def test_step_in_place_quasistatic_REF(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_REF.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False) - - def test_step_in_place_quasistatic_WB(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_WB.cs")) - self.assertEqual(cs.size(), 9) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) - self.assertTrue(cs.haveJointsTrajectories()) - self.assertTrue(cs.haveJointsDerivativesTrajectories()) - self.assertTrue(cs.haveContactForcesTrajectories()) - self.assertTrue(cs.haveZMPtrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True) - - def test_walk_20cm(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm.cs")) - self.assertEqual(cs.size(), 23) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - self.assertTrue(cs.haveConsistentContacts()) - - def test_walk_20cm_COM(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_COM.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - checkCS(self, cs, effector=False, wholeBody=False) - - def test_walk_20cm_REF(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_REF.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, root=True, effector=True, wholeBody=False) - - def test_walk_20cm_WB(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_WB.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) - self.assertTrue(cs.haveJointsTrajectories()) - self.assertTrue(cs.haveJointsDerivativesTrajectories()) - self.assertTrue(cs.haveContactForcesTrajectories()) - self.assertTrue(cs.haveZMPtrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, effector=True, wholeBody=True) - - def test_walk_20cm_quasistatic(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs")) - self.assertEqual(cs.size(), 23) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - self.assertTrue(cs.haveConsistentContacts()) - - def test_walk_20cm_quasistatic_COM(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertFalse(cs.haveFriction()) - self.assertFalse(cs.haveContactModelDefined()) - checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False) - - def test_walk_20cm_quasistatic_REF(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_REF.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False) - - def test_walk_20cm_quasistatic_WB(self): - cs = ContactSequence() - cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs")) - self.assertEqual(cs.size(), 23) - self.assertTrue(cs.haveConsistentContacts()) - self.assertTrue(cs.haveTimings()) - self.assertTrue(cs.haveCentroidalValues()) - self.assertTrue(cs.haveCentroidalTrajectories()) - self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) - self.assertTrue(cs.haveJointsTrajectories()) - self.assertTrue(cs.haveJointsDerivativesTrajectories()) - self.assertTrue(cs.haveContactForcesTrajectories()) - self.assertTrue(cs.haveZMPtrajectories()) - self.assertTrue(cs.haveFriction()) - self.assertTrue(cs.haveContactModelDefined()) - checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True) - - -if __name__ == '__main__': - unittest.main() diff --git a/unittest/serialization_versionning.cpp b/unittest/serialization_versionning.cpp deleted file mode 100644 index fc58353..0000000 --- a/unittest/serialization_versionning.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright (c) 2019-2020, CNRS -// Authors: Pierre Fernbach , - -#include - -#define BOOST_TEST_MODULE StatsTests -#include -#include - -#include "multicontact-api/scenario/contact-sequence.hpp" -#include "multicontact-api/scenario/fwd.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * This unit test try to deserialize the ContactSequences in the examples/previous_versions folder - * And check that they are compatible with the current version - */ - -using namespace multicontact_api::scenario; - -const std::string path = TEST_DATA_PATH; -BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) - -BOOST_AUTO_TEST_CASE(api_0) { - ContactSequence cs; - cs.loadFromBinary(path + "previous_versions/api_0.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(!cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_CASE(api_1) { - ContactSequence cs; - cs.loadFromBinary(path + "previous_versions/api_1.cs"); - BOOST_CHECK_EQUAL(cs.size(), 9); - BOOST_CHECK(cs.haveConsistentContacts()); - BOOST_CHECK(cs.haveTimings()); - BOOST_CHECK(cs.haveCentroidalValues()); - BOOST_CHECK(cs.haveCentroidalTrajectories()); - BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1)); - BOOST_CHECK(cs.haveJointsTrajectories()); - BOOST_CHECK(cs.haveJointsDerivativesTrajectories()); - BOOST_CHECK(cs.haveContactForcesTrajectories()); - BOOST_CHECK(cs.haveZMPtrajectories()); - BOOST_CHECK(cs.haveFriction()); - BOOST_CHECK(cs.haveContactModelDefined()); -} - -BOOST_AUTO_TEST_SUITE_END()