[Gym] Generic locomotion envs - PART II

.github/workflows/win.yml

@@ -68,7 +68,7 @@ jobs:
               -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE `
               -DBoost_USE_STATIC_LIBS=OFF -DPYTHON_REQUIRED_VERSION="${{ matrix.python-version }}" `
               -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON `
-              -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC"
+              -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC"
         cmake --build . --config "${Env:BUILD_TYPE}" --parallel 2
 
         if (-not (Test-Path -PathType Container "$RootDir/build/pypi/jiminy_py/src/jiminy_py/core")) {

build_tools/build_install_deps_linux.sh

@@ -68,11 +68,13 @@ git clone -b "v5.0.1" https://github.com/assimp/assimp.git "$RootDir/assimp"
 cd "$RootDir/assimp"
 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_linux/assimp.patch"
 
-### Checkout hpp-fcl
+### Checkout hpp-fcl, and update qhull to the latest release v8.0.2
 git clone -b "v1.5.3" https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl"
 cd "$RootDir/hpp-fcl"
 git submodule --quiet update --init --recursive --jobs 8
 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_linux/hppfcl.patch"
+cd "$RootDir/hpp-fcl/third-parties/qhull"
+git checkout v8.0.2
 
 ### Checkout pinocchio and its submodules
 git clone -b "v2.5.0" https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio"
@@ -151,15 +153,15 @@ cmake "$RootDir/console_bridge" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="
       -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 
-############################## Build and install urdfdom_headers ######################################
+############################### Build and install urdfdom_headers ######################################
 
 mkdir -p "$RootDir/urdfdom_headers/build"
 cd "$RootDir/urdfdom_headers/build"
 cmake "$RootDir/urdfdom_headers" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
       -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 
-################################# Build and install urdfdom ###########################################
+################################## Build and install urdfdom ###########################################
 
 mkdir -p "$RootDir/urdfdom/build"
 cd "$RootDir/urdfdom/build"
@@ -168,29 +170,36 @@ cmake "$RootDir/urdfdom" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$Instal
       -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 
-############################## Build and install assimp ######################################
+###################################### Build and install assimp ########################################
 
 mkdir -p "$RootDir/assimp/build"
 cd "$RootDir/assimp/build"
 cmake "$RootDir/assimp" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
       -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF \
       -DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF \
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-strict-overflow" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-strict-overflow -Wno-class-memaccess" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 
-################################# Build and install hpp-fcl ###########################################
+############################# Build and install qhull and hpp-fcl ######################################
+
+mkdir -p "$RootDir/hpp-fcl/third-parties/qhull/build"
+cd "$RootDir/hpp-fcl/third-parties/qhull/build"
+cmake "$RootDir/hpp-fcl/third-parties/qhull" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
+      -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_C_FLAGS="-fPIC" \
+      -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
+make install -j2
 
 mkdir -p "$RootDir/hpp-fcl/build"
 cd "$RootDir/hpp-fcl/build"
 cmake "$RootDir/hpp-fcl" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-      -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \
+      -DCMAKE_PREFIX_PATH="$InstallDir" -DQhull_PREFIX="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \
       -DPYTHON_STANDARD_LAYOUT=ON -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \
       -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" -DBoost_USE_STATIC_LIBS=OFF \
-      -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=OFF -DINSTALL_DOCUMENTATION=OFF \
+      -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON -DINSTALL_DOCUMENTATION=OFF \
       -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-unused-parameter" -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 
-################################ Build and install Pinocchio ##########################################
+################################# Build and install Pinocchio ##########################################
 
 ### Build and install pinocchio, finally !
 mkdir -p "$RootDir/pinocchio/build"

build_tools/build_install_deps_windows.ps1

@@ -70,9 +70,11 @@ git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/ass
 
 ### Checkout hpp-fcl
 git clone -b "v1.5.3" https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl"
-cd "$RootDir/hpp-fcl"
+Set-Location -Path "$RootDir/hpp-fcl"
 git submodule --quiet update --init --recursive --jobs 8
 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/hppfcl.patch"
+Set-Location -Path "$RootDir/hpp-fcl/third-parties/qhull"
+git checkout v8.0.2
 
 ### Checkout pinocchio and its submodules, then apply some patches (generated using `git diff --submodule=diff`)
 git clone -b "v2.5.0" https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio"
@@ -116,7 +118,7 @@ Set-Location -Path "$RootDir/eigen3/build"
 cmake "$RootDir/eigen3" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
       -DBUILD_TESTING=OFF -DEIGEN_BUILD_PKGCONFIG=OFF `
