Skip to content

Commit 072e963

Browse files
yuvaltassacopybara-github
yuvaltassa
authored and
copybara-github
committed
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
1 parent a8545ac commit 072e963

49 files changed

Lines changed: 1771 additions & 103 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎doc/APIreference/APItypes.rst‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -347,6 +347,16 @@ Actuator bias types. These values are used in ``m->actuator_biastype``.
347347
.. mujoco-include:: mjtBias
348348

349349

350+
.. _mjtCtrlChart:
351+
352+
mjtCtrlChart
353+
~~~~~~~~~~~~
354+
355+
Orientation input charts of so3 actuators. These values are used in ``m->actuator_ctrlspec``.
356+
357+
.. mujoco-include:: mjtCtrlChart
358+
359+
350360
.. _mjtObj:
351361

352362
mjtObj

‎doc/APIreference/functions.rst‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -603,6 +603,16 @@ Get id of object with the specified :ref:`mjtObj` type and name, returns -1 if i
603603

604604
Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`` if name not found.
605605

606+
.. _mj_actuatorInputName:
607+
608+
`mj_actuatorInputName <#mj_actuatorInputName>`__
609+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
610+
611+
.. mujoco-include:: mj_actuatorInputName
612+
613+
Get name of actuator input, determined by the actuator type and input signature;
614+
return NULL if the actuator type defines no input names.
615+
606616
.. _mj_fullM:
607617

608618
`mj_fullM <#mj_fullM>`__
@@ -1821,6 +1831,15 @@ m is only required to contain the size fields from MJMODEL_INTS.
18211831

18221832
Copy mjData, skip large arrays not required for visualization.
18231833

1834+
.. _mj_resetCtrl:
1835+
1836+
`mj_resetCtrl <#mj_resetCtrl>`__
1837+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1838+
1839+
.. mujoco-include:: mj_resetCtrl
1840+
1841+
Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.
1842+
18241843
.. _mj_resetData:
18251844

18261845
`mj_resetData <#mj_resetData>`__
@@ -5258,6 +5277,15 @@ Set actuator to integrated velocity; return error if any.
52585277

52595278
Set actuator to velocity servo; return error if any.
52605279

5280+
.. _mjs_setToOrientation:
5281+
5282+
`mjs_setToOrientation <#mjs_setToOrientation>`__
5283+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
5284+
5285+
.. mujoco-include:: mjs_setToOrientation
5286+
5287+
Set actuator to orientation servo.
5288+
52615289
.. _mjs_setToDamper:
52625290

52635291
`mjs_setToDamper <#mjs_setToDamper>`__

‎doc/XMLreference.rst‎

Lines changed: 113 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5488,6 +5488,8 @@ specify them independently.
54885488

54895489
:at:`forcerange`: :at-val:`real(2), "0 0"`
54905490
Range for clamping the force output. The first value must be no greater than the second value.
5491+
On :ref:`orientation<actuator-orientation>` actuators the force is a 3D torque, clamped on its norm: the second
5492+
value bounds the torque magnitude and the first value must be 0.
54915493
|br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in
54925494
:ref:`compiler <compiler>`.
54935495

@@ -5678,7 +5680,7 @@ specify them independently.
56785680

56795681
.. _actuator-general-gaintype:
56805682

