[core] Enable simulation of procedurally generated robots. (#427)

* [core] Improve efficiency of PGS solver. (#423) * [core] Add contact solver 'tolAbs', 'tolRel' options. (#424) * [core] Enable to initialize 'jiminy::Robot' using dynamics and collision pinocchio model. * [core] Add visual model to 'jiminy::Model'. * [core] Add name getter to 'jiminy::Model'. * [core] Fix wrong computation of external forces. * [core/python] Add method to build reduced model. * [python] Do not rely on 'Robot.urdf_path' anymore. Enable to load hardware config on existing robot. * [python/robot] Check if motors and sensors defined in hardware file can be added successfully. * [python/dynamics] Fix 'compute_freeflyer_state_from_fixed_body' method in fixed body is not provided. * [python/plot] Fix support of matplotlib >= 3.4. * [gym/common] Make sure the provided neutral configuration is valid. * [gym/envs] Tune Cassie PID controllers and model hyperparameters to improve numerical stability. * [gym/envs] Add atlas env with fixed upper body. Tune Cassie and Atlas PID for impulse contact model. * [misc] Stricter gym_jiminy optional deps requirements. (#425) * [misc] Fix documentation generation. * [misc] Fix gym_jiminy wheels deployement on pypi. Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
duburcqa · Oct 27, 2021 · 0369d72 · 0369d72
1 parent 3fb0bb8
commit 0369d72
Show file tree

Hide file tree

Showing 63 changed files with 1,211 additions and 590 deletions.
diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml
@@ -136,7 +136,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/manylinux2010_x86_64' && matrix.PYTHON_VERSION == 'cp36' && matrix.legacy == false &&
+        success() && matrix.container == 'quay.io/pypa/manylinux2014_x86_64' && matrix.PYTHON_VERSION == 'cp36' && matrix.legacy == false &&
         github.repository == 'duburcqa/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master'
       uses: pypa/gh-action-pypi-publish@master
       with:

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.2)
+set(BUILD_VERSION 1.7.3)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

diff --git a/build_tools/build_install_deps_linux.sh b/build_tools/build_install_deps_linux.sh
@@ -10,9 +10,9 @@ if [ -z ${BUILD_TYPE} ]; then
 fi
 
 ### Set common CMAKE_C/CXX_FLAGS
-CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC -s"
+CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC"
 if [ "${BUILD_TYPE}" == "Release" ]; then
-  CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -O3 -DNDEBUG"
+  CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -O3 -s -DNDEBUG"
 else
   CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -O0 -g"
 fi

diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h
@@ -116,6 +116,8 @@ namespace jiminy
             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["stabilizationFreq"] = 20.0;      // [s-1]: 0.0 to disable
             config["stiffness"] = 1.0e6;
@@ -200,6 +202,8 @@ namespace jiminy
         {
             std::string const model;
             std::string const solver;
+            float64_t const tolAbs;
+            float64_t const tolRel;
             float64_t const regularization;
             float64_t const stabilizationFreq;
             float64_t const stiffness;
@@ -212,6 +216,8 @@ namespace jiminy
             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"))),

diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h
@@ -329,6 +329,7 @@ namespace jiminy
         virtual void reset(void);
 
         bool_t const & getIsInitialized(void) const;
+        std::string const & getName(void) const;
         std::string const & getUrdfPath(void) const;
         std::vector<std::string> const & getMeshPackageDirs(void) const;
         bool_t const & getHasFreeflyer(void) const;
@@ -369,7 +370,8 @@ namespace jiminy
 
     protected:
         hresult_t initialize(pinocchio::Model         const & pncModel,
-                             pinocchio::GeometryModel const & collisionModel);
+                             pinocchio::GeometryModel const & collisionModel,
+                             pinocchio::GeometryModel const & visualModel);
 
         hresult_t generateModelFlexible(void);
         hresult_t generateModelBiased(void);
@@ -397,12 +399,13 @@ namespace jiminy
         virtual hresult_t refreshProxies(void);
 
     public:
+        pinocchio::Model pncModelRigidOrig_;
         pinocchio::Model pncModel_;
-        mutable pinocchio::Data pncData_;
         pinocchio::GeometryModel collisionModel_;
-        mutable std::unique_ptr<pinocchio::GeometryData> pncGeometryData_;  // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6
-        pinocchio::Model pncModelRigidOrig_;
+        pinocchio::GeometryModel visualModel_;
         pinocchio::Data pncDataRigidOrig_;
+        mutable pinocchio::Data pncData_;
+        mutable std::unique_ptr<pinocchio::GeometryData> pncCollisionData_;  // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6
         std::unique_ptr<modelOptions_t const> mdlOptions_;
         forceVector_t contactForces_;                       ///< Buffer storing the contact forces
 

diff --git a/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h b/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h
@@ -349,13 +349,15 @@ namespace pinocchio_overload
             pinocchio::cholesky::decompose(model, data);
         }
 
