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

Improvements related to models where joint-actuator relationship is not one-to-one:

Browse Source 
- Add `joint-actuatorforcerange` for clamping total actuator force at joints. Add `sensor-jointactuatorfrc` for sensing total actuator forces on a single joint.
See [documentation](https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping) for justification and use cases.
- Add simple car model to `model/`.
- Move actuation-related test models into `engine/testdata/actuation/`.

PiperOrigin-RevId: 549355941
Change-Id: I27f6c1f80426d73a2811ef5ae74684a228b2fbd1
This commit is contained in:
Yuval Tassa
2023-07-19 10:28:24 -07:00
committed by Copybara-Service
parent 7603b07a20
commit 51aa375af0
29 changed files with 453 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+10
View File
@@ -301,6 +301,16 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// clamp qfrc_actuator
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
if (m->jnt_actfrclimited[i]) {
mjtNum *forcerange = m->jnt_actfrcrange + 2*i;
mjtNum *qfrc = d->qfrc_actuator + m->jnt_dofadr[i];
qfrc[0] = mju_clip(qfrc[0], forcerange[0], forcerange[1]);
}
}
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
src/engine/engine_io.c
+1
View File
@@ -1471,6 +1471,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_ACTUATORPOS:
case mjSENS_ACTUATORVEL:
case mjSENS_ACTUATORFRC:
case mjSENS_JOINTACTFRC:
case mjSENS_JOINTLIMITPOS:
case mjSENS_JOINTLIMITVEL:
case mjSENS_JOINTLIMITFRC:
src/engine/engine_sensor.c
+5
View File
@@ -593,6 +593,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
if (rnePost == 0 &&
type != mjSENS_TOUCH &&
type != mjSENS_ACTUATORFRC &&
type != mjSENS_JOINTACTFRC &&
type != mjSENS_JOINTLIMITFRC &&
type != mjSENS_TENDONLIMITFRC) {
// compute cacc, cfrc_int, cfrc_ext
@@ -686,6 +687,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
d->sensordata[adr] = d->actuator_force[objid];
break;
case mjSENS_JOINTACTFRC: // jointactfrc
d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
break;
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
d->sensordata[adr] = 0;
for (int j=ne+nf; j < nefc; j++) {
src/user/user_model.cc
+2
View File
@@ -1407,6 +1407,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_type[jid] = pj->type;
m->jnt_group[jid] = pj->group;
m->jnt_limited[jid] = pj->limited;
m->jnt_actfrclimited[jid] = pj->actfrclimited;
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
@@ -1414,6 +1415,7 @@ void mjCModel::CopyTree(mjModel* m) {
copyvec(m->jnt_axis+3*jid, pj->locaxis, 3);
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness;
copyvec(m->jnt_range+2*jid, pj->range, 2);
copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP);
m->jnt_margin[jid] = (mjtNum)pj->margin;
src/user/user_objects.cc
+28 -1
View File
@@ -1059,9 +1059,12 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) {
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(axis, 0, 0, 1);
limited = 2;
actfrclimited = 2;
stiffness = 0;
range[0] = 0;
range[1] = 0;
actfrcrange[0] = 0;
actfrcrange[1] = 0;
springdamper[0] = 0;
springdamper[1] = 0;
mj_defaultSolRefImp(solref_limit, solimp_limit);
@@ -1146,6 +1149,27 @@ int mjCJoint::Compile(void) {
}
}
// actuator force range: none for free or ball joints
if (type==mjJNT_FREE || type==mjJNT_BALL) {
actfrclimited = 0;
}
// otherwise if actfrclimited is auto, set according to whether actfrcrange is specified
else if (actfrclimited==2) {
bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0);
checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange);
actfrclimited = hasrange ? 1 : 0;
}
// resolve actuator force range limits
if (actfrclimited) {
// check data
if (actfrcrange[0]>=actfrcrange[1]) {
throw mjCError(this,
"actfrcrange[0] should be smaller than actfrcrange[1] in joint '%s' (id = %d)",
name.c_str(), id);
}
}
// FREE or BALL: set axis to (0,0,1)
if (type==mjJNT_FREE || type==mjJNT_BALL) {
axis[0] = axis[1] = 0;
@@ -4024,6 +4048,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTPOS:
case mjSENS_JOINTVEL:
case mjSENS_JOINTACTFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this,
@@ -4041,8 +4066,10 @@ void mjCSensor::Compile(void) {
datatype = mjDATATYPE_REAL;
if (type==mjSENS_JOINTPOS) {
needstage = mjSTAGE_POS;
} else {
} else if (type==mjSENS_JOINTVEL) {
needstage = mjSTAGE_VEL;
} else if (type==mjSENS_JOINTACTFRC) {
needstage = mjSTAGE_ACC;
}
break;
src/user/user_objects.h
+2
View File
@@ -296,11 +296,13 @@ class mjCJoint : public mjCBase {
mjtJoint type; // type of Joint
int group; // used for rendering
int limited; // does joint have limits: 0 false, 1 true, 2 auto
int actfrclimited; // are actuator forces on joints limited: 0 false, 1 true, 2 auto
double pos[3]; // anchor position
double axis[3]; // joint axis
double stiffness; // stiffness coefficient
double springdamper[2]; // timeconst, dampratio
double range[2]; // joint limits
double actfrcrange[2]; // actuator force limits
mjtNum solref_limit[mjNREF]; // solver reference: joint limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits
mjtNum solref_friction[mjNREF]; // solver reference: dof friction
src/xml/xml_native_reader.cc
+16 -9
View File
@@ -79,7 +79,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 190;
static const int nMJCF = 191;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -138,10 +138,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mesh", "?", "1", "scale"},
{"material", "?", "8", "texture", "emission", "specular", "shininess",
"reflectance", "rgba", "texrepeat", "texuniform"},
{"joint", "?", "19", "type", "group", "pos", "axis", "springdamper",
"limited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "margin",
"ref", "springref", "armature", "damping", "frictionloss", "user"},
{"joint", "?", "21", "type", "group", "pos", "axis", "springdamper",
"limited", "actuatorforcelimited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "actuatorforcerange",
"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",
@@ -237,11 +237,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{">"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "21", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited",
{"joint", "*", "23", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited", "actuatorforcelimited",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"stiffness", "range", "margin", "ref", "springref", "armature", "damping",
"frictionloss", "user"},
"stiffness", "range", "actuatorforcerange", "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",
@@ -391,6 +391,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuatorpos", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -1314,6 +1315,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) {
pjoint->type = (mjtJoint)n;
}
MapValue(elem, "limited", &pjoint->limited, TFAuto_map, 3);
MapValue(elem, "actuatorforcelimited", &pjoint->actfrclimited, TFAuto_map, 3);
ReadAttrInt(elem, "group", &pjoint->group);
ReadAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, text, false, false);
ReadAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, text, false, false);
@@ -1324,6 +1326,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) {
ReadAttr(elem, "springdamper", 2, pjoint->springdamper, text);
ReadAttr(elem, "stiffness", 1, &pjoint->stiffness, text);
ReadAttr(elem, "range", 2, pjoint->range, text);
ReadAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, text);
ReadAttr(elem, "margin", 1, &pjoint->margin, text);
ReadAttr(elem, "ref", 1, &pjoint->ref, text);
ReadAttr(elem, "springref", 1, &pjoint->springref, text);
@@ -3004,6 +3007,10 @@ void mjXReader::Sensor(XMLElement* section) {
psen->type = mjSENS_ACTUATORFRC;
psen->objtype = mjOBJ_ACTUATOR;
ReadAttrTxt(elem, "actuator", psen->objname, true);
} else if (type=="jointactuatorfrc") {
psen->type = mjSENS_JOINTACTFRC;
psen->objtype = mjOBJ_JOINT;
ReadAttrTxt(elem, "joint", psen->objname, true);
}
// sensors related to ball joints
src/xml/xml_native_writer.cc
+11
View File
@@ -225,6 +225,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
}
bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined;
if (writingdefaults || !aflimited_inferred) {
WriteAttrKey(elem, "actutorforcelimited", TFAuto_map, 3,
pjoint->actfrclimited, def->joint.actfrclimited);
}
// defaults and regular
if (pjoint->type != def->joint.type) {
@@ -239,6 +245,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
WriteAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);
WriteAttr(elem, "damping", 1, &pjoint->damping, &def->joint.damping);
@@ -1645,6 +1652,10 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "actuatorfrc");
WriteAttrTxt(elem, "actuator", psen->objname);
break;
case mjSENS_JOINTACTFRC:
elem = InsertEnd(section, "jointactuatorfrc");
WriteAttrTxt(elem, "joint", psen->objname);
break;
// sensors related to ball joints
case mjSENS_BALLQUAT: