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