-        // Compute sqrt(D)^-1 * U^-1 * J.T
+        // Compute sDUiJt := sqrt(D)^-1 * U^-1 * J.T
         data.sDUiJt = J.transpose();
         pinocchio::cholesky::Uiv(model, data, data.sDUiJt);
         data.sDUiJt.array().colwise() /= data.D.array().sqrt();
 
-        // Compute JMinvJt
-        data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
+        // Compute JMinvJt := sDUiJt.T * sDUiJt
+        data.JMinvJt = matrixN_t::Zero(J.rows(), J.rows());
+        data.JMinvJt.selfadjointView<Eigen::Lower>().rankUpdate(data.sDUiJt.transpose());
+        data.JMinvJt.triangularView<Eigen::Upper>() = data.JMinvJt.transpose();
 
         return data.JMinvJt;
     }

diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h
@@ -38,6 +38,9 @@ namespace jiminy
         auto shared_from_this() { return shared_from(this); }
         auto shared_from_this() const { return shared_from(this); }
 
+        hresult_t initialize(pinocchio::Model         const & pncModel,
+                             pinocchio::GeometryModel const & collisionModel,
+                             pinocchio::GeometryModel const & visualModel);
         hresult_t initialize(std::string const & urdfPath,
                              bool_t const & hasFreeflyer = true,
                              std::vector<std::string> const & meshPackageDirs = {});

