Merge branch 'master' into integrate_transmission

.github/workflows/linux.yml

@@ -90,9 +90,9 @@ jobs:
 
         "$InstallDir/bin/jiminy_double_pendulum"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
-        cmake "$RootDir/examples/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
+        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" \
                -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \
                -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
         make install

.github/workflows/manylinux.yml

@@ -15,7 +15,7 @@ jobs:
 
     strategy:
       matrix:
-        container: ['quay.io/pypa/manylinux2010_x86_64',
+        container: ['quay.io/pypa/manylinux2014_x86_64',
                     'quay.io/pypa/manylinux_2_24_x86_64']
         PYTHON_VERSION: ['cp36', 'cp37', 'cp38', 'cp39']
         legacy: [false]
@@ -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 == '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:

.github/workflows/ubuntu.yml

@@ -22,9 +22,6 @@ jobs:
       run:
         shell: bash -ieo pipefail {0}  # Using bash enables automatic sourcing `.bashrc` and fail-fast behavior
 
-    env:
-      BUILD_TYPE: "Release"
-
     #####################################################################################
 
     steps:
@@ -49,8 +46,8 @@ jobs:
     - name: Installing requirements
       run: |
         sudo env "PATH=$PATH" "${GITHUB_WORKSPACE}/build_tools/easy_install_deps_ubuntu.sh"
-        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
         "${PYTHON_EXECUTABLE}" -m pip install tensorflow
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
         "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -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"
         "${PYTHON_EXECUTABLE}" -m pip install "ray[default,rllib]<=1.4.0"  # Type checking is not working with 1.4.1
@@ -73,7 +70,9 @@ 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="${BUILD_TYPE}"
+              -DCMAKE_BUILD_TYPE="Debug"
+        make install -j2
+        cmake -DCMAKE_BUILD_TYPE="Release" .
         make install -j2
 
     #####################################################################################
@@ -82,10 +81,10 @@ jobs:
       run: |
         "$InstallDir/bin/jiminy_double_pendulum"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
-        cmake "$RootDir/examples/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-              -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
+        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"
         make install
 
         "$InstallDir/bin/pip_double_pendulum"

.github/workflows/win.yml

@@ -136,13 +136,13 @@ jobs:
 
         & "$InstallDir/bin/jiminy_double_pendulum.exe"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
+        mkdir -p "$RootDir/examples/cpp/pip_extension/build"
+        cd "$RootDir/examples/cpp/pip_extension/build"
 
         $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/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" `
+        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 --build . --target INSTALL --config "${env:BUILD_TYPE}"

CMakeLists.txt

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

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

core/include/jiminy/core/Types.h

@@ -67,10 +67,11 @@ namespace jiminy
     using matrixN_t = Eigen::Matrix<float64_t, Eigen::Dynamic, Eigen::Dynamic>;
     using matrix2_t = Eigen::Matrix<float64_t, 2, 2>;
     using matrix3_t = Eigen::Matrix<float64_t, 3, 3>;
+    using matrix6N_t = Eigen::Matrix<float64_t, 6, Eigen::Dynamic>;
     using vectorN_t = Eigen::Matrix<float64_t, Eigen::Dynamic, 1>;
+    using vector2_t = Eigen::Matrix<float64_t, 2, 1>;
     using vector3_t = Eigen::Matrix<float64_t, 3, 1>;
     using vector6_t = Eigen::Matrix<float64_t, 6, 1>;
-    using rowN_t = Eigen::Matrix<float64_t, 1, Eigen::Dynamic>;
 
     using constMatrixBlock_t = Eigen::Block<matrixN_t const, Eigen::Dynamic, Eigen::Dynamic> const;
     using constVectorBlock_t = Eigen::VectorBlock<vectorN_t const, Eigen::Dynamic> const;
@@ -120,7 +121,7 @@ namespace jiminy
 
     /* Ground profile signature.
        Note that it is impossible to use function pointer since it does not support functors. */
-    using heatMapFunctor_t = std::function<std::pair<float64_t, vector3_t>(vector3_t const & /*pos*/)>;
+    using heightmapFunctor_t = std::function<std::pair<float64_t, vector3_t>(vector3_t const & /*pos*/)>;
 
     // Flexible joints
     struct flexibleJointData_t
@@ -159,7 +160,7 @@ namespace jiminy
 
     // Configuration/option holder
     using configField_t = boost::make_recursive_variant<
-        bool_t, uint32_t, int32_t, float64_t, std::string, vectorN_t, matrixN_t, heatMapFunctor_t,
+        bool_t, uint32_t, int32_t, float64_t, std::string, vectorN_t, matrixN_t, heightmapFunctor_t,
         std::vector<std::string>, std::vector<vectorN_t>, std::vector<matrixN_t>,
         flexibilityConfig_t, std::unordered_map<std::string, boost::recursive_variant_>
     >::type;

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

@@ -39,19 +39,19 @@ namespace jiminy
         /// \param[in]  frameName   Name of the frame on which the constraint is to be applied.
         ///////////////////////////////////////////////////////////////////////////////////////////////
         FixedFrameConstraint(std::string const & frameName,
-                             Eigen::Matrix<bool_t, 6, 1> const & maskFixed = Eigen::Matrix<bool_t, 6, 1>::Constant(true),
-                             pinocchio::ReferenceFrame const & frameRef = pinocchio::LOCAL_WORLD_ALIGNED);
+                             Eigen::Matrix<bool_t, 6, 1> const & maskFixed = Eigen::Matrix<bool_t, 6, 1>::Constant(true));
         virtual ~FixedFrameConstraint(void);
 
         std::string const & getFrameName(void) const;
         frameIndex_t const & getFrameIdx(void) const;
 
         std::vector<uint32_t> const & getDofsFixed(void) const;
 
