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

Preparation for MIMO actuators: split actuator counts: nu (inputs), nactuator (objects), nout (outputs).

Browse Source 
An actuator now owns a block of consecutive controls
(actuator_ctrladr/ctrlnum, width defined by the actuator type) and a block
of consecutive force outputs (actuator_outadr/outnum, width defined by the
transmission type). Force outputs are the scalars of actuation space: one
force, length, velocity and moment row each. nout = dim(actuator_force) is
derived from transmission types; all current types have width 1, so all
three counts coincide for every existing model and behavior is bit-exact.

Array re-keying: ctrlrange/ctrllimited by nu; forcerange/forcelimited/gear/
acc0/length0/lengthrange and the moment row structure by nout; everything
else per actuator. The mjModel actuator block is re-sorted by size key.

Layout-breaking, not behavior-breaking: saved .mjb files are invalidated
(size list changed) and recompilation is required.

PiperOrigin-RevId: 948351772
Change-Id: Icbc196ffa083cb1eaa6f1a3710869c89d8f62540
This commit is contained in:
Yuval Tassa
2026-07-15 08:29:38 -07:00
committed by Copybara-Service
parent 06f12a9372
commit d507e92198
27 changed files with 750 additions and 538 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+28 -26
View File
@@ -1263,10 +1263,10 @@ mjtNum mj_tendonDot(const mjModel* m, mjData* d, int id, const mjtNum* vec) {
// compute actuator/transmission lengths and moments
void mj_transmission(const mjModel* m, mjData* d) {
int nv = m->nv, nu = m->nu;
int nv = m->nv, nactuator = m->nactuator;
// nothing to do
if (!nu) {
if (!nactuator) {
return;
}
@@ -1295,19 +1295,21 @@ void mj_transmission(const mjModel* m, mjData* d) {
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < nv;
// compute lengths and moments
for (int i=0; i < nu; i++) {
rowadr[i] = i == 0 ? 0 : rowadr[i-1] + rownnz[i-1];
int nnz, adr = rowadr[i];
for (int i=0; i < nactuator; i++) {
// address of the actuator's output block (single row for all current types)
int out = m->actuator_outadr[i];
rowadr[out] = out == 0 ? 0 : rowadr[out-1] + rownnz[out-1];
int nnz, adr = rowadr[out];
// skip sleeping actuator
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
rownnz[i] = 0;
rownnz[out] = 0;
continue;
}
// extract info
int id = m->actuator_trnid[2*i];
mjtNum* gear = m->actuator_gear+6*i;
mjtNum* gear = m->actuator_gear+6*out;
// process according to transmission type
switch ((mjtTrn) m->actuator_trntype[i]) {
@@ -1316,10 +1318,10 @@ void mj_transmission(const mjModel* m, mjData* d) {
// slide and hinge joint: scalar gear
if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) {
// sparsity
rownnz[i] = 1;
rownnz[out] = 1;
colind[adr] = m->jnt_dofadr[id];
length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0];
length[out] = d->qpos[m->jnt_qposadr[id]]*gear[0];
moment[adr] = gear[0];
}
@@ -1341,7 +1343,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// length: axis*gearAxis
length[i] = mju_dot3(axis, gearAxis);
length[out] = mju_dot3(axis, gearAxis);
// dof start address
int jnt_dofadr = m->jnt_dofadr[id];
@@ -1350,7 +1352,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
for (int j = 0; j < 3; j++) {
colind[adr+j] = jnt_dofadr + j;
}
rownnz[i] = 3;
rownnz[out] = 3;
// moment: gearAxis
mji_copy3(moment+adr, gearAxis);
@@ -1359,7 +1361,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// free joint: 6D wrench gear
else {
// cannot compute meaningful length, set to 0
length[i] = 0;
length[out] = 0;
// gearAxis: rotate to world frame if necessary
mjtNum gearAxis[3];
@@ -1380,7 +1382,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
for (int j = 0; j < 6; j++) {
colind[adr+j] = jnt_dofadr + j;
}
rownnz[i] = 6;
rownnz[out] = 6;
// moment: gear(tran), gearAxis
mji_copy3(moment+adr, gear);
@@ -1392,7 +1394,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
{
// get data
int idslider = m->actuator_trnid[2*i+1];
mjtNum rod = m->actuator_cranklength[i];
mjtNum rod = m->actuator_cranklength[out];
mjtNum axis[3] = {d->site_xmat[9 * idslider + 2],
d->site_xmat[9 * idslider + 5],
d->site_xmat[9 * idslider + 8]};
@@ -1407,10 +1409,10 @@ void mj_transmission(const mjModel* m, mjData* d) {
if (det <= 0) {
ok = 0;
sdet = 0;
length[i] = av;
length[out] = av;
} else {
sdet = mju_sqrt(det);
length[i] = av - sdet;
length[out] = av - sdet;
}
// compute derivatives of length w.r.t. vec and axis
@@ -1445,7 +1447,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// scale by gear ratio
length[i] *= gear[0];
length[out] *= gear[0];
// sparsity (compress)
nnz = 0;
@@ -1456,18 +1458,18 @@ void mj_transmission(const mjModel* m, mjData* d) {
nnz++;
}
}
rownnz[i] = nnz;
rownnz[out] = nnz;
}
break;
case mjTRN_TENDON: // tendon
length[i] = d->ten_length[id]*gear[0];
length[out] = d->ten_length[id]*gear[0];
// moment
{
int ten_J_rownnz = m->ten_J_rownnz[id];
int ten_J_rowadr = m->ten_J_rowadr[id];
rownnz[i] = ten_J_rownnz;
rownnz[out] = ten_J_rownnz;
mju_copyInt(colind + adr, m->ten_J_colind + ten_J_rowadr, ten_J_rownnz);
mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz);
@@ -1479,7 +1481,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mj_jacSite(m, d, jac, jacS, id);
// clear length
length[i] = 0;
length[out] = 0;
if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum);
@@ -1540,7 +1542,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_mulMatTVec3(vec, d->site_xmat+9*refid, vec);
// length: dot product with gear
length[i] += mju_dot3(vec, gear);
length[out] += mju_dot3(vec, gear);
// jacref: global Jacobian of reference site
mj_jacSite(m, d, jacref, NULL, refid);
@@ -1579,7 +1581,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mji_subQuat(vec, quat, refquat);
// add length: dot product with gear
length[i] += mju_dot3(vec, gear+3);
length[out] += mju_dot3(vec, gear+3);
// jacref: global rotational Jacobian of reference site
mj_jacSite(m, d, NULL, jacref, refid);
@@ -1616,13 +1618,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
nnz++;
}
}
rownnz[i] = nnz;
rownnz[out] = nnz;
break;
case mjTRN_BODY: // body (adhesive contacts)
// cannot compute meaningful length, set to 0
length[i] = 0;
length[out] = 0;
// clear moment
if (!moment_row) moment_row = mjSTACKALLOC(d, nv, mjtNum);
@@ -1730,7 +1732,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
nnz++;
}
}
rownnz[i] = nnz;
rownnz[out] = nnz;
break;
src/engine/engine_core_util.c
+6 -6
View File
@@ -1102,7 +1102,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN
// single actuator contributes damping
if (actuatorid >= 0) {
mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid];
mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]];
damping = m->actuator_damping[actuatorid] * gear2;
for (int k = 0; k < mjNPOLY; k++) {
poly[k] += m->actuator_dampingpoly[mjNPOLY*actuatorid+k] * gear2;
@@ -1111,7 +1111,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN
// actuatorid < -1: scan all actuators for contributions
else {
for (int k = 0; k < m->nu; k++) {
for (int k = 0; k < m->nactuator; k++) {
// skip actuators that don't actuate the given joint/tendon
if (m->actuator_trnid[2*k] != id) {
continue;
@@ -1126,7 +1126,7 @@ mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjN
}
// accumulate damping contribution
mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k];
mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]];
damping += m->actuator_damping[k] * gear2;
for (int j = 0; j < mjNPOLY; j++) {
poly[j] += m->actuator_dampingpoly[mjNPOLY*k+j] * gear2;
@@ -1157,13 +1157,13 @@ mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) {
// single actuator contributes armature
if (actuatorid >= 0) {
mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid];
mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]];
armature = m->actuator_armature[actuatorid] * gear2;
}
// actuatorid < -1: scan all actuators for contributions
else {
for (int k = 0; k < m->nu; k++) {
for (int k = 0; k < m->nactuator; k++) {
// skip actuators that don't actuate the given joint/tendon
if (m->actuator_trnid[2*k] != id) {
continue;
@@ -1178,7 +1178,7 @@ mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) {
}
// accumulate armature contribution
mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k];
mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]];
armature += m->actuator_armature[k] * gear2;
}
}
src/engine/engine_derivative.c
+13 -10
View File
@@ -1211,7 +1211,7 @@ void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* ve
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nu = m->nu;
int nactuator = m->nactuator;
int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
// disabled: nothing to add
@@ -1220,7 +1220,10 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
}
// process actuators
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
int uadr = m->actuator_ctrladr[i];
int oadr = m->actuator_outadr[i];
// skip if disabled
if (mj_actuatorDisabled(m, i)) {
continue;
@@ -1233,8 +1236,8 @@ 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[i];
mjtNum* range = m->actuator_forcerange + 2*i;
mjtNum force = d->actuator_force[oadr];
mjtNum* range = m->actuator_forcerange + 2*oadr;
if (force <= range[0] || force >= range[1]) {
continue;
}
@@ -1267,10 +1270,10 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
// muscle gain
else if (m->actuator_gaintype[i] == mjGAIN_MUSCLE) {
gain_vel = mjd_muscleGain_vel(d->actuator_length[i],
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
gain_vel = mjd_muscleGain_vel(d->actuator_length[oadr],
d->actuator_velocity[oadr],
m->actuator_lengthrange+2*oadr,
m->actuator_acc0[oadr],
m->actuator_gainprm + mjNGAIN*i);
}
@@ -1311,7 +1314,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
// force = gain .* [ctrl/act]
if (gain_vel != 0) {
if (m->actuator_dyntype[i] == mjDYN_NONE) {
bias_vel += gain_vel * d->ctrl[i];
bias_vel += gain_vel * d->ctrl[uadr];
} else {
int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
mjtNum act = d->act[act_adr];
@@ -1327,7 +1330,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
// add
if (bias_vel != 0) {
addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, i,
addJTBJSparse(m, d, d->actuator_moment, &bias_vel, 1, oadr,
d->moment_rownnz, d->moment_rowadr, d->moment_colind);
}
}
src/engine/engine_forward.c
+66 -49
View File
@@ -197,10 +197,10 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// actuator velocity: always sparse
if (!mjDISABLED(mjDSBL_ACTUATION)) {
mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu,
mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nout,
d->moment_rownnz, d->moment_rowadr, d->moment_colind, NULL);
} else {
mju_zero(d->actuator_velocity, m->nu);
mju_zero(d->actuator_velocity, m->nout);
}
// com-based velocities, passive forces, constraint references
@@ -264,17 +264,17 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, i
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu, ntendon = m->ntendon;
int nv = m->nv, nu = m->nu, nactuator = m->nactuator, nout = m->nout, ntendon = m->ntendon;
mjtNum gain, bias, tau;
mjtNum *force = d->actuator_force;
// clear actuator_force
mju_zero(force, nu);
mju_zero(force, nout);
int sleep_filter = mjENABLED(mjENBL_SLEEP);
// disabled or no actuation: return
if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
if (nactuator == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
mju_zero(d->qfrc_actuator, nv);
return;
}
@@ -287,9 +287,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
// read from ctrl or history buffer for delayed actuators
for (int i = 0; i < nu; i++) {
int interp = m->actuator_history[2*i+1];
ctrl[i] = m->actuator_delay[i] ? mj_readCtrl(m, d, i, d->time, interp) : d->ctrl[i];
for (int i = 0; i < nactuator; i++) {
int adr = m->actuator_ctrladr[i];
if (m->actuator_delay[i]) {
// delayed: read from history buffer (scalar input)
int interp = m->actuator_history[2*i+1];
ctrl[adr] = mj_readCtrl(m, d, i, d->time, interp);
} else {
mju_copy(ctrl + adr, d->ctrl + adr, m->actuator_ctrlnum[i]);
}
}
// clamp local copy
@@ -307,7 +313,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
}
@@ -317,6 +323,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
continue;
}
// addresses of the actuator's input and output blocks
int uadr = m->actuator_ctrladr[i];
int oadr = m->actuator_outadr[i];
// zero act_dot for actuator plugins
int actnum = m->actuator_actnum[i];
if (actnum) {
@@ -334,17 +344,17 @@ 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[i];
d->act_dot[act_last] = ctrl[uadr];
break;
case mjDYN_FILTER: // linear filter: dynprm = tau
case mjDYN_FILTEREXACT:
tau = mju_max(mjMINVAL, dynprm[0]);
d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau;
d->act_dot[act_last] = (ctrl[uadr] - d->act[act_last]) / tau;
break;
case mjDYN_MUSCLE: // muscle model: dynprm = (tau_act, tau_deact)
d->act_dot[act_last] = mju_muscleDynamics(ctrl[i], d->act[act_last], dynprm);
d->act_dot[act_last] = mju_muscleDynamics(ctrl[uadr], d->act[act_last], dynprm);
break;
case mjDYN_DCMOTOR: { // DC motor: up to 5 optional states
@@ -356,7 +366,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
int adr = act_first;
mjtNum velocity = d->actuator_velocity[i];
mjtNum velocity = d->actuator_velocity[oadr];
mjtNum R = gainprm[0]; // resistance
mjtNum K = gainprm[1]; // motor constant
mjtNum ki = gainprm[5]; // integral gain
@@ -369,9 +379,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
if (slew_s > 0) {
mjtNum u_prev = d->act[adr];
mjtNum slew = slew_s * m->opt.timestep;
mjtNum u_eff = mju_clip(ctrl[i], u_prev - slew, u_prev + slew);
mjtNum u_eff = mju_clip(ctrl[uadr], u_prev - slew, u_prev + slew);
d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep;
ctrl[i] = u_eff;
ctrl[uadr] = u_eff;
adr++;
}
@@ -381,11 +391,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
x_I = d->act[adr];
int input_mode = (int)gainprm[8];
mjtNum Imax = dynprm[8]; // integral clamp
mjtNum act_dot = ctrl[i]; // default raw accumulator for voltage and velocity modes
mjtNum act_dot = ctrl[uadr]; // default raw accumulator for voltage and velocity modes
// position mode
if (input_mode == 1) {
act_dot = ctrl[i] - d->actuator_length[i];
act_dot = ctrl[uadr] - d->actuator_length[oadr];
}
// clamp act_dot based on integral state
@@ -401,7 +411,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// compute physical voltage to feed into current and temperature equations
mjtNum V = dcmotorVoltage(ctrl[i], d->actuator_length[i], velocity, x_I, gainprm);
mjtNum V = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], velocity, x_I, gainprm);
// temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient
mjtNum RT = dynprm[2]; // thermal resistance
@@ -476,7 +486,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// force = gain .* [ctrl/act] + bias
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
// skip if sleeping
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
@@ -492,6 +502,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
continue;
}
// addresses of the actuator's input and output blocks
int uadr = m->actuator_ctrladr[i];
int oadr = m->actuator_outadr[i];
// 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]];
@@ -510,14 +524,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
break;
case mjGAIN_AFFINE: // affine: prm = [const, kp, kv]
gain = gainprm[0] + gainprm[1]*d->actuator_length[i] + gainprm[2]*d->actuator_velocity[i];
gain = gainprm[0] + gainprm[1]*d->actuator_length[oadr] +
gainprm[2]*d->actuator_velocity[oadr];
break;
case mjGAIN_MUSCLE: // muscle gain
gain = mju_muscleGain(d->actuator_length[i],
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
gain = mju_muscleGain(d->actuator_length[oadr],
d->actuator_velocity[oadr],
m->actuator_lengthrange+2*oadr,
m->actuator_acc0[oadr],
gainprm);
break;
@@ -546,11 +561,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// stateless: gain = K/R, force = K/R * ctrl (condition below)
gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
// controller: compute voltage, override ctrl[i] for force computation
// controller: compute voltage, override ctrl[uadr] for force computation
if ((int)gainprm[8] > 0) {
mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0;
ctrl[i] = dcmotorVoltage(ctrl[i], d->actuator_length[i],
d->actuator_velocity[i], x_I, gainprm);
ctrl[uadr] = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr],
d->actuator_velocity[oadr], x_I, gainprm);
}
break;
}
@@ -568,7 +583,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// DC motor without current state: use ctrl even if other activations exist
int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);
if (actnum == 0 || dcmotor_no_current) {
force[i] = gain * ctrl[i];
force[oadr] = gain * ctrl[uadr];
} else {
// use last activation variable associated with actuator i
int act_adr = m->actuator_actadr[i] + actnum - 1;
@@ -579,7 +594,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
} else {
act = d->act[act_adr];
}
force[i] = gain * act;
force[oadr] = gain * act;
}
// extract bias info
@@ -593,13 +608,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
break;
case mjBIAS_AFFINE: // affine: biasprm = [const, kp, kv]
bias = biasprm[0] + biasprm[1]*d->actuator_length[i] + biasprm[2]*d->actuator_velocity[i];
bias = biasprm[0] + biasprm[1]*d->actuator_length[oadr] +
biasprm[2]*d->actuator_velocity[oadr];
break;
case mjBIAS_MUSCLE: // muscle passive force
bias = mju_muscleBias(d->actuator_length[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
bias = mju_muscleBias(d->actuator_length[oadr],
m->actuator_lengthrange+2*oadr,
m->actuator_acc0[oadr],
biasprm);
break;
@@ -610,7 +626,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mjtNum te = m->actuator_dynprm[mjNDYN*i]; // electrical time constant
if (te <= 0) {
mjtNum K = gainprm[1]; // motor constant
bias -= gain * K * d->actuator_velocity[i];
bias -= gain * K * d->actuator_velocity[oadr];
}
break;
}
@@ -624,7 +640,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// add bias
force[i] += bias;
force[oadr] += bias;
}
// handle actuator plugins
@@ -650,17 +666,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// compute total force for each tendon
mjtNum* tendon_total_force = mjSTACKALLOC(d, ntendon, mjtNum);
mju_zero(tendon_total_force, ntendon);
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
if (m->actuator_trntype[i] == mjTRN_TENDON) {
int tendon_id = m->actuator_trnid[2*i];
if (m->tendon_actfrclimited[tendon_id]) {
tendon_total_force[tendon_id] += force[i];
tendon_total_force[tendon_id] += force[m->actuator_outadr[i]];
}
}
}
// scale tendon actuator forces if limited and outside range
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
if (m->actuator_trntype[i] != mjTRN_TENDON) {
continue;
}
@@ -669,19 +685,19 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
if (m->tendon_actfrclimited[tendon_id] && tendon_force) {
const mjtNum* range = m->tendon_actfrcrange + 2 * tendon_id;
if (tendon_force < range[0]) {
force[i] *= range[0] / tendon_force;
force[m->actuator_outadr[i]] *= range[0] / tendon_force;
} else if (tendon_force > range[1]) {
force[i] *= range[1] / tendon_force;
force[m->actuator_outadr[i]] *= range[1] / tendon_force;
}
}
}
}
// clamp actuator_force
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nout, NULL);
// add DC motor mechanical forces (not subject to current limits)
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) {
continue;
}
@@ -694,13 +710,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
int oadr = m->actuator_outadr[i];
// cogging torque
mjtNum A = biasprm[0];
if (A != 0) {
mjtNum Np = biasprm[1];
mjtNum phi = biasprm[2];
force[i] += A * mju_sin(Np*d->actuator_length[i] + phi);
force[oadr] += A * mju_sin(Np*d->actuator_length[oadr] + phi);
}
// LuGre friction
@@ -711,12 +728,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
int adr = m->actuator_actadr[i] + slots.bristle;
mjtNum z = d->act[adr];
mjtNum z_dot = d->act_dot[adr];
force[i] -= sigma0 * z + sigma1 * z_dot;
force[oadr] -= sigma0 * z + sigma1 * z_dot;
}
}
// qfrc_actuator = moment' * force
mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv,
mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nout, nv,
d->moment_rownnz, d->moment_rowadr, d->moment_colind);
// actuator-level gravity compensation
@@ -980,18 +997,18 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
// qvel: optional velocity used for position integration; if NULL, use d->qvel
static void mj_advance(const mjModel* m, mjData* d,
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
int nu = m->nu, nsensor = m->nsensor;
int nactuator = m->nactuator, nsensor = m->nsensor;
// advance history buffers
if (m->nhistory > 0) {
// advance ctrl history buffers
for (int i = 0; i < nu; i++) {
for (int i = 0; i < nactuator; i++) {
int nsample = m->actuator_history[2*i];
if (nsample == 0) continue;
// get history buffer pointer and insert ctrl at current time
mjtNum* buf = d->history + m->actuator_historyadr[i];
*mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i];
*mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[m->actuator_ctrladr[i]];
}
// advance sensor history buffers
@@ -1033,7 +1050,7 @@ static void mj_advance(const mjModel* m, mjData* d,
// advance activations
if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
for (int i=0; i < nu; i++) {
for (int i=0; i < nactuator; i++) {
int actadr = m->actuator_actadr[i];
int actadr_end = actadr + m->actuator_actnum[i];
for (int j=actadr; j < actadr_end; j++) {
src/engine/engine_io.c
+16 -10
View File
@@ -225,7 +225,8 @@ static void freeModelBuffers(mjModel* m) {
// allocate and initialize mjModel structure
void mj_makeModel(mjModel** dest,
mjtSize nq, mjtSize nv, mjtSize nu, mjtSize na, mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic,
mjtSize nq, mjtSize nv, mjtSize nu, mjtSize nactuator, mjtSize nout, mjtSize na,
mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic,
mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB,
mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight,
mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem,
@@ -297,6 +298,8 @@ void mj_makeModel(mjModel** dest,
m->nq = nq;
m->nv = nv;
m->nu = nu;
m->nactuator = nactuator;
m->nout = nout;
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
@@ -374,7 +377,7 @@ void mj_makeModel(mjModel** dest,
m->nuser_sensor = nuser_sensor;
m->nnames = nnames;
long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin +
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nactuator + nsensor +
nnumeric + ntext + ntuple + nkey + nplugin;
if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
if (allocate) mju_free(m);
@@ -428,7 +431,8 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
// allocate new model if needed
if (!dest) {
mj_makeModel(
&dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->nbvhstatic,
&dest, src->nq, src->nv, src->nu, src->nactuator, src->nout, src->na,
src->nbody, src->nbvh, src->nbvhstatic,
src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert,
src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness,
@@ -611,7 +615,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62],
sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69],
sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76],
sizes[77], sizes[78], sizes[79]);
sizes[77], sizes[78], sizes[79], sizes[80], sizes[81]);
// mj_makeModel may fail if the input buffer has invalid sizes
if (!m) {
@@ -1384,7 +1388,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zero(d->mocap_quat, 4*m->nmocap);
// initialize ctrl history buffers: timestamps at [-n*dt, ..., -dt]
for (int i = 0; i < m->nu; i++) {
for (int i = 0; i < m->nactuator; i++) {
int n = m->actuator_history[2*i];
if (n > 0) {
mjtNum* buf = d->history + m->actuator_historyadr[i];
@@ -1785,7 +1789,7 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
case mjOBJ_TENDON:
return m->ntendon;
case mjOBJ_ACTUATOR:
return m->nu;
return m->nactuator;
case mjOBJ_SENSOR:
return m->nsensor;
case mjOBJ_NUMERIC:
@@ -1871,8 +1875,10 @@ const char* mj_validateReferences(const mjModel* m) {
X(skin_bonevertid, nskinbonevert, nskinvert , 0 ) \
X(pair_geom1, npair, ngeom , 0 ) \
X(pair_geom2, npair, ngeom , 0 ) \
X(actuator_plugin, nu, nplugin , 0 ) \
X(actuator_actadr, nu, na , m->actuator_actnum ) \
X(actuator_plugin, nactuator, nplugin , 0 ) \
X(actuator_actadr, nactuator, na , m->actuator_actnum ) \
X(actuator_ctrladr, nactuator, nu , m->actuator_ctrlnum ) \
X(actuator_outadr, nactuator, nout , m->actuator_outnum ) \
X(sensor_plugin, nsensor, nplugin , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \
X(plugin_attradr, nplugin, npluginattr , 0 ) \
@@ -1897,7 +1903,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(name_excludeadr, nexclude, nnames , 0 ) \
X(name_eqadr, neq, nnames , 0 ) \
X(name_tendonadr, ntendon, nnames , 0 ) \
X(name_actuatoradr, nu, nnames , 0 ) \
X(name_actuatoradr, nactuator, nnames , 0 ) \
X(name_sensoradr, nsensor, nnames , 0 ) \
X(name_numericadr, nnumeric, nnames , 0 ) \
X(name_textadr, ntext, nnames , 0 ) \
@@ -2085,7 +2091,7 @@ const char* mj_validateReferences(const mjModel* m) {
break;
}
}
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
int actuator_trntype = m->actuator_trntype[i];
int id = m->actuator_trnid[2*i];
int idslider = m->actuator_trnid[2*i+1];
src/engine/engine_io.h
+2 -1
View File
@@ -48,7 +48,8 @@ void mj_defaultStatistic(mjStatistic* stat);
// allocate mjModel
void mj_makeModel(mjModel** dest,
mjtSize nq, mjtSize nv, mjtSize nu, mjtSize na, mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic,
mjtSize nq, mjtSize nv, mjtSize nu, mjtSize nactuator, mjtSize nout, mjtSize na,
mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic,
mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB,
mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight,
mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem,
src/engine/engine_name.c
+2 -2
View File
@@ -160,10 +160,10 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
mjFALLTHROUGH;
case mjOBJ_ACTUATOR:
*mapadr -= mjLOAD_MULTIPLE*m->nu;
*mapadr -= mjLOAD_MULTIPLE*m->nactuator;
if (num < 0) {
*padr = m->name_actuatoradr;
num = m->nu;
num = m->nactuator;
}
mjFALLTHROUGH;
src/engine/engine_print.c
+32 -10
View File
@@ -503,6 +503,7 @@ static bool validateFloatFormat(const char* float_format) {
// print mjModel to text file, specifying format. float_format must be a
// valid printf-style format string for a single float value
// NOLINTBEGIN(readability/fn_size)
void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format) {
// get file
FILE* fp;
@@ -938,15 +939,35 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->ntendon) fprintf(fp, "\n");
// actuators
object_class = &m->nu;
for (int i=0; i < m->nu; i++) {
// actuators: per-actuator fields, then per-input (nu) and per-output (nout) blocks
for (int i=0; i < m->nactuator; i++) {
fprintf(fp, "\nACTUATOR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_actuatoradr[i]);
object_class = &m->nactuator;
MJMODEL_POINTERS_ACTUATOR
{
int actuator = i;
int ctrladr = m->actuator_ctrladr[actuator];
int ctrlnum = m->actuator_ctrlnum[actuator];
int outadr = m->actuator_outadr[actuator];
int outnum = m->actuator_outnum[actuator];
object_class = &m->nu;
i = ctrladr;
while (i < ctrladr + ctrlnum) {
MJMODEL_POINTERS_ACTUATOR
i++;
}
object_class = &m->nout;
i = outadr;
while (i < outadr + outnum) {
MJMODEL_POINTERS_ACTUATOR
i++;
}
i = actuator;
}
}
if (m->nu) fprintf(fp, "\n");
if (m->nactuator) fprintf(fp, "\n");
// sensors
object_class = &m->nsensor;
@@ -1170,6 +1191,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
fclose(fp);
}
}
// NOLINTEND(readability/fn_size)
// print mjModel to text file
void mj_printModel(const mjModel* m, const char* filename) {
@@ -1335,7 +1357,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
fprintf(fp, "DELAY\n");
// actuator history buffers
for (int i = 0; i < m->nu; i++) {
for (int i = 0; i < m->nactuator; i++) {
int adr = m->actuator_historyadr[i];
if (adr >= 0) {
char name[100];
@@ -1423,10 +1445,10 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
fprintf(fp, "\n");
}
printArray2d("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format);
mj_printSparsity("actuator_moment", m->nu, m->nv,
printArray2d("ACTUATOR_LENGTH", m->nout, 1, d->actuator_length, fp, float_format);
mj_printSparsity("actuator_moment", m->nout, m->nv,
d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp);
printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz,
printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nout, d->moment_rownnz,
d->moment_rowadr, d->moment_colind, fp, float_format);
printArray2d("CRB", m->nbody, 10, d->crb, fp, float_format);
printSparse("M", d->M, m->nv, m->M_rownnz,
@@ -1568,7 +1590,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format);
printArray2d("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format);
printArray2d("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, fp, float_format);
printArray2d("ACTUATOR_VELOCITY", m->nout, 1, d->actuator_velocity, fp, float_format);
printArray2d("CVEL", m->nbody, 6, d->cvel, fp, float_format);
printArray2d("CDOF_DOT", m->nv, 6, d->cdof_dot, fp, float_format);
@@ -1587,7 +1609,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("SUBTREE_LINVEL", m->nbody, 3, d->subtree_linvel, fp, float_format);
printArray2d("SUBTREE_ANGMOM", m->nbody, 3, d->subtree_angmom, fp, float_format);
printArray2d("ACTUATOR_FORCE", m->nu, 1, d->actuator_force, fp, float_format);
printArray2d("ACTUATOR_FORCE", m->nout, 1, d->actuator_force, fp, float_format);
printArray2d("QFRC_ACTUATOR", m->nv, 1, d->qfrc_actuator, fp, float_format);
printArray2d("QFRC_SMOOTH", m->nv, 1, d->qfrc_smooth, fp, float_format);
src/engine/engine_sensor.c
+6 -6
View File
@@ -650,7 +650,7 @@ static void mj_computeSensorPos(const mjModel* m, mjData* d, int i, mjtNum* sens
break;
case mjSENS_ACTUATORPOS: // actuator position
sensordata[0] = d->actuator_length[objid];
sensordata[0] = d->actuator_length[m->actuator_outadr[objid]];
break;
case mjSENS_BALLQUAT: // ball joint quaternion
@@ -879,7 +879,7 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens
break;
case mjSENS_ACTUATORVEL: // actuator velocity
sensordata[0] = d->actuator_velocity[objid];
sensordata[0] = d->actuator_velocity[m->actuator_outadr[objid]];
break;
case mjSENS_BALLANGVEL: // ball joint angular velocity
@@ -956,7 +956,7 @@ static void mj_computeSensorVel(const mjModel* m, mjData* d, int i, mjtNum* sens
// compute acceleration-stage sensor value, write to data buffer
static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sensordata) {
int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
int ne = d->ne, nf = d->nf, nefc = d->nefc, nactuator = m->nactuator;
mjtSensor type = (mjtSensor)m->sensor_type[i];
int objtype = m->sensor_objtype[i];
int objid = m->sensor_objid[i];
@@ -1303,7 +1303,7 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens
break;
case mjSENS_ACTUATORFRC: // actuator force
sensordata[0] = d->actuator_force[objid];
sensordata[0] = d->actuator_force[m->actuator_outadr[objid]];
break;
case mjSENS_JOINTACTFRC: // actuator force at joint
@@ -1312,9 +1312,9 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens
case mjSENS_TENDONACTFRC: // actuator force at tendon
frc = 0.0;
for (int j=0; j < nu; j++) {
for (int j=0; j < nactuator; j++) {
if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) {
frc += d->actuator_force[j];
frc += d->actuator_force[m->actuator_outadr[j]];
}
}
sensordata[0] = frc;
src/engine/engine_setconst.c
+27 -25
View File
@@ -212,7 +212,7 @@ static void setFixed(mjModel* m, mjData* d) {
// set jnt_actuatorid and tendon_actuatorid
mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
mju_fillInt(m->tendon_actuatorid, -1, m->ntendon);
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
// skip actuator with no damping and no armature
if (m->actuator_damping[i] == 0 &&
mju_isZero(m->actuator_dampingpoly+mjNPOLY*i, mjNPOLY) &&
@@ -312,7 +312,7 @@ static void setFixed(mjModel* m, mjData* d) {
// ----- apply compiler AUTO tree sleep policy
// actuators: trees with any actuated joint, site, body, or tendon do not auto-sleep
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
int bodyid = -1;
int tid = m->actuator_trnid[2*i];
switch ((mjtTrn)m->actuator_trntype[i]) {
@@ -868,7 +868,7 @@ static void set0(mjModel* m, mjData* d) {
// copy fields
mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge);
mju_copy(m->tendon_length0, d->ten_length, m->ntendon);
mju_copy(m->actuator_length0, d->actuator_length, m->nu);
mju_copy(m->actuator_length0, d->actuator_length, m->nout);
// compute body_invweight0
m->body_invweight0[0] = m->body_invweight0[1] = 0.0;
@@ -1002,8 +1002,8 @@ static void set0(mjModel* m, mjData* d) {
m->tendon_invweight0[i] = mju_dot(tmp, tmp+nv, nv);
}
// compute actuator_acc0
for (int i=0; i < m->nu; i++) {
// compute actuator_acc0, one per force output (moment row)
for (int i=0; i < m->nout; i++) {
mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + i,
d->moment_rowadr + i, d->moment_colind);
mj_solveM(m, d, tmp, moment, 1);
@@ -1011,7 +1011,7 @@ static void set0(mjModel* m, mjData* d) {
}
} else {
mju_zero(m->tendon_invweight0, m->ntendon);
mju_zero(m->actuator_acc0, m->nu);
mju_zero(m->actuator_acc0, m->nout);
}
// compute missing eq_data for body constraints
@@ -1101,7 +1101,7 @@ static void set0(mjModel* m, mjData* d) {
}
// compute actuator damping from dampratio
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
// get bias, gain parameters
mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
@@ -1119,8 +1119,8 @@ static void set0(mjModel* m, mjData* d) {
// === interpret biasprm[2] > 0 as dampratio for position-like actuators
// "reflected" inertia (inversely scaled by transmission squared)
int rownnz = d->moment_rownnz[i];
int rowadr = d->moment_rowadr[i];
int rownnz = d->moment_rownnz[m->actuator_outadr[i]];
int rowadr = d->moment_rowadr[m->actuator_outadr[i]];
mjtNum* transmission = d->actuator_moment + rowadr;
mjtNum mass = 0;
for (int j=0; j < rownnz; j++) {
@@ -1363,6 +1363,7 @@ void mj_setConst(mjModel* m, mjData* d) {
static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
const mjLROpt* opt) {
int nv = m->nv;
int out = m->actuator_outadr[index];
// reduce velocity
mju_scl(d->qvel, d->qvel, mju_exp(-m->opt.timestep/mjMAX(0.01, opt->timeconst)), nv);
@@ -1373,8 +1374,8 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
// dense actuator_moment row
mj_markStack(d);
mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum);
mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + index,
d->moment_rowadr + index, d->moment_colind);
mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + out,
d->moment_rowadr + out, d->moment_colind);
// set force to generate desired acceleration
mj_solveM(m, d, d->qfrc_applied, moment, 1);
@@ -1393,7 +1394,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
mj_freeStack(d);
// return actuator length
return d->actuator_length[index];
return d->actuator_length[out];
}
@@ -1401,9 +1402,10 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
int mj_setLengthRange(mjModel* m, mjData* d, int index,
const mjLROpt* opt, char* error, int error_sz) {
// check index
if (index < 0 || index >= m->nu) {
if (index < 0 || index >= m->nactuator) {
mjERROR("invalid actuator index");
}
int out = m->actuator_outadr[index];
// skip depending on mode and type
int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE ||
@@ -1417,7 +1419,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// use existing length range if available
if (opt->useexisting && (m->actuator_lengthrange[2*index] < m->actuator_lengthrange[2*index+1])) {
if (opt->useexisting && (m->actuator_lengthrange[2*out] < m->actuator_lengthrange[2*out+1])) {
return 1;
}
@@ -1432,8 +1434,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// make sure joint is limited
if (m->jnt_limited[threadid]) {
// copy range
m->actuator_lengthrange[2*index] = m->jnt_range[2*threadid];
m->actuator_lengthrange[2*index+1] = m->jnt_range[2*threadid+1];
m->actuator_lengthrange[2*out] = m->jnt_range[2*threadid];
m->actuator_lengthrange[2*out+1] = m->jnt_range[2*threadid+1];
// skip optimization
return 1;
@@ -1445,8 +1447,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// make sure tendon is limited
if (m->tendon_limited[threadid]) {
// copy range
m->actuator_lengthrange[2*index] = m->tendon_range[2*threadid];
m->actuator_lengthrange[2*index+1] = m->tendon_range[2*threadid+1];
m->actuator_lengthrange[2*out] = m->tendon_range[2*threadid];
m->actuator_lengthrange[2*out+1] = m->tendon_range[2*threadid+1];
// skip optimization
return 1;
@@ -1491,12 +1493,12 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// check range
mjtNum dif = m->actuator_lengthrange[2*index+1] - m->actuator_lengthrange[2*index];
mjtNum dif = m->actuator_lengthrange[2*out+1] - m->actuator_lengthrange[2*out];
if (dif <= 0) {
snprintf(error, error_sz,
"Invalid lengthrange (%g, %g) in actuator %d",
m->actuator_lengthrange[2*index],
m->actuator_lengthrange[2*index+1], index);
m->actuator_lengthrange[2*out],
m->actuator_lengthrange[2*out+1], index);
return 0;
}
@@ -1506,8 +1508,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",
index, lmin[0], lmax[0],
m->actuator_lengthrange[2*index],
m->actuator_lengthrange[2*index+1]);
m->actuator_lengthrange[2*out],
m->actuator_lengthrange[2*out+1]);
return 0;
}
@@ -1517,8 +1519,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",
index, lmin[1], lmax[1],
m->actuator_lengthrange[2*index],
m->actuator_lengthrange[2*index+1]);
m->actuator_lengthrange[2*out],
m->actuator_lengthrange[2*out+1]);
return 0;
}
src/engine/engine_support.c
+3 -3
View File
@@ -885,7 +885,7 @@ void mju_camIntrinsics(const mjModel* m, int camid,
// read delayed ctrl value for actuator at given time
mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp) {
// validate actuator id
if (id < 0 || id >= m->nu) {
if (id < 0 || id >= m->nactuator) {
mjERROR("invalid actuator id %d", id);
return 0;
}
@@ -893,7 +893,7 @@ mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int i
// no delay: return current ctrl value
int nsample = m->actuator_history[2*id];
if (nsample == 0) {
return d->ctrl[id];
return d->ctrl[m->actuator_ctrladr[id]];
}
// resolve interpolation order: use model's interp if argument is -1
@@ -938,7 +938,7 @@ const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum ti
void mj_initCtrlHistory(const mjModel* m, mjData* d, int id,
const mjtNum* times, const mjtNum* values) {
// validate actuator id
if (id < 0 || id >= m->nu) {
if (id < 0 || id >= m->nactuator) {
mjERROR("invalid actuator id %d", id);
return;
}
src/engine/engine_vis_visualize.c
+7 -7
View File
@@ -1116,7 +1116,7 @@ int mjv_isCatenary(const mjModel* m, const mjData* d, int i, mjtNum* length) {
// no actuator
if (draw_catenary) {
for (int j=0; j < m->nu; j++) {
for (int j=0; j < m->nactuator; j++) {
if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == i) {
draw_catenary = 0;
break;
@@ -1271,7 +1271,7 @@ static void addSliderCrankGeoms(const mjModel* m, mjData* d, const mjvOption* vo
}
const float scl = m->stat.meansize;
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
if (m->actuator_trntype[i] == mjTRN_SLIDERCRANK) {
// get data
int j = m->actuator_trnid[2*i]; // crank
@@ -2077,7 +2077,7 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
const float scl = m->stat.meansize;
for (int i=0; i < m->nu; i++) {
for (int i=0; i < m->nactuator; i++) {
if (!vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) {
continue;
}
@@ -2088,9 +2088,9 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// determine extended range
mjtNum rng[3] = {-1, 0, +1};
mjtNum rmin = -1, rmax = 1, act = 0;
if (m->actuator_ctrllimited[i]) {
rmin = m->actuator_ctrlrange[2*i];
rmax = m->actuator_ctrlrange[2*i+1];
if (m->actuator_ctrllimited[m->actuator_ctrladr[i]]) {
rmin = m->actuator_ctrlrange[2*m->actuator_ctrladr[i]];
rmax = m->actuator_ctrlrange[2*m->actuator_ctrladr[i]+1];
} else if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_actlimited[i]) {
rmin = m->actuator_actrange[2*i];
rmax = m->actuator_actrange[2*i+1];
@@ -2121,7 +2121,7 @@ static void addActuatorGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) {
act = mju_clip(d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1], rng[0], rng[2]);
} else {
act = mju_clip(d->ctrl[i], rng[0], rng[2]);
act = mju_clip(d->ctrl[m->actuator_ctrladr[i]], rng[0], rng[2]);
}
// compute interpolants