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
2026-07-21 11:35:28 -07:00
parent a8545ac7cc
commit 072e963fa0
49 changed files with 1772 additions and 104 deletions
@@ -1301,9 +1301,12 @@ void mj_transmission(const mjModel* m, mjData* d) {
    rowadr[out] = out == 0 ? 0 : rowadr[out-1] + rownnz[out-1];
    int nnz, adr = rowadr[out];

-    // skip sleeping actuator
+    // skip sleeping actuator: zero all rows of its output block
    if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
-      rownnz[out] = 0;
+      for (int k=0; k < m->actuator_outnum[i]; k++) {
+        rowadr[out+k] = out+k == 0 ? 0 : rowadr[out+k-1] + rownnz[out+k-1];
+        rownnz[out+k] = 0;
+      }
      continue;
    }

@@ -1476,6 +1479,97 @@ void mj_transmission(const mjModel* m, mjData* d) {
      }
      break;

+    case mjTRN_SO3:                     // relative orientation: 3 lengths, 3 moment rows
+      // ball joint: lengths = expmap of the joint quaternion, rows = identity at the joint dofs
+      if (m->actuator_trnid[2*i+1] == -1) {
+        mjtNum axis[3], quat[4];
+        mji_copy4(quat, d->qpos+m->jnt_qposadr[id]);
+        mju_normalize4(quat);
+        mji_quat2Vel(axis, quat, 1);
+        for (int k=0; k < 3; k++) {
+          int outk = out + k;
+          rowadr[outk] = k == 0 ? adr : rowadr[outk-1] + rownnz[outk-1];
+          length[outk] = axis[k];
+          rownnz[outk] = 1;
+          colind[rowadr[outk]] = m->jnt_dofadr[id] + k;
+          moment[rowadr[outk]] = 1;
+        }
+      }
+
+      // site+refsite: lengths = relative expmap, rows = relative rotational Jacobian
+      else {
+        int refid = m->actuator_trnid[2*i+1];
+        if (!jacref) jacref = mjSTACKALLOC(d, 3*nv, mjtNum);
+        if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum);
+
+        // relative rotation as expmap in the refsite frame
+        mjtNum quat[4], refquat[4], vec[3];
+        mji_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]);
+        mji_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]);
+        mji_subQuat(vec, quat, refquat);
+
+        // relative rotational Jacobian in global frame
+        mj_jacSite(m, d, NULL, jacS, id);
+        mj_jacSite(m, d, NULL, jacref, refid);
+        mju_subFrom(jacS, jacref, 3*nv);
+
+        // if common ancestral dof exists, clear the columns of its parental chain
+        {
+          int b0 = m->body_weldid[m->site_bodyid[id]];
+          int b1 = m->body_weldid[m->site_bodyid[refid]];
+          int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1;
+          int dofadr1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
+          int dofadr_common = -1;
+          if (dofadr0 >= 0 && dofadr1 >= 0) {
+            while (dofadr0 != dofadr1) {
+              if (dofadr0 < dofadr1) {
+                dofadr1 = m->dof_parentid[dofadr1];
+              } else {
+                dofadr0 = m->dof_parentid[dofadr0];
+              }
+              if (dofadr0 == -1 || dofadr1 == -1) {
+                break;
+              }
+            }
+            if (dofadr0 == dofadr1) {
+              dofadr_common = dofadr0;
+            }
+          }
+          int da = dofadr_common;
+          while (da >= 0) {
+            jacS[nv*0 + da] = 0;
+            jacS[nv*1 + da] = 0;
+            jacS[nv*2 + da] = 0;
+            da = m->dof_parentid[da];
+          }
+        }
+
+        // row k: site axis k in global frame, projected on the relative Jacobian; torque and
+        // velocity components are in the site frame, matching the ball branch's child frame
+        // (the expmap length components are frame-invariant: exp fixes its own axis)
+        for (int k=0; k < 3; k++) {
+          int outk = out + k;
+          rowadr[outk] = k == 0 ? adr : rowadr[outk-1] + rownnz[outk-1];
+          length[outk] = vec[k];
+
+          mjtNum wrench[3] = {d->site_xmat[9*id+k], d->site_xmat[9*id+k+3],
+                              d->site_xmat[9*id+k+6]};
+          mju_mulMatTVec(moment_row, jacS, wrench, 3, nv);
+
+          // sparsity (compress)
+          nnz = 0;
+          for (int j=0; j < nv; j++) {
+            if (moment_row[j]) {
+              moment[rowadr[outk]+nnz] = moment_row[j];
+              colind[rowadr[outk]+nnz] = j;
+              nnz++;
+            }
+          }
+          rownnz[outk] = nnz;
+        }
+      }
+      break;
+
    case mjTRN_SITE:                    // site
      // get site translation (jac) and rotation (jacS) Jacobians in global frame
      mj_jacSite(m, d, jac, jacS, id);
