Jiminy 1.7.8 (#500)

* [core] Add random sensor jitter. * [core] Fix potential mismatch between collision pairs and constraints. * [core] Improve contact constraint solver. * [core] Remove dedicated tolerance for PGS solver. * [core] Initialize stepper with smallest timestep. * [core] Enable config baumgarte freq for user-specified constraints. * [core] Simplify compute extra terms. * [core] Fix engine not reset at start if not done before. * [core] Avoid discontinuities at init by starting with contact enabled and adding transitionEps for joint bounds. * [python/viewer] Enable dynamic update of available/default backend. * [python/viewer] Add synchronous panda3d backend for offscreen only subprocess rendering. * [python/viewer] Fix support of Intel GPU for rendering and on-demand Nvidia with panda3d. * [python/viewer] Default to sync panda3d backend if onscreen impossible. * [python/viewer] Disable shader in headless mode with panda3d to avoid segfault. * [python/viewer] Speed-up x2 replay video recording using panda3d backend. * [python/viewer] Use physics-based shader on Intel GPU with Panda3d. * [python/viewer] Officially support opening viewer without robot. * [gym/common] Automatically render ground profile. * [gym/common] Fix init sensor data proxy and send llc command. * [gym/common] Use fastest modern rng 'SFC64'. * [misc] Update ANYmal robot model. * [misc] Add support of Gym<0.24.0. * [misc] Fix support of ray==1.10.0 Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
duburcqa · Apr 6, 2022 · 6a85af1 · 6a85af1
1 parent a1e571b
commit 6a85af1
Show file tree

Hide file tree

Showing 123 changed files with 5,315 additions and 7,720 deletions.
diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml
@@ -10,19 +10,14 @@ on:
 jobs:
   build-test-and-publish-pypi-manylinux:
     name: >-
-      (${{ matrix.container }}) (${{ matrix.PYTHON_VERSION }}) (${{ matrix.legacy }})
+      (${{ matrix.container }}) (${{ matrix.PYTHON_VERSION }})
       Build and run the unit tests. Then generate and publish the wheels on PyPi.
 
     strategy:
       matrix:
         container: ['quay.io/pypa/manylinux2014_x86_64',
                     'quay.io/pypa/manylinux_2_24_x86_64']
         PYTHON_VERSION: ['cp36', 'cp37', 'cp38', 'cp39']
-        legacy: [false]
-        include:
-          - container: 'quay.io/pypa/manylinux_2_24_x86_64'
-            PYTHON_VERSION: 'cp38'
-            legacy: true
 
     runs-on: ubuntu-20.04
     container: ${{ matrix.container }}
@@ -58,11 +53,7 @@ jobs:
 
         "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip
         "${PYTHON_EXECUTABLE}" -m pip install --upgrade twine wheel auditwheel cmake
-        if [ "${{ matrix.legacy }}" == true ] ; then
-          "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.20"  # for tensorflow compat.
-        else
-          "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22"  # for numba compat.
-        fi
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22"  # for numba compat.
     - name: Build project dependencies
       run: |
         ./build_tools/build_install_deps_unix.sh
@@ -117,7 +108,7 @@ jobs:
       if: success() && github.repository == 'duburcqa/jiminy'
       uses: actions/upload-artifact@v1
       with:
-        name: jiminy_py-${{ matrix.PYTHON_VERSION }}-wheel-${{ matrix.legacy }}
+        name: jiminy_py-${{ matrix.PYTHON_VERSION }}-wheel
         path: build/wheelhouse
 
     #####################################################################################
@@ -134,7 +125,7 @@ jobs:
 
     - name: Publish on PyPi the wheel for Linux of Jiminy_py
       if: >-
-        success() && matrix.legacy == false &&
+        success() &&
         github.repository == 'duburcqa/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master'
       uses: pypa/gh-action-pypi-publish@master
       with:
@@ -143,7 +134,7 @@ jobs:
         packages_dir: build/wheelhouse
     - name: Publish on PyPi the wheel of Gym Jiminy (Any platform / Any python3 version)
       if: >-
-        success() && matrix.container == 'quay.io/pypa/manylinux2014_x86_64' && matrix.PYTHON_VERSION == 'cp36' && matrix.legacy == false &&
+        success() && matrix.container == 'quay.io/pypa/manylinux2014_x86_64' && matrix.PYTHON_VERSION == 'cp36' &&
         github.repository == 'duburcqa/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master'
       uses: pypa/gh-action-pypi-publish@master
       with:

diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml
@@ -52,7 +52,9 @@ jobs:
         sudo env "PATH=$PATH" "${GITHUB_WORKSPACE}/build_tools/easy_install_deps_ubuntu.sh"
         "${PYTHON_EXECUTABLE}" -m pip install tensorflow
         "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22"  # for numba compat.
-        "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html
+        "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" --trusted-host pypi.org \
+        --trusted-host pytorch.org --trusted-host download.pytorch.org --trusted-host files.pypi.org \
+        --trusted-host files.pytorch.org -f https://download.pytorch.org/whl/torch_stable.html
         "${PYTHON_EXECUTABLE}" -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0"
 
     #####################################################################################

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.7.7)
+set(BUILD_VERSION 1.7.8)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

diff --git a/INSTALL.md b/INSTALL.md
@@ -84,7 +84,7 @@ make install -j2
 
 ```
 sudo apt install -y gnupg curl wget build-essential cmake doxygen graphviz
-python -m pip install "numpy<1.20"
+python -m pip install "numpy>=1.16,<1.22"
 ```
 
 ### Jiminy dependencies build and install
@@ -185,7 +185,7 @@ You have to preinstall by yourself the (free) MSVC 2019 toolchain.
 Then, install `numpy` and `wheel`.
 
 ```
-python -m pip install wheel "numpy<1.20"
+python -m pip install wheel "numpy>=1.16,<1.22"
 ```
 
 ### Jiminy dependencies build and install

diff --git a/README.md b/README.md
@@ -13,7 +13,7 @@ Jiminy is built around [Pinocchio](https://github.com/stack-of-tasks/pinocchio),
 
 - **build an efficient and flexible platform for machine learning in robotics.**
 
-Beside a strong focus on performance to answer machine learning's need for running computationally demanding distributed simulations, Jiminy offers convenience tools for learning via a dedicated module [Gym-Jiminy](#gym-jiminy). It is fully compliant with `gym` standard API and provides an highly customizable wrapper to interface any robotics system with state-of-the-art learning frameworks.
+Beside a strong focus on performance to answer machine learning's need for running computationally demanding distributed simulations, Jiminy offers convenience tools for learning via a dedicated module [Gym-Jiminy](#gym-jiminy). It is fully compliant with `gym` standard API and provides a highly customizable wrapper to interface any robotics system with state-of-the-art learning frameworks.
 
 ## Key features
 
@@ -22,12 +22,11 @@ Beside a strong focus on performance to answer machine learning's need for runni
 - Simulation of multi-body systems using minimal coordinates and Lagrangian dynamics.
 - Comprehensive API for computing dynamic quantities and their derivatives, exposing and extending Pinocchio.
 - C++ core with full python bindings, providing frontend API parity between both languages.
-- Designed with machine learning in mind, with seemless wrapping of robots as [OpenAI Gym](https://github.com/openai/gym) environments using one-liners. Jiminy provides both the physical engine and the robot model (including sensors) required for learning.
-- Easy to install: `pip` is all that is needed to [get you started](#getting-started) !
-- Dedicated integration in Google Colab, Jupyter Lab, and VSCode working out-of-the-box - including interactive 3D viewer based on [Meshcat](https://github.com/rdeits/MeshCat.jl). This facilitates working on remote headless environnement such as machine learning clusters.
-- Cross-platform offscreen rendering capability, without requiring X-server, based on [Panda3d](https://github.com/panda3d/panda3d).
+- Designed with machine learning in mind, with seamless wrapping of robots as [OpenAI Gym](https://github.com/openai/gym) environments using one-liners. Jiminy provides both the physical engine and the robot model (including sensors) required for learning.
 - Rich simulation log output, easily customizable for recording, introspection and debugging. The simulation log is made available in RAM directly for fast access, and can be exported in raw binary, CSV or [HDF5](https://portal.hdfgroup.org/display/HDF5/Introduction+to+HDF5) format.
-- Support Linux, Mac and Windows platforms.
+- Dedicated integration in Google Colab, Jupyter Lab, and VSCode working out-of-the-box - including interactive 3D viewer based on [Meshcat](https://github.com/rdeits/MeshCat.jl). This facilitates working on remote environments.
+- Cross-platform offscreen rendering capability, without requiring X-server, based on [Panda3d](https://github.com/panda3d/panda3d).
+- Easy to install: `pip` is all that is needed to [get you started](#getting-started) ! Support Linux, Mac and Windows platforms.
 
 ### Physics
 
@@ -37,7 +36,7 @@ Beside a strong focus on performance to answer machine learning's need for runni
 - Support of compliant joints with force-based spring-damper dynamics, to model joint elasticity, a common phenomenon particularly in legged robotics.
 - Simulate both continuous or discrete-time controller, with possibly different controller and sensor update frequencies.
 
-A more complete list of features, development status, and changelog are available on the [wiki](https://github.com/duburcqa/jiminy/wiki).
+A more complete list of features is available on the [wiki](https://github.com/duburcqa/jiminy/wiki).
 
 **The documentation is available on [Github.io](https://duburcqa.github.io/jiminy/), or locally in `docs/html/index.html` if built from source.**
 
@@ -46,7 +45,7 @@ A more complete list of features, development status, and changelog are availabl
 Gym Jiminy is an interface between Jiminy simulator and reinforcement learning frameworks. It is fully compliant with now standard [Open AI Gym](https://github.com/openai/gym) API. Additionally, it offers a generic and easily configurable learning environment for learning locomotion tasks, with minimal intervention from the user, who usually only needs to provide the robot's [URDF](https://wiki.ros.org/urdf) file. Furthermore, Gym Jiminy enables easy modification of many aspects of the simulation to provide richer exploration and ensure robust learning. This ranges from external perturbation forces to sensor noise and bias, including randomization of masses and inertias, ground friction model or even gravity itself. Note that learning can
 easily be done on any high-level dynamics features, or restricted to mock sensor data for end-to-end learning.
 
-Gym is cross-platform and compatible with most Reinforcement Learning frameworks implementing standard algorithms. For instance, [Stable Baselines 3](https://github.com/DLR-RM/stable-baselines3), [RL Coach](https://github.com/NervanaSystems/coach), [Tianshou](https://github.com/thu-ml/tianshou), or [Rllib](https://github.com/ray-project/ray). RL Coach leverages the open-source Machine Learning framework [Tensorflow](https://github.com/tensorflow/tensorflow) as backend, Stable Baselines 3 and  Tianshou use its counterpart [Pytorch](https://pytorch.org/), and Rllib supports both. A few learning examples relying on those packages are also provided.
+Gym is cross-platform and compatible with most Reinforcement Learning frameworks implementing standard algorithms. For instance, [Stable Baselines 3](https://github.com/DLR-RM/stable-baselines3), [Tianshou](https://github.com/thu-ml/tianshou), or [Rllib](https://github.com/ray-project/ray). Stable Baselines 3 and Tianshou use its counterpart [Pytorch](https://pytorch.org/), and Rllib supports both. A few learning examples relying on those packages are also provided.
 
 Pre-configured environments for some well-known toys models and reference robotics platforms are provided: [cartpole](https://gym.openai.com/envs/CartPole-v1/), [acrobot](https://gym.openai.com/envs/Acrobot-v1/), [pendulum](https://gym.openai.com/envs/Pendulum-v0/), [Ant](https://gym.openai.com/envs/Ant-v2/), [ANYmal](https://www.anymal-research.org/#getting-started), and [Cassie](https://www.agilityrobotics.com/robots#cassie), and [Atlas](https://www.bostondynamics.com/atlas).
 

diff --git a/build_tools/build_install_deps_unix.sh b/build_tools/build_install_deps_unix.sh
@@ -130,6 +130,7 @@ git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_unix/assimp
 ### Checkout hpp-fcl
 if [ ! -d "$RootDir/hpp-fcl" ]; then
   git clone https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl"
+  git config --global url."https://".insteadOf git://
 fi
 cd "$RootDir/hpp-fcl"
 git reset --hard
@@ -143,6 +144,7 @@ git checkout --force "v8.0.2"
 ### Checkout pinocchio and its submodules
 if [ ! -d "$RootDir/pinocchio" ]; then
   git clone https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio"
+  git config --global url."https://".insteadOf git://
 fi
 cd "$RootDir/pinocchio"
 git reset --hard

diff --git a/build_tools/build_install_deps_windows.ps1 b/build_tools/build_install_deps_windows.ps1
@@ -119,6 +119,7 @@ git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/ass
 ### Checkout hpp-fcl
 if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl")) {
   git clone https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl"
+  git config --global url."https://".insteadOf git://
 }
 Set-Location -Path "$RootDir/hpp-fcl"
 git reset --hard
@@ -133,6 +134,7 @@ git checkout --force "v8.0.2"
 ### Checkout pinocchio and its submodules, then apply some patches (generated using `git diff --submodule=diff`)
 if (-not (Test-Path -PathType Container "$RootDir/pinocchio")) {
   git clone https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio"
+  git config --global url."https://".insteadOf git://
 }
 Set-Location -Path "$RootDir/pinocchio"
 git reset --hard

diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
@@ -66,7 +66,7 @@ set(SRC
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/BasicSensors.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/Robot.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/control/AbstractController.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/solver/LCPSolvers.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/solver/ConstraintSolvers.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/AbstractStepper.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/EulerExplicitStepper.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/stepper/AbstractRungeKuttaStepper.cc"

diff --git a/core/include/jiminy/core/Types.h b/core/include/jiminy/core/Types.h
@@ -73,9 +73,6 @@ namespace jiminy
     using vector3_t = Eigen::Matrix<float64_t, 3, 1>;
     using vector6_t = Eigen::Matrix<float64_t, 6, 1>;
 
-    using constMatrixBlock_t = Eigen::Block<matrixN_t const, Eigen::Dynamic, Eigen::Dynamic> const;
-    using constVectorBlock_t = Eigen::VectorBlock<vectorN_t const, Eigen::Dynamic> const;
-
     using quaternion_t = Eigen::Quaternion<float64_t>;
 
     // Pinocchio types

diff --git a/core/include/jiminy/core/constraints/FixedFrameConstraint.h b/core/include/jiminy/core/constraints/FixedFrameConstraint.h
@@ -63,13 +63,11 @@ namespace jiminy
         std::string const frameName_;                       ///< Name of the frame on which the constraint operates.
         frameIndex_t frameIdx_;                             ///< Corresponding frame index.
         std::vector<uint32_t> dofsFixed_;                   ///< Degrees of freedom to fix.
-        bool_t isFixedPositionXY_;                          ///< Whether or not the frame is fixed for both X and Y translations
         pinocchio::SE3 transformRef_;                       ///< Reference pose of the frame to enforce.
         vector3_t normal_;                                  ///< Normal direction locally at the interface.
         matrix3_t rotationLocal_;                           ///< Rotation matrix of the local frame in which to apply masking
         matrix6N_t frameJacobian_;                          ///< Stores full frame jacobian in reference frame.
         pinocchio::Motion frameDrift_;                      ///< Stores full frame drift in reference frame.
-        Eigen::Matrix<float64_t, Eigen::Dynamic, 2> UiJt_;  ///< Used to store intermediary computation to compute diag(J.Minv.Jt)_t
     };
 }
 

diff --git a/core/include/jiminy/core/constraints/JointConstraint.h b/core/include/jiminy/core/constraints/JointConstraint.h
@@ -37,16 +37,20 @@ namespace jiminy
         void setReferenceConfiguration(vectorN_t const & configurationRef);
         vectorN_t const & getReferenceConfiguration(void) const;
 
+        void setRotationDir(bool_t isReversed);
+        bool_t getRotationDir();
+
         virtual hresult_t reset(vectorN_t const & q,
                                 vectorN_t const & v) override final;
 
         virtual hresult_t computeJacobianAndDrift(vectorN_t const & q,
                                                   vectorN_t const & v) override final;
 
     private:
-        std::string jointName_;       ///< Name of the joint on which the constraint operates.
-        jointIndex_t jointIdx_;       ///< Corresponding joint index.
-        vectorN_t configurationRef_;  ///< Reference position of the joint to enforce.
+        std::string jointName_;        ///< Name of the joint on which the constraint operates.
+        jointIndex_t jointIdx_;        ///< Corresponding joint index.
+        vectorN_t configurationRef_;   ///< Reference position of the joint to enforce.
+        bool_t isReversed_;            ///< Whether or not to reverse the sign of the constraint.
     };
 }
 

diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h
@@ -45,7 +45,6 @@ namespace jiminy
     class Timer;
     class Robot;
     class AbstractConstraintBase;
-    class AbstractLCPSolver;
     class AbstractController;
     class AbstractStepper;
     class TelemetryData;
@@ -115,8 +114,6 @@ namespace jiminy
         {
             configHolder_t config;
             config["solver"] = std::string("PGS");   // ["PGS",]
-            config["tolAbs"] = 1.0e-5;
-            config["tolRel"] = 1.0e-7;
             config["regularization"] = 1.0e-3;       // Relative inverse damping wrt. diagonal of J.Minv.J.t. 0.0 to enforce the minimum absolute regularizer.
             config["stabilizationFreq"] = 20.0;      // [s-1]: 0.0 to disable
 
@@ -209,15 +206,11 @@ namespace jiminy
         struct constraintOptions_t
         {
             std::string const solver;
-            float64_t const tolAbs;
-            float64_t const tolRel;
             float64_t const regularization;
             float64_t const stabilizationFreq;
 
             constraintOptions_t(configHolder_t const & options) :
             solver(boost::get<std::string>(options.at("solver"))),
-            tolAbs(boost::get<float64_t>(options.at("tolAbs"))),
-            tolRel(boost::get<float64_t>(options.at("tolRel"))),
             regularization(boost::get<float64_t>(options.at("regularization"))),
             stabilizationFreq(boost::get<float64_t>(options.at("stabilizationFreq")))
             {
@@ -671,7 +664,6 @@ namespace jiminy
     private:
         std::unique_ptr<Timer> timer_;
         contactModel_t contactModel_;
-        std::unique_ptr<AbstractLCPSolver> constraintSolver_;
         TelemetrySender telemetrySender_;
         std::shared_ptr<TelemetryData> telemetryData_;
         std::unique_ptr<TelemetryRecorder> telemetryRecorder_;

diff --git a/core/include/jiminy/core/engine/System.h b/core/include/jiminy/core/engine/System.h
@@ -13,6 +13,7 @@
 namespace jiminy
 {
     class Robot;
+    class AbstractConstraintSolver;
     class AbstractConstraintBase;
     class AbstractController;
     class LockGuardLocal;
@@ -147,14 +148,11 @@ namespace jiminy
         std::set<float64_t>::const_iterator forcesImpulseBreakNextIt;  ///< Iterator related to the time of the next breakpoint associated with the impulse forces
         std::vector<bool_t> forcesImpulseActive;                       ///< Flag to active the forces. This is used to handle t-, t+ properly. Otherwise, it is impossible to determine at time t if the force is active or not.
 
+        std::unique_ptr<AbstractConstraintSolver> constraintSolver;
         constraintsHolder_t constraintsHolder;                         ///< Store copy of constraints register for fast access.
-        std::vector<int32_t> boundJointsActiveDir;                     ///< Store the active "direction" of the bound (0 for lower, 1 for higher)
         forceVector_t contactFramesForces;                             ///< Contact forces for each contact frames in local frame
         vector_aligned_t<forceVector_t> collisionBodiesForces;         ///< Contact forces for each geometries of each collision bodies in local frame
         matrix6N_t jointJacobian;                                      ///< Buffer used for intermediary computation of `data.u`
-        vectorN_t lo;                                                  ///< Lower bound of LCP problem
-        vectorN_t hi;                                                  ///< Higher bound of LCP problem
-        std::vector<std::vector<int32_t> > fIndices;                   ///< Used to indicate linear coupling between bounds of LCP and the solution (i.e. friction cone: - mu * F_z < F_x/F_y < mu * F_z)
 
         std::vector<std::string> positionFieldnames;
         std::vector<std::string> velocityFieldnames;