Add SO3 transmission and native orientation actuator.

https://youtu.be/17XpwnqyCXs

New transmission type mjTRN_SO3: a relative orientation, targeting a ball
joint or a site+refsite pair. It is the first transmission with more than
one force output: its length is the norm of the expmap vector of the
relative rotation and its moment axes are the 3 rows of the
relative rotational Jacobian, without projecting onto per-actuator gears.

New force law mjGAIN_SO3/mjBIAS_SO3: a geodesic PD servo, force =
kp * log(q_current^-1 * q_target) - kv * velocity, exact for arbitrary axis
combinations with a unique equilibrium at every commanded orientation.
Error, moment rows and velocity all live in the child frame (joint or
site): the right-difference error is the gradient of the geodesic
potential in that frame. The parent-frame (left) error is not: driving
child-frame torques with it pumps energy at large angles, settling into
steady-spinning limit cycles (the SO3LargeAngleConvergence test). The
integrator variant stores the 3D orientation setpoint in act (actnum = 3,
re-anchored to a bounded representative at integration time). Exposed in
MJCF as <orientation joint=|site=+refsite= kp kv|dampratio>, or via
<general gaintype="so3" biastype="so3">.

The setpoint input has two charts: an expmap target (3 controls, default)
or a quaternion target (4 controls) -- <orientation input="quat">, the
first actuator with different input and output widths. The signature is
recorded in a new per-actuator field actuator_ctrlspec (mjtCtrlChart),
whose meaning is scoped by the gain type the way gain/bias parameters are;
ctrlnum is derived from it at compile time and remains the layout
authority. An explicit field rather than width inference or a prm slot:
width-as-chart cannot express same-width signatures (upcoming servo input
subsets), and prm slots are the input_mode pattern this stack retires.
The force law normalizes the commanded quaternion, making it scale- and
antipodally-invariant. The all-zero ctrl still maps to the identity via
mju_normalize4, but it is a degenerate point (a nudge of any component
commands a half-turn), so quat inputs reset to the identity quaternion:
new mj_resetCtrl sets neutral ctrl values (zero, except qw = 1), called
by mj_resetData and the viewers' Clear All. The quat chart is
restricted to dyntype 'none': integrating a quaternion setpoint linearly
is not meaningful on the manifold. New mjsActuator.ctrlspec field carries
the signature through the spec and XML round-trip.

Actuator sensors (actuatorpos/vel/frc) now report one value per force
output; dim = 3 on an SO3 actuator.

As the first actuator with nu != nactuator, this commit also makes the
viewers multi-input aware: the control sliders in simulate and studio,
which indexed per-actuator arrays by control index (out of bounds on
this model class), are generated per control and labeled with the
actuator name plus an input suffix ("orient/qw"), via the new
introspection helper mj_actuatorInputName -- the single source of truth
for input names, extended by each new multi-input type (quaternion
components are w-first: qw, qx, qy, qz). Slider ranges now honor a
defined ctrlrange even when ctrllimited is false: range is the UI hint,
limited is the clamp -- wrapped and expmap setpoints are unbounded but
still want finite sliders, while quat components are truly bounded.

The rotational demo model is orientation.xml under
test/engine/testdata/actuation/, upgraded to a three-way contrast:
per-axis wrapped servos vs an expmap-commanded vs a quat-commanded
orientation actuator, on identical checker-textured boxes. It is loaded
by the mixed-axis contrast and input-name tests, and doubles as the
viewer test model (slider groups of 3 independent, 3 grouped, 4 grouped).

PiperOrigin-RevId: 951607063
Change-Id: If235dba8e2f2ca72672e7c62531a27e967c6a373

src/user/user_api.cc

@@ -1324,6 +1324,31 @@ const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1],
 
 
 
+// Set to orientation actuator.
+const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
+                                 double dampratio[1], int ctrlspec) {
+  if (kv && dampratio) {
+    return "kv and dampratio cannot both be defined";
+  }
+  actuator->gainprm[0] = kp;
+  actuator->biasprm[1] = -kp;
+  if (kv) {
+    if (*kv < 0) return "kv cannot be negative";
+    actuator->biasprm[2] = -(*kv);
+  }
+  if (dampratio) {
+    if (*dampratio < 0) return "dampratio cannot be negative";
+    actuator->biasprm[2] = *dampratio;
+  }
+  actuator->ctrlspec = ctrlspec;
+  actuator->gaintype = mjGAIN_SO3;
+  actuator->biastype = mjBIAS_SO3;
+  actuator->dyntype = mjDYN_NONE;
+  return "";
+}
+
+
+
 // Set to velocity actuator.
 const char* mjs_setToVelocity(mjsActuator* actuator, double kv) {
   mjuu_zerovec(actuator->biasprm, mjNBIAS);

src/user/user_api.h

@@ -185,6 +185,10 @@ MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double
 // Set actuator to velocity, return error on failure.
 MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
 
+// Set to orientation actuator.
+MJAPI const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
+                                       double dampratio[1], int ctrlspec);
+
 // Set actuator to damper, return error on failure.
 MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
 

