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

Add SO3 transmission and native orientation actuator.

Browse Source 
https://youtu.be/17XpwnqyCXs

New transmission type mjTRN_SO3: a relative orientation, targeting a ball
joint or a site+refsite pair. It is the first transmission with more than
one force output: its length is the norm of the expmap vector of the
relative rotation and its moment axes are the 3 rows of the
relative rotational Jacobian, without projecting onto per-actuator gears.

New force law mjGAIN_SO3/mjBIAS_SO3: a geodesic PD servo, force =
kp * log(q_current^-1 * q_target) - kv * velocity, exact for arbitrary axis
combinations with a unique equilibrium at every commanded orientation.
Error, moment rows and velocity all live in the child frame (joint or
site): the right-difference error is the gradient of the geodesic
potential in that frame. The parent-frame (left) error is not: driving
child-frame torques with it pumps energy at large angles, settling into
steady-spinning limit cycles (the SO3LargeAngleConvergence test). The
integrator variant stores the 3D orientation setpoint in act (actnum = 3,
re-anchored to a bounded representative at integration time). Exposed in
MJCF as <orientation joint=|site=+refsite= kp kv|dampratio>, or via
<general gaintype="so3" biastype="so3">.

The setpoint input has two charts: an expmap target (3 controls, default)
or a quaternion target (4 controls) -- <orientation input="quat">, the
first actuator with different input and output widths. The signature is
recorded in a new per-actuator field actuator_ctrlspec (mjtCtrlChart),
whose meaning is scoped by the gain type the way gain/bias parameters are;
ctrlnum is derived from it at compile time and remains the layout
authority. An explicit field rather than width inference or a prm slot:
width-as-chart cannot express same-width signatures (upcoming servo input
subsets), and prm slots are the input_mode pattern this stack retires.
The force law normalizes the commanded quaternion, making it scale- and
antipodally-invariant. The all-zero ctrl still maps to the identity via
mju_normalize4, but it is a degenerate point (a nudge of any component
commands a half-turn), so quat inputs reset to the identity quaternion:
new mj_resetCtrl sets neutral ctrl values (zero, except qw = 1), called
by mj_resetData and the viewers' Clear All. The quat chart is
restricted to dyntype 'none': integrating a quaternion setpoint linearly
is not meaningful on the manifold. New mjsActuator.ctrlspec field carries
the signature through the spec and XML round-trip.

Actuator sensors (actuatorpos/vel/frc) now report one value per force
output; dim = 3 on an SO3 actuator.

As the first actuator with nu != nactuator, this commit also makes the
viewers multi-input aware: the control sliders in simulate and studio,
which indexed per-actuator arrays by control index (out of bounds on
this model class), are generated per control and labeled with the
actuator name plus an input suffix ("orient/qw"), via the new
introspection helper mj_actuatorInputName -- the single source of truth
for input names, extended by each new multi-input type (quaternion
components are w-first: qw, qx, qy, qz). Slider ranges now honor a
defined ctrlrange even when ctrllimited is false: range is the UI hint,
limited is the clamp -- wrapped and expmap setpoints are unbounded but
still want finite sliders, while quat components are truly bounded.

The rotational demo model is orientation.xml under
test/engine/testdata/actuation/, upgraded to a three-way contrast:
per-axis wrapped servos vs an expmap-commanded vs a quat-commanded
orientation actuator, on identical checker-textured boxes. It is loaded
by the mixed-axis contrast and input-name tests, and doubles as the
viewer test model (slider groups of 3 independent, 3 grouped, 4 grouped).

