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

Precompute mappings between M <-> D sparse representations.

Browse Source 
PiperOrigin-RevId: 670898353
Change-Id: I4b15477b1b6eb4a64cf7d4a0bb5af8fcb95d4e45
This commit is contained in:
Yuval Tassa
2024-09-04 02:56:05 -07:00
committed by Copybara-Service
parent 303e5e7718
commit 42eb669e4e
11 changed files with 156 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_inverse.c
+7 -3
View File
@@ -71,7 +71,7 @@ void mj_invVelocity(const mjModel* m, mjData* d) {
// convert discrete-time qacc to continuous-time qacc
static void mj_discreteAcc(const mjModel* m, mjData* d) {
int nv = m->nv, dof_damping;
int nv = m->nv, nM = m->nM, nD = m->nD, dof_damping;
mjtNum *qacc = d->qacc;
mj_markStack(d);
@@ -114,7 +114,9 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// set qLU = qM
mj_copyM2DSparse(m, d, d->qLU, d->qM);
for (int i=0; i < nD; i++) {
d->qLU[i] = d->qM[d->mapM2D[i]];
}
// set qLU = qM - dt*qDeriv
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
@@ -134,7 +136,9 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set M = M - dt*qDeriv (reduced to M nonzeros)
mjtNum* qDerivReduced = mj_stackAllocNum(d, m->nM);
mj_copyD2MSparse(m, d, qDerivReduced, d->qDeriv);
for (int i=0; i < nM; i++) {
qDerivReduced[i] = d->qDeriv[d->mapD2M[i]];
}
mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM);
// set qfrc = (M - dt*qDeriv) * qacc