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

Allow plugin actuators to have activation state.

Browse Source 
Accept new attributes on the plugin/actuator tag:
  - dyntype
  - dynprm
  - actlimited
  - actrange
  - actearly

This addresses some of the requests in #1098.

PiperOrigin-RevId: 584866281
Change-Id: I4f7559e446888216093efba4ced9119cfc0528a4
This commit is contained in:
Nimrod Gileadi
2023-11-23 05:13:16 -08:00
committed by Copybara-Service
parent 20eeb75086
commit f2025c6abf
13 changed files with 229 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+31 -10
View File
@@ -287,45 +287,66 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
int act_first = m->actuator_actadr[i];
if (act_first < 0) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
// zero act_dot for actuator plugins
if (m->actuator_actnum[i]) {
mju_zero(d->act_dot + act_first, m->actuator_actnum[i]);
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
// 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;
// compute act_dot according to dynamics type
switch ((mjtDyn) m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = ctrl[i];
d->act_dot[act_last] = ctrl[i];
break;
case mjDYN_FILTER: // linear filter: prm = tau
case mjDYN_FILTEREXACT:
tau = mju_max(mjMINVAL, prm[0]);
d->act_dot[j] = (ctrl[i] - d->act[j]) / tau;
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[j] = mju_muscleDynamics(ctrl[i], d->act[j], prm);
d->act_dot[act_last] = mju_muscleDynamics(
ctrl[i], d->act[act_last], prm);
break;
default: // user dynamics
if (mjcb_act_dyn) {
if (m->actuator_actnum[i] == 1) {
// scalar activation dynamics, get act_dot
d->act_dot[j] = mjcb_act_dyn(m, d, i);
d->act_dot[act_last] = mjcb_act_dyn(m, d, i);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
mju_zero(d->act_dot + j, m->actuator_actnum[i]);
}
}
}
// get act_dot from actuator plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mjERROR("invalid plugin slot: %d", slot);
}
if (plugin->capabilityflags & mjPLUGIN_ACTUATOR) {
if (plugin->actuator_act_dot) {
plugin->actuator_act_dot(m, d, i);
}
}
}
}
src/user/user_model.cc
+18 -11
View File
@@ -944,13 +944,9 @@ void mjCModel::SetSizes(void) {
}
// nu, na
for (int i=0; i<(int)actuators.size(); i++) {
if (actuators[i]->dyntype == mjDYN_NONE) {
nu++;
} else {
nu++;
na += actuators[i]->actdim;
}
for (int i=0; i<actuators.size(); i++) {
nu++;
na += actuators[i]->actdim + actuators[i]->plugin_actdim;
}
// nbvh, nbvhstatic, nbvhdynamic
@@ -1811,6 +1807,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->nconmax = nconmax;
m->nsensordata = nsensordata;
m->nuserdata = nuserdata;
m->na = na;
// find bvh_adr after bodies
bvh_adr = 0;
@@ -2205,9 +2202,9 @@ void mjCModel::CopyObjects(mjModel* m) {
m->actuator_biastype[i] = pac->biastype;
m->actuator_trnid[2*i] = pac->trnid[0];
m->actuator_trnid[2*i+1] = pac->trnid[1];
m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr;
adr += pac->actdim;
m->actuator_actnum[i] = pac->actdim;
m->actuator_actnum[i] = pac->actdim + pac->plugin_actdim;
m->actuator_actadr[i] = m->actuator_actnum[i] ? adr : -1;
adr += m->actuator_actnum[i];
m->actuator_group[i] = pac->group;
m->actuator_ctrllimited[i] = pac->ctrllimited;
m->actuator_forcelimited[i] = pac->forcelimited;
@@ -2982,9 +2979,12 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// query and set plugin-related information
{
// set actuator_plugin to the plugin instance ID
std::vector<std::vector<int>> plugin_to_actuators(nplugin);
for (int i = 0; i < nu; ++i) {
if (actuators[i]->is_plugin) {
m->actuator_plugin[i] = actuators[i]->plugin_instance->id;
int actuator_plugin = actuators[i]->plugin_instance->id;
m->actuator_plugin[i] = actuator_plugin;
plugin_to_actuators[actuator_plugin].push_back(i);
} else {
m->actuator_plugin[i] = -1;
}
@@ -3041,6 +3041,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
this->nsensordata += nsensordata;
}
}
if ((plugin->capabilityflags & mjPLUGIN_ACTUATOR) && plugin->actuator_actdim) {
for (int actuator_id : plugin_to_actuators[i]) {
int plugin_actdim = plugin->actuator_actdim(m, i, actuator_id);
actuators[actuator_id]->plugin_actdim = plugin_actdim;
this->na += plugin_actdim;
}
}
}
m->npluginstate = stateadr;
}
src/user/user_objects.cc
+1
View File
@@ -3775,6 +3775,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
forcelimited = 2;
actlimited = 2;
actdim = -1;
plugin_actdim = 0;
trntype = mjTRN_UNDEFINED;
dyntype = mjDYN_NONE;
gaintype = mjGAIN_FIXED;
src/user/user_objects.h
+1
View File
@@ -1099,6 +1099,7 @@ class mjCActuator : public mjCBase {
int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto
int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto
int actdim; // dimension of associated activations
int plugin_actdim; // actuator state size for plugins
mjtDyn dyntype; // dynamics type
mjtTrn trntype; // transmission type
mjtGain gaintype; // gain type
src/xml/xml_native_reader.cc
+11 -3
View File
@@ -413,10 +413,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "9", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{"plugin", "*", "19", "name", "class", "plugin", "instance", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
{"plugin", "*", "24", "name", "class", "plugin", "instance", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
"site", "tendon", "cranksite", "slidersite", "user"},
"site", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user", "actearly"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
@@ -2043,6 +2043,14 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
else if (type == "plugin") {
OnePlugin(elem, pact);
int n;
if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) {
pact->dyntype = (mjtDyn)n;
}
if (MapValue(elem, "actearly", &n, bool_map, 2)) {
pact->actearly = (n==1);
}
ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false);
}
else { // SHOULD NOT OCCUR
src/xml/xml_native_writer.cc
+2 -2
View File
@@ -721,6 +721,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly,
def->actuator.actearly);
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
// plugins: write config attributes
if (pact->is_plugin) {
@@ -736,10 +738,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
int default_actdim = pact->dyntype == mjDYN_NONE ? 0 : 1;
WriteAttrInt(elem, "actdim", pact->actdim, default_actdim);
}
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm);
}