@@ -2016,10 +2016,18 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {

    // skip if force is clamped by forcerange
    if (m->actuator_forcelimited[i]) {
-      mjtNum force = d->actuator_force[oadr];
-      mjtNum* range = m->actuator_forcerange + 2*oadr;
-      if (force <= range[0] || force >= range[1]) {
-        continue;
+      const mjtNum* range = m->actuator_forcerange + 2*i;
+
+      // SO3: force is norm-clamped (approximation: saturated force still varies tangentially)
+      if (m->actuator_gaintype[i] == mjGAIN_SO3) {
+        if (mju_norm3(d->actuator_force + oadr) >= range[1]) {
+          continue;
+        }
+      } else {
+        mjtNum force = d->actuator_force[oadr];
+        if (force <= range[0] || force >= range[1]) {
+          continue;
+        }
      }
    }

@@ -2031,6 +2039,11 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
      bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2];
    }

+    // SO3 geodesic servo: kv term, applied to each output row below
+    else if (m->actuator_biastype[i] == mjBIAS_SO3) {
+      bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2];
+    }
+
    // DC motor bias (back-EMF)
    else if (m->actuator_biastype[i] == mjBIAS_DCMOTOR) {
      const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
@@ -2108,10 +2121,12 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
      }
    }

-    // add
+    // add, once per output row
    if (bias_vel != 0) {
-      addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr,
-                    d->moment_rownnz, d->moment_rowadr, d->moment_colind);
+      for (int k=0; k < m->actuator_outnum[i]; k++) {
+        addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr+k,
+                      d->moment_rownnz, d->moment_rowadr, d->moment_colind);
+      }
    }
  }
 }
@@ -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;
+          }
+        }
+      }
    }
  }

@@ -1390,7 +1390,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
  mju_zero(d->qpos, m->nq);
  mju_zero(d->qvel, m->nv);
  mju_zero(d->act, m->na);
-  mju_zero(d->ctrl, m->nu);
+  mj_resetCtrl(m, d);
  for (int i=0; i < m->neq; i++) d->eq_active[i] = m->eq_active0[i];
  mju_zero(d->qfrc_applied, m->nv);
  mju_zero(d->xfrc_applied, 6*m->nbody);
@@ -1639,6 +1639,17 @@ static void mj_logTimingDiagnostics(const mjData* d) {
 }


+// set ctrl to neutral values: zero, except quaternion inputs which reset to the identity
+void mj_resetCtrl(const mjModel* m, mjData* d) {
+  mju_zero(d->ctrl, m->nu);
+  for (int i=0; i < m->nactuator; i++) {
+    if (m->actuator_gaintype[i] == mjGAIN_SO3 && m->actuator_ctrlspec[i] == mjCHART_QUAT) {
+      d->ctrl[m->actuator_ctrladr[i]] = 1;
+    }
+  }
+}
+
+
 // clear data, set data->qpos = model->qpos0
 void mj_resetData(const mjModel* m, mjData* d) {
  // emit step timing diagnostics before timers are cleared
@@ -2139,6 +2150,18 @@ const char* mj_validateReferences(const mjModel* m) {
        return "Invalid model: actuator_trnid out of bounds.";
      }
      break;
+    case mjTRN_SO3:
+      // ball joint target (idslider == -1) or site + refsite target
+      if (idslider == -1) {
+        if (id < 0 || id >= m->njnt) {
+          return "Invalid model: actuator_trnid out of bounds.";
+        }
+      } else {
+        if (id < 0 || id >= m->nsite || idslider < 0 || idslider >= m->nsite) {
+          return "Invalid model: actuator_trnid out of bounds.";
+        }
+      }
+      break;
    case mjTRN_UNDEFINED:
      // actuator_trnid not used.
      break;
@@ -124,6 +124,9 @@ MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
 // copy mjData, skip large arrays not required for abstract visualization
 MJAPI mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);

+// set ctrl to neutral values: zero, except quaternion inputs which reset to the identity
+MJAPI void mj_resetCtrl(const mjModel* m, mjData* d);
+
 // set data to defaults
 MJAPI void mj_resetData(const mjModel* m, mjData* d);

