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

Rotate undeformed normals before using them in the bending passive forces.

Browse Source 
The normal jump residual in flex elasticity calculations now rotates the rest-frame normal jump into the current frame using the face's corotational quaternion before subtracting the jump from the current normal difference.

PiperOrigin-RevId: 913242127
Change-Id: Ia62b28ecccd59e79225737d8225ef4131b333c96
This commit is contained in:
Alessio Quaglino
2026-05-10 02:59:22 -07:00
committed by Copybara-Service
parent cb5a9caa5c
commit 5ee8bd7b9c
2 changed files with 70 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_passive.c
+22 -5
View File
@@ -291,15 +291,16 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f,
if (stiffness == 0) continue;
// gather face A and B positions + velocities
// gather face A and B positions + corotational quats
mjtNum quat_A[4], quat_B[4];
mju_flexGatherFaceState(order, cx, cy, cz, fe_A, xpos_g,
enbl_damper ? vel_g : NULL, NULL,
xpos_A, enbl_damper ? vel_A : NULL, NULL,
gidx_A, NULL);
gidx_A, quat_A);
mju_flexGatherFaceState(order, cx, cy, cz, fe_B, xpos_g,
enbl_damper ? vel_g : NULL, NULL,
xpos_B, enbl_damper ? vel_B : NULL, NULL,
gidx_B, NULL);
gidx_B, quat_B);
// compute deformed normals at edge midpoint
mjtNum n_A[3], t1_A[3], t2_A[3];
@@ -316,10 +317,26 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f,
n_A[0] *= inv_A; n_A[1] *= inv_A; n_A[2] *= inv_A;
n_B[0] *= inv_B; n_B[1] *= inv_B; n_B[2] *= inv_B;
// normal jump residual: r = (n_A - n_B) - dn0
// average corotational frame: symmetric under face swap
// quat_A and quat_B encode R^{-1}; average them, then negate to get R_avg
// ensure quaternions are in the same hemisphere before averaging
if (mju_dot(quat_A, quat_B, 4) < 0) {
mju_scl(quat_B, quat_B, -1, 4);
}
mjtNum quat_avg[4];
mju_add(quat_avg, quat_A, quat_B, 4);
mju_normalize(quat_avg, 4); // NLERP = SLERP at t=0.5 for two quaternions
// negate to get R_avg (from rest frame to current frame)
mju_negQuat(quat_avg, quat_avg);
// rotate dn0 from rest frame to current frame using average corotational R
mjtNum dn0_rot[3];
mju_rotVecQuat(dn0_rot, dn0, quat_avg);
// normal jump residual: r = (n_A - n_B) - R_avg * dn0
mjtNum r[3];
mji_sub3(r, n_A, n_B);
r[0] -= dn0[0]; r[1] -= dn0[1]; r[2] -= dn0[2];
r[0] -= dn0_rot[0]; r[1] -= dn0_rot[1]; r[2] -= dn0_rot[2];
// --- spring force ---
if (enbl_spring) {