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

Add SO3 transmission and native orientation actuator.

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

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

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

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

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

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

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

PiperOrigin-RevId: 951607063
Change-Id: If235dba8e2f2ca72672e7c62531a27e967c6a373
This commit is contained in:
Yuval Tassa
2026-07-21 11:35:28 -07:00
committed by Copybara-Service
parent a8545ac7cc
commit 072e963fa0
49 changed files with 1772 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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" ||