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

Cartesian end-effector control using site transmission.

Browse Source 
- Added `refsite` attribute to actuators with `site` transmission.
- Such actuators now have a well defined length and can be used for e.g., Cartesian end-effector control.
- Added example models and documentation describing the new feature.

PiperOrigin-RevId: 471561218
Change-Id: I538f09af9600d5c53992c4a9fed325648aab4fa3
This commit is contained in:
Yuval Tassa
2022-09-01 10:06:11 -07:00
committed by Copybara-Service
parent 834e8dd506
commit 46da1285af
10 changed files with 283 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+77 -12
View File
@@ -614,6 +614,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mjtNum wrench[6], gearAxis[3];
mjtNum *jac, *jacA, *jacS;
mjtNum *length = d->actuator_length, *moment = d->actuator_moment, *gear;
mjtNum *jacref = NULL, *moment_tmp = NULL; // required for site actuators
mjMARKSTACK;
if (!nu) {
@@ -763,7 +764,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
case mjTRN_TENDON: // tendon
length[i] = d->ten_length[id]*gear[0];
// moment: dense or sparse
// moment: sparse or dense
if (mj_isSparse(m)) {
int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id];
for (int j=d->ten_J_rowadr[id]; j<end; j++) {
@@ -775,20 +776,84 @@ void mj_transmission(const mjModel* m, mjData* d) {
break;
case mjTRN_SITE: // site
// cannot compute meaningful length, set to 0
length[i] = 0;
// get site translation and rotation global Jacobians
// get site translation (jac) and rotation (jacS) Jacobians in global frame
mj_jacSite(m, d, jac, jacS, id);
// wrench: site gear vector in global coordinates
mju_mulMatVec(wrench, d->site_xmat+9*id, gear, 3, 3); // translation
mju_mulMatVec(wrench+3, d->site_xmat+9*id, gear+3, 3, 3); // rotation
// reference site undefined
if (m->actuator_trnid[2*i+1] == -1) {
// cannot compute meaningful length, set to 0
length[i] = 0;
// wrench: gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*id); // translation
mju_rotVecMat(wrench+3, gear+3, d->site_xmat+9*id); // rotation
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation
mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation
mju_addTo(moment+i*nv, jac, nv); // add the two
}
// reference site defined
else {
int refid = m->actuator_trnid[2*i+1];
if (!jacref) jacref = mj_stackAlloc(d, 3*nv);
// clear length
length[i] = 0;
// translational transmission
if (!mju_isZero(gear, 3)) {
// vec: site position in reference site frame
mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*refid);
mju_rotVecMatT(vec, vec, d->site_xmat+9*refid);
// length: dot product with gear
length[i] += mju_dot3(vec, gear);
// jacref: global Jacobian of reference site
mj_jacSite(m, d, jacref, NULL, refid);
// subtract jacref from jac
mju_subFrom(jac, jacref, 3*nv);
// wrench: translational gear expressed in global frame
mju_rotVecMat(wrench, gear, d->site_xmat+9*refid);
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv);
}
// rotational transmission
if (!mju_isZero(gear+3, 3)) {
mjtNum refquat[4];
// get site and refsite quats from parent bodies (avoiding mju_mat2Quat)
mju_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]);
mju_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]);
// convert difference to expmap (axis-angle)
mju_subQuat(vec, quat, refquat);
// add length: dot product with gear
length[i] += mju_dot3(vec, gear+3);
// jacref: global rotational Jacobian of reference site
mj_jacSite(m, d, NULL, jacref, refid);
// subtract jacref from jacS
mju_subFrom(jacS, jacref, 3*nv);
// wrench: rotational gear expressed in global frame
mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);
// moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mj_stackAlloc(d, nv);
mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv);
mju_addTo(moment+i*nv, moment_tmp, nv);
}
}
// moment: global Jacobian projected on wrench
mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation
mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation
mju_addTo(moment+i*nv, jac, nv); // add the two
break;
case mjTRN_BODY: // body (adhesive contacts)
src/user/user_objects.cc
+11 -1
View File
@@ -3358,6 +3358,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
cranklength = 0;
target.clear();
slidersite.clear();
refsite.clear();
userdata.clear();
// clear private variables
@@ -3499,7 +3500,16 @@ void mjCActuator::Compile(void) {
break;
case mjTRN_SITE:
// get site
// get refsite, copy into trnid[1]
if (!refsite.empty()) {
ptarget = model->FindObject(mjOBJ_SITE, refsite);
if (!ptarget) {
throw mjCError(this, "reference site '%s' not found for actuator %d", refsite.c_str(), id);
}
trnid[1] = ptarget->id;
}
// proceed with regular site target
ptarget = model->FindObject(mjOBJ_SITE, target);
break;
src/user/user_objects.h
+1
View File
@@ -863,6 +863,7 @@ class mjCActuator : public mjCBase {
std::vector<double> userdata; // user data
std::string target; // transmission target name
std::string slidersite; // site defining cylinder, for slider-crank only
std::string refsite; // reference site, for site transmission only
private:
mjCActuator(mjCModel* = 0, mjCDef* = 0);// constructor
src/xml/xml_native_reader.cc
+21 -15
View File
@@ -265,40 +265,40 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "26", "name", "class", "group",
{"general", "*", "27", "name", "class", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "body",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "*", "17", "name", "class", "group",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"body", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "*", "18", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site"},
{"position", "*", "18", "name", "class", "group",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
{"position", "*", "19", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp"},
{"velocity", "*", "18", "name", "class", "group",
{"velocity", "*", "19", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"intvelocity", "*", "19", "name", "class", "group",
{"intvelocity", "*", "20", "name", "class", "group",
"ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "lengthrange",
"gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp"},
{"damper", "*", "17", "name", "class", "group",
{"damper", "*", "18", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"cylinder", "*", "21", "name", "class", "group",
{"cylinder", "*", "22", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"timeconst", "area", "diameter", "bias"},
{"muscle", "*", "25", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -1439,6 +1439,12 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
throw mjXError(elem, "cranklength and slidersite can only be used in slidercrank transmission");
}
// site-specific parameters (refsite)
int r3 = ReadAttrTxt(elem, "refsite", pact->refsite);
if (r3 && pact->trntype!=mjTRN_SITE && pact->trntype!=mjTRN_UNDEFINED) {
throw mjXError(elem, "refsite can only be used with site transmission");
}
// get predefined type
type = elem->Value();
src/xml/xml_native_writer.cc
+1
View File
@@ -566,6 +566,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
case mjTRN_SITE:
WriteAttrTxt(elem, "site", pact->target);
WriteAttrTxt(elem, "refsite", pact->refsite);
break;
case mjTRN_BODY: