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

Redesign the dcmotor controller: setpoint inputs, torque-space gains.

Browse Source 
The dcmotor input block is any subset of the canonical list [pos, vel,
ff, voltage], selected with input="pos vel ff voltage" and recorded as
mjtCtrlInput bits in actuator_ctrlspec like pid. Tokens are required in
canonical order: the attribute denotes a set, the block always packs
canonically, and accepting permutations invites reading the string as a
layout choice. The mode flag in gainprm[8] is retired (reserved,
written 0).

Controller gains are now in torque space, as for pid: the controller
commands tau = kp*(q*-l) + kd*(v*-ldot) + ki*x_I + tau_ff over the
present inputs (absent setpoints frozen at zero) and converts to drive
voltage V = R/K * tau + K*ldot. The second term compensates back-EMF,
as the current loop of a real torque-mode driver does (torque commands
are current commands): commanded torque is delivered exactly until a
limit binds, and the torque-speed envelope emerges from the Vmax clamp.
The map uses the nameplate R: thermal resistance growth is not
compensated, so a hot motor under-delivers by R/R(T). A stateless
setpoint dcmotor now matches <pid> exactly, for any K and R; the old
back-EMF droop remains available as the physical behavior of the raw
voltage path. Voltage-space datasheet gains convert by K/R. Controller
inputs require a positive motor constant (the map divides by K), and
controller gains require a controller input.

