From b1a941db8b1e22ef93c01eb0f0744ece1abfdf6b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 10 Mar 2024 05:33:56 -0700 Subject: [PATCH] Add special handling for perturbations applied to static bodies. These bodies have `body_invweight0 == 0` (infinite mass) and no degrees of freedom (identically 0 Jacobians). This was breaking some assumptions of the perturbation code which assumed perturbation applied only to moving bodies and led to very large forces/torques being applied. While these huge forces had no effect on the dynamics, they were reported by sensors, leading to unreasonable sensor readings. PiperOrigin-RevId: 614398354 Change-Id: If5efe1c9e57e2be28af33d109679db8b3129da7f --- src/engine/engine_vis_interact.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 6eacc958..62eec2af 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -543,7 +543,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mjtNum invmass = mju_dot(jacM2+0*nv, jacM2+0*nv, nv) + mju_dot(jacM2+1*nv, jacM2+1*nv, nv) + mju_dot(jacM2+2*nv, jacM2+2*nv, nv); - pert->localmass = 3 / mju_max(invmass, mjMINVAL); + pert->localmass = (invmass == 0) ? 1 : 3 / mju_max(invmass, mjMINVAL); // scale localmass with flex average number of edges per vertex if (pert->flexselect >= 0 && !m->flex_rigid[pert->flexselect]) { @@ -642,7 +642,8 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) mjtNum *body_rotvel = bvel; // body rotational inertia - mjtNum inertia = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel+1]); + mjtNum invweight = m->body_invweight0[2*sel+1]; + mjtNum inertia = invweight ? 1.0/mju_max(mjMINVAL, invweight) : 1; if (((pert->active | pert->active2) & mjPERT_TRANSLATE)) { // compute selection point in world coordinates