Merge pull request #12 from loco-3d/devel

Devel

CMakeLists.txt

-# Copyright (c) 2015-2018, CNRS
-# Authors: Justin Carpentier <jcarpent@laas.fr>
+# Copyright (c) 2015-2020, CNRS
+# Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel
 
-CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
 
-SET(PROJECT_NAMESPACE loco-3d)
+# Project properties
+SET(PROJECT_ORG loco-3d)
 SET(PROJECT_NAME multicontact-api)
 SET(PROJECT_DESCRIPTION "API to define and store Contact phases and Contact Sequences")
-SET(PROJECT_URL "https://gepgitlab.laas.fr/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
+SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
 
+# Project options
+OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
+OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
+
+# Project configuration
+IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  SET(PROJECT_USE_CMAKE_EXPORT TRUE)
+ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
+SET(CMAKE_CXX_STANDARD 11)
+SET(CXX_DISABLE_WERROR TRUE)
+SET(DOXYGEN_USE_MATHJAX YES)
+
+# JRL-cmakemodule setup
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/eigen.cmake)
 INCLUDE(cmake/python.cmake)
 INCLUDE(cmake/ide.cmake)
-INCLUDE(cmake/test.cmake)
-
-SET(CMAKE_CXX_STANDARD 11)
-
-OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
-OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
-
-IF(INSTALL_PYTHON_INTERFACE_ONLY)
-  SET(INSTALL_DOCUMENTATION OFF CACHE BOOL "documentation is not in python interface")
-  SET(INSTALL_GENERATED_HEADERS OFF CACHE BOOL "headers are not in python interface")
-  SET(INSTALL_PKG_CONFIG_FILE OFF CACHE BOOL ".pc is not in python interface")
-ENDIF(INSTALL_PYTHON_INTERFACE_ONLY)
-
-SET(DOXYGEN_USE_MATHJAX YES)
-SET(CXX_DISABLE_WERROR True)
-SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
 
+# Project definition
 COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
 PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
-ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
-ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
-ADD_REQUIRED_DEPENDENCY("curves >= 0.3")
-
+# Project dependencies
+ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.0.0")
+ADD_PROJECT_DEPENDENCY(curves 0.4.1 REQUIRED PKG_CONFIG_REQUIRES "curves >= 0.4.1")
+IF(NOT CURVES_WITH_PINOCCHIO_SUPPORT)
+  MESSAGE(FATAL_ERROR "you need to use a curves version compiled with pinocchio support")
+ENDIF(NOT CURVES_WITH_PINOCCHIO_SUPPORT)
 SET(BOOST_COMPONENTS unit_test_framework serialization)
 
 IF(BUILD_PYTHON_INTERFACE)
-  ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.6.12")
-  SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
   FINDPYTHON()
+  STRING(REGEX REPLACE "-" "_" PY_NAME ${CUSTOM_HEADER_DIR})
+  ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED PKG_CONFIG_REQUIRES "eigenpy >= 1.6.12")
+  SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
   INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
   STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
   SET(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME})
@@ -51,6 +54,16 @@ ENDIF(BUILD_PYTHON_INTERFACE)
 SEARCH_FOR_BOOST()
 INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
 
+# Main Library
+
+ADD_LIBRARY(${PROJECT_NAME} INTERFACE)
+TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} INTERFACE pinocchio::pinocchio curves::curves)
+
+IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
+ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
 IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
   ADD_SUBDIRECTORY(include/${CUSTOM_HEADER_DIR})
 ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)

bindings/python/CMakeLists.txt

-# Copyright (c) 2015-2018, CNRS
-# Authors: Justin Carpentier <jcarpent@laas.fr>
+# Copyright (c) 2015-2020, CNRS
+# Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel
 
 SET(${PROJECT_NAME}_PYTHON_SOURCES
   module.cpp
@@ -14,16 +14,22 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   )
 
 ADD_LIBRARY(${PY_NAME} SHARED ${${PROJECT_NAME}_PYTHON_SOURCES} ${${PROJECT_NAME}_PYTHON_HEADERS})
