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

doc/APIreference/APItypes.rst

@@ -347,6 +347,16 @@ Actuator bias types. These values are used in ``m->actuator_biastype``.
 .. mujoco-include:: mjtBias
 
 
+.. _mjtCtrlChart:
+
+mjtCtrlChart
+~~~~~~~~~~~~
+
+Orientation input charts of so3 actuators. These values are used in ``m->actuator_ctrlspec``.
+
+.. mujoco-include:: mjtCtrlChart
+
+
 .. _mjtObj:
 
 mjtObj

doc/APIreference/functions.rst

@@ -603,6 +603,16 @@ Get id of object with the specified :ref:`mjtObj` type and name, returns -1 if i
 
 Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`` if name not found.
 
+.. _mj_actuatorInputName:
+
+`mj_actuatorInputName <#mj_actuatorInputName>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_actuatorInputName
+
+Get name of actuator input, determined by the actuator type and input signature;
+return NULL if the actuator type defines no input names.
+
 .. _mj_fullM:
 
 `mj_fullM <#mj_fullM>`__
@@ -1821,6 +1831,15 @@ m is only required to contain the size fields from MJMODEL_INTS.
 
 Copy mjData, skip large arrays not required for visualization.
 
+.. _mj_resetCtrl:
+
+`mj_resetCtrl <#mj_resetCtrl>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_resetCtrl
+
+Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.
+
 .. _mj_resetData:
 
 `mj_resetData <#mj_resetData>`__
@@ -5258,6 +5277,15 @@ Set actuator to integrated velocity; return error if any.
 
 Set actuator to velocity servo; return error if any.
 
+.. _mjs_setToOrientation:
+
+`mjs_setToOrientation <#mjs_setToOrientation>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mjs_setToOrientation
+
+Set actuator to orientation servo.
+
 .. _mjs_setToDamper:
 
 `mjs_setToDamper <#mjs_setToDamper>`__

doc/XMLreference.rst

@@ -5488,6 +5488,8 @@ specify them independently.
 
 :at:`forcerange`: :at-val:`real(2), "0 0"`
    Range for clamping the force output. The first value must be no greater than the second value.
+   On :ref:`orientation<actuator-orientation>` actuators the force is a 3D torque, clamped on its norm: the second
+   value bounds the torque magnitude and the first value must be 0.
    |br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in
    :ref:`compiler <compiler>`.
 
@@ -5678,7 +5680,7 @@ specify them independently.
 
 .. _actuator-general-gaintype:
 
-:at:`gaintype`: :at-val:`[fixed, affine, muscle, user], "fixed"`
+:at:`gaintype`: :at-val:`[fixed, affine, muscle, so3, user], "fixed"`
    The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be
    affine. As already explained in :ref:`Actuation model <geActuation>`, the general formula is:
    scalar_force = gain_term \* (act or ctrl) + bias_term.
@@ -5691,12 +5693,13 @@ specify them independently.
    fixed   gain_term = gainprm[0]
    affine  gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity
    muscle  gain_term = mju_muscleGain(...)
+   so3     geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-orientation>`
    user    gain_term = mjcb_act_gain(...)
    ======= ===============================
 
 .. _actuator-general-biastype:
 
-:at:`biastype`: :at-val:`[none, affine, muscle, user], "none"`
+:at:`biastype`: :at-val:`[none, affine, muscle, so3, user], "none"`
    The keywords have the following meaning:
 
    ======= ================================================================
@@ -5705,9 +5708,12 @@ specify them independently.
    none    bias_term = 0
    affine  bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity
    muscle  bias_term = mju_muscleBias(...)
+   so3     damping term of the geodesic orientation servo, see :ref:`orientation<actuator-orientation>`
    user    bias_term = mjcb_act_bias(...)
    ======= ================================================================
 
+   Note that :at:`gaintype` and :at:`biastype` must either both be "so3" or neither.
+
 .. _actuator-general-dynprm:
 
 :at:`dynprm`: :at-val:`real(10), "1 0 ... 0"`
@@ -5731,6 +5737,13 @@ specify them independently.
    so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see
    :ref:`muscle <actuator-muscle>` below.
 
+.. _actuator-general-input:
+
+:at:`input`: :at-val:`string, optional`
+   Input signature of the actuator: which controls make up its control block, recorded in
+   ``mjModel.actuator_ctrlspec``. Available for gaintype "so3", where it selects the orientation chart: "expmap"
+   (3 controls, the default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`.
+
 .. _actuator-general-actearly:
 
 :at:`actearly`: :at-val:`[false, true], "false"`
@@ -5939,6 +5952,102 @@ This element has one custom attribute in addition to the common attributes:
    :ref:`position<actuator-position>` attribute and in the :ref:`default class<default-position-inheritrange>`,
    saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator.
 
+.. _actuator-orientation:
+
+:el-prefix:`actuator/` |-| **orientation** |*|
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. youtube:: 17XpwnqyCXs
+   :align: right
+   :width: 40%
+
+This element creates an orientation servo: a geodesic PD controller on a relative orientation, targeting a ball
+:ref:`joint<actuator-general-joint>` or a :ref:`site<actuator-general-site>` with a
+:ref:`refsite<actuator-general-refsite>`. Unlike per-axis :ref:`position<actuator-position>` servos, the servo acts
+jointly on the full orientation: the force is :math:`k_p \log(q^{-1} q_{target}) - k_v \omega`, exact for arbitrary axis
+combinations, with a unique equilibrium at every commanded orientation. The transmission has 3 force outputs; force,
+error and angular velocity are expressed in the child (joint or site) frame. The commanded orientation is given in the
+:ref:`input<actuator-orientation-input>` chart: an exponential-map vector (3 controls, the default) or a quaternion (4
+controls). :ref:`forcerange<actuator-general-forcerange>` clamps the norm of the output torque,
+preserving its direction; the lower bound must be 0.
+:ref:`Actuator sensors<sensor-actuatorpos>` report one value per force output. The integrator variant, which
+stores the orientation setpoint in :ref:`act<siPhysicsState>`, is available via :ref:`general<actuator-general>` with
+:ref:`dyntype<actuator-general-dyntype>` "integrator" and is expmap-only. The video on the right shows this `example
+model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/actuation/orientation.xml>`__.
+The underlying :el:`general` attributes are set as follows:
+
+========= ======= ========= =========
+Attribute Setting Attribute Setting
+========= ======= ========= =========
+dyntype   none    gainprm   kp 0 0
+gaintype  so3     biasprm   0 -kp -kv
+biastype  so3
+========= ======= ========= =========
+
+.. _actuator-orientation-ctrlrange:
+
+:at:`ctrlrange`: :at-val:`real(2), "0 0"`
+   Range for clamping the control input, as described in :ref:`ctrlrange <actuator-general-ctrlrange>`. For this
+   multi-input actuator, the same range limits are replicated and applied independently to each of the 3 (expmap) or 4
+   (quaternion) control inputs in the control block.
+
+.. _actuator-orientation-forcerange:
+
+:at:`forcerange`: :at-val:`real(2), "0 0"`
+   Range for clamping the torque output, as described in :ref:`forcerange <actuator-general-forcerange>`. The torque is
+   clamped on its norm, preserving its direction: the second value bounds the torque magnitude and the first value must
+   be 0.
+
+This element has custom attributes in addition to the common attributes:
+
+.. _actuator-orientation-name:
+
+.. _actuator-orientation-class:
+
+.. _actuator-orientation-group:
+
+.. _actuator-orientation-nsample:
+
+.. _actuator-orientation-interp:
+
+.. _actuator-orientation-delay:
+
+.. _actuator-orientation-forcelimited:
+
+.. _actuator-orientation-user:
+
+.. _actuator-orientation-joint:
+
+.. _actuator-orientation-site:
+
+.. _actuator-orientation-refsite:
+
+.. _actuator-orientation-kp:
+
+:at:`kp`: :at-val:`real, "1"`
+   Position feedback gain, in units of torque per radian of geodesic error.
+
+.. _actuator-orientation-kv:
+
+:at:`kv`: :at-val:`real, "0"`
+   Damping applied by the actuator, per force output.
+   When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
+
+.. _actuator-orientation-dampratio:
+
+:at:`dampratio`: :at-val:`real, "0"`
+   Damping applied by the actuator, using damping ratio units, as for
+   :ref:`position/dampratio<actuator-position-dampratio>`. This attribute is exclusive with :at:`kv`.
+
+.. _actuator-orientation-input:
+
+:at:`input`: :at-val:`[expmap, quat], "expmap"`
+   `Chart <https://en.wikipedia.org/wiki/Manifold#Charts>`__ of the commanded orientation. With "expmap" the control
+   block is an exponential-map vector (3 controls, in radians). With "quat" the control block is a quaternion (4
+   controls, :ref:`w-first <siLayout>`); the commanded quaternion is normalized by the servo, making the force scale-
+   and antipodally-invariant, and the control block resets to the identity quaternion. The quat chart requires
+   ``dyntype="none"``.
+
 .. _actuator-velocity:
 
 :el-prefix:`actuator/` |-| **velocity** |*|
@@ -9907,6 +10016,8 @@ if omitted.
 
 .. _default-general-biasprm:
 
+.. _default-general-input:
+
 .. _default-general-actearly:
 
 :el-prefix:`default/` |-| **general** |?|

doc/XMLschema.rst

@@ -2375,6 +2375,9 @@
             .. grid-item::
                :ref:`actdim<actuator-general-actdim>`
 
+            .. grid-item::
+               :ref:`input<actuator-general-input>`
+
             .. grid-item::
                :ref:`dyntype<actuator-general-dyntype>`
 
@@ -2733,6 +2736,63 @@
                :ref:`dampratio<actuator-intvelocity-dampratio>`
 
 
+      .. dropdown:: :ref:`orientation<actuator-orientation>` |*|
+
+         .. grid:: 2 3 4 4
+            :gutter: 0
+
+            .. grid-item::
+               :ref:`name<actuator-orientation-name>`
+
+            .. grid-item::
+               :ref:`class<actuator-orientation-class>`
+
+            .. grid-item::
+               :ref:`group<actuator-orientation-group>`
+
+            .. grid-item::
+               :ref:`nsample<actuator-orientation-nsample>`
+
+            .. grid-item::
+               :ref:`interp<actuator-orientation-interp>`
+
+            .. grid-item::
+               :ref:`delay<actuator-orientation-delay>`
+
+            .. grid-item::
+               :ref:`forcelimited<actuator-orientation-forcelimited>`
+
+            .. grid-item::
+               :ref:`ctrlrange<actuator-orientation-ctrlrange>`
+
+            .. grid-item::
+               :ref:`forcerange<actuator-orientation-forcerange>`
+
+            .. grid-item::
+               :ref:`user<actuator-orientation-user>`
+
+            .. grid-item::
+               :ref:`joint<actuator-orientation-joint>`
+
+            .. grid-item::
+               :ref:`site<actuator-orientation-site>`
+
+            .. grid-item::
+               :ref:`refsite<actuator-orientation-refsite>`
+
+            .. grid-item::
+               :ref:`kp<actuator-orientation-kp>`
+
+            .. grid-item::
+               :ref:`kv<actuator-orientation-kv>`
+
+            .. grid-item::
+               :ref:`dampratio<actuator-orientation-dampratio>`
+
+            .. grid-item::
+               :ref:`input<actuator-orientation-input>`
+
+
       .. dropdown:: :ref:`damper<actuator-damper>` |*|
 
          .. grid:: 2 3 4 4
@@ -5864,6 +5924,9 @@
             .. grid-item::
                :ref:`actdim<default-general-actdim>`
 
