Jiminy 1.7.0 (#411)

* [core] Move init of contact solver in reset instead of setOptions. * [core] Add support of torsional friction and contact normal to impulse contact model. * [core] Better default 'stabilizationFreq' for impulse contact model. * [core] 'engine_options.contacts.stabilizationFreq' only applies to contact constraints. * [core] Refactor 'JointConstraint' to be consistent with 'FixedFrameConstraint'. * [python/robot] Fix support of URDF model with a single link for automatic hardware config generation. * [python/viewer] 'update_floor' method in panda3d now expects numpy array instead of callable to fix serialization issues. * [python/viewer] Add support of contact frames display. And method to render floor based on . * [python/viewer] Add option to enable/disable display of ground profile meshes. * [python/viewer] Speedup rendering of ground profile and improve visual by providing normal to panda3d. * [python/viewer] Add support of ipykernel 6 for meshcat backend interactive display. * [python/simulator] 'replay' now display the ground profile used by the engine. * [python/simulator] Do not reset camera pose if gui already available. * [python/dynamics] Circumvent weird segfault in pinocchio bindings of 'centerOfMass' method. * [python/generator] Add random tile ground profile generator. * [gym/common] Fix automatic telemetry registration of the action. * [gym/rllib] Add temporal barrier regularizer to PPO. * [gym/envs] Set baumgarte frequency for cassie internal constraints. * [misc] Enforce scipy>=1.2.0 to fix buggy optimizer. * [misc] Update 'typing_extensions' required version to fix pylint support for Python 3.6. Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
duburcqa · Oct 10, 2021 · 792278b · 792278b
1 parent b5b9b70
commit 792278b
Show file tree

Hide file tree

Showing 31 changed files with 711 additions and 342 deletions.
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.6.30)
+set(BUILD_VERSION 1.7.0)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

diff --git a/core/include/jiminy/core/Types.h b/core/include/jiminy/core/Types.h
@@ -67,10 +67,10 @@ 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 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;

diff --git a/core/include/jiminy/core/constraints/FixedFrameConstraint.h b/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.
     };
 }
 

diff --git a/core/include/jiminy/core/constraints/JointConstraint.h b/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;

diff --git a/core/include/jiminy/core/constraints/SphereConstraint.h b/core/include/jiminy/core/constraints/SphereConstraint.h
@@ -50,7 +50,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;

diff --git a/core/include/jiminy/core/constraints/WheelConstraint.h b/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;

diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h
@@ -117,11 +117,12 @@ namespace jiminy
             config["model"] = std::string("spring_damper");   // ["spring_damper", "impulse"]
             config["solver"] = std::string("PGS");   // ["PGS",]
             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;
@@ -205,6 +206,7 @@ namespace jiminy
             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) :
@@ -216,6 +218,7 @@ namespace jiminy
             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.
@@ -515,8 +518,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 +528,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 +549,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,

diff --git a/core/src/constraints/FixedFrameConstraint.cc b/core/src/constraints/FixedFrameConstraint.cc
@@ -12,14 +12,13 @@ namespace jiminy
     std::string const AbstractConstraintTpl<FixedFrameConstraint>::type_("FixedFrameConstraint");
 
     FixedFrameConstraint::FixedFrameConstraint(std::string const & frameName,
-                                               Eigen::Matrix<bool_t, 6, 1> const & maskFixed,
-                                               pinocchio::ReferenceFrame const & frameRef) :
+                                               Eigen::Matrix<bool_t, 6, 1> const & maskFixed) :
     AbstractConstraintTpl(),
     frameName_(frameName),
     frameIdx_(0),
-    frameRef_(frameRef),
     dofsFixed_(),
     transformRef_(),
+    rotationLocal_(),
     frameJacobian_(),
     frameDrift_()
     {
@@ -50,11 +49,6 @@ namespace jiminy
         return frameIdx_;
     }
 
-    pinocchio::ReferenceFrame const & FixedFrameConstraint::getReferenceFrame(void) const
-    {
-        return frameRef_;
-    }
-
     std::vector<uint32_t> const & FixedFrameConstraint::getDofsFixed(void) const
     {
         return dofsFixed_;
@@ -65,11 +59,21 @@ namespace jiminy
         transformRef_ = transformRef;
     }
 
-    pinocchio::SE3 & FixedFrameConstraint::getReferenceTransform(void)
+    pinocchio::SE3 const & FixedFrameConstraint::getReferenceTransform(void) const
     {
         return transformRef_;
     }
 
+    void FixedFrameConstraint::setLocalFrame(matrix3_t const & frameRot)
+    {
+        rotationLocal_ = frameRot;
+    }
+
+    matrix3_t const & FixedFrameConstraint::getLocalFrame(void) const
+    {
+        return rotationLocal_;
+    }
+
     hresult_t FixedFrameConstraint::reset(vectorN_t const & /* q */,
                                           vectorN_t const & /* v */)
     {
@@ -92,7 +96,7 @@ namespace jiminy
         if (returnCode == hresult_t::SUCCESS)
         {
             // Initialize jacobian, drift and multipliers
-            frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv);
+            frameJacobian_ = matrix6N_t::Zero(6, model->pncModel_.nv);
             frameDrift_ = vector6_t::Zero();
             uint64_t const dim = dofsFixed_.size();
             jacobian_ = matrixN_t::Zero(dim, model->pncModel_.nv);
@@ -101,6 +105,9 @@ namespace jiminy
 
             // Get the current frame position and use it as reference
             transformRef_ = model->pncData_.oMf[frameIdx_];
+
+            // Set local frame to world by default
+            rotationLocal_.setIdentity();
         }
 
         return returnCode;
