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

Add tendon actuator force limits and tendon actuator force sensor.

Browse Source 
PiperOrigin-RevId: 745096883
Change-Id: Ib9acb727fbbfc6b0b0323ee6a889053a7a878056
This commit is contained in:
Taylor Howell
2025-04-08 05:15:27 -07:00
committed by Copybara-Service
parent 16e49f2761
commit 96dda6ea75
26 changed files with 464 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+41 -1
View File
@@ -276,7 +276,7 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu;
int nv = m->nv, nu = m->nu, ntendon = m->ntendon;
mjtNum gain, bias, tau;
mjtNum *prm, *force = d->actuator_force;
@@ -289,6 +289,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
return;
}
// any tendon transmission targets with force limits
int tendon_frclimited = 0;
// local, clamped copy of ctrl
mj_markStack(d);
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
@@ -384,6 +387,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
continue;
}
// check for tendon transmission with force limits
if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) {
tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
}
// extract gain info
prm = m->actuator_gainprm + mjNGAIN*i;
@@ -479,6 +487,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
}
// clamp tendon total actuator force
if (tendon_frclimited) {
// compute total force for each tendon
mjtNum* tendon_total_force = mjSTACKALLOC(d, ntendon, mjtNum);
mju_zero(tendon_total_force, ntendon);
for (int i=0; i < nu; i++) {
if (m->actuator_trntype[i] == mjTRN_TENDON) {
int tendon_id = m->actuator_trnid[2*i];
if (m->tendon_actfrclimited[tendon_id]) {
tendon_total_force[tendon_id] += force[i];
}
}
}
// scale tendon actuator forces if limited and outside range
for (int i=0; i < nu; i++) {
if (m->actuator_trntype[i] != mjTRN_TENDON) {
continue;
}
int tendon_id = m->actuator_trnid[2*i];
mjtNum tendon_force = tendon_total_force[tendon_id];
if (m->tendon_actfrclimited[tendon_id] && tendon_force) {
const mjtNum* range = m->tendon_actfrcrange + 2 * tendon_id;
if (tendon_force < range[0]) {
force[i] *= range[0] / tendon_force;
} else if (tendon_force > range[1]) {
force[i] *= range[1] / tendon_force;
}
}
}
}
// clamp actuator_force
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
src/engine/engine_io.c
+1
View File
@@ -2079,6 +2079,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_ACTUATORVEL:
case mjSENS_ACTUATORFRC:
case mjSENS_JOINTACTFRC:
case mjSENS_TENDONACTFRC:
case mjSENS_JOINTLIMITPOS:
case mjSENS_JOINTLIMITVEL:
case mjSENS_JOINTLIMITFRC:
src/engine/engine_sensor.c
+12 -2
View File
@@ -698,8 +698,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// acceleration/force-dependent sensors
void mj_sensorAcc(const mjModel* m, mjData* d) {
int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum tmp[6], conforce[6], conray[3];
int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
mjtNum tmp[6], conforce[6], conray[3], frc;
mjContact* con;
// disabled sensors: return
@@ -825,6 +825,16 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
break;
case mjSENS_TENDONACTFRC: // tendonactfrc
frc = 0.0;
for (int j=0; j < nu; j++) {
if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) {
frc += d->actuator_force[j];
}
}
d->sensordata[adr] = frc;
break;
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
d->sensordata[adr] = 0;
for (int j=ne+nf; j < nefc; j++) {
src/user/user_model.cc
+4
View File
@@ -3351,6 +3351,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_matid[i] = pte->matid;
m->tendon_group[i] = pte->group;
m->tendon_limited[i] = (mjtByte)pte->is_limited();
m->tendon_actfrclimited[i] = (mjtByte)pte->is_actfrclimited();
m->tendon_width[i] = (mjtNum)pte->width;
mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
@@ -3358,6 +3359,8 @@ void mjCModel::CopyObjects(mjModel* m) {
mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
m->tendon_range[2*i] = (mjtNum)pte->range[0];
m->tendon_range[2*i+1] = (mjtNum)pte->range[1];
m->tendon_actfrcrange[2*i] = (mjtNum)pte->actfrcrange[0];
m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
m->tendon_margin[i] = (mjtNum)pte->margin;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
m->tendon_damping[i] = (mjtNum)pte->damping;
@@ -5005,6 +5008,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// tendons
for (int i=0; i < ntendon; i++) {
mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
mjuu_copyvec(tendons_[i]->actfrcrange, m->tendon_actfrcrange+2*i, 2);
mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);
src/user/user_objects.cc
+30 -1
View File
@@ -5388,7 +5388,9 @@ mjCTendon& mjCTendon::operator=(const mjCTendon& other) {
bool mjCTendon::is_limited() const {
return islimited(limited, range);
}
bool mjCTendon::is_actfrclimited() const {
return islimited(actfrclimited, actfrcrange);
}
void mjCTendon::PointToLocal() {
spec.element = static_cast<mjsElement*>(this);
@@ -5686,6 +5688,21 @@ void mjCTendon::Compile(void) {
throw mjCError(this, "invalid limits in tendon");
}
// if limited is auto, set to 1 if range is specified, otherwise unlimited
if (actfrclimited == mjLIMITED_AUTO) {
bool hasactfrcrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0);
checklimited(this, compiler->autolimits, "tendon", "", actfrclimited,
hasactfrcrange);
}
// check actfrclimits
if (actfrcrange[0] >= actfrcrange[1] && is_actfrclimited()) {
throw mjCError(this, "invalid actuatorfrcrange in tendon");
}
if ((actfrcrange[0] > 0 || actfrcrange[1] < 0) && is_actfrclimited()) {
throw mjCError(this, "invalid actuatorfrcrange in tendon");
}
// check springlength
if (springlength[0] > springlength[1]) {
throw mjCError(this, "invalid springlength in tendon");
@@ -6479,6 +6496,18 @@ void mjCSensor::Compile(void) {
}
break;
case mjSENS_TENDONACTFRC:
// must be attached to tendon
if (objtype != mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon");
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_ACC;
break;
case mjSENS_TENDONPOS:
case mjSENS_TENDONVEL:
// must be attached to tendon
src/user/user_objects.h
+1
View File
@@ -1523,6 +1523,7 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
void SetModel(mjCModel* _model);
bool is_limited() const;
bool is_actfrclimited() const;
private:
void Compile(void); // compiler
src/xml/xml_native_reader.cc
+11 -4
View File
@@ -363,8 +363,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"tendon", "*", "0"},
{"<"},
{"spatial", "*", "19", "name", "class", "group", "limited", "range",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
{"spatial", "*", "21", "name", "class", "group", "limited", "actuatorfrclimited", "range",
"actuatorfrcrange", "solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"frictionloss", "springlength", "width", "material",
"margin", "stiffness", "damping", "armature", "rgba", "user"},
{"<"},
@@ -372,8 +372,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"geom", "*", "2", "geom", "sidesite"},
{"pulley", "*", "1", "divisor"},
{">"},
{"fixed", "*", "16", "name", "class", "group", "limited", "range",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
{"fixed", "*", "18", "name", "class", "group", "limited", "actuatorfrclimited", "range",
"actuatorfrcrange","solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"frictionloss", "springlength", "margin", "stiffness", "damping", "armature", "user"},
{"<"},
{"joint", "*", "2", "joint", "coef"},
@@ -455,6 +455,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"tendonactuatorfrc", "*", "5", "name", "tendon", "cutoff", "noise", "user"},
{"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -2052,12 +2053,14 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
mjs_setString(tendon->material, material.c_str());
}
MapValue(elem, "limited", &tendon->limited, TFAuto_map, 3);
MapValue(elem, "actuatorfrclimited", &tendon->actfrclimited, TFAuto_map, 3);
ReadAttr(elem, "width", 1, &tendon->width, text);
ReadAttr(elem, "solreflimit", mjNREF, tendon->solref_limit, text, false, false);
ReadAttr(elem, "solimplimit", mjNIMP, tendon->solimp_limit, text, false, false);
ReadAttr(elem, "solreffriction", mjNREF, tendon->solref_friction, text, false, false);
ReadAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, text, false, false);
ReadAttr(elem, "range", 2, tendon->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text);
ReadAttr(elem, "margin", 1, &tendon->margin, text);
ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text);
ReadAttr(elem, "damping", 1, &tendon->damping, text);
@@ -3973,6 +3976,10 @@ void mjXReader::Sensor(XMLElement* section) {
sensor->type = mjSENS_JOINTACTFRC;
sensor->objtype = mjOBJ_JOINT;
ReadAttrTxt(elem, "joint", objname, true);
} else if (type=="tendonactuatorfrc") {
sensor->type = mjSENS_TENDONACTFRC;
sensor->objtype = mjOBJ_TENDON;
ReadAttrTxt(elem, "tendon", objname, true);
}
// sensors related to ball joints
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 237
#define nMJCF 238
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+6
View File
@@ -725,7 +725,9 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def
WriteAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, def->Tendon().solimp_friction,
true);
WriteAttrKey(elem, "limited", TFAuto_map, 3, tendon->limited, def->Tendon().limited);
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, tendon->actfrclimited, def->Tendon().actfrclimited);
WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range);
WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange);
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin);
WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness);
WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping);
@@ -2033,6 +2035,10 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "jointactuatorfrc");
WriteAttrTxt(elem, "joint", sensor->get_objname());
break;
case mjSENS_TENDONACTFRC:
elem = InsertEnd(section, "tendonactuatorfrc");
WriteAttrTxt(elem, "tendon", sensor->get_objname());
break;
// sensors related to ball joints
case mjSENS_BALLQUAT: