From 0bcaa85650d56b5d5d4564d52e05563c00c3e965 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Aug 2024 04:06:30 -0700 Subject: [PATCH] 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 --- doc/changelog.rst | 7 +++ src/engine/engine_derivative.c | 12 ++-- test/engine/engine_derivative_test.cc | 79 +++++++++++++++++++++++++++ 3 files changed, 93 insertions(+), 5 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 9504fd2d..ea4a2edb 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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) --------------------------- diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index dec37eb8..b321ba88 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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]; } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 9a7a776e..778ddadb 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -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"( + + + )"; + + 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"( + + + )"; + + 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,