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/engine/engine_io.c
+5
View File
@@ -1773,6 +1773,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_TENDONLIMITPOS:
case mjSENS_TENDONLIMITVEL:
case mjSENS_TENDONLIMITFRC:
case mjSENS_GEOMDIST:
case mjSENS_CLOCK:
return 1;
@@ -1797,8 +1798,12 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_SUBTREECOM:
case mjSENS_SUBTREELINVEL:
case mjSENS_SUBTREEANGMOM:
case mjSENS_GEOMNORMAL:
return 3;
case mjSENS_GEOMFROMTO:
return 6;
case mjSENS_BALLQUAT:
case mjSENS_FRAMEQUAT:
return 4;
src/engine/engine_ray.c
+4 -5
View File
@@ -1137,14 +1137,13 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
//---------------------------- main entry point ---------------------------------------------------
//---------------------------- main entry point ----------------------------------------------------
// intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude
// return geomid and distance (x) to nearest surface, or -1 if no intersection
// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1]) {
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) {
mjtNum dist, newdist;
// check vector length
@@ -1154,7 +1153,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
// clear result
dist = -1;
*geomid = -1;
if (geomid) *geomid = -1;
// loop over geoms not eliminated by mask and bodyexclude
for (int i=0; i < m->ngeom; i++) {
@@ -1177,7 +1176,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
// update if closer intersection found
if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist;
*geomid = i;
if (geomid) *geomid = i;
}
}
}
src/engine/engine_sensor.c
+118 -16
View File
@@ -40,6 +40,11 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process sensors matching stage and having positive cutoff
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) {
// skip fromto sensors
if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
continue;
}
// get sensor info
int adr = m->sensor_adr[i];
int dim = m->sensor_dim[i];
@@ -214,9 +219,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
// position-dependent sensors
void mj_sensorPos(const mjModel* m, mjData* d) {
int rgeomid, objtype, objid, reftype, refid, adr, offset, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
int nusersensor = 0;
// disabled sensors: return
if (mjDISABLED(mjDSBL_SENSOR)) {
@@ -224,22 +228,26 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
}
// process sensors matching stage
for (int i=0; i < m->nsensor; i++) {
for (int i=0; i < nsensor; i++) {
mjtSensor type = (mjtSensor) m->sensor_type[i];
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
if (type == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i] == mjSTAGE_POS) {
// get sensor info
objtype = m->sensor_objtype[i];
objid = m->sensor_objid[i];
refid = m->sensor_refid[i];
reftype = m->sensor_reftype[i];
adr = m->sensor_adr[i];
int objtype = m->sensor_objtype[i];
int objid = m->sensor_objid[i];
int refid = m->sensor_refid[i];
int reftype = m->sensor_reftype[i];
int adr = m->sensor_adr[i];
mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
// process according to type
switch ((mjtSensor) m->sensor_type[i]) {
switch (type) {
case mjSENS_MAGNETOMETER: // magnetometer
mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
break;
@@ -255,7 +263,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
rvec[1] = d->site_xmat[9*objid+5];
rvec[2] = d->site_xmat[9*objid+8];
d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
m->site_bodyid[objid], &rgeomid);
m->site_bodyid[objid], NULL);
break;
case mjSENS_JOINTPOS: // jointpos
@@ -303,11 +312,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame unspecified: global frame
if (refid == -1) {
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
if (type == mjSENS_FRAMEPOS) {
mju_copy3(d->sensordata+adr, xpos);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
int offset = type - mjSENS_FRAMEXAXIS;
d->sensordata[adr] = xmat[offset];
d->sensordata[adr+1] = xmat[offset+3];
d->sensordata[adr+2] = xmat[offset+6];
@@ -317,12 +326,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame specified
else {
get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
if (type == mjSENS_FRAMEPOS) {
mju_sub3(rvec, xpos, xpos_ref);
mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
int offset = type - mjSENS_FRAMEXAXIS;
mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]};
mju_rotVecMatT(d->sensordata+adr, axis, xmat_ref);
}
@@ -354,6 +363,99 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
break;
case mjSENS_GEOMDIST: // signed distance between two geoms
case mjSENS_GEOMNORMAL: // normal direction between two geoms
case mjSENS_GEOMFROMTO: // segment between two geoms
{
// use cutoff for collision margin
mjtNum margin = m->sensor_cutoff[i];
// initialize outputs
mjtNum dist = margin; // collision distance
mjtNum fromto[6] = {0}; // segment between geoms
// get lists of geoms to collide
int n1, id1;
if (objtype == mjOBJ_BODY) {
n1 = m->body_geomnum[objid];
id1 = m->body_geomadr[objid];
} else {
n1 = 1;
id1 = objid;
}
int n2, id2;
if (reftype == mjOBJ_BODY) {
n2 = m->body_geomnum[refid];
id2 = m->body_geomadr[refid];
} else {
n2 = 1;
id2 = refid;
}
// collide all pairs
for (int geom1=id1; geom1 < id1+n1; geom1++) {
for (int geom2=id2; geom2 < id2+n2; geom2++) {
mjtNum fromto_new[6] = {0};
mjtNum dist_new = mj_geomDistance(m, d, geom1, geom2, margin, fromto_new);
if (dist_new < dist) {
dist = dist_new;
mju_copy(fromto, fromto_new, 6);
}
}
}
// write sensordata for this sensor and all subsequent sensors with identical signature
int write_sensor = 1;
while (write_sensor) {
// write geom distance
if (type == mjSENS_GEOMDIST) {
d->sensordata[adr] = dist;
}
// write distance normal
else if (type == mjSENS_GEOMNORMAL) {
mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]};
if (normal[0] || normal[1] || normal[2]) {
mju_normalize3(normal);
}
mju_copy3(d->sensordata + adr, normal);
}
// write distance fromto
else {
mju_copy(d->sensordata + adr, fromto, 6);
}
// if this is the last sensor, break
if (i+1 == nsensor) {
break;
}
// type of the next sensor
mjtSensor type_next = m->sensor_type[i+1];
// check if signature of next sensor matches this sensor
write_sensor = (type_next == mjSENS_GEOMDIST ||
type_next == mjSENS_GEOMNORMAL ||
type_next == mjSENS_GEOMFROMTO) &&
m->sensor_objtype[i+1] == objtype &&
m->sensor_objid[i+1] == objid &&
m->sensor_reftype[i+1] == reftype &&
m->sensor_refid[i+1] == refid &&
m->sensor_cutoff[i+1] == margin;
// if signature matches, increment external loop variable i
if (write_sensor) {
i++;
// update adr and type, everything else is the same
adr = m->sensor_adr[i];
type = type_next;
}
}
}
break;
case mjSENS_CLOCK: // clock
d->sensordata[adr] = d->time;
break;
src/engine/engine_support.c
+43
View File
@@ -20,6 +20,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
@@ -1703,6 +1704,48 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
//-------------------------- miscellaneous ---------------------------------------------------------
// returns the smallest distance between two geoms
mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, mjtNum distmax,
mjtNum fromto[6]) {
mjContact con[mjMAXCONPAIR];
mjtNum dist = distmax;
if (fromto) mju_zero(fromto, 6);
// flip geom order if required
int flip = m->geom_type[geom1] > m->geom_type[geom2];
int g1 = flip ? geom2 : geom1;
int g2 = flip ? geom1 : geom2;
int type1 = m->geom_type[g1];
int type2 = m->geom_type[g2];
// call collision function if it exists
if (!mjCOLLISIONFUNC[type1][type2]) {
return dist;
}
int num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, distmax);
// find smallest distance
int smallest = -1;
for (int i=0; i < num; i++) {
mjtNum dist_i = con[i].dist;
if (dist_i < dist) {
dist = dist_i;
smallest = i;
}
}
// write fromto if given and a collision has been found
if (fromto && smallest >= 0) {
mjtNum sign = flip ? -1 : 1;
mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist);
mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist);
}
return dist;
}
// extract 6D force:torque for one contact, in contact frame
void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) {
mjContact* con;
src/engine/engine_support.h
+4
View File
@@ -189,6 +189,10 @@ MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
//-------------------------- miscellaneous ---------------------------------------------------------
// returns the smallest distance between two geoms
MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
mjtNum distmax, mjtNum fromto[6]);
// extract 6D force:torque for one contact, in contact frame
MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
src/engine/engine_vis_visualize.c
+14
View File
@@ -1974,6 +1974,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
FINISH
} else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
// sensor data
mjtNum* fromto = d->sensordata + m->sensor_adr[i];
// null output: nothing to render
if (mju_isZero(fromto, 6)) {
continue;
}
// make ray
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
FINISH
}
}
}
src/user/user_objects.cc
+40 -12
View File
@@ -5388,7 +5388,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
((mjCGeom*)obj)->SetNotVisual();
}
// get sensorized object id
} else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) {
throw mjCError(this, "invalid type in sensor");
}
@@ -5402,7 +5401,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
// find name
if (!ref) {
throw mjCError(this, "unrecognized name '%s' of reference frame object", refname_.c_str());
throw mjCError(this, "unrecognized name '%s' of object", refname_.c_str());
}
// must be attached to object with spatial frame
@@ -5460,7 +5459,7 @@ void mjCSensor::Compile(void) {
case mjSENS_CAMPROJECTION:
// must be attached to site
if (objtype!=mjOBJ_SITE) {
throw mjCError(this, "sensor must be attached to site: sensor");
throw mjCError(this, "sensor must be attached to site");
}
// set dim and datatype
@@ -5498,7 +5497,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTACTFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint is slide or hinge
@@ -5522,7 +5521,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TENDONVEL:
// must be attached to tendon
if (objtype!=mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon: sensor");
throw mjCError(this, "sensor must be attached to tendon");
}
// set
@@ -5540,7 +5539,7 @@ void mjCSensor::Compile(void) {
case mjSENS_ACTUATORFRC:
// must be attached to actuator
if (objtype!=mjOBJ_ACTUATOR) {
throw mjCError(this, "sensor must be attached to actuator: sensor");
throw mjCError(this, "sensor must be attached to actuator");
}
// set
@@ -5559,7 +5558,7 @@ void mjCSensor::Compile(void) {
case mjSENS_BALLANGVEL:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint is ball
@@ -5584,7 +5583,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTLIMITFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint has limit
@@ -5609,7 +5608,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TENDONLIMITFRC:
// must be attached to tendon
if (objtype!=mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon: sensor");
throw mjCError(this, "sensor must be attached to tendon");
}
// make sure tendon has limit
@@ -5675,7 +5674,7 @@ void mjCSensor::Compile(void) {
case mjSENS_SUBTREEANGMOM:
// must be attached to body
if (objtype!=mjOBJ_BODY) {
throw mjCError(this, "sensor must be attached to body: sensor");
throw mjCError(this, "sensor must be attached to body");
}
// set
@@ -5688,6 +5687,34 @@ void mjCSensor::Compile(void) {
}
break;
case mjSENS_GEOMDIST:
case mjSENS_GEOMNORMAL:
case mjSENS_GEOMFROMTO:
// must be attached to body or geom
if ((objtype!=mjOBJ_BODY && objtype!=mjOBJ_GEOM) ||
(reftype!=mjOBJ_BODY && reftype!=mjOBJ_GEOM)) {
throw mjCError(this, "sensor must be attached to body or geom");
}
// objects must be different
if (objtype == reftype && obj == ref) {
throw mjCError(this, "1st body/geom must be different from 2nd body/geom");
}
// set
needstage = mjSTAGE_POS;
if (type==mjSENS_GEOMDIST) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type==mjSENS_GEOMNORMAL) {
dim = 3;
datatype = mjDATATYPE_AXIS;
} else {
dim = 6;
datatype = mjDATATYPE_REAL;
}
break;
case mjSENS_CLOCK:
dim = 1;
needstage = mjSTAGE_POS;
@@ -5705,7 +5732,7 @@ void mjCSensor::Compile(void) {
throw mjCError(this,
"datatype AXIS requires dim=3 in sensor");
}
if (datatype==mjDATATYPE_QUATERNION && dim!=4) {
if (datatype==mjDATATYPE_QUATERNION && dim != 4) {
throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor");
}
break;
@@ -5737,7 +5764,8 @@ void mjCSensor::Compile(void) {
}
// check cutoff for incompatible data types
if (cutoff>0 && (datatype==mjDATATYPE_AXIS || datatype==mjDATATYPE_QUATERNION)) {
if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
(datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
throw mjCError(this, "cutoff applied to axis or quaternion datatype in sensor");
}
}
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;
src/xml/xml_native_reader.h
+1 -1
View File
@@ -99,7 +99,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 227
#define nMJCF 230
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+51 -6
View File
@@ -1875,46 +1875,82 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "framepos");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEQUAT:
elem = InsertEnd(section, "framequat");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEXAXIS:
elem = InsertEnd(section, "framexaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEYAXIS:
elem = InsertEnd(section, "frameyaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEZAXIS:
elem = InsertEnd(section, "framezaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMELINVEL:
elem = InsertEnd(section, "framelinvel");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEANGVEL:
elem = InsertEnd(section, "frameangvel");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMELINACC:
elem = InsertEnd(section, "framelinacc");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEANGACC:
elem = InsertEnd(section, "frameangacc");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
// sensors related to kinematic subtrees; attached to a body (which is the subtree root)
@@ -1930,6 +1966,21 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "subtreeangmom");
WriteAttrTxt(elem, "body", psen->get_objname());
break;
case mjSENS_GEOMDIST:
elem = InsertEnd(section, "distance");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
case mjSENS_GEOMNORMAL:
elem = InsertEnd(section, "normal");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
case mjSENS_GEOMFROMTO:
elem = InsertEnd(section, "fromto");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
// global sensors
case mjSENS_CLOCK:
@@ -1968,12 +2019,6 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttr(elem, "noise", 1, &psen->noise, &zero);
}
WriteVector(elem, "user", psen->get_userdata());
// add reference if present
if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
}
// remove section if empty