chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add special handling for perturbations applied to static bodies.

Browse Source 
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
This commit is contained in:
Yuval Tassa
2024-03-10 05:33:56 -07:00
committed by Copybara-Service
parent 2ba80206ab
commit b1a941db8b
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_interact.c
+3 -2
View File
@@ -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