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

Clean up mj_jacSparse.

Browse Source 
PiperOrigin-RevId: 725994792
Change-Id: I928a6312a6910231bfeb96ef7a3f29f1d128565c
This commit is contained in:
Yuval Tassa
2025-02-12 04:04:59 -08:00
committed by Copybara-Service
parent 7af9bcbf00
commit faed1a1137
1 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+15 -15
View File
@@ -15,7 +15,6 @@
#include "engine/engine_support.h" #include "engine/engine_support.h"
#include <stddef.h> #include <stddef.h>
#include <string.h>
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
@@ -527,9 +526,6 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
void mj_jacSparse(const mjModel* m, const mjData* d, void mj_jacSparse(const mjModel* m, const mjData* d,
mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body, mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
int NV, const int* chain) { int NV, const int* chain) {
int da, ci;
mjtNum offset[3], tmp[3], *cdof = d->cdof;
// clear jacobians // clear jacobians
if (jacp) { if (jacp) {
mju_zero(jacp, 3*NV); mju_zero(jacp, 3*NV);
@@ -539,6 +535,7 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
} }
// compute point-com offset // compute point-com offset
mjtNum offset[3];
mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]); mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
// skip fixed bodies // skip fixed bodies
@@ -552,10 +549,10 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
} }
// get last dof that affects this (as well as the original) body // get last dof that affects this (as well as the original) body
da = m->body_dofadr[body] + m->body_dofnum[body] - 1; int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
// start and the end of the chain (chain is in increasing order) // start and the end of the chain (chain is in increasing order)
ci = NV-1; int ci = NV-1;
// backward pass over dof ancestor chain // backward pass over dof ancestor chain
while (da >= 0) { while (da >= 0) {
@@ -569,20 +566,23 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
mjERROR("dof index %d not found in chain", da); mjERROR("dof index %d not found in chain", da);
} }
const mjtNum* cdof = d->cdof + 6*da;
// construct rotation jacobian // construct rotation jacobian
if (jacr) { if (jacr) {
jacr[ci] = cdof[6*da]; jacr[ci+0*NV] = cdof[0];
jacr[ci+NV] = cdof[6*da+1]; jacr[ci+1*NV] = cdof[1];
jacr[ci+2*NV] = cdof[6*da+2]; jacr[ci+2*NV] = cdof[2];
} }
// construct translation jacobian (correct for rotation) // construct translation jacobian (correct for rotation)
if (jacp) { if (jacp) {
mju_cross(tmp, cdof+6*da, offset); mjtNum tmp[3];
mju_cross(tmp, cdof, offset);
jacp[ci] = cdof[6*da+3] + tmp[0]; jacp[ci+0*NV] = cdof[3] + tmp[0];
jacp[ci+NV] = cdof[6*da+4] + tmp[1]; jacp[ci+1*NV] = cdof[4] + tmp[1];
jacp[ci+2*NV] = cdof[6*da+5] + tmp[2]; jacp[ci+2*NV] = cdof[5] + tmp[2];
} }
// advance to parent dof // advance to parent dof
@@ -596,9 +596,8 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
void mj_jacSparseSimple(const mjModel* m, const mjData* d, void mj_jacSparseSimple(const mjModel* m, const mjData* d,
mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point, mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point,
int body, int flg_second, int NV, int start) { int body, int flg_second, int NV, int start) {
mjtNum offset[3], tmp[3];
// compute point-com offset // compute point-com offset
mjtNum offset[3];
mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]); mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
// skip fixed body // skip fixed body
@@ -631,6 +630,7 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d,
// construct translation jacobian (correct for rotation) // construct translation jacobian (correct for rotation)
if (jacdifp) { if (jacdifp) {
mjtNum tmp[3];
mju_cross(tmp, cdof, offset); mju_cross(tmp, cdof, offset);
// plus sign // plus sign