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

Add dampratio attribute to position and intvelocity actuators.

Browse Source 
PiperOrigin-RevId: 639745135
Change-Id: I94ba4c346d2750c6c454a60a568f2e6f6bce19d4
This commit is contained in:
Yuval Tassa
2024-06-03 05:26:14 -07:00
committed by Copybara-Service
parent d6712f2944
commit 2830a4071f
9 changed files with 314 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_setconst.c
+37 -1
View File
@@ -341,12 +341,48 @@ static void set0(mjModel* m, mjData* d) {
// compute positional offsets
mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id));
// copy dir
mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
}
// compute actuator damping from dampratio
for (int i=0; i < m->nu; i++) {
// get bias, gain parameters
mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
// not a position-like actuator: skip
if (gainprm[0] != -biasprm[1]) {
continue;
}
// damping is 0 or negative (interpreted as regular "kv"): skip
if (biasprm[2] <= 0) {
continue;
}
// === interpret biasprm[2] > 0 as dampratio for position-like actuators
// "reflected" inertia (inversely scaled by transmission squared)
mjtNum* transmission = d->actuator_moment + i*nv;
mjtNum mass = 0;
for (int j=0; j < nv; j++) {
mjtNum trn = mju_abs(transmission[j]);
mjtNum trn2 = trn*trn; // transmission squared
if (trn2 > mjMINVAL) {
mass += m->dof_M0[j] / trn2;
}
}
// damping = dampratio * 2 * sqrt(kp * mass)
mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass);
// set biasprm[2] to negative damping
biasprm[2] = -damping;
}
mj_freeStack(d);
}
src/xml/xml_native_reader.cc
+29 -16
View File
@@ -177,18 +177,16 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
{"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
{"position", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
"forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "timeconst"},
{"position", "?", "13", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
"forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"},
{"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"kv"},
{"intvelocity", "?", "12", "ctrllimited", "forcelimited",
"gear", "cranklength", "user", "group", "kv"},
{"intvelocity", "?", "13", "ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange",
"gear", "cranklength", "user", "group",
"kp", "kv"},
"kp", "kv", "dampratio"},
{"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"kv"},
"gear", "cranklength", "user", "group", "kv"},
{"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"timeconst", "area", "diameter", "bias"},
@@ -389,22 +387,22 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
{"position", "*", "22", "name", "class", "group",
{"position", "*", "23", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv", "timeconst"},
"kp", "kv", "dampratio", "timeconst"},
{"velocity", "*", "19", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"intvelocity", "*", "22", "name", "class", "group",
{"intvelocity", "*", "23", "name", "class", "group",
"ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
"gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv"},
"kp", "kv", "dampratio"},
{"damper", "*", "18", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -2087,18 +2085,33 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) {
ReadAttr(elem, "kp", 1, pact->gainprm, text);
pact->biasprm[1] = -pact->gainprm[0];
if (ReadAttr(elem, "kv", 1, pact->biasprm + 2, text)) {
if (pact->biasprm[2] < 0)
throw mjXError(elem, "kv cannot be negative");
pact->biasprm[2] *= -1;
// read kv
double kv = -1; // -1: undefined
if (ReadAttr(elem, "kv", 1, &kv, text)) {
if (kv < 0) throw mjXError(elem, "kv cannot be negative");
}
// read dampratio
double dampratio = -1; // -1: undefined
if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) {
if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative");
}
// set biasprm[2]; negative: regular damping, positive: dampratio
if (dampratio > 0 && kv > 0) {
throw mjXError(elem, "kv and dampratio cannot both be defined");
}
if (kv > 0) pact->biasprm[2] = -kv;
if (dampratio > 0) pact->biasprm[2] = dampratio;
// read timeconst, set dyntype
if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) {
if (pact->dynprm[0] < 0)
throw mjXError(elem, "timeconst cannot be negative");
pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE;
}
// handle inheritrange
ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text);
if (pact->inheritrange > 0) {
if (type == "position") {