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_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;