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