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

Add filterexact and actearly options to MuJoCo.

Browse Source 
When filterexact is a new dyntype which is just like the existing `filter`, but the activation state is integrated with exact integration, instead of simple Euler integration.

When actearly is specified on a general actuator, `qfrc_actuator` is computed using the next timestep's `act` value, instead of the current value.

PiperOrigin-RevId: 554438423
Change-Id: If901e4988fa6b518d6f3097f149770665a189a4b
This commit is contained in:
Nimrod Gileadi
2023-08-07 04:49:42 -07:00
committed by Copybara-Service
parent f0f535ed04
commit 2c3297b3e7
17 changed files with 441 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+82 -53
View File
@@ -152,7 +152,31 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
TM_END(mjTIMER_VELOCITY);
}
// returns the next act given the current act_dot, after clamping
static mjtNum nextActivation(const mjModel* m, const mjData* d,
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
if (m->actuator_dyntype[actuator_id] == 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
act = act + act_dot * m->opt.timestep;
}
// clamp to actrange
if (m->actuator_actlimited[actuator_id]) {
mjtNum* actrange = m->actuator_actrange + 2 * actuator_id;
act = mju_clip(act, actrange[0], actrange[1]);
}
return act;
}
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
@@ -196,6 +220,51 @@ 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) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = 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;
break;
case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact)
d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], 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);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
d->act_dot[j] = 0;
}
}
}
// force = gain .* [ctrl/act] + bias
for (int i=0; i < nu; i++) {
// skip actuator plugins -- these are handled after builtin actuator types
@@ -237,7 +306,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
force[i] = gain * ctrl[i];
} else {
// use last activation variable associated with actuator i
force[i] = gain * d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1];
int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
mjtNum act;
if (m->actuator_actearly[i]) {
act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
} else {
act = d->act[act_adr];
}
force[i] = gain * act;
}
// extract bias info
@@ -311,49 +388,6 @@ 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) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = ctrl[i];
break;
case mjDYN_FILTER: // linear filter: prm = tau
tau = mju_max(mjMINVAL, prm[0]);
d->act_dot[j] = (ctrl[i] - d->act[j]) / tau;
break;
case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact)
d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], 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);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
d->act_dot[j] = 0;
}
}
}
mjFREESTACK;
TM_END(mjTIMER_ACTUATION);
}
@@ -514,16 +548,11 @@ static void mj_advance(const mjModel* m, mjData* d,
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
// advance activations and clamp
if (m->na) {
mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
// clamp activations
for (int i=0; i < m->nu; i++) {
int j = m->actuator_actadr[i];
if (j > -1 && m->actuator_actlimited[i]) {
mjtNum* actrange = m->actuator_actrange + 2*i;
for (int k=0; k < m->actuator_actnum[i]; k++) {
d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]);
}
int actadr = m->actuator_actadr[i];
int actadr_end = actadr + m->actuator_actnum[i];
for (int j=actadr; j < actadr_end; j++) {
d->act[j] = nextActivation(m, d, i, j, act_dot[j]);
}
}
}
src/user/user_model.cc
+1
View File
@@ -1962,6 +1962,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->actuator_ctrllimited[i] = pac->ctrllimited;
m->actuator_forcelimited[i] = pac->forcelimited;
m->actuator_actlimited[i] = pac->actlimited;
m->actuator_actearly[i] = pac->actearly;
m->actuator_cranklength[i] = (mjtNum)pac->cranklength;
copyvec(m->actuator_gear + 6*i, pac->gear, 6);
copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN);
src/user/user_objects.h
+1
View File
@@ -1026,6 +1026,7 @@ class mjCActuator : public mjCBase {
double dynprm[mjNDYN]; // dynamics parameters
double gainprm[mjNGAIN]; // gain parameters
double biasprm[mjNGAIN]; // bias parameters
bool actearly = false; // apply activations to qfrc instantly
double ctrlrange[2]; // control range
double forcerange[2]; // force range
double actrange[2]; // activation range
src/xml/xml_native_reader.cc
+11 -5
View File
@@ -40,6 +40,7 @@
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
@@ -160,9 +161,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"frictionloss", "springlength", "width", "material",
"margin", "stiffness", "damping", "rgba", "user"},
{"general", "?", "17", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
{"general", "?", "18", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
"forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
{"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
{"position", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -322,11 +323,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "28", "name", "class", "group",
{"general", "*", "29", "name", "class", "group",
"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"},
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
"actearly"},
{"motor", "*", "18", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -596,11 +598,12 @@ const mjMap mark_map[mark_sz] = {
// dyn type
const int dyn_sz = 5;
const int dyn_sz = 6;
const mjMap dyn_map[dyn_sz] = {
{"none", mjDYN_NONE},
{"integrator", mjDYN_INTEGRATOR},
{"filter", mjDYN_FILTER},
{"filterexact", mjDYN_FILTEREXACT},
{"muscle", mjDYN_MUSCLE},
{"user", mjDYN_USER}
};
@@ -1686,6 +1689,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
if (MapValue(elem, "biastype", &n, bias_map, bias_sz)) {
pact->biastype = (mjtBias)n;
}
if (MapValue(elem, "actearly", &n, bool_map, 2)) {
pact->actearly = (n==1);
}
ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false);
ReadAttr(elem, "gainprm", mjNGAIN, pact->gainprm, text, false, false);
ReadAttr(elem, "biasprm", mjNBIAS, pact->biasprm, text, false, false);
src/xml/xml_native_writer.cc
+4 -2
View File
@@ -14,12 +14,10 @@
#include "xml/xml_native_writer.h"
#include <cfloat>
#include <cstddef>
#include <cstdio>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>
#include <mujoco/mjmodel.h>
@@ -28,8 +26,10 @@
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
@@ -628,6 +628,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange);
WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear);
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly,
def->actuator.actearly);
// plugins: write config attributes
if (pact->is_plugin) {