Remove requirement that stateless actuators come before stateful actuators.

PiperOrigin-RevId: 484332415
Change-Id: I1dae92c0f8e027e4ca71c20ea3f68b2926526f53

doc/changelog.rst

@@ -10,6 +10,7 @@ General
 ^^^^^^^
 
 - Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions.
+- Removed the requirement that stateless actuators come before stateful actuators.
 
 Version 2.3.0 (October 18, 2022)
 --------------------------------

include/mujoco/mjmodel.h

@@ -849,6 +849,7 @@ struct mjModel_ {
   int*      actuator_gaintype;    // gain type (mjtGain)                      (nu x 1)
   int*      actuator_biastype;    // bias type (mjtBias)                      (nu x 1)
   int*      actuator_trnid;       // transmission id: joint, tendon, site     (nu x 2)
+  int*      actuator_actadr;      // first activation address; -1: stateless  (nu x 1)
   int*      actuator_group;       // group for visibility                     (nu x 1)
   mjtByte*  actuator_ctrllimited; // is control limited                       (nu x 1)
   mjtByte*  actuator_forcelimited;// is force limited                         (nu x 1)

include/mujoco/mjxmacro.h

@@ -354,6 +354,7 @@
     X( int,     actuator_gaintype,     nu,            1                    ) \
     X( int,     actuator_biastype,     nu,            1                    ) \
     X( int,     actuator_trnid,        nu,            2                    ) \
+    X( int,     actuator_actadr,       nu,            1                    ) \
     X( int,     actuator_group,        nu,            1                    ) \
     X( mjtByte, actuator_ctrllimited,  nu,            1                    ) \
     X( mjtByte, actuator_forcelimited, nu,            1                    ) \

python/mujoco/indexer_xmacro.h

@@ -23,6 +23,7 @@
   X( int,     actuator_, gaintype,     nu, 1                    ) \
   X( int,     actuator_, biastype,     nu, 1                    ) \
   X( int,     actuator_, trnid,        nu, 2                    ) \
+  X( int,     actuator_, actadr,       nu, 1                    ) \
   X( int,     actuator_, group,        nu, 1                    ) \
   X( mjtByte, actuator_, ctrllimited,  nu, 1                    ) \
   X( mjtByte, actuator_, forcelimited, nu, 1                    ) \

src/engine/engine_forward.c

@@ -155,7 +155,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
 // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
 void mj_fwdActuation(const mjModel* m, mjData* d) {
   TM_START;
-  int nv = m->nv, nu = m->nu, na = m->na;
+  int nv = m->nv, nu = m->nu;
   mjtNum gain, bias, tau;
   mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force;
 
@@ -231,10 +231,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     }
 
     // set force = gain .* [ctrl/act]
-    if (m->actuator_dyntype[i]==mjDYN_NONE) {
+    if (m->actuator_actadr[i] == -1) {
       force[i] = gain * ctrl[i];
     } else {
-      force[i] = gain * d->act[i-(nu-na)];
+      force[i] = gain * d->act[m->actuator_actadr[i]];
     }
 
     // extract bias info
@@ -299,14 +299,18 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
   mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
 
   // act_dot for stateful actuators
-  for (int i=nu-na; i<nu; i++) {
+  for (int i=0; i<nu; i++) {
     if (m->actuator_plugin[i] >= 0) {
       continue;
     }
 
+    int j = m->actuator_actadr[i];
+    if (j < 0) {
+      continue;
+    }
+
     // extract info
     prm = m->actuator_dynprm + i*mjNDYN;
-    int j = i-(nu-na);
 
     // compute act_dot according to dynamics type
     switch (m->actuator_dyntype[i]) {
@@ -493,11 +497,11 @@ static void mj_advance(const mjModel* m, mjData* d,
     mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
 
     // clamp activations
-    for (int i=0; i<m->na; i++) {
+    for (int i=0; i<m->nu; i++) {
-      int iu = i + m->nu - m->na;
+      int j = m->actuator_actadr[i];
-      if (m->actuator_actlimited[iu]) {
+      if (j > -1 && m->actuator_actlimited[i]) {
-        mjtNum* actrange = m->actuator_actrange + 2*iu;
+        mjtNum* actrange = m->actuator_actrange + 2*i;
-        d->act[i] = mju_clip(d->act[i], actrange[0], actrange[1]);
+        d->act[j] = mju_clip(d->act[j], actrange[0], actrange[1]);
       }
     }
   }

src/engine/engine_io.c

@@ -1369,6 +1369,7 @@ const char* mj_validateReferences(const mjModel* m) {
   X(pair_geom1,         npair,         ngeom        , 0                      ) \
   X(pair_geom2,         npair,         ngeom        , 0                      ) \
   X(actuator_plugin,    nu,            nplugin      , 0                      ) \
+  X(actuator_actadr,    nu,            na           , 0                      ) \
   X(sensor_plugin,      nsensor,       nplugin      , 0                      ) \
   X(plugin_stateadr,    nplugin,       npluginstate , m->plugin_statenum     ) \
   X(plugin_attradr,     nplugin,       npluginattr  , 0                      ) \

src/engine/engine_vis_visualize.c

@@ -808,7 +808,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
 
         // clamp act to extended range
         if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) {
-          act = mju_clip(d->act[i-(m->nu-m->na)], rng[0], rng[2]);
+          act = mju_clip(d->act[m->actuator_actadr[i]], rng[0], rng[2]);
         } else {
           act = mju_clip(d->ctrl[i], rng[0], rng[2]);
         }

src/user/user_model.cc

@@ -926,11 +926,6 @@ void mjCModel::SetSizes(void) {
   // nu, na
   for (i=0; i<(int)actuators.size(); i++) {
     if (actuators[i]->dyntype == mjDYN_NONE) {
-      //  make sure all 2nd-order come before all 3rd-order
-      if (na) {
-        throw mjCError(0, "stateless actuators must come before stateful actuators");
-      }
-
       nu++;
     } else {
       nu++;
@@ -1825,6 +1820,7 @@ void mjCModel::CopyObjects(mjModel* m) {
   }
 
   // actuators
+  adr = 0;
   for (i=0; i<nu; i++) {
     // get pointer
     mjCActuator* pac = actuators[i];
@@ -1836,6 +1832,7 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->actuator_biastype[i] = pac->biastype;
     m->actuator_trnid[2*i] = pac->trnid[0];
     m->actuator_trnid[2*i+1] = pac->trnid[1];
+    m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr++;
     m->actuator_group[i] = pac->group;
     m->actuator_ctrllimited[i] = pac->ctrllimited;
     m->actuator_forcelimited[i] = pac->forcelimited;

test/user/user_objects_test.cc

@@ -104,7 +104,7 @@ TEST_F(KeyframeTest, BadSize) {
   char error[1024];
   size_t error_sz = 1024;
   mjModel* model = LoadModelFromString(xml, error, error_sz);
-  EXPECT_THAT(model, ::testing::IsNull());
+  EXPECT_THAT(model, IsNull());
   EXPECT_THAT(error, HasSubstr("invalid qpos size, expected length 0"));
 }
 
@@ -313,7 +313,7 @@ TEST_F(MjCHFieldTest, PngMap) {
   std::array<char, 1024> error;
   mjModel* model =
       mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
-  ASSERT_THAT(model, ::testing::NotNull()) << error.data();
+  ASSERT_THAT(model, NotNull()) << error.data();
   EXPECT_EQ(model->nhfield, 1);
   EXPECT_EQ(model->geom_type[0], mjGEOM_HFIELD);
   mj_deleteModel(model);
@@ -348,8 +348,8 @@ TEST_F(QuatNorm, QuatNotNormalized) {
 
 using ActuatorTest = MujocoTest;
 
-TEST_F(ActuatorTest, BadOrder) {
+TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) {
-  static constexpr char xml[] = R"(
+  static constexpr char xml1[] = R"(
   <mujoco>
     <worldbody>
       <body>
@@ -363,11 +363,52 @@ TEST_F(ActuatorTest, BadOrder) {
     </actuator>
   </mujoco>
   )";
-  char error[1024];
+  static constexpr char xml2[] = R"(
-  size_t error_sz = 1024;
+  <mujoco>
-  mjModel* model = LoadModelFromString(xml, error, error_sz);
+    <worldbody>
-  EXPECT_THAT(model, ::testing::IsNull());
+      <body>
-  EXPECT_THAT(error, HasSubstr("stateless actuators must come before"));
+        <joint name="hinge"/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <general joint="hinge"/>
+      <general joint="hinge" dyntype="filter"/>
+    </actuator>
+  </mujoco>
+  )";
+  mjModel* model1 = LoadModelFromString(xml1, nullptr, 0);
+  mjData* data1 = mj_makeData(model1);
+  mjModel* model2 = LoadModelFromString(xml2, nullptr, 0);
+  mjData* data2 = mj_makeData(model2);
+
+  // check activation indexing
+  EXPECT_EQ(model1->actuator_actadr[0], 0);
+  EXPECT_EQ(model1->actuator_actadr[1], -1);
+  EXPECT_EQ(model2->actuator_actadr[0], -1);
+  EXPECT_EQ(model2->actuator_actadr[1], 0);
+
+  // integrate both models, flipping the controls
+  while (data1->time < 1) {
+    data1->ctrl[0] = data1->time;
+    data1->ctrl[1] = -data1->time;
+    mj_step(model1, data1);
+  }
+  while (data2->time < 1) {
+    data2->ctrl[0] = -data2->time;
+    data2->ctrl[1] = data2->time;
+    mj_step(model2, data2);
+  }
+
+  // expect states to match exactly
+  EXPECT_EQ(data1->qpos[0], data2->qpos[0]);
+  EXPECT_EQ(data1->qvel[0], data2->qvel[0]);
+  EXPECT_EQ(data1->act[0], data2->act[0]);
+
+  mj_deleteData(data2);
+  mj_deleteModel(model2);
+  mj_deleteData(data1);
+  mj_deleteModel(model1);
 }
 
 

unity/Runtime/Bindings/MujocoBindings.cs

@@ -2129,6 +2129,7 @@ public unsafe struct mjModel_ {
   public int* actuator_gaintype;
   public int* actuator_biastype;
   public int* actuator_trnid;
+  public int* actuator_actadr;
   public int* actuator_group;
   public byte* actuator_ctrllimited;
   public byte* actuator_forcelimited;