ff and voltage are distinct inputs, different in kind: ff is a torque
feedforward added to the controller output, uniform with pid's ff
(feedforward in the actuator's output space), while voltage is the raw
terminal voltage of the physical device, injected downstream of the
controller and its Vmax clamp, unclamped (ctrlrange bounds it if
desired). input="voltage" is the default: the plain voltage-commanded
motor, whose behavior is unchanged by this commit. The integrator
always accumulates position error; the old velocity mode's integral
term, ki*(int(u)dt - theta), which tracked the integral of the velocity
command, is retired without replacement, keeping ki mode-independent --
commanded integrated velocity belongs to an integrator activation
state, not to controller gains. slewmax rate-limits the first controller
input -- position setpoint (rad/s), velocity setpoint (rad/s^2) or torque
feedforward (N*m/s), each a real driver feature (reference ramping,
ramped-velocity and ramped-torque input modes); the raw voltage input
is never rate-limited and slewmax requires a controller input.

input="none" selects the empty signature: the actuator owns no controls
at all (nu = 0 is now legal with actuators present) and is purely
passive -- LuGre friction, cogging and back-EMF braking as passive
joint forces. This exists because auxiliary dynamic states (the LuGre
bristle) attach to actuators, not joints. The terminal voltage is
identically zero, i.e. a shorted motor (dynamic braking); motorconst=0
decouples the electrical branch. mjINPUT_NONE is a distinct enum value
because ctrlspec = 0 means "unset, use the type default". History and
delay require an input; the controller voltage override and input read
in mj_fwdActuation are gated on a nonempty block.

The analytic velocity derivative of the controller becomes
dV/dw = -kd*R/K + K, whose second term cancels the back-EMF bias
exactly: the net damping of an unclipped torque-mode motor is -kd, and
of a voltage-mode or passive motor -K^2/R. Viewers label inputs via
mj_actuatorInputName: pos, vel, ff, voltage.

The dcmotor LaTeX design doc is updated accordingly: torque-space
units, the tau->V map and its saturation-generated envelope, the
input-block pipeline figure, and a Passive Operation section.

PiperOrigin-RevId: 965795351
Change-Id: Ibc308ca21bd6bad014e77f950ee08feaad449b73
This commit is contained in:
Yuval Tassa
2026-08-17 00:42:47 -07:00
committed by Copybara-Service
parent 11fa4a5b45
commit 2f1843f4a7
29 changed files with 574 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_derivative.c
+9 -4
View File
@@ -2433,11 +2433,16 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
mjtNum te = dynprm[0];
// controller velocity derivative: dV/dω
int input_mode = (int)gainprm[8];
// controller velocity derivative dV/dw: torque-space kd through the tau->V map,
// plus the back-EMF compensation K, which cancels the -K^2/R back-EMF bias term so
// the net damping of an unclipped torque-mode motor is -kd; Vmax clipping is ignored
// here, matching the treatment of the other saturations
mjtNum dVdw = 0;
if (input_mode == 1) dVdw = -gainprm[6]; // position: -kd
else if (input_mode == 2) dVdw = -gainprm[4]; // velocity: -kp
if (m->actuator_ctrlspec[i] & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
mjtNum R = mju_max(mjMINVAL, gainprm[0]);
mjtNum K = gainprm[1]; // K > 0 on this path, enforced by the compiler
dVdw = -gainprm[6]*R/K + K;
}
if (te > 0) {
// stateful current with actearly: d(K*next_act)/dω
src/engine/engine_forward.c
+40 -43
View File
@@ -227,42 +227,44 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// unpack servo-family inputs from control block in canonical order [pos, vel, ff]
// absent input: setpoint 0
static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[3]) {
static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[4]) {
int adr = 0;
out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0;
out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0;
out[2] = (spec & mjINPUT_FF) ? u[adr] : 0;
out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0;
out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0;
out[2] = (spec & mjINPUT_FF) ? u[adr++] : 0;
out[3] = (spec & mjINPUT_VOLTAGE) ? u[adr] : 0;
}
// helper for DC motor: computes control voltage from PID state
static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity,
static mjtNum dcmotorVoltage(const mjtNum* u, int spec, mjtNum length, mjtNum velocity,
mjtNum x_I, const mjtNum* gainprm) {
int input_mode = (int)gainprm[8];
mjtNum Vmax = gainprm[7];
mjtNum voltage;
mjtNum voltage = 0;
// get voltage
if (input_mode > 0) {
// unpack present inputs in canonical order [pos, vel, ff, voltage]; absent input: 0
mjtNum u4[4];
unpackServoInputs(u, spec, u4);
// on-board controller: torque-space PID + torque feedforward
if (spec & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
mjtNum kp = gainprm[4]; // proportional gain
mjtNum ki = gainprm[5]; // integral gain
mjtNum kd = gainprm[6]; // derivative gain
mjtNum torque = kp*(u4[0] - length) + kd*(u4[1] - velocity) + ki*x_I + u4[2];
if (input_mode == 1) {
// position mode
voltage = kp * (ctrl - length) + ki * x_I - kd * velocity;
} else {
// velocity mode
voltage = kp * (ctrl - velocity) + ki * (x_I - length);
}
} else {
voltage = ctrl;
// torque mode is current control: V = R/K * torque + K * velocity, the second
// term compensating back-EMF; the compiler requires K > 0 on this path
mjtNum R = gainprm[0];
mjtNum K = gainprm[1];
voltage = R/K * torque + K*velocity;
// driver supply limit
mjtNum Vmax = gainprm[7];
if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax);
}
// clip voltage
if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax);
return voltage;
// raw terminal voltage input: downstream of the controller, unclamped
return voltage + u4[3];
}
@@ -504,26 +506,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// controller state: slew rate limiting
mjtNum slew_s = dynprm[7]; // slew rate limit
if (slew_s > 0) {
mjtNum u_prev = d->act[adr];
mjtNum slew = slew_s * m->opt.timestep;
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[uadr] = u_eff;
ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep,
0, d->act_dot + adr);
adr++;
}
// controller state: integral state
// controller state: integral of the position error (setpoint mode only)
mjtNum x_I = 0;
if (ki > 0) {
x_I = d->act[adr];
int input_mode = (int)gainprm[8];
mjtNum Imax = dynprm[8]; // integral clamp
mjtNum act_dot = ctrl[uadr]; // default raw accumulator for voltage and velocity modes
// position mode
if (input_mode == 1) {
act_dot = ctrl[uadr] - d->actuator_length[oadr];
}
mjtNum act_dot = ctrl[uadr] - d->actuator_length[oadr];
// clamp act_dot based on integral state
if (Imax > 0) {
@@ -538,7 +531,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// compute physical voltage to feed into current and temperature equations
mjtNum V = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], velocity, x_I, gainprm);
mjtNum V = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
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
@@ -738,9 +732,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
// controller: compute voltage, override ctrl[uadr] for force computation
if ((int)gainprm[8] > 0) {
// (pure raw-voltage motor reads ctrl directly; empty block reads as 0 below)
if (m->actuator_ctrlspec[i] != mjINPUT_VOLTAGE && m->actuator_ctrlnum[i] > 0) {
mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0;
ctrl[uadr] = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr],
ctrl[uadr] = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
d->actuator_length[oadr],
d->actuator_velocity[oadr], x_I, gainprm);
}
break;
@@ -769,9 +765,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
// unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0
mjtNum u3[3];
unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u3);
mjtNum qref = u3[0], vref = u3[1], ff = u3[2];
mjtNum u4[4];
unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u4);
mjtNum qref = u4[0], vref = u4[1], ff = u4[2];
// position setpoint: representative nearest the length on rotational transmissions
mjtNum period = wrapPeriod(m, i);
@@ -792,7 +788,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
}
else if (actnum == 0 || dcmotor_no_current) {
mjtNum input = ctrl[uadr];
// empty input block (passive dcmotor): input is 0
mjtNum input = m->actuator_ctrlnum[i] ? ctrl[uadr] : 0;
// rotational setpoint: use representative nearest the length (local, no state change)
mjtNum period = wrapPeriod(m, i);
src/engine/engine_name.c
+5 -5
View File
@@ -297,12 +297,12 @@ const char* mj_actuatorInputName(const mjModel* m, int id, int input) {
return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input];
}
// servo family: input names are the present members of [pos, vel, ff]
if (m->actuator_gaintype[id] == mjGAIN_PID) {
static const char* servo[3] = {"pos", "vel", "ff"};
static const int bits[3] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF};
// servo family: input names are the present members of [pos, vel, ff, voltage]
if (m->actuator_gaintype[id] == mjGAIN_PID || m->actuator_gaintype[id] == mjGAIN_DCMOTOR) {
static const char* servo[4] = {"pos", "vel", "ff", "voltage"};
static const int bits[4] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF, mjINPUT_VOLTAGE};
int spec = m->actuator_ctrlspec[id];
for (int k=0; k < 3; k++) {
for (int k=0; k < 4; k++) {
if (spec & bits[k]) {
if (input == 0) {
return servo[k];
src/user/user_api.cc
+3 -2
View File
@@ -1506,7 +1506,7 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) {
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode) {
double lugre[5], int ctrlspec) {
double R = resistance; // electrical resistance
double Kt = motorconst ? motorconst[0] : 0; // torque constant
double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant
@@ -1652,7 +1652,8 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
}
// set input mode and activation dimension
actuator->gainprm[8] = input_mode;
actuator->gainprm[8] = 0; // reserved (was input_mode)
actuator->ctrlspec = ctrlspec;
actuator->actdim = actdim;
// enforce actlimited = 0; homogeneous bounds are invalid across DC motor states
src/user/user_api.h
+1 -1
View File
@@ -213,7 +213,7 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode);
double lugre[5], int ctrlspec);
//---------------------------------- Add assets ----------------------------------------------------
src/user/user_objects.cc
+43 -6
View File
@@ -7233,16 +7233,49 @@ void mjCActuator::Compile(void) {
!!(ctrlspec_ & mjINPUT_FF);
}
// DC motor: resolve input block (default: raw voltage command)
if (gaintype == mjGAIN_DCMOTOR) {
ctrlspec_ = ctrlspec ? ctrlspec : mjINPUT_VOLTAGE;
if (ctrlspec_ != mjINPUT_NONE &&
(ctrlspec_ & ~(mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF | mjINPUT_VOLTAGE))) {
throw mjCError(this, "dcmotor inputs are 'none' or a subset of [pos, vel, ff, voltage] in "
"actuator '%s' (id = %d)", name.c_str(), id);
}
// controller inputs engage the torque-space controller, which divides by the motor constant
int controller = ctrlspec_ == mjINPUT_NONE ?
0 : ctrlspec_ & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF);
if (controller && gainprm[1] <= 0) {
throw mjCError(this, "dcmotor controller inputs require a positive motor constant in "
"actuator '%s' (id = %d)", name.c_str(), id);
}
if (!controller && (gainprm[4] || gainprm[5] || gainprm[6])) {
throw mjCError(this, "dcmotor controller gains require a controller input [pos, vel, ff] "
"in actuator '%s' (id = %d)", name.c_str(), id);
}
if (gainprm[5] > 0 && !(ctrlspec_ & mjINPUT_POS)) {
throw mjCError(this, "dcmotor integral gain requires the pos input in actuator '%s' "
"(id = %d)", name.c_str(), id);
}
ctrlnum_ = !!(ctrlspec_ & mjINPUT_POS) + !!(ctrlspec_ & mjINPUT_VEL) +
!!(ctrlspec_ & mjINPUT_FF) + !!(ctrlspec_ & mjINPUT_VOLTAGE);
if (!controller && dynprm[7] > 0) {
throw mjCError(this, "dcmotor slew rate limiting requires a controller input [pos, vel, "
"ff] in actuator '%s' (id = %d)", name.c_str(), id);
}
}
// pid dynamics are pid-only
if (dyntype == mjDYN_PID && gaintype != mjGAIN_PID) {
throw mjCError(this, "dyntype 'pid' requires gaintype 'pid', actuator '%s' (id = %d)",
name.c_str(), id);
}
// input signature selection is so3- or pid-only
if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID) {
throw mjCError(this, "input is only available for so3 and pid actuators, actuator '%s' "
"(id = %d)", name.c_str(), id);
// input signature selection is so3-, pid- or dcmotor-only
if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID &&
gaintype != mjGAIN_DCMOTOR) {
throw mjCError(this, "input is only available for so3, pid and dcmotor actuators, "
"actuator '%s' (id = %d)", name.c_str(), id);
}
// check damping/armature only valid for joint and tendon transmission
@@ -7273,7 +7306,7 @@ void mjCActuator::Compile(void) {
double* range;
if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT ||
dyntype == mjDYN_PID) {
// position or pd actuator: range applies to the position input
// position or pid actuator: range applies to the position input
range = ctrlrange;
} else if (dyntype == mjDYN_INTEGRATOR) {
// intvelocity actuator
@@ -7417,6 +7450,10 @@ void mjCActuator::Compile(void) {
if (delay > 0 && nsample <= 0) {
throw mjCError(this, "setting delay > 0 without a history buffer");
}
if ((delay > 0 || nsample > 0) && ctrlnum_ == 0) {
throw mjCError(this, "history and delay require an input in actuator '%s' (id = %d)",
name.c_str(), id);
}
// nsample is limited to 2^24 because the cursor is stored as an mjtNum, which may be a float
// single-precision floats can represent all integers up to 2^24 exactly
@@ -7424,7 +7461,7 @@ void mjCActuator::Compile(void) {
throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample);
}
// resolve per-input control ranges: broadcast ctrlrange, pd overrides vel and ff
// resolve per-input control ranges: broadcast ctrlrange, pid overrides vel and ff
for (int j=0; j < ctrlnum_ && j < 4; j++) {
ctrllimiteds_[j] = (mjtByte)is_ctrllimited();
ctrlranges_[j][0] = ctrlrange[0];
src/xml/generated/mjcf.xsd
+17 -7
View File
@@ -223,11 +223,12 @@
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="kw_dcmotorinput">
<xs:simpleType name="kw_inputkeyword">
<xs:annotation>
<xs:documentation>whole-attribute keyword: the empty signature</xs:documentation>
</xs:annotation>
<xs:restriction base="xs:string">
<xs:enumeration value="voltage"/>
<xs:enumeration value="position"/>
<xs:enumeration value="velocity"/>
<xs:enumeration value="none"/>
</xs:restriction>
</xs:simpleType>
@@ -252,12 +253,13 @@
<xs:simpleType name="kw_inputbit">
<xs:annotation>
<xs:documentation>bitflags: keywords combine bitwise</xs:documentation>
<xs:documentation>bitflags: tokens combine bitwise, canonical order enforced</xs:documentation>
</xs:annotation>
<xs:restriction base="xs:string">
<xs:enumeration value="pos"/>
<xs:enumeration value="vel"/>
<xs:enumeration value="ff"/>
<xs:enumeration value="voltage"/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="kwlist_inputbit">
@@ -1880,7 +1882,11 @@
<xs:attribute name="controller" type="double1to6"/>
<xs:attribute name="thermal" type="double1to6"/>
<xs:attribute name="lugre" type="double1to5"/>
<xs:attribute name="input" type="kw_dcmotorinput"/>
<xs:attribute name="input" type="kwlist_inputbit">
<xs:annotation>
<xs:documentation>token subset, or the none keyword</xs:documentation>
</xs:annotation>
</xs:attribute>
</xs:complexType>
<xs:complexType name="extension_plugin">
@@ -3055,7 +3061,11 @@
<xs:attribute name="controller" type="double1to6"/>
<xs:attribute name="thermal" type="double1to6"/>
<xs:attribute name="lugre" type="double1to5"/>
<xs:attribute name="input" type="kw_dcmotorinput"/>
<xs:attribute name="input" type="kwlist_inputbit">
<xs:annotation>
<xs:documentation>token subset, or the none keyword</xs:documentation>
</xs:annotation>
</xs:attribute>
</xs:complexType>
<xs:complexType name="actuator_plugin">
src/xml/generated/mjcf_map.h
+9 -10
View File
@@ -243,13 +243,11 @@ inline constexpr mjMap dyn_map[] = {
};
inline constexpr int dyn_sz = 8;
// enum dcmotorinput
inline constexpr mjMap dcmotorinput_map[] = {
{"voltage", 0},
{"position", 1},
{"velocity", 2},
// enum inputkeyword
inline constexpr mjMap inputkeyword_map[] = {
{"none", mjINPUT_NONE},
};
inline constexpr int dcmotorinput_sz = 3;
inline constexpr int inputkeyword_sz = 1;
// enum gain
inline constexpr mjMap gain_map[] = {
@@ -272,11 +270,12 @@ inline constexpr int inputchart_sz = 2;
// enum inputbit
inline constexpr mjMap inputbit_map[] = {
{"pos", mjINPUT_POS},
{"vel", mjINPUT_VEL},
{"ff", mjINPUT_FF},
{"pos", mjINPUT_POS},
{"vel", mjINPUT_VEL},
{"ff", mjINPUT_FF},
{"voltage", mjINPUT_VOLTAGE},
};
inline constexpr int inputbit_sz = 3;
inline constexpr int inputbit_sz = 4;
// enum bias
inline constexpr mjMap bias_map[] = {
src/xml/mjcf.schema
+8 -9
View File
@@ -237,10 +237,8 @@ enum dyn : mjtDyn {
user = mjDYN_USER
}
enum dcmotorinput {
voltage = 0
position = 1
velocity = 2
enum inputkeyword : mjtCtrlInput { # whole-attribute keyword: the empty signature
none = mjINPUT_NONE
}
enum gain : mjtGain {
@@ -258,10 +256,11 @@ enum inputchart : mjtCtrlChart {
quat = mjCHART_QUAT
}
enum inputbit : mjtCtrlInput { # bitflags: keywords combine bitwise
pos = mjINPUT_POS
vel = mjINPUT_VEL
ff = mjINPUT_FF
enum inputbit : mjtCtrlInput { # bitflags: tokens combine bitwise, canonical order enforced
pos = mjINPUT_POS
vel = mjINPUT_VEL
ff = mjINPUT_FF
voltage = mjINPUT_VOLTAGE
}
enum bias : mjtBias {
@@ -1680,7 +1679,7 @@ element dcmotor : mjsActuator {
controller : double[1..6]
thermal : double[1..6]
lugre : double[1..5]
input : enum<dcmotorinput>
input : flags<inputbit> (reading=custom) # token subset, or the none keyword
}
element actuator_plugin : mjsActuator (xml=plugin) {
src/xml/xml_native_reader.cc
+17 -8
View File
@@ -1149,7 +1149,8 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
// read the "input" attribute: so3 chart keyword, or servo input token list
// read the "input" attribute: so3 chart keyword, the "none" keyword (empty signature),
// or a servo input token list, required to be in canonical order [pos, vel, ff, voltage]
static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) {
std::string text;
if (!mjXUtil::ReadAttrTxt(elem, "input", text)) {
@@ -1163,11 +1164,21 @@ static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) {
return true;
}
// servo input tokens
// empty-signature keyword
int keyword = mjXUtil::FindKey(inputkeyword_map, inputkeyword_sz, text);
if (keyword >= 0) {
*ctrlspec = keyword;
return true;
}
// servo input tokens; strictly ascending bits = canonical order, no duplicates
int bits[inputbit_sz];
int nbit = mjXUtil::MapValues(elem, "input", bits, inputbit_map, inputbit_sz);
int spec = 0;
for (int k=0; k < nbit; k++) {
if (bits[k] <= (k ? bits[k-1] : 0)) {
throw mjXError(elem, "inputs must be listed in canonical order [pos, vel, ff, voltage]");
}
spec |= bits[k];
}
*ctrlspec = spec;
@@ -1235,7 +1246,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
// explicit attributes
string err;
if (type == "general") {
// so3 chart keyword or servo token subset
// so3 chart keyword or servo token subset; dcmotor accepts the voltage keyword
ReadInputSpec(elem, &actuator->ctrlspec);
}
@@ -1431,7 +1442,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
inherited ? actuator->biasprm[3] : 0,
inherited ? actuator->biasprm[4] : 0,
inherited ? actuator->biasprm[5] : 0};
int input_mode = inherited ? (int)actuator->gainprm[8] : 0;
int ctrlspec = inherited ? actuator->ctrlspec : 0;
ReadAttr(elem, "motorconst", 2, motorconst, text, false, false);
ReadAttr(elem, "resistance", 1, &resistance, text);
ReadAttr(elem, "nominal", 3, nominal, text, false, false);
@@ -1441,12 +1452,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
ReadAttr(elem, "controller", 6, controller, text, false, false);
ReadAttr(elem, "thermal", 6, thermal, text, false, false);
ReadAttr(elem, "lugre", 5, lugre, text, false, false);
if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) {
// successfully parsed
}
ReadInputSpec(elem, &ctrlspec);
err = mjs_setToDCMotor(actuator, motorconst, resistance,
nominal, saturation, inductance,
cogging, controller, thermal, lugre, input_mode);
cogging, controller, thermal, lugre, ctrlspec);
}
else if (type == "plugin") {
src/xml/xml_native_writer.cc
+9 -5
View File
@@ -838,13 +838,17 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec,
def->Actuator().ctrlspec);
} else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
std::string tokens;
for (int k=0; k < inputbit_sz; k++) {
if (actuator->ctrlspec & inputbit_map[k].value) {
tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key;
if (actuator->ctrlspec == mjINPUT_NONE) {
WriteAttrTxt(elem, "input", "none");
} else {
std::string tokens;
for (int k=0; k < inputbit_sz; k++) {
if (actuator->ctrlspec & inputbit_map[k].value) {
tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key;
}
}
WriteAttrTxt(elem, "input", tokens);
}
WriteAttrTxt(elem, "input", tokens);
}
WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);