Runtime disabling of actuators according to group.

Added `option-actuatorgroupdisable` attribute and associated `mjOption.disableactuator` integer bitfield, used to disable sets of actuators at runtime according to their group.

- The first 6 actuator groups are toggleable in the `simulate` viewer.
- Minor refactor and cleanup of actuator documentation.

https://youtu.be/H9qG9Zf2W44

Fixes #1092.

PiperOrigin-RevId: 578335600
Change-Id: I4cf663b90ea768e4380acfa2fe3b8c15c7cbb568

src/engine/engine_forward.c

@@ -213,6 +213,8 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
   TM_END(mjTIMER_VELOCITY);
 }
 
+
+
 // returns the next act given the current act_dot, after clamping
 static mjtNum nextActivation(const mjModel* m, const mjData* d,
                              int actuator_id, int act_adr, mjtNum act_dot) {
@@ -239,6 +241,8 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d,
   return act;
 }
 
+
+
 // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
 void mj_fwdActuation(const mjModel* m, mjData* d) {
   TM_START;
@@ -246,12 +250,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
   mjtNum gain, bias, tau;
   mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force;
 
-  // clear outputs
-  mju_zero(d->qfrc_actuator, nv);
-  mju_zero(d->actuator_force, nu);
+  // clear actuator_force
+  mju_zero(force, nu);
 
   // disabled or no actuation: return
   if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
+    mju_zero(d->qfrc_actuator, nv);
     return;
   }
 
@@ -328,6 +332,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 
   // force = gain .* [ctrl/act] + bias
   for (int i=0; i < nu; i++) {
+    // skip if disabled
+    if (mj_actuatorDisabled(m, i)) {
+      continue;
+    }
+
     // skip actuator plugins -- these are handled after builtin actuator types
     if (m->actuator_plugin[i] >= 0) {
       continue;
@@ -457,7 +466,6 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 
 // add up all non-constraint forces, compute qacc_smooth
 void mj_fwdAcceleration(const mjModel* m, mjData* d) {
-  mj_markStack(d);
   int nv = m->nv;
 
   // qforce = sum of all non-constraint forces
@@ -468,8 +476,6 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) {
 
   // qacc_smooth = M \ qfr_smooth
   mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
-
-  mj_freeStack(d);
 }
 
 
@@ -685,7 +691,8 @@ static void mj_advance(const mjModel* m, mjData* d,
       int actadr = m->actuator_actadr[i];
       int actadr_end = actadr + m->actuator_actnum[i];
       for (int j=actadr; j < actadr_end; j++) {
-        d->act[j] = nextActivation(m, d, i, j, act_dot[j]);
+        // if disabled, set act_dot to 0
+        d->act[j] = nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
       }
     }
   }

src/engine/engine_io.c

@@ -161,6 +161,7 @@ void mj_defaultOption(mjOption* opt) {
   opt->mpr_iterations     = 50;
   opt->disableflags       = 0;
   opt->enableflags        = 0;
+  opt->disableactuator    = 0;
 
   // sdf collisions
   opt->sdf_initpoints     = 40;

src/engine/engine_support.c

@@ -1784,7 +1784,15 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
   }
 }
 
-
+// return 1 if actuator i is disabled, 0 otherwise
+int mj_actuatorDisabled(const mjModel* m, int i) {
+  int group = m->actuator_group[i];
+  if (group < 0 || group > 30) {
+    return 0;
+  } else {
+    return m->opt.disableactuator & (1 << group) ? 1 : 0;
+  }
+}
 
 // sum all body masses
 mjtNum mj_getTotalmass(const mjModel* m) {

src/engine/engine_support.h

@@ -204,6 +204,9 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
                            const mjtNum pos[3], const mjtNum quat[4],
                            int body, mjtByte sameframe);
 
+// return 1 if actuator i is disabled, 0 otherwise
+MJAPI int mj_actuatorDisabled(const mjModel* m, int i);
+
 // sum all body masses
 MJAPI mjtNum mj_getTotalmass(const mjModel* m);
 

src/engine/engine_vis_visualize.c

@@ -1072,6 +1072,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   if (vopt->flags[mjVIS_ACTUATOR] && (category & catmask)) {
     for (int i=0; i < m->nu; i++) {
       if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) {
+        // skip if disabled
+        if (mj_actuatorDisabled(m, i)) {
+          continue;
+        }
+
         // determine extended range
         mjtNum rng[3] = {-1, 0, +1};
         mjtNum rmin = -1, rmax = 1, act = 0;

src/xml/xml_native_reader.cc

@@ -93,13 +93,13 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "inttotal", "interval", "tolrange"},
     {">"},
 
-    {"option", "*", "26",
+    {"option", "*", "27",
         "timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
         "mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
         "o_margin", "o_solref", "o_solimp", "o_friction",
         "integrator", "cone", "jacobian",
         "solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
-        "sdf_iterations", "sdf_initpoints"},
+        "sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"},
     {"<"},
         {"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact",
             "passive", "gravity", "clampctrl", "warmstart",
@@ -1036,6 +1036,22 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
   ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations);
   ReadAttrInt(section, "sdf_initpoints", &opt->sdf_initpoints);
 
+  // actuatorgroupdisable
+  constexpr int num_bitflags = 31;
+  int disabled_act_groups[num_bitflags];
+  int num_found = ReadAttr(section, "actuatorgroupdisable", num_bitflags, disabled_act_groups,
+                           text, false, false);
+  for (int i=0; i < num_found; i++) {
+    int group = disabled_act_groups[i];
+    if (group < 0 ) {
+      throw mjXError(section, "disabled actuator group value must be non-negative");
+    }
+    if (group > num_bitflags - 1) {
+      throw mjXError(section, "disabled actuator group value cannot exceed 30");
+    }
+    opt->disableactuator |= (1 << group);
+  }
+
   // read disable sub-element
   XMLElement* elem = FindSubElem(section, "flag");
   if (elem) {

src/xml/xml_native_writer.cc

@@ -909,6 +909,16 @@ void mjXWriter::Option(XMLElement* root) {
   WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations);
   WriteAttrInt(section, "sdf_initpoints", model->option.sdf_initpoints, opt.sdf_initpoints);
 
+  // actuator group disable
+  int disabled_groups[31];
+  int ndisabled = 0;
+  for (int i = 0; i < 31; ++i) {
+    if (model->option.disableactuator & (1 << i)) {
+      disabled_groups[ndisabled++] = i;
+    }
+  }
+  WriteAttr(section, "actuatorgroupdisable", ndisabled, disabled_groups);
+
   // write disable/enable flags if any of them are set; invert while writing
   if (model->option.disableflags || model->option.enableflags) {
     XMLElement* sub = InsertEnd(section, "flag");