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

Add mjModel.ngravcomp, number of bodies with nonzero gravcomp attribute.

Browse Source 
PiperOrigin-RevId: 627087335
Change-Id: Ie85ae4150962463b8a8c8435fec068c44877aa16
This commit is contained in:
Yuval Tassa
2024-04-22 10:44:33 -07:00
committed by Copybara-Service
parent 546a27ca72
commit 8ebc9ed2c8
8 changed files with 37 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+20 -21
View File
@@ -256,6 +256,23 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i
// return number of dofs given joint type
static int jnt_dofnum(mjtJoint type) {
switch (type) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
return 1;
case mjJNT_BALL:
return 3;
case mjJNT_FREE:
return 6;
}
}
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
@@ -469,7 +486,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// actuator-level gravity compensation
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added as passive force
@@ -477,28 +494,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofnum = jnt_dofnum(m->jnt_type[i]);
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum);
}
}