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

Add engine-internal convenience macro for allocating typed arrays, improve error message.

Browse Source 
PiperOrigin-RevId: 705126655
Change-Id: I2bd8fada6d33a919d2fb82297f93ac57958355a4
This commit is contained in:
Yuval Tassa
2024-12-11 09:03:39 -08:00
committed by Copybara-Service
parent 3ca97248a3
commit 2691887500
25 changed files with 283 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_inverse.c
+9 -9
View File
@@ -76,7 +76,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjtNum *qacc = d->qacc;
mj_markStack(d);
mjtNum* qfrc = mj_stackAllocNum(d, nv);
mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum);
// use selected integrator
switch ((mjtIntegrator) m->opt.integrator) {
@@ -132,11 +132,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjd_smooth_vel(m, d, /* flg_bias = */ 0);
// save mass matrix
mjtNum* qMsave = mj_stackAllocNum(d, m->nM);
mjtNum* qMsave = mjSTACKALLOC(d, m->nM, mjtNum);
mju_copy(qMsave, d->qM, m->nM);
// set M = M - dt*qDeriv (reduced to M nonzeros)
mjtNum* qDerivReduced = mj_stackAllocNum(d, m->nM);
mjtNum* qDerivReduced = mjSTACKALLOC(d, m->nM, mjtNum);
for (int i=0; i < nM; i++) {
qDerivReduced[i] = d->qDeriv[d->mapD2M[i]];
}
@@ -171,7 +171,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
}
mj_markStack(d);
mjtNum* jar = mj_stackAllocNum(d, nefc);
mjtNum* jar = mjSTACKALLOC(d, nefc, mjtNum);
// compute jar = Jac*qacc - aref
mj_mulJacVec(m, d, jar, d->qacc);
@@ -218,7 +218,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
if (mjENABLED(mjENBL_INVDISCRETE)) {
// save current qacc
qacc = mj_stackAllocNum(d, nv);
qacc = mjSTACKALLOC(d, nv, mjtNum);
mju_copy(qacc, d->qacc, nv);
// modify qacc in-place
@@ -271,10 +271,10 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
// allocate
mj_markStack(d);
qforce = mj_stackAllocNum(d, nv);
dif = mj_stackAllocNum(d, nv);
save_qfrc_constraint = mj_stackAllocNum(d, nv);
save_efc_force = mj_stackAllocNum(d, nefc);
qforce = mjSTACKALLOC(d, nv, mjtNum);
dif = mjSTACKALLOC(d, nv, mjtNum);
save_qfrc_constraint = mjSTACKALLOC(d, nv, mjtNum);
save_efc_force = mjSTACKALLOC(d, nefc, mjtNum);
// qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator
// should equal result of inverse dynamics