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
}
}
}