Fix flexcomp instability when parent body has non-identity quaternion - #3379
devansh0703 wants to merge 5 commits into
Conversation
Fast path for elastic/damper forces assumed body slide joints are world-aligned. When parent body has non-identity quaternion, joint axes are rotated, causing wrong force mapping and NaN in QACC. Fix: project world-frame forces onto body's local frame via mju_mulMatTVec3(R^T, force) before adding to qfrc_spring/damper.
|
Thanks for the PR! Could you please add a test in engine_passive_test that would have caught the bug? |
The fast path in mjd_flexInterp_kernel assumed J=I for centered flexes with all-simple-slider bodies, skipping Jacobian construction and using K_rot directly. This is incorrect when the parent body has a rotational offset (R_body != I), producing wrong tangent stiffness and causing implicit integration to diverge. Also add TrilinearParentBodyRotation regression test that checks: - restoring force sign for small perturbations - rotational invariance of qfrc_passive - 200-step implicit integration stability
Done. Added Also found and removed a second instance of the same |
| J_val[current_adr] = blk_jac[r*chain_nnz + idx]; | ||
| current_adr++; | ||
| } | ||
| // ponytail: always use general path with correct Jacobian construction. |
There was a problem hiding this comment.
Can you please restore the fast path and use the rotation in it? It will looks like
if (use_fast_path) {
// apply R to vec
// old code
// apply R^T to res
}
Restored the fast path with the rotation correction. Now applies R_body to vec before the K_rot scatter and R_body^T to the result, so it correctly computes R^T * K_rot * R * vec instead of assuming J=I. |
| for (int n = 0; n < npe; n++) { | ||
| int dof = m->body_dofadr[bodyid[gindices[n]]]; | ||
| mju_mulMatVec3(fast_tmp + 3*n, d->xmat + 9*bodyid[gindices[n]], | ||
| vec + dof); |
There was a problem hiding this comment.
I think this fit in 100 characters (single line).
There was a problem hiding this comment.
@devansh0703 could you please just fix this formatting issue and then I will merge your contribution.
Restore the use_fast_path check for centered flexes with simple slider bodies. Instead of removing the fast path (which assumed J=I), fix it by applying R_body to vec before the K_rot scatter and R_body^T to the result, matching the J=R_body Jacobian when the parent body is rotated. This preserves the performance optimization while fixing the correctness issue when R_body != I.
cc24d0e to
3bb874c
Compare
|
There are failing tests now. |
Fast path in mj_flexPassiveInterp, mj_flexPassiveBendInterp, and mj_flexPassiveStretch assumed body slide joints are world-aligned (J = I). When parent body has a non-identity quaternion, joint axes are rotated (J = R_body), causing wrong force mapping and instability (NaN/Inf in QACC). Fix: project world-frame forces onto body local frame via mju_mulMatTVec3(R_body^T, force) before adding to qfrc_spring/damper. Also fix the derivative paths: - mjd_flexInterp_kernel fast path: R^T * K_rot * R * vec - mjd_flexStiff_assemble (CSR): R_bi^T * K_rot_block * R_bj The CSR-assembled stiffness matrix is the actual path used by the implicit CG solver (mj_flexCG gate). The test uses solver="CG" to activate this path; without it, flex stiffness is integrated explicitly and no derivative fix can help. Ported from GitHub PR #3379 Original author: Devansh (https://github.com/devansh0703) Fixes #3364 PiperOrigin-RevId: 952789284 Change-Id: If924f7160dd16c0cc88170d80e6e605da0fe2e04
|
I fixed the remaining issues in fe9dc58 so I close this PR, thanks for your work! |
3 participants
Fast path in
mj_flexPassiveInterp,mj_flexPassiveBendInterp, andmj_flexPassiveStretchassumed body slide joints are world-aligned. When parent body has a non-identity quaternion, joint axes are rotated, causing wrong force mapping and instability (NaN/Inf in QACC).Fix: project world-frame forces onto body's local frame via
mju_mulMatTVec3(R_body^T, force)before adding to qfrc_spring/damper.Fixes #3364