[Gym] Generic locomotion envs - PART IX

build_tools/build_install_deps_linux.sh

@@ -69,7 +69,7 @@ cd "$RootDir/assimp"
 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_linux/assimp.patch"
 
 ### 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"
+git clone -b "v1.5.4" 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"

build_tools/build_install_deps_windows.ps1

@@ -69,7 +69,7 @@ dos2unix "$RootDir/build_tools/patch_deps_windows/assimp.patch"  # Fix encoding,
 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/assimp.patch"
 
 ### Checkout hpp-fcl
-git clone -b "v1.5.3" https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl"
+git clone -b "v1.5.4" https://github.com/humanoid-path-planner/hpp-fcl.git "$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"

build_tools/easy_install_deps_ubuntu.sh

@@ -62,10 +62,10 @@ if ! [-d "/opt/openrobots/lib/${PYTHON_BIN}/site-packages/" ] ; then
 
     # 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
+        robotpkg-urdfdom=1.0.3 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.5.4 \
+        robotpkg-py3*-qt5-gepetto-viewer=4.10.1r1 robotpkg-py3*-qt5-gepetto-viewer-corba=5.5.1 robotpkg-py3*-omniorbpy=4.2.4 \
+        robotpkg-py3*-eigenpy=2.5.0 robotpkg-py3*-hpp-fcl=1.5.4 \
+        robotpkg-pinocchio=2.5.0 robotpkg-py3*-pinocchio=2.5.0
 
     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

@@ -56,31 +56,31 @@ index d675c2c..c503cf1 100644
  pkg_config_append_libs("hpp-fcl")
  IF(HPP_FCL_HAVE_OCTOMAP)
 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
-index dfef812..fee68b9 100644
+index 043d18b..dcc79be 100644
 --- a/src/CMakeLists.txt
 +++ b/src/CMakeLists.txt
-@@ -146,7 +146,6 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
+@@ -148,7 +148,6 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
  ENDFOREACH()
  LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/config.hh)
  add_library(${LIBRARY_NAME}
 -  SHARED
    ${PROJECT_HEADERS_FULL_PATH}
    ${${LIBRARY_NAME}_SOURCES}
    )
-@@ -155,16 +154,19 @@ add_library(${LIBRARY_NAME}
+@@ -157,6 +156,9 @@ add_library(${LIBRARY_NAME}
  ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
  ADD_HEADER_GROUP(PROJECT_HEADERS_FULL_PATH)
- 
+
 +find_package(zlib NO_MODULE)
 +find_package(IrrXML NO_MODULE)
 +find_package(assimp NO_MODULE)
+ TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME}
+   SYSTEM PUBLIC
+   ${Boost_INCLUDE_DIRS}
+@@ -164,9 +166,14 @@ TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME}
+
  TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
-   PUBLIC
--  Boost::boost
--  )
--
--TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
--  PRIVATE
+   PRIVATE
 -  ${assimp_LIBRARIES}
 -  # assimp::assimp # Not working
 +  Boost::boost assimp::assimp IrrXML::IrrXML zlib::zlib

build_tools/patch_deps_windows/hppfcl.patch

@@ -56,31 +56,31 @@ index d675c2c..c503cf1 100644
  pkg_config_append_libs("hpp-fcl")
  IF(HPP_FCL_HAVE_OCTOMAP)
 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
-index dfef812..fee68b9 100644
+index 043d18b..dcc79be 100644
 --- a/src/CMakeLists.txt
 +++ b/src/CMakeLists.txt
-@@ -146,7 +146,6 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
+@@ -148,7 +148,6 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
  ENDFOREACH()
  LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/config.hh)
  add_library(${LIBRARY_NAME}
 -  SHARED
    ${PROJECT_HEADERS_FULL_PATH}
    ${${LIBRARY_NAME}_SOURCES}
    )
-@@ -155,16 +154,19 @@ add_library(${LIBRARY_NAME}
+@@ -157,6 +156,9 @@ add_library(${LIBRARY_NAME}
  ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
  ADD_HEADER_GROUP(PROJECT_HEADERS_FULL_PATH)
 
 +find_package(zlib NO_MODULE)
 +find_package(IrrXML NO_MODULE)
 +find_package(assimp NO_MODULE)
+ TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME}
+   SYSTEM PUBLIC
+   ${Boost_INCLUDE_DIRS}
+@@ -164,9 +166,14 @@ TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME}
+
  TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
-   PUBLIC
--  Boost::boost
--  )
--
--TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
--  PRIVATE
+   PRIVATE
 -  ${assimp_LIBRARIES}
 -  # assimp::assimp # Not working
 +  Boost::boost assimp::assimp IrrXML::IrrXML zlib::zlib

core/CMakeLists.txt

@@ -28,7 +28,7 @@ if(NOT LEGACY_MODE)
     if(pinocchio_FOUND)
         message("-- Found pinocchio: version ${pinocchio_VERSION}")
     else()