diff --git a/core/include/jiminy/core/solver/LCPSolvers.h b/core/include/jiminy/core/solver/LCPSolvers.h
@@ -6,8 +6,6 @@
 
 namespace jiminy
 {
-    float64_t const EPS_DIVISION = 1.0e-9;
-
     class AbstractLCPSolver
     {
     public:
@@ -17,7 +15,7 @@ namespace jiminy
         ///
         ///        using boxed bounds lo < x < hi instead of 0 < x:
         ///        s.t. if fIdx[i] < 0, lo[i] < x[i] < hi[i]
-        ///             else, - hi[i] x[fIdx[i]] < x[i] < hi[i] x[fIdx[i]]
+        ///             else, - hi[i] * x[fIdx[i]] < x[i] < hi[i] * x[fIdx[i]]
         ///
         /// The result x will be stored in data.lambda_c.
         virtual bool_t BoxedForwardDynamics(pinocchio::Model const & model,
@@ -55,8 +53,6 @@ namespace jiminy
                                         vectorN_t const & lo,
                                         vectorN_t const & hi,
                                         std::vector<int32_t> const & fIdx,
-                                        bool_t const & checkAbs,
-                                        bool_t const & checkRel,
                                         vectorN_t & x);
         bool_t ProjectedGaussSeidelSolver(matrixN_t & A,
                                           vectorN_t & b,

diff --git a/core/include/jiminy/core/utilities/Helpers.h b/core/include/jiminy/core/utilities/Helpers.h
@@ -107,20 +107,16 @@ namespace jiminy
     template<typename T0, typename T1, typename... Ts>
     typename std::common_type_t<T0, T1, Ts...> min(T0 && val1, T1 && val2, Ts &&... vs);
 
-    template<typename DerivedType>
-    auto clamp(Eigen::MatrixBase<DerivedType> const & data,
-               float64_t const & minThr = -INF,
-               float64_t const & maxThr = +INF);
+    template<typename ScalarType>
+    constexpr ScalarType const & clamp(ScalarType const & data,
+                                       ScalarType const & minThr,
+                                       ScalarType const & maxThr);
 
     template<typename DerivedType1, typename DerivedType2, typename DerivedType3>
     Eigen::MatrixBase<DerivedType1> clamp(Eigen::MatrixBase<DerivedType1> const & data,
                                           Eigen::MatrixBase<DerivedType2> const & minThr,
                                           Eigen::MatrixBase<DerivedType2> const & maxThr);
 
-    float64_t clamp(float64_t const & data,
-                    float64_t const & minThr = -INF,
-                    float64_t const & maxThr = +INF);
-
     template<typename... Args>
     float64_t minClipped(float64_t val1, float64_t val2, Args ... vs);
 

diff --git a/core/include/jiminy/core/utilities/Helpers.tpp b/core/include/jiminy/core/utilities/Helpers.tpp
@@ -18,6 +18,22 @@ namespace jiminy
         return min(std::min(val1, val2), std::forward<Ts>(vs)...);
     }
 
+    template<typename ScalarType>
+    constexpr ScalarType const & clamp(ScalarType const & data,
+                                       ScalarType const & minThr,
+                                       ScalarType const & maxThr)
+    {
+        if (data < minThr)
+        {
+            return minThr;
+        }
+        if (maxThr < data)
+        {
+            return maxThr;
+        }
+        return data;
+    }
+
     template<typename DerivedType>
     auto clamp(Eigen::MatrixBase<DerivedType> const & data,
                float64_t const & minThr,

diff --git a/core/src/Constants.cc b/core/src/Constants.cc
@@ -19,7 +19,7 @@ namespace jiminy
     float64_t const SIMULATION_MIN_TIMESTEP = 1e-6;
     float64_t const SIMULATION_MAX_TIMESTEP = 0.02;
 
-    uint32_t const PGS_MAX_ITERATIONS = 100U;
+    uint32_t const PGS_MAX_ITERATIONS = 50U;
     uint32_t const PGS_RANDOM_PERMUTATION_PERIOD = 0U;  // 0 to disable
     float64_t const PGS_MIN_REGULARIZER = 1.0e-12;
 }
diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -888,12 +888,13 @@ namespace jiminy
             contactSolver_ = std::make_unique<PGSSolver>(
                 PGS_MAX_ITERATIONS,
                 PGS_RANDOM_PERMUTATION_PERIOD,
-                engineOptions_->stepper.tolAbs,
-                engineOptions_->stepper.tolRel);
+                engineOptions_->contacts.tolAbs,
+                engineOptions_->contacts.tolRel);
         }
     }
 
