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/engine/engine_forward.c

@@ -268,6 +268,19 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, i
 }
 
 
+// expmap (axis-angle) vector to quaternion
+static void expmap2Quat(mjtNum quat[4], const mjtNum v[3]) {
+  mjtNum angle = mju_norm3(v);
+  if (angle < mjMINVAL) {
+    quat[0] = 1;
+    quat[1] = quat[2] = quat[3] = 0;
+  } else {
+    mjtNum axis[3] = {v[0]/angle, v[1]/angle, v[2]/angle};
+    mju_axisAngle2Quat(quat, axis, angle);
+  }
+}
+
+
 // period of the rotational transmission for wrap-eligible servo actuators, 0 otherwise
 static mjtNum wrapPeriod(const mjModel* m, int i) {
   // servo shape: fixed gain, affine bias, matching kp, setpoint input
@@ -387,9 +400,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 
     // compute act_dot according to dynamics type
     switch (dyntype) {
-    case mjDYN_INTEGRATOR:          // simple integrator
-      d->act_dot[act_last] = ctrl[uadr];
+    case mjDYN_INTEGRATOR: {        // simple integrator, one per control
+      int num = m->actuator_ctrlnum[i];
+      for (int j=0; j < num; j++) {
+        d->act_dot[act_last-num+1+j] = ctrl[uadr+j];
+      }
       break;
+    }
 
     case mjDYN_FILTER:              // linear filter: dynprm = tau
     case mjDYN_FILTEREXACT:
@@ -550,6 +567,51 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     int uadr = m->actuator_ctrladr[i];
     int oadr = m->actuator_outadr[i];
 
+    // SO(3) geodesic servo: 3 or 4 inputs and 3 outputs on an SO3 transmission
+    if (m->actuator_gaintype[i] == mjGAIN_SO3) {
+      mjtNum q_tgt[4];
+
+      // quat input: normalize ctrl directly (zero maps to the identity)
+      if (m->actuator_ctrlspec[i] == mjCHART_QUAT) {
+        mju_copy4(q_tgt, ctrl + uadr);
+        mju_normalize4(q_tgt);
+      }
+
+      // expmap input: ctrl block (position) or act block (integrator)
+      else {
+        mjtNum u[3];
+        if (m->actuator_dyntype[i] == mjDYN_NONE) {
+          mju_copy3(u, ctrl + uadr);
+        } else {
+          int act_adr = m->actuator_actadr[i];
+          if (m->actuator_actearly[i]) {
+            for (int k=0; k < 3; k++) {
+              u[k] = mj_nextActivation(m, d, i, act_adr+k, d->act_dot[act_adr+k]);
+            }
+          } else {
+            mju_copy3(u, d->act + act_adr);
+          }
+        }
+        expmap2Quat(q_tgt, u);
+      }
+
+      // error rotation from current to target: e = log(q_cur^-1 * q_tgt), in the local frame
+      // of the transmission, matching the frame of the moment rows and of actuator_velocity
+      // note: the force is invariant to the setpoint representative (exp is ray-periodic),
+      // so no wrapping is required; act is re-anchored at integration time in mj_advance
+      mjtNum q_cur[4], e[3];
+      expmap2Quat(q_cur, d->actuator_length + oadr);
+      mju_subQuat(e, q_tgt, q_cur);
+
+      // output force: kp * error + constant - kv * velocity
+      mjtNum kp = m->actuator_gainprm[mjNGAIN*i];
+      const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
+      for (int k=0; k < 3; k++) {
+        force[oadr+k] = kp*e[k] + prm[0] + prm[2]*d->actuator_velocity[oadr+k];
+      }
+      continue;
+    }
+
     // check for tendon transmission with force limits
     if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) {
       tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
@@ -561,7 +623,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     mjtGain gaintype = m->actuator_gaintype[i];
     int actnum = m->actuator_actnum[i];
 
-    // handle according to gain type
+    // handle SISO actuators according to gain type
     switch (gaintype) {
     case mjGAIN_FIXED:              // fixed gain: prm = gain
       gain = gainprm[0];
@@ -614,6 +676,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
       break;
     }
 
+    case mjGAIN_SO3:                // handled above via early continue
+      mjERROR("mjGAIN_SO3 reached SISO switch (actuator %d)", i);
+      break;
+
     default:                        // user gain
       if (mjcb_act_gain) {
         gain = mjcb_act_gain(m, d, i);
@@ -751,7 +817,29 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
   }
 
   // clamp actuator_force
-  clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nout, NULL);
+  for (int i=0; i < nactuator; i++) {
+    if (!m->actuator_forcelimited[i]) {
+      continue;
+    }
+    const mjtNum* range = m->actuator_forcerange + 2*i;
+    mjtNum* f = force + m->actuator_outadr[i];
+
+    // SO3: clamp the norm of the output torque, preserving its direction
+    if (m->actuator_gaintype[i] == mjGAIN_SO3) {
+      mjtNum norm = mju_norm3(f);
+      if (norm > range[1]) {
+        mju_scl3(f, f, range[1]/norm);
+      }
+    }
+
+    // otherwise: clamp each output
+    else {
+      int outnum = m->actuator_outnum[i];
+      for (int j=0; j < outnum; j++) {
+        f[j] = mju_clip(f[j], range[0], range[1]);
+      }
+    }
+  }
 
   // add DC motor mechanical forces (not subject to current limits)
   for (int i=0; i < nactuator; i++) {
@@ -1151,6 +1239,18 @@ static void mj_advance(const mjModel* m, mjData* d,
         int adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
         d->act[adr] = wrapSetpoint(d->act[adr], d->actuator_length[m->actuator_outadr[i]], period);
       }
+
+      // SO3 servo: re-anchor the act setpoint to the canonical representative
+      else if (m->actuator_gaintype[i] == mjGAIN_SO3) {
+        int adr = m->actuator_actadr[i];
+        mjtNum angle = mju_norm3(d->act + adr);
+        if (angle > mjPI) {
+          mjtNum scale = (angle - 2*mjPI*mju_round(angle/(2*mjPI))) / angle;
+          for (int k=0; k < 3; k++) {
+            d->act[adr+k] *= scale;
+          }
+        }
+      }
     }
   }