-        find_package(hpp-fcl 1.4.0 REQUIRED NO_MODULE NO_CMAKE_SYSTEM_PATH)  # Old pinocchio releases do not export hpp-fcl dependency
+        find_package(hpp-fcl 1.5.4 REQUIRED NO_MODULE NO_CMAKE_SYSTEM_PATH)  # hpp-fcl >= 1.5.4 adds support of collision of primitives with halfspace
     endif()
 endif()
 

core/src/engine/EngineMultiRobot.cc

@@ -1843,8 +1843,8 @@ namespace jiminy
                                                     vectorN_t          const & a)
     {
         pinocchio::forwardKinematics(system.robot->pncModel_, system.robot->pncData_, q, v, a);
-        pinocchio::updateFramePlacements(system.robot->pncModel_, system.robot->pncData_);
         pinocchio::centerOfMass(system.robot->pncModel_, system.robot->pncData_);
+        pinocchio::updateFramePlacements(system.robot->pncModel_, system.robot->pncData_);
         pinocchio::updateGeometryPlacements(system.robot->pncModel_,
                                             system.robot->pncData_,
                                             system.robot->pncGeometryModel_,

gym_jiminy/gym_jiminy/common/env_bases.py

@@ -359,19 +359,56 @@ def _refresh_action_space(self) -> None:
             high=effort_limit[self.robot.motors_velocity_idx],
             dtype=np.float64)
 
+    def _neutral(self) -> np.ndarray:
+        """
+        @brief Returns a neutral valid configuration for the robot.
+
+        @details The default implementation returns the neutral configuration
+                 if valid, the "mean" configuration otherwise (right in the
+                 middle of the position lower and upper bounds).
+
+                 Beware there is no guarantee for this configuration to be
+                 statically stable.
+
+        @remark This method is called internally by '_sample_state' to
+                generate the initial state. It can be overloaded to ensure
+                static stability of the configuration.
+        """
+        qpos = neutral(self.robot.pinocchio_model)
+        if np.any(self.robot.position_limit_upper < qpos) or \
+                np.any(qpos < self.robot.position_limit_lower):
+            mask = np.isfinite(self.robot.position_limit_upper)
+            qpos[mask] = 0.5 * (self.robot.position_limit_upper[mask] +
+                self.robot.position_limit_lower[mask])
+        return qpos
+
     def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]:
         """
-        @brief Returns a random valid configuration and velocity for the robot.
+        @brief Returns a valid configuration and velocity for the robot.
 
         @details The default implementation returns the neutral configuration
                  and zero velocity.
 
+                 Offsets are applied on the freeflyer to ensure no contact
+                 points are going through the ground and up to three are in
+                 contact.
+
         @remark This method is called internally by 'reset' to generate the
                 initial state. It can be overloaded to act as a random state
                 generator.
         """
-        qpos = neutral(self.robot.pinocchio_model)
+        # Get the neutral configuration
+        qpos = self._neutral()
+
+        # Make sure the robot touches the ground
+        if self.robot.has_freeflyer:
+            ground_fun = self.engine_py.get_options()['world']['groundProfile']
+            compute_freeflyer_state_from_fixed_body(
+                self.robot, qpos, ground_profile=ground_fun)
+
+        # Zero velocity
         qvel = np.zeros(self.robot.nv)
+
         return qpos, qvel
 
     def _update_obs(self, obs: SpaceDictRecursive) -> None:
@@ -474,17 +511,7 @@ def seed(self, seed: Optional[int] = None) -> List[int]:
     def set_state(self, qpos: np.ndarray, qvel: np.ndarray) -> None:
         """
         @brief Reset the simulation and specify the initial state of the robot.
-
-        @details Offsets are applied on the freeflyer to ensure no contact
-                 points are going through the ground and up to three are in
-                 contact.
         """
-        # Make sure the robot touches the ground
-        if self.robot.has_freeflyer:
-            ground_fun = self.engine_py.get_options()['world']['groundProfile']
-            compute_freeflyer_state_from_fixed_body(
-                self.robot, qpos, ground_profile=ground_fun)
-
         # Reset the simulator and set the initial state
         self.engine_py.reset(np.concatenate((qpos, qvel)))
 

python/jiminy_py/src/jiminy_py/dynamics.py

@@ -1,13 +1,18 @@
 ## @file jiminy_py/dynamics.py
+import logging
 import numpy as np
 from typing import Optional, Tuple, Callable, Dict, Any
 
+import hppfcl
 import pinocchio as pin
 from pinocchio.rpy import rpyToMatrix, matrixToRpy
 
 from . import core as jiminy
 
 
+logger = logging.getLogger(__name__)
+
+
 ######################################################################
 ########################## Generic math ##############################
 ######################################################################
@@ -105,16 +110,22 @@ def update_quantities(robot: jiminy.Robot,
                     pnc_model, pnc_data, position, velocity, acceleration)
             pin.framesForwardKinematics(pnc_model, pnc_data, position)
 
-        pin.computeCollisions(robot.collision_model, robot.collision_data,
-            stop_at_first_collision=False)
-        pin.computeDistances(robot.collision_model, robot.collision_data)
-
         if update_energy:
             if velocity is not None:
                 pin.kineticEnergy(
                     pnc_model, pnc_data, position, velocity, False)
             pin.potentialEnergy(pnc_model, pnc_data, position, False)
 
+    pin.updateGeometryPlacements(pnc_model, pnc_data,
+        robot.collision_model, robot.collision_data)
+    pin.computeCollisions(robot.collision_model, robot.collision_data,
+        stop_at_first_collision=False)
+    pin.computeDistances(robot.collision_model, robot.collision_data)
+    for dist_req in robot.collision_data.distanceResults:
+        if np.linalg.norm(dist_req.normal) < 1e-6:
+            pin.computeDistances(robot.collision_model, robot.collision_data)
+            break
+
 def get_body_index_and_fixedness(
         robot: jiminy.Robot,
         body_name: str,
@@ -297,10 +308,12 @@ def compute_transform_contact(robot: jiminy.Robot,
 
     # Compute the contact plane normal
     if len(contact_frames_pos_rel) > 2:
-        contact_edge_ref = contact_frames_pos_rel[0] - contact_frames_pos_rel[1]
+        contact_edge_ref = \
+            contact_frames_pos_rel[0] - contact_frames_pos_rel[1]
         contact_edge_ref /= np.linalg.norm(contact_edge_ref)
         for i in range(2, len(contact_frames_pos_rel)):
-            contact_edge_alt = contact_frames_pos_rel[0] - contact_frames_pos_rel[i]
+            contact_edge_alt = \
+                contact_frames_pos_rel[0] - contact_frames_pos_rel[i]
             contact_edge_alt /= np.linalg.norm(contact_edge_alt)
             normal_offset = np.cross(contact_edge_ref, contact_edge_alt)
             if np.linalg.norm(normal_offset) > 0.2:  # At least 11 degrees of angle
@@ -327,10 +340,39 @@ def compute_transform_contact(robot: jiminy.Robot,
     # Take into account the collision bodies
     # TODO: Take into account the ground profile
     min_distance = np.inf
-    for dist_req in robot.collision_data.distanceResults:
-        min_distance = min(min_distance, dist_req.min_distance)
-    transform_offset.translation[2] = max(
-        transform_offset.translation[2], -min_distance)
+    deepest_idx = -1
+    for i, dist_req in enumerate(robot.collision_data.distanceResults):
+        if np.linalg.norm(dist_req.normal) > 1e-6:
+            body_idx = robot.collision_model.collisionPairs[0].first
+            body_geom = robot.collision_model.geometryObjects[body_idx]
+            if dist_req.normal[2] > 0.0 and \
+                    isinstance(body_geom.geometry, hppfcl.Box):
+                ground_idx = robot.collision_model.collisionPairs[0].second
+                ground_geom = robot.collision_model.geometryObjects[ground_idx]
+                box_size = 2.0 * ground_geom.geometry.halfSide
+                body_size = 2.0 * body_geom.geometry.halfSide
+                distance = - body_size[2] - box_size[2] - dist_req.min_distance
+            else:
+                distance = dist_req.min_distance
+            if distance < min_distance:
+                min_distance = distance
+                deepest_idx = i
+        else:
+            logger.warning("Collision computation failed for some reason. "
+                           "Skipping this collision pair.")
+    if not contact_frames_pos_rel or \
+            transform_offset.translation[2] < -min_distance:
+        transform_offset.translation[2] = -min_distance
+        if not contact_frames_pos_rel:
+            geom_idx = robot.collision_model.collisionPairs[deepest_idx].first
+            geom = robot.collision_model.geometryObjects[geom_idx]
+            if isinstance(geom.geometry, hppfcl.Box):
+                dist_rslt = robot.collision_data.distanceResults[deepest_idx]
+                collision_position = dist_rslt.getNearestPoint1()
+                transform_offset.rotation = \
+                    robot.collision_data.oMg[geom_idx].rotation.T
+                transform_offset.translation[2] += (collision_position -
+                    transform_offset.rotation @ collision_position)[2]
 
     return transform_offset
 
@@ -416,6 +458,9 @@ def compute_freeflyer_state_from_fixed_body(
             base_link_acceleration = - ff_a_fixedBody
             acceleration[:6] = base_link_acceleration.vector
 
+    update_quantities(robot, position, velocity, acceleration,
+        update_physics=False, use_theoretical_model=False)
+
     return fixed_body_name
 
 def compute_efforts_from_fixed_body(

python/jiminy_py/src/jiminy_py/engine.py

@@ -209,7 +209,10 @@ def reset(self,
         # Restore the initial internal pinocchio data
         update_quantities(self.robot,
                           x0[:self.robot.nq],
+                          x0[self.robot.nq:],
                           update_physics=True,
+                          update_com=True,
+                          update_energy=True,
                           use_theoretical_model=False)
 
     def step(self,