+
 ADD_SOURCE_GROUP(${PROJECT_NAME}_PYTHON_SOURCES)
-PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
-PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} pinocchio)
-PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} curves)
-TARGET_LINK_LIBRARIES(${PY_NAME} ${Boost_SERIALIZATION_LIBRARIES})
+
+TARGET_LINK_LIBRARIES(${PY_NAME} ${PROJECT_NAME} ${Boost_SERIALIZATION_LIBRARIES} eigenpy::eigenpy)
 TARGET_LINK_BOOST_PYTHON(${PY_NAME})
-SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PY_NAME})
+
+SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PY_NAME} SOVERSION ${PROJECT_VERSION})
 TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-conversion")
 TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-enum-compare")
 
 INSTALL(TARGETS ${PY_NAME} DESTINATION  ${${PY_NAME}_INSTALL_DIR})
 
-ADD_SUBDIRECTORY(${PY_NAME})
+SET(${PROJECT_NAME}_PYTHON_FILES
+  __init__.py
+  )
+
+FOREACH(file ${${PROJECT_NAME}_PYTHON_FILES})
+  PYTHON_INSTALL_ON_SITE(${PY_NAME} ${file})
+ENDFOREACH(file ${${PROJECT_NAME}_PYTHON_FILES})

bindings/python/multicontact_api/CMakeLists.txt

-SET(${PROJECT_NAME}_PYTHON_FILES
-  __init__.py
-  )
-
-FOREACH(PYTHON_FILE ${${PROJECT_NAME}_PYTHON_FILES})
-  PYTHON_BUILD(. ${PYTHON_FILE})
-  INSTALL(FILES ${PYTHON_FILE} DESTINATION ${${PY_NAME}_INSTALL_DIR})
-ENDFOREACH(PYTHON_FILE ${${PROJECT_NAME}_PYTHON_FILES})

bindings/python/scenario/contact-model.cpp

@@ -2,12 +2,12 @@
 // Authors: Justin Carpentier <jcarpent@laas.fr>
 
 #include "multicontact-api/bindings/python/scenario/expose-scenario.hpp"
-#include "multicontact-api/bindings/python/scenario/contact-model-planar.hpp"
+#include "multicontact-api/bindings/python/scenario/contact-model.hpp"
 
 namespace multicontact_api {
 namespace python {
 void exposeContactModels() {
-  ContactModelPlanarPythonVisitor<multicontact_api::scenario::ContactModelPlanar>::expose("ContactModelPlanar");
+  ContactModelPythonVisitor<multicontact_api::scenario::ContactModel>::expose("ContactModel");
 }
 }  // namespace python
 }  // namespace multicontact_api

bindings/python/scenario/enums.cpp

@@ -19,6 +19,16 @@ void exposeEnumConicType() {
       .value("CONIC_UNDEFINED", CONIC_UNDEFINED);
 }
 
-void exposeScenarioEnums() { exposeEnumConicType(); }
+void exposeEnumContactType() {
+  bp::enum_<ContactType>("ContactType")
+      .value("CONTACT_UNDEFINED", CONTACT_UNDEFINED)
+      .value("CONTACT_PLANAR", CONTACT_PLANAR)
+      .value("CONTACT_POINT", CONTACT_POINT);
+}
+
+void exposeScenarioEnums() {
+  exposeEnumConicType();
+  exposeEnumContactType();
+}
 }  // namespace python
 }  // namespace multicontact_api

cmake @ a5c65a0e

-Subproject commit 65e7f52151c93dd3bf4e0017a44c170d0547d060
+Subproject commit a5c65a0ee51191be3f2e2667b95830706c211bb2

examples/README.md

@@ -24,6 +24,20 @@ 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:

examples/display_gepetto_gui.py

+import curves
+from multicontact_api import ContactSequence
+import gepetto.corbaserver
+import pinocchio as pin
+from rospkg import RosPack
+import time
+import argparse
+import subprocess
+import atexit
+import os
+
+# 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.initDisplay(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)
+
+
+
+
+
+
+
+
+