[core] Compensate PGS bias to ensure maximum energy dissipation for tangential friction.

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

@@ -63,11 +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/src/constraints/FixedFrameConstraint.cc

@@ -1,4 +1,5 @@
-#include "pinocchio/algorithm/frames.hpp"  // `pinocchio::getFrameVelocity`, `pinocchio::getFrameAcceleration`
+#include "pinocchio/algorithm/frames.hpp"    // `pinocchio::getFrameVelocity`, `pinocchio::getFrameAcceleration`
+#include "pinocchio/algorithm/cholesky.hpp"  // `pinocchio::cholesky::`
 
 #include "jiminy/core/robot/Model.h"
 #include "jiminy/core/utilities/Pinocchio.h"
@@ -17,11 +18,13 @@ namespace jiminy
     frameName_(frameName),
     frameIdx_(0),
     dofsFixed_(),
+    isFixedPositionXY_(maskFixed.head<2>().all()),
     transformRef_(),
     normal_(),
     rotationLocal_(matrix3_t::Identity()),
     frameJacobian_(),
-    frameDrift_()
+    frameDrift_(),
+    UiJt_()
     {
         dofsFixed_.resize(static_cast<std::size_t>(maskFixed.cast<int32_t>().array().sum()));
         uint32_t dofIndex = 0;
@@ -155,16 +158,33 @@ namespace jiminy
                                            frameIdx_,
                                            pinocchio::LOCAL_WORLD_ALIGNED);
 
-        // Add Baumgarte stabilization to drift in world frame
+        // Compute pose error
         pinocchio::SE3 const & framePose = model->pncData_.oMf[frameIdx_];
-        vector3_t const deltaPosition = framePose.translation() - transformRef_.translation();
+        vector3_t deltaPosition = framePose.translation() - transformRef_.translation();
         matrix3_t const deltaRotation = transformRef_.rotation().transpose() * framePose.rotation();
+
+        // Compute velocity error
+        pinocchio::Motion velocity = getFrameVelocity(model->pncModel_,
+                                                      model->pncData_,
+                                                      frameIdx_,
+                                                      pinocchio::LOCAL_WORLD_ALIGNED);
+
+        // Correct bias of PGS solver in tangential plane
+        if (isFixedPositionXY_)
+        {
+            UiJt_ = frameJacobian_.topRows<2>().transpose();
+            pinocchio::cholesky::Uiv(model->pncModel_, model->pncData_, UiJt_);
+            vector2_t a;
+            a << model->pncData_.D[1] * UiJt_.col(0).squaredNorm(),
+                 model->pncData_.D[0] * UiJt_.col(1).squaredNorm();
+            a /= a.array().maxCoeff();
+            velocity.linear().head<2>().array() *= a.array();
+            deltaPosition.head<2>().array() *= a.array();
+        }
+
+        // Add Baumgarte stabilization to drift in world frame
         frameDrift_.linear() += kp_ * deltaPosition;
         frameDrift_.angular() += kp_ * framePose.rotation() * 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

core/src/engine/EngineMultiRobot.cc

@@ -3785,9 +3785,6 @@ namespace jiminy
             // Compute non-linear effects
             pinocchio::nonLinearEffects(model, data, q, v);
 
-            // Compute inertia matrix, adding rotor inertia
-            pinocchio_overload::crba(model, data, q);
-
             // Call forward dynamics
             constraintSolver_->BoxedForwardDynamics(model,
                                                     data,

core/src/robot/Model.cc

@@ -1193,6 +1193,13 @@ namespace jiminy
         // Compute joint jacobian manually since not done by engine for efficiency
         pinocchio::computeJointJacobians(pncModel_, pncData_);
 
+        // Compute inertia matrix, taking into account armature
+        pinocchio_overload::crba(pncModel_, pncData_, q);
+
+        // Compute the mass matrix decomposition, since it may be used for
+        // constraint stabilization.
+        pinocchio::cholesky::decompose(pncModel_, pncData_);
+
         /* Computing forward kinematics without acceleration to get the drift.
            Note that it will alter the actual joints spatial accelerations, so
            it is necessary to do a backup first to restore it later on. */

core/src/solver/LCPSolvers.cc

@@ -155,12 +155,16 @@ namespace jiminy
         vectorN_t & f = data.lambda_c;
 
         // Compute JMinvJt, including cholesky decomposition of inertia matrix
-        matrixN_t & A = pinocchio_overload::computeJMinvJt(model, data, J, true);
+        matrixN_t & A = pinocchio_overload::computeJMinvJt(model, data, J, false);
 
         // Compute the dynamic drift (control - nle)
         data.torque_residual = tau - data.nle;
         pinocchio::cholesky::solve(model, data, data.torque_residual);
 
+        // Compute b
+        b_.noalias() = - J * data.torque_residual;
+        b_ -= gamma;
+
         /* Add regularization term in case A is not inversible.
            Note that Mujoco defines an impedance function that depends on
            the distance instead of a constant value to model soft contacts.
@@ -171,10 +175,6 @@ namespace jiminy
             PGS_MIN_REGULARIZER,
             INF);
 
-        // Compute b
-        b_.noalias() = - J * data.torque_residual;
-        b_ -= gamma;
-
         // Compute resulting forces solving forward dynamics
         bool_t isSuccess = false;
         if (lo.array().isInf().all() && hi.array().isInf().all())

python/jiminy_py/src/jiminy_py/dynamics.py

@@ -658,20 +658,20 @@ def compute_inverse_dynamics(robot: jiminy.Model,
         pnc_model, pnc_data, position, velocity, acceleration)
     pin.updateFramePlacements(pnc_model, pnc_data)
 
-    # Compute inverted inertia matrix, taking into account rotor inertias
-    jiminy.crba(pnc_model, pnc_data, position)
-    pin.cholesky.decompose(pnc_model, pnc_data)
+    # Compute constraint jacobian and drift
+    robot.compute_constraints(position, velocity)
+    J = robot.get_constraints_jacobian()
+    drift = robot.get_constraints_drift()
+
+    # No need to compute the internal matrix using `crba` nor to perform the
+    # cholesky decomposition since it is already done by `compute_constraints`
+    # internally.
     M_inv = pin.cholesky.computeMinv(pnc_model, pnc_data)
 
     # Compute non-linear effects
     pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
     nle = pnc_data.nle
 
-    # Compute constraint jacobian and drift
-    robot.compute_constraints(position, velocity)
-    J = robot.get_constraints_jacobian()
-    drift = robot.get_constraints_drift()
-
     # Compute constraint forces
     jiminy.computeJMinvJt(pnc_model, pnc_data, J, False)
     a_f = jiminy.solveJMinvJtv(pnc_data, - drift + J @ M_inv @ nle)

unit_py/utilities.py

@@ -180,7 +180,8 @@ def simulate_and_get_state_evolution(
            name = system.name
            q0[name] = x0[name][:system.robot.nq]
            v0[name] = x0[name][-system.robot.nv:]
-    engine.simulate(tf, q0, v0)
+    hresult = engine.simulate(tf, q0, v0)
+    assert hresult == jiminy.hresult_t.SUCCESS
 
     # Get log data
     log_data, _ = engine.get_log()