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) {
test/engine/engine_passive_test.cc
+48 -16
View File
@@ -955,11 +955,11 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1"
dim="3" cellcount="2 2 1" radius=".001" rgba="0 .7 .7 1"
<flexcomp type="grid" count="8 2 12" spacing=".025 .05 .025" pos="0 0 1"
dim="3" cellcount="6 1 6" radius=".001" rgba="0 .7 .7 1"
mass="5" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0"
elastic2d="bend" thickness="0.02"/>
<elasticity young="1e5" poisson="0.3" damping="0"
elastic2d="bend" thickness="0.03"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
@@ -971,25 +971,57 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
// apply a rigid rotation by setting all body quats to a 30 degree rotation
// about z-axis (all flex node bodies get the same rotation)
mjtNum angle = 30 * 3.14159265358979 / 180.0;
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
// compute geometric center from body positions (skip world body)
mjtNum center[3] = {0, 0, 0};
int nnodes = 0;
for (int b = 1; b < m->nbody; b++) {
int qadr = m->jnt_qposadr[m->body_jntadr[b]];
if (m->body_jntnum[b] > 0 && m->jnt_type[m->body_jntadr[b]] == mjJNT_FREE) {
d->qpos[qadr + 3] = ca;
d->qpos[qadr + 4] = 0;
d->qpos[qadr + 5] = 0;
d->qpos[qadr + 6] = sa;
center[0] += m->body_pos[3*b + 0];
center[1] += m->body_pos[3*b + 1];
center[2] += m->body_pos[3*b + 2];
nnodes++;
}
ASSERT_GT(nnodes, 0);
center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes;
// rotation: 45 degrees about (1,1,1)/sqrt(3)
mjtNum angle = 45 * 3.14159265358979 / 180.0;
mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2);
mjtNum inv_sqrt3 = 1.0 / mju_sqrt(3.0);
mjtNum quat[4] = {ca, sa * inv_sqrt3, sa * inv_sqrt3, sa * inv_sqrt3};
mjtNum neg_quat[4];
mju_negQuat(neg_quat, quat);
// apply rigid rotation via slide joint displacements:
// new_pos = center + R * (body_pos - center)
// qpos = new_pos - body_pos
for (int b = 1; b < m->nbody; b++) {
mjtNum rel[3] = {m->body_pos[3*b+0] - center[0],
m->body_pos[3*b+1] - center[1],
m->body_pos[3*b+2] - center[2]};
mjtNum rotated[3];
mju_rotVecQuat(rotated, rel, neg_quat);
// each body has 3 slide joints (x, y, z)
for (int j = 0; j < m->body_jntnum[b] && j < 3; j++) {
int jid = m->body_jntadr[b] + j;
int qadr = m->jnt_qposadr[jid];
int axis = -1;
for (int a = 0; a < 3; a++) {
if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; }
}
if (axis >= 0) {
d->qpos[qadr] =
(center[axis] + rotated[axis]) - m->body_pos[3 * b + axis];
}
}
}
mj_forward(m, d);
// spring forces should still be zero (or very small) after rigid rotation
// spring forces should still be zero after rigid rotation
constexpr mjtNum tol = MjTol(1e-6, 1e-3);
for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-6)
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
<< "nonzero spring force at DOF " << i << " after rigid rotation";
}