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

Fix rotation Jacobian offset in mj_jacSum sparse path.

Browse Source 
PiperOrigin-RevId: 933640368
Change-Id: Ib30acd651b6022b0835e07b3f3b01e648791a511
This commit is contained in:
Yuval Tassa
2026-06-17 04:23:16 -07:00
committed by Copybara-Service
parent cffb077c4b
commit 38c3388046
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_util.c
+5 -2
View File
@@ -520,12 +520,10 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
const mjtNum point[3], mjtNum* jac, int flg_rot) { const mjtNum point[3], mjtNum* jac, int flg_rot) {
int nv = m->nv, NV; int nv = m->nv, NV;
mjtNum* jacp = jac; mjtNum* jacp = jac;
mjtNum* jacr = flg_rot ? jac + 3*nv : NULL;
mj_markStack(d); mj_markStack(d);
mjtNum* jtmp = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum); mjtNum* jtmp = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum);
mjtNum* jp = jtmp; mjtNum* jp = jtmp;
mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL;
// sparse // sparse
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
@@ -537,6 +535,7 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
NV = mj_bodyChain(m, body[0], chain); NV = mj_bodyChain(m, body[0], chain);
if (NV) { if (NV) {
// get Jacobian // get Jacobian
mjtNum* jacr = flg_rot ? jac + 3*NV : NULL;
if (m->body_simple[body[0]]) { if (m->body_simple[body[0]]) {
mj_jacSparseSimple(m, d, jacp, jacr, point, body[0], 1, NV, 0); mj_jacSparseSimple(m, d, jacp, jacr, point, body[0], 1, NV, 0);
} else { } else {
@@ -554,6 +553,7 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
if (!bodyNV) { if (!bodyNV) {
continue; continue;
} }
mjtNum* jr = flg_rot ? jtmp + 3*bodyNV : NULL;
if (m->body_simple[body[i]]) { if (m->body_simple[body[i]]) {
mj_jacSparseSimple(m, d, jp, jr, point, body[i], 1, bodyNV, 0); mj_jacSparseSimple(m, d, jp, jr, point, body[i], 1, bodyNV, 0);
} else { } else {
@@ -568,6 +568,9 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
// dense // dense
else { else {
mjtNum* jacr = flg_rot ? jac + 3*nv : NULL;
mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL;
// set first // set first
mj_jac(m, d, jacp, jacr, point, body[0]); mj_jac(m, d, jacp, jacr, point, body[0]);
mju_scl(jac, jac, weight[0], flg_rot ? 6*nv : 3*nv); mju_scl(jac, jac, weight[0], flg_rot ? 6*nv : 3*nv);