src/user/user_init.c

@@ -341,6 +341,7 @@ void mjs_defaultActuator(mjsActuator* actuator) {
   actuator->dyntype = mjDYN_NONE;
   actuator->dynprm[0] = 1;
   actuator->actdim = -1;
+  actuator->ctrlspec = 0;
 
   // transmission
   actuator->trntype = mjTRN_UNDEFINED;

src/user/user_model.cc

@@ -3394,6 +3394,11 @@ int mjCModel::CountNJmom(const mjModel* m) {
 
     // process according to transmission type
     switch ((mjtTrn)m->actuator_trntype[i]) {
+      case mjTRN_SO3:
+        // ball joint: 3 identity rows; site+refsite: 3 dense rows
+        count += m->actuator_trnid[2*i+1] >= 0 ? 3*nv : 3;
+        break;
+
       case mjTRN_JOINT:
       case mjTRN_JOINTINPARENT:
         switch ((mjtJoint)m->jnt_type[id]) {
@@ -4011,7 +4016,7 @@ void mjCModel::CopyObjects(mjModel* m) {
     mjCActuator* pac = actuators_[i];
 
     // set fields
-    m->actuator_trntype[i] = pac->trntype;
+    m->actuator_trntype[i] = pac->so3_ ? mjTRN_SO3 : pac->trntype;
     m->actuator_dyntype[i] = pac->dyntype;
     m->actuator_gaintype[i] = pac->gaintype;
     m->actuator_biastype[i] = pac->biastype;
@@ -4024,9 +4029,10 @@ void mjCModel::CopyObjects(mjModel* m) {
     adr += m->actuator_actnum[i];
     m->actuator_group[i] = pac->group;
 
-    // input and output blocks; all actuator types are currently 1x1
+    // input and output blocks
     m->actuator_ctrladr[i] = ctrladr;
     m->actuator_ctrlnum[i] = pac->ctrlnum_;
+    m->actuator_ctrlspec[i] = pac->ctrlspec_;
     pac->ctrladr_ = ctrladr;
     ctrladr += pac->ctrlnum_;
     m->actuator_outadr[i] = outadr;
@@ -4055,6 +4061,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN);
     mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS);
     mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2);
+    m->actuator_forcelimited[i] = (mjtBool)pac->is_forcelimited();
+    mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2);
     mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator);
 
     // per-input arrays, at the actuator's ctrl block
@@ -4066,8 +4074,6 @@ void mjCModel::CopyObjects(mjModel* m) {
 
     // per-output arrays, at the actuator's output block
     for (int j=m->actuator_outadr[i]; j < m->actuator_outadr[i]+m->actuator_outnum[i]; j++) {
-      m->actuator_forcelimited[j] = (mjtBool)pac->is_forcelimited();
-      mjuu_copyvec(m->actuator_forcerange + 2*j, pac->forcerange, 2);
       mjuu_copyvec(m->actuator_gear + 6*j, pac->gear, 6);
       mjuu_copyvec(m->actuator_lengthrange + 2*j, pac->lengthrange, 2);
     }
@@ -5972,7 +5978,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
     mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN);
     mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS);
     mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*m->actuator_ctrladr[i], 2);
-    mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*m->actuator_outadr[i], 2);
+    mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2);
     mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2);
     mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*m->actuator_outadr[i], 2);
     mjuu_copyvec(pa->gear, m->actuator_gear+6*m->actuator_outadr[i], 6);

src/user/user_objects.cc

@@ -6931,8 +6931,10 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
   // input and output blocks, set by mjCModel; all actuator types are currently 1x1
   ctrladr_ = -1;
   ctrlnum_ = 1;
+  ctrlspec_ = 0;
   outadr_ = -1;
   outnum_ = 1;
+  so3_ = false;
 }
 
 