-      -DCMAKE_CXX_FLAGS="/bigobj"
+      -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
 ################################### Build and install eigenpy ##########################################
@@ -132,7 +134,7 @@ cmake "$RootDir/eigenpy" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_
       -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" `
       -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF `
       -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj $(
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus $(
 )     -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
@@ -148,7 +150,7 @@ if (-not (Test-Path -PathType Container "$RootDir/tinyxml/build")) {
 Set-Location -Path "$RootDir/tinyxml/build"
 cmake "$RootDir/tinyxml" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -DTIXML_USE_STL"
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -DTIXML_USE_STL"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
 ############################## Build and install console_bridge ########################################
@@ -160,10 +162,10 @@ if (-not (Test-Path -PathType Container "$RootDir/console_bridge/build")) {
 Set-Location -Path "$RootDir/console_bridge/build"
 cmake "$RootDir/console_bridge" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj"
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
-############################## Build and install urdfdom_headers ######################################
+############################### Build and install urdfdom_headers ######################################
 
 ###
 if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers/build")) {
@@ -172,10 +174,10 @@ if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers/build")) {
 Set-Location -Path "$RootDir/urdfdom_headers/build"
 cmake -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
-      -DCMAKE_CXX_FLAGS="/EHsc /bigobj" "$RootDir/urdfdom_headers"
+      -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" "$RootDir/urdfdom_headers"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
-################################# Build and install urdfdom ###########################################
+################################## Build and install urdfdom ###########################################
 
 ###
 if (-not (Test-Path -PathType Container "$RootDir/urdfdom/build")) {
@@ -185,10 +187,10 @@ Set-Location -Path "$RootDir/urdfdom/build"
 cmake "$RootDir/urdfdom" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
       -DBUILD_TESTING=OFF `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES -DURDFDOM_STATIC"
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES -DURDFDOM_STATIC"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
-############################## Build and install assimp ######################################
+###################################### Build and install assimp ########################################
 
 ###
 if (-not (Test-Path -PathType Container "$RootDir/assimp/build")) {
@@ -200,10 +202,20 @@ cmake -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
       -DCMAKE_CXX_FLAGS="/EHsc /bigobj" "$RootDir/assimp" `
       -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF `
       -DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF -DASSIMP_INSTALL_PDB=OFF `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES"
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
-################################# Build and install hpp-fcl ###########################################
+############################# Build and install qhull and hpp-fcl ######################################
+
+### Build qhull
+if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl/third-parties/qhull/build")) {
+  New-Item -ItemType "directory" -Force -Path "$RootDir/hpp-fcl/third-parties/qhull/build"
+}
+Set-Location -Path "$RootDir/hpp-fcl/third-parties/qhull/build"
+cmake "$RootDir/hpp-fcl/third-parties/qhull" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
+      -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" `
+      -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" -DCMAKE_C_FLAGS="/EHsc /bigobj"
+cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 
 ### Build hpp-fcl
 if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl/build")) {
@@ -215,7 +227,7 @@ cmake "$RootDir/hpp-fcl" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_
       -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" `
       -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" `
       -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF `
-      -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=OFF -DINSTALL_DOCUMENTATION=OFF `
+      -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON -DINSTALL_DOCUMENTATION=OFF `
       -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4267 /permissive- /Zc:__cplusplus $(
 )     -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DHPP_FCL_STATIC"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
@@ -244,7 +256,7 @@ cmake "$RootDir/pinocchio" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATO
       -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF `
       -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF `
       -DBUILD_WITH_URDF_SUPPORT=ON -DBUILD_PYTHON_INTERFACE=ON `
-      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4715 /wd4834 /permissive- $(
+      -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4715 /wd4834 /permissive- /Zc:__cplusplus $(
 )     -D_USE_MATH_DEFINES -DNOMINMAX -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC"
 cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2
 

build_tools/easy_install_deps_ubuntu.sh

@@ -60,17 +60,12 @@ if ! [-d "/opt/openrobots/lib/${PYTHON_BIN}/site-packages/" ] ; then
     curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | apt-key add - && \
     apt update
 
-    if [ $DISTRIB_RELEASE == "18.04" ] ; then
-        apt install -y --allow-downgrades robotpkg-urdfdom=0.3.0r2 robotpkg-urdfdom-headers=0.3.0 robotpkg-hpp-fcl=1.4.2 \
-                                          robotpkg-gepetto-viewer=4.4.0 robotpkg-py36-qt4-gepetto-viewer-corba=5.1.2 robotpkg-py36-omniorbpy \
-                                          robotpkg-py36-eigenpy=2.3.0 robotpkg-py36-hpp-fcl=1.4.2 \
-                                          robotpkg-pinocchio=2.3.1 robotpkg-py36-pinocchio=2.3.1
-    else
-        apt install -y --allow-downgrades robotpkg-urdfdom=1.0.3 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.4.5 \
-                                          robotpkg-py38-qt5-gepetto-viewer=4.9.0r3 robotpkg-py38-qt5-gepetto-viewer-corba=5.4.0r2 robotpkg-py38-omniorbpy=4.2.4 \
-                                          robotpkg-py38-eigenpy=2.5.0 robotpkg-py38-hpp-fcl=1.4.5 \
-                                          robotpkg-pinocchio=2.4.7 robotpkg-py38-pinocchio=2.4.7
-    fi
+    # apt-get must be used instead of apt to support wildcard in package name on Ubuntu 20
+    apt-get install -y --allow-downgrades --allow-unauthenticated \
+        robotpkg-urdfdom=1.0.3 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.4.5 \
+        robotpkg-py3*-qt5-gepetto-viewer=4.9.0r3 robotpkg-py3*-qt5-gepetto-viewer-corba=5.4.0r2 robotpkg-py3*-omniorbpy=4.2.4 \
+        robotpkg-py3*-eigenpy=2.5.0 robotpkg-py3*-hpp-fcl=1.4.5 \
+        robotpkg-pinocchio=2.4.7 robotpkg-py3*-pinocchio=2.4.7
 
     sudo -H -u $(id -nu $SUDO_UID) bash -c " \
     echo 'export LD_LIBRARY_PATH=\"/opt/openrobots/lib:\${LD_LIBRARY_PATH}\"' >> \$HOME/.bashrc && \

build_tools/patch_deps_linux/hppfcl.patch

@@ -33,6 +33,15 @@ index d675c2c..c503cf1 100644
 
  option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE)
  if(HPP_FCL_HAS_QHULL)
+@@ -115,7 +108,7 @@ if(HPP_FCL_HAS_QHULL)
+     PATHS ${Qhull_PREFIX}
+     )
+   find_library(Qhull_r_LIBRARY
+-    NAMES libqhull_r.so
++    NAMES libqhullstatic_r.a
+     PATHS ${Qhull_PREFIX}
+     )
+ endif()
 @@ -172,12 +165,10 @@ SET(${PROJECT_NAME}_HEADERS
    include/hpp/fcl/internal/traversal.h
    )