+            .. grid-item::
+               :ref:`input<default-general-input>`
+
             .. grid-item::
                :ref:`dyntype<default-general-dyntype>`
 

doc/changelog.rst

@@ -68,8 +68,11 @@ Engine
 .. admonition:: Breaking ABI changes
    :class: caution
 
+   - :ref:`mjModel` gained the ``actuator_ctrlspec`` field (input signature of each actuator), and :ref:`mjsActuator`
+     gained ``ctrlspec``, changing their size and layout. The :ref:`mjtGain` and :ref:`mjtBias` enums gained ``so3``
+     members, shifting the values of ``mjGAIN_USER`` and ``mjBIAS_USER``.
    - Added ``texid``, ``texuniform`` and ``texrepeat`` fields to :ref:`mjvGeom`.
-   - The :ref:`mjContact`` struct gained an ``adhesion`` member, changing its size and layout.
+   - The :ref:`mjContact` struct gained an ``adhesion`` member, changing its size and layout.
 
 .. admonition:: Bug fixes
    :class: admonition
@@ -93,6 +96,24 @@ Actuation
   :ref:`general<actuator-general>` actuators it defaults to "auto", so activation clamping is enabled by specifying
   ``actrange``. Unclamped integrated setpoints are well-behaved on rotational transmissions, where they wrap.
 
+.. youtube:: 17XpwnqyCXs
+   :align: right
+   :width: 35%
+
+- Added the :ref:`orientation<actuator-orientation>` actuator: a geodesic servo on a new SO(3) transmission (ball
+  joints, or a site with a :ref:`refsite<actuator-general-refsite>`), acting jointly on the full relative orientation
+  with an exact equilibrium at every commanded orientation. This is the first actuator with multiple force outputs
+  (3), and, with ``input="quat"``, the first with different input and output dimensions (4 controls, 3 outputs). The
+  input signature is recorded in the new ``mjModel.actuator_ctrlspec``, exposed as the
+  :ref:`input<actuator-general-input>` attribute.
+- Added :ref:`mj_actuatorInputName`, returning the name of an actuator input (e.g. "qw" for the first control of a
+  quaternion-commanded orientation actuator). The control sliders in :ref:`simulate<saSimulate>` and MuJoCo Studio are
+  now generated per control and labeled with the actuator name plus the input name suffix.
+- Viewer control sliders now use a defined :ref:`ctrlrange<actuator-general-ctrlrange>` even when
+  :ref:`ctrllimited<actuator-general-ctrllimited>` is "false": the range sets the slider span, while clamping remains
+  controlled by :at:`ctrllimited`.
+- Added :ref:`mj_resetCtrl`, setting controls to neutral values: zero, except quaternion inputs which reset to the
+  identity quaternion. Called by :ref:`mj_resetData` and the viewers' "Clear All".
 
 Solvers
 ^^^^^^^

doc/includes/references.h

@@ -1115,6 +1115,7 @@ typedef struct mjModel_ {
   int*      actuator_biastype;    // bias type (mjtBias)                      (nactuator x 1)
   int*      actuator_ctrladr;     // address of first control                 (nactuator x 1)
   int*      actuator_ctrlnum;     // number of controls                       (nactuator x 1)
+  int*      actuator_ctrlspec;    // input signature, scoped by gaintype      (nactuator x 1)
   int*      actuator_outadr;      // address of first force output            (nactuator x 1)
   int*      actuator_outnum;      // number of force outputs, from trntype    (nactuator x 1)
   int*      actuator_actadr;      // first activation address; -1: stateless  (nactuator x 1)
@@ -1136,11 +1137,11 @@ typedef struct mjModel_ {
   int*      actuator_group;       // group for visibility                     (nactuator x 1)
   mjtNum*   actuator_user;        // user data                                (nactuator x nuser_actuator)
   int*      actuator_plugin;      // plugin instance id; -1: not a plugin     (nactuator x 1)
+  mjtBool*  actuator_forcelimited;// is force limited                         (nactuator x 1)
+  mjtNum*   actuator_forcerange;  // range of forces                          (nactuator x 2)
   mjtBool*  actuator_ctrllimited; // is control limited                       (nu x 1)
   mjtNum*   actuator_ctrlrange;   // range of controls                        (nu x 2)
   mjtNum*   actuator_gear;        // scale length and transmitted force       (nout x 6)
-  mjtBool*  actuator_forcelimited;// is force limited                         (nout x 1)
-  mjtNum*   actuator_forcerange;  // range of forces                          (nout x 2)
   mjtNum*   actuator_acc0;        // acceleration from unit force in qpos0    (nout x 1)
   mjtNum*   actuator_length0;     // actuator length in qpos0                 (nout x 1)
   mjtNum*   actuator_lengthrange; // feasible actuator length range           (nout x 2)
@@ -2241,6 +2242,7 @@ typedef struct mjsActuator_ {      // actuator specification
   mjtDyn dyntype;                  // dynamics type
   double dynprm[mjNDYN];           // dynamics parameters
   int actdim;                      // number of activation variables
+  int ctrlspec;                    // input signature, scoped by gaintype; 0: type default
   mjtBool actearly;                // apply next activations to qfrc
 
   // transmission
@@ -2499,6 +2501,7 @@ typedef enum mjtTrn {             // type of actuator transmission
   mjTRN_TENDON,                   // force on tendon
   mjTRN_SITE,                     // force on site
   mjTRN_BODY,                     // adhesion force on a body's geoms
+  mjTRN_SO3,                      // torque on a relative orientation (3 force outputs)
 
   mjTRN_UNDEFINED     = 1000      // undefined transmission type
 } mjtTrn;
@@ -2516,6 +2519,7 @@ typedef enum mjtGain {            // type of actuator gain
   mjGAIN_AFFINE,                  // const + kp*length + kv*velocity
   mjGAIN_MUSCLE,                  // muscle FLV curve computed by mju_muscleGain()
   mjGAIN_DCMOTOR,                 // DC motor gain: K or K/R
+  mjGAIN_SO3,                     // geodesic servo on an SO3 transmission: force = kp * log(error)
   mjGAIN_USER                     // user-defined gain type
 } mjtGain;
 typedef enum mjtBias {            // type of actuator bias
@@ -2523,8 +2527,13 @@ typedef enum mjtBias {            // type of actuator bias
   mjBIAS_AFFINE,                  // const + kp*length + kv*velocity
   mjBIAS_MUSCLE,                  // muscle passive force computed by mju_muscleBias()
   mjBIAS_DCMOTOR,                 // DC motor bias: back-EMF, cogging, LuGre friction
+  mjBIAS_SO3,                     // damping term of the SO3 geodesic servo
   mjBIAS_USER                     // user-defined bias type
 } mjtBias;
+typedef enum mjtCtrlChart {       // so3 input signature (actuator_ctrlspec): orientation chart
+  mjCHART_EXPMAP      = 1,        // exponential-map orientation target: 3 controls
+  mjCHART_QUAT        = 2         // quaternion orientation target: 4 controls
+} mjtCtrlChart;
 typedef enum mjtObj {             // type of MujoCo object
   mjOBJ_UNKNOWN       = 0,        // unknown object type
   mjOBJ_BODY,                     // body
@@ -3500,6 +3509,7 @@ mjtSize mj_sizeModel(const mjModel* m);
 mjData* mj_makeData(const mjModel* m);
 mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
 mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);
+void mj_resetCtrl(const mjModel* m, mjData* d);
 void mj_resetData(const mjModel* m, mjData* d);
 void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
 void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
@@ -3611,6 +3621,7 @@ void mj_jacDot(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
 void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body);
 int mj_name2id(const mjModel* m, int type, const char* name);
 const char* mj_id2name(const mjModel* m, int type, int id);
+const char* mj_actuatorInputName(const mjModel* m, int id, int input);
 void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
 void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
@@ -3970,6 +3981,8 @@ const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1],
 const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1],
                                  double dampratio[1], double timeconst[1], double inheritrange);
 const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
+const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
+                                 double dampratio[1], int ctrlspec);
 const char* mjs_setToDamper(mjsActuator* actuator, double kv);
 const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst,
                               double bias, double area, double diameter);

include/mujoco/mjmodel.h

@@ -784,6 +784,7 @@ typedef struct mjModel_ {
   int*      actuator_biastype;    // bias type (mjtBias)                      (nactuator x 1)
   int*      actuator_ctrladr;     // address of first control                 (nactuator x 1)
   int*      actuator_ctrlnum;     // number of controls                       (nactuator x 1)
+  int*      actuator_ctrlspec;    // input signature, scoped by gaintype      (nactuator x 1)
   int*      actuator_outadr;      // address of first force output            (nactuator x 1)
   int*      actuator_outnum;      // number of force outputs, from trntype    (nactuator x 1)
   int*      actuator_actadr;      // first activation address; -1: stateless  (nactuator x 1)
@@ -805,11 +806,11 @@ typedef struct mjModel_ {
   int*      actuator_group;       // group for visibility                     (nactuator x 1)
   mjtNum*   actuator_user;        // user data                                (nactuator x nuser_actuator)
   int*      actuator_plugin;      // plugin instance id; -1: not a plugin     (nactuator x 1)
+  mjtBool*  actuator_forcelimited;// is force limited                         (nactuator x 1)
+  mjtNum*   actuator_forcerange;  // range of forces                          (nactuator x 2)
   mjtBool*  actuator_ctrllimited; // is control limited                       (nu x 1)
   mjtNum*   actuator_ctrlrange;   // range of controls                        (nu x 2)
   mjtNum*   actuator_gear;        // scale length and transmitted force       (nout x 6)
-  mjtBool*  actuator_forcelimited;// is force limited                         (nout x 1)
-  mjtNum*   actuator_forcerange;  // range of forces                          (nout x 2)
   mjtNum*   actuator_acc0;        // acceleration from unit force in qpos0    (nout x 1)
   mjtNum*   actuator_length0;     // actuator length in qpos0                 (nout x 1)
   mjtNum*   actuator_lengthrange; // feasible actuator length range           (nout x 2)

include/mujoco/mjspec.h

@@ -724,6 +724,7 @@ typedef struct mjsActuator_ {      // actuator specification
   mjtDyn dyntype;                  // dynamics type
   double dynprm[mjNDYN];           // dynamics parameters
   int actdim;                      // number of activation variables
+  int ctrlspec;                    // input signature, scoped by gaintype; 0: type default
   mjtBool actearly;                // apply next activations to qfrc
 
   // transmission

include/mujoco/mjspecmacro.h

@@ -496,6 +496,7 @@
     X   ( mjtDyn,        dyntype,       1         ) \
     XVEC( double,        dynprm,        mjNDYN    ) \
     X   ( int,           actdim,        1         ) \
+    X   ( int,           ctrlspec,      1         ) \
     X   ( mjtBool,       actearly,      1         ) \
     X   ( mjtTrn,        trntype,       1         ) \
     XVEC( double,        gear,          6         ) \

include/mujoco/mjtype.h

@@ -234,6 +234,7 @@ typedef enum mjtTrn {             // type of actuator transmission
   mjTRN_TENDON,                   // force on tendon
   mjTRN_SITE,                     // force on site
   mjTRN_BODY,                     // adhesion force on a body's geoms
+  mjTRN_SO3,                      // torque on a relative orientation (3 force outputs)
 
   mjTRN_UNDEFINED     = 1000      // undefined transmission type
 } mjtTrn;
@@ -255,6 +256,7 @@ typedef enum mjtGain {            // type of actuator gain
   mjGAIN_AFFINE,                  // const + kp*length + kv*velocity
   mjGAIN_MUSCLE,                  // muscle FLV curve computed by mju_muscleGain()
   mjGAIN_DCMOTOR,                 // DC motor gain: K or K/R
+  mjGAIN_SO3,                     // geodesic servo on an SO3 transmission: force = kp * log(error)
   mjGAIN_USER                     // user-defined gain type
 } mjtGain;
 
@@ -264,10 +266,17 @@ typedef enum mjtBias {            // type of actuator bias
   mjBIAS_AFFINE,                  // const + kp*length + kv*velocity
   mjBIAS_MUSCLE,                  // muscle passive force computed by mju_muscleBias()
   mjBIAS_DCMOTOR,                 // DC motor bias: back-EMF, cogging, LuGre friction
+  mjBIAS_SO3,                     // damping term of the SO3 geodesic servo
   mjBIAS_USER                     // user-defined bias type
 } mjtBias;
 
 
+typedef enum mjtCtrlChart {       // so3 input signature (actuator_ctrlspec): orientation chart
+  mjCHART_EXPMAP      = 1,        // exponential-map orientation target: 3 controls
+  mjCHART_QUAT        = 2         // quaternion orientation target: 4 controls
+} mjtCtrlChart;
+
+
 typedef enum mjtObj {             // type of MujoCo object
   mjOBJ_UNKNOWN       = 0,        // unknown object type
   mjOBJ_BODY,                     // body

include/mujoco/mjxmacro.h

@@ -684,6 +684,7 @@
     X   ( int,     actuator_biastype,     nactuator,     1                    ) \
     X   ( int,     actuator_ctrladr,      nactuator,     1                    ) \
     X   ( int,     actuator_ctrlnum,      nactuator,     1                    ) \
+    X   ( int,     actuator_ctrlspec,     nactuator,     1                    ) \
     X   ( int,     actuator_outadr,       nactuator,     1                    ) \
     X   ( int,     actuator_outnum,       nactuator,     1                    ) \
     X   ( int,     actuator_actadr,       nactuator,     1                    ) \
@@ -705,11 +706,11 @@
     X   ( int,     actuator_group,        nactuator,     1                    ) \
     X   ( mjtNum,  actuator_user,         nactuator,     MJ_M(nuser_actuator) ) \
     X   ( int,     actuator_plugin,       nactuator,     1                    ) \
+    X   ( mjtBool, actuator_forcelimited, nactuator,     1                    ) \
+    X   ( mjtNum,  actuator_forcerange,   nactuator,     2                    ) \
     X   ( mjtBool, actuator_ctrllimited,  nu,            1                    ) \
     X   ( mjtNum,  actuator_ctrlrange,    nu,            2                    ) \
     X   ( mjtNum,  actuator_gear,         nout,          6                    ) \
-    X   ( mjtBool, actuator_forcelimited, nout,          1                    ) \
-    X   ( mjtNum,  actuator_forcerange,   nout,          2                    ) \
     X   ( mjtNum,  actuator_acc0,         nout,          1                    ) \
     X   ( mjtNum,  actuator_length0,      nout,          1                    ) \
     X   ( mjtNum,  actuator_lengthrange,  nout,          2                    )

include/mujoco/mujoco.h

@@ -255,6 +255,9 @@ MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
 // Copy mjData, skip large arrays not required for visualization.
 MJAPI mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);
 
+// Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.
+MJAPI void mj_resetCtrl(const mjModel* m, mjData* d);
+
 // Reset data to defaults.
 MJAPI void mj_resetData(const mjModel* m, mjData* d);
 
@@ -601,6 +604,10 @@ MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
 // Get name of object with the specified mjtObj type and id; return NULL if name not found.
 MJAPI const char* mj_id2name(const mjModel* m, int type, int id);
 
+// Get name of actuator input, determined by the actuator type and input signature;
+// return NULL if the actuator type defines no input names.
+MJAPI const char* mj_actuatorInputName(const mjModel* m, int id, int input);
+
 // Convert sparse inertia matrix into full (i.e. dense) matrix.
 MJAPI void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
 
@@ -1745,6 +1752,10 @@ MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double
 // Set actuator to velocity servo; return error if any.
 MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
 
+// Set actuator to orientation servo.
+MJAPI const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
+                                       double dampratio[1], int ctrlspec);
+
 // Set actuator to activate damper; return error if any.
 MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
 

mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py

@@ -1279,11 +1279,11 @@ class Model:
     actuator_history: history buffer sizes                   (nu, 2)
     actuator_historyadr: history buffer address              (nu,)
     actuator_delay: delay in seconds                         (nu,)
+    actuator_forcelimited: is force limited                  (nu,)
+    actuator_forcerange: range of forces                     (*, nu, 2)
     actuator_ctrllimited: is control limited                 (nu,)
     actuator_ctrlrange: range of controls                    (*, nu, 2)
     actuator_gear: scale length and transmitted force        (*, nu, 6)
-    actuator_forcelimited: is force limited                  (nu,)
-    actuator_forcerange: range of forces                     (*, nu, 2)
     actuator_acc0: acceleration from unit force in qpos0     (*, nu)
     actuator_lengthrange: feasible actuator length range     (*, nu, 2)
     sensor_type: sensor type (SensorType)                    (nsensor,)
@@ -1762,11 +1762,11 @@ class Model:
   actuator_history: array("nu", wp.vec2i)
   actuator_historyadr: array("nu", int)
   actuator_delay: array("nu", float)
+  actuator_forcelimited: array("nu", bool)
+  actuator_forcerange: array("*", "nu", wp.vec2)
   actuator_ctrllimited: array("nu", bool)
   actuator_ctrlrange: array("*", "nu", wp.vec2)
   actuator_gear: array("*", "nu", wp.spatial_vector)
-  actuator_forcelimited: array("nu", bool)
-  actuator_forcerange: array("*", "nu", wp.vec2)
   actuator_acc0: array("*", "nu", float)
   actuator_lengthrange: array("*", "nu", wp.vec2)
   sensor_type: array("nsensor", int)

python/mujoco/functions.cc

@@ -166,6 +166,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) {
         InterceptMjErrors(::mj_copyData)(dest, m, src);
       });
   Def<traits::mj_resetData>(pymodule);
+  Def<traits::mj_resetCtrl>(pymodule);
   Def<traits::mj_resetDataDebug>(pymodule);
   Def<traits::mj_resetDataKeyframe>(pymodule);
   // Skipped: mj_stackAllocByte (doesn't make sense in Python)
@@ -589,6 +590,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) {
       });
   Def<traits::mj_name2id>(pymodule);
   Def<traits::mj_id2name>(pymodule);
+  Def<traits::mj_actuatorInputName>(pymodule);
   Def<traits::mj_fullM>(
       pymodule,
       [](const raw::MjModel* m, const raw::MjData* d,

python/mujoco/introspect/enums.py

@@ -251,6 +251,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjTRN_TENDON', 3),
              ('mjTRN_SITE', 4),
              ('mjTRN_BODY', 5),
+             ('mjTRN_SO3', 6),
              ('mjTRN_UNDEFINED', 1000),
          ]),
      )),
@@ -277,7 +278,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjGAIN_AFFINE', 1),
              ('mjGAIN_MUSCLE', 2),
              ('mjGAIN_DCMOTOR', 3),
-             ('mjGAIN_USER', 4),
+             ('mjGAIN_SO3', 4),
+             ('mjGAIN_USER', 5),
          ]),
      )),
     ('mjtBias',
@@ -289,7 +291,17 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjBIAS_AFFINE', 1),
              ('mjBIAS_MUSCLE', 2),
              ('mjBIAS_DCMOTOR', 3),
-             ('mjBIAS_USER', 4),
+             ('mjBIAS_SO3', 4),
+             ('mjBIAS_USER', 5),
+         ]),
+     )),
+    ('mjtCtrlChart',
+     EnumDecl(
+         name='mjtCtrlChart',
+         declname='enum mjtCtrlChart',
+         values=dict([
+             ('mjCHART_EXPMAP', 1),
+             ('mjCHART_QUAT', 2),
          ]),
      )),
     ('mjtObj',

python/mujoco/introspect/functions.py

@@ -1102,6 +1102,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Copy mjData, skip large arrays not required for visualization.',
      )),
+    ('mj_resetCtrl',
+     FunctionDecl(
+         name='mj_resetCtrl',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData'),
+                 ),
+             ),
+         ),
+         doc='Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.',  # pylint: disable=line-too-long
+     )),
     ('mj_resetData',
      FunctionDecl(
          name='mj_resetData',
@@ -3467,6 +3487,30 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Get name of object with the specified mjtObj type and id; return NULL if name not found.',  # pylint: disable=line-too-long
      )),
+    ('mj_actuatorInputName',
+     FunctionDecl(
+         name='mj_actuatorInputName',
+         return_type=PointerType(
+             inner_type=ValueType(name='char', is_const=True),
+         ),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='id',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='input',
+                 type=ValueType(name='int'),
+             ),
+         ),
+         doc='Get name of actuator input, determined by the actuator type and input signature; return NULL if the actuator type defines no input names.',  # pylint: disable=line-too-long
+     )),
     ('mj_fullM',
      FunctionDecl(
          name='mj_fullM',
@@ -10927,6 +10971,44 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Set actuator to velocity servo; return error if any.',
      )),
+    ('mjs_setToOrientation',
+     FunctionDecl(
+         name='mjs_setToOrientation',
+         return_type=PointerType(
+             inner_type=ValueType(name='char', is_const=True),
+         ),
+         parameters=(
+             FunctionParameterDecl(
+                 name='actuator',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjsActuator'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='kp',
+                 type=ValueType(name='double'),
+             ),
+             FunctionParameterDecl(
+                 name='kv',
+                 type=ArrayType(
+                     inner_type=ValueType(name='double'),
+                     extents=(1,),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='dampratio',
+                 type=ArrayType(
+                     inner_type=ValueType(name='double'),
+                     extents=(1,),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='ctrlspec',
+                 type=ValueType(name='int'),
+             ),
+         ),
+         doc='Set actuator to orientation servo.',
+     )),
     ('mjs_setToDamper',
      FunctionDecl(
          name='mjs_setToDamper',

python/mujoco/introspect/structs.py

@@ -4452,6 +4452,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='number of controls',
                  array_extent=('nactuator',),
              ),
+             StructFieldDecl(
+                 name='actuator_ctrlspec',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='input signature, scoped by gaintype',
+                 array_extent=('nactuator',),
+             ),
              StructFieldDecl(
                  name='actuator_outadr',
                  type=PointerType(
@@ -4620,6 +4628,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='plugin instance id; -1: not a plugin',
                  array_extent=('nactuator',),
              ),
+             StructFieldDecl(
+                 name='actuator_forcelimited',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtBool'),
+                 ),
+                 doc='is force limited',
+                 array_extent=('nactuator',),
+             ),
+             StructFieldDecl(
+                 name='actuator_forcerange',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='range of forces',
+                 array_extent=('nactuator', 2),
+             ),
              StructFieldDecl(
                  name='actuator_ctrllimited',
                  type=PointerType(
@@ -4644,22 +4668,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='scale length and transmitted force',
                  array_extent=('nout', 6),
              ),
-             StructFieldDecl(
-                 name='actuator_forcelimited',
-                 type=PointerType(
-                     inner_type=ValueType(name='mjtBool'),
-                 ),
-                 doc='is force limited',
-                 array_extent=('nout',),
-             ),
-             StructFieldDecl(
-                 name='actuator_forcerange',
-                 type=PointerType(
-                     inner_type=ValueType(name='mjtNum'),
-                 ),
-                 doc='range of forces',
-                 array_extent=('nout', 2),
-             ),
              StructFieldDecl(
                  name='actuator_acc0',
                  type=PointerType(
@@ -9658,6 +9666,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='int'),
                  doc='number of activation variables',
              ),
+             StructFieldDecl(
+                 name='ctrlspec',
+                 type=ValueType(name='int'),
+                 doc='input signature, scoped by gaintype; 0: type default',
+             ),
              StructFieldDecl(
                  name='actearly',
                  type=ValueType(name='mjtBool'),

python/mujoco/specs.cc

@@ -1554,6 +1554,19 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) {
         }
       },
       py::arg("kv"));
+  mjsActuator.def(
+      "set_to_orientation",
+      [](raw::MjsActuator* self, double kp, double kv, double dampratio,
+         int ctrlspec) {
+        std::string err = mjs_setToOrientation(
+            self, kp, kv == -1 ? nullptr : &kv,
+            dampratio == -1 ? nullptr : &dampratio, ctrlspec);
+        if (!err.empty()) {
+          throw pybind11::value_error(err);
+        }
+      },
+      py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1,
+      py::arg("ctrlspec") = 0);
   mjsActuator.def(
       "set_to_damper",
       [](raw::MjsActuator* self, double kv) {

python/mujoco/specs_test.py

@@ -1588,6 +1588,14 @@ class SpecsTest(absltest.TestCase):
     self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE)
     self.assertEqual(actuator.inheritrange, True)
 
+    actuator.set_to_orientation(kp=2.0, dampratio=1.0)
+    self.assertEqual(actuator.gainprm[0], 2)
+    self.assertEqual(actuator.biasprm[1], -2)
+    self.assertEqual(actuator.biasprm[2], 1)
+    self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_SO3)
+    self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_SO3)
+    self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
+
     actuator.set_to_velocity(kv=5.0)
     self.assertEqual(actuator.gainprm[0], 5)
     self.assertEqual(actuator.biasprm[2], -5)

simulate/simulate.cc

@@ -2160,7 +2160,8 @@ void Simulate::Sync(bool state_only) {
 
   for (int i = 0; i < m_->nu; ++i) {
     std::optional<std::pair<mjtNum, mjtNum>> range;
-    if (m_->actuator_ctrllimited[i]) {
+    // a defined ctrlrange sets the slider range, even when ctrl is not clamped
+    if (m_->actuator_ctrlrange[2*i] < m_->actuator_ctrlrange[2*i + 1]) {
       range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]);
     }
     if (actuator_ctrlrange_[i] != range) {
@@ -2304,7 +2305,7 @@ void Simulate::Sync(bool state_only) {
   }
 
   if (pending_.zero_ctrl) {
-    mju_zero(d_->ctrl, m_->nu);
+    mj_resetCtrl(m_, d_);
     pending_.zero_ctrl = false;
   }
 
@@ -2534,14 +2535,11 @@ void Simulate::LoadOnRenderThread() {
     jnt_names_.emplace_back(name ? name : "");
   }
 
-  actuator_group_.resize(this->m_->nu);
-  std::memcpy(actuator_group_.data(), this->m_->actuator_group,
-              sizeof(this->m_->actuator_group[0]) * this->m_->nu);
-
   actuator_ctrlrange_.clear();
   actuator_ctrlrange_.reserve(this->m_->nu);
   for (int i = 0; i < this->m_->nu; ++i) {
-    if (this->m_->actuator_ctrllimited[i]) {
+    // a defined ctrlrange sets the slider range, even when ctrl is not clamped
+    if (this->m_->actuator_ctrlrange[2 * i] < this->m_->actuator_ctrlrange[2 * i + 1]) {
       actuator_ctrlrange_.push_back(std::make_pair(
           this->m_->actuator_ctrlrange[2 * i], this->m_->actuator_ctrlrange[2 * i + 1]));
     } else {
@@ -2549,11 +2547,23 @@ void Simulate::LoadOnRenderThread() {
     }
   }
 
+  // per-control group and name; multi-input actuators suffix the input name
+  actuator_group_.resize(this->m_->nu);
   actuator_names_.clear();
   actuator_names_.reserve(this->m_->nu);
-  for (int i = 0; i < this->m_->nu; ++i) {
+  for (int i = 0; i < this->m_->nactuator; ++i) {
-    const char* name = mj_id2name(this->m_, mjOBJ_ACTUATOR, i);
+    const char* actname = mj_id2name(this->m_, mjOBJ_ACTUATOR, i);
-    actuator_names_.emplace_back(name ? name : "");
+    int ctrlnum = this->m_->actuator_ctrlnum[i];
+    for (int k = 0; k < ctrlnum; ++k) {
+      actuator_group_[this->m_->actuator_ctrladr[i] + k] = this->m_->actuator_group[i];
+      std::string name = actname ? actname : "";
+      if (ctrlnum > 1 && actname) {
+        const char* input_name = mj_actuatorInputName(this->m_, i, k);
+        name += '/';
+        name += input_name ? input_name : std::to_string(k);
+      }
+      actuator_names_.emplace_back(std::move(name));
+    }
   }
 
   equality_names_.clear();

src/engine/engine_core_smooth.c

@@ -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);

src/engine/engine_derivative.c

@@ -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];
+      const mjtNum* range = m->actuator_forcerange + 2*i;
-      mjtNum* range = m->actuator_forcerange + 2*oadr;
+
-      if (force <= range[0] || force >= range[1]) {
+      // SO3: force is norm-clamped (approximation: saturated force still varies tangentially)
-        continue;
+      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,
+      for (int k=0; k < m->actuator_outnum[i]; k++) {
-                    d->moment_rownnz, d->moment_rowadr, d->moment_colind);
+        addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr+k,
+                      d->moment_rownnz, d->moment_rowadr, d->moment_colind);
+      }
     }
   }
 }

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
+    case mjDYN_INTEGRATOR: {        // simple integrator, one per control
-      d->act_dot[act_last] = ctrl[uadr];
+      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;
+          }
+        }
+      }
     }
   }
 

src/engine/engine_io.c

@@ -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;

src/engine/engine_io.h

@@ -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);
 

src/engine/engine_name.c

@@ -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;
+}

src/engine/engine_name.h

@@ -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

src/engine/engine_sensor.c

@@ -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
+  case mjSENS_ACTUATORPOS:                            // actuator position, one per output
-    sensordata[0] = d->actuator_length[m->actuator_outadr[objid]];
+    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
+  case mjSENS_ACTUATORVEL:                            // actuator velocity, one per output
-    sensordata[0] = d->actuator_velocity[m->actuator_outadr[objid]];
+    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
+  case mjSENS_ACTUATORFRC:                            // actuator force, one per output
-    sensordata[0] = d->actuator_force[m->actuator_outadr[objid]];
+    mju_copy(sensordata, d->actuator_force + m->actuator_outadr[objid], m->sensor_dim[i]);
     break;
 
   case mjSENS_JOINTACTFRC:                            // actuator force at joint

src/engine/engine_setconst.c

@@ -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];

src/engine/engine_sleep.c

@@ -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);
 

src/experimental/platform/ux/gui.cc

@@ -1295,17 +1295,17 @@ void JointsGui(const mjModel* model, const mjData* data,
   ImGui::PopItemWidth();
 }
 
-void ControlsGui(const mjModel* model, const mjData* data,
+void ControlsGui(const mjModel* model, mjData* data,
                  const mjvOption* vis_options) {
   const float item_width = ImGui::GetWindowWidth() * .6f;
   ImGui::PushItemWidth(item_width);
 
   if (ImGui::Button("Clear All")) {
-    mju_zero(data->ctrl, model->nu);
+    mj_resetCtrl(model, data);
   }
 
   char name[100];
-  for (int i = 0; i < model->nu; i++) {
+  for (int i = 0; i < model->nactuator; i++) {
     int group = std::clamp(model->actuator_group[i], 0, mjNGROUP - 1);
     if (!vis_options->actuatorgroup[group]) {
       continue;
@@ -1315,25 +1315,38 @@ void ControlsGui(const mjModel* model, const mjData* data,
       continue;
     }
 
-    const char* ctrl_name = mj_id2name(model, mjOBJ_ACTUATOR, i);
+    // one slider per control; multi-input actuators suffix the input name
-    if (ctrl_name) {
+    const char* act_name = mj_id2name(model, mjOBJ_ACTUATOR, i);
-      std::snprintf(name, sizeof(name), "%s", ctrl_name);
+    int ctrlnum = model->actuator_ctrlnum[i];
-    } else {
+    for (int k = 0; k < ctrlnum; k++) {
-      std::snprintf(name, sizeof(name), "control %d", i);
+      int j = model->actuator_ctrladr[i] + k;
-    }
+      if (act_name && ctrlnum > 1) {
-
+        const char* input_name = mj_actuatorInputName(model, i, k);
-    double min = -1.0;
+        if (input_name) {
-    double max = 1.0;
+          std::snprintf(name, sizeof(name), "%s/%s", act_name, input_name);
-    if (model->actuator_ctrllimited[i]) {
+        } else {
-      min = model->actuator_ctrlrange[2 * i + 0];
+          std::snprintf(name, sizeof(name), "%s/%d", act_name, k);
-      max = model->actuator_ctrlrange[2 * i + 1];
+        }
-    }
+      } else if (act_name) {
-    ImGui_Slider(name, &data->ctrl[i], min, max);
+        std::snprintf(name, sizeof(name), "%s", act_name);
-    if (ImGui::BeginPopupContextItem()) {
+      } else {
-      if (ImGui::MenuItem("Reset to 0")) {
+        std::snprintf(name, sizeof(name), "control %d", j);
-        data->ctrl[i] = mju_clip(0.0, min, max);
+      }
+
+      double min = -1.0;
+      double max = 1.0;
+      // a defined ctrlrange sets the slider range, even when ctrl is not clamped
+      if (model->actuator_ctrlrange[2 * j] < model->actuator_ctrlrange[2 * j + 1]) {
+        min = model->actuator_ctrlrange[2 * j + 0];
+        max = model->actuator_ctrlrange[2 * j + 1];
+      }
+      ImGui_Slider(name, &data->ctrl[j], min, max);
+      if (ImGui::BeginPopupContextItem()) {
+        if (ImGui::MenuItem("Reset to 0")) {
+          data->ctrl[j] = mju_clip(0.0, min, max);
+        }
+        ImGui::EndPopup();
       }
-      ImGui::EndPopup();
     }
   }
 

src/experimental/platform/ux/gui.h

@@ -125,7 +125,7 @@ void VisualizationGui(mjModel* model, mjvOption* vis_options, mjvCamera* camera,
                       float min_width);
 
 // UX for visualizing actuator controls data in mjData.
-void ControlsGui(const mjModel* model, const mjData* data,
+void ControlsGui(const mjModel* model, mjData* data,
                  const mjvOption* vis_options);
 
 // UX for visualizing joint data in mjData.

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:

src/xml/xml_base.h

@@ -41,6 +41,7 @@ extern const int colorspace_sz;
 extern const int builtin_sz;
 extern const int mark_sz;
 extern const int dyn_sz;
+extern const int input_sz;
 extern const int gain_sz;
 extern const int bias_sz;
 extern const int interp_sz;
@@ -75,6 +76,7 @@ extern const mjMap texrole_map[];
 extern const mjMap builtin_map[];
 extern const mjMap mark_map[];
 extern const mjMap dyn_map[];
+extern const mjMap input_map[];
 extern const mjMap gain_map[];
 extern const mjMap bias_map[];
 extern const mjMap interp_map[];

src/xml/xml_native_reader.cc

@@ -258,7 +258,7 @@ std::vector<const char*> MJCF[nMJCF] = {
             "margin", "stiffness", "damping", "rgba", "user"},
         {"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
             "actrange", "gear", "damping", "armature", "cranklength", "user", "group", "nsample",
-            "interp", "delay", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
+            "interp", "delay", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
             "biasprm", "actearly"},
         {"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay"},
@@ -476,7 +476,7 @@ std::vector<const char*> MJCF[nMJCF] = {
             "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
             "lengthrange", "gear", "damping", "armature", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
-            "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
+            "body", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
             "actearly"},
         {"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -498,6 +498,10 @@ std::vector<const char*> MJCF[nMJCF] = {
             "gear", "damping", "armature", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
             "kp", "kv", "dampratio"},
+        {"orientation", "*", "name", "class", "group", "nsample", "interp", "delay",
+            "forcelimited", "ctrlrange", "forcerange", "user",
+            "joint", "site", "refsite",
+            "kp", "kv", "dampratio", "input"},
         {"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
             "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "damping", "armature", "cranklength", "user",
@@ -840,23 +844,33 @@ const mjMap dcmotorinput_map[dcmotorinput_sz] = {
 
 
 // gain type
-const int gain_sz = 5;
+const int gain_sz = 6;
 const mjMap gain_map[gain_sz] = {
   {"fixed",         mjGAIN_FIXED},
   {"affine",        mjGAIN_AFFINE},
   {"muscle",        mjGAIN_MUSCLE},
   {"dcmotor",       mjGAIN_DCMOTOR},
+  {"so3",           mjGAIN_SO3},
   {"user",          mjGAIN_USER}
 };
 
 
+// so3 input chart
+const int input_sz = 2;
+const mjMap input_map[input_sz] = {
+  {"expmap",        mjCHART_EXPMAP},
+  {"quat",          mjCHART_QUAT}
+};
+
+
 // bias type
-const int bias_sz = 5;
+const int bias_sz = 6;
 const mjMap bias_map[bias_sz] = {
   {"none",          mjBIAS_NONE},
   {"affine",        mjBIAS_AFFINE},
   {"muscle",        mjBIAS_MUSCLE},
   {"dcmotor",       mjBIAS_DCMOTOR},
+  {"so3",           mjBIAS_SO3},
   {"user",          mjBIAS_USER}
 };
 
@@ -2514,6 +2528,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
     ReadAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, text, false, false);
     ReadAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, text, false, false);
     ReadAttrInt(elem, "actdim", &actuator->actdim);
+    if (MapValue(elem, "input", &n, input_map, input_sz)) {
+      actuator->ctrlspec = n;
+    }
   }
 
   // direct drive motor
@@ -2558,6 +2575,32 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
     }
   }
 
+  // orientation servo: geodesic PD on an SO3 transmission
+  else if (type == "orientation") {
+    double kp = actuator->gainprm[0];
+    ReadAttr(elem, "kp", 1, &kp, text);
+
+    double kv_data;
+    double *kv = &kv_data;
+    if (!ReadAttr(elem, "kv", 1, kv, text)) {
+      kv = nullptr;
+    }
+
+    double dampratio_data;
+    double *dampratio = &dampratio_data;
+    if (!ReadAttr(elem, "dampratio", 1, dampratio, text)) {
+      dampratio = nullptr;
+    }
+
+    // input chart: expmap (default) or quat
+    int n;
+    if (MapValue(elem, "input", &n, input_map, input_sz)) {
+      actuator->ctrlspec = n;
+    }
+
+    err = mjs_setToOrientation(actuator, kp, kv, dampratio, actuator->ctrlspec);
+  }
+
   // velocity servo
   else if (type == "velocity") {
     double kv = actuator->gainprm[0];
@@ -3122,6 +3165,7 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS*
              name == "velocity"    ||
              name == "damper"      ||
              name == "intvelocity" ||
+             name == "orientation" ||
              name == "cylinder"    ||
              name == "muscle"      ||
              name == "adhesion"    ||

src/xml/xml_native_reader.h

@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
 };
 
 // MJCF schema
-#define nMJCF 248
+#define nMJCF 249
 extern std::vector<const char*> MJCF[nMJCF];
 
 #endif  // MUJOCO_SRC_XML_XML_NATIVE_READER_H_

src/xml/xml_native_writer.cc

@@ -899,6 +899,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
   // non-plugins: write actuator parameters
   else {
     WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
+    WriteAttrKey(elem, "input", input_map, input_sz, actuator->ctrlspec, def->Actuator().ctrlspec);
     WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
     WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
     WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);

test/engine/engine_core_smooth_test.cc

@@ -38,6 +38,8 @@ namespace {
 using ::std::string;
 using ::std::vector;
 using ::testing::Each;
+using ::testing::HasSubstr;
+using ::testing::IsNull;
 using ::testing::ElementsAre;
 using ::testing::Not;
 using ::testing::NotNull;
@@ -782,6 +784,600 @@ TEST_F(CoreSmoothTest, ForwardDoesNotMutateAct) {
   mj_deleteData(data);
 }
 
+// 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);
+  }
+}
+
+// geodesic distance between the orientations given by expmap vectors u and v
+static mjtNum GeodesicError(const mjtNum u[3], const mjtNum v[3]) {
+  mjtNum q_tgt[4], q_cur[4], q_err[4], e[3];
+  Expmap2Quat(q_tgt, u);
+  Expmap2Quat(q_cur, v);
+  mju_negQuat(q_cur, q_cur);
+  mju_mulQuat(q_err, q_tgt, q_cur);
+  mju_quat2Vel(e, q_err, 1);
+  return mju_norm3(e);
+}
+
+// mixed model: three scalar translation servos and one SO3 orientation servo
+static constexpr char kSO3RefsiteXml[] = R"(
+<mujoco>
+  <option integrator="implicitfast">
+    <flag contact="disable" gravity="disable"/>
+  </option>
+  <worldbody>
+    <site name="reference"/>
+    <body name="box">
+      <freejoint/>
+      <geom type="box" size=".05 .07 .03"/>
+      <site name="end_effector"/>
+    </body>
+  </worldbody>
+  <actuator>
+    <position name="x" site="end_effector" refsite="reference" gear="1 0 0 0 0 0"
+              kp="100" dampratio="1"/>
+    <orientation name="orient" site="end_effector" refsite="reference" kp="1" dampratio="1"/>
+    <position name="y" site="end_effector" refsite="reference" gear="0 1 0 0 0 0"
+              kp="100" dampratio="1"/>
+    <position name="z" site="end_effector" refsite="reference" gear="0 0 1 0 0 0"
+              kp="100" dampratio="1"/>
+  </actuator>
+  <sensor>
+    <actuatorpos actuator="orient"/>
+    <actuatorfrc actuator="orient"/>
+  </sensor>
+</mujoco>
+)";
+
+// Layout of a mixed model: the SO3 actuator owns 3-wide control and output
+// blocks, misaligning nu/nout/nactuator with the actuator index; sensors on it
+// are 3-dimensional; the model round-trips through XML.
+TEST_F(CoreSmoothTest, SO3MixedModelLayout) {
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+
+  // counts: 4 actuators, 3+3 controls, 3+3 force outputs
+  EXPECT_EQ(model->nactuator, 4);
+  EXPECT_EQ(model->nu, 6);
+  EXPECT_EQ(model->nout, 6);
+
+  // the orientation actuator is second, exercising address accumulation
+  int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
+  ASSERT_EQ(orient, 1);
+  EXPECT_EQ(model->actuator_ctrladr[orient], 1);
+  EXPECT_EQ(model->actuator_ctrlnum[orient], 3);
+  EXPECT_EQ(model->actuator_outadr[orient], 1);
+  EXPECT_EQ(model->actuator_outnum[orient], 3);
+  EXPECT_EQ(model->actuator_trntype[orient], mjTRN_SO3);
+
+  // actuator sensors report one value per force output
+  EXPECT_EQ(model->sensor_dim[0], 3);
+  EXPECT_EQ(model->sensor_dim[1], 3);
+  EXPECT_EQ(model->sensor_adr[1], 3);
+
+  // XML round-trip preserves the layout
+  std::string saved = SaveAndReadXml(model.get());
+  MjModelPtr model2 = LoadModelFromString(saved.c_str(), error, sizeof(error));
+  ASSERT_THAT(model2.get(), NotNull()) << error;
+  EXPECT_EQ(model2->nactuator, 4);
+  EXPECT_EQ(model2->nu, 6);
+  EXPECT_EQ(model2->nout, 6);
+  EXPECT_EQ(model2->actuator_trntype[orient], mjTRN_SO3);
+}
+
+// A mixed-axis orientation target beyond the pi shell is a true equilibrium:
+// zero force when the body is at the commanded orientation, convergence to it
+// from the initial state.
+TEST_F(CoreSmoothTest, SO3RefsiteMixedAxisEquilibrium) {
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  mjData* data = mj_makeData(model.get());
+
+  int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
+  int uadr = model->actuator_ctrladr[orient];
+  int oadr = model->actuator_outadr[orient];
+
+  // target: 5.66 rad rotation about the mixed axis (1,1,0)/sqrt(2), beyond pi;
+  // canonical (shortest) expmap is u*(1 - 2*pi/norm(u)) = (-.4429, -.4429, 0)
+  mjtNum target[3] = {4, 4, 0};
+  mjtNum shrink = 1 - 2*mjPI/mju_norm3(target);
+  mjtNum canonical[3] = {target[0]*shrink, target[1]*shrink, target[2]*shrink};
+  mju_copy3(data->ctrl + uadr, target);
+
+  // place the body exactly at the commanded orientation: force must vanish
+  Expmap2Quat(data->qpos + 3, target);
+  mj_forward(model.get(), data);
+  for (int k=0; k < 3; k++) {
+    EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
+    EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - canonical[k]),
+              MjTol(1e-10, 1e-6));
+
+    // sensors: actuatorpos = canonical expmap, actuatorfrc = 0
+    EXPECT_LT(mju_abs(data->sensordata[k] - canonical[k]), MjTol(1e-10, 1e-6));
+    EXPECT_LT(mju_abs(data->sensordata[3 + k]), MjTol(1e-10, 1e-6));
+  }
+
+  // from the initial state, converge to the commanded orientation
+  mj_resetData(model.get(), data);
+  mju_copy3(data->ctrl + uadr, target);
+  while (data->time < 10) {
+    mj_step(model.get(), data);
+  }
+  for (int k=0; k < 3; k++) {
+    EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - canonical[k]), 1e-3);
+    EXPECT_LT(mju_abs(data->actuator_velocity[oadr + k]), 1e-3);
+  }
+
+  mj_deleteData(data);
+}
+
+// Test smooth tracking while winding one axis with another axis held nonzero:
+// the regime where per-axis servo errors cannot work and only the geodesic
+// error on SO(3) tracks correctly.
+TEST_F(CoreSmoothTest, SO3RefsiteTracksMixedWindingTarget) {
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  mjData* data = mj_makeData(model.get());
+
+  int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
+  int uadr = model->actuator_ctrladr[orient];
+  int oadr = model->actuator_outadr[orient];
+
+  // hold rx at 1 rad, let the servo settle
+  data->ctrl[uadr] = 1;
+  while (data->time < 2) {
+    mj_step(model.get(), data);
+  }
+
+  // ramp the rz target from 0 to 2*pi
+  const mjtNum rate = 0.5;  // rad/s
+  mjtNum start = data->time;
+  while (data->time - start < 2*mjPI / rate) {
+    data->ctrl[uadr + 2] = rate * (data->time - start);
+    mj_step(model.get(), data);
+
+    // geodesic distance between commanded and current orientation
+    mjtNum err = GeodesicError(data->ctrl + uadr, data->actuator_length + oadr);
+    ASSERT_LT(err, 0.5) << "tracking lost at time " << data->time
+                        << ", target rz " << data->ctrl[uadr + 2];
+  }
+
+  mj_deleteData(data);
+}
+
+// Mixed-axis target beyond the pi shell on ball joints: the SO3 actuator has
+// an exact equilibrium at the commanded orientation, per-axis wrapped servos
+// do not.
+TEST_F(CoreSmoothTest, SO3BallMixedAxisContrast) {
+  constexpr char kOrientationPath[] =
+      "engine/testdata/actuation/orientation.xml";
+  const std::string xml_path = GetTestDataFilePath(kOrientationPath);
+  char error[1024];
+  MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
+  ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
+  mjData* data = mj_makeData(model.get());
+
+  // place both joints exactly at the target orientation, expmap (4, 4, 0)
+  mjtNum target[3] = {4, 4, 0};
+  mjtNum quat[4];
+  Expmap2Quat(quat, target);
+  for (const char* name : {"peraxis", "expmap"}) {
+    int jnt = mj_name2id(model.get(), mjOBJ_JOINT, name);
+    mju_copy4(data->qpos + model->jnt_qposadr[jnt], quat);
+  }
+
+  int rx_peraxis = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rx_peraxis");
+  int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
+  int uadr = model->actuator_ctrladr[orient];
+  int oadr = model->actuator_outadr[orient];
+  data->ctrl[rx_peraxis] = data->ctrl[rx_peraxis + 1] = 4;
+  data->ctrl[uadr] = data->ctrl[uadr + 1] = 4;
+  mj_forward(model.get(), data);
+
+  // SO3: zero force at the commanded orientation
+  for (int k=0; k < 3; k++) {
+    EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
+  }
+
+  // per-axis: residual force kp*(4.443 - 2*pi) = -1.84 on the wound members
+  EXPECT_GT(mju_abs(data->actuator_force[rx_peraxis]), 1);
+  EXPECT_GT(mju_abs(data->actuator_force[rx_peraxis + 1]), 1);
+
+  mj_deleteData(data);
+}
+
+// Geodesic servo converges to large mixed-axis targets from rest: no limit
+// cycles. Regression test: a parent-frame error driving child-frame torques is
+// non-gradient feedback which pumps energy at large angles, settling into
+// steady spinning.
+TEST_F(CoreSmoothTest, SO3LargeAngleConvergence) {
+  constexpr char kOrientationPath[] =
+      "engine/testdata/actuation/orientation.xml";
+  const std::string xml_path = GetTestDataFilePath(kOrientationPath);
+  char error[1024];
+  MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
+  ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
+  mjData* data = mj_makeData(model.get());
+
+  int expmap = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
+  int jnt = mj_name2id(model.get(), mjOBJ_JOINT, "expmap");
+  int uadr = model->actuator_ctrladr[expmap];
+  int dofadr = model->jnt_dofadr[jnt];
+
+  mjtNum targets[4][3] = {{1, 1, 1}, {-1, -1, 0}, {1, -1, 1}, {2.2, 2.2, 2.2}};
+  for (const auto& u : targets) {
+    SCOPED_TRACE(testing::Message()
+                 << "target (" << u[0] << ", " << u[1] << ", " << u[2] << ")");
+    mj_resetData(model.get(), data);
+    mju_copy3(data->ctrl + uadr, u);
+    for (int i = 0; i < 4000; i++) {
+      mj_step(model.get(), data);
+    }
+
+    // orientation error and angular velocity vanish
+    mjtNum q_tgt[4], q_cur[4], e[3];
+    mjtNum axis[3] = {u[0], u[1], u[2]};
+    mjtNum angle = mju_normalize3(axis);
+    mju_axisAngle2Quat(q_tgt, axis, angle);
+    mju_copy4(q_cur, data->qpos + model->jnt_qposadr[jnt]);
+    mju_normalize4(q_cur);
+    mju_subQuat(e, q_tgt, q_cur);
+    EXPECT_LT(mju_norm3(e), MjTol(1e-4, 1e-2));
+    EXPECT_LT(mju_norm3(data->qvel + dofadr), MjTol(1e-4, 1e-2));
+  }
+  mj_deleteData(data);
+}
+
+// Neutral ctrl: reset zeroes all controls except quat (to the identity).
+TEST_F(CoreSmoothTest, SO3QuatNeutralCtrl) {
+  constexpr char kOrientationPath[] =
+      "engine/testdata/actuation/orientation.xml";
+  const std::string xml_path = GetTestDataFilePath(kOrientationPath);
+  char error[1024];
+  MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
+  ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
+  mjData* data = mj_makeData(model.get());
+
+  int quat = mj_name2id(model.get(), mjOBJ_ACTUATOR, "quat");
+  int uadr = model->actuator_ctrladr[quat];
+  for (int trial = 0; trial < 2; trial++) {
+    for (int j = 0; j < model->nu; j++) {
+      EXPECT_EQ(data->ctrl[j], j == uadr ? 1 : 0)
+          << "ctrl " << j << " trial " << trial;
+    }
+    mju_fill(data->ctrl, 0.5, model->nu);
+    mj_resetData(model.get(), data);
+  }
+  mj_deleteData(data);
+}
+
+// Input names: NULL for single-input actuators, chart components for SO3.
+TEST_F(CoreSmoothTest, ActuatorInputNames) {
+  constexpr char kOrientationPath[] =
+      "engine/testdata/actuation/orientation.xml";
+  const std::string xml_path = GetTestDataFilePath(kOrientationPath);
+  char error[1024];
+  MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
+  ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
+
+  int rx_peraxis = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rx_peraxis");
+  int expmap = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
+  int quat = mj_name2id(model.get(), mjOBJ_ACTUATOR, "quat");
+  EXPECT_EQ(mj_actuatorInputName(model.get(), rx_peraxis, 0), nullptr);
+  EXPECT_STREQ(mj_actuatorInputName(model.get(), expmap, 0), "rx");
+  EXPECT_STREQ(mj_actuatorInputName(model.get(), expmap, 2), "rz");
+  EXPECT_EQ(mj_actuatorInputName(model.get(), expmap, 3),
+            nullptr);  // out of range
+  EXPECT_STREQ(mj_actuatorInputName(model.get(), quat, 0), "qw");
+  EXPECT_STREQ(mj_actuatorInputName(model.get(), quat, 3), "qz");
+}
+
+// SO3 integrator variant: act is the 3D orientation setpoint; constant ctrl
+// produces steady rotation over many periods with bounded activation.
+TEST_F(CoreSmoothTest, SO3IntVelocityWindsWithBoundedAct) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option integrator="implicitfast">
+      <flag contact="disable" gravity="disable"/>
+    </option>
+    <worldbody>
+      <body>
+        <joint name="ball" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <general name="rot" joint="ball" dyntype="integrator"
+               gaintype="so3" biastype="so3" gainprm="1" biasprm="0 -1 -1"/>
+    </actuator>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+
+  // one actuator: 3 controls, 3 outputs, 3 activations
+  EXPECT_EQ(model->nu, 3);
+  EXPECT_EQ(model->nout, 3);
+  EXPECT_EQ(model->na, 3);
+
+  mjData* data = mj_makeData(model.get());
+
+  // spin about z for 4 full turns
+  const mjtNum rate = 1.0;  // rad/s
+  data->ctrl[2] = rate;
+  while (data->time < 8*mjPI / rate) {
+    mj_step(model.get(), data);
+    ASSERT_LT(mju_norm3(data->act), mjPI + 0.1) << "act unbounded";
+  }
+
+  // steady rotation at the commanded rate about z
+  EXPECT_NEAR(data->actuator_velocity[2], rate, 0.02);
+
+  mj_deleteData(data);
+}
+
+// Compile-time validation of the SO3 actuator.
+TEST_F(CoreSmoothTest, SO3CompileErrors) {
+  char error[1024];
+
+  // hinge joint target: rejected
+  static constexpr char hinge_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="hinge"/>
+        <geom size=".05"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <orientation joint="hinge" kp="1"/>
+    </actuator>
+  </mujoco>
+  )";
+  MjModelPtr model = LoadModelFromString(hinge_xml, error, sizeof(error));
+  EXPECT_THAT(model.get(), IsNull());
+  EXPECT_THAT(error, HasSubstr("ball"));
+
+  // site without refsite: rejected
+  static constexpr char nosite_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <freejoint/>
+        <geom size=".05"/>
+        <site name="ee"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <orientation site="ee" kp="1"/>
+    </actuator>
+  </mujoco>
+  )";
+  model = LoadModelFromString(nosite_xml, error, sizeof(error));
+  EXPECT_THAT(model.get(), IsNull());
+  EXPECT_THAT(error, HasSubstr("refsite"));
+
+  // mismatched gaintype/biastype: rejected
+  static constexpr char mismatch_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="ball" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <general joint="ball" gaintype="so3" biastype="affine" gainprm="1"/>
+    </actuator>
+  </mujoco>
+  )";
+  model = LoadModelFromString(mismatch_xml, error, sizeof(error));
+  EXPECT_THAT(model.get(), IsNull());
+  EXPECT_THAT(error, HasSubstr("both"));
+
+  // forcerange with nonzero lower bound: rejected (force clamped on the norm)
+  static constexpr char forcerange_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="ball" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <orientation joint="ball" kp="1" forcerange="-4 4"/>
+    </actuator>
+  </mujoco>
+  )";
+  model = LoadModelFromString(forcerange_xml, error, sizeof(error));
+  EXPECT_THAT(model.get(), IsNull());
+  EXPECT_THAT(error, HasSubstr("lower bound must be 0"));
+}
+
+// forcerange clamps the norm of the SO3 output torque, preserving direction.
+TEST_F(CoreSmoothTest, SO3ForcerangeClampsNorm) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag contact="disable" gravity="disable"/>
+    </option>
+    <worldbody>
+      <body>
+        <joint name="unclamped" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+      <body pos="0 0 .3">
+        <joint name="clamped" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <orientation name="unclamped" joint="unclamped" kp="1"/>
+      <orientation name="clamped" joint="clamped" kp="1" forcerange="0 .5"/>
+    </actuator>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  mjData* data = mj_makeData(model.get());
+
+  // command the same mixed-axis target, error norm sqrt(5) > 0.5
+  int unclamped = mj_name2id(model.get(), mjOBJ_ACTUATOR, "unclamped");
+  int clamped = mj_name2id(model.get(), mjOBJ_ACTUATOR, "clamped");
+  for (int i : {unclamped, clamped}) {
+    int uadr = model->actuator_ctrladr[i];
+    data->ctrl[uadr + 0] = 1;
+    data->ctrl[uadr + 1] = 2;
+    data->ctrl[uadr + 2] = 0;
+  }
+  mj_forward(model.get(), data);
+
+  // clamped force has norm forcerange[1], parallel to the unclamped force
+  const mjtNum* f_unclamped =
+      data->actuator_force + model->actuator_outadr[unclamped];
+  const mjtNum* f_clamped =
+      data->actuator_force + model->actuator_outadr[clamped];
+  mjtNum norm_unclamped = mju_norm3(f_unclamped);
+  EXPECT_GT(norm_unclamped, 0.5);
+  EXPECT_NEAR(mju_norm3(f_clamped), 0.5, MjTol(1e-12, 1e-6));
+  mjtNum scale = 0.5 / norm_unclamped;
+  for (int k = 0; k < 3; k++) {
+    EXPECT_NEAR(f_clamped[k], scale * f_unclamped[k], MjTol(1e-12, 1e-6));
+  }
+
+  mj_deleteData(data);
+}
+
+// Quat-setpoint variant of the SO3 servo: 4 inputs, 3 outputs -- the first
+// actuator with different input and output widths.
+TEST_F(CoreSmoothTest, SO3QuatSetpoint) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option integrator="implicitfast">
+      <flag contact="disable" gravity="disable"/>
+    </option>
+    <worldbody>
+      <body pos="-.15 0 .2">
+        <joint name="ball_scalar" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+      <body pos=".15 0 .2">
+        <joint name="ball_quat" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <position name="rz" joint="ball_scalar" gear="0 0 1" kp="1" dampratio="1"/>
+      <orientation name="orient" joint="ball_quat" kp="1" dampratio="1" input="quat"/>
+    </actuator>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+
+  // rectangular layout: 2 actuators, 1+4 controls, 1+3 force outputs
+  EXPECT_EQ(model->nactuator, 2);
+  EXPECT_EQ(model->nu, 5);
+  EXPECT_EQ(model->nout, 4);
+  int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
+  int uadr = model->actuator_ctrladr[orient];
+  int oadr = model->actuator_outadr[orient];
+  EXPECT_EQ(uadr, 1);
+  EXPECT_EQ(model->actuator_ctrlnum[orient], 4);
+  EXPECT_EQ(oadr, 1);
+  EXPECT_EQ(model->actuator_outnum[orient], 3);
+
+  // XML round-trip preserves the input chart
+  std::string saved = SaveAndReadXml(model.get());
+  MjModelPtr model2 = LoadModelFromString(saved.c_str(), error, sizeof(error));
+  ASSERT_THAT(model2.get(), NotNull()) << error;
+  EXPECT_EQ(model2->nu, 5);
+  EXPECT_EQ(model2->nout, 4);
+
+  mjData* data = mj_makeData(model.get());
+
+  // zero ctrl commands the identity orientation: zero force at qpos0
+  mj_forward(model.get(), data);
+  for (int k=0; k < 3; k++) {
+    EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
+  }
+
+  // target beyond the pi shell, mixed axis
+  mjtNum target[3] = {4, 4, 0};
+  mjtNum q_tgt[4];
+  Expmap2Quat(q_tgt, target);
+  int jnt = mj_name2id(model.get(), mjOBJ_JOINT, "ball_quat");
+
+  // scale and antipodal invariance: q, 2q and -q command the same orientation
+  mjtNum ctrl_variants[3][4];
+  mju_copy4(ctrl_variants[0], q_tgt);
+  for (int k=0; k < 4; k++) {
+    ctrl_variants[1][k] = 2*q_tgt[k];
+    ctrl_variants[2][k] = -q_tgt[k];
+  }
+  for (int v=0; v < 3; v++) {
+    mj_resetData(model.get(), data);
+    mju_copy4(data->qpos + model->jnt_qposadr[jnt], q_tgt);
+    mju_copy4(data->ctrl + uadr, ctrl_variants[v]);
+    mj_forward(model.get(), data);
+    for (int k=0; k < 3; k++) {
+      EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6))
+          << "variant " << v;
+    }
+  }
+
+  // from the initial state, converge to the commanded orientation
+  mj_resetData(model.get(), data);
+  mju_copy4(data->ctrl + uadr, q_tgt);
+  while (data->time < 10) {
+    mj_step(model.get(), data);
+  }
+  mjtNum shrink = 1 - 2*mjPI/mju_norm3(target);
+  for (int k=0; k < 3; k++) {
+    EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - target[k] * shrink),
+              1e-3);
+    EXPECT_LT(mju_abs(data->actuator_velocity[oadr + k]), 1e-3);
+  }
+
+  mj_deleteData(data);
+}
+
+// The quat input chart requires stateless dynamics.
+TEST_F(CoreSmoothTest, SO3QuatSetpointRequiresStateless) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="ball" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <general joint="ball" dyntype="integrator" input="quat"
+               gaintype="so3" biastype="so3" gainprm="1" biasprm="0 -1 -1"/>
+    </actuator>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  EXPECT_THAT(model.get(), IsNull());
+  EXPECT_THAT(error, HasSubstr("dyntype"));
+}
+
 static const char* const kInertiaPath = "engine/testdata/inertia.xml";
 
 TEST_F(CoreSmoothTest, FactorI) {

test/engine/engine_derivative_test.cc

@@ -1226,6 +1226,59 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
       << "when forcerange derivatives are correctly handled";
 }
 
+// forcelimited actuator following a multi-output SO3 actuator: the derivative
+// skip for saturated actuators must index forcerange per actuator, not per
+// output.
+TEST_F(DerivativeTest, ForcerangeClampedAfterSO3) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag contact="disable" gravity="disable"/>
+    </option>
+    <worldbody>
+      <body>
+        <joint name="ball" type="ball"/>
+        <geom type="box" size=".05 .07 .03"/>
+      </body>
+      <body pos="0 0 .3">
+        <joint name="hinge"/>
+        <geom size=".05"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <orientation joint="ball" kp="1" kv="1"/>
+      <velocity joint="hinge" kv="10" forcerange="-1 1"/>
+    </actuator>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model.get(), NotNull()) << error;
+  MjDataPtr data = MakeData(model);
+  mjModel* m = model.get();
+  mjData* d = data.get();
+
+  // spin the hinge so the velocity actuator saturates: force -50, clamped -1
+  mjtNum qvel[4] = {0.1, 0.2, 0.3, 5};
+  mju_copy(d->qvel, qvel, 4);
+  mj_forward(m, d);
+  ASSERT_EQ(d->actuator_force[3], -1);
+
+  // analytic qDeriv
+  mju_zero(d->qDeriv, m->nD);
+  mjd_smooth_vel(m, d, /*flg_bias=*/1);
+  vector<mjtNum> qDerivAnalytic = AsVector(d->qDeriv, m->nD);
+  EXPECT_GT(mju_norm(qDerivAnalytic.data(), m->nD), 0);
+
+  // expect match with finite differences: the saturated actuator contributes
+  // nothing, the SO3 actuator's damping is unaffected by its neighbor
+  mjtNum eps = MjTol(1e-7, 1e-3);
+  mju_zero(d->qDeriv, m->nD);
+  mjd_smooth_velFD(m, d, eps);
+  EXPECT_THAT(AsVector(d->qDeriv, m->nD),
+              Pointwise(MjNear(1e-7, 3e-3), qDerivAnalytic));
+}
+
 TEST_F(DerivativeTest, NonlinearDampingDerivative) {
   static constexpr char xml[] = R"(
   <mujoco>

test/engine/testdata/actuation/orientation.xml

@@ -0,0 +1,78 @@
+<mujoco>
+  <compiler autolimits="true"/>
+
+  <option integrator="implicitfast">
+    <flag contact="disable" gravity="disable"/>
+  </option>
+
+  <statistic meansize=".05" extent=".9" center="0 0 .15"/>
+
+  <visual>
+    <headlight diffuse=".1 .1 .1" ambient=".15 .15 .15"/>
+  </visual>
+
+  <!--
+  Three identical ball-jointed boxes, illustrating the three rotational setpoint semantics.
+  The "per-axis" box has three independent position servos: each setpoint is interpreted on
+  its own circle (wrapped), exact for rotations about a single gear axis. The "expmap" and
+  "quat" boxes each have a single orientation actuator: the setpoints are interpreted
+  jointly as an orientation on SO(3), exact for any combination of axes, commanded in the
+  exponential-map and quaternion charts respectively.
+  -->
+
+  <asset>
+    <texture name="checker" type="cube" builtin="checker" rgb1=".75 .55 .28" rgb2=".22 .18 .14" width="32" height="32"/>
+    <material name="checker" texture="checker"/>
+    <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
+    <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
+  </asset>
+
+  <default>
+    <geom type="box" size=".07 .09 .05" material="checker"/>
+    <position kp="5" dampratio="1"/>
+    <default class="angle">
+      <position ctrlrange="-6.2832 6.2832" ctrllimited="false"/>
+    </default>
+    <default class="quat">
+      <position ctrlrange="-1 1" ctrllimited="true"/>
+    </default>
+    <site size="1e-5"/>
+  </default>
+
+  <worldbody>
+    <light pos="0 0 10" type="point" diffuse=".75 .72 .65" intensity="1200000" range="20" attenuation="1 0 .04"/>
+    <light pos="-.3 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="13" attenuation="1 0 0" exponent="60"/>
+    <light pos="0 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="16" exponent="60" attenuation="1 0 .5"/>
+    <light pos=".3 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="16" exponent="60" attenuation="1 0 .5"/>
+
+    <light type="point" pos="0 -1.5 2" diffuse=".3 .35 .45" intensity="15000" range="8" castshadow="false" attenuation="1 0 .3"/>
+
+    <geom name="floor" type="plane" size="2 2 .025" material="grid"/>
+
+    <site name="per-axis" pos="-.3 0 .4"/>
+    <body name="peraxis" pos="-.3 0 .2">
+      <joint name="peraxis" type="ball"/>
+      <geom/>
+    </body>
+
+    <site name="expmap" pos="0 0 .4"/>
+    <body name="expmap" pos="0 0 .2">
+      <joint name="expmap" type="ball"/>
+      <geom/>
+    </body>
+
+    <site name="quat" pos=".3 0 .4"/>
+    <body name="quat" pos=".3 0 .2">
+      <joint name="quat" type="ball"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <position name="rx_peraxis" joint="peraxis" gear="1 0 0" class="angle"/>
+    <position name="ry_peraxis" joint="peraxis" gear="0 1 0" class="angle"/>
+    <position name="rz_peraxis" joint="peraxis" gear="0 0 1" class="angle"/>
+    <orientation name="expmap" joint="expmap" kp="1" dampratio="1" class="angle"/>
+    <orientation name="quat" joint="quat" input="quat" kp="1" dampratio="1" class="quat"/>
+  </actuator>
+</mujoco>

test/user/user_api_test.cc

@@ -483,6 +483,36 @@ TEST_F(MujocoTest, SetToDCMotorLuGre) {
   mj_deleteSpec(spec);
 }
 
+TEST_F(MujocoTest, SetToOrientation) {
+  mjSpec* spec = mj_makeSpec();
+  mjsActuator* actuator = mjs_addActuator(spec, 0);
+
+  // kv variant, default (expmap) chart
+  double kv = 2.0;
+  const char* err = mjs_setToOrientation(actuator, 5.0, &kv, nullptr, 0);
+  EXPECT_STREQ(err, "");
+  EXPECT_EQ(actuator->gaintype, mjGAIN_SO3);
+  EXPECT_EQ(actuator->biastype, mjBIAS_SO3);
+  EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
+  EXPECT_EQ(actuator->gainprm[0], 5.0);
+  EXPECT_EQ(actuator->biasprm[1], -5.0);
+  EXPECT_EQ(actuator->biasprm[2], -2.0);
+  EXPECT_EQ(actuator->ctrlspec, 0);
+
+  // dampratio variant, quat chart
+  double dampratio = 1.0;
+  err = mjs_setToOrientation(actuator, 5.0, nullptr, &dampratio, mjCHART_QUAT);
+  EXPECT_STREQ(err, "");
+  EXPECT_EQ(actuator->biasprm[2], 1.0);
+  EXPECT_EQ(actuator->ctrlspec, mjCHART_QUAT);
+
+  // kv and dampratio are mutually exclusive
+  err = mjs_setToOrientation(actuator, 5.0, &kv, &dampratio, 0);
+  EXPECT_STREQ(err, "kv and dampratio cannot both be defined");
+
+  mj_deleteSpec(spec);
+}
+
 static constexpr char xml_plugin_1[] = R"(
   <mujoco model="MuJoCo Model">
     <worldbody>

unity/Runtime/Bindings/MjBindings.cs

@@ -265,6 +265,7 @@ public enum mjtTrn : int{
   mjTRN_TENDON = 3,
   mjTRN_SITE = 4,
   mjTRN_BODY = 5,
+  mjTRN_SO3 = 6,
   mjTRN_UNDEFINED = 1000,
 }
 public enum mjtDyn : int{
@@ -281,14 +282,20 @@ public enum mjtGain : int{
   mjGAIN_AFFINE = 1,
   mjGAIN_MUSCLE = 2,
   mjGAIN_DCMOTOR = 3,
-  mjGAIN_USER = 4,
+  mjGAIN_SO3 = 4,
+  mjGAIN_USER = 5,
 }
 public enum mjtBias : int{
   mjBIAS_NONE = 0,
   mjBIAS_AFFINE = 1,
   mjBIAS_MUSCLE = 2,
   mjBIAS_DCMOTOR = 3,
-  mjBIAS_USER = 4,
+  mjBIAS_SO3 = 4,
+  mjBIAS_USER = 5,
+}
+public enum mjtCtrlChart : int{
+  mjCHART_EXPMAP = 1,
+  mjCHART_QUAT = 2,
 }
 public enum mjtObj : int{
   mjOBJ_UNKNOWN = 0,
@@ -1497,6 +1504,7 @@ public unsafe struct mjModel_ {
   public int* actuator_biastype;
   public int* actuator_ctrladr;
   public int* actuator_ctrlnum;
+  public int* actuator_ctrlspec;
   public int* actuator_outadr;
   public int* actuator_outnum;
   public int* actuator_actadr;
@@ -1518,11 +1526,11 @@ public unsafe struct mjModel_ {
   public int* actuator_group;
   public double* actuator_user;
   public int* actuator_plugin;
+  public byte* actuator_forcelimited;
+  public double* actuator_forcerange;
   public byte* actuator_ctrllimited;
   public double* actuator_ctrlrange;
   public double* actuator_gear;
-  public byte* actuator_forcelimited;
-  public double* actuator_forcerange;
   public double* actuator_acc0;
   public double* actuator_length0;
   public double* actuator_lengthrange;
@@ -6809,6 +6817,9 @@ public static unsafe extern mjData_* mj_copyData(mjData_* dest, mjModel_* m, mjD
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern mjData_* mjv_copyData(mjData_* dest, mjModel_* m, mjData_* src);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern void mj_resetCtrl(mjModel_* m, mjData_* d);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_resetData(mjModel_* m, mjData_* d);
 
@@ -7085,6 +7096,9 @@ public static unsafe extern int mj_name2id(mjModel_* m, int type, [MarshalAs(Unm
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern IntPtr mj_id2name(mjModel_* m, int type, int id);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern IntPtr mj_actuatorInputName(mjModel_* m, int id, int input);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_fullM(mjModel_* m, mjData_* d, double* dst);
 

wasm/codegen/generated/bindings.cc

@@ -1436,6 +1436,10 @@ void mj_RungeKutta_wrapper(const MjModel& m, MjData& d, int N) {
   mj_RungeKutta(m.get(), d.get(), N);
 }
 
+std::string mj_actuatorInputName_wrapper(const MjModel& m, int id, int input) {
+  return std::string(mj_actuatorInputName(m.get(), id, input));
+}
+
 int mj_addContact_wrapper(const MjModel& m, MjData& d, const MjContact& con) {
   return mj_addContact(m.get(), d.get(), con.get());
 }
@@ -1935,6 +1939,10 @@ void mj_referenceConstraint_wrapper(const MjModel& m, MjData& d) {
   mj_referenceConstraint(m.get(), d.get());
 }
 
+void mj_resetCtrl_wrapper(const MjModel& m, MjData& d) {
+  mj_resetCtrl(m.get(), d.get());
+}
+
 void mj_resetData_wrapper(const MjModel& m, MjData& d) {
   mj_resetData(m.get(), d.get());
 }
@@ -2909,6 +2917,12 @@ std::string mjs_setToMuscle_wrapper(MjsActuator& actuator, const val& timeconst,
   return std::string(mjs_setToMuscle(actuator.get(), timeconst_.data(), tausmooth, range_.data(), force, scale, lmin, lmax, vmax, fpmax, fvmax));
 }
 
+std::string mjs_setToOrientation_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, int ctrlspec) {
+  UNPACK_VALUE(double, kv);
+  UNPACK_VALUE(double, dampratio);
+  return std::string(mjs_setToOrientation(actuator.get(), kp, kv_.data(), dampratio_.data(), ctrlspec));
+}
+
 std::string mjs_setToPosition_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, const val& timeconst, double inheritrange) {
   UNPACK_VALUE(double, kv);
   UNPACK_VALUE(double, dampratio);
@@ -3877,6 +3891,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjBIAS_AFFINE", mjBIAS_AFFINE)
     .value("mjBIAS_MUSCLE", mjBIAS_MUSCLE)
     .value("mjBIAS_DCMOTOR", mjBIAS_DCMOTOR)
+    .value("mjBIAS_SO3", mjBIAS_SO3)
     .value("mjBIAS_USER", mjBIAS_USER);
   enum_<mjtBuiltin>("mjtBuiltin")
     .value("mjBUILTIN_NONE", mjBUILTIN_NONE)
@@ -3957,6 +3972,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjCNSTRSTATE_LINEARNEG", mjCNSTRSTATE_LINEARNEG)
     .value("mjCNSTRSTATE_LINEARPOS", mjCNSTRSTATE_LINEARPOS)
     .value("mjCNSTRSTATE_CONE", mjCNSTRSTATE_CONE);
+  enum_<mjtCtrlChart>("mjtCtrlChart")
+    .value("mjCHART_EXPMAP", mjCHART_EXPMAP)
+    .value("mjCHART_QUAT", mjCHART_QUAT);
   enum_<mjtDataType>("mjtDataType")
     .value("mjDATATYPE_REAL", mjDATATYPE_REAL)
     .value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE)
@@ -4057,6 +4075,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjGAIN_AFFINE", mjGAIN_AFFINE)
     .value("mjGAIN_MUSCLE", mjGAIN_MUSCLE)
     .value("mjGAIN_DCMOTOR", mjGAIN_DCMOTOR)
+    .value("mjGAIN_SO3", mjGAIN_SO3)
     .value("mjGAIN_USER", mjGAIN_USER);
   enum_<mjtGeom>("mjtGeom")
     .value("mjGEOM_PLANE", mjGEOM_PLANE)
@@ -4419,6 +4438,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjTRN_TENDON", mjTRN_TENDON)
     .value("mjTRN_SITE", mjTRN_SITE)
     .value("mjTRN_BODY", mjTRN_BODY)
+    .value("mjTRN_SO3", mjTRN_SO3)
     .value("mjTRN_UNDEFINED", mjTRN_UNDEFINED);
   enum_<mjtVisFlag>("mjtVisFlag")
     .value("mjVIS_CONVEXHULL", mjVIS_CONVEXHULL)
@@ -4806,6 +4826,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .property("actuator_ctrllimited", &MjModel::actuator_ctrllimited)
     .property("actuator_ctrlnum", &MjModel::actuator_ctrlnum)
     .property("actuator_ctrlrange", &MjModel::actuator_ctrlrange)
+    .property("actuator_ctrlspec", &MjModel::actuator_ctrlspec)
     .property("actuator_damping", &MjModel::actuator_damping)
     .property("actuator_dampingpoly", &MjModel::actuator_dampingpoly)
     .property("actuator_delay", &MjModel::actuator_delay)
@@ -5582,6 +5603,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference())
     .property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference())
     .property("ctrlrange", &MjsActuator::ctrlrange)
+    .property("ctrlspec", &MjsActuator::ctrlspec, &MjsActuator::set_ctrlspec, reference())
     .property("damping", &MjsActuator::damping)
     .property("delay", &MjsActuator::delay, &MjsActuator::set_delay, reference())
     .property("dynprm", &MjsActuator::dynprm)
@@ -6241,6 +6263,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
 
   function("mj_Euler", &mj_Euler_wrapper);
   function("mj_RungeKutta", &mj_RungeKutta_wrapper);
+  function("mj_actuatorInputName", &mj_actuatorInputName_wrapper);
   function("mj_addContact", &mj_addContact_wrapper);
   function("mj_addM", &mj_addM_wrapper);
   function("mj_angmomMat", &mj_angmomMat_wrapper);
@@ -6331,6 +6354,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
   function("mj_readCtrl", &mj_readCtrl_wrapper);
   function("mj_referenceConstraint", &mj_referenceConstraint_wrapper);
   function("mj_resetCallbacks", &mj_resetCallbacks);
+  function("mj_resetCtrl", &mj_resetCtrl_wrapper);
   function("mj_resetData", &mj_resetData_wrapper);
   function("mj_resetDataDebug", &mj_resetDataDebug_wrapper);
   function("mj_resetDataKeyframe", &mj_resetDataKeyframe_wrapper);
@@ -6482,6 +6506,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
   function("mjs_setToIntVelocity", &mjs_setToIntVelocity_wrapper);
   function("mjs_setToMotor", &mjs_setToMotor_wrapper);
   function("mjs_setToMuscle", &mjs_setToMuscle_wrapper);
+  function("mjs_setToOrientation", &mjs_setToOrientation_wrapper);
   function("mjs_setToPosition", &mjs_setToPosition_wrapper);
   function("mjs_setToVelocity", &mjs_setToVelocity_wrapper);
   function("mjs_wrapGeom", &mjs_wrapGeom_wrapper);

wasm/codegen/generated/bindings.h

@@ -5463,6 +5463,9 @@ struct MjModel {
   emscripten::val actuator_ctrlnum() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_ctrlnum));
   }
+  emscripten::val actuator_ctrlspec() const {
+    return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_ctrlspec));
+  }
   emscripten::val actuator_outadr() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_outadr));
   }
@@ -5526,6 +5529,12 @@ struct MjModel {
   emscripten::val actuator_plugin() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_plugin));
   }
+  emscripten::val actuator_forcelimited() const {
+    return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator, ptr_->actuator_forcelimited));
+  }
+  emscripten::val actuator_forcerange() const {
+    return emscripten::val(emscripten::typed_memory_view(ptr_->nactuator * 2, ptr_->actuator_forcerange));
+  }
   emscripten::val actuator_ctrllimited() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_ctrllimited));
   }
@@ -5535,12 +5544,6 @@ struct MjModel {
   emscripten::val actuator_gear() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nout * 6, ptr_->actuator_gear));
   }
-  emscripten::val actuator_forcelimited() const {
-    return emscripten::val(emscripten::typed_memory_view(ptr_->nout, ptr_->actuator_forcelimited));
-  }
-  emscripten::val actuator_forcerange() const {
-    return emscripten::val(emscripten::typed_memory_view(ptr_->nout * 2, ptr_->actuator_forcerange));
-  }
   emscripten::val actuator_acc0() const {
     return emscripten::val(emscripten::typed_memory_view(ptr_->nout, ptr_->actuator_acc0));
   }
@@ -6016,6 +6019,12 @@ struct MjsActuator {
   void set_actdim(int value) {
     ptr_->actdim = value;
   }
+  int ctrlspec() const {
+    return ptr_->ctrlspec;
+  }
+  void set_ctrlspec(int value) {
+    ptr_->ctrlspec = value;
+  }
   mjtBool actearly() const {
     return ptr_->actearly;
   }