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

Add adhesion actuators.

Browse Source 
- Adhesion actuators using contact normals as force transmission mechanism.
- Related video: https://youtu.be/HdBue4MUZys

Closes #229

PiperOrigin-RevId: 464389367
Change-Id: I9f69b3cd152d957e8f65870d208788463c036a6d
This commit is contained in:
Yuval Tassa
2022-07-31 08:53:29 -07:00
committed by Copybara-Service
parent 5c5449bf82
commit 3d77eb1ef4
17 changed files with 475 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+42
View File
@@ -785,6 +785,48 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_addTo(moment+i*nv, jac, nv); // add the two
break;
case mjTRN_BODY: // body (adhesive contacts)
// cannot compute meaningful length, set to 0
length[i] = 0;
// moment is average of all contact normal Jacobians
{
// find and count all relevant contacts, mark them in efc_force
int counter = 0;
mjtNum* efc_force = mj_stackAlloc(d, d->nefc);
mju_zero(efc_force, d->nefc);
for (int j=0; j<d->ncon; j++) {
const mjContact* con = d->contact+j;
if (m->geom_bodyid[con->geom1]==id || m->geom_bodyid[con->geom2]==id) {
if (!con->exclude) {
counter++;
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
}
} else if (con->exclude == 1) {
// TODO(b/240848298): compute Jacobians for excluded contact (in gap)
}
}
}
// moment is average over contact normal Jacobians, make negative for adhesion
if (counter) {
mj_mulJacTVec(m, d, moment+i*nv, efc_force);
mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv);
}
}
break;
default:
mju_error_i("Unknown transmission type %d", m->actuator_trntype[i]); // SHOULD NOT OCCUR
}
src/engine/engine_forward.c
+1 -1
View File
@@ -97,7 +97,6 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_comPos(m, d);
mj_camlight(m, d);
mj_tendon(m, d);
mj_transmission(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
TM_RESTART;
@@ -111,6 +110,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
TM_RESTART;
mj_makeConstraint(m, d);
mj_transmission(m, d);
TM_END(mjTIMER_POS_MAKE);
TM_RESTART;
src/engine/engine_vis_visualize.c
+30 -2
View File
@@ -847,8 +847,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// site actuators
if (m->actuator_trntype[i]==mjTRN_SITE) {
// set size of the geometry
mju_scl3(sz, m->site_size+3*j, 1.1);
// inflate sizes by 5%
mju_scl3(sz, m->site_size+3*j, 1.05);
// make geom
mjv_initGeom(thisgeom,
@@ -895,6 +895,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
FINISH
}
// body actuators
else if (m->actuator_trntype[i]==mjTRN_BODY) {
// iterate over body's geoms
int geomnum = m->body_geomnum[j];
int geomadr = m->body_geomadr[j];
for (int k=geomadr; k<geomadr+geomnum; k++) {
int geomtype = m->geom_type[k];
// add inflated geom if it is a regular primitive
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) {
START
// inflate sizes by 5%
mju_scl3(sz, m->geom_size+3*k, 1.05);
// make geom
mjv_initGeom(thisgeom,
m->geom_type[k], sz,
d->geom_xpos + 3*k,
d->geom_xmat + 9*k,
thisgeom->rgba);
// set interpolated color
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
// spatial tendon actuators
else if (m->actuator_trntype[i]==mjTRN_TENDON && d->ten_wrapnum[j]) {
for (int k=d->ten_wrapadr[j]; k<d->ten_wrapadr[j]+d->ten_wrapnum[j]-1; k++) {
src/user/user_objects.cc
+5
View File
@@ -3461,6 +3461,11 @@ void mjCActuator::Compile(void) {
ptarget = model->FindObject(mjOBJ_SITE, target);
break;
case mjTRN_BODY:
// get body
ptarget = model->FindObject(mjOBJ_BODY, target);
break;
default:
throw mjCError(this, "invalid transmission type in actuator '%s' (id = %d)", name.c_str(), id);
}
src/xml/xml_native_reader.cc
+40 -8
View File
@@ -42,7 +42,7 @@ using tinyxml2::XMLElement;
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 163;
static const int nMJCF = 165;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -145,6 +145,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"gear", "cranklength", "user", "group",
"timeconst", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "?", "6", "forcelimited", "ctrlrange", "forcerange",
"gain", "user", "group"},
{">"},
{"custom", "*", "0"},
@@ -262,10 +264,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "25", "name", "class", "group",
{"general", "*", "26", "name", "class", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "body",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "*", "17", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -303,6 +305,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
"timeconst", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "9", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{">"},
{"sensor", "*", "0"},
@@ -1426,7 +1430,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->trntype = mjTRN_SITE;
cnt++;
}
if (ReadAttrTxt(elem, "body", pact->target)) {
pact->trntype = mjTRN_BODY;
cnt++;
}
// check for repeated transmission
if (cnt>1) {
throw mjXError(elem, "actuator can have at most one of transmission target");
@@ -1522,7 +1529,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
// damper
else if (type=="damper") {
// clear bias
// clear gain
mjuu_zerovec(pact->gainprm, mjNGAIN);
// explicit attributes
@@ -1531,10 +1538,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
throw mjXError(elem, "damping coefficient cannot be negative");
pact->gainprm[2] = -pact->gainprm[2];
// Require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text);
// require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "control range cannot be negative");
throw mjXError(elem, "damper control range cannot be negative");
}
// implied parameters
@@ -1597,6 +1604,31 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->biastype = mjBIAS_MUSCLE;
}
// adhesion
else if (type=="adhesion") {
// clear bias, set default gain
mjuu_zerovec(pact->biasprm, mjNBIAS);
mjuu_zerovec(pact->gainprm, mjNGAIN);
pact->gainprm[0] = 1;
// explicit attributes
ReadAttr(elem, "gain", 1, pact->gainprm, text);
if (pact->gainprm[0]<0)
throw mjXError(elem, "adhesion gain cannot be negative");
// require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "adhesion control range cannot be negative");
}
// implied parameters
pact->ctrllimited = true;
pact->dyntype = mjDYN_NONE;
pact->gaintype = mjGAIN_FIXED;
pact->biastype = mjBIAS_NONE;
}
else { // SHOULD NOT OCCUR
throw mjXError(elem, "unrecognized actuator type: %s", type.c_str());
}
src/xml/xml_native_writer.cc
+4
View File
@@ -554,6 +554,10 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttrTxt(elem, "site", pact->target);
break;
case mjTRN_BODY:
WriteAttrTxt(elem, "body", pact->target);
break;
default: // SHOULD NOT OCCUR
break;
}