build_tools/patch_deps_windows/hppfcl.patch

@@ -33,6 +33,15 @@ index d675c2c..c503cf1 100644
 
  option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE)
  if(HPP_FCL_HAS_QHULL)
+@@ -115,7 +108,7 @@ if(HPP_FCL_HAS_QHULL)
+     PATHS ${Qhull_PREFIX}
+     )
+   find_library(Qhull_r_LIBRARY
+-    NAMES libqhull_r.so
++    NAMES qhullstatic_r.lib
+     PATHS ${Qhull_PREFIX}
+     )
+ endif()
 @@ -172,12 +165,10 @@ SET(${PROJECT_NAME}_HEADERS
    include/hpp/fcl/internal/traversal.h
    )

core/CMakeLists.txt

@@ -24,7 +24,7 @@ if(NOT LEGACY_MODE)
 
     find_package(Eigen3 3.3.0 REQUIRED NO_MODULE)  # It adds the target Eigen3::Eigen
 
-    find_package(pinocchio 2.4.0 NO_MODULE NO_CMAKE_SYSTEM_PATH)  # It adds target pinocchio::pinocchio. Pinocchio was header-only before 2.4.0.
+    find_package(pinocchio 2.4.4 NO_MODULE NO_CMAKE_SYSTEM_PATH)  # Pinocchio >= 2.4.4 is required to support convex mesh collision and display
     if(pinocchio_FOUND)
         message("-- Found pinocchio: version ${pinocchio_VERSION}")
     else()
