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

Add actuatorgravcomp joint attribute, to treat gravity compensation forces as applied by actuators, rather than passive buoyancy.

Browse Source 
PiperOrigin-RevId: 620049593
Change-Id: I2c8a9dc152c087b408e4f904838034271a7dd910
This commit is contained in:
Yuval Tassa
2024-03-28 14:00:55 -07:00
committed by Copybara-Service
parent d258d5e152
commit 47ba72ea59
16 changed files with 257 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+41 -7
View File
@@ -244,7 +244,7 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d,
// clamp vector to range
static void mju_clamp(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
const int* index) {
for (int i=0; i < n; i++) {
int j = index ? index[i] : i;
@@ -277,7 +277,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mjtNum *ctrl = mj_stackAllocNum(d, nu);
mju_copy(ctrl, d->ctrl, nu);
if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_clamp(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
}
// check controls, set all to 0 if any are bad
@@ -463,13 +463,47 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// clamp actuator_force
mju_clamp(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// clamp qfrc_actuator
mju_clamp(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);
// actuator-level gravity compensation
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added as passive force
if (!m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// clamp qfrc_actuator to joint-level actuator force limits
clampVec(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);
mj_freeStack(d);
TM_END(mjTIMER_ACTUATION);
@@ -481,13 +515,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
void mj_fwdAcceleration(const mjModel* m, mjData* d) {
int nv = m->nv;
// qforce = sum of all non-constraint forces
// qfrc_smooth = sum of all non-constraint forces
mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv); // qfrc_bias is negative
mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
mj_xfrcAccumulate(m, d, d->qfrc_smooth);
// qacc_smooth = M \ qfr_smooth
// qacc_smooth = M \ qfrc_smooth
mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
}
src/engine/engine_passive.c
+32 -1
View File
@@ -255,8 +255,39 @@ void mj_passive(const mjModel* m, mjData* d) {
// add passive forces into qfrc_passive
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv);
if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
if (has_gravcomp) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added via actuators
if (m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// user callback: add custom passive forces
if (mjcb_passive) {
src/user/user_api.h
+1
View File
@@ -230,6 +230,7 @@ typedef struct _mjsJoint { // joint specification
// other
int group; // group
mjtByte actgravcomp; // is gravcomp force applied via actuators
double urdfeffort; // effort (urdf)
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
src/user/user_model.cc
+1
View File
@@ -1768,6 +1768,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_group[jid] = pj->group;
m->jnt_limited[jid] = (mjtByte)pj->is_limited();
m->jnt_actfrclimited[jid] = (mjtByte)pj->is_actfrclimited();
m->jnt_actgravcomp[jid] = pj->actgravcomp;
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
src/xml/xml_native_reader.cc
+9 -5
View File
@@ -138,10 +138,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mesh", "?", "1", "scale"},
{"material", "?", "8", "texture", "emission", "specular", "shininess",
"reflectance", "rgba", "texrepeat", "texuniform"},
{"joint", "?", "21", "type", "group", "pos", "axis", "springdamper",
{"joint", "?", "22", "type", "group", "pos", "axis", "springdamper",
"limited", "actuatorfrclimited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "actuatorfrcrange",
"margin", "ref", "springref", "armature", "damping", "frictionloss", "user"},
"actuatorgravcomp", "margin", "ref", "springref", "armature", "damping",
"frictionloss", "user"},
{"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"shellinertia", "solmix", "solref", "solimp",
@@ -240,11 +241,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"<"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "23", "name", "class", "type", "group", "pos", "axis",
{"joint", "*", "24", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited", "actuatorfrclimited",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"stiffness", "range", "actuatorfrcrange", "margin", "ref", "springref",
"armature", "damping", "frictionloss", "user"},
"stiffness", "range", "actuatorfrcrange", "actuatorgravcomp", "margin", "ref",
"springref", "armature", "damping", "frictionloss", "user"},
{"freejoint", "*", "2", "name", "group"},
{"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
@@ -1573,6 +1574,9 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) {
ReadAttr(elem, "armature", 1, &pjoint->armature, text);
ReadAttr(elem, "damping", 1, &pjoint->damping, text);
ReadAttr(elem, "frictionloss", 1, &pjoint->frictionloss, text);
if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) {
pjoint->actgravcomp = (n==1);
}
// read userdata
if (ReadVector(elem, "user", userdata, text)) {
src/xml/xml_native_writer.cc
+1
View File
@@ -307,6 +307,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited,
def->joint.actfrclimited);
WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->joint.actgravcomp);
WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);