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

Replace function with static array in engine_forward.c

Browse Source 
PiperOrigin-RevId: 663268322
Change-Id: Ibd161a8eda55fc04cf37e465f0d0ac0adcfc7847
This commit is contained in:
Yuval Tassa
2024-08-15 05:10:37 -07:00
committed by Copybara-Service
parent 7f09a7d8c6
commit b2018ff506
1 changed files with 3 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+3 -16
View File
@@ -264,21 +264,6 @@ 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) {
if (type == mjJNT_FREE) {
return 6;
}
if (type == mjJNT_BALL) {
return 3;
}
return 1;
}
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) { void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START; TM_START;
@@ -493,6 +478,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// actuator-level gravity compensation // actuator-level gravity compensation
if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
// number of dofs for each joint type: {mjJNT_FREE, mjJNT_BALL, mjJNT_SLIDE, mjJNT_HINGE}
static const int jnt_dofnum[4] = {6, 3, 1, 1};
int njnt = m->njnt; int njnt = m->njnt;
for (int i=0; i < njnt; i++) { for (int i=0; i < njnt; i++) {
// skip if gravcomp added as passive force // skip if gravcomp added as passive force
@@ -501,7 +488,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
} }
// add gravcomp force // add gravcomp force
int dofnum = jnt_dofnum(m->jnt_type[i]); int dofnum = jnt_dofnum[m->jnt_type[i]];
int dofadr = m->jnt_dofadr[i]; int dofadr = m->jnt_dofadr[i];
mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum); mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum);
} }