@@ -281,3 +281,21 @@ const char* mj_id2name(const mjModel* m, int type, int id) {

  return NULL;
 }
+
+
+// get name of actuator input, determined by the actuator type and input signature,
+// returns NULL if the actuator type defines no input names
+const char* mj_actuatorInputName(const mjModel* m, int id, int input) {
+  if (id < 0 || id >= m->nactuator || input < 0 || input >= m->actuator_ctrlnum[id]) {
+    return NULL;
+  }
+
+  // so3 orientation actuator: input names are chart components
+  if (m->actuator_gaintype[id] == mjGAIN_SO3) {
+    static const char* expmap[3] = {"rx", "ry", "rz"};
+    static const char* quat[4] = {"qw", "qx", "qy", "qz"};
+    return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input];
+  }
+
+  return NULL;
+}
@@ -34,6 +34,9 @@ MJAPI int mj_name2id(const mjModel* m, int type, const char* name);

 // get name of object with the specified mjtObj type and id, returns NULL if name not found
 MJAPI const char* mj_id2name(const mjModel* m, int type, int id);
+
+// get name of actuator input; NULL if the actuator type defines no input names
+MJAPI const char* mj_actuatorInputName(const mjModel* m, int id, int input);
 #ifdef __cplusplus
 }
 #endif
@@ -649,8 +649,8 @@ static void mj_computeSensorPos(const mjModel* m, mjData* d, int i, mjtNum* sens
    sensordata[0] = d->ten_length[objid];
    break;

-  case mjSENS_ACTUATORPOS:                            // actuator position
-    sensordata[0] = d->actuator_length[m->actuator_outadr[objid]];
+  case mjSENS_ACTUATORPOS:                            // actuator position, one per output
+    mju_copy(sensordata, d->actuator_length + m->actuator_outadr[objid], m->sensor_dim[i]);
    break;

  case mjSENS_BALLQUAT:                               // ball joint quaternion
@@ -878,8 +878,8 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens
    sensordata[0] = d->ten_velocity[objid];
    break;

-  case mjSENS_ACTUATORVEL:                            // actuator velocity
-    sensordata[0] = d->actuator_velocity[m->actuator_outadr[objid]];
+  case mjSENS_ACTUATORVEL:                            // actuator velocity, one per output
+    mju_copy(sensordata, d->actuator_velocity + m->actuator_outadr[objid], m->sensor_dim[i]);
    break;

  case mjSENS_BALLANGVEL:                             // ball joint angular velocity
@@ -1302,8 +1302,8 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens
    mju_copy3(sensordata, tmp);
    break;

-  case mjSENS_ACTUATORFRC:                            // actuator force
-    sensordata[0] = d->actuator_force[m->actuator_outadr[objid]];
+  case mjSENS_ACTUATORFRC:                            // actuator force, one per output
+    mju_copy(sensordata, d->actuator_force + m->actuator_outadr[objid], m->sensor_dim[i]);
    break;

  case mjSENS_JOINTACTFRC:                            // actuator force at joint
@@ -349,6 +349,9 @@ static void setFixed(mjModel* m, mjData* d) {
    case mjTRN_JOINTINPARENT:
      bodyid = m->jnt_bodyid[tid];
      break;
+    case mjTRN_SO3:
+      bodyid = m->actuator_trnid[2*i+1] >= 0 ? m->site_bodyid[tid] : m->jnt_bodyid[tid];
+      break;
    case mjTRN_SITE:
    case mjTRN_SLIDERCRANK:
      bodyid = m->site_bodyid[tid];
@@ -676,6 +676,15 @@ static mjtSleepState mj_actuatorSleepState(const mjModel* m, const mjData* d, in
  case mjTRN_SITE:
    return mj_sleepState(m, d, mjOBJ_SITE, trnid);

+  case mjTRN_SO3:
+    // ball joint target or site + refsite target
+    if (m->actuator_trnid[i*2+1] == -1) {
+      return mj_sleepState(m, d, mjOBJ_JOINT, trnid);
+    }
+    s1 = mj_sleepState(m, d, mjOBJ_SITE, trnid);
+    s2 = mj_sleepState(m, d, mjOBJ_SITE, m->actuator_trnid[i*2+1]);
+    return (s1 == mjS_AWAKE || s2 == mjS_AWAKE) ? mjS_AWAKE : mjS_ASLEEP;
+
  case mjTRN_BODY:
    return mj_sleepState(m, d, mjOBJ_BODY, trnid);