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

Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit integrators, wrong derivatives would be computed.

Browse Source 
PiperOrigin-RevId: 663680592
Change-Id: I5d72888cab19dd5e51f552f63fe0968db93f5dcd
This commit is contained in:
Yuval Tassa
2024-08-16 04:06:30 -07:00
committed by Copybara-Service
parent 6db96e07e4
commit 0bcaa85650
3 changed files with 93 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_derivative.c
+7 -5
View File
@@ -338,7 +338,7 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt
mju_zero(Dcvel, nbody*6*nv);
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
for (int i=1; i < m->nbody; i++) {
for (int i=1; i < nbody; i++) {
// Dcvel = Dcvel_parent
mju_copy(Dcvel+i*6*nv, Dcvel+m->body_parentid[i]*6*nv, 6*nv);
@@ -450,7 +450,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mju_zero(Dcfrcbody, 6*nv);
// backward pass over bodies: accumulate Dcfrcbody
for (int i=m->nbody-1; i > 0; i--) {
for (int i=nbody-1; i > 0; i--) {
if (m->body_parentid[i]) {
mju_addTo(Dcfrcbody+m->body_parentid[i]*6*nv, Dcfrcbody+i*6*nv, 6*nv);
}
@@ -825,7 +825,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nv = m->nv;
int nv = m->nv, nu = m->nu;
// disabled: nothing to add
if (mjDISABLED(mjDSBL_ACTUATION)) {
@@ -833,7 +833,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
}
// process actuators
for (int i=0; i < m->nu; i++) {
for (int i=0; i < nu; i++) {
// skip if disabled
if (mj_actuatorDisabled(m, i)) {
continue;
@@ -867,7 +867,9 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
if (m->actuator_dyntype[i] == mjDYN_NONE) {
bias_vel += gain_vel * d->ctrl[i];
} else {
bias_vel += gain_vel * d->act[i-(m->nu - m->na)];
int act_first = m->actuator_actadr[i];
int act_last = act_first + m->actuator_actnum[i] - 1;
bias_vel += gain_vel * d->act[act_last];
}
}