chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Add SO3 transmission and native orientation actuator.

Browse Source 
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
This commit is contained in:
Yuval Tassa
2026-07-21 11:35:28 -07:00
committed by Copybara-Service
parent a8545ac7cc
commit 072e963fa0
49 changed files with 1772 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APItypes.rst
+10
View File
@@ -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
+28
View File
@@ -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
+113 -2
View File
@@ -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
+63
View File
@@ -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
+22 -1
View File
@@ -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
+15 -2
View File
@@ -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);