@@ -7123,6 +7125,12 @@ void mjCActuator::ResolveReferences(const mjCModel* m) {
 void mjCActuator::Compile(void) {
   CopyFromSpec();
 
+  // reset input/output block widths, resolved below
+  ctrlnum_ = 1;
+  ctrlspec_ = 0;
+  outnum_ = 1;
+  so3_ = false;
+
   // resize userdata
   if (userdata_.size() > model->nuser_actuator) {
     throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)",
@@ -7139,6 +7147,79 @@ void mjCActuator::Compile(void) {
   // find transmission target in object arrays
   ResolveReferences(model);
 
+  // SO3 geodesic servo: validate and resolve the SO3 transmission
+  if (gaintype == mjGAIN_SO3 || biastype == mjBIAS_SO3) {
+    if (gaintype != mjGAIN_SO3 || biastype != mjBIAS_SO3) {
+      throw mjCError(this, "gaintype and biastype must both be 'so3' in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    if (dyntype != mjDYN_NONE && dyntype != mjDYN_INTEGRATOR) {
+      throw mjCError(this, "so3 requires dyntype 'none' or 'integrator' in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    if (gainprm[0] != -biasprm[1]) {
+      throw mjCError(this, "so3 requires gainprm[0] == -biasprm[1] in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    if (trntype == mjTRN_SITE) {
+      if (refsite_.empty()) {
+        throw mjCError(this, "so3 site transmission requires refsite in actuator '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+    } else if (trntype == mjTRN_JOINT) {
+      if (((mjCJoint*)ptarget)->spec.type != mjJNT_BALL) {
+        throw mjCError(this, "so3 joint transmission requires a ball joint in actuator '%s' "
+                       "(id = %d)", name.c_str(), id);
+      }
+    } else {
+      throw mjCError(this, "so3 requires site or ball joint transmission in actuator '%s' "
+                     "(id = %d)", name.c_str(), id);
+    }
+
+    // integrator variant: activation is the 3D orientation setpoint
+    if (dyntype == mjDYN_INTEGRATOR) {
+      if (actdim > 0 && actdim != 3) {
+        throw mjCError(this, "so3 integrator requires actdim 3 in actuator '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+      actdim = 3;
+
+      // the act setpoint is re-anchored to a bounded representative at integration time
+      if (actlimited == mjLIMITED_TRUE && actrange[0] == 0 && actrange[1] == 0) {
+        actlimited = mjLIMITED_FALSE;
+      }
+    }
+
+    // input chart: expmap (3 controls, default) or quat (4 controls)
+    ctrlspec_ = ctrlspec ? ctrlspec : mjCHART_EXPMAP;
+    if (ctrlspec_ == mjCHART_QUAT) {
+      if (dyntype != mjDYN_NONE) {
+        throw mjCError(this, "so3 quat input requires dyntype 'none' in actuator '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+    } else if (ctrlspec_ != mjCHART_EXPMAP) {
+      throw mjCError(this, "so3 input must be expmap or quat in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+
+    // force is clamped on the norm of the output torque: lower bound must be 0
+    if (is_forcelimited() && forcerange[0] != 0) {
+      throw mjCError(this, "so3 forcerange bounds the force norm, lower bound must be 0 in "
+                     "actuator '%s' (id = %d)", name.c_str(), id);
+    }
+
+    // input and output blocks
+    ctrlnum_ = ctrlspec_ == mjCHART_QUAT ? 4 : 3;
+    outnum_ = 3;
+    so3_ = true;
+  }
+
+  // input signature selection is so3-only
+  if (ctrlspec && gaintype != mjGAIN_SO3) {
+    throw mjCError(this, "input is only available for so3 actuators, actuator '%s' (id = %d)",
+                   name.c_str(), id);
+  }
+
   // check damping/armature only valid for joint and tendon transmission
   bool has_damping = false;
   for (int i = 0; i < mjNPOLY+1; i++) {
@@ -7234,7 +7315,7 @@ void mjCActuator::Compile(void) {
 
   // check and set actdim
   if (!plugin.active) {
-    if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR) {
+    if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR && !so3_) {
       throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' and 'dcmotor'");
     }
     if (actdim == 1 && dyntype == mjDYN_NONE) {
@@ -7970,6 +8051,11 @@ void mjCSensor::Compile(void) {
 
   dim = mjs_sensorDim(this);
 
+  // actuator sensors report one value per force output
+  if (type == mjSENS_ACTUATORPOS || type == mjSENS_ACTUATORVEL || type == mjSENS_ACTUATORFRC) {
+    dim = ((mjCActuator*)obj)->outnum_;
+  }
+
   // check cutoff for incompatible data types
   if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
                      (datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {

src/user/user_objects.h

@@ -1812,8 +1812,10 @@ class mjCActuator_ : public mjCBase {
   int actdim_;                                      // number of dofs in data->act
   int ctrladr_;                                     // address of first control in data->ctrl
   int ctrlnum_;                                     // number of controls
+  int ctrlspec_;                                    // resolved input signature, scoped by gaintype
   int outadr_;                                      // address of first force output
   int outnum_;                                      // number of force outputs, from trntype
+  bool so3_;                                        // compiles to an SO3 transmission
   std::map<std::string, std::vector<mjtNum>> act_;  // act at the previous step
   std::map<std::string, mjtNum> ctrl_;              // ctrl at the previous step
 
@@ -1833,6 +1835,7 @@ class mjCActuator_ : public mjCBase {
 class mjCActuator : public mjCActuator_, private mjsActuator {
   friend class mjCDef;
   friend class mjCModel;
+  friend class mjCSensor;
   friend class mjXWriter;
 
  public: