diff --git a/core/include/jiminy/core/constraints/FixedFrameConstraint.h b/core/include/jiminy/core/constraints/FixedFrameConstraint.h index f02ce4474..d96d3b763 100644 --- a/core/include/jiminy/core/constraints/FixedFrameConstraint.h +++ b/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 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} }; } diff --git a/core/src/constraints/FixedFrameConstraint.cc b/core/src/constraints/FixedFrameConstraint.cc index d50106f3b..ef2c5b82b 100644 --- a/core/src/constraints/FixedFrameConstraint.cc +++ b/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(maskFixed.cast().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 diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 13c146ac1..6e7872902 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/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, diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 2445c44ec..53c504452 100644 --- a/core/src/robot/Model.cc +++ b/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. */ diff --git a/core/src/solver/LCPSolvers.cc b/core/src/solver/LCPSolvers.cc index 0d02dcacf..c8d708d6f 100644 --- a/core/src/solver/LCPSolvers.cc +++ b/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()) diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index b986dcff0..4b61b6c8f 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/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) diff --git a/unit_py/utilities.py b/unit_py/utilities.py index d851338e1..1ccda18d5 100644 --- a/unit_py/utilities.py +++ b/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()