diff --git a/README.md b/README.md index 02c1bb7..55f783e 100644 --- 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 new file mode 100644 index 0000000..00c4dcc --- /dev/null +++ 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 new file mode 100644 index 0000000..26c633f Binary files /dev/null and b/examples/com_motion_above_feet_COM.cs differ diff --git a/examples/com_motion_above_feet_REF.cs b/examples/com_motion_above_feet_REF.cs new file mode 100644 index 0000000..cdaf690 Binary files /dev/null and b/examples/com_motion_above_feet_REF.cs differ diff --git a/examples/com_motion_above_feet_WB.cs b/examples/com_motion_above_feet_WB.cs new file mode 100644 index 0000000..d58a8fd Binary files /dev/null and b/examples/com_motion_above_feet_WB.cs differ diff --git a/examples/display_gepetto_gui.py b/examples/display_gepetto_gui.py new file mode 100644 index 0000000..cb259e4 --- /dev/null +++ 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 new file mode 100644 index 0000000..3df7776 Binary files /dev/null and b/examples/platforms.cs differ diff --git a/examples/platforms_COM.cs b/examples/platforms_COM.cs new file mode 100644 index 0000000..fb12f7f Binary files /dev/null and b/examples/platforms_COM.cs differ diff --git a/examples/platforms_REF.cs b/examples/platforms_REF.cs new file mode 100644 index 0000000..28859bf Binary files /dev/null and b/examples/platforms_REF.cs differ diff --git a/examples/platforms_WB.cs b/examples/platforms_WB.cs new file mode 100644 index 0000000..2e9ad96 Binary files /dev/null and b/examples/platforms_WB.cs differ diff --git a/examples/previous_versions/api_0.cs b/examples/previous_versions/api_0.cs new file mode 100644 index 0000000..6ec4bd6 Binary files /dev/null and b/examples/previous_versions/api_0.cs differ diff --git a/examples/previous_versions/api_1.cs b/examples/previous_versions/api_1.cs new file mode 100644 index 0000000..7f2e825 Binary files /dev/null and b/examples/previous_versions/api_1.cs differ diff --git a/examples/step_in_place.cs b/examples/step_in_place.cs new file mode 100644 index 0000000..1bf7403 Binary files /dev/null and b/examples/step_in_place.cs differ diff --git a/examples/step_in_place_COM.cs b/examples/step_in_place_COM.cs new file mode 100644 index 0000000..4a0f327 Binary files /dev/null and b/examples/step_in_place_COM.cs differ diff --git a/examples/step_in_place_REF.cs b/examples/step_in_place_REF.cs new file mode 100644 index 0000000..23ce1cf Binary files /dev/null and b/examples/step_in_place_REF.cs differ diff --git a/examples/step_in_place_WB.cs b/examples/step_in_place_WB.cs new file mode 100644 index 0000000..b700d1e Binary files /dev/null and b/examples/step_in_place_WB.cs differ diff --git a/examples/step_in_place_quasistatic.cs b/examples/step_in_place_quasistatic.cs new file mode 100644 index 0000000..ef81f27 Binary files /dev/null and b/examples/step_in_place_quasistatic.cs differ diff --git a/examples/step_in_place_quasistatic_COM.cs b/examples/step_in_place_quasistatic_COM.cs new file mode 100644 index 0000000..6f9ee1d Binary files /dev/null and b/examples/step_in_place_quasistatic_COM.cs differ diff --git a/examples/step_in_place_quasistatic_REF.cs b/examples/step_in_place_quasistatic_REF.cs new file mode 100644 index 0000000..c95646a Binary files /dev/null and b/examples/step_in_place_quasistatic_REF.cs differ diff --git a/examples/step_in_place_quasistatic_WB.cs b/examples/step_in_place_quasistatic_WB.cs new file mode 100644 index 0000000..5339b4f Binary files /dev/null and b/examples/step_in_place_quasistatic_WB.cs differ diff --git a/examples/update_xml.py b/examples/update_xml.py new file mode 100755 index 0000000..1ef79d4 --- /dev/null +++ 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(""): + if prev in DEL_ROWS: + continue + elif line.strip().startswith(""): + 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 new file mode 100644 index 0000000..8abc368 Binary files /dev/null and b/examples/videos/com_motion_above_feet.gif differ diff --git a/examples/videos/step_in_place.gif b/examples/videos/step_in_place.gif new file mode 100644 index 0000000..b97cd00 Binary files /dev/null and b/examples/videos/step_in_place.gif differ diff --git a/examples/videos/step_in_place_quasistatic.gif b/examples/videos/step_in_place_quasistatic.gif new file mode 100644 index 0000000..e097556 Binary files /dev/null and b/examples/videos/step_in_place_quasistatic.gif differ diff --git a/examples/videos/walk_20cm.gif b/examples/videos/walk_20cm.gif new file mode 100644 index 0000000..6458a4b Binary files /dev/null and b/examples/videos/walk_20cm.gif differ diff --git a/examples/videos/walk_20cm_quasistatic.gif b/examples/videos/walk_20cm_quasistatic.gif new file mode 100644 index 0000000..e6ea656 Binary files /dev/null and b/examples/videos/walk_20cm_quasistatic.gif differ diff --git a/examples/walk_20cm.cs b/examples/walk_20cm.cs new file mode 100644 index 0000000..d92b35a Binary files /dev/null and b/examples/walk_20cm.cs differ diff --git a/examples/walk_20cm_COM.cs b/examples/walk_20cm_COM.cs new file mode 100644 index 0000000..2f93fee Binary files /dev/null and b/examples/walk_20cm_COM.cs differ diff --git a/examples/walk_20cm_REF.cs b/examples/walk_20cm_REF.cs new file mode 100644 index 0000000..c654049 Binary files /dev/null and b/examples/walk_20cm_REF.cs differ diff --git a/examples/walk_20cm_WB.cs b/examples/walk_20cm_WB.cs new file mode 100644 index 0000000..4e1817a Binary files /dev/null and b/examples/walk_20cm_WB.cs differ diff --git a/examples/walk_20cm_quasistatic.cs b/examples/walk_20cm_quasistatic.cs new file mode 100644 index 0000000..d92b35a Binary files /dev/null and b/examples/walk_20cm_quasistatic.cs differ diff --git a/examples/walk_20cm_quasistatic_COM.cs b/examples/walk_20cm_quasistatic_COM.cs new file mode 100644 index 0000000..34dc59f Binary files /dev/null and b/examples/walk_20cm_quasistatic_COM.cs differ diff --git a/examples/walk_20cm_quasistatic_REF.cs b/examples/walk_20cm_quasistatic_REF.cs new file mode 100644 index 0000000..af4440f Binary files /dev/null and b/examples/walk_20cm_quasistatic_REF.cs differ diff --git a/examples/walk_20cm_quasistatic_WB.cs b/examples/walk_20cm_quasistatic_WB.cs new file mode 100644 index 0000000..5433484 Binary files /dev/null and b/examples/walk_20cm_quasistatic_WB.cs differ diff --git a/include/multicontact-api/serialization/eigen-matrix.hpp b/include/multicontact-api/serialization/eigen-matrix.hpp index 8d474a5..6504418 100644 --- 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 +#if PINOCCHIO_VERSION_AT_LEAST(2, 6, 0) +#define CURVES_WITH_PINOCCHIO_260 +#endif +#endif + +#ifdef CURVES_WITH_PINOCCHIO_260 +#include +#else + #include #include #include @@ -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 index c17819e..6069116 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -6,6 +6,8 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK) SET(${PROJECT_NAME}_TESTS geometry scenario + examples + serialization_versionning ) FOREACH(TEST ${${PROJECT_NAME}_TESTS}) @@ -13,6 +15,9 @@ 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 new file mode 100644 index 0000000..37698ca --- /dev/null +++ b/unittest/examples.cpp @@ -0,0 +1,262 @@ +// 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 bd86ead..a0d4851 100644 --- a/unittest/python/CMakeLists.txt +++ b/unittest/python/CMakeLists.txt @@ -2,6 +2,7 @@ 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 new file mode 100644 index 0000000..956f4a4 --- /dev/null +++ b/unittest/python/serialization_examples.py @@ -0,0 +1,330 @@ +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 new file mode 100644 index 0000000..fc58353 --- /dev/null +++ b/unittest/serialization_versionning.cpp @@ -0,0 +1,65 @@ +// 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()