diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index b1470ec3..b7ae4280 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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) { diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 461b0090..be0f2850 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -955,11 +955,11 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {