chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 6 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/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);