Add SO3 transmission and native orientation actuator.

https://youtu.be/17XpwnqyCXs

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

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