PiperOrigin-RevId: 951607063
Change-Id: If235dba8e2f2ca72672e7c62531a27e967c6a373
This commit is contained in:
Yuval Tassa
2026-07-21 11:35:28 -07:00
committed by Copybara-Service
parent a8545ac7cc
commit 072e963fa0
49 changed files with 1772 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APItypes.rst
+10
View File
@@ -347,6 +347,16 @@ Actuator bias types. These values are used in ``m->actuator_biastype``.
.. mujoco-include:: mjtBias
.. _mjtCtrlChart:
mjtCtrlChart
~~~~~~~~~~~~
Orientation input charts of so3 actuators. These values are used in ``m->actuator_ctrlspec``.
.. mujoco-include:: mjtCtrlChart
.. _mjtObj:
mjtObj
doc/APIreference/functions.rst
+28
View File
@@ -603,6 +603,16 @@ Get id of object with the specified :ref:`mjtObj` type and name, returns -1 if i
Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`` if name not found.
.. _mj_actuatorInputName:
`mj_actuatorInputName <#mj_actuatorInputName>`__
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mj_actuatorInputName
Get name of actuator input, determined by the actuator type and input signature;
return NULL if the actuator type defines no input names.
.. _mj_fullM:
`mj_fullM <#mj_fullM>`__
@@ -1821,6 +1831,15 @@ m is only required to contain the size fields from MJMODEL_INTS.
Copy mjData, skip large arrays not required for visualization.
.. _mj_resetCtrl:
`mj_resetCtrl <#mj_resetCtrl>`__
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mj_resetCtrl
Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.
.. _mj_resetData:
`mj_resetData <#mj_resetData>`__
@@ -5258,6 +5277,15 @@ Set actuator to integrated velocity; return error if any.
Set actuator to velocity servo; return error if any.
.. _mjs_setToOrientation:
`mjs_setToOrientation <#mjs_setToOrientation>`__
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mjs_setToOrientation
Set actuator to orientation servo.
.. _mjs_setToDamper:
`mjs_setToDamper <#mjs_setToDamper>`__
doc/XMLreference.rst
+113 -2
View File
@@ -5488,6 +5488,8 @@ specify them independently.
:at:`forcerange`: :at-val:`real(2), "0 0"`
Range for clamping the force output. The first value must be no greater than the second value.
On :ref:`orientation<actuator-orientation>` actuators the force is a 3D torque, clamped on its norm: the second
value bounds the torque magnitude and the first value must be 0.
|br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
@@ -5678,7 +5680,7 @@ specify them independently.
.. _actuator-general-gaintype:
:at:`gaintype`: :at-val:`[fixed, affine, muscle, user], "fixed"`
:at:`gaintype`: :at-val:`[fixed, affine, muscle, so3, user], "fixed"`
The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be
affine. As already explained in :ref:`Actuation model <geActuation>`, the general formula is:
scalar_force = gain_term \* (act or ctrl) + bias_term.
@@ -5691,12 +5693,13 @@ specify them independently.
fixed gain_term = gainprm[0]
affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity
muscle gain_term = mju_muscleGain(...)
so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-orientation>`
user gain_term = mjcb_act_gain(...)
======= ===============================
.. _actuator-general-biastype:
:at:`biastype`: :at-val:`[none, affine, muscle, user], "none"`
:at:`biastype`: :at-val:`[none, affine, muscle, so3, user], "none"`
The keywords have the following meaning:
======= ================================================================
@@ -5705,9 +5708,12 @@ specify them independently.
none bias_term = 0
affine bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity
muscle bias_term = mju_muscleBias(...)
so3 damping term of the geodesic orientation servo, see :ref:`orientation<actuator-orientation>`
user bias_term = mjcb_act_bias(...)
======= ================================================================
Note that :at:`gaintype` and :at:`biastype` must either both be "so3" or neither.
.. _actuator-general-dynprm:
:at:`dynprm`: :at-val:`real(10), "1 0 ... 0"`
@@ -5731,6 +5737,13 @@ specify them independently.
so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see
:ref:`muscle <actuator-muscle>` below.
.. _actuator-general-input:
:at:`input`: :at-val:`string, optional`
Input signature of the actuator: which controls make up its control block, recorded in
``mjModel.actuator_ctrlspec``. Available for gaintype "so3", where it selects the orientation chart: "expmap"
(3 controls, the default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`.
.. _actuator-general-actearly:
:at:`actearly`: :at-val:`[false, true], "false"`
@@ -5939,6 +5952,102 @@ This element has one custom attribute in addition to the common attributes:
:ref:`position<actuator-position>` attribute and in the :ref:`default class<default-position-inheritrange>`,
saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator.
.. _actuator-orientation:
:el-prefix:`actuator/` |-| **orientation** |*|
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. youtube:: 17XpwnqyCXs
:align: right
:width: 40%
This element creates an orientation servo: a geodesic PD controller on a relative orientation, targeting a ball
:ref:`joint<actuator-general-joint>` or a :ref:`site<actuator-general-site>` with a
:ref:`refsite<actuator-general-refsite>`. Unlike per-axis :ref:`position<actuator-position>` servos, the servo acts
jointly on the full orientation: the force is :math:`k_p \log(q^{-1} q_{target}) - k_v \omega`, exact for arbitrary axis
combinations, with a unique equilibrium at every commanded orientation. The transmission has 3 force outputs; force,
error and angular velocity are expressed in the child (joint or site) frame. The commanded orientation is given in the
:ref:`input<actuator-orientation-input>` chart: an exponential-map vector (3 controls, the default) or a quaternion (4
controls). :ref:`forcerange<actuator-general-forcerange>` clamps the norm of the output torque,
preserving its direction; the lower bound must be 0.
:ref:`Actuator sensors<sensor-actuatorpos>` report one value per force output. The integrator variant, which
stores the orientation setpoint in :ref:`act<siPhysicsState>`, is available via :ref:`general<actuator-general>` with
:ref:`dyntype<actuator-general-dyntype>` "integrator" and is expmap-only. The video on the right shows this `example
model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/actuation/orientation.xml>`__.
The underlying :el:`general` attributes are set as follows:
========= ======= ========= =========
Attribute Setting Attribute Setting
========= ======= ========= =========
dyntype none gainprm kp 0 0
gaintype so3 biasprm 0 -kp -kv
biastype so3
========= ======= ========= =========
.. _actuator-orientation-ctrlrange:
:at:`ctrlrange`: :at-val:`real(2), "0 0"`
Range for clamping the control input, as described in :ref:`ctrlrange <actuator-general-ctrlrange>`. For this
multi-input actuator, the same range limits are replicated and applied independently to each of the 3 (expmap) or 4
(quaternion) control inputs in the control block.
.. _actuator-orientation-forcerange:
:at:`forcerange`: :at-val:`real(2), "0 0"`
Range for clamping the torque output, as described in :ref:`forcerange <actuator-general-forcerange>`. The torque is
clamped on its norm, preserving its direction: the second value bounds the torque magnitude and the first value must
be 0.
This element has custom attributes in addition to the common attributes:
.. _actuator-orientation-name:
.. _actuator-orientation-class:
.. _actuator-orientation-group:
.. _actuator-orientation-nsample:
.. _actuator-orientation-interp:
.. _actuator-orientation-delay:
.. _actuator-orientation-forcelimited:
.. _actuator-orientation-user:
.. _actuator-orientation-joint:
.. _actuator-orientation-site:
.. _actuator-orientation-refsite:
.. _actuator-orientation-kp:
:at:`kp`: :at-val:`real, "1"`
Position feedback gain, in units of torque per radian of geodesic error.
.. _actuator-orientation-kv:
:at:`kv`: :at-val:`real, "0"`
Damping applied by the actuator, per force output.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-orientation-dampratio:
:at:`dampratio`: :at-val:`real, "0"`
Damping applied by the actuator, using damping ratio units, as for
:ref:`position/dampratio<actuator-position-dampratio>`. This attribute is exclusive with :at:`kv`.
.. _actuator-orientation-input:
:at:`input`: :at-val:`[expmap, quat], "expmap"`
`Chart <https://en.wikipedia.org/wiki/Manifold#Charts>`__ of the commanded orientation. With "expmap" the control
block is an exponential-map vector (3 controls, in radians). With "quat" the control block is a quaternion (4
controls, :ref:`w-first <siLayout>`); the commanded quaternion is normalized by the servo, making the force scale-
and antipodally-invariant, and the control block resets to the identity quaternion. The quat chart requires
``dyntype="none"``.
.. _actuator-velocity:
:el-prefix:`actuator/` |-| **velocity** |*|
@@ -9907,6 +10016,8 @@ if omitted.
.. _default-general-biasprm:
.. _default-general-input:
.. _default-general-actearly:
:el-prefix:`default/` |-| **general** |?|
doc/XMLschema.rst
+63
View File
@@ -2375,6 +2375,9 @@
.. grid-item::
:ref:`actdim<actuator-general-actdim>`
.. grid-item::
:ref:`input<actuator-general-input>`
.. grid-item::
:ref:`dyntype<actuator-general-dyntype>`
@@ -2733,6 +2736,63 @@
:ref:`dampratio<actuator-intvelocity-dampratio>`
.. dropdown:: :ref:`orientation<actuator-orientation>` |*|
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`name<actuator-orientation-name>`
.. grid-item::
:ref:`class<actuator-orientation-class>`
.. grid-item::
:ref:`group<actuator-orientation-group>`
.. grid-item::
:ref:`nsample<actuator-orientation-nsample>`
.. grid-item::
:ref:`interp<actuator-orientation-interp>`
.. grid-item::
:ref:`delay<actuator-orientation-delay>`
.. grid-item::
:ref:`forcelimited<actuator-orientation-forcelimited>`
.. grid-item::
:ref:`ctrlrange<actuator-orientation-ctrlrange>`
.. grid-item::
:ref:`forcerange<actuator-orientation-forcerange>`
.. grid-item::
:ref:`user<actuator-orientation-user>`
.. grid-item::
:ref:`joint<actuator-orientation-joint>`
.. grid-item::
:ref:`site<actuator-orientation-site>`
.. grid-item::
:ref:`refsite<actuator-orientation-refsite>`
.. grid-item::
:ref:`kp<actuator-orientation-kp>`
.. grid-item::
:ref:`kv<actuator-orientation-kv>`
.. grid-item::
:ref:`dampratio<actuator-orientation-dampratio>`
.. grid-item::
:ref:`input<actuator-orientation-input>`
.. dropdown:: :ref:`damper<actuator-damper>` |*|
.. grid:: 2 3 4 4
@@ -5864,6 +5924,9 @@
.. grid-item::
:ref:`actdim<default-general-actdim>`
.. grid-item::
:ref:`input<default-general-input>`
.. grid-item::
:ref:`dyntype<default-general-dyntype>`
doc/changelog.rst
+22 -1
View File
@@ -68,8 +68,11 @@ Engine
.. admonition:: Breaking ABI changes
:class: caution
- :ref:`mjModel` gained the ``actuator_ctrlspec`` field (input signature of each actuator), and :ref:`mjsActuator`
gained ``ctrlspec``, changing their size and layout. The :ref:`mjtGain` and :ref:`mjtBias` enums gained ``so3``
members, shifting the values of ``mjGAIN_USER`` and ``mjBIAS_USER``.
- Added ``texid``, ``texuniform`` and ``texrepeat`` fields to :ref:`mjvGeom`.
- The :ref:`mjContact`` struct gained an ``adhesion`` member, changing its size and layout.
- The :ref:`mjContact` struct gained an ``adhesion`` member, changing its size and layout.
.. admonition:: Bug fixes
:class: admonition
@@ -93,6 +96,24 @@ Actuation
:ref:`general<actuator-general>` actuators it defaults to "auto", so activation clamping is enabled by specifying
``actrange``. Unclamped integrated setpoints are well-behaved on rotational transmissions, where they wrap.
.. youtube:: 17XpwnqyCXs
:align: right
:width: 35%
- Added the :ref:`orientation<actuator-orientation>` actuator: a geodesic servo on a new SO(3) transmission (ball
joints, or a site with a :ref:`refsite<actuator-general-refsite>`), acting jointly on the full relative orientation
with an exact equilibrium at every commanded orientation. This is the first actuator with multiple force outputs
(3), and, with ``input="quat"``, the first with different input and output dimensions (4 controls, 3 outputs). The
input signature is recorded in the new ``mjModel.actuator_ctrlspec``, exposed as the
:ref:`input<actuator-general-input>` attribute.
- Added :ref:`mj_actuatorInputName`, returning the name of an actuator input (e.g. "qw" for the first control of a
quaternion-commanded orientation actuator). The control sliders in :ref:`simulate<saSimulate>` and MuJoCo Studio are
now generated per control and labeled with the actuator name plus the input name suffix.
- Viewer control sliders now use a defined :ref:`ctrlrange<actuator-general-ctrlrange>` even when
:ref:`ctrllimited<actuator-general-ctrllimited>` is "false": the range sets the slider span, while clamping remains
controlled by :at:`ctrllimited`.
- Added :ref:`mj_resetCtrl`, setting controls to neutral values: zero, except quaternion inputs which reset to the
identity quaternion. Called by :ref:`mj_resetData` and the viewers' "Clear All".
Solvers
^^^^^^^
doc/includes/references.h
+15 -2
View File
@@ -1115,6 +1115,7 @@ typedef struct mjModel_ {
int* actuator_biastype; // bias type (mjtBias) (nactuator x 1)
int* actuator_ctrladr; // address of first control (nactuator x 1)
int* actuator_ctrlnum; // number of controls (nactuator x 1)
int* actuator_ctrlspec; // input signature, scoped by gaintype (nactuator x 1)
int* actuator_outadr; // address of first force output (nactuator x 1)
int* actuator_outnum; // number of force outputs, from trntype (nactuator x 1)
int* actuator_actadr; // first activation address; -1: stateless (nactuator x 1)
@@ -1136,11 +1137,11 @@ typedef struct mjModel_ {
int* actuator_group; // group for visibility (nactuator x 1)
mjtNum* actuator_user; // user data (nactuator x nuser_actuator)
int* actuator_plugin; // plugin instance id; -1: not a plugin (nactuator x 1)
mjtBool* actuator_forcelimited;// is force limited (nactuator x 1)
mjtNum* actuator_forcerange; // range of forces (nactuator x 2)
mjtBool* actuator_ctrllimited; // is control limited (nu x 1)
mjtNum* actuator_ctrlrange; // range of controls (nu x 2)
mjtNum* actuator_gear; // scale length and transmitted force (nout x 6)
mjtBool* actuator_forcelimited;// is force limited (nout x 1)
mjtNum* actuator_forcerange; // range of forces (nout x 2)
mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nout x 1)
mjtNum* actuator_length0; // actuator length in qpos0 (nout x 1)
mjtNum* actuator_lengthrange; // feasible actuator length range (nout x 2)
@@ -2241,6 +2242,7 @@ typedef struct mjsActuator_ { // actuator specification
mjtDyn dyntype; // dynamics type
double dynprm[mjNDYN]; // dynamics parameters
int actdim; // number of activation variables
int ctrlspec; // input signature, scoped by gaintype; 0: type default
mjtBool actearly; // apply next activations to qfrc
// transmission
@@ -2499,6 +2501,7 @@ typedef enum mjtTrn { // type of actuator transmission
mjTRN_TENDON, // force on tendon
mjTRN_SITE, // force on site
mjTRN_BODY, // adhesion force on a body's geoms
mjTRN_SO3, // torque on a relative orientation (3 force outputs)
mjTRN_UNDEFINED = 1000 // undefined transmission type
} mjtTrn;
@@ -2516,6 +2519,7 @@ typedef enum mjtGain { // type of actuator gain
mjGAIN_AFFINE, // const + kp*length + kv*velocity
mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain()
mjGAIN_DCMOTOR, // DC motor gain: K or K/R
mjGAIN_SO3, // geodesic servo on an SO3 transmission: force = kp * log(error)
mjGAIN_USER // user-defined gain type
} mjtGain;
typedef enum mjtBias { // type of actuator bias
@@ -2523,8 +2527,13 @@ typedef enum mjtBias { // type of actuator bias
mjBIAS_AFFINE, // const + kp*length + kv*velocity
mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias()
mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction
mjBIAS_SO3, // damping term of the SO3 geodesic servo
mjBIAS_USER // user-defined bias type
} mjtBias;
typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): orientation chart
mjCHART_EXPMAP = 1, // exponential-map orientation target: 3 controls
mjCHART_QUAT = 2 // quaternion orientation target: 4 controls
} mjtCtrlChart;
typedef enum mjtObj { // type of MujoCo object
mjOBJ_UNKNOWN = 0, // unknown object type
mjOBJ_BODY, // body
@@ -3500,6 +3509,7 @@ mjtSize mj_sizeModel(const mjModel* m);
mjData* mj_makeData(const mjModel* m);
mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);
void mj_resetCtrl(const mjModel* m, mjData* d);
void mj_resetData(const mjModel* m, mjData* d);
void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
@@ -3611,6 +3621,7 @@ void mj_jacDot(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body);
int mj_name2id(const mjModel* m, int type, const char* name);
const char* mj_id2name(const mjModel* m, int type, int id);
const char* mj_actuatorInputName(const mjModel* m, int id, int input);
void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
@@ -3970,6 +3981,8 @@ const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1],
const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1],
double dampratio[1], double timeconst[1], double inheritrange);
const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
double dampratio[1], int ctrlspec);
const char* mjs_setToDamper(mjsActuator* actuator, double kv);
const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst,
double bias, double area, double diameter);
include/mujoco/mjmodel.h
+3 -2
View File
@@ -784,6 +784,7 @@ typedef struct mjModel_ {
int* actuator_biastype; // bias type (mjtBias) (nactuator x 1)
int* actuator_ctrladr; // address of first control (nactuator x 1)
int* actuator_ctrlnum; // number of controls (nactuator x 1)
int* actuator_ctrlspec; // input signature, scoped by gaintype (nactuator x 1)
int* actuator_outadr; // address of first force output (nactuator x 1)
int* actuator_outnum; // number of force outputs, from trntype (nactuator x 1)
int* actuator_actadr; // first activation address; -1: stateless (nactuator x 1)
@@ -805,11 +806,11 @@ typedef struct mjModel_ {
int* actuator_group; // group for visibility (nactuator x 1)
mjtNum* actuator_user; // user data (nactuator x nuser_actuator)
int* actuator_plugin; // plugin instance id; -1: not a plugin (nactuator x 1)
mjtBool* actuator_forcelimited;// is force limited (nactuator x 1)
mjtNum* actuator_forcerange; // range of forces (nactuator x 2)
mjtBool* actuator_ctrllimited; // is control limited (nu x 1)
mjtNum* actuator_ctrlrange; // range of controls (nu x 2)
mjtNum* actuator_gear; // scale length and transmitted force (nout x 6)
mjtBool* actuator_forcelimited;// is force limited (nout x 1)
mjtNum* actuator_forcerange; // range of forces (nout x 2)
mjtNum* actuator_acc0; // acceleration from unit force in qpos0 (nout x 1)
mjtNum* actuator_length0; // actuator length in qpos0 (nout x 1)
mjtNum* actuator_lengthrange; // feasible actuator length range (nout x 2)
include/mujoco/mjspec.h
+1
View File
@@ -724,6 +724,7 @@ typedef struct mjsActuator_ { // actuator specification
mjtDyn dyntype; // dynamics type
double dynprm[mjNDYN]; // dynamics parameters
int actdim; // number of activation variables
int ctrlspec; // input signature, scoped by gaintype; 0: type default
mjtBool actearly; // apply next activations to qfrc
// transmission
include/mujoco/mjspecmacro.h
+1
View File
@@ -496,6 +496,7 @@
X ( mjtDyn, dyntype, 1 ) \
XVEC( double, dynprm, mjNDYN ) \
X ( int, actdim, 1 ) \
X ( int, ctrlspec, 1 ) \
X ( mjtBool, actearly, 1 ) \
X ( mjtTrn, trntype, 1 ) \
XVEC( double, gear, 6 ) \
include/mujoco/mjtype.h
+9
View File
@@ -234,6 +234,7 @@ typedef enum mjtTrn { // type of actuator transmission
mjTRN_TENDON, // force on tendon
mjTRN_SITE, // force on site
mjTRN_BODY, // adhesion force on a body's geoms
mjTRN_SO3, // torque on a relative orientation (3 force outputs)
mjTRN_UNDEFINED = 1000 // undefined transmission type
} mjtTrn;
@@ -255,6 +256,7 @@ typedef enum mjtGain { // type of actuator gain
mjGAIN_AFFINE, // const + kp*length + kv*velocity
mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain()
mjGAIN_DCMOTOR, // DC motor gain: K or K/R
mjGAIN_SO3, // geodesic servo on an SO3 transmission: force = kp * log(error)
mjGAIN_USER // user-defined gain type
} mjtGain;
@@ -264,10 +266,17 @@ typedef enum mjtBias { // type of actuator bias
mjBIAS_AFFINE, // const + kp*length + kv*velocity
mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias()
mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction
mjBIAS_SO3, // damping term of the SO3 geodesic servo
mjBIAS_USER // user-defined bias type
} mjtBias;
typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): orientation chart
mjCHART_EXPMAP = 1, // exponential-map orientation target: 3 controls
mjCHART_QUAT = 2 // quaternion orientation target: 4 controls
} mjtCtrlChart;
typedef enum mjtObj { // type of MujoCo object
mjOBJ_UNKNOWN = 0, // unknown object type
mjOBJ_BODY, // body
include/mujoco/mjxmacro.h
+3 -2
View File
@@ -684,6 +684,7 @@
X ( int, actuator_biastype, nactuator, 1 ) \
X ( int, actuator_ctrladr, nactuator, 1 ) \
X ( int, actuator_ctrlnum, nactuator, 1 ) \
X ( int, actuator_ctrlspec, nactuator, 1 ) \
X ( int, actuator_outadr, nactuator, 1 ) \
X ( int, actuator_outnum, nactuator, 1 ) \
X ( int, actuator_actadr, nactuator, 1 ) \
@@ -705,11 +706,11 @@
X ( int, actuator_group, nactuator, 1 ) \
X ( mjtNum, actuator_user, nactuator, MJ_M(nuser_actuator) ) \
X ( int, actuator_plugin, nactuator, 1 ) \
X ( mjtBool, actuator_forcelimited, nactuator, 1 ) \
X ( mjtNum, actuator_forcerange, nactuator, 2 ) \
X ( mjtBool, actuator_ctrllimited, nu, 1 ) \
X ( mjtNum, actuator_ctrlrange, nu, 2 ) \
X ( mjtNum, actuator_gear, nout, 6 ) \
X ( mjtBool, actuator_forcelimited, nout, 1 ) \
X ( mjtNum, actuator_forcerange, nout, 2 ) \
X ( mjtNum, actuator_acc0, nout, 1 ) \
X ( mjtNum, actuator_length0, nout, 1 ) \
X ( mjtNum, actuator_lengthrange, nout, 2 )
include/mujoco/mujoco.h
+11
View File
@@ -255,6 +255,9 @@ MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
// Copy mjData, skip large arrays not required for visualization.
MJAPI mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);
// Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.
MJAPI void mj_resetCtrl(const mjModel* m, mjData* d);
// Reset data to defaults.
MJAPI void mj_resetData(const mjModel* m, mjData* d);
@@ -601,6 +604,10 @@ MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
// Get name of object with the specified mjtObj type and id; return NULL if name not found.
MJAPI const char* mj_id2name(const mjModel* m, int type, int id);
// Get name of actuator input, determined by the actuator type and input signature;
// return NULL if the actuator type defines no input names.
MJAPI const char* mj_actuatorInputName(const mjModel* m, int id, int input);
// Convert sparse inertia matrix into full (i.e. dense) matrix.
MJAPI void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
@@ -1745,6 +1752,10 @@ MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double
// Set actuator to velocity servo; return error if any.
MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
// Set actuator to orientation servo.
MJAPI const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
double dampratio[1], int ctrlspec);
// Set actuator to activate damper; return error if any.
MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py
Vendored
+4 -4
View File
@@ -1279,11 +1279,11 @@ class Model:
actuator_history: history buffer sizes (nu, 2)
actuator_historyadr: history buffer address (nu,)
actuator_delay: delay in seconds (nu,)
actuator_forcelimited: is force limited (nu,)
actuator_forcerange: range of forces (*, nu, 2)
actuator_ctrllimited: is control limited (nu,)
actuator_ctrlrange: range of controls (*, nu, 2)
actuator_gear: scale length and transmitted force (*, nu, 6)
actuator_forcelimited: is force limited (nu,)
actuator_forcerange: range of forces (*, nu, 2)
actuator_acc0: acceleration from unit force in qpos0 (*, nu)
actuator_lengthrange: feasible actuator length range (*, nu, 2)
sensor_type: sensor type (SensorType) (nsensor,)
@@ -1762,11 +1762,11 @@ class Model:
actuator_history: array("nu", wp.vec2i)
actuator_historyadr: array("nu", int)
actuator_delay: array("nu", float)
actuator_forcelimited: array("nu", bool)
actuator_forcerange: array("*", "nu", wp.vec2)
actuator_ctrllimited: array("nu", bool)
actuator_ctrlrange: array("*", "nu", wp.vec2)
actuator_gear: array("*", "nu", wp.spatial_vector)
actuator_forcelimited: array("nu", bool)
actuator_forcerange: array("*", "nu", wp.vec2)
actuator_acc0: array("*", "nu", float)
actuator_lengthrange: array("*", "nu", wp.vec2)
sensor_type: array("nsensor", int)
python/mujoco/functions.cc
+2
View File
@@ -166,6 +166,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) {
InterceptMjErrors(::mj_copyData)(dest, m, src);
});
Def<traits::mj_resetData>(pymodule);
Def<traits::mj_resetCtrl>(pymodule);
Def<traits::mj_resetDataDebug>(pymodule);
Def<traits::mj_resetDataKeyframe>(pymodule);
// Skipped: mj_stackAllocByte (doesn't make sense in Python)
@@ -589,6 +590,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) {
});
Def<traits::mj_name2id>(pymodule);
Def<traits::mj_id2name>(pymodule);
Def<traits::mj_actuatorInputName>(pymodule);
Def<traits::mj_fullM>(
pymodule,
[](const raw::MjModel* m, const raw::MjData* d,
python/mujoco/introspect/enums.py
+14 -2
View File
@@ -251,6 +251,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjTRN_TENDON', 3),
('mjTRN_SITE', 4),
('mjTRN_BODY', 5),
('mjTRN_SO3', 6),
('mjTRN_UNDEFINED', 1000),
]),
)),
@@ -277,7 +278,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjGAIN_AFFINE', 1),
('mjGAIN_MUSCLE', 2),
('mjGAIN_DCMOTOR', 3),
('mjGAIN_USER', 4),
('mjGAIN_SO3', 4),
('mjGAIN_USER', 5),
]),
)),
('mjtBias',
@@ -289,7 +291,17 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjBIAS_AFFINE', 1),
('mjBIAS_MUSCLE', 2),
('mjBIAS_DCMOTOR', 3),
('mjBIAS_USER', 4),
('mjBIAS_SO3', 4),
('mjBIAS_USER', 5),
]),
)),
('mjtCtrlChart',
EnumDecl(
name='mjtCtrlChart',
declname='enum mjtCtrlChart',
values=dict([
('mjCHART_EXPMAP', 1),
('mjCHART_QUAT', 2),
]),
)),
('mjtObj',
python/mujoco/introspect/functions.py
+82
View File
@@ -1102,6 +1102,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Copy mjData, skip large arrays not required for visualization.',
)),
('mj_resetCtrl',
FunctionDecl(
name='mj_resetCtrl',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
),
doc='Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.', # pylint: disable=line-too-long
)),
('mj_resetData',
FunctionDecl(
name='mj_resetData',
@@ -3467,6 +3487,30 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Get name of object with the specified mjtObj type and id; return NULL if name not found.', # pylint: disable=line-too-long
)),
('mj_actuatorInputName',
FunctionDecl(
name='mj_actuatorInputName',
return_type=PointerType(
inner_type=ValueType(name='char', is_const=True),
),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='id',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='input',
type=ValueType(name='int'),
),
),
doc='Get name of actuator input, determined by the actuator type and input signature; return NULL if the actuator type defines no input names.', # pylint: disable=line-too-long
)),
('mj_fullM',
FunctionDecl(
name='mj_fullM',
@@ -10927,6 +10971,44 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Set actuator to velocity servo; return error if any.',
)),
('mjs_setToOrientation',
FunctionDecl(
name='mjs_setToOrientation',
return_type=PointerType(
inner_type=ValueType(name='char', is_const=True),
),
parameters=(
FunctionParameterDecl(
name='actuator',
type=PointerType(
inner_type=ValueType(name='mjsActuator'),
),
),
FunctionParameterDecl(
name='kp',
type=ValueType(name='double'),
),
FunctionParameterDecl(
name='kv',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(1,),
),
),
FunctionParameterDecl(
name='dampratio',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(1,),
),
),
FunctionParameterDecl(
name='ctrlspec',
type=ValueType(name='int'),
),
),
doc='Set actuator to orientation servo.',
)),
('mjs_setToDamper',
FunctionDecl(
name='mjs_setToDamper',
python/mujoco/introspect/structs.py
+29 -16
View File
@@ -4452,6 +4452,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='number of controls',
array_extent=('nactuator',),
),
StructFieldDecl(
name='actuator_ctrlspec',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='input signature, scoped by gaintype',
array_extent=('nactuator',),
),
StructFieldDecl(
name='actuator_outadr',
type=PointerType(
@@ -4620,6 +4628,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='plugin instance id; -1: not a plugin',
array_extent=('nactuator',),
),
StructFieldDecl(
name='actuator_forcelimited',
type=PointerType(
inner_type=ValueType(name='mjtBool'),
),
doc='is force limited',
array_extent=('nactuator',),
),
StructFieldDecl(
name='actuator_forcerange',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='range of forces',
array_extent=('nactuator', 2),
),
StructFieldDecl(
name='actuator_ctrllimited',
type=PointerType(
@@ -4644,22 +4668,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='scale length and transmitted force',
array_extent=('nout', 6),
),
StructFieldDecl(
name='actuator_forcelimited',
type=PointerType(
inner_type=ValueType(name='mjtBool'),
),
doc='is force limited',
array_extent=('nout',),
),
StructFieldDecl(
name='actuator_forcerange',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='range of forces',
array_extent=('nout', 2),
),
StructFieldDecl(
name='actuator_acc0',
type=PointerType(
@@ -9658,6 +9666,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='number of activation variables',
),
StructFieldDecl(
name='ctrlspec',
type=ValueType(name='int'),
doc='input signature, scoped by gaintype; 0: type default',
),
StructFieldDecl(
name='actearly',
type=ValueType(name='mjtBool'),
python/mujoco/specs.cc
+13
View File
@@ -1554,6 +1554,19 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) {
}
},
py::arg("kv"));
mjsActuator.def(
"set_to_orientation",
[](raw::MjsActuator* self, double kp, double kv, double dampratio,
int ctrlspec) {
std::string err = mjs_setToOrientation(
self, kp, kv == -1 ? nullptr : &kv,
dampratio == -1 ? nullptr : &dampratio, ctrlspec);
if (!err.empty()) {
throw pybind11::value_error(err);
}
},
py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1,
py::arg("ctrlspec") = 0);
mjsActuator.def(
"set_to_damper",
[](raw::MjsActuator* self, double kv) {
python/mujoco/specs_test.py
+8
View File
@@ -1588,6 +1588,14 @@ class SpecsTest(absltest.TestCase):
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE)
self.assertEqual(actuator.inheritrange, True)
actuator.set_to_orientation(kp=2.0, dampratio=1.0)
self.assertEqual(actuator.gainprm[0], 2)
self.assertEqual(actuator.biasprm[1], -2)
self.assertEqual(actuator.biasprm[2], 1)
self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_SO3)
self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_SO3)
self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE)
actuator.set_to_velocity(kv=5.0)
self.assertEqual(actuator.gainprm[0], 5)
self.assertEqual(actuator.biasprm[2], -5)
simulate/simulate.cc
+20 -10
View File
@@ -2160,7 +2160,8 @@ void Simulate::Sync(bool state_only) {
for (int i = 0; i < m_->nu; ++i) {
std::optional<std::pair<mjtNum, mjtNum>> range;
if (m_->actuator_ctrllimited[i]) {
// a defined ctrlrange sets the slider range, even when ctrl is not clamped
if (m_->actuator_ctrlrange[2*i] < m_->actuator_ctrlrange[2*i + 1]) {
range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]);
}
if (actuator_ctrlrange_[i] != range) {
@@ -2304,7 +2305,7 @@ void Simulate::Sync(bool state_only) {
}
if (pending_.zero_ctrl) {
mju_zero(d_->ctrl, m_->nu);
mj_resetCtrl(m_, d_);
pending_.zero_ctrl = false;
}
@@ -2534,14 +2535,11 @@ void Simulate::LoadOnRenderThread() {
jnt_names_.emplace_back(name ? name : "");
}
actuator_group_.resize(this->m_->nu);
std::memcpy(actuator_group_.data(), this->m_->actuator_group,
sizeof(this->m_->actuator_group[0]) * this->m_->nu);
actuator_ctrlrange_.clear();
actuator_ctrlrange_.reserve(this->m_->nu);
for (int i = 0; i < this->m_->nu; ++i) {
if (this->m_->actuator_ctrllimited[i]) {
// a defined ctrlrange sets the slider range, even when ctrl is not clamped
if (this->m_->actuator_ctrlrange[2 * i] < this->m_->actuator_ctrlrange[2 * i + 1]) {
actuator_ctrlrange_.push_back(std::make_pair(
this->m_->actuator_ctrlrange[2 * i], this->m_->actuator_ctrlrange[2 * i + 1]));
} else {
@@ -2549,11 +2547,23 @@ void Simulate::LoadOnRenderThread() {
}
}
// per-control group and name; multi-input actuators suffix the input name
actuator_group_.resize(this->m_->nu);
actuator_names_.clear();
actuator_names_.reserve(this->m_->nu);
for (int i = 0; i < this->m_->nu; ++i) {
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();
src/engine/engine_core_smooth.c
+96 -2
View File
@@ -1301,9 +1301,12 @@ void mj_transmission(const mjModel* m, mjData* d) {
rowadr[out] = out == 0 ? 0 : rowadr[out-1] + rownnz[out-1];
int nnz, adr = rowadr[out];
// skip sleeping actuator
// skip sleeping actuator: zero all rows of its output block
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
rownnz[out] = 0;
for (int k=0; k < m->actuator_outnum[i]; k++) {
rowadr[out+k] = out+k == 0 ? 0 : rowadr[out+k-1] + rownnz[out+k-1];
rownnz[out+k] = 0;
}
continue;
}
@@ -1476,6 +1479,97 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
break;
case mjTRN_SO3: // relative orientation: 3 lengths, 3 moment rows
// ball joint: lengths = expmap of the joint quaternion, rows = identity at the joint dofs
if (m->actuator_trnid[2*i+1] == -1) {
mjtNum axis[3], quat[4];
mji_copy4(quat, d->qpos+m->jnt_qposadr[id]);
mju_normalize4(quat);
mji_quat2Vel(axis, quat, 1);
for (int k=0; k < 3; k++) {
int outk = out + k;
rowadr[outk] = k == 0 ? adr : rowadr[outk-1] + rownnz[outk-1];
length[outk] = axis[k];
rownnz[outk] = 1;
colind[rowadr[outk]] = m->jnt_dofadr[id] + k;
moment[rowadr[outk]] = 1;
}
}
// site+refsite: lengths = relative expmap, rows = relative rotational Jacobian
else {
int refid = m->actuator_trnid[2*i+1];
if (!jacref) jacref = mjSTACKALLOC(d, 3*nv, mjtNum);
if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum);
// relative rotation as expmap in the refsite frame
mjtNum quat[4], refquat[4], vec[3];
mji_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]);
mji_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]);
mji_subQuat(vec, quat, refquat);
// relative rotational Jacobian in global frame
mj_jacSite(m, d, NULL, jacS, id);
mj_jacSite(m, d, NULL, jacref, refid);
mju_subFrom(jacS, jacref, 3*nv);
// if common ancestral dof exists, clear the columns of its parental chain
{
int b0 = m->body_weldid[m->site_bodyid[id]];
int b1 = m->body_weldid[m->site_bodyid[refid]];
int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1;
int dofadr1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
int dofadr_common = -1;
if (dofadr0 >= 0 && dofadr1 >= 0) {
while (dofadr0 != dofadr1) {
if (dofadr0 < dofadr1) {
dofadr1 = m->dof_parentid[dofadr1];
} else {
dofadr0 = m->dof_parentid[dofadr0];
}
if (dofadr0 == -1 || dofadr1 == -1) {
break;
}
}
if (dofadr0 == dofadr1) {
dofadr_common = dofadr0;
}
}
int da = dofadr_common;
while (da >= 0) {
jacS[nv*0 + da] = 0;
jacS[nv*1 + da] = 0;
jacS[nv*2 + da] = 0;
da = m->dof_parentid[da];
}
}
// row k: site axis k in global frame, projected on the relative Jacobian; torque and
// velocity components are in the site frame, matching the ball branch's child frame
// (the expmap length components are frame-invariant: exp fixes its own axis)
for (int k=0; k < 3; k++) {
int outk = out + k;
rowadr[outk] = k == 0 ? adr : rowadr[outk-1] + rownnz[outk-1];
length[outk] = vec[k];
mjtNum wrench[3] = {d->site_xmat[9*id+k], d->site_xmat[9*id+k+3],
d->site_xmat[9*id+k+6]};
mju_mulMatTVec(moment_row, jacS, wrench, 3, nv);
// sparsity (compress)
nnz = 0;
for (int j=0; j < nv; j++) {
if (moment_row[j]) {
moment[rowadr[outk]+nnz] = moment_row[j];
colind[rowadr[outk]+nnz] = j;
nnz++;
}
}
rownnz[outk] = nnz;
}
}
break;
case mjTRN_SITE: // site
// get site translation (jac) and rotation (jacS) Jacobians in global frame
mj_jacSite(m, d, jac, jacS, id);
src/engine/engine_derivative.c
+22 -7
View File
@@ -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);
}
}
}
}
src/engine/engine_forward.c
+104 -4
View File
@@ -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;
}
}
}
}
}
src/engine/engine_io.c
+24 -1
View File
@@ -1390,7 +1390,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zero(d->qpos, m->nq);
mju_zero(d->qvel, m->nv);
mju_zero(d->act, m->na);
mju_zero(d->ctrl, m->nu);
mj_resetCtrl(m, d);
for (int i=0; i < m->neq; i++) d->eq_active[i] = m->eq_active0[i];
mju_zero(d->qfrc_applied, m->nv);
mju_zero(d->xfrc_applied, 6*m->nbody);
@@ -1639,6 +1639,17 @@ static void mj_logTimingDiagnostics(const mjData* d) {
}
// set ctrl to neutral values: zero, except quaternion inputs which reset to the identity
void mj_resetCtrl(const mjModel* m, mjData* d) {
mju_zero(d->ctrl, m->nu);
for (int i=0; i < m->nactuator; i++) {
if (m->actuator_gaintype[i] == mjGAIN_SO3 && m->actuator_ctrlspec[i] == mjCHART_QUAT) {
d->ctrl[m->actuator_ctrladr[i]] = 1;
}
}
}
// clear data, set data->qpos = model->qpos0
void mj_resetData(const mjModel* m, mjData* d) {
// emit step timing diagnostics before timers are cleared
@@ -2139,6 +2150,18 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_SO3:
// ball joint target (idslider == -1) or site + refsite target
if (idslider == -1) {
if (id < 0 || id >= m->njnt) {
return "Invalid model: actuator_trnid out of bounds.";
}
} else {
if (id < 0 || id >= m->nsite || idslider < 0 || idslider >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
}
break;
case mjTRN_UNDEFINED:
// actuator_trnid not used.
break;
src/engine/engine_io.h
+3
View File
@@ -124,6 +124,9 @@ MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
// copy mjData, skip large arrays not required for abstract visualization
MJAPI mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src);
// set ctrl to neutral values: zero, except quaternion inputs which reset to the identity
MJAPI void mj_resetCtrl(const mjModel* m, mjData* d);
// set data to defaults
MJAPI void mj_resetData(const mjModel* m, mjData* d);
src/engine/engine_name.c
+18
View File
@@ -281,3 +281,21 @@ const char* mj_id2name(const mjModel* m, int type, int id) {
return NULL;
}
// get name of actuator input, determined by the actuator type and input signature,
// returns NULL if the actuator type defines no input names
const char* mj_actuatorInputName(const mjModel* m, int id, int input) {
if (id < 0 || id >= m->nactuator || input < 0 || input >= m->actuator_ctrlnum[id]) {
return NULL;
}
// so3 orientation actuator: input names are chart components
if (m->actuator_gaintype[id] == mjGAIN_SO3) {
static const char* expmap[3] = {"rx", "ry", "rz"};
static const char* quat[4] = {"qw", "qx", "qy", "qz"};
return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input];
}
return NULL;
}
src/engine/engine_name.h
+3
View File
@@ -34,6 +34,9 @@ MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
// get name of object with the specified mjtObj type and id, returns NULL if name not found
MJAPI const char* mj_id2name(const mjModel* m, int type, int id);
// get name of actuator input; NULL if the actuator type defines no input names
MJAPI const char* mj_actuatorInputName(const mjModel* m, int id, int input);
#ifdef __cplusplus
}
#endif
src/engine/engine_sensor.c
+6 -6
View File
@@ -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
src/engine/engine_setconst.c
+3
View File
@@ -349,6 +349,9 @@ static void setFixed(mjModel* m, mjData* d) {
case mjTRN_JOINTINPARENT:
bodyid = m->jnt_bodyid[tid];
break;
case mjTRN_SO3:
bodyid = m->actuator_trnid[2*i+1] >= 0 ? m->site_bodyid[tid] : m->jnt_bodyid[tid];
break;
case mjTRN_SITE:
case mjTRN_SLIDERCRANK:
bodyid = m->site_bodyid[tid];
src/engine/engine_sleep.c
+9
View File
@@ -676,6 +676,15 @@ static mjtSleepState mj_actuatorSleepState(const mjModel* m, const mjData* d, in
case mjTRN_SITE:
return mj_sleepState(m, d, mjOBJ_SITE, trnid);
case mjTRN_SO3:
// ball joint target or site + refsite target
if (m->actuator_trnid[i*2+1] == -1) {
return mj_sleepState(m, d, mjOBJ_JOINT, trnid);
}
s1 = mj_sleepState(m, d, mjOBJ_SITE, trnid);
s2 = mj_sleepState(m, d, mjOBJ_SITE, m->actuator_trnid[i*2+1]);
return (s1 == mjS_AWAKE || s2 == mjS_AWAKE) ? mjS_AWAKE : mjS_ASLEEP;
case mjTRN_BODY:
return mj_sleepState(m, d, mjOBJ_BODY, trnid);
src/experimental/platform/ux/gui.cc
+34 -21
View File
@@ -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();
}
}
src/experimental/platform/ux/gui.h
+1 -1
View File
@@ -125,7 +125,7 @@ void VisualizationGui(mjModel* model, mjvOption* vis_options, mjvCamera* camera,
float min_width);
// UX for visualizing actuator controls data in mjData.
void ControlsGui(const mjModel* model, const mjData* data,
void ControlsGui(const mjModel* model, mjData* data,
const mjvOption* vis_options);
// UX for visualizing joint data in mjData.
src/user/user_api.cc
+25
View File
@@ -1324,6 +1324,31 @@ const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1],
// Set to orientation actuator.
const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
double dampratio[1], int ctrlspec) {
if (kv && dampratio) {
return "kv and dampratio cannot both be defined";
}
actuator->gainprm[0] = kp;
actuator->biasprm[1] = -kp;
if (kv) {
if (*kv < 0) return "kv cannot be negative";
actuator->biasprm[2] = -(*kv);
}
if (dampratio) {
if (*dampratio < 0) return "dampratio cannot be negative";
actuator->biasprm[2] = *dampratio;
}
actuator->ctrlspec = ctrlspec;
actuator->gaintype = mjGAIN_SO3;
actuator->biastype = mjBIAS_SO3;
actuator->dyntype = mjDYN_NONE;
return "";
}
// Set to velocity actuator.
const char* mjs_setToVelocity(mjsActuator* actuator, double kv) {
mjuu_zerovec(actuator->biasprm, mjNBIAS);
src/user/user_api.h
+4
View File
@@ -185,6 +185,10 @@ MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double
// Set actuator to velocity, return error on failure.
MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
// Set to orientation actuator.
MJAPI const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
double dampratio[1], int ctrlspec);
// Set actuator to damper, return error on failure.
MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
src/user/user_init.c
+1
View File
@@ -341,6 +341,7 @@ void mjs_defaultActuator(mjsActuator* actuator) {
actuator->dyntype = mjDYN_NONE;
actuator->dynprm[0] = 1;
actuator->actdim = -1;
actuator->ctrlspec = 0;
// transmission
actuator->trntype = mjTRN_UNDEFINED;
src/user/user_model.cc
+11 -5
View File
@@ -3394,6 +3394,11 @@ int mjCModel::CountNJmom(const mjModel* m) {
// process according to transmission type
switch ((mjtTrn)m->actuator_trntype[i]) {
case mjTRN_SO3:
// ball joint: 3 identity rows; site+refsite: 3 dense rows
count += m->actuator_trnid[2*i+1] >= 0 ? 3*nv : 3;
break;
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
switch ((mjtJoint)m->jnt_type[id]) {
@@ -4011,7 +4016,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mjCActuator* pac = actuators_[i];
// set fields
m->actuator_trntype[i] = pac->trntype;
m->actuator_trntype[i] = pac->so3_ ? mjTRN_SO3 : pac->trntype;
m->actuator_dyntype[i] = pac->dyntype;
m->actuator_gaintype[i] = pac->gaintype;
m->actuator_biastype[i] = pac->biastype;
@@ -4024,9 +4029,10 @@ void mjCModel::CopyObjects(mjModel* m) {
adr += m->actuator_actnum[i];
m->actuator_group[i] = pac->group;
// input and output blocks; all actuator types are currently 1x1
// input and output blocks
m->actuator_ctrladr[i] = ctrladr;
m->actuator_ctrlnum[i] = pac->ctrlnum_;
m->actuator_ctrlspec[i] = pac->ctrlspec_;
pac->ctrladr_ = ctrladr;
ctrladr += pac->ctrlnum_;
m->actuator_outadr[i] = outadr;
@@ -4055,6 +4061,8 @@ void mjCModel::CopyObjects(mjModel* m) {
mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN);
mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS);
mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2);
m->actuator_forcelimited[i] = (mjtBool)pac->is_forcelimited();
mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2);
mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator);
// per-input arrays, at the actuator's ctrl block
@@ -4066,8 +4074,6 @@ void mjCModel::CopyObjects(mjModel* m) {
// per-output arrays, at the actuator's output block
for (int j=m->actuator_outadr[i]; j < m->actuator_outadr[i]+m->actuator_outnum[i]; j++) {
m->actuator_forcelimited[j] = (mjtBool)pac->is_forcelimited();
mjuu_copyvec(m->actuator_forcerange + 2*j, pac->forcerange, 2);
mjuu_copyvec(m->actuator_gear + 6*j, pac->gear, 6);
mjuu_copyvec(m->actuator_lengthrange + 2*j, pac->lengthrange, 2);
}
@@ -5972,7 +5978,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN);
mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS);
mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*m->actuator_ctrladr[i], 2);
mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*m->actuator_outadr[i], 2);
mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2);
mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2);
mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*m->actuator_outadr[i], 2);
mjuu_copyvec(pa->gear, m->actuator_gear+6*m->actuator_outadr[i], 6);
src/user/user_objects.cc
+87 -1
View File
@@ -6931,8 +6931,10 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
// input and output blocks, set by mjCModel; all actuator types are currently 1x1
ctrladr_ = -1;
ctrlnum_ = 1;
ctrlspec_ = 0;
outadr_ = -1;
outnum_ = 1;
so3_ = false;
}
@@ -7123,6 +7125,12 @@ void mjCActuator::ResolveReferences(const mjCModel* m) {
void mjCActuator::Compile(void) {
CopyFromSpec();
// reset input/output block widths, resolved below
ctrlnum_ = 1;
ctrlspec_ = 0;
outnum_ = 1;
so3_ = false;
// resize userdata
if (userdata_.size() > model->nuser_actuator) {
throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)",
@@ -7139,6 +7147,79 @@ void mjCActuator::Compile(void) {
// find transmission target in object arrays
ResolveReferences(model);
// SO3 geodesic servo: validate and resolve the SO3 transmission
if (gaintype == mjGAIN_SO3 || biastype == mjBIAS_SO3) {
if (gaintype != mjGAIN_SO3 || biastype != mjBIAS_SO3) {
throw mjCError(this, "gaintype and biastype must both be 'so3' in actuator '%s' (id = %d)",
name.c_str(), id);
}
if (dyntype != mjDYN_NONE && dyntype != mjDYN_INTEGRATOR) {
throw mjCError(this, "so3 requires dyntype 'none' or 'integrator' in actuator '%s' (id = %d)",
name.c_str(), id);
}
if (gainprm[0] != -biasprm[1]) {
throw mjCError(this, "so3 requires gainprm[0] == -biasprm[1] in actuator '%s' (id = %d)",
name.c_str(), id);
}
if (trntype == mjTRN_SITE) {
if (refsite_.empty()) {
throw mjCError(this, "so3 site transmission requires refsite in actuator '%s' (id = %d)",
name.c_str(), id);
}
} else if (trntype == mjTRN_JOINT) {
if (((mjCJoint*)ptarget)->spec.type != mjJNT_BALL) {
throw mjCError(this, "so3 joint transmission requires a ball joint in actuator '%s' "
"(id = %d)", name.c_str(), id);
}
} else {
throw mjCError(this, "so3 requires site or ball joint transmission in actuator '%s' "
"(id = %d)", name.c_str(), id);
}
// integrator variant: activation is the 3D orientation setpoint
if (dyntype == mjDYN_INTEGRATOR) {
if (actdim > 0 && actdim != 3) {
throw mjCError(this, "so3 integrator requires actdim 3 in actuator '%s' (id = %d)",
name.c_str(), id);
}
actdim = 3;
// the act setpoint is re-anchored to a bounded representative at integration time
if (actlimited == mjLIMITED_TRUE && actrange[0] == 0 && actrange[1] == 0) {
actlimited = mjLIMITED_FALSE;
}
}
// input chart: expmap (3 controls, default) or quat (4 controls)
ctrlspec_ = ctrlspec ? ctrlspec : mjCHART_EXPMAP;
if (ctrlspec_ == mjCHART_QUAT) {
if (dyntype != mjDYN_NONE) {
throw mjCError(this, "so3 quat input requires dyntype 'none' in actuator '%s' (id = %d)",
name.c_str(), id);
}
} else if (ctrlspec_ != mjCHART_EXPMAP) {
throw mjCError(this, "so3 input must be expmap or quat in actuator '%s' (id = %d)",
name.c_str(), id);
}
// force is clamped on the norm of the output torque: lower bound must be 0
if (is_forcelimited() && forcerange[0] != 0) {
throw mjCError(this, "so3 forcerange bounds the force norm, lower bound must be 0 in "
"actuator '%s' (id = %d)", name.c_str(), id);
}
// input and output blocks
ctrlnum_ = ctrlspec_ == mjCHART_QUAT ? 4 : 3;
outnum_ = 3;
so3_ = true;
}
// input signature selection is so3-only
if (ctrlspec && gaintype != mjGAIN_SO3) {
throw mjCError(this, "input is only available for so3 actuators, actuator '%s' (id = %d)",
name.c_str(), id);
}
// check damping/armature only valid for joint and tendon transmission
bool has_damping = false;
for (int i = 0; i < mjNPOLY+1; i++) {
@@ -7234,7 +7315,7 @@ void mjCActuator::Compile(void) {
// check and set actdim
if (!plugin.active) {
if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR) {
if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR && !so3_) {
throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' and 'dcmotor'");
}
if (actdim == 1 && dyntype == mjDYN_NONE) {
@@ -7970,6 +8051,11 @@ void mjCSensor::Compile(void) {
dim = mjs_sensorDim(this);
// actuator sensors report one value per force output
if (type == mjSENS_ACTUATORPOS || type == mjSENS_ACTUATORVEL || type == mjSENS_ACTUATORFRC) {
dim = ((mjCActuator*)obj)->outnum_;
}
// check cutoff for incompatible data types
if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
(datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
src/user/user_objects.h
+3
View File
@@ -1812,8 +1812,10 @@ class mjCActuator_ : public mjCBase {
int actdim_; // number of dofs in data->act
int ctrladr_; // address of first control in data->ctrl
int ctrlnum_; // number of controls
int ctrlspec_; // resolved input signature, scoped by gaintype
int outadr_; // address of first force output
int outnum_; // number of force outputs, from trntype
bool so3_; // compiles to an SO3 transmission
std::map<std::string, std::vector<mjtNum>> act_; // act at the previous step
std::map<std::string, mjtNum> ctrl_; // ctrl at the previous step
@@ -1833,6 +1835,7 @@ class mjCActuator_ : public mjCBase {
class mjCActuator : public mjCActuator_, private mjsActuator {
friend class mjCDef;
friend class mjCModel;
friend class mjCSensor;
friend class mjXWriter;
public:
src/xml/xml_base.h
+2
View File
@@ -41,6 +41,7 @@ extern const int colorspace_sz;
extern const int builtin_sz;
extern const int mark_sz;
extern const int dyn_sz;
extern const int input_sz;
extern const int gain_sz;
extern const int bias_sz;
extern const int interp_sz;
@@ -75,6 +76,7 @@ extern const mjMap texrole_map[];
extern const mjMap builtin_map[];
extern const mjMap mark_map[];
extern const mjMap dyn_map[];
extern const mjMap input_map[];
extern const mjMap gain_map[];
extern const mjMap bias_map[];
extern const mjMap interp_map[];
src/xml/xml_native_reader.cc
+48 -4
View File
@@ -258,7 +258,7 @@ std::vector<const char*> MJCF[nMJCF] = {
"margin", "stiffness", "damping", "rgba", "user"},
{"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
"actrange", "gear", "damping", "armature", "cranklength", "user", "group", "nsample",
"interp", "delay", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
"interp", "delay", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
"biasprm", "actearly"},
{"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay"},
@@ -476,7 +476,7 @@ std::vector<const char*> MJCF[nMJCF] = {
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "damping", "armature", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
"body", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
"actearly"},
{"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -498,6 +498,10 @@ std::vector<const char*> MJCF[nMJCF] = {
"gear", "damping", "armature", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv", "dampratio"},
{"orientation", "*", "name", "class", "group", "nsample", "interp", "delay",
"forcelimited", "ctrlrange", "forcerange", "user",
"joint", "site", "refsite",
"kp", "kv", "dampratio", "input"},
{"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
"forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "damping", "armature", "cranklength", "user",
@@ -840,23 +844,33 @@ const mjMap dcmotorinput_map[dcmotorinput_sz] = {
// gain type
const int gain_sz = 5;
const int gain_sz = 6;
const mjMap gain_map[gain_sz] = {
{"fixed", mjGAIN_FIXED},
{"affine", mjGAIN_AFFINE},
{"muscle", mjGAIN_MUSCLE},
{"dcmotor", mjGAIN_DCMOTOR},
{"so3", mjGAIN_SO3},
{"user", mjGAIN_USER}
};
// so3 input chart
const int input_sz = 2;
const mjMap input_map[input_sz] = {
{"expmap", mjCHART_EXPMAP},
{"quat", mjCHART_QUAT}
};
// bias type
const int bias_sz = 5;
const int bias_sz = 6;
const mjMap bias_map[bias_sz] = {
{"none", mjBIAS_NONE},
{"affine", mjBIAS_AFFINE},
{"muscle", mjBIAS_MUSCLE},
{"dcmotor", mjBIAS_DCMOTOR},
{"so3", mjBIAS_SO3},
{"user", mjBIAS_USER}
};
@@ -2514,6 +2528,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
ReadAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, text, false, false);
ReadAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, text, false, false);
ReadAttrInt(elem, "actdim", &actuator->actdim);
if (MapValue(elem, "input", &n, input_map, input_sz)) {
actuator->ctrlspec = n;
}
}
// direct drive motor
@@ -2558,6 +2575,32 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
}
}
// orientation servo: geodesic PD on an SO3 transmission
else if (type == "orientation") {
double kp = actuator->gainprm[0];
ReadAttr(elem, "kp", 1, &kp, text);
double kv_data;
double *kv = &kv_data;
if (!ReadAttr(elem, "kv", 1, kv, text)) {
kv = nullptr;
}
double dampratio_data;
double *dampratio = &dampratio_data;
if (!ReadAttr(elem, "dampratio", 1, dampratio, text)) {
dampratio = nullptr;
}
// input chart: expmap (default) or quat
int n;
if (MapValue(elem, "input", &n, input_map, input_sz)) {
actuator->ctrlspec = n;
}
err = mjs_setToOrientation(actuator, kp, kv, dampratio, actuator->ctrlspec);
}
// velocity servo
else if (type == "velocity") {
double kv = actuator->gainprm[0];
@@ -3122,6 +3165,7 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS*
name == "velocity" ||
name == "damper" ||
name == "intvelocity" ||
name == "orientation" ||
name == "cylinder" ||
name == "muscle" ||
name == "adhesion" ||
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 248
#define nMJCF 249
extern std::vector<const char*> MJCF[nMJCF];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+1
View File
@@ -899,6 +899,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
// non-plugins: write actuator parameters
else {
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
WriteAttrKey(elem, "input", input_map, input_sz, actuator->ctrlspec, def->Actuator().ctrlspec);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);
test/engine/engine_core_smooth_test.cc
+596
View File
@@ -38,6 +38,8 @@ namespace {
using ::std::string;
using ::std::vector;
using ::testing::Each;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::ElementsAre;
using ::testing::Not;
using ::testing::NotNull;
@@ -782,6 +784,600 @@ TEST_F(CoreSmoothTest, ForwardDoesNotMutateAct) {
mj_deleteData(data);
}
// expmap (axis-angle) vector to quaternion
static void Expmap2Quat(mjtNum quat[4], const mjtNum v[3]) {
mjtNum angle = mju_norm3(v);
if (angle < mjMINVAL) {
quat[0] = 1;
quat[1] = quat[2] = quat[3] = 0;
} else {
mjtNum axis[3] = {v[0]/angle, v[1]/angle, v[2]/angle};
mju_axisAngle2Quat(quat, axis, angle);
}
}
// geodesic distance between the orientations given by expmap vectors u and v
static mjtNum GeodesicError(const mjtNum u[3], const mjtNum v[3]) {
mjtNum q_tgt[4], q_cur[4], q_err[4], e[3];
Expmap2Quat(q_tgt, u);
Expmap2Quat(q_cur, v);
mju_negQuat(q_cur, q_cur);
mju_mulQuat(q_err, q_tgt, q_cur);
mju_quat2Vel(e, q_err, 1);
return mju_norm3(e);
}
// mixed model: three scalar translation servos and one SO3 orientation servo
static constexpr char kSO3RefsiteXml[] = R"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<site name="reference"/>
<body name="box">
<freejoint/>
<geom type="box" size=".05 .07 .03"/>
<site name="end_effector"/>
</body>
</worldbody>
<actuator>
<position name="x" site="end_effector" refsite="reference" gear="1 0 0 0 0 0"
kp="100" dampratio="1"/>
<orientation name="orient" site="end_effector" refsite="reference" kp="1" dampratio="1"/>
<position name="y" site="end_effector" refsite="reference" gear="0 1 0 0 0 0"
kp="100" dampratio="1"/>
<position name="z" site="end_effector" refsite="reference" gear="0 0 1 0 0 0"
kp="100" dampratio="1"/>
</actuator>
<sensor>
<actuatorpos actuator="orient"/>
<actuatorfrc actuator="orient"/>
</sensor>
</mujoco>
)";
// Layout of a mixed model: the SO3 actuator owns 3-wide control and output
// blocks, misaligning nu/nout/nactuator with the actuator index; sensors on it
// are 3-dimensional; the model round-trips through XML.
TEST_F(CoreSmoothTest, SO3MixedModelLayout) {
char error[1024];
MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
// counts: 4 actuators, 3+3 controls, 3+3 force outputs
EXPECT_EQ(model->nactuator, 4);
EXPECT_EQ(model->nu, 6);
EXPECT_EQ(model->nout, 6);
// the orientation actuator is second, exercising address accumulation
int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
ASSERT_EQ(orient, 1);
EXPECT_EQ(model->actuator_ctrladr[orient], 1);
EXPECT_EQ(model->actuator_ctrlnum[orient], 3);
EXPECT_EQ(model->actuator_outadr[orient], 1);
EXPECT_EQ(model->actuator_outnum[orient], 3);
EXPECT_EQ(model->actuator_trntype[orient], mjTRN_SO3);
// actuator sensors report one value per force output
EXPECT_EQ(model->sensor_dim[0], 3);
EXPECT_EQ(model->sensor_dim[1], 3);
EXPECT_EQ(model->sensor_adr[1], 3);
// XML round-trip preserves the layout
std::string saved = SaveAndReadXml(model.get());
MjModelPtr model2 = LoadModelFromString(saved.c_str(), error, sizeof(error));
ASSERT_THAT(model2.get(), NotNull()) << error;
EXPECT_EQ(model2->nactuator, 4);
EXPECT_EQ(model2->nu, 6);
EXPECT_EQ(model2->nout, 6);
EXPECT_EQ(model2->actuator_trntype[orient], mjTRN_SO3);
}
// A mixed-axis orientation target beyond the pi shell is a true equilibrium:
// zero force when the body is at the commanded orientation, convergence to it
// from the initial state.
TEST_F(CoreSmoothTest, SO3RefsiteMixedAxisEquilibrium) {
char error[1024];
MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model.get());
int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
int uadr = model->actuator_ctrladr[orient];
int oadr = model->actuator_outadr[orient];
// target: 5.66 rad rotation about the mixed axis (1,1,0)/sqrt(2), beyond pi;
// canonical (shortest) expmap is u*(1 - 2*pi/norm(u)) = (-.4429, -.4429, 0)
mjtNum target[3] = {4, 4, 0};
mjtNum shrink = 1 - 2*mjPI/mju_norm3(target);
mjtNum canonical[3] = {target[0]*shrink, target[1]*shrink, target[2]*shrink};
mju_copy3(data->ctrl + uadr, target);
// place the body exactly at the commanded orientation: force must vanish
Expmap2Quat(data->qpos + 3, target);
mj_forward(model.get(), data);
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - canonical[k]),
MjTol(1e-10, 1e-6));
// sensors: actuatorpos = canonical expmap, actuatorfrc = 0
EXPECT_LT(mju_abs(data->sensordata[k] - canonical[k]), MjTol(1e-10, 1e-6));
EXPECT_LT(mju_abs(data->sensordata[3 + k]), MjTol(1e-10, 1e-6));
}
// from the initial state, converge to the commanded orientation
mj_resetData(model.get(), data);
mju_copy3(data->ctrl + uadr, target);
while (data->time < 10) {
mj_step(model.get(), data);
}
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - canonical[k]), 1e-3);
EXPECT_LT(mju_abs(data->actuator_velocity[oadr + k]), 1e-3);
}
mj_deleteData(data);
}
// Test smooth tracking while winding one axis with another axis held nonzero:
// the regime where per-axis servo errors cannot work and only the geodesic
// error on SO(3) tracks correctly.
TEST_F(CoreSmoothTest, SO3RefsiteTracksMixedWindingTarget) {
char error[1024];
MjModelPtr model = LoadModelFromString(kSO3RefsiteXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model.get());
int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
int uadr = model->actuator_ctrladr[orient];
int oadr = model->actuator_outadr[orient];
// hold rx at 1 rad, let the servo settle
data->ctrl[uadr] = 1;
while (data->time < 2) {
mj_step(model.get(), data);
}
// ramp the rz target from 0 to 2*pi
const mjtNum rate = 0.5; // rad/s
mjtNum start = data->time;
while (data->time - start < 2*mjPI / rate) {
data->ctrl[uadr + 2] = rate * (data->time - start);
mj_step(model.get(), data);
// geodesic distance between commanded and current orientation
mjtNum err = GeodesicError(data->ctrl + uadr, data->actuator_length + oadr);
ASSERT_LT(err, 0.5) << "tracking lost at time " << data->time
<< ", target rz " << data->ctrl[uadr + 2];
}
mj_deleteData(data);
}
// Mixed-axis target beyond the pi shell on ball joints: the SO3 actuator has
// an exact equilibrium at the commanded orientation, per-axis wrapped servos
// do not.
TEST_F(CoreSmoothTest, SO3BallMixedAxisContrast) {
constexpr char kOrientationPath[] =
"engine/testdata/actuation/orientation.xml";
const std::string xml_path = GetTestDataFilePath(kOrientationPath);
char error[1024];
MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model.get());
// place both joints exactly at the target orientation, expmap (4, 4, 0)
mjtNum target[3] = {4, 4, 0};
mjtNum quat[4];
Expmap2Quat(quat, target);
for (const char* name : {"peraxis", "expmap"}) {
int jnt = mj_name2id(model.get(), mjOBJ_JOINT, name);
mju_copy4(data->qpos + model->jnt_qposadr[jnt], quat);
}
int rx_peraxis = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rx_peraxis");
int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
int uadr = model->actuator_ctrladr[orient];
int oadr = model->actuator_outadr[orient];
data->ctrl[rx_peraxis] = data->ctrl[rx_peraxis + 1] = 4;
data->ctrl[uadr] = data->ctrl[uadr + 1] = 4;
mj_forward(model.get(), data);
// SO3: zero force at the commanded orientation
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
}
// per-axis: residual force kp*(4.443 - 2*pi) = -1.84 on the wound members
EXPECT_GT(mju_abs(data->actuator_force[rx_peraxis]), 1);
EXPECT_GT(mju_abs(data->actuator_force[rx_peraxis + 1]), 1);
mj_deleteData(data);
}
// Geodesic servo converges to large mixed-axis targets from rest: no limit
// cycles. Regression test: a parent-frame error driving child-frame torques is
// non-gradient feedback which pumps energy at large angles, settling into
// steady spinning.
TEST_F(CoreSmoothTest, SO3LargeAngleConvergence) {
constexpr char kOrientationPath[] =
"engine/testdata/actuation/orientation.xml";
const std::string xml_path = GetTestDataFilePath(kOrientationPath);
char error[1024];
MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model.get());
int expmap = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
int jnt = mj_name2id(model.get(), mjOBJ_JOINT, "expmap");
int uadr = model->actuator_ctrladr[expmap];
int dofadr = model->jnt_dofadr[jnt];
mjtNum targets[4][3] = {{1, 1, 1}, {-1, -1, 0}, {1, -1, 1}, {2.2, 2.2, 2.2}};
for (const auto& u : targets) {
SCOPED_TRACE(testing::Message()
<< "target (" << u[0] << ", " << u[1] << ", " << u[2] << ")");
mj_resetData(model.get(), data);
mju_copy3(data->ctrl + uadr, u);
for (int i = 0; i < 4000; i++) {
mj_step(model.get(), data);
}
// orientation error and angular velocity vanish
mjtNum q_tgt[4], q_cur[4], e[3];
mjtNum axis[3] = {u[0], u[1], u[2]};
mjtNum angle = mju_normalize3(axis);
mju_axisAngle2Quat(q_tgt, axis, angle);
mju_copy4(q_cur, data->qpos + model->jnt_qposadr[jnt]);
mju_normalize4(q_cur);
mju_subQuat(e, q_tgt, q_cur);
EXPECT_LT(mju_norm3(e), MjTol(1e-4, 1e-2));
EXPECT_LT(mju_norm3(data->qvel + dofadr), MjTol(1e-4, 1e-2));
}
mj_deleteData(data);
}
// Neutral ctrl: reset zeroes all controls except quat (to the identity).
TEST_F(CoreSmoothTest, SO3QuatNeutralCtrl) {
constexpr char kOrientationPath[] =
"engine/testdata/actuation/orientation.xml";
const std::string xml_path = GetTestDataFilePath(kOrientationPath);
char error[1024];
MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model.get());
int quat = mj_name2id(model.get(), mjOBJ_ACTUATOR, "quat");
int uadr = model->actuator_ctrladr[quat];
for (int trial = 0; trial < 2; trial++) {
for (int j = 0; j < model->nu; j++) {
EXPECT_EQ(data->ctrl[j], j == uadr ? 1 : 0)
<< "ctrl " << j << " trial " << trial;
}
mju_fill(data->ctrl, 0.5, model->nu);
mj_resetData(model.get(), data);
}
mj_deleteData(data);
}
// Input names: NULL for single-input actuators, chart components for SO3.
TEST_F(CoreSmoothTest, ActuatorInputNames) {
constexpr char kOrientationPath[] =
"engine/testdata/actuation/orientation.xml";
const std::string xml_path = GetTestDataFilePath(kOrientationPath);
char error[1024];
MjModelPtr model(mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)));
ASSERT_THAT(model.get(), NotNull()) << "Failed to load model: " << error;
int rx_peraxis = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rx_peraxis");
int expmap = mj_name2id(model.get(), mjOBJ_ACTUATOR, "expmap");
int quat = mj_name2id(model.get(), mjOBJ_ACTUATOR, "quat");
EXPECT_EQ(mj_actuatorInputName(model.get(), rx_peraxis, 0), nullptr);
EXPECT_STREQ(mj_actuatorInputName(model.get(), expmap, 0), "rx");
EXPECT_STREQ(mj_actuatorInputName(model.get(), expmap, 2), "rz");
EXPECT_EQ(mj_actuatorInputName(model.get(), expmap, 3),
nullptr); // out of range
EXPECT_STREQ(mj_actuatorInputName(model.get(), quat, 0), "qw");
EXPECT_STREQ(mj_actuatorInputName(model.get(), quat, 3), "qz");
}
// SO3 integrator variant: act is the 3D orientation setpoint; constant ctrl
// produces steady rotation over many periods with bounded activation.
TEST_F(CoreSmoothTest, SO3IntVelocityWindsWithBoundedAct) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<general name="rot" joint="ball" dyntype="integrator"
gaintype="so3" biastype="so3" gainprm="1" biasprm="0 -1 -1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
// one actuator: 3 controls, 3 outputs, 3 activations
EXPECT_EQ(model->nu, 3);
EXPECT_EQ(model->nout, 3);
EXPECT_EQ(model->na, 3);
mjData* data = mj_makeData(model.get());
// spin about z for 4 full turns
const mjtNum rate = 1.0; // rad/s
data->ctrl[2] = rate;
while (data->time < 8*mjPI / rate) {
mj_step(model.get(), data);
ASSERT_LT(mju_norm3(data->act), mjPI + 0.1) << "act unbounded";
}
// steady rotation at the commanded rate about z
EXPECT_NEAR(data->actuator_velocity[2], rate, 0.02);
mj_deleteData(data);
}
// Compile-time validation of the SO3 actuator.
TEST_F(CoreSmoothTest, SO3CompileErrors) {
char error[1024];
// hinge joint target: rejected
static constexpr char hinge_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size=".05"/>
</body>
</worldbody>
<actuator>
<orientation joint="hinge" kp="1"/>
</actuator>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(hinge_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("ball"));
// site without refsite: rejected
static constexpr char nosite_xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<geom size=".05"/>
<site name="ee"/>
</body>
</worldbody>
<actuator>
<orientation site="ee" kp="1"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(nosite_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("refsite"));
// mismatched gaintype/biastype: rejected
static constexpr char mismatch_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<general joint="ball" gaintype="so3" biastype="affine" gainprm="1"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(mismatch_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("both"));
// forcerange with nonzero lower bound: rejected (force clamped on the norm)
static constexpr char forcerange_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<orientation joint="ball" kp="1" forcerange="-4 4"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(forcerange_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("lower bound must be 0"));
}
// forcerange clamps the norm of the SO3 output torque, preserving direction.
TEST_F(CoreSmoothTest, SO3ForcerangeClampsNorm) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="unclamped" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
<body pos="0 0 .3">
<joint name="clamped" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<orientation name="unclamped" joint="unclamped" kp="1"/>
<orientation name="clamped" joint="clamped" kp="1" forcerange="0 .5"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model.get());
// command the same mixed-axis target, error norm sqrt(5) > 0.5
int unclamped = mj_name2id(model.get(), mjOBJ_ACTUATOR, "unclamped");
int clamped = mj_name2id(model.get(), mjOBJ_ACTUATOR, "clamped");
for (int i : {unclamped, clamped}) {
int uadr = model->actuator_ctrladr[i];
data->ctrl[uadr + 0] = 1;
data->ctrl[uadr + 1] = 2;
data->ctrl[uadr + 2] = 0;
}
mj_forward(model.get(), data);
// clamped force has norm forcerange[1], parallel to the unclamped force
const mjtNum* f_unclamped =
data->actuator_force + model->actuator_outadr[unclamped];
const mjtNum* f_clamped =
data->actuator_force + model->actuator_outadr[clamped];
mjtNum norm_unclamped = mju_norm3(f_unclamped);
EXPECT_GT(norm_unclamped, 0.5);
EXPECT_NEAR(mju_norm3(f_clamped), 0.5, MjTol(1e-12, 1e-6));
mjtNum scale = 0.5 / norm_unclamped;
for (int k = 0; k < 3; k++) {
EXPECT_NEAR(f_clamped[k], scale * f_unclamped[k], MjTol(1e-12, 1e-6));
}
mj_deleteData(data);
}
// Quat-setpoint variant of the SO3 servo: 4 inputs, 3 outputs -- the first
// actuator with different input and output widths.
TEST_F(CoreSmoothTest, SO3QuatSetpoint) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body pos="-.15 0 .2">
<joint name="ball_scalar" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
<body pos=".15 0 .2">
<joint name="ball_quat" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<position name="rz" joint="ball_scalar" gear="0 0 1" kp="1" dampratio="1"/>
<orientation name="orient" joint="ball_quat" kp="1" dampratio="1" input="quat"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
// rectangular layout: 2 actuators, 1+4 controls, 1+3 force outputs
EXPECT_EQ(model->nactuator, 2);
EXPECT_EQ(model->nu, 5);
EXPECT_EQ(model->nout, 4);
int orient = mj_name2id(model.get(), mjOBJ_ACTUATOR, "orient");
int uadr = model->actuator_ctrladr[orient];
int oadr = model->actuator_outadr[orient];
EXPECT_EQ(uadr, 1);
EXPECT_EQ(model->actuator_ctrlnum[orient], 4);
EXPECT_EQ(oadr, 1);
EXPECT_EQ(model->actuator_outnum[orient], 3);
// XML round-trip preserves the input chart
std::string saved = SaveAndReadXml(model.get());
MjModelPtr model2 = LoadModelFromString(saved.c_str(), error, sizeof(error));
ASSERT_THAT(model2.get(), NotNull()) << error;
EXPECT_EQ(model2->nu, 5);
EXPECT_EQ(model2->nout, 4);
mjData* data = mj_makeData(model.get());
// zero ctrl commands the identity orientation: zero force at qpos0
mj_forward(model.get(), data);
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6));
}
// target beyond the pi shell, mixed axis
mjtNum target[3] = {4, 4, 0};
mjtNum q_tgt[4];
Expmap2Quat(q_tgt, target);
int jnt = mj_name2id(model.get(), mjOBJ_JOINT, "ball_quat");
// scale and antipodal invariance: q, 2q and -q command the same orientation
mjtNum ctrl_variants[3][4];
mju_copy4(ctrl_variants[0], q_tgt);
for (int k=0; k < 4; k++) {
ctrl_variants[1][k] = 2*q_tgt[k];
ctrl_variants[2][k] = -q_tgt[k];
}
for (int v=0; v < 3; v++) {
mj_resetData(model.get(), data);
mju_copy4(data->qpos + model->jnt_qposadr[jnt], q_tgt);
mju_copy4(data->ctrl + uadr, ctrl_variants[v]);
mj_forward(model.get(), data);
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_force[oadr + k]), MjTol(1e-10, 1e-6))
<< "variant " << v;
}
}
// from the initial state, converge to the commanded orientation
mj_resetData(model.get(), data);
mju_copy4(data->ctrl + uadr, q_tgt);
while (data->time < 10) {
mj_step(model.get(), data);
}
mjtNum shrink = 1 - 2*mjPI/mju_norm3(target);
for (int k=0; k < 3; k++) {
EXPECT_LT(mju_abs(data->actuator_length[oadr + k] - target[k] * shrink),
1e-3);
EXPECT_LT(mju_abs(data->actuator_velocity[oadr + k]), 1e-3);
}
mj_deleteData(data);
}
// The quat input chart requires stateless dynamics.
TEST_F(CoreSmoothTest, SO3QuatSetpointRequiresStateless) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<general joint="ball" dyntype="integrator" input="quat"
gaintype="so3" biastype="so3" gainprm="1" biasprm="0 -1 -1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("dyntype"));
}
static const char* const kInertiaPath = "engine/testdata/inertia.xml";
TEST_F(CoreSmoothTest, FactorI) {
test/engine/engine_derivative_test.cc
+53
View File
@@ -1226,6 +1226,59 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
<< "when forcerange derivatives are correctly handled";
}
// forcelimited actuator following a multi-output SO3 actuator: the derivative
// skip for saturated actuators must index forcerange per actuator, not per
// output.
TEST_F(DerivativeTest, ForcerangeClampedAfterSO3) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
<body pos="0 0 .3">
<joint name="hinge"/>
<geom size=".05"/>
</body>
</worldbody>
<actuator>
<orientation joint="ball" kp="1" kv="1"/>
<velocity joint="hinge" kv="10" forcerange="-1 1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
// spin the hinge so the velocity actuator saturates: force -50, clamped -1
mjtNum qvel[4] = {0.1, 0.2, 0.3, 5};
mju_copy(d->qvel, qvel, 4);
mj_forward(m, d);
ASSERT_EQ(d->actuator_force[3], -1);
// analytic qDeriv
mju_zero(d->qDeriv, m->nD);
mjd_smooth_vel(m, d, /*flg_bias=*/1);
vector<mjtNum> qDerivAnalytic = AsVector(d->qDeriv, m->nD);
EXPECT_GT(mju_norm(qDerivAnalytic.data(), m->nD), 0);
// expect match with finite differences: the saturated actuator contributes
// nothing, the SO3 actuator's damping is unaffected by its neighbor
mjtNum eps = MjTol(1e-7, 1e-3);
mju_zero(d->qDeriv, m->nD);
mjd_smooth_velFD(m, d, eps);
EXPECT_THAT(AsVector(d->qDeriv, m->nD),
Pointwise(MjNear(1e-7, 3e-3), qDerivAnalytic));
}
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
static constexpr char xml[] = R"(
<mujoco>
test/engine/testdata/actuation/orientation.xml
Vendored
+78
View File
@@ -0,0 +1,78 @@
<mujoco>
<compiler autolimits="true"/>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<statistic meansize=".05" extent=".9" center="0 0 .15"/>
<visual>
<headlight diffuse=".1 .1 .1" ambient=".15 .15 .15"/>
</visual>
<!--
Three identical ball-jointed boxes, illustrating the three rotational setpoint semantics.
The "per-axis" box has three independent position servos: each setpoint is interpreted on
its own circle (wrapped), exact for rotations about a single gear axis. The "expmap" and
"quat" boxes each have a single orientation actuator: the setpoints are interpreted
jointly as an orientation on SO(3), exact for any combination of axes, commanded in the
exponential-map and quaternion charts respectively.
-->
<asset>
<texture name="checker" type="cube" builtin="checker" rgb1=".75 .55 .28" rgb2=".22 .18 .14" width="32" height="32"/>
<material name="checker" texture="checker"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<geom type="box" size=".07 .09 .05" material="checker"/>
<position kp="5" dampratio="1"/>
<default class="angle">
<position ctrlrange="-6.2832 6.2832" ctrllimited="false"/>
</default>
<default class="quat">
<position ctrlrange="-1 1" ctrllimited="true"/>
</default>
<site size="1e-5"/>
</default>
<worldbody>
<light pos="0 0 10" type="point" diffuse=".75 .72 .65" intensity="1200000" range="20" attenuation="1 0 .04"/>
<light pos="-.3 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="13" attenuation="1 0 0" exponent="60"/>
<light pos="0 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="16" exponent="60" attenuation="1 0 .5"/>
<light pos=".3 -.4 1.3" dir="0 .34 -.94" diffuse=".75 .72 .65" intensity="100000" range="4" cutoff="16" exponent="60" attenuation="1 0 .5"/>
<light type="point" pos="0 -1.5 2" diffuse=".3 .35 .45" intensity="15000" range="8" castshadow="false" attenuation="1 0 .3"/>
<geom name="floor" type="plane" size="2 2 .025" material="grid"/>
<site name="per-axis" pos="-.3 0 .4"/>
<body name="peraxis" pos="-.3 0 .2">
<joint name="peraxis" type="ball"/>
<geom/>
</body>
<site name="expmap" pos="0 0 .4"/>
<body name="expmap" pos="0 0 .2">
<joint name="expmap" type="ball"/>
<geom/>
</body>
<site name="quat" pos=".3 0 .4"/>
<body name="quat" pos=".3 0 .2">
<joint name="quat" type="ball"/>
<geom/>
</body>
</worldbody>
<actuator>
<position name="rx_peraxis" joint="peraxis" gear="1 0 0" class="angle"/>
<position name="ry_peraxis" joint="peraxis" gear="0 1 0" class="angle"/>
<position name="rz_peraxis" joint="peraxis" gear="0 0 1" class="angle"/>
<orientation name="expmap" joint="expmap" kp="1" dampratio="1" class="angle"/>
<orientation name="quat" joint="quat" input="quat" kp="1" dampratio="1" class="quat"/>
</actuator>
</mujoco>
test/user/user_api_test.cc
+30
View File
@@ -483,6 +483,36 @@ TEST_F(MujocoTest, SetToDCMotorLuGre) {
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToOrientation) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
// kv variant, default (expmap) chart
double kv = 2.0;
const char* err = mjs_setToOrientation(actuator, 5.0, &kv, nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gaintype, mjGAIN_SO3);
EXPECT_EQ(actuator->biastype, mjBIAS_SO3);
EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
EXPECT_EQ(actuator->gainprm[0], 5.0);
EXPECT_EQ(actuator->biasprm[1], -5.0);
EXPECT_EQ(actuator->biasprm[2], -2.0);
EXPECT_EQ(actuator->ctrlspec, 0);
// dampratio variant, quat chart
double dampratio = 1.0;
err = mjs_setToOrientation(actuator, 5.0, nullptr, &dampratio, mjCHART_QUAT);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->biasprm[2], 1.0);
EXPECT_EQ(actuator->ctrlspec, mjCHART_QUAT);
// kv and dampratio are mutually exclusive
err = mjs_setToOrientation(actuator, 5.0, &kv, &dampratio, 0);
EXPECT_STREQ(err, "kv and dampratio cannot both be defined");
mj_deleteSpec(spec);
}
static constexpr char xml_plugin_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
unity/Runtime/Bindings/MjBindings.cs
+18 -4
View File
@@ -265,6 +265,7 @@ public enum mjtTrn : int{
mjTRN_TENDON = 3,
mjTRN_SITE = 4,
mjTRN_BODY = 5,
mjTRN_SO3 = 6,
mjTRN_UNDEFINED = 1000,
}
public enum mjtDyn : int{
@@ -281,14 +282,20 @@ public enum mjtGain : int{
mjGAIN_AFFINE = 1,
mjGAIN_MUSCLE = 2,
mjGAIN_DCMOTOR = 3,
mjGAIN_USER = 4,
mjGAIN_SO3 = 4,
mjGAIN_USER = 5,
}
public enum mjtBias : int{
mjBIAS_NONE = 0,
mjBIAS_AFFINE = 1,
mjBIAS_MUSCLE = 2,
mjBIAS_DCMOTOR = 3,
mjBIAS_USER = 4,
mjBIAS_SO3 = 4,
mjBIAS_USER = 5,
}
public enum mjtCtrlChart : int{
mjCHART_EXPMAP = 1,
mjCHART_QUAT = 2,
}
public enum mjtObj : int{
mjOBJ_UNKNOWN = 0,
@@ -1497,6 +1504,7 @@ public unsafe struct mjModel_ {
public int* actuator_biastype;
public int* actuator_ctrladr;
public int* actuator_ctrlnum;
public int* actuator_ctrlspec;
public int* actuator_outadr;
public int* actuator_outnum;
public int* actuator_actadr;
@@ -1518,11 +1526,11 @@ public unsafe struct mjModel_ {
public int* actuator_group;
public double* actuator_user;
public int* actuator_plugin;
public byte* actuator_forcelimited;
public double* actuator_forcerange;
public byte* actuator_ctrllimited;
public double* actuator_ctrlrange;
public double* actuator_gear;
public byte* actuator_forcelimited;
public double* actuator_forcerange;
public double* actuator_acc0;
public double* actuator_length0;
public double* actuator_lengthrange;
@@ -6809,6 +6817,9 @@ public static unsafe extern mjData_* mj_copyData(mjData_* dest, mjModel_* m, mjD
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern mjData_* mjv_copyData(mjData_* dest, mjModel_* m, mjData_* src);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_resetCtrl(mjModel_* m, mjData_* d);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_resetData(mjModel_* m, mjData_* d);
@@ -7085,6 +7096,9 @@ public static unsafe extern int mj_name2id(mjModel_* m, int type, [MarshalAs(Unm
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern IntPtr mj_id2name(mjModel_* m, int type, int id);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern IntPtr mj_actuatorInputName(mjModel_* m, int id, int input);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_fullM(mjModel_* m, mjData_* d, double* dst);
wasm/codegen/generated/bindings.cc
+25
View File
@@ -1436,6 +1436,10 @@ void mj_RungeKutta_wrapper(const MjModel& m, MjData& d, int N) {
mj_RungeKutta(m.get(), d.get(), N);
}
std::string mj_actuatorInputName_wrapper(const MjModel& m, int id, int input) {
return std::string(mj_actuatorInputName(m.get(), id, input));
}
int mj_addContact_wrapper(const MjModel& m, MjData& d, const MjContact& con) {
return mj_addContact(m.get(), d.get(), con.get());
}
@@ -1935,6 +1939,10 @@ void mj_referenceConstraint_wrapper(const MjModel& m, MjData& d) {
mj_referenceConstraint(m.get(), d.get());
}
void mj_resetCtrl_wrapper(const MjModel& m, MjData& d) {
mj_resetCtrl(m.get(), d.get());
}
void mj_resetData_wrapper(const MjModel& m, MjData& d) {
mj_resetData(m.get(), d.get());
}
@@ -2909,6 +2917,12 @@ std::string mjs_setToMuscle_wrapper(MjsActuator& actuator, const val& timeconst,
return std::string(mjs_setToMuscle(actuator.get(), timeconst_.data(), tausmooth, range_.data(), force, scale, lmin, lmax, vmax, fpmax, fvmax));
}
std::string mjs_setToOrientation_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, int ctrlspec) {
UNPACK_VALUE(double, kv);
UNPACK_VALUE(double, dampratio);
return std::string(mjs_setToOrientation(actuator.get(), kp, kv_.data(), dampratio_.data(), ctrlspec));
}
std::string mjs_setToPosition_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, const val& timeconst, double inheritrange) {
UNPACK_VALUE(double, kv);
UNPACK_VALUE(double, dampratio);
@@ -3877,6 +3891,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.value("mjBIAS_AFFINE", mjBIAS_AFFINE)
.value("mjBIAS_MUSCLE", mjBIAS_MUSCLE)
.value("mjBIAS_DCMOTOR", mjBIAS_DCMOTOR)
.value("mjBIAS_SO3", mjBIAS_SO3)
.value("mjBIAS_USER", mjBIAS_USER);
enum_<mjtBuiltin>("mjtBuiltin")
.value("mjBUILTIN_NONE", mjBUILTIN_NONE)
@@ -3957,6 +3972,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.value("mjCNSTRSTATE_LINEARNEG", mjCNSTRSTATE_LINEARNEG)
.value("mjCNSTRSTATE_LINEARPOS", mjCNSTRSTATE_LINEARPOS)
.value("mjCNSTRSTATE_CONE", mjCNSTRSTATE_CONE);
enum_<mjtCtrlChart>("mjtCtrlChart")
.value("mjCHART_EXPMAP", mjCHART_EXPMAP)
.value("mjCHART_QUAT", mjCHART_QUAT);
enum_<mjtDataType>("mjtDataType")
.value("mjDATATYPE_REAL", mjDATATYPE_REAL)
.value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE)
@@ -4057,6 +4075,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.value("mjGAIN_AFFINE", mjGAIN_AFFINE)
.value("mjGAIN_MUSCLE", mjGAIN_MUSCLE)
.value("mjGAIN_DCMOTOR", mjGAIN_DCMOTOR)
.value("mjGAIN_SO3", mjGAIN_SO3)
.value("mjGAIN_USER", mjGAIN_USER);
enum_<mjtGeom>("mjtGeom")
.value("mjGEOM_PLANE", mjGEOM_PLANE)
@@ -4419,6 +4438,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.value("mjTRN_TENDON", mjTRN_TENDON)
.value("mjTRN_SITE", mjTRN_SITE)
.value("mjTRN_BODY", mjTRN_BODY)
.value("mjTRN_SO3", mjTRN_SO3)
.value("mjTRN_UNDEFINED", mjTRN_UNDEFINED);
enum_<mjtVisFlag>("mjtVisFlag")
.value("mjVIS_CONVEXHULL", mjVIS_CONVEXHULL)
@@ -4806,6 +4826,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("actuator_ctrllimited", &MjModel::actuator_ctrllimited)
.property("actuator_ctrlnum", &MjModel::actuator_ctrlnum)
.property("actuator_ctrlrange", &MjModel::actuator_ctrlrange)
.property("actuator_ctrlspec", &MjModel::actuator_ctrlspec)
.property("actuator_damping", &MjModel::actuator_damping)
.property("actuator_dampingpoly", &MjModel::actuator_dampingpoly)
.property("actuator_delay", &MjModel::actuator_delay)
@@ -5582,6 +5603,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference())
.property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference())
.property("ctrlrange", &MjsActuator::ctrlrange)
.property("ctrlspec", &MjsActuator::ctrlspec, &MjsActuator::set_ctrlspec, reference())
.property("damping", &MjsActuator::damping)
.property("delay", &MjsActuator::delay, &MjsActuator::set_delay, reference())
.property("dynprm", &MjsActuator::dynprm)
@@ -6241,6 +6263,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
function("mj_Euler", &mj_Euler_wrapper);
function("mj_RungeKutta", &mj_RungeKutta_wrapper);
function("mj_actuatorInputName", &mj_actuatorInputName_wrapper);
function("mj_addContact", &mj_addContact_wrapper);
function("mj_addM", &mj_addM_wrapper);
function("mj_angmomMat", &mj_angmomMat_wrapper);
@@ -6331,6 +6354,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
function("mj_readCtrl", &mj_readCtrl_wrapper);
function("mj_referenceConstraint", &mj_referenceConstraint_wrapper);
function("mj_resetCallbacks", &mj_resetCallbacks);
function("mj_resetCtrl", &mj_resetCtrl_wrapper);
function("mj_resetData", &mj_resetData_wrapper);
function("mj_resetDataDebug", &mj_resetDataDebug_wrapper);
function("mj_resetDataKeyframe", &mj_resetDataKeyframe_wrapper);
@@ -6482,6 +6506,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
function("mjs_setToIntVelocity", &mjs_setToIntVelocity_wrapper);
function("mjs_setToMotor", &mjs_setToMotor_wrapper);
function("mjs_setToMuscle", &mjs_setToMuscle_wrapper);
function("mjs_setToOrientation", &mjs_setToOrientation_wrapper);
function("mjs_setToPosition", &mjs_setToPosition_wrapper);
function("mjs_setToVelocity", &mjs_setToVelocity_wrapper);
function("mjs_wrapGeom", &mjs_wrapGeom_wrapper);
wasm/codegen/generated/bindings.h
+15 -6
View File
@@ -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;
}