@@ -118,40 +125,53 @@ namespace jiminy
         // Assuming the model still exists.
         auto model = model_.lock();
 
-        // Get jacobian
+        // Define inverse rotation matrix of local frame
+        auto rotInvLocal = rotationLocal_.transpose();
+
+        // Get jacobian in local frame
         getFrameJacobian(model->pncModel_,
                          model->pncData_,
                          frameIdx_,
-                         frameRef_,
+                         pinocchio::LOCAL_WORLD_ALIGNED,
                          frameJacobian_);
 
-        // Get drift
+        pinocchio::Frame const & frame = model->pncModel_.frames[frameIdx_];
+        pinocchio::JointModel const & joint = model->pncModel_.joints[frame.parent];
+        int32_t const colRef = joint.nv() + joint.idx_v() - 1;
+        for(Eigen::DenseIndex j=colRef; j>=0; j=model->pncData_.parents_fromRow[j])
+        {
+            pinocchio::MotionRef<matrix6N_t::ColXpr> J_col(frameJacobian_.col(j));
+            J_col.linear() = rotInvLocal * J_col.linear();
+            J_col.angular() = rotInvLocal * J_col.angular();
+        }
+
+        // Get drift in world frame
         frameDrift_ = getFrameAcceleration(model->pncModel_,
                                            model->pncData_,
                                            frameIdx_,
-                                           frameRef_).toVector();
-
-        // Add Baumgarte stabilization drift
-        if (frameRef_ == pinocchio::LOCAL_WORLD_ALIGNED || frameRef_ == pinocchio::WORLD)
-        {
-            auto deltaPosition = model->pncData_.oMf[frameIdx_].translation() - transformRef_.translation();
-            frameDrift_.head<3>() += kp_ * deltaPosition;
-            auto deltaRotation = transformRef_.rotation().transpose() * model->pncData_.oMf[frameIdx_].rotation();
-            vectorN_t const axis = pinocchio::log3(deltaRotation);
-            frameDrift_.tail<3>() += kp_ * axis;
-        }
-        vector6_t const velocity = getFrameVelocity(model->pncModel_,
-                                                    model->pncData_,
-                                                    frameIdx_,
-                                                    frameRef_).toVector();
+                                           pinocchio::LOCAL_WORLD_ALIGNED);
+
+        // Add Baumgarte stabilization to drift in world frame
+        vector3_t const deltaPosition = model->pncData_.oMf[frameIdx_].translation() - transformRef_.translation();
+        matrix3_t const deltaRotation = transformRef_.rotation().transpose() * model->pncData_.oMf[frameIdx_].rotation();
+        frameDrift_.linear() += kp_ * deltaPosition;
+        frameDrift_.angular() += kp_ * pinocchio::log3(deltaRotation);
+        pinocchio::Motion const velocity = getFrameVelocity(model->pncModel_,
+                                                            model->pncData_,
+                                                            frameIdx_,
+                                                            pinocchio::LOCAL_WORLD_ALIGNED);
         frameDrift_ += kd_ * velocity;
 
+        // Compute drift in local frame
+        frameDrift_.linear() = rotInvLocal * frameDrift_.linear();
+        frameDrift_.angular() = rotInvLocal * frameDrift_.angular();
+
         // Extract masked jacobian and drift, only containing fixed dofs
         for (uint32_t i=0; i < dofsFixed_.size(); ++i)
         {
             uint32_t const & dofIndex = dofsFixed_[i];
             jacobian_.row(i) = frameJacobian_.row(dofIndex);
-            drift_[i] = frameDrift_[dofIndex];
+            drift_[i] = frameDrift_.toVector()[dofIndex];
         }
 
         return hresult_t::SUCCESS;

diff --git a/core/src/constraints/JointConstraint.cc b/core/src/constraints/JointConstraint.cc
@@ -34,7 +34,12 @@ namespace jiminy
         return jointIdx_;
     }
 
-    vectorN_t & JointConstraint::getReferenceConfiguration(void)
+    void JointConstraint::setReferenceConfiguration(vectorN_t const & configurationRef)
+    {
+        configurationRef_ = configurationRef;
+    }
+
+    vectorN_t const & JointConstraint::getReferenceConfiguration(void) const
     {
         return configurationRef_;
     }

diff --git a/core/src/constraints/SphereConstraint.cc b/core/src/constraints/SphereConstraint.cc
@@ -46,7 +46,7 @@ namespace jiminy
         transformRef_ = transformRef;
     }
 
-    pinocchio::SE3 & SphereConstraint::getReferenceTransform(void)
+    pinocchio::SE3 const & SphereConstraint::getReferenceTransform(void) const
     {
         return transformRef_;
     }

diff --git a/core/src/constraints/WheelConstraint.cc b/core/src/constraints/WheelConstraint.cc
@@ -50,7 +50,7 @@ namespace jiminy
         transformRef_ = transformRef;
     }
 
-    pinocchio::SE3 & WheelConstraint::getReferenceTransform(void)
+    pinocchio::SE3 const & WheelConstraint::getReferenceTransform(void) const
     {
         return transformRef_;
     }