5681-
:at:`gaintype`: :at-val:`[fixed, affine, muscle, user], "fixed"`
5683+
:at:`gaintype`: :at-val:`[fixed, affine, muscle, so3, user], "fixed"`
56825684
The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be
56835685
affine. As already explained in :ref:`Actuation model <geActuation>`, the general formula is:
56845686
scalar_force = gain_term \* (act or ctrl) + bias_term.
@@ -5691,12 +5693,13 @@ specify them independently.
56915693
fixed gain_term = gainprm[0]
56925694
affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity
56935695
muscle gain_term = mju_muscleGain(...)
5696+
so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-orientation>`
56945697
user gain_term = mjcb_act_gain(...)
56955698
======= ===============================
56965699

56975700
.. _actuator-general-biastype:
56985701

5699-
:at:`biastype`: :at-val:`[none, affine, muscle, user], "none"`
5702+
:at:`biastype`: :at-val:`[none, affine, muscle, so3, user], "none"`
57005703
The keywords have the following meaning:
57015704

57025705
======= ================================================================
@@ -5705,9 +5708,12 @@ specify them independently.
57055708
none bias_term = 0
57065709
affine bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity
57075710
muscle bias_term = mju_muscleBias(...)
5711+
so3 damping term of the geodesic orientation servo, see :ref:`orientation<actuator-orientation>`
57085712
user bias_term = mjcb_act_bias(...)
57095713
======= ================================================================
57105714

5715+
Note that :at:`gaintype` and :at:`biastype` must either both be "so3" or neither.
5716+
57115717
.. _actuator-general-dynprm:
57125718

57135719
:at:`dynprm`: :at-val:`real(10), "1 0 ... 0"`
@@ -5731,6 +5737,13 @@ specify them independently.
57315737
so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see
57325738
:ref:`muscle <actuator-muscle>` below.
57335739

5740+
.. _actuator-general-input:
5741+
5742+
:at:`input`: :at-val:`string, optional`
5743+
Input signature of the actuator: which controls make up its control block, recorded in
5744+
``mjModel.actuator_ctrlspec``. Available for gaintype "so3", where it selects the orientation chart: "expmap"
5745+
(3 controls, the default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`.
5746+
57345747
.. _actuator-general-actearly:
57355748

57365749
:at:`actearly`: :at-val:`[false, true], "false"`
@@ -5939,6 +5952,102 @@ This element has one custom attribute in addition to the common attributes:
59395952
:ref:`position<actuator-position>` attribute and in the :ref:`default class<default-position-inheritrange>`,
59405953
saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator.
59415954

5955+
.. _actuator-orientation:
5956+
5957+
:el-prefix:`actuator/` |-| **orientation** |*|
5958+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
5959+
5960+
.. youtube:: 17XpwnqyCXs
5961+
:align: right
5962+
:width: 40%
5963+
5964+
This element creates an orientation servo: a geodesic PD controller on a relative orientation, targeting a ball
5965+
:ref:`joint<actuator-general-joint>` or a :ref:`site<actuator-general-site>` with a
5966+
:ref:`refsite<actuator-general-refsite>`. Unlike per-axis :ref:`position<actuator-position>` servos, the servo acts
5967+
jointly on the full orientation: the force is :math:`k_p \log(q^{-1} q_{target}) - k_v \omega`, exact for arbitrary axis
5968+
combinations, with a unique equilibrium at every commanded orientation. The transmission has 3 force outputs; force,
5969+
error and angular velocity are expressed in the child (joint or site) frame. The commanded orientation is given in the
5970+
:ref:`input<actuator-orientation-input>` chart: an exponential-map vector (3 controls, the default) or a quaternion (4
5971+
controls). :ref:`forcerange<actuator-general-forcerange>` clamps the norm of the output torque,
5972+
preserving its direction; the lower bound must be 0.
5973+
:ref:`Actuator sensors<sensor-actuatorpos>` report one value per force output. The integrator variant, which
5974+
stores the orientation setpoint in :ref:`act<siPhysicsState>`, is available via :ref:`general<actuator-general>` with
5975+
:ref:`dyntype<actuator-general-dyntype>` "integrator" and is expmap-only. The video on the right shows this `example
5976+
model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/actuation/orientation.xml>`__.
5977+
The underlying :el:`general` attributes are set as follows:
5978+
5979+
========= ======= ========= =========
5980+
Attribute Setting Attribute Setting
5981+
========= ======= ========= =========
5982+
dyntype none gainprm kp 0 0
5983+
gaintype so3 biasprm 0 -kp -kv
5984+
biastype so3
5985+
========= ======= ========= =========
5986+
5987+
.. _actuator-orientation-ctrlrange:
5988+
5989+
:at:`ctrlrange`: :at-val:`real(2), "0 0"`
5990+
Range for clamping the control input, as described in :ref:`ctrlrange <actuator-general-ctrlrange>`. For this
5991+
multi-input actuator, the same range limits are replicated and applied independently to each of the 3 (expmap) or 4
5992+
(quaternion) control inputs in the control block.
5993+
5994+
.. _actuator-orientation-forcerange:
5995+
5996+
:at:`forcerange`: :at-val:`real(2), "0 0"`
5997+
Range for clamping the torque output, as described in :ref:`forcerange <actuator-general-forcerange>`. The torque is
5998+
clamped on its norm, preserving its direction: the second value bounds the torque magnitude and the first value must
5999+
be 0.
6000+
6001+
This element has custom attributes in addition to the common attributes:
6002+
6003+
.. _actuator-orientation-name:
6004+
6005+
.. _actuator-orientation-class:
6006+
6007+
.. _actuator-orientation-group:
6008+
6009+
.. _actuator-orientation-nsample:
6010+
6011+
.. _actuator-orientation-interp:
6012+
6013+
.. _actuator-orientation-delay:
6014+
6015+
.. _actuator-orientation-forcelimited:
6016+
6017+
.. _actuator-orientation-user:
6018+
6019+
.. _actuator-orientation-joint:
6020+
6021+
.. _actuator-orientation-site:
6022+
6023+
.. _actuator-orientation-refsite:
6024+
6025+
.. _actuator-orientation-kp:
6026+
6027+
:at:`kp`: :at-val:`real, "1"`
6028+
Position feedback gain, in units of torque per radian of geodesic error.
6029+
6030+
.. _actuator-orientation-kv:
6031+
6032+
:at:`kv`: :at-val:`real, "0"`
6033+
Damping applied by the actuator, per force output.
6034+
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
6035+
6036+
.. _actuator-orientation-dampratio:
6037+
6038+
:at:`dampratio`: :at-val:`real, "0"`
6039+
Damping applied by the actuator, using damping ratio units, as for
6040+
:ref:`position/dampratio<actuator-position-dampratio>`. This attribute is exclusive with :at:`kv`.
6041+
6042+
.. _actuator-orientation-input:
6043+
6044+
:at:`input`: :at-val:`[expmap, quat], "expmap"`
6045+
`Chart <https://en.wikipedia.org/wiki/Manifold#Charts>`__ of the commanded orientation. With "expmap" the control
6046+
block is an exponential-map vector (3 controls, in radians). With "quat" the control block is a quaternion (4
6047+
controls, :ref:`w-first <siLayout>`); the commanded quaternion is normalized by the servo, making the force scale-
6048+
and antipodally-invariant, and the control block resets to the identity quaternion. The quat chart requires
6049+
``dyntype="none"``.
6050+
59426051
.. _actuator-velocity:
59436052

59446053
:el-prefix:`actuator/` |-| **velocity** |*|
@@ -9907,6 +10016,8 @@ if omitted.
990710016

990810017
.. _default-general-biasprm:
990910018

10019+
.. _default-general-input:
10020+
991010021
.. _default-general-actearly:
991110022

991210023
:el-prefix:`default/` |-| **general** |?|

‎doc/XMLschema.rst‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2375,6 +2375,9 @@
23752375
.. grid-item::
23762376
:ref:`actdim<actuator-general-actdim>`
23772377

2378+
.. grid-item::
2379+
:ref:`input<actuator-general-input>`
2380+
23782381
.. grid-item::
23792382
:ref:`dyntype<actuator-general-dyntype>`
23802383

@@ -2733,6 +2736,63 @@
27332736
:ref:`dampratio<actuator-intvelocity-dampratio>`
27342737

27352738

2739+
.. dropdown:: :ref:`orientation<actuator-orientation>` |*|
2740+
2741+
.. grid:: 2 3 4 4
2742+
:gutter: 0
2743+
2744+
.. grid-item::
2745+
:ref:`name<actuator-orientation-name>`
2746+
2747+
.. grid-item::
2748+
:ref:`class<actuator-orientation-class>`
2749+
2750+
.. grid-item::
2751+
:ref:`group<actuator-orientation-group>`
2752+
2753+
.. grid-item::
2754+
:ref:`nsample<actuator-orientation-nsample>`
2755+
2756+
.. grid-item::
2757+
:ref:`interp<actuator-orientation-interp>`
2758+
2759+
.. grid-item::
2760+
:ref:`delay<actuator-orientation-delay>`
2761+
2762+
.. grid-item::
2763+
:ref:`forcelimited<actuator-orientation-forcelimited>`
2764+
2765+
.. grid-item::
2766+
:ref:`ctrlrange<actuator-orientation-ctrlrange>`
2767+
2768+
.. grid-item::
2769+
:ref:`forcerange<actuator-orientation-forcerange>`
2770+
2771+
.. grid-item::
2772+
:ref:`user<actuator-orientation-user>`
2773+
2774+
.. grid-item::
2775+
:ref:`joint<actuator-orientation-joint>`
2776+
2777+
.. grid-item::
2778+
:ref:`site<actuator-orientation-site>`
2779+
2780+
.. grid-item::
2781+
:ref:`refsite<actuator-orientation-refsite>`
2782+
2783+
.. grid-item::
2784+
:ref:`kp<actuator-orientation-kp>`
2785+
2786+
.. grid-item::
2787+
:ref:`kv<actuator-orientation-kv>`
2788+
2789+
.. grid-item::
2790+
:ref:`dampratio<actuator-orientation-dampratio>`
2791+
2792+
.. grid-item::
2793+
:ref:`input<actuator-orientation-input>`
2794+
2795+
27362796
.. dropdown:: :ref:`damper<actuator-damper>` |*|
27372797

