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

Add <dcmotor> actuator and related docs and tests.

Browse Source 
PiperOrigin-RevId: 892927987
Change-Id: I38ed6412801341ba03ddf5fe7b93a6081df24d37
This commit is contained in:
Yuval Tassa
2026-04-01 07:49:53 -07:00
committed by Copybara-Service
parent 6da210c794
commit 70a7647ad9
31 changed files with 3994 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_derivative.c
+33
View File
@@ -1107,6 +1107,17 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
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;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
if (dynprm[0] <= 0) {
mjtNum R = mju_max(mjMINVAL, gainprm[0]);
mjtNum K = gainprm[1];
bias_vel -= K * K / R;
}
}
// affine gain
if (m->actuator_gaintype[i] == mjGAIN_AFFINE) {
// extract bias info: prm = [const, kp, kv]
@@ -1122,6 +1133,28 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
m->actuator_gainprm + mjNGAIN*i);
}
// DC motor controller damping and LuGre micro-damping
else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
int input_mode = (int)gainprm[8];
if (input_mode > 0) {
mjtNum R = gainprm[0];
mjtNum K = gainprm[1];
mjtNum gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
mjtNum kp = gainprm[4];
mjtNum kd = gainprm[6];
bias_vel -= gain * (input_mode == 1 ? kd : kp);
}
// LuGre: force includes -sigma1*z_dot, z_dot = a*z + v
// d(sigma1*z_dot)/dv = sigma1*(da/dv*z + 1), ignoring higher-order da/dv*z
mjtNum sigma1 = dynprm[6];
if (sigma1 > 0) {
bias_vel -= sigma1;
}
}
// force = gain .* [ctrl/act]
if (gain_vel != 0) {
if (m->actuator_dyntype[i] == mjDYN_NONE) {
src/engine/engine_forward.c
+243 -26
View File
@@ -257,6 +257,36 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
}
// helper for DC motor: computes control voltage from PID state
static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity,
mjtNum x_I, const mjtNum* gainprm) {
int input_mode = (int)gainprm[8];
mjtNum Vmax = gainprm[7];
mjtNum voltage;
// get voltage
if (input_mode > 0) {
mjtNum kp = gainprm[4]; // proportional gain
mjtNum ki = gainprm[5]; // integral gain
mjtNum kd = gainprm[6]; // derivative gain
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;
}
// clip voltage
if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax);
return voltage;
}
// clamp vector to range
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
@@ -275,7 +305,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu, ntendon = m->ntendon;
mjtNum gain, bias, tau;
mjtNum *prm, *force = d->actuator_force;
mjtNum *force = d->actuator_force;
// clear actuator_force
mju_zero(force, nu);
@@ -327,37 +357,136 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// zero act_dot for actuator plugins
if (m->actuator_actnum[i]) {
mju_zero(d->act_dot + act_first, m->actuator_actnum[i]);
int actnum = m->actuator_actnum[i];
if (actnum) {
mju_zero(d->act_dot + act_first, actnum);
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
const mjtNum* dynprm = m->actuator_dynprm + i*mjNDYN;
mjtDyn dyntype = m->actuator_dyntype[i];
// index into the last element in act. For most actuators it's also the
// first element, but actuator plugins might store their own state in act.
int act_last = act_first + m->actuator_actnum[i] - 1;
// first element, but actuator plugins might store their own state in act
int act_last = act_first + actnum - 1;
// compute act_dot according to dynamics type
switch ((mjtDyn) m->actuator_dyntype[i]) {
switch (dyntype) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[act_last] = ctrl[i];
break;
case mjDYN_FILTER: // linear filter: prm = tau
case mjDYN_FILTER: // linear filter: dynprm = tau
case mjDYN_FILTEREXACT:
tau = mju_max(mjMINVAL, prm[0]);
tau = mju_max(mjMINVAL, dynprm[0]);
d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau;
break;
case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact)
d->act_dot[act_last] = mju_muscleDynamics(
ctrl[i], d->act[act_last], prm);
case mjDYN_MUSCLE: // muscle model: dynprm = (tau_act, tau_deact)
d->act_dot[act_last] = mju_muscleDynamics(ctrl[i], d->act[act_last], dynprm);
break;
case mjDYN_DCMOTOR: { // DC motor: up to 5 optional states
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
// verify allocated state size matches parameters; SHOULD NOT OCCUR
if (mj_dcmotorSlots(dynprm, gainprm).num_slots != actnum) {
mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i);
}
int adr = act_first;
mjtNum velocity = d->actuator_velocity[i];
mjtNum R = gainprm[0]; // resistance
mjtNum K = gainprm[1]; // motor constant
mjtNum ki = gainprm[5]; // integral gain
mjtNum te = dynprm[0]; // electrical time constant
// slot order: slew, integral, temperature, bristle, current
// 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[i], u_prev - slew, u_prev + slew);
d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep;
ctrl[i] = u_eff;
adr++;
}
// controller state: integral state
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[i]; // default raw accumulator for voltage and velocity modes
// position mode
if (input_mode == 1) {
act_dot = ctrl[i] - d->actuator_length[i];
}
// clamp act_dot based on integral state
if (Imax > 0) {
if (x_I >= Imax) {
act_dot = mju_min(act_dot, 0);
} else if (x_I <= -Imax) {
act_dot = mju_max(act_dot, 0);
}
}
d->act_dot[adr] = act_dot;
adr++;
}
// compute physical voltage to feed into current and temperature equations
mjtNum V = dcmotorVoltage(ctrl[i], d->actuator_length[i], velocity, x_I, gainprm);
// temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient
mjtNum RT = dynprm[2]; // thermal resistance
if (RT > 0) {
mjtNum C = dynprm[3]; // thermal capacitance
mjtNum Ta = dynprm[4]; // ambient temperature
mjtNum alpha = gainprm[2]; // temperature coefficient
mjtNum T0 = gainprm[3]; // reference temperature
mjtNum T = d->act[adr]; // temperature rise above ambient
R *= 1 + alpha * (T + Ta - T0);
// get current: from act_last if stateful, from (V - K*omega)/R if stateless
mjtNum current = (te > 0) ? d->act[act_last] : (V - K * velocity) / R;
d->act_dot[adr] = (R*current*current - T / RT) / C;
adr++;
}
// LuGre bristle state: dz/dt = v - sigma0 * |v| / g(v) * z
mjtNum sigma0 = dynprm[5]; // bristle stiffness
if (sigma0 > 0) {
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
mjtNum F_C = biasprm[3]; // Coulomb friction
mjtNum F_S = biasprm[4]; // static friction
mjtNum v_S = biasprm[5]; // Stribeck velocity
mjtNum z = d->act[adr]; // bristle state
mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g);
d->act_dot[adr] = a * z + velocity;
adr++;
}
// current state: di/dt = (V/R - K/R*omega - i) / te
if (te > 0) {
mjtNum dimax = dynprm[1]; // current rate limit (di/dt)_max
mjtNum i_dot = (V/R - K/R*velocity - d->act[act_last]) / te;
if (dimax > 0) {
i_dot = mju_clip(i_dot, -dimax, dimax);
}
d->act_dot[act_last] = i_dot;
}
break;
}
default: // user dynamics
if (mjcb_act_dyn) {
if (m->actuator_actnum[i] == 1) {
if (actnum == 1) {
// scalar activation dynamics, get act_dot
d->act_dot[act_last] = mjcb_act_dyn(m, d, i);
} else {
@@ -407,17 +536,20 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
}
// extract gain info
prm = m->actuator_gainprm + mjNGAIN*i;
// extract info
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
mjtGain gaintype = m->actuator_gaintype[i];
int actnum = m->actuator_actnum[i];
// handle according to gain type
switch ((mjtGain) m->actuator_gaintype[i]) {
switch (gaintype) {
case mjGAIN_FIXED: // fixed gain: prm = gain
gain = prm[0];
gain = gainprm[0];
break;
case mjGAIN_AFFINE: // affine: prm = [const, kp, kv]
gain = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i];
gain = gainprm[0] + gainprm[1]*d->actuator_length[i] + gainprm[2]*d->actuator_velocity[i];
break;
case mjGAIN_MUSCLE: // muscle gain
@@ -425,9 +557,43 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
prm);
gainprm);
break;
case mjGAIN_DCMOTOR: { // DC motor: gain = K or K/R
mjtNum R = gainprm[0]; // resistance
mjtNum K = gainprm[1]; // motor constant
mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm);
// verify allocated state size matches parameters; SHOULD NOT OCCUR
if (slots.num_slots != actnum) {
mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i);
}
int adr = m->actuator_actadr[i];
// adjust R for temperature if enabled
if (slots.temperature >= 0) {
mjtNum T = d->act[adr + slots.temperature];
mjtNum alpha = gainprm[2]; // temperature coefficient
mjtNum T0 = gainprm[3]; // reference temperature
mjtNum Ta = dynprm[4]; // ambient temperature
R *= 1 + alpha * (T + Ta - T0);
}
// stateful current: gain = K, force = K * act[last] (generic path)
// 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
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);
}
break;
}
default: // user gain
if (mjcb_act_gain) {
gain = mjcb_act_gain(m, d, i);
@@ -437,11 +603,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// set force = gain .* [ctrl/act]
if (m->actuator_actadr[i] == -1) {
// 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];
} else {
// use last activation variable associated with actuator i
int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
int act_adr = m->actuator_actadr[i] + actnum - 1;
mjtNum act;
if (m->actuator_actearly[i]) {
@@ -453,25 +622,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// extract bias info
prm = m->actuator_biasprm + mjNBIAS*i;
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
mjtBias biastype = m->actuator_biastype[i];
// handle according to bias type
switch ((mjtBias) m->actuator_biastype[i]) {
switch (biastype) {
case mjBIAS_NONE: // none
bias = 0.0;
break;
case mjBIAS_AFFINE: // affine: prm = [const, kp, kv]
bias = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i];
case mjBIAS_AFFINE: // affine: biasprm = [const, kp, kv]
bias = biasprm[0] + biasprm[1]*d->actuator_length[i] + biasprm[2]*d->actuator_velocity[i];
break;
case mjBIAS_MUSCLE: // muscle passive force
bias = mju_muscleBias(d->actuator_length[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
prm);
biasprm);
break;
case mjBIAS_DCMOTOR: { // DC motor: back-EMF only (current-limited)
bias = 0;
// back-EMF (stateless only; for stateful current it's in the ODE)
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];
}
break;
}
default: // user bias
if (mjcb_act_bias) {
bias = mjcb_act_bias(m, d, i);
@@ -537,6 +719,41 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// clamp actuator_force
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
// add DC motor mechanical forces (not subject to current limits)
for (int i=0; i < nu; i++) {
if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) {
continue;
}
if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
continue;
}
if (mj_actuatorDisabled(m, i) || m->actuator_plugin[i] >= 0) {
continue;
}
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*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);
}
// LuGre friction
mjtNum sigma0 = dynprm[5];
if (sigma0 > 0) {
mjtNum sigma1 = dynprm[6];
mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, m->actuator_gainprm + mjNGAIN*i);
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;
}
}
// qfrc_actuator = moment' * force
mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv,
d->moment_rownnz, d->moment_rowadr, d->moment_colind);
src/engine/engine_support.c
+53 -6
View File
@@ -709,22 +709,69 @@ int mj_actuatorDisabled(const mjModel* m, int i) {
mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
int dyntype = m->actuator_dyntype[actuator_id];
if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
if (dyntype == mjDYN_FILTEREXACT) {
// exact filter integration
// act_dot(0) = (ctrl-act(0)) / tau
// act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau))
// = act(0) + act_dot(0) * tau * (1 - exp(-h / tau))
mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id*mjNDYN]);
act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau));
} else {
// Euler integration
} else if (dyntype == mjDYN_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + actuator_id * mjNDYN;
const mjtNum* gainprm = m->actuator_gainprm + actuator_id * mjNGAIN;
mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm);
int offset = act_adr - m->actuator_actadr[actuator_id];
// current filter: exact integration
if (offset == slots.current) {
mjtNum te = mju_max(mjMINVAL, dynprm[0]);
act = act + act_dot * te * (1 - mju_exp(-m->opt.timestep / te));
}
// LuGre bristle: dz/dt = a*z + v where a = -sigma0*|v|/g(v)
else if (offset == slots.bristle) {
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*actuator_id;
mjtNum F_C = biasprm[3]; // Coulomb friction
mjtNum F_S = biasprm[4]; // static friction
mjtNum v_S = biasprm[5]; // Stribeck velocity
mjtNum sigma0 = dynprm[5]; // bristle stiffness
mjtNum velocity = d->actuator_velocity[actuator_id];
mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
// ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v
mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); // decay rate
mjtNum h = m->opt.timestep;
mjtNum exp_ah = mju_exp(a * h); // state transition
mjtNum int_h = mju_abs(a) > mjMINVAL ? (exp_ah - 1) / a : h; // input integral
act = exp_ah * act + int_h * velocity;
}
// integral state: Euler integration with anti-windup clamp
else if (offset == slots.integral) {
act = act + act_dot * m->opt.timestep;
mjtNum Imax = dynprm[8];
if (Imax > 0) {
act = mju_clip(act, -Imax, Imax);
}
}
// temperature and slew: Euler integration
else {
act = act + act_dot * m->opt.timestep;
}
}
// otherwise Euler integration
else {
act = act + act_dot * m->opt.timestep;
}
// clamp to actrange
if (m->actuator_actlimited[actuator_id]) {
mjtNum* actrange = m->actuator_actrange + 2*actuator_id;
// clamp to actrange unless DC motor
if (dyntype != mjDYN_DCMOTOR && m->actuator_actlimited[actuator_id]) {
const mjtNum* actrange = m->actuator_actrange + 2*actuator_id;
act = mju_clip(act, actrange[0], actrange[1]);
}
src/engine/engine_util_misc.c
+20
View File
@@ -769,6 +769,26 @@ mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) {
}
// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2)
mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S) {
mjtNum ratio = velocity / mju_max(mjMINVAL, v_S);
return F_C + (F_S - F_C) * mju_exp(-ratio*ratio);
}
// compute DC motor activation slot indices from parameter arrays
mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm) {
mjDCMotorSlots s = {-1, -1, -1, -1, -1, 0};
if (dynprm[7] > 0) s.slew = s.num_slots++; // slew rate limiting
if (gainprm[5] > 0) s.integral = s.num_slots++; // PI integral
if (dynprm[2] > 0) s.temperature = s.num_slots++; // thermal model
if (dynprm[5] > 0) s.bristle = s.num_slots++; // LuGre bristle
if (dynprm[0] > 0) s.current = s.num_slots++; // current filter
return s;
}
//---------------------------------------- Base64 --------------------------------------------------
// decoding function for Base64
src/engine/engine_util_misc.h
+17
View File
@@ -50,6 +50,23 @@ MJAPI mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum ta
// muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width)
MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2)
mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S);
// DC motor activation slot indices (-1 = slot not active)
typedef struct {
int slew; // slew rate state
int integral; // integral state
int temperature; // temperature state
int bristle; // LuGre bristle state
int current; // current state
int num_slots; // number of DC motor states
} mjDCMotorSlots;
// compute activation slot indices for a DC motor actuator
// dynprm = actuator_dynprm row, gainprm = actuator_gainprm row
mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm);
// all 3 semi-axes of a geom
MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type);
src/user/user_api.cc
+161
View File
@@ -15,6 +15,7 @@
#include "user/user_api.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
@@ -1120,6 +1121,166 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) {
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode) {
double Kt = motorconst[0]; // torque constant
double Ke = motorconst[1]; // back-EMF constant
double R = resistance; // electrical resistance
double vn = nominal[0]; // nominal voltage
double tau0 = nominal[1]; // stall torque
double omega0 = nominal[2]; // no-load speed
// derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B)
if (vn > 0 && Ke <= 0 && omega0 > 0) {
// viscous damping (linear), add lugre sigma2 contribution if any
double B = actuator->damping[0];
if (lugre[0] > 0) B += lugre[2];
if (B > 0 && R > 0) {
// R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0
double disc = vn*vn - 4*R*B*omega0*omega0;
Ke = disc > 0 ? (vn + sqrt(disc)) / (2*omega0) : vn / omega0;
} else if (B > 0 && tau0 > 0) {
// R from nominal (tau0 = Ke*vn/R, so R = Ke*vn/tau0)
// substituting into omega0 = vn*Ke/(Ke^2 + R*B):
// omega0 = vn/(Ke + vn*B/tau0) => Ke = vn/omega0 - vn*B/tau0
double Ke_exact = vn / omega0 - vn*B / tau0;
Ke = Ke_exact > 0 ? Ke_exact : vn / omega0;
} else {
// B = 0 or insufficient data for B-correction: omega0 = vn*Ke/Ke^2 = vn/Ke
Ke = vn / omega0;
}
}
// resolve effective motor constant K from [Kt, Ke]
double K = (Kt > 0 && Ke > 0) ? sqrt(Kt * Ke) :
(Kt > 0) ? Kt : Ke;
// derive R from nominal: tau0 = K*vn/R
if (R == 0 && vn > 0 && tau0 > 0 && K > 0) {
R = K * vn / tau0;
}
if (K <= 0) return "DC motor: motor constant K must be positive";
if (R <= 0) return "DC motor: resistance R must be positive";
// set types
actuator->dyntype = mjDYN_DCMOTOR;
actuator->gaintype = mjGAIN_DCMOTOR;
actuator->biastype = mjBIAS_DCMOTOR;
// gainprm: [R, K, alpha, T0]
actuator->gainprm[0] = R;
actuator->gainprm[1] = K;
// controller parameters: gainprm[4:6] for kp, ki, kd
actuator->gainprm[4] = controller[0]; // kp
actuator->gainprm[5] = controller[1]; // ki
actuator->gainprm[6] = controller[2]; // kd
// controller parameters: dynprm[7,8] for slewmax, Imax
actuator->dynprm[7] = controller[3]; // slewmax
actuator->dynprm[8] = controller[4]; // Imax
// saturation: [tau_max, i_max, (di/dt)_max, v_max]
if (saturation[2] > 0) {
actuator->dynprm[1] = saturation[2]; // (di/dt)_max
}
if (saturation[3] > 0) {
actuator->gainprm[7] = saturation[3]; // v_max
}
// saturation -> forcerange
if (saturation[0] > 0 || saturation[1] > 0) {
double tau_max = saturation[0];
if (tau_max == 0 && saturation[1] > 0) {
tau_max = K * saturation[1]; // tau_max = K * i_max
}
actuator->forcerange[0] = -tau_max;
actuator->forcerange[1] = tau_max;
actuator->forcelimited = 1;
}
// cogging: [amplitude, periodicity, phase] -> biasprm[0:3]
actuator->biasprm[0] = cogging[0]; // amplitude
actuator->biasprm[1] = cogging[1]; // periodicity
actuator->biasprm[2] = cogging[2]; // phase
// count activation variables: slot order is slew, integral, temperature, bristle, current
int actdim = 0;
// inductance: [L, te]
if (inductance[0] < 0) return "DC motor: inductance must be non-negative";
if (inductance[1] < 0) return "DC motor: electrical time constant must be non-negative";
double te = inductance[0] > 0 ? inductance[0] / R : inductance[1];
actuator->dynprm[0] = te;
if (te > 0) {
actdim++;
}
// controller states: slew rate limiting
if (controller[3] > 0) { // slewmax
actdim++;
}
// controller states: integral
if (controller[1] > 0) { // ki
actdim++;
}
// thermal -> temperature activation
if (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0) {
double RT = thermal[0]; // thermal resistance
double C = thermal[1]; // thermal capacitance
double tth = thermal[2]; // thermal time constant
double alpha = thermal[3]; // temperature coefficient
double T0 = thermal[4]; // reference temperature
double Ta = thermal[5]; // ambient temperature
if (tth > 0 && RT > 0 && C == 0) {
C = tth / RT;
} else if (tth > 0 && C > 0 && RT == 0) {
RT = tth / C;
} else if (tth == 0 && RT > 0 && C > 0) {
tth = RT * C;
}
if (RT <= 0) return "DC motor: thermal resistance must be positive";
if (C <= 0) return "DC motor: thermal capacitance must be positive";
actuator->dynprm[2] = RT;
actuator->dynprm[3] = C;
actuator->dynprm[4] = Ta;
actuator->gainprm[2] = alpha;
actuator->gainprm[3] = T0;
actdim++;
}
// lugre: {stiffness, damping, viscous, coulomb, static, stribeck}
if (lugre[0] > 0) {
actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0
actuator->dynprm[6] = lugre[1]; // damping -> sigma1
actuator->damping[0] += lugre[2]; // viscous -> sigma2
actuator->biasprm[3] = lugre[3]; // coulomb -> tau_c
actuator->biasprm[4] = lugre[4]; // static -> tau_s
actuator->biasprm[5] = lugre[5]; // stribeck -> omega_s
actdim++;
}
// set input mode and activation dimension
actuator->gainprm[8] = input_mode;
actuator->actdim = actdim;
// enforce actlimited = 0; homogeneous bounds are invalid across DC motor states
actuator->actlimited = 0;
return "";
}
// get spec from body
mjSpec* mjs_getSpec(mjsElement* element) {
return &(static_cast<mjCBase*>(element)->model->spec);
src/user/user_objects.cc
+5 -5
View File
@@ -7222,20 +7222,20 @@ void mjCActuator::Compile(void) {
// check and set actdim
if (!plugin.active) {
if (actdim > 1 && dyntype != mjDYN_USER) {
throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator");
if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR) {
throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' and 'dcmotor'");
}
if (actdim == 1 && dyntype == mjDYN_NONE) {
throw mjCError(this, "invalid actdim 1 in stateless actuator");
}
if (actdim == 0 && dyntype != mjDYN_NONE) {
if (actdim == 0 && dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR) {
throw mjCError(this, "invalid actdim 0 in stateful actuator");
}
}
// set actdim
// set actdim to 1 if it is unset and type is standard one-activation dyntype
if (actdim < 0) {
actdim = (dyntype != mjDYN_NONE);
actdim = (dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR);
}
// check muscle parameters
src/xml/xml_native_reader.cc
+75 -4
View File
@@ -206,6 +206,10 @@ std::vector<const char*> MJCF[nMJCF] = {
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "?", "forcelimited", "ctrlrange", "forcerange",
"gain", "user", "group", "nsample", "interp", "delay"},
{"dcmotor", "?", "ctrllimited", "ctrlrange",
"gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay",
"motorconst", "resistance", "nominal", "saturation",
"inductance", "cogging", "controller", "input", "thermal", "lugre"},
{">"},
{"extension", "*"},
@@ -436,6 +440,12 @@ std::vector<const char*> MJCF[nMJCF] = {
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{"dcmotor", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "ctrlrange",
"lengthrange", "gear", "damping", "armature", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"motorconst", "resistance", "nominal", "saturation",
"inductance", "cogging", "controller", "thermal", "lugre", "input"},
{"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "damping", "armature", "cranklength", "joint", "jointinparent",
@@ -724,33 +734,45 @@ const mjMap mark_map[mark_sz] = {
// dyn type
const int dyn_sz = 6;
const int dyn_sz = 7;
const mjMap dyn_map[dyn_sz] = {
{"none", mjDYN_NONE},
{"integrator", mjDYN_INTEGRATOR},
{"filter", mjDYN_FILTER},
{"filterexact", mjDYN_FILTEREXACT},
{"muscle", mjDYN_MUSCLE},
{"dcmotor", mjDYN_DCMOTOR},
{"user", mjDYN_USER}
};
// dcmotor controller input mode
const int dcmotorinput_sz = 3;
const mjMap dcmotorinput_map[dcmotorinput_sz] = {
{"voltage", 0},
{"position", 1},
{"velocity", 2}
};
// gain type
const int gain_sz = 4;
const int gain_sz = 5;
const mjMap gain_map[gain_sz] = {
{"fixed", mjGAIN_FIXED},
{"affine", mjGAIN_AFFINE},
{"muscle", mjGAIN_MUSCLE},
{"dcmotor", mjGAIN_DCMOTOR},
{"user", mjGAIN_USER}
};
// bias type
const int bias_sz = 4;
const int bias_sz = 5;
const mjMap bias_map[bias_sz] = {
{"none", mjBIAS_NONE},
{"affine", mjBIAS_AFFINE},
{"muscle", mjBIAS_MUSCLE},
{"dcmotor", mjBIAS_DCMOTOR},
{"user", mjBIAS_USER}
};
@@ -2498,6 +2520,54 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
err = mjs_setToAdhesion(actuator, gain);
}
// DC motor
else if (type == "dcmotor") {
bool inherited = (actuator->gaintype == mjGAIN_DCMOTOR);
double motorconst[2] = {inherited ? actuator->gainprm[1] : 0, 0};
double resistance = inherited ? actuator->gainprm[0] : 0;
double nominal[3] = {0, 0, 0};
double saturation[4] = {0, 0,
inherited ? actuator->dynprm[1] : 0,
inherited ? actuator->gainprm[8] : 0};
double controller[5] = {inherited ? actuator->gainprm[5] : 0,
inherited ? actuator->gainprm[6] : 0,
inherited ? actuator->gainprm[7] : 0,
inherited ? actuator->dynprm[7] : 0,
inherited ? actuator->dynprm[8] : 0};
double inductance[2] = {0, inherited ? actuator->dynprm[0] : 0};
double cogging[3] = {inherited ? actuator->biasprm[0] : 0,
inherited ? actuator->biasprm[1] : 0,
inherited ? actuator->biasprm[2] : 0};
double thermal[6] = {inherited ? actuator->dynprm[2] : 0,
inherited ? actuator->dynprm[3] : 0,
0,
inherited ? actuator->gainprm[2] : 0,
inherited ? actuator->gainprm[3] : 0,
inherited ? actuator->dynprm[4] : 0};
double lugre[6] = {inherited ? actuator->dynprm[5] : 0,
inherited ? actuator->dynprm[6] : 0,
inherited ? actuator->damping[0] : 0,
inherited ? actuator->biasprm[3] : 0,
inherited ? actuator->biasprm[4] : 0,
inherited ? actuator->biasprm[5] : 0};
int input_mode = inherited ? (int)actuator->gainprm[9] : 0;
ReadAttr(elem, "motorconst", 2, motorconst, text, false, false);
ReadAttr(elem, "resistance", 1, &resistance, text);
ReadAttr(elem, "nominal", 3, nominal, text, false, false);
ReadAttr(elem, "saturation", 4, saturation, text, false, false);
ReadAttr(elem, "inductance", 2, inductance, text, false, false);
ReadAttr(elem, "cogging", 3, cogging, text, false, false);
ReadAttr(elem, "controller", 5, controller, text, false, false);
ReadAttr(elem, "thermal", 6, thermal, text, false, false);
ReadAttr(elem, "lugre", 6, lugre, text, false, false);
if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) {
// successfully parsed
}
err = mjs_setToDCMotor(actuator, motorconst, resistance,
nominal, saturation, inductance,
cogging, controller, thermal, lugre, input_mode);
}
else if (type == "plugin") {
OnePlugin(elem, &actuator->plugin);
int n;
@@ -2962,7 +3032,8 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS*
name == "intvelocity" ||
name == "cylinder" ||
name == "muscle" ||
name == "adhesion") {
name == "adhesion" ||
name == "dcmotor") {
OneActuator(elem, def->actuator);
}
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 246
#define nMJCF 248
extern std::vector<const char*> MJCF[nMJCF];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+1 -1
View File
@@ -871,7 +871,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
if (writingdefaults) {
WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim);
} else {
int default_actdim = actuator->dyntype == mjDYN_NONE ? 0 : 1;
int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR);
WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim);
}
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, actuator->dyntype, def->Actuator().dyntype);