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

Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

Browse Source 
PiperOrigin-RevId: 632458301
Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3
This commit is contained in:
Yuval Tassa
2024-05-10 04:51:46 -07:00
committed by Copybara-Service
parent 31de1dad9d
commit 02d015458c
29 changed files with 1002 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+26
View File
@@ -472,6 +472,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"clock", "*", "4", "name", "cutoff", "noise", "user"},
{"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage",
"dim", "cutoff", "noise", "user"},
@@ -3949,6 +3952,29 @@ void mjXReader::Sensor(XMLElement* section) {
ReadAttrTxt(elem, "body", objname, true);
}
// sensors for geometric distance; attached to geoms or bodies
else if (type=="distance" || type=="normal" || type=="fromto") {
bool has_body1 = ReadAttrTxt(elem, "body1", objname);
bool has_geom1 = ReadAttrTxt(elem, "geom1", objname);
if (has_body1 == has_geom1) {
throw mjXError(elem, "exactly one of (geom1, body1) must be specified");
}
psen->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM;
bool has_body2 = ReadAttrTxt(elem, "body2", refname);
bool has_geom2 = ReadAttrTxt(elem, "geom2", refname);
if (has_body2 == has_geom2) {
throw mjXError(elem, "exactly one of (geom2, body2) must be specified");
}
psen->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM;
if (type=="distance") {
psen->type = mjSENS_GEOMDIST;
} else if (type=="normal") {
psen->type = mjSENS_GEOMNORMAL;
} else {
psen->type = mjSENS_GEOMFROMTO;
}
}
// global sensors
else if (type=="clock") {
psen->type = mjSENS_CLOCK;