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

Add additional data fields that can be reported by rangefinder sensors.

Browse Source 
PiperOrigin-RevId: 848316991
Change-Id: Idbf7ba81b4da711a22c23302c8782ab2b0b98d82
This commit is contained in:
Yuval Tassa
2025-12-23 15:31:53 -08:00
committed by Copybara-Service
parent f2e9097ed6
commit 70bc7be4bc
27 changed files with 712 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+32 -5
View File
@@ -662,11 +662,11 @@ static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid,
// triangle normal
mjtNum normal_tri[3];
// first triangle
// first triangle: swap v1 and v2 for consistent CCW winding (normals point up)
mjtNum va[3][3] = {
{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
{dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]}
{dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]},
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]}
};
mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);
if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1458,6 +1458,18 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
AABB[3] = mju_max(AABB[3], elevation);
}
// add distance-dependent angular margin to account for edge/face curvature
// margin = atan(max_half_size / dist) bounds the angular deviation of face centers
mjtNum max_half = mju_max(aabb[3], mju_max(aabb[4], aabb[5]));
mjtNum dist = mju_dist3(pnt, xpos);
if (dist > mjMINVAL) {
mjtNum margin = mju_atan2(max_half, dist);
AABB[0] -= margin;
AABB[1] -= margin;
AABB[2] += margin;
AABB[3] += margin;
}
// azimuth crosses discontinuity, fall back to no angular culling
if (AABB[2]-AABB[0] > mjPI) {
AABB[0] = -mjPI;
@@ -1520,8 +1532,23 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// exclude geom using bounding angles
if (m->body_bvhadr[b] != -1) {
if (azimuth < (geom_ba+4*i)[0] || elevation < (geom_ba+4*i)[1] ||
azimuth > (geom_ba+4*i)[2] || elevation > (geom_ba+4*i)[3]) {
mjtNum az_min = (geom_ba+4*i)[0];
mjtNum az_max = (geom_ba+4*i)[2];
mjtNum el_min = (geom_ba+4*i)[1];
mjtNum el_max = (geom_ba+4*i)[3];
// check elevation
if (elevation < el_min || elevation > el_max) {
continue;
}
// check azimuth with wraparound
mjtNum az_center = (az_min + az_max) * 0.5;
mjtNum az_half_width = (az_max - az_min) * 0.5;
mjtNum az_diff = azimuth - az_center;
if (az_diff > mjPI) az_diff -= 2*mjPI;
else if (az_diff < -mjPI) az_diff += 2*mjPI;
if (mju_abs(az_diff) > az_half_width) {
continue;
}
}
src/engine/engine_sensor.c
+141 -46
View File
@@ -385,6 +385,61 @@ static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3
//-------------------------------- sensor ----------------------------------------------------------
// fill one pixel's worth of rangefinder data, advance ptr
static mjtNum* fill_raydata(mjtNum* ptr, int dataspec, mjtNum dist,
const mjtNum origin[3], const mjtNum direction[3],
const mjtNum normal[3], const mjtNum cam_xpos[3],
const mjtNum cam_z[3]) {
int hit = (dist >= 0);
if (dataspec & (1 << mjRAYDATA_DIST)) {
*ptr++ = dist;
}
if (dataspec & (1 << mjRAYDATA_DIR)) {
if (hit) mju_copy3(ptr, direction);
else mju_zero3(ptr);
ptr += 3;
}
if (dataspec & (1 << mjRAYDATA_ORIGIN)) {
mju_copy3(ptr, origin);
ptr += 3;
}
// compute point if needed for POINT or DEPTH fields
mjtNum point[3] = {0, 0, 0};
if ((dataspec & (1 << mjRAYDATA_POINT)) || (dataspec & (1 << mjRAYDATA_DEPTH))) {
if (hit) mju_addScl3(point, origin, direction, dist);
}
if (dataspec & (1 << mjRAYDATA_POINT)) {
mju_copy3(ptr, point);
ptr += 3;
}
if (dataspec & (1 << mjRAYDATA_NORMAL)) {
if (hit) mju_copy3(ptr, normal);
else mju_zero3(ptr);
ptr += 3;
}
if (dataspec & (1 << mjRAYDATA_DEPTH)) {
if (hit) {
if (cam_z) {
// camera depth: project onto camera z-axis
mjtNum delta[3];
mju_sub3(delta, point, cam_xpos);
*ptr++ = -mju_dot3(delta, cam_z);
} else {
// site sensor: depth = dist
*ptr++ = dist;
}
} else {
*ptr++ = -1;
}
}
return ptr;
}
// position-dependent sensors
void mj_sensorPos(const mjModel* m, mjData* d) {
int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
@@ -435,58 +490,98 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
break;
case mjSENS_RANGEFINDER: // rangefinder
if (objtype == mjOBJ_SITE) {
rvec[0] = d->site_xmat[9*objid+2];
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], NULL);
} else {
// camera-attached rangefinder: depth image
const int width = m->cam_resolution[2*objid];
const int height = m->cam_resolution[2*objid+1];
const int bodyexclude = m->cam_bodyid[objid];
const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
const int projection = m->cam_projection[objid];
{
// get dataspec
int dataspec = m->sensor_intprm[i*mjNSENS];
mjtNum* ptr = d->sensordata + adr;
// compute focal length in pixels using helper
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
if (objtype == mjOBJ_SITE) {
// site-attached rangefinder: single ray
rvec[0] = d->site_xmat[9*objid+2];
rvec[1] = d->site_xmat[9*objid+5];
rvec[2] = d->site_xmat[9*objid+8];
const mjtNum* origin = d->site_xpos + 3*objid;
if (projection == mjPROJ_PERSPECTIVE) {
// perspective: all rays share origin, different directions
const int npixel = width * height;
mj_markStack(d);
mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum);
int* geomid = mjSTACKALLOC(d, npixel, int);
int geomid;
mjtNum normal[3];
mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL;
mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1,
m->site_bodyid[objid], &geomid, p_normal);
// compute ray directions using helper (normalized)
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum origin[3];
mju_camPixelRay(origin, vec + 3*idx, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
}
}
// for site sensor: pass NULL for cam_z so depth = dist
fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL);
// cast all rays
mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, d->sensordata + adr, npixel, mjMAXVAL);
mj_freeStack(d);
} else {
// orthographic: parallel rays, different origins
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum origin[3], direction[3];
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
// camera-attached rangefinder: depth image
const int width = m->cam_resolution[2*objid];
const int height = m->cam_resolution[2*objid+1];
const int bodyexclude = m->cam_bodyid[objid];
const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
const int projection = m->cam_projection[objid];
d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1,
bodyexclude, NULL);
// camera z-axis (pointing into scene, negative of optical axis)
mjtNum cam_z[3] = {cam_xmat[2], cam_xmat[5], cam_xmat[8]};
// compute focal length in pixels using helper
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
if (projection == mjPROJ_PERSPECTIVE) {
// perspective: all rays share origin, different directions
const int npixel = width * height;
mj_markStack(d);
mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum);
int* geomid = mjSTACKALLOC(d, npixel, int);
mjtNum* dist = mjSTACKALLOC(d, npixel, mjtNum);
mjtNum* normals = NULL;
if (dataspec & (1 << mjRAYDATA_NORMAL)) {
normals = mjSTACKALLOC(d, 3*npixel, mjtNum);
}
// compute ray directions using helper (normalized)
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum origin[3];
mju_camPixelRay(origin, vec + 3*idx, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
}
}
// cast all rays with normals if needed
mj_multiRayNormal(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, dist, normals, npixel, mjMAXVAL);
// fill in output for each pixel
ptr = d->sensordata + adr;
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum* normal_ptr = normals ? normals + 3*idx : NULL;
ptr = fill_raydata(ptr, dataspec, dist[idx], cam_xpos, vec + 3*idx,
normal_ptr, cam_xpos, cam_z);
}
}
mj_freeStack(d);
} else {
// orthographic: parallel rays, different origins
ptr = d->sensordata + adr;
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
mjtNum origin[3], direction[3];
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
int geomid;
mjtNum normal[3];
mjtNum dist = mj_rayNormal(m, d, origin, direction, NULL, 1,
bodyexclude, &geomid, normal);
ptr = fill_raydata(ptr, dataspec, dist, origin, direction,
normal, cam_xpos, cam_z);
}
}
}
}
src/engine/engine_support.c
+22
View File
@@ -112,6 +112,16 @@ const int mjCONDATA_SIZE[mjNCONDATA] = {
};
// size of ray data fields
const int mjRAYDATA_SIZE[mjNRAYDATA] = {
1, // mjRAYDATA_DIST
3, // mjRAYDATA_DIR
3, // mjRAYDATA_ORIGIN
3, // mjRAYDATA_POINT
3, // mjRAYDATA_NORMAL
1 // mjRAYDATA_DEPTH
};
//-------------------------- get/set state ---------------------------------------------------------
// return size of a single state element
@@ -745,6 +755,18 @@ int mju_condataSize(int dataspec) {
}
// return total size of data in a rangefinder sensor bitfield specification
int mju_raydataSize(int dataspec) {
int size = 0;
for (int i=0; i < mjNRAYDATA; i++) {
if (dataspec & (1 << i)) {
size += mjRAYDATA_SIZE[i];
}
}
return size;
}
// compute camera pixel parameters from model, output are:
// pixel units: fx, fy (focal lengths), cx, cy (principal point)
// length units: extent
src/engine/engine_support.h
+4
View File
@@ -31,6 +31,7 @@ MJAPI extern const char* mjTIMERSTRING[mjNTIMER];
// arrays
MJAPI extern const int mjCONDATA_SIZE[mjNCONDATA]; // TODO(tassa): expose in public header?
extern const int mjRAYDATA_SIZE[mjNRAYDATA];
//-------------------------- get/set state ---------------------------------------------------------
@@ -120,6 +121,9 @@ MJAPI const char* mj_versionString(void);
// return total size of data fields in a contact sensor bitfield specification
MJAPI int mju_condataSize(int dataSpec);
// return total size of data fields in a rangefinder sensor bitfield specification
int mju_raydataSize(int dataspec);
// compute camera pixel parameters from model
// outputs: fx, fy (focal length in pixels), cx, cy (principal point), ortho_extent
void mju_camIntrinsics(const mjModel* m, int camid,
src/engine/engine_vis_visualize.c
+97 -21
View File
@@ -2476,27 +2476,74 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
return;
}
const float scl = m->stat.meansize;
mjtNum framewidth = m->vis.scale.framewidth * scl;
mjtNum framelength = m->vis.scale.framelength * scl;
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_type[i] == mjSENS_RANGEFINDER) {
int objid = m->sensor_objid[i];
int adr = m->sensor_adr[i];
// get dataspec and compute field offsets
int dataspec = m->sensor_intprm[i*mjNSENS];
int size = mju_raydataSize(dataspec);
int offset[mjNRAYDATA] = {0};
int increment = 0;
for (int j=0; j < mjNRAYDATA; j++) {
offset[j] = increment;
if (dataspec & (1 << j)) {
increment += mjRAYDATA_SIZE[j];
}
}
// site-attached rangefinder
if (m->sensor_objtype[i] == mjOBJ_SITE) {
mjtNum dst = d->sensordata[adr];
const mjtNum* ptr = d->sensordata + adr;
// null output: nothing to render
if (dst < 0) {
continue;
// get distance (if present)
mjtNum dist = -1;
if (dataspec & (1 << mjRAYDATA_DIST)) {
dist = ptr[offset[mjRAYDATA_DIST]];
}
// make ray
mjtNum* from = d->site_xpos+3*objid;
mjtNum to[3] = {from[0] + d->site_xmat[9*objid+2]*dst,
from[1] + d->site_xmat[9*objid+5]*dst,
from[2] + d->site_xmat[9*objid+8]*dst};
addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
// get point and draw line if dist is valid
mjtNum point[3] = {0};
if (dist >= 0) {
mjtNum* origin = d->site_xpos + 3*objid;
point[0] = origin[0] + d->site_xmat[9*objid+2]*dist;
point[1] = origin[1] + d->site_xmat[9*objid+5]*dist;
point[2] = origin[2] + d->site_xmat[9*objid+8]*dist;
addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
// draw point if present and non-zero
if (dataspec & (1 << mjRAYDATA_POINT)) {
const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
if (point_data[0] || point_data[1] || point_data[2]) {
mju_copy3(point, point_data);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
if (thisgeom) {
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.5 * framewidth;
mju_n2f(thisgeom->pos, point, 3);
mju_n2f(thisgeom->mat, IDENTITY, 9);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
releaseGeom(&thisgeom, scn);
}
}
}
// draw normal if present and point is valid
int valid_point = dist >= 0 || point[0] || point[1] || point[2];
if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
mjtNum to[3];
mju_addScl3(to, point, normal, 2*framelength);
addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
// camera-attached rangefinder
@@ -2511,15 +2558,16 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
// draw ray for each pixel
// draw for each pixel
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum dst = d->sensordata[adr + idx];
const mjtNum* ptr = d->sensordata + adr + idx*size;
// null output: nothing to render
if (dst < 0) {
continue;
// get distance (if present)
mjtNum dist = -1;
if (dataspec & (1 << mjRAYDATA_DIST)) {
dist = ptr[offset[mjRAYDATA_DIST]];
}
// compute ray origin and direction
@@ -2527,12 +2575,40 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
// compute endpoint
mjtNum to[3];
mju_addScl3(to, origin, direction, dst);
// get point and draw line if dist is valid
mjtNum point[3] = {0};
if (dist >= 0) {
mju_addScl3(point, origin, direction, dist);
addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
addConnector(scn, mjGEOM_LINE, 3, origin, to, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
// draw point if present and non-zero
if (dataspec & (1 << mjRAYDATA_POINT)) {
const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
if (point_data[0] || point_data[1] || point_data[2]) {
mju_copy3(point, point_data);
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
if (thisgeom) {
thisgeom->type = mjGEOM_SPHERE;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.3 * framewidth;
mju_n2f(thisgeom->pos, point, 3);
mju_n2f(thisgeom->mat, IDENTITY, 9);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
releaseGeom(&thisgeom, scn);
}
}
}
// draw normal if present and point is valid
int valid_point = dist >= 0 || point[0] || point[1] || point[2];
if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
mjtNum to[3];
mju_addScl3(to, point, normal, 2*framelength);
addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
}
}