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,