@@ -105,7 +105,7 @@ if(pinocchio_FOUND)
 elseif(hpp-fcl_FOUND)
     target_link_libraries_system(${PROJECT_NAME} hpp-fcl::hpp-fcl)
 elseif(LEGACY_MODE)
-    target_link_libraries(${PROJECT_NAME} hpp-fcl)
+    target_link_libraries(${PROJECT_NAME} pinocchio hpp-fcl)  # It is necessary to link to Pinocchio shared lib since 2.4.0
 endif()
 if(NOT LEGACY_MODE)
     if (NOT "${urdfdom_LIBRARIES}" MATCHES ".*tinyxml.*")

core/include/jiminy/core/Utilities.h

@@ -100,7 +100,7 @@ namespace jiminy
     {
         return std::static_pointer_cast<That>(shared_from_base(that));
     }
-    
+
     // ======================== is_vector ===========================
 
     template<typename T>
@@ -282,6 +282,13 @@ namespace jiminy
     hresult_t getJointTypeVelocitySuffixes(joint_t                  const & jointTypeIn,
                                            std::vector<std::string>       & jointTypeSuffixesOut);
 
+    hresult_t getBodyIdx(pinocchio::Model const & model,
+                         std::string      const & bodyName,
+                         int32_t                & bodyIdx);
+    hresult_t getBodiesIdx(pinocchio::Model         const & model,
+                           std::vector<std::string> const & bodiesNames,
+                           std::vector<int32_t>           & bodiesIdx);
+
     hresult_t getFrameIdx(pinocchio::Model const & model,
                           std::string      const & frameName,
                           int32_t                & frameIdx);

core/include/jiminy/core/engine/EngineMultiRobot.h

@@ -625,13 +625,27 @@ namespace jiminy
                                              vectorN_t          const & v,
                                              vectorN_t          const & a);
 
+        /// \brief Compute the force resulting from ground contact on a given body.
+        ///
+        /// \param[in] system              System for which to perform computation.
+        /// \param[in] collisionPairIdx    Id of the collision pair associated with the body
+        /// \return Contact force, at parent frame, in the global frame.
+        pinocchio::Force computeContactDynamicsAtBody(systemDataHolder_t const & system,
+                                                      int32_t            const & collisionPairIdx) const;
+
         /// \brief Compute the force resulting from ground contact on a given frame.
         ///
         /// \param[in] system      System for which to perform computation.
-        /// \param[in] header      Id of the frame in contact.
+        /// \param[in] frameIdx    Id of the frame in contact.
         /// \return Contact force, in the global frame.
+        pinocchio::Force computeContactDynamicsAtFrame(systemDataHolder_t const & system,
+                                                       int32_t            const & frameIdx) const;
+
+        /// \brief Compute the force resulting from ground contact for a given normal direction and depth.
         pinocchio::Force computeContactDynamics(systemDataHolder_t const & system,
-                                                int32_t            const & frameIdx) const;
+                                                vector3_t          const & nGround,
+                                                float64_t          const & depth,
+                                                vector3_t          const & vFrameInWorld) const;
 
         void computeCommand(systemDataHolder_t                & system,
                             float64_t                   const & t,