-        pinocchio::ReferenceFrame const & getReferenceFrame(void) const;
-
         void setReferenceTransform(pinocchio::SE3 const & transformRef);
-        pinocchio::SE3 & getReferenceTransform(void);
+        pinocchio::SE3 const & getReferenceTransform(void) const;
+
+        void setLocalFrame(matrix3_t const & frameRot);
+        matrix3_t const & getLocalFrame(void) const;
 
         virtual hresult_t reset(vectorN_t const & q,
                                 vectorN_t const & v) override final;
@@ -62,11 +62,11 @@ namespace jiminy
     private:
         std::string const frameName_;         ///< Name of the frame on which the constraint operates.
         frameIndex_t frameIdx_;               ///< Corresponding frame index.
-        pinocchio::ReferenceFrame frameRef_;  ///< Reference frame.
         std::vector<uint32_t> dofsFixed_;     ///< Degrees of freedom to fix.
         pinocchio::SE3 transformRef_;         ///< Reference pose of the frame to enforce.
-        matrixN_t frameJacobian_;             ///< Stores full frame jacobian in reference frame.
-        vector6_t frameDrift_;                ///< Stores full frame drift in reference frame.
+        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.
     };
 }
 

core/include/jiminy/core/constraints/JointConstraint.h

@@ -34,12 +34,8 @@ namespace jiminy
         std::string const & getJointName(void) const;
         jointIndex_t const & getJointIdx(void) const;
 
-        template<typename DerivedType>
-        void setReferenceConfiguration(Eigen::MatrixBase<DerivedType> const & configurationRef)
-        {
-            configurationRef_ = configurationRef;
-        }
-        vectorN_t & getReferenceConfiguration(void);
+        void setReferenceConfiguration(vectorN_t const & configurationRef);
+        vectorN_t const & getReferenceConfiguration(void) const;
 
         virtual hresult_t reset(vectorN_t const & q,
                                 vectorN_t const & v) override final;

core/include/jiminy/core/constraints/SphereConstraint.h

@@ -43,14 +43,14 @@ namespace jiminy
         ///////////////////////////////////////////////////////////////////////////////////////////////
         SphereConstraint(std::string const & frameName,
                          float64_t   const & sphereRadius,
-                         vector3_t   const & groundNormal = (vector3_t() << 0.0, 0.0, 1.0).finished());
+                         vector3_t   const & groundNormal = vector3_t::UnitZ());
         virtual ~SphereConstraint(void);
 
         std::string const & getFrameName(void) const;
         frameIndex_t const & getFrameIdx(void) const;
 
         void setReferenceTransform(pinocchio::SE3 const & transformRef);
-        pinocchio::SE3 & getReferenceTransform(void);
+        pinocchio::SE3 const & getReferenceTransform(void) const;
 
         virtual hresult_t reset(vectorN_t const & /* q */,
                                 vectorN_t const & /* v */) override final;

core/include/jiminy/core/constraints/WheelConstraint.h

@@ -52,7 +52,7 @@ namespace jiminy
         frameIndex_t const & getFrameIdx(void) const;
 
         void setReferenceTransform(pinocchio::SE3 const & transformRef);
-        pinocchio::SE3 & getReferenceTransform(void);
+        pinocchio::SE3 const & getReferenceTransform(void) const;
 
         virtual hresult_t reset(vectorN_t const & /* q */,
                                 vectorN_t const & /* v */) override final;

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

