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.
PiperOrigin-RevId: 663680592 Change-Id: I5d72888cab19dd5e51f552f63fe0968db93f5dcd
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
6db96e07e4
commit
0bcaa85650
@@ -17,6 +17,13 @@ MJX
|
||||
5. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``,
|
||||
``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``SUBTREECOM``, ``CLOCK``.
|
||||
|
||||
Bug fixes
|
||||
^^^^^^^^^
|
||||
6. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`,
|
||||
contribution by :github:user:`michael-ahn`).
|
||||
7. 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.
|
||||
|
||||
Version 3.2.2 (Aug 8, 2024)
|
||||
---------------------------
|
||||
|
||||
|
||||
@@ -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];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -223,6 +223,85 @@ TEST_F(DerivativeTest, DisabledActuators) {
|
||||
mj_deleteModel(m1);
|
||||
}
|
||||
|
||||
// actuator order has no effect
|
||||
TEST_F(DerivativeTest, ActuatorOrder) {
|
||||
// model with stateful actuator first
|
||||
static constexpr char xml1[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="0" type="slide" range="-1 1"/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
<body pos="1 0 0">
|
||||
<joint name="1" type="slide" range="-1 1"/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<muscle joint="0" ctrlrange="0 6"/>
|
||||
<damper joint="1" kv="200" ctrlrange="0 6"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024];
|
||||
mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error));
|
||||
ASSERT_THAT(m1, NotNull()) << "Failed to load model: " << error;
|
||||
mjData* d1 = mj_makeData(m1);
|
||||
|
||||
d1->ctrl[0] = 6;
|
||||
d1->ctrl[1] = 6;
|
||||
|
||||
while (d1->time < 1)
|
||||
mj_step(m1, d1);
|
||||
|
||||
// model with stateful actuator second
|
||||
static constexpr char xml2[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast"/>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="0" type="slide" range="-1 1"/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
<body pos="1 0 0">
|
||||
<joint name="1" type="slide" range="-1 1"/>
|
||||
<geom size=".1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<damper joint="1" kv="200" ctrlrange="0 6"/>
|
||||
<muscle joint="0" ctrlrange="0 6"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
mjModel* m2 = LoadModelFromString(xml2, error, sizeof(error));
|
||||
ASSERT_THAT(m2, NotNull()) << "Failed to load model: " << error;
|
||||
mjData* d2 = mj_makeData(m2);
|
||||
|
||||
d2->ctrl[0] = 6;
|
||||
d2->ctrl[1] = 6;
|
||||
|
||||
while (d2->time < 1)
|
||||
mj_step(m2, d2);
|
||||
|
||||
// expect same qvel in both models
|
||||
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
|
||||
EXPECT_EQ(d1->qvel[1], d2->qvel[1]);
|
||||
|
||||
mj_deleteData(d2);
|
||||
mj_deleteModel(m2);
|
||||
mj_deleteData(d1);
|
||||
mj_deleteModel(m1);
|
||||
}
|
||||
|
||||
// compare analytic and fin-diff d_qfrc_passive/d_qvel
|
||||
TEST_F(DerivativeTest, PassiveDvel) {
|
||||
for (const char* local_path : {kTumblingThinObjectPath,
|
||||
|
||||
Reference in New Issue
Block a user