From 072e963fa0d0efac05308b80efc85eff57df20df Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 21 Jul 2026 11:35:28 -0700 Subject: [PATCH] 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 , or via . The setpoint input has two charts: an expmap target (3 controls, default) or a quaternion target (4 controls) -- , 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 | 10 + doc/APIreference/functions.rst | 28 + doc/XMLreference.rst | 115 +++- doc/XMLschema.rst | 63 ++ doc/changelog.rst | 23 +- doc/includes/references.h | 17 +- include/mujoco/mjmodel.h | 5 +- include/mujoco/mjspec.h | 1 + include/mujoco/mjspecmacro.h | 1 + include/mujoco/mjtype.h | 9 + include/mujoco/mjxmacro.h | 5 +- include/mujoco/mujoco.h | 11 + .../mjx/third_party/mujoco_warp/_src/types.py | 8 +- python/mujoco/functions.cc | 2 + python/mujoco/introspect/enums.py | 16 +- python/mujoco/introspect/functions.py | 82 +++ python/mujoco/introspect/structs.py | 45 +- python/mujoco/specs.cc | 13 + python/mujoco/specs_test.py | 8 + simulate/simulate.cc | 30 +- src/engine/engine_core_smooth.c | 98 ++- src/engine/engine_derivative.c | 29 +- src/engine/engine_forward.c | 108 +++- src/engine/engine_io.c | 25 +- src/engine/engine_io.h | 3 + src/engine/engine_name.c | 18 + src/engine/engine_name.h | 3 + src/engine/engine_sensor.c | 12 +- src/engine/engine_setconst.c | 3 + src/engine/engine_sleep.c | 9 + src/experimental/platform/ux/gui.cc | 55 +- src/experimental/platform/ux/gui.h | 2 +- src/user/user_api.cc | 25 + src/user/user_api.h | 4 + src/user/user_init.c | 1 + src/user/user_model.cc | 16 +- src/user/user_objects.cc | 88 ++- src/user/user_objects.h | 3 + src/xml/xml_base.h | 2 + src/xml/xml_native_reader.cc | 52 +- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 1 + test/engine/engine_core_smooth_test.cc | 596 ++++++++++++++++++ test/engine/engine_derivative_test.cc | 53 ++ .../engine/testdata/actuation/orientation.xml | 78 +++ test/user/user_api_test.cc | 30 + unity/Runtime/Bindings/MjBindings.cs | 22 +- wasm/codegen/generated/bindings.cc | 25 + wasm/codegen/generated/bindings.h | 21 +- 49 files changed, 1772 insertions(+), 104 deletions(-) create mode 100644 test/engine/testdata/actuation/orientation.xml diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 77b9fc70..b595c4df 100644 --- a/doc/APIreference/APItypes.rst +++ b/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 diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 2f30b473..d37bdd0f 100644 --- a/doc/APIreference/functions.rst +++ b/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>`__ diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 5111bdf9..8a409cf8 100644 --- a/doc/XMLreference.rst +++ b/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` 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 `. @@ -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 `, 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` 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` 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 ` 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-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` attribute and in the :ref:`default class`, 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` or a :ref:`site` with a +:ref:`refsite`. Unlike per-axis :ref:`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` chart: an exponential-map vector (3 controls, the default) or a quaternion (4 +controls). :ref:`forcerange` clamps the norm of the output torque, +preserving its direction; the lower bound must be 0. +:ref:`Actuator sensors` report one value per force output. The integrator variant, which +stores the orientation setpoint in :ref:`act`, is available via :ref:`general` with +:ref:`dyntype` "integrator" and is expmap-only. The video on the right shows this `example +model `__. +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 `. 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 `. 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`. + +.. _actuator-orientation-dampratio: + +:at:`dampratio`: :at-val:`real, "0"` + Damping applied by the actuator, using damping ratio units, as for + :ref:`position/dampratio`. This attribute is exclusive with :at:`kv`. + +.. _actuator-orientation-input: + +:at:`input`: :at-val:`[expmap, quat], "expmap"` + `Chart `__ 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 `); 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** |?| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index c83e7afa..0fb12668 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2375,6 +2375,9 @@ .. grid-item:: :ref:`actdim` + .. grid-item:: + :ref:`input` + .. grid-item:: :ref:`dyntype` @@ -2733,6 +2736,63 @@ :ref:`dampratio` + .. dropdown:: :ref:`orientation` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`forcelimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`forcerange` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`joint` + + .. grid-item:: + :ref:`site` + + .. grid-item:: + :ref:`refsite` + + .. grid-item:: + :ref:`kp` + + .. grid-item:: + :ref:`kv` + + .. grid-item:: + :ref:`dampratio` + + .. grid-item:: + :ref:`input` + + .. dropdown:: :ref:`damper` |*| .. grid:: 2 3 4 4 @@ -5864,6 +5924,9 @@ .. grid-item:: :ref:`actdim` + .. grid-item:: + :ref:`input` + .. grid-item:: :ref:`dyntype` diff --git a/doc/changelog.rst b/doc/changelog.rst index f1ad20dd..1aab844a 100644 --- a/doc/changelog.rst +++ b/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` 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: a geodesic servo on a new SO(3) transmission (ball + joints, or a site with a :ref:`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` 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` 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` even when + :ref:`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 ^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index 953abab1..ddb52846 100644 --- a/doc/includes/references.h +++ b/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); diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 49e1d30b..dce083b1 100644 --- a/include/mujoco/mjmodel.h +++ b/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) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 77e71a22..8bb782a7 100644 --- a/include/mujoco/mjspec.h +++ b/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 diff --git a/include/mujoco/mjspecmacro.h b/include/mujoco/mjspecmacro.h index c2f4b600..14a47c12 100644 --- a/include/mujoco/mjspecmacro.h +++ b/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 ) \ diff --git a/include/mujoco/mjtype.h b/include/mujoco/mjtype.h index b46b7ae5..f5697e2f 100644 --- a/include/mujoco/mjtype.h +++ b/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 diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 8f36c166..34e637d8 100644 --- a/include/mujoco/mjxmacro.h +++ b/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 ) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fbb8f372..20ee0a97 100644 --- a/include/mujoco/mujoco.h +++ b/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); diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 708293e5..4b2b2803 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/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) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index caa2f5e7..b48ecb5d 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -166,6 +166,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) { InterceptMjErrors(::mj_copyData)(dest, m, src); }); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); // Skipped: mj_stackAllocByte (doesn't make sense in Python) @@ -589,6 +590,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) { }); Def(pymodule); Def(pymodule); + Def(pymodule); Def( pymodule, [](const raw::MjModel* m, const raw::MjData* d, diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index dfa54efd..03ef94c8 100644 --- a/python/mujoco/introspect/enums.py +++ b/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', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 38b61c8c..050d0f66 100644 --- a/python/mujoco/introspect/functions.py +++ b/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', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index f10405c7..8d302304 100644 --- a/python/mujoco/introspect/structs.py +++ b/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'), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 17391474..94372f5e 100644 --- a/python/mujoco/specs.cc +++ b/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) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 1a725097..0b6fa50c 100644 --- a/python/mujoco/specs_test.py +++ b/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) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 30761ecc..a19bbe64 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -2160,7 +2160,8 @@ void Simulate::Sync(bool state_only) { for (int i = 0; i < m_->nu; ++i) { std::optional> 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) { - const char* name = mj_id2name(this->m_, mjOBJ_ACTUATOR, i); - actuator_names_.emplace_back(name ? name : ""); + for (int i = 0; i < this->m_->nactuator; ++i) { + const char* actname = mj_id2name(this->m_, mjOBJ_ACTUATOR, i); + 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(); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index dc75e577..0ee2da8d 100644 --- a/src/engine/engine_core_smooth.c +++ b/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); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index fbb998c2..d68318f7 100644 --- a/src/engine/engine_derivative.c +++ b/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]; - mjtNum* range = m->actuator_forcerange + 2*oadr; - if (force <= range[0] || force >= range[1]) { - continue; + const mjtNum* range = m->actuator_forcerange + 2*i; + + // SO3: force is norm-clamped (approximation: saturated force still varies tangentially) + if (m->actuator_gaintype[i] == mjGAIN_SO3) { + if (mju_norm3(d->actuator_force + oadr) >= range[1]) { + continue; + } + } else { + mjtNum force = d->actuator_force[oadr]; + if (force <= range[0] || force >= range[1]) { + continue; + } } } @@ -2031,6 +2039,11 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; } + // SO3 geodesic servo: kv term, applied to each output row below + else if (m->actuator_biastype[i] == mjBIAS_SO3) { + bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; + } + // DC motor bias (back-EMF) else if (m->actuator_biastype[i] == mjBIAS_DCMOTOR) { const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; @@ -2108,10 +2121,12 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } } - // add + // add, once per output row if (bias_vel != 0) { - addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr, - d->moment_rownnz, d->moment_rowadr, d->moment_colind); + for (int k=0; k < m->actuator_outnum[i]; k++) { + addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr+k, + d->moment_rownnz, d->moment_rowadr, d->moment_colind); + } } } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 507448f2..c7c78af2 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -268,6 +268,19 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, i } +// expmap (axis-angle) vector to quaternion +static void expmap2Quat(mjtNum quat[4], const mjtNum v[3]) { + mjtNum angle = mju_norm3(v); + if (angle < mjMINVAL) { + quat[0] = 1; + quat[1] = quat[2] = quat[3] = 0; + } else { + mjtNum axis[3] = {v[0]/angle, v[1]/angle, v[2]/angle}; + mju_axisAngle2Quat(quat, axis, angle); + } +} + + // period of the rotational transmission for wrap-eligible servo actuators, 0 otherwise static mjtNum wrapPeriod(const mjModel* m, int i) { // servo shape: fixed gain, affine bias, matching kp, setpoint input @@ -387,9 +400,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // compute act_dot according to dynamics type switch (dyntype) { - case mjDYN_INTEGRATOR: // simple integrator - d->act_dot[act_last] = ctrl[uadr]; + case mjDYN_INTEGRATOR: { // simple integrator, one per control + int num = m->actuator_ctrlnum[i]; + for (int j=0; j < num; j++) { + d->act_dot[act_last-num+1+j] = ctrl[uadr+j]; + } break; + } case mjDYN_FILTER: // linear filter: dynprm = tau case mjDYN_FILTEREXACT: @@ -550,6 +567,51 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { int uadr = m->actuator_ctrladr[i]; int oadr = m->actuator_outadr[i]; + // SO(3) geodesic servo: 3 or 4 inputs and 3 outputs on an SO3 transmission + if (m->actuator_gaintype[i] == mjGAIN_SO3) { + mjtNum q_tgt[4]; + + // quat input: normalize ctrl directly (zero maps to the identity) + if (m->actuator_ctrlspec[i] == mjCHART_QUAT) { + mju_copy4(q_tgt, ctrl + uadr); + mju_normalize4(q_tgt); + } + + // expmap input: ctrl block (position) or act block (integrator) + else { + mjtNum u[3]; + if (m->actuator_dyntype[i] == mjDYN_NONE) { + mju_copy3(u, ctrl + uadr); + } else { + int act_adr = m->actuator_actadr[i]; + if (m->actuator_actearly[i]) { + for (int k=0; k < 3; k++) { + u[k] = mj_nextActivation(m, d, i, act_adr+k, d->act_dot[act_adr+k]); + } + } else { + mju_copy3(u, d->act + act_adr); + } + } + expmap2Quat(q_tgt, u); + } + + // error rotation from current to target: e = log(q_cur^-1 * q_tgt), in the local frame + // of the transmission, matching the frame of the moment rows and of actuator_velocity + // note: the force is invariant to the setpoint representative (exp is ray-periodic), + // so no wrapping is required; act is re-anchored at integration time in mj_advance + mjtNum q_cur[4], e[3]; + expmap2Quat(q_cur, d->actuator_length + oadr); + mju_subQuat(e, q_tgt, q_cur); + + // output force: kp * error + constant - kv * velocity + mjtNum kp = m->actuator_gainprm[mjNGAIN*i]; + const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i; + for (int k=0; k < 3; k++) { + force[oadr+k] = kp*e[k] + prm[0] + prm[2]*d->actuator_velocity[oadr+k]; + } + continue; + } + // check for tendon transmission with force limits if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) { tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]]; @@ -561,7 +623,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtGain gaintype = m->actuator_gaintype[i]; int actnum = m->actuator_actnum[i]; - // handle according to gain type + // handle SISO actuators according to gain type switch (gaintype) { case mjGAIN_FIXED: // fixed gain: prm = gain gain = gainprm[0]; @@ -614,6 +676,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { break; } + case mjGAIN_SO3: // handled above via early continue + mjERROR("mjGAIN_SO3 reached SISO switch (actuator %d)", i); + break; + default: // user gain if (mjcb_act_gain) { gain = mjcb_act_gain(m, d, i); @@ -751,7 +817,29 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // clamp actuator_force - clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nout, NULL); + for (int i=0; i < nactuator; i++) { + if (!m->actuator_forcelimited[i]) { + continue; + } + const mjtNum* range = m->actuator_forcerange + 2*i; + mjtNum* f = force + m->actuator_outadr[i]; + + // SO3: clamp the norm of the output torque, preserving its direction + if (m->actuator_gaintype[i] == mjGAIN_SO3) { + mjtNum norm = mju_norm3(f); + if (norm > range[1]) { + mju_scl3(f, f, range[1]/norm); + } + } + + // otherwise: clamp each output + else { + int outnum = m->actuator_outnum[i]; + for (int j=0; j < outnum; j++) { + f[j] = mju_clip(f[j], range[0], range[1]); + } + } + } // add DC motor mechanical forces (not subject to current limits) for (int i=0; i < nactuator; i++) { @@ -1151,6 +1239,18 @@ static void mj_advance(const mjModel* m, mjData* d, int adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; d->act[adr] = wrapSetpoint(d->act[adr], d->actuator_length[m->actuator_outadr[i]], period); } + + // SO3 servo: re-anchor the act setpoint to the canonical representative + else if (m->actuator_gaintype[i] == mjGAIN_SO3) { + int adr = m->actuator_actadr[i]; + mjtNum angle = mju_norm3(d->act + adr); + if (angle > mjPI) { + mjtNum scale = (angle - 2*mjPI*mju_round(angle/(2*mjPI))) / angle; + for (int k=0; k < 3; k++) { + d->act[adr+k] *= scale; + } + } + } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 482f5915..fe006699 100644 --- a/src/engine/engine_io.c +++ b/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; diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 00e4c7e4..54b413cc 100644 --- a/src/engine/engine_io.h +++ b/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); diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c index 1dcec37b..caae8bcd 100644 --- a/src/engine/engine_name.c +++ b/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; +} diff --git a/src/engine/engine_name.h b/src/engine/engine_name.h index 3bb98b2c..c89b0c72 100644 --- a/src/engine/engine_name.h +++ b/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 diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 0f11af4c..ffeacddd 100644 --- a/src/engine/engine_sensor.c +++ b/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 - sensordata[0] = d->actuator_length[m->actuator_outadr[objid]]; + case mjSENS_ACTUATORPOS: // actuator position, one per output + mju_copy(sensordata, d->actuator_length + m->actuator_outadr[objid], m->sensor_dim[i]); break; case mjSENS_BALLQUAT: // ball joint quaternion @@ -878,8 +878,8 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens sensordata[0] = d->ten_velocity[objid]; break; - case mjSENS_ACTUATORVEL: // actuator velocity - sensordata[0] = d->actuator_velocity[m->actuator_outadr[objid]]; + case mjSENS_ACTUATORVEL: // actuator velocity, one per output + mju_copy(sensordata, d->actuator_velocity + m->actuator_outadr[objid], m->sensor_dim[i]); break; case mjSENS_BALLANGVEL: // ball joint angular velocity @@ -1302,8 +1302,8 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens mju_copy3(sensordata, tmp); break; - case mjSENS_ACTUATORFRC: // actuator force - sensordata[0] = d->actuator_force[m->actuator_outadr[objid]]; + case mjSENS_ACTUATORFRC: // actuator force, one per output + mju_copy(sensordata, d->actuator_force + m->actuator_outadr[objid], m->sensor_dim[i]); break; case mjSENS_JOINTACTFRC: // actuator force at joint diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 11d4e9b0..610f13ee 100644 --- a/src/engine/engine_setconst.c +++ b/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]; diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index 93cbd55f..03d18e49 100644 --- a/src/engine/engine_sleep.c +++ b/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); diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index 26359c7d..95a7c7c7 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/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); - if (ctrl_name) { - std::snprintf(name, sizeof(name), "%s", ctrl_name); - } else { - std::snprintf(name, sizeof(name), "control %d", i); - } - - double min = -1.0; - double max = 1.0; - if (model->actuator_ctrllimited[i]) { - min = model->actuator_ctrlrange[2 * i + 0]; - max = model->actuator_ctrlrange[2 * i + 1]; - } - ImGui_Slider(name, &data->ctrl[i], min, max); - if (ImGui::BeginPopupContextItem()) { - if (ImGui::MenuItem("Reset to 0")) { - data->ctrl[i] = mju_clip(0.0, min, max); + // one slider per control; multi-input actuators suffix the input name + const char* act_name = mj_id2name(model, mjOBJ_ACTUATOR, i); + int ctrlnum = model->actuator_ctrlnum[i]; + for (int k = 0; k < ctrlnum; k++) { + int j = model->actuator_ctrladr[i] + k; + if (act_name && ctrlnum > 1) { + const char* input_name = mj_actuatorInputName(model, i, k); + if (input_name) { + std::snprintf(name, sizeof(name), "%s/%s", act_name, input_name); + } else { + std::snprintf(name, sizeof(name), "%s/%d", act_name, k); + } + } else if (act_name) { + std::snprintf(name, sizeof(name), "%s", act_name); + } else { + std::snprintf(name, sizeof(name), "control %d", j); + } + + 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(); } } diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index 15096081..9a686edb 100644 --- a/src/experimental/platform/ux/gui.h +++ b/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. diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 1eb207d5..1f1bc4ee 100644 --- a/src/user/user_api.cc +++ b/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); diff --git a/src/user/user_api.h b/src/user/user_api.h index a9bd4e83..0375c033 100644 --- a/src/user/user_api.h +++ b/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); diff --git a/src/user/user_init.c b/src/user/user_init.c index 572ae721..4ae97ff7 100644 --- a/src/user/user_init.c +++ b/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; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index de5d81be..9f46d8d4 100644 --- a/src/user/user_model.cc +++ b/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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index ad557e57..f1510b1b 100644 --- a/src/user/user_objects.cc +++ b/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))) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 42f51f17..78e1d322 100644 --- a/src/user/user_objects.h +++ b/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> act_; // act at the previous step std::map 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: diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 2f865810..00b02e89 100644 --- a/src/xml/xml_base.h +++ b/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[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c4ab9140..6ad9c563 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -258,7 +258,7 @@ std::vector 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 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 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" || diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 0f506efc..9c9efd11 100644 --- a/src/xml/xml_native_reader.h +++ b/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 MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0848ae77..b3a24836 100644 --- a/src/xml/xml_native_writer.cc +++ b/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); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 8ed484ac..39fe7878 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/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"( + + + + + + + + + + + + + + + + + + + + + +)"; + +// 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"( + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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) { diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index ff516847..54d5ac13 100644 --- a/test/engine/engine_derivative_test.cc +++ b/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"( + + + + + + + + + + + + + + + + + + )"; + 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 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"( diff --git a/test/engine/testdata/actuation/orientation.xml b/test/engine/testdata/actuation/orientation.xml new file mode 100644 index 00000000..631f9f5b --- /dev/null +++ b/test/engine/testdata/actuation/orientation.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 84227907..bcbb2cdd 100644 --- a/test/user/user_api_test.cc +++ b/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"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e982ac26..3941cc5f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index ff9f4419..861be173 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/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") .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") + .value("mjCHART_EXPMAP", mjCHART_EXPMAP) + .value("mjCHART_QUAT", mjCHART_QUAT); enum_("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") .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") .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); diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index 8e5f2cff..48697ecc 100644 --- a/wasm/codegen/generated/bindings.h +++ b/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; }