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);
}
}
}
}
src/user/user_api.cc
+12 -8
View File
@@ -1274,14 +1274,6 @@ int mjs_sensorDim(const mjsSensor* sensor) {
case mjSENS_CLOCK:
return 1;
case mjSENS_RANGEFINDER:
if (sensor->objtype == mjOBJ_CAMERA) {
const mjCCamera* camera = static_cast<const mjCCamera*>(
static_cast<mjCSensor*>(sensor->element)->get_obj());
return camera->spec.resolution[0] * camera->spec.resolution[1];
}
return 1; // site-attached: single ray
case mjSENS_CAMPROJECTION:
return 2;
@@ -1321,6 +1313,18 @@ int mjs_sensorDim(const mjsSensor* sensor) {
static_cast<mjCSensor*>(sensor->element)->get_obj())
->nvert();
case mjSENS_RANGEFINDER:
{
int size = mju_raydataSize(sensor->intprm[0]);
int num_rays = 1;
if (sensor->objtype == mjOBJ_CAMERA) {
const mjCCamera* camera = static_cast<const mjCCamera*>(
static_cast<mjCSensor*>(sensor->element)->get_obj());
num_rays = camera->spec.resolution[0] * camera->spec.resolution[1];
}
return size * num_rays;
}
case mjSENS_USER:
return sensor->dim;
src/user/user_objects.cc
+21 -4
View File
@@ -7149,7 +7149,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
mjtDataType sensorDatatype(mjtSensor type) {
switch (type) {
case mjSENS_TOUCH:
case mjSENS_RANGEFINDER:
case mjSENS_INSIDESITE:
return mjDATATYPE_POSITIVE;
@@ -7196,6 +7195,7 @@ mjtDataType sensorDatatype(mjtSensor type) {
case mjSENS_SUBTREEANGMOM:
case mjSENS_GEOMDIST:
case mjSENS_GEOMFROMTO:
case mjSENS_RANGEFINDER:
case mjSENS_CONTACT:
case mjSENS_TACTILE:
case mjSENS_E_POTENTIAL:
@@ -7329,9 +7329,26 @@ void mjCSensor::Compile(void) {
break;
case mjSENS_RANGEFINDER:
// must be attached to site or camera
if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
throw mjCError(this, "sensor must be attached to site or camera");
{
// must be attached to site or camera
if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
throw mjCError(this, "sensor must be attached to site or camera");
}
// check for dataspec correctness
int dataspec = intprm[0];
if (dataspec <= 0) {
throw mjCError(this, "data spec (intprm[0]) must be positive, got %d", nullptr, dataspec);
}
int mask = (1 << mjNRAYDATA) - 1;
if (!(dataspec & mask)) {
throw mjCError(this, "data spec intprm[0]=%d must have at least one bit set of the first "
"mjNRAYDATA bits", nullptr, dataspec);
}
if (dataspec & ~mask) {
throw mjCError(this, "data spec intprm[0]=%d has bits set beyond the first "
"mjNRAYDATA bits", nullptr, dataspec);
}
}
break;
src/xml/xml_base.h
+1
View File
@@ -76,6 +76,7 @@ extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap condata_map[];
extern const mjMap raydata_map[];
extern const mjMap reduce_map[];
extern const mjMap meshtype_map[];
extern const mjMap meshinertia_map[];
src/xml/xml_native_reader.cc
+34 -1
View File
@@ -454,7 +454,7 @@ std::vector<const char*> MJCF[nMJCF] = {
{"torque", "*", "name", "site", "cutoff", "noise", "user"},
{"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
{"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"},
{"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
{"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
@@ -783,6 +783,17 @@ const mjMap condata_map[mjNCONDATA] = {
};
// rangefinder data type
const mjMap raydata_map[mjNRAYDATA] = {
{"dist", mjRAYDATA_DIST},
{"dir", mjRAYDATA_DIR},
{"origin", mjRAYDATA_ORIGIN},
{"point", mjRAYDATA_POINT},
{"normal", mjRAYDATA_NORMAL},
{"depth", mjRAYDATA_DEPTH}
};
// contact reduction type
const int reduce_sz = 4;
const mjMap reduce_map[reduce_sz] = {
@@ -4058,6 +4069,28 @@ void mjXReader::Sensor(XMLElement* section) {
throw mjXError(elem, "rangefinder requires exactly one of 'site' or 'camera'");
}
sensor->objtype = use_site ? mjOBJ_SITE : mjOBJ_CAMERA;
// process data specification (intprm[0])
int dataspec = 1 << mjRAYDATA_DIST;
std::vector<int> raydata(mjNRAYDATA);
int nkeys = MapValues(elem, "data", raydata.data(), raydata_map, mjNRAYDATA);
if (nkeys) {
dataspec = 1 << raydata[0];
// check ordering while adding bits to dataspec
for (int i = 1; i < nkeys; ++i) {
if (raydata[i] <= raydata[i-1]) {
std::string correct_order;
for (int j = 0; j < mjNRAYDATA; ++j) {
correct_order += raydata_map[j].key;
if (j < mjNRAYDATA - 1) correct_order += ", ";
}
throw mjXError(elem, "data attributes must be in order: %s", correct_order.c_str());
}
dataspec |= 1 << raydata[i];
}
}
sensor->intprm[0] = dataspec;
}
// sensors related to scalar joints, tendons, actuators
src/xml/xml_native_writer.cc
+16 -5
View File
@@ -2028,11 +2028,22 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, "site", sensor->get_objname());
break;
case mjSENS_RANGEFINDER:
elem = InsertEnd(section, "rangefinder");
if (sensor->objtype == mjOBJ_SITE) {
WriteAttrTxt(elem, "site", sensor->get_objname());
} else {
WriteAttrTxt(elem, "camera", sensor->get_objname());
{
elem = InsertEnd(section, "rangefinder");
if (sensor->objtype == mjOBJ_SITE) {
WriteAttrTxt(elem, "site", sensor->get_objname());
} else {
WriteAttrTxt(elem, "camera", sensor->get_objname());
}
int dataspec = sensor->intprm[0];
int data[mjNRAYDATA];
int ndata = 0;
for (int i=0; i < mjNRAYDATA; i++) {
if (dataspec & (1 << i)) {
data[ndata++] = i;
}
}
WriteAttrKeys(elem, "data", raydata_map, mjNRAYDATA, data, ndata, 0);
}
break;
case mjSENS_CAMPROJECTION: