[core] Contact handling improvements. (#470)

* [core] Enforce isotropic solution for PGS. (#467)
* [core] Compensate PGS bias to ensure maximum energy dissipation for tangential friction. (#468)
* [core] Compute PGS error in residual space. (#462)
* [core] PGS skip parameter update if irrelevant. (#464)
* [core] Force sensors now measure total external force applied on a body. 
* [core] Split constraint and contact options. (#462)
* [core] Move to next breakpoint if possible to avoid very small timesteps during integration. (#464)
* [core] Fix non-linear PGS solving of friction cone. (#461)
* [core] Fix 'removeContactPoints' if input vector is empty.
* [core] Fix timestep adjustment for fixed timestep euler explicit stepper. (#464)
* [core] Fix baumgarte stabilization of orientation for 'FixedFrameConstraint'. (#465)
* [core] Fix json dump for empty dictionary. (#466)
* [core] Remove random permutation from PGS since preserving order is important. (#467)
* [core] Fix 'JointConstraint' position difference not properly computed. (#469)
* [core] Fix telemetry header to null terminate leading to segfault. (#469)
* [core] Fix inconsistency between flexible model and data. (#469)
* [core] Use 'impulse' contact model as default. (#462)
* [core] Modify 'FixedFrameConstraint' to set ground normal instead of local rotation. (#464)
* [core] Order 'FixedFrameConstraint' components by solving order. (#467)
* [python/viewer] Add 'frame' markers. (#464)
* [python/viewer] Add support of 'pin.SE3' object for marker pose. (#465)
* [python/viewer] Support specifying marker orientation using rotation matrix. (#464)
* [python/log] Fix segfault when loading robot from log. (#466)
* [python/viewer] Exception handling and timeout if backend recorder for meshcat fails to open. (#452)
* [python/viewer] Avoid crashing when replying simulations with 'nan'. (#458)
* [python/viewer] Fix extraction of available sensor data for replay. (#463)
* [python/viewer] Keep floor hidden even if updated if it was previously hidden for panda3d backend. (#463)
* [python/viewer] Fix 'display' method if velocity is not provided. (#463)
* [python/viewer] Fix body selection for panda3d. (#465)
* [python/viewer] Do not overwrite color for 'frame' marker by default. (#464)
* [gym/common] Improve exception handling during 'step'. (#458)
* [gym/common] Do not try to register action to telemetry if empty.
* [gym/common] Add 'evaluate' method to evaluate a callback policy over a whole episode. (#453)
* [gym/common] Rename 'refresh_*' in 'initialize_' when appropriate.  (#466)
* [gym/common] Rename 'refresh_internal' in 'refresh_buffers' and add 'initialize_buffers' method. (#466)
* [gym/envs] Reduce KD gain for AkY joints of Atlas to prevent numerical instability. (#464)
* [gym/envs] Automatically detect relevant contact points for Atlas. (#464)
* [misc] Update python packages description and development status. (#458)
* [misc] Run unit tests in debug mode on Ubuntu 20.04. (#469)

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>

.github/workflows/ubuntu.yml

@@ -16,7 +16,11 @@ jobs:
 
     strategy:
       matrix:
-        os: [ubuntu-18.04, ubuntu-20.04]
+        include:
+          - os: ubuntu-18.04
+            BUILD_TYPE: 'Release'
+          - os: ubuntu-20.04
+            BUILD_TYPE: 'Debug'
 
     defaults:
       run:
@@ -70,9 +74,7 @@ jobs:
               -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF \
               -DBoost_NO_SYSTEM_PATHS=OFF -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \
               -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \
-              -DCMAKE_BUILD_TYPE="Debug"
-        make install -j2
-        cmake -DCMAKE_BUILD_TYPE="Release" .
+              -DCMAKE_BUILD_TYPE="${{ matrix.BUILD_TYPE }}"
         make install -j2
 
     #####################################################################################
@@ -84,18 +86,21 @@ jobs:
         mkdir -p "$RootDir/examples/cpp/pip_extension/build"
         cd "$RootDir/examples/cpp/pip_extension/build"
         cmake "$RootDir/examples/cpp/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-              -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" -DCMAKE_BUILD_TYPE="Release"
+              -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" -DCMAKE_BUILD_TYPE="${{ matrix.BUILD_TYPE }}"
         make install
 
         "$InstallDir/bin/pip_double_pendulum"
 
-    - name: Run unit tests
+    - name: Run jiminy unit tests
       run: |
         "$RootDir/build/unit/unit"
 
         cd "$RootDir/unit_py"
         "${PYTHON_EXECUTABLE}" -m unittest discover -v
 
+    - name: Run gym_jiminy unit tests
+      if: matrix.BUILD_TYPE == 'Release'
+      run: |
         cd "$RootDir/python/gym_jiminy/unit_py"
         "${PYTHON_EXECUTABLE}" -m unittest discover -v
 

.github/workflows/win.yml

@@ -142,9 +142,9 @@ jobs:
         $JIMINY_LIB_DIR = (python -c "import os, jiminy_py ; print(os.path.dirname(jiminy_py.get_libraries()))")
         $env:Path += ";$JIMINY_LIB_DIR"
 
-        cmake "$RootDir/examples/cpp/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" `
-              -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_REQUIRED_VERSION="${{ matrix.python-version }}" `
-              -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
+        cmake "$RootDir/examples/cpp/pip_extension" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 `
+              -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" `
+              -DPYTHON_REQUIRED_VERSION="${{ matrix.python-version }}"
         cmake --build . --target INSTALL --config "${env:BUILD_TYPE}"
 
         & "$InstallDir/bin/pip_double_pendulum.exe"

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.7.5)
+set(BUILD_VERSION 1.7.6)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

README.md

@@ -31,7 +31,7 @@ Beside a strong focus on performance to answer machine learning's need for runni
 
 ### Physics
 
-- Provide both classical phenomenological force-level spring-damper contact model and impulse-level LCP based on maximum energy dissipation principle.
+- Provide both classical phenomenological force-level spring-damper contact model and constraint solver based on maximum energy dissipation principle.
 - Support contact and collision with the ground, using either a fixed set of contact points or collision meshes and primitives.
 - Able to simulate multiple articulated systems simultaneously, interacting with each other, to support use cases such as multi-agent reinforcement learning or swarm robotics.
 - Support of compliant joints with force-based spring-damper dynamics, to model joint elasticity, a common phenomenon particularly in legged robotics.

core/include/jiminy/core/Constants.h

@@ -36,7 +36,6 @@ namespace jiminy
     extern float64_t const SIMULATION_MAX_TIMESTEP;
 
     extern uint32_t const PGS_MAX_ITERATIONS;
-    extern uint32_t const PGS_RANDOM_PERMUTATION_PERIOD;
     extern float64_t const PGS_MIN_REGULARIZER;
 }
 

core/include/jiminy/core/constraints/FixedFrameConstraint.h

@@ -50,7 +50,7 @@ namespace jiminy
         void setReferenceTransform(pinocchio::SE3 const & transformRef);
         pinocchio::SE3 const & getReferenceTransform(void) const;
 
-        void setLocalFrame(matrix3_t const & frameRot);
+        void setNormal(vector3_t const & normal);
         matrix3_t const & getLocalFrame(void) const;
 
         virtual hresult_t reset(vectorN_t const & q,
@@ -63,10 +63,13 @@ 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.
+        matrixN_t UiJt_;                      ///< Used to store intermediary computation to compute (J.Minv.Jt)_{i,i}
     };
 }
 

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

@@ -18,28 +18,28 @@ namespace jiminy
     {
         NONE = 0,
         SPRING_DAMPER = 1,
-        IMPULSE = 2
+        CONSTRAINT = 2
     };
 
-    enum class contactSolver_t : uint8_t
+    enum class constraintSolver_t : uint8_t
     {
         NONE = 0,
         PGS = 1  // Projected Gauss-Seidel
     };
 
     std::map<std::string, contactModel_t> const CONTACT_MODELS_MAP {
         {"spring_damper", contactModel_t::SPRING_DAMPER},
-        {"impulse", contactModel_t::IMPULSE},
+        {"constraint", contactModel_t::CONSTRAINT},
     };
 
-    std::map<std::string, contactSolver_t> const CONTACT_SOLVERS_MAP {
-        {"PGS", contactSolver_t::PGS}
+    std::map<std::string, constraintSolver_t> const CONSTRAINT_SOLVERS_MAP {
+        {"PGS", constraintSolver_t::PGS}
     };
 
     std::set<std::string> const STEPPERS {
+        "euler_explicit",
         "runge_kutta_4",
-        "runge_kutta_dopri5",
-        "euler_explicit"
+        "runge_kutta_dopri5"
     };
 
     class Timer;
@@ -111,20 +111,27 @@ namespace jiminy
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     public:
-        configHolder_t getDefaultContactOptions()
+        configHolder_t getDefaultConstraintOptions()
         {
             configHolder_t config;
-            config["model"] = std::string("spring_damper");   // ["spring_damper", "impulse"]
             config["solver"] = std::string("PGS");   // ["PGS",]
-            config["tolAbs"] = 1.0e-4;
-            config["tolRel"] = 1.0e-3;
-            config["regularization"] = 0.0;     // Relative inverse damping wrt. diagonal of J.Minv.J.t. 0.0 to enforce the minimum absolute regularizer.
+            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
+
+            return config;
+        };
+
+        configHolder_t getDefaultContactOptions()
+        {
+            configHolder_t config;
+            config["model"] = std::string("constraint");   // ["spring_damper", "constraint"]
             config["stiffness"] = 1.0e6;
             config["damping"] = 2.0e3;
-            config["transitionEps"] = 1.0e-3;  // [m]
             config["friction"] = 1.0;
             config["torsion"] = 0.0;
+            config["transitionEps"] = 1.0e-3;  // [m]
             config["transitionVelocity"] = 1.0e-2;  // [m.s-1]
 
             return config;
@@ -193,38 +200,48 @@ namespace jiminy
             config["stepper"] = getDefaultStepperOptions();
             config["world"] = getDefaultWorldOptions();
             config["joints"] = getDefaultJointOptions();
+            config["constraints"] = getDefaultConstraintOptions();
             config["contacts"] = getDefaultContactOptions();
 
             return config;
         };
 
-        struct contactOptions_t
+        struct constraintOptions_t
         {
-            std::string const model;
             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")))
+            {
+                // Empty.
+            }
+        };
+
+        struct contactOptions_t
+        {
+            std::string const model;
             float64_t const stiffness;
             float64_t const damping;
-            float64_t const transitionEps;
             float64_t const friction;
             float64_t const torsion;
+            float64_t const transitionEps;
             float64_t const transitionVelocity;
 
             contactOptions_t(configHolder_t const & options) :
             model(boost::get<std::string>(options.at("model"))),
-            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"))),
             stiffness(boost::get<float64_t>(options.at("stiffness"))),
             damping(boost::get<float64_t>(options.at("damping"))),
-            transitionEps(boost::get<float64_t>(options.at("transitionEps"))),
             friction(boost::get<float64_t>(options.at("friction"))),
             torsion(boost::get<float64_t>(options.at("torsion"))),
+            transitionEps(boost::get<float64_t>(options.at("transitionEps"))),
             transitionVelocity(boost::get<float64_t>(options.at("transitionVelocity")))
             {
                 // Empty.
@@ -319,17 +336,19 @@ namespace jiminy
 
         struct engineOptions_t
         {
-            telemetryOptions_t const telemetry;
-            stepperOptions_t   const stepper;
-            worldOptions_t     const world;
-            jointOptions_t     const joints;
-            contactOptions_t   const contacts;
+            telemetryOptions_t  const telemetry;
+            stepperOptions_t    const stepper;
+            worldOptions_t      const world;
+            jointOptions_t      const joints;
+            constraintOptions_t const constraints;
+            contactOptions_t    const contacts;
 
             engineOptions_t(configHolder_t const & options) :
             telemetry(boost::get<configHolder_t>(options.at("telemetry"))),
             stepper(boost::get<configHolder_t>(options.at("stepper"))),
             world(boost::get<configHolder_t>(options.at("world"))),
             joints(boost::get<configHolder_t>(options.at("joints"))),
+            constraints(boost::get<configHolder_t>(options.at("constraints"))),
             contacts(boost::get<configHolder_t>(options.at("contacts")))
             {
                 // Empty.
@@ -652,7 +671,7 @@ namespace jiminy
     private:
         std::unique_ptr<Timer> timer_;
         contactModel_t contactModel_;
-        std::unique_ptr<AbstractLCPSolver> contactSolver_;
+        std::unique_ptr<AbstractLCPSolver> constraintSolver_;
         TelemetrySender telemetrySender_;
         std::shared_ptr<TelemetryData> telemetryData_;
         std::unique_ptr<TelemetryRecorder> telemetryRecorder_;

core/include/jiminy/core/engine/System.h

@@ -151,8 +151,7 @@ namespace jiminy
         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
-        matrixN_t jointJacobian;                                       ///< Buffer used for intermediary computation of `uAugmented`
-        vectorN_t uAugmented;                                          ///< Used to store the input effort plus the effect of external forces
+        matrixN_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)

core/include/jiminy/core/robot/AbstractSensor.h

@@ -319,20 +319,22 @@ namespace jiminy
         /// \remark     This method is not intended to be called manually. The Robot to which the
         ///             sensor is added is taking care of it while updating the state of the sensors.
         ///
-        /// \param[in]  t       Current time.
-        /// \param[in]  q       Current configuration vector of the motor.
-        /// \param[in]  v       Current velocity vector of the motor.
-        /// \param[in]  a       Current acceleration vector of the motor.
-        /// \param[in]  uMotor  Current motor effort vector of the motor.
+        /// \param[in]  t          Current time.
+        /// \param[in]  q          Current configuration vector of the robot.
+        /// \param[in]  v          Current velocity vector of the robot.
+        /// \param[in]  a          Current acceleration vector of the robot.
+        /// \param[in]  uMotor     Current motor effort vector.
+        /// \param[in]  fExternal  std::vector of current external force vectors applied on the robot.
         ///
         /// \return     Return code to determine whether the execution of the method was successful.
         ///
         ///////////////////////////////////////////////////////////////////////////////////////////////
-        virtual hresult_t setAll(float64_t const & t,
-                                 vectorN_t const & q,
-                                 vectorN_t const & v,
-                                 vectorN_t const & a,
-                                 vectorN_t const & uMotor) = 0;
+        virtual hresult_t setAll(float64_t     const & t,
+                                 vectorN_t     const & q,
+                                 vectorN_t     const & v,
+                                 vectorN_t     const & a,
+                                 vectorN_t     const & uMotor,
+                                 forceVector_t const & fExternal) = 0;
 
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///
@@ -341,20 +343,22 @@ namespace jiminy
         /// \details    It assumes that the internal state of the robot is consistent with the
         ///             input arguments.
         ///
-        /// \param[in]  t       Current time.
-        /// \param[in]  q       Current configuration vector of the robot.
-        /// \param[in]  v       Current velocity vector of the robot.
-        /// \param[in]  a       Current acceleration vector of the robot.
-        /// \param[in]  uMotor  Current motor effort vector of the robot.
+        /// \param[in]  t          Current time.
+        /// \param[in]  q          Current configuration vector of the robot.
+        /// \param[in]  v          Current velocity vector of the robot.
+        /// \param[in]  a          Current acceleration vector of the robot.
+        /// \param[in]  uMotor     Current motor effort vector.
+        /// \param[in]  fExternal  std::vector of current external force vectors applied on the robot.
         ///
         /// \return     Return code to determine whether the execution of the method was successful.
         ///
         ///////////////////////////////////////////////////////////////////////////////////////////////
-        virtual hresult_t set(float64_t const & t,
-                              vectorN_t const & q,
-                              vectorN_t const & v,
-                              vectorN_t const & a,
-                              vectorN_t const & uMotor) = 0;
+        virtual hresult_t set(float64_t     const & t,
+                              vectorN_t     const & q,
+                              vectorN_t     const & v,
+                              vectorN_t     const & a,
+                              vectorN_t     const & uMotor,
+                              forceVector_t const & fExternal) = 0;
 
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///
@@ -458,11 +462,12 @@ namespace jiminy
         virtual Eigen::Ref<vectorN_t const> get(void) const override final;
 
     protected:
-        virtual hresult_t setAll(float64_t const & t,
-                                 vectorN_t const & q,
-                                 vectorN_t const & v,
-                                 vectorN_t const & a,
-                                 vectorN_t const & uMotor) override final;
+        virtual hresult_t setAll(float64_t     const & t,
+                                 vectorN_t     const & q,
+                                 vectorN_t     const & v,
+                                 vectorN_t     const & a,
+                                 vectorN_t     const & uMotor,
+                                 forceVector_t const & fExternal) override final;
         virtual Eigen::Ref<vectorN_t> get(void) override final;
         virtual Eigen::Ref<vectorN_t> data(void) override final;
 

core/include/jiminy/core/robot/AbstractSensor.tpp

@@ -422,11 +422,12 @@ namespace jiminy
     }
 
     template<typename T>
-    hresult_t AbstractSensorTpl<T>::setAll(float64_t const & t,
-                                           vectorN_t const & q,
-                                           vectorN_t const & v,
-                                           vectorN_t const & a,
-                                           vectorN_t const & uMotor)
+    hresult_t AbstractSensorTpl<T>::setAll(float64_t     const & t,
+                                           vectorN_t     const & q,
+                                           vectorN_t     const & v,
+                                           vectorN_t     const & a,
+                                           vectorN_t     const & uMotor,
+                                           forceVector_t const & fExternal)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
@@ -498,7 +499,7 @@ namespace jiminy
         {
             if (returnCode == hresult_t::SUCCESS)
             {
-                returnCode = sensor->set(t, q, v, a, uMotor);
+                returnCode = sensor->set(t, q, v, a, uMotor, fExternal);
             }
         }