-    void computeExtraTerms(systemHolder_t & system)
+    void computeExtraTerms(systemHolder_t           & system,
+                           systemDataHolder_t const & systemData)
     {
         /// This method is optimized to avoid redundant computations.
         /// See `pinocchio::computeAllTerms` for reference:
@@ -942,8 +943,9 @@ namespace jiminy
         for (int32_t i = 1; i < model.njoints; ++i)
         {
             data.h[i] = model.inertias[i] * data.v[i];
-            data.f[i] = model.inertias[i] * data.a[i] + data.v[i].cross(data.h[i]);
+            data.f[i] = model.inertias[i] * data.a[i] + data.v[i].cross(data.h[i]) - systemData.state.fExternal[i];
         }
+
         for (int32_t i = model.njoints - 1; i > 0; --i)
         {
             jointIndex_t const & parentIdx = model.parents[i];
@@ -971,11 +973,14 @@ namespace jiminy
         data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
     }
 
-    void computeAllExtraTerms(std::vector<systemHolder_t> & systems)
+    void computeAllExtraTerms(std::vector<systemHolder_t>                & systems,
+                              vector_aligned_t<systemDataHolder_t> const & systemsDataHolder)
     {
-        for (auto & system : systems)
+        auto systemIt = systems.begin();
+        auto systemDataIt = systemsDataHolder.begin();
+        for ( ; systemIt != systems.end(); ++systemIt, ++systemDataIt)
         {
-            computeExtraTerms(system);
+            computeExtraTerms(*systemIt, *systemDataIt);
         }
     }
 
@@ -1418,7 +1423,7 @@ namespace jiminy
                 a = computeAcceleration(*systemIt, *systemDataIt, q, v, u, fext);
 
                 // Compute joints accelerations and forces
-                computeExtraTerms(*systemIt);
+                computeExtraTerms(*systemIt, *systemDataIt);
                 syncAccelerationsAndForces(*systemIt, *fPrevIt, *aPrevIt);
 
                 // Update the sensor data once again, with the updated effort and acceleration
@@ -1860,7 +1865,7 @@ namespace jiminy
             if (!std::isfinite(stepperUpdatePeriod_) && hasDynamicsChanged)
             {
                 computeSystemsDynamics(t, qSplit, vSplit, aSplit);
-                computeAllExtraTerms(systems_);
+                computeAllExtraTerms(systems_, systemsDataHolder_);
                 syncAllAccelerationsAndForces(systems_, fPrev_, aPrev_);
                 syncSystemsStateWithStepper(true);
                 hasDynamicsChanged = false;
@@ -1909,7 +1914,7 @@ namespace jiminy
                     if (hasDynamicsChanged)
                     {
                         computeSystemsDynamics(t, qSplit, vSplit, aSplit);
-                        computeAllExtraTerms(systems_);
+                        computeAllExtraTerms(systems_, systemsDataHolder_);
                         syncAllAccelerationsAndForces(systems_, fPrev_, aPrev_);
                         syncSystemsStateWithStepper(true);
                         hasDynamicsChanged = false;
@@ -1974,7 +1979,7 @@ namespace jiminy
 
                         /* Compute the actual joint acceleration and forces, based on
                            up-to-date pinocchio::Data. */
-                        computeAllExtraTerms(systems_);
+                        computeAllExtraTerms(systems_, systemsDataHolder_);
 
                         // Synchronize the individual system states
                         syncAllAccelerationsAndForces(systems_, fPrev_, aPrev_);
@@ -2065,7 +2070,7 @@ namespace jiminy
 
                         /* Compute the actual joint acceleration and forces, based on
                            up-to-date pinocchio::Data. */
-                        computeAllExtraTerms(systems_);
+                        computeAllExtraTerms(systems_, systemsDataHolder_);
 
                         // Synchronize the individual system states
                         syncAllAccelerationsAndForces(systems_, fPrev_, aPrev_);
@@ -2790,7 +2795,7 @@ namespace jiminy
         pinocchio::Model const & model = system.robot->pncModel_;
         pinocchio::Data & data = system.robot->pncData_;
         pinocchio::GeometryModel const & geomModel = system.robot->collisionModel_;
-        pinocchio::GeometryData & geomData = *system.robot->pncGeometryData_;
+        pinocchio::GeometryData & geomData = *system.robot->pncCollisionData_;
 
         // Update forward kinematics
         pinocchio::forwardKinematics(model, data, q, v, a);
@@ -2869,7 +2874,7 @@ namespace jiminy
         jointIndex_t const & parentJointIdx = system.robot->collisionModel_.geometryObjects[geometryIdx].parentJoint;
 
         // Extract collision and distance results
-        hpp::fcl::CollisionResult const & collisionResult = system.robot->pncGeometryData_->collisionResults[collisionPairIdx];
+        hpp::fcl::CollisionResult const & collisionResult = system.robot->pncCollisionData_->collisionResults[collisionPairIdx];
 
         fextLocal.setZero();