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

Use sparse (uncompressed) actuator_moment in mj_transmission.

Browse Source 
PiperOrigin-RevId: 692179704
Change-Id: Ic30ac5a98dc13de2028e378df65dc88ba3912bf5
This commit is contained in:
Taylor Howell
2024-11-01 08:05:14 -07:00
committed by Copybara-Service
parent 1d58576d28
commit a51f346059
13 changed files with 230 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+8 -4
View File
@@ -208,8 +208,11 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv);
}
// actuator velocity: always dense
mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv);
// actuator velocity: always sparse
if (!mjDISABLED(mjDSBL_ACTUATION)) {
mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu,
d->moment_rownnz, d->moment_rowadr, d->moment_colind, NULL);
}
// com-based velocities, passive forces, constraint references
mj_comVel(m, d);
@@ -270,7 +273,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu;
mjtNum gain, bias, tau;
mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force;
mjtNum *prm, *force = d->actuator_force;
// clear actuator_force
mju_zero(force, nu);
@@ -475,7 +478,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv,
d->moment_rownnz, d->moment_rowadr, d->moment_colind);
// actuator-level gravity compensation
if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {