Add actuator and sensor delays. Fixes #1004
PiperOrigin-RevId: 866478839 Change-Id: Id21a6da0f98454c8fa39ea5af8a5e213d6eae497
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
84fa527723
commit
6419534bad
@@ -182,29 +182,29 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
"frictionloss", "springlength", "width", "material",
|
||||
"margin", "stiffness", "damping", "rgba", "user"},
|
||||
{"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
|
||||
"forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim",
|
||||
"forcerange", "actrange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "actdim",
|
||||
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
|
||||
{"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "cranklength", "user", "group"},
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay"},
|
||||
{"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
|
||||
"forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"},
|
||||
"forcerange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kp", "kv", "dampratio", "timeconst"},
|
||||
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "cranklength", "user", "group", "kv"},
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||
{"intvelocity", "?", "ctrllimited", "forcelimited",
|
||||
"ctrlrange", "forcerange", "actrange", "inheritrange",
|
||||
"gear", "cranklength", "user", "group",
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||
"kp", "kv", "dampratio"},
|
||||
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "cranklength", "user", "group", "kv"},
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||
{"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "cranklength", "user", "group",
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||
"timeconst", "area", "diameter", "bias"},
|
||||
{"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "cranklength", "user", "group",
|
||||
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||
"timeconst", "range", "force", "scale",
|
||||
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
||||
{"adhesion", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gain", "user", "group"},
|
||||
"gain", "user", "group", "nsample", "interp", "delay"},
|
||||
{">"},
|
||||
|
||||
{"extension", "*"},
|
||||
@@ -389,51 +389,51 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
|
||||
{"actuator", "*"},
|
||||
{"<"},
|
||||
{"general", "*", "name", "class", "group",
|
||||
{"general", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
|
||||
"actearly"},
|
||||
{"motor", "*", "name", "class", "group",
|
||||
{"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
|
||||
{"position", "*", "name", "class", "group",
|
||||
{"position", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"kp", "kv", "dampratio", "timeconst"},
|
||||
{"velocity", "*", "name", "class", "group",
|
||||
{"velocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"kv"},
|
||||
{"intvelocity", "*", "name", "class", "group",
|
||||
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited",
|
||||
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
||||
"gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"kp", "kv", "dampratio"},
|
||||
{"damper", "*", "name", "class", "group",
|
||||
{"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"forcelimited", "ctrlrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"kv"},
|
||||
{"cylinder", "*", "name", "class", "group",
|
||||
{"cylinder", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"timeconst", "area", "diameter", "bias"},
|
||||
{"muscle", "*", "name", "class", "group",
|
||||
{"muscle", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||
"lengthrange", "gear", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
|
||||
"timeconst", "tausmooth", "range", "force", "scale",
|
||||
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
||||
{"adhesion", "*", "name", "class", "group",
|
||||
{"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
|
||||
{"plugin", "*", "name", "class", "plugin", "instance", "group",
|
||||
{"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
|
||||
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
|
||||
"site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user",
|
||||
@@ -445,56 +445,56 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
|
||||
{"sensor", "*"},
|
||||
{"<"},
|
||||
{"touch", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"accelerometer", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"velocimeter", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"gyro", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"force", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"torque", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
|
||||
{"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
|
||||
{"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"},
|
||||
{"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"tendonvel", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"actuatorpos", "*", "name", "actuator", "cutoff", "noise", "user"},
|
||||
{"actuatorvel", "*", "name", "actuator", "cutoff", "noise", "user"},
|
||||
{"actuatorfrc", "*", "name", "actuator", "cutoff", "noise", "user"},
|
||||
{"jointactuatorfrc", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"tendonactuatorfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"ballquat", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"ballangvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"jointlimitpos", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"jointlimitvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"jointlimitfrc", "*", "name", "joint", "cutoff", "noise", "user"},
|
||||
{"tendonlimitpos", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"tendonlimitvel", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"tendonlimitfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
|
||||
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
||||
{"framelinacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
|
||||
{"frameangacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
|
||||
{"subtreecom", "*", "name", "body", "cutoff", "noise", "user"},
|
||||
{"subtreelinvel", "*", "name", "body", "cutoff", "noise", "user"},
|
||||
{"subtreeangmom", "*", "name", "body", "cutoff", "noise", "user"},
|
||||
{"insidesite", "*", "name", "site", "objtype", "objname", "cutoff", "noise", "user"},
|
||||
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
|
||||
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
|
||||
{"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
|
||||
{"touch", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"accelerometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"velocimeter", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"gyro", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"force", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"torque", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"magnetometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"camprojection", "*", "name", "site", "camera", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"rangefinder", "*", "name", "site", "camera", "data", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"actuatorpos", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"actuatorvel", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"actuatorfrc", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointactuatorfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonactuatorfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"ballquat", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"ballangvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointlimitpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointlimitvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"jointlimitfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonlimitpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonlimitvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tendonlimitfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"framelinacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"frameangacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"subtreecom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"subtreelinvel", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"subtreeangmom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"insidesite", "*", "name", "site", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"contact", "*", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site",
|
||||
"num", "data", "reduce", "cutoff", "noise", "user"},
|
||||
{"e_potential", "*", "name", "cutoff", "noise", "user"},
|
||||
{"e_kinetic", "*", "name", "cutoff", "noise", "user"},
|
||||
{"clock", "*", "name", "cutoff", "noise", "user"},
|
||||
"num", "data", "reduce", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"e_potential", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"e_kinetic", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"clock", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||
{"tactile", "*", "name", "geom", "mesh", "nsample", "interp", "delay", "interval", "user"},
|
||||
{"user", "*", "name", "objtype", "objname", "datatype", "needstage",
|
||||
"dim", "cutoff", "noise", "user"},
|
||||
{"tactile", "*", "name", "geom", "mesh", "user"},
|
||||
{"plugin", "*", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname",
|
||||
"user"},
|
||||
{"<"},
|
||||
@@ -751,6 +751,15 @@ const mjMap bias_map[bias_sz] = {
|
||||
};
|
||||
|
||||
|
||||
// interpolation type
|
||||
const int interp_sz = 3;
|
||||
const mjMap interp_map[interp_sz] = {
|
||||
{"zoh", 0},
|
||||
{"linear", 1},
|
||||
{"cubic", 2}
|
||||
};
|
||||
|
||||
|
||||
// stage type
|
||||
const int stage_sz = 4;
|
||||
const mjMap stage_map[stage_sz] = {
|
||||
@@ -2285,6 +2294,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
|
||||
}
|
||||
}
|
||||
ReadAttrInt(elem, "group", &actuator->group);
|
||||
ReadAttrInt(elem, "nsample", &actuator->nsample);
|
||||
MapValue(elem, "interp", &actuator->interp, interp_map, interp_sz);
|
||||
ReadAttr(elem, "delay", 1, &actuator->delay, text);
|
||||
MapValue(elem, "ctrllimited", &actuator->ctrllimited, TFAuto_map, 3);
|
||||
MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3);
|
||||
MapValue(elem, "actlimited", &actuator->actlimited, TFAuto_map, 3);
|
||||
@@ -4037,6 +4049,10 @@ void mjXReader::Sensor(XMLElement* section) {
|
||||
}
|
||||
ReadAttr(elem, "cutoff", 1, &sensor->cutoff, text);
|
||||
ReadAttr(elem, "noise", 1, &sensor->noise, text);
|
||||
ReadAttrInt(elem, "nsample", &sensor->nsample);
|
||||
MapValue(elem, "interp", &sensor->interp, interp_map, interp_sz);
|
||||
ReadAttr(elem, "delay", 1, &sensor->delay, text);
|
||||
ReadAttr(elem, "interval", 2, sensor->interval, text, /*required=*/false, /*exact=*/false);
|
||||
if (ReadVector(elem, "user", userdata, text)) {
|
||||
mjs_setDouble(sensor->userdata, userdata.data(), userdata.size());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user