27382798
.. grid:: 2 3 4 4
@@ -5864,6 +5924,9 @@
58645924
.. grid-item::
58655925
:ref:`actdim<default-general-actdim>`
58665926

5927+
.. grid-item::
5928+
:ref:`input<default-general-input>`
5929+
58675930
.. grid-item::
58685931
:ref:`dyntype<default-general-dyntype>`
58695932

‎doc/changelog.rst‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,11 @@ Engine
6868
.. admonition:: Breaking ABI changes
6969
:class: caution
7070

71+
- :ref:`mjModel` gained the ``actuator_ctrlspec`` field (input signature of each actuator), and :ref:`mjsActuator`
72+
gained ``ctrlspec``, changing their size and layout. The :ref:`mjtGain` and :ref:`mjtBias` enums gained ``so3``
73+
members, shifting the values of ``mjGAIN_USER`` and ``mjBIAS_USER``.
7174
- Added ``texid``, ``texuniform`` and ``texrepeat`` fields to :ref:`mjvGeom`.
72-
- The :ref:`mjContact`` struct gained an ``adhesion`` member, changing its size and layout.
75+
- The :ref:`mjContact` struct gained an ``adhesion`` member, changing its size and layout.
7376

7477
.. admonition:: Bug fixes
7578
:class: admonition
@@ -93,6 +96,24 @@ Actuation
9396
:ref:`general<actuator-general>` actuators it defaults to "auto", so activation clamping is enabled by specifying
9497
``actrange``. Unclamped integrated setpoints are well-behaved on rotational transmissions, where they wrap.
9598

99+
.. youtube:: 17XpwnqyCXs
100+
:align: right
101+
:width: 35%
102+
103+
- Added the :ref:`orientation<actuator-orientation>` actuator: a geodesic servo on a new SO(3) transmission (ball
104+
joints, or a site with a :ref:`refsite<actuator-general-refsite>`), acting jointly on the full relative orientation
105+
with an exact equilibrium at every commanded orientation. This is the first actuator with multiple force outputs
106+
(3), and, with ``input="quat"``, the first with different input and output dimensions (4 controls, 3 outputs). The
107+
input signature is recorded in the new ``mjModel.actuator_ctrlspec``, exposed as the
108+
:ref:`input<actuator-general-input>` attribute.
109+
- Added :ref:`mj_actuatorInputName`, returning the name of an actuator input (e.g. "qw" for the first control of a
110+
quaternion-commanded orientation actuator). The control sliders in :ref:`simulate<saSimulate>` and MuJoCo Studio are
111+
now generated per control and labeled with the actuator name plus the input name suffix.
112+
- Viewer control sliders now use a defined :ref:`ctrlrange<actuator-general-ctrlrange>` even when
113+
:ref:`ctrllimited<actuator-general-ctrllimited>` is "false": the range sets the slider span, while clamping remains
114+
controlled by :at:`ctrllimited`.
115+
- Added :ref:`mj_resetCtrl`, setting controls to neutral values: zero, except quaternion inputs which reset to the
116+
identity quaternion. Called by :ref:`mj_resetData` and the viewers' "Clear All".
96117

97118
Solvers
98119
^^^^^^^

0 commit comments

Comments
 (0)