@@ -116,12 +116,15 @@ 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"] = 0.0;  // [s-1]: 0.0 to disable
+            config["stabilizationFreq"] = 20.0;      // [s-1]: 0.0 to disable
             config["stiffness"] = 1.0e6;
             config["damping"] = 2.0e3;
             config["transitionEps"] = 1.0e-3;  // [m]
             config["friction"] = 1.0;
+            config["torsion"] = 0.0;
             config["transitionVelocity"] = 1.0e-2;  // [m.s-1]
 
             return config;
@@ -140,10 +143,10 @@ namespace jiminy
         {
             configHolder_t config;
             config["gravity"] = (vectorN_t(6) << 0.0, 0.0, -9.81, 0.0, 0.0, 0.0).finished();
-            config["groundProfile"] = heatMapFunctor_t(
-                [](vector3_t const & /* pos */) -> std::pair <float64_t, vector3_t>
+            config["groundProfile"] = heightmapFunctor_t(
+                [](vector3_t const & /* pos */) -> std::pair<float64_t, vector3_t>
                 {
-                    return {0.0, (vector3_t() << 0.0, 0.0, 1.0).finished()};
+                    return {0.0, vector3_t::UnitZ()};
                 });
 
             return config;
@@ -199,23 +202,29 @@ 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;
             float64_t const damping;
             float64_t const transitionEps;
             float64_t const friction;
+            float64_t const torsion;
             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"))),
             transitionVelocity(boost::get<float64_t>(options.at("transitionVelocity")))
             {
                 // Empty.
@@ -238,11 +247,11 @@ namespace jiminy
         struct worldOptions_t
         {
             vectorN_t const gravity;
-            heatMapFunctor_t const groundProfile;
+            heightmapFunctor_t const groundProfile;
 
             worldOptions_t(configHolder_t const & options) :
             gravity(boost::get<vectorN_t>(options.at("gravity"))),
-            groundProfile(boost::get<heatMapFunctor_t>(options.at("groundProfile")))
+            groundProfile(boost::get<heightmapFunctor_t>(options.at("groundProfile")))
             {
                 // Empty.
             }
@@ -515,8 +524,6 @@ namespace jiminy
         /// \return Contact force, at parent joint, in the local frame.
         void computeContactDynamicsAtBody(systemHolder_t const & system,
                                           pairIndex_t const & collisionPairIdx,
-                                          vectorN_t const & q,
-                                          vectorN_t const & v,
                                           std::shared_ptr<AbstractConstraintBase> & contactConstraint,
                                           pinocchio::Force & fextLocal) const;
 
@@ -527,8 +534,6 @@ namespace jiminy
         /// \return Contact force, at parent joint, in the local frame.
         void computeContactDynamicsAtFrame(systemHolder_t const & system,
                                            frameIndex_t const & frameIdx,
-                                           vectorN_t const & q,
-                                           vectorN_t const & v,
                                            std::shared_ptr<AbstractConstraintBase> & collisionConstraint,
                                            pinocchio::Force & fextLocal) const;
 
@@ -550,8 +555,6 @@ namespace jiminy
                                      vectorN_t                & uInternal) const;
         void computeCollisionForces(systemHolder_t     const & system,
                                     systemDataHolder_t       & systemData,
-                                    vectorN_t          const & q,
-                                    vectorN_t          const & v,
                                     forceVector_t            & fext) const;
         void computeExternalForces(systemHolder_t     const & system,
                                    systemDataHolder_t       & systemData,

core/include/jiminy/core/robot/AbstractMotor.h

@@ -101,9 +101,6 @@ namespace jiminy
         AbstractMotorBase(AbstractMotorBase const & abstractMotor) = delete;
         AbstractMotorBase & operator = (AbstractMotorBase const & other) = delete;
 
-        auto shared_from_this() { return shared_from(this); }
-        auto shared_from_this() const { return shared_from(this); }
-
         ///////////////////////////////////////////////////////////////////////////////////////////////
         /// \brief      Constructor
         ///

core/include/jiminy/core/robot/Model.h

@@ -251,9 +251,6 @@ namespace jiminy
         Model(void);
         ~Model(void) = default;
 
-        auto shared_from_this() { return shared_from(this); }
-        auto shared_from_this() const { return shared_from(this); }
-
         hresult_t initialize(std::string              const & urdfPath,
                              bool_t                   const & hasFreeflyer = true,
                              std::vector<std::string> const & meshPackageDirs = {});
@@ -332,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;
@@ -372,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);
@@ -400,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