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

Add camera projection sensor.

Browse Source 
BEGIN_PUBLIC
Add camera projection sensor.
END_PUBLIC

PiperOrigin-RevId: 564726114
Change-Id: I33b8e5562eff29c21538bf8f38ce3116c7dfc5a9
This commit is contained in:
Alessio Quaglino
2023-09-12 08:15:14 -07:00
committed by Copybara-Service
parent ccda87aafa
commit 8064ad59c8
19 changed files with 369 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+42 -1
View File
@@ -3054,6 +3054,13 @@ and the +Y axis points up. Thus the frame position and orientation are the key a
Vertical field of view of the camera, expressed in degrees regardless of the global angle setting. The horizontal
field of view is computed automatically given the window size and the vertical field of view.
.. _body-camera-resolution:
:at:`resolution`: :at-val:`int(2), "1 1"`
Resolution of the camera in pixels [width height]. Note that these values are not used for rendering since those
dimensions are determined by the size of the rendering context. This attribute serves as a convenient
location to save the required resolution when creating a context.
.. _body-camera-ipd:
:at:`ipd`: :at-val:`real, "0.068"`
@@ -5412,7 +5419,6 @@ site frame. The output is a 3D vector.
:at:`site`: :at-val:`string, required`
The site where the sensor is attached.
.. _sensor-rangefinder:
:el-prefix:`sensor/` |-| **rangefinder** (*)
@@ -5441,6 +5447,39 @@ excluded; this is because sensor calculations are independent of the visualizer.
:at:`site`: :at-val:`string, required`
The site where the sensor is attached.
.. _sensor-camprojection:
:el-prefix:`sensor/` |-| **camprojection** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a camprojection sensor, which returns the location of a target site, projected onto a camera image
in pixel coordinates. The origin of this system is located at the center of the top left pixel, so a target which
projects exactly onto the corner of the image, will have value (-0.5, -0.5). Values are not clipped, so targets which
fall outside the camera image will take values above or below the pixel limits. Moreover, points behind the camera
are also projected onto the image, so it is up to the user to filter out such points, if desired. This can be done using
a `framepos<sensor-framepos>` sensor with the camera as reference frame, then a negative/positive value in the
z-coordinate indicates (respectively) a location in the front/back of the camera.
.. _sensor-camprojection-site:
:at:`site`: :at-val:`string, required`
The site which is projected on to the camera image.
.. _sensor-camprojection-camera:
:at:`camera`: :at-val:`string, required`
The camera used for the projection, its :ref:`resolution<body-camera-resolution>` attribute must be positive.
.. _sensor-camprojection-name:
.. _sensor-camprojection-noise:
.. _sensor-camprojection-cutoff:
.. _sensor-camprojection-user:
:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user`
See :ref:`CSensor`.
.. _sensor-jointpos:
@@ -6655,6 +6694,8 @@ if omitted.
.. _default-camera-fovy:
.. _default-camera-resolution:
.. _default-camera-ipd:
.. _default-camera-pos:
doc/XMLschema.rst
+14 -5
View File
@@ -364,11 +364,11 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<body-camera-name>` | :ref:`class<body-camera-class>` | :ref:`fovy<body-camera-fovy>` | :ref:`ipd<body-camera-ipd>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`pos<body-camera-pos>` | :ref:`quat<body-camera-quat>` | :ref:`axisangle<body-camera-axisangle>` | :ref:`xyaxes<body-camera-xyaxes>` | |
| | | | :ref:`resolution<body-camera-resolution>` | :ref:`pos<body-camera-pos>` | :ref:`quat<body-camera-quat>` | :ref:`axisangle<body-camera-axisangle>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`zaxis<body-camera-zaxis>` | :ref:`euler<body-camera-euler>` | :ref:`mode<body-camera-mode>` | :ref:`target<body-camera-target>` | |
| | | | :ref:`xyaxes<body-camera-xyaxes>` | :ref:`zaxis<body-camera-zaxis>` | :ref:`euler<body-camera-euler>` | :ref:`mode<body-camera-mode>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<body-camera-user>` | | | | |
| | | | :ref:`target<body-camera-target>` | :ref:`user<body-camera-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
@@ -835,6 +835,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`camprojection | \* | :class: mjcf-attributes |
| <sensor-camprojection>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-camprojection-name>` | :ref:`site<sensor-camprojection-site>` | :ref:`camera<sensor-camprojection-camera>` | :ref:`cutoff<sensor-camprojection-cutoff>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`noise<sensor-camprojection-noise>` | :ref:`user<sensor-camprojection-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`rangefinder | \* | :class: mjcf-attributes |
| <sensor-rangefinder>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -1223,9 +1232,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fovy<default-camera-fovy>` | :ref:`ipd<default-camera-ipd>` | :ref:`pos<default-camera-pos>` | :ref:`quat<default-camera-quat>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`axisangle<default-camera-axisangle>` | :ref:`xyaxes<default-camera-xyaxes>` | :ref:`zaxis<default-camera-zaxis>` | :ref:`euler<default-camera-euler>` | |
| | | | :ref:`resolution<default-camera-resolution>` | :ref:`axisangle<default-camera-axisangle>` | :ref:`xyaxes<default-camera-xyaxes>` | :ref:`zaxis<default-camera-zaxis>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`mode<default-camera-mode>` | :ref:`user<default-camera-user>` | | | |
| | | | :ref:`euler<default-camera-euler>` | :ref:`mode<default-camera-mode>` | :ref:`user<default-camera-user>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+6 -2
View File
@@ -65,16 +65,20 @@ General
12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning.
13. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store normalizing transformation.
14. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
target site, projected onto the camera image, in pixel coordinates.
Python bindings
^^^^^^^^^^^^^^^
14. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
15. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
Bug fixes
^^^^^^^^^
15. Fixed a bug that was causing the geom margins to be ignored during the midphase.
16. Fixed a bug that was causing the geom margins to be ignored during the midphase.
Version 2.3.7 (July 20, 2023)
doc/includes/references.h
+2
View File
@@ -577,6 +577,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_TORQUE, // 3D torque between site's body and its parent body
mjSENS_MAGNETOMETER, // 3D magnetometer
mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis
mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image
// sensors related to scalar joints, tendons, actuators
mjSENS_JOINTPOS, // scalar joint position (hinge and slide only)
@@ -1010,6 +1011,7 @@ struct mjModel_ {
mjtNum* cam_poscom0; // global position rel. to sub-com in qpos0 (ncam x 3)
mjtNum* cam_pos0; // global position rel. to body in qpos0 (ncam x 3)
mjtNum* cam_mat0; // global orientation in qpos0 (ncam x 9)
int* cam_resolution; // [width, height] in pixels (ncam x 2)
mjtNum* cam_fovy; // y-field of view (deg) (ncam x 1)
mjtNum* cam_ipd; // inter-pupilary distance (ncam x 1)
mjtNum* cam_user; // user data (ncam x nuser_cam)
include/mujoco/mjmodel.h
+2
View File
@@ -283,6 +283,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_TORQUE, // 3D torque between site's body and its parent body
mjSENS_MAGNETOMETER, // 3D magnetometer
mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis
mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image
// sensors related to scalar joints, tendons, actuators
mjSENS_JOINTPOS, // scalar joint position (hinge and slide only)
@@ -745,6 +746,7 @@ struct mjModel_ {
mjtNum* cam_poscom0; // global position rel. to sub-com in qpos0 (ncam x 3)
mjtNum* cam_pos0; // global position rel. to body in qpos0 (ncam x 3)
mjtNum* cam_mat0; // global orientation in qpos0 (ncam x 9)
int* cam_resolution; // [width, height] in pixels (ncam x 2)
mjtNum* cam_fovy; // y-field of view (deg) (ncam x 1)
mjtNum* cam_ipd; // inter-pupilary distance (ncam x 1)
mjtNum* cam_user; // user data (ncam x nuser_cam)
include/mujoco/mjxmacro.h
+1
View File
@@ -258,6 +258,7 @@
X ( int, cam_mode, ncam, 1 ) \
X ( int, cam_bodyid, ncam, 1 ) \
X ( int, cam_targetbodyid, ncam, 1 ) \
X ( int, cam_resolution, ncam, 2 ) \
X ( mjtNum, cam_pos, ncam, 3 ) \
X ( mjtNum, cam_quat, ncam, 4 ) \
X ( mjtNum, cam_poscom0, ncam, 3 ) \
introspect/enums.py
+32 -31
View File
@@ -313,37 +313,38 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSENS_TORQUE', 5),
('mjSENS_MAGNETOMETER', 6),
('mjSENS_RANGEFINDER', 7),
('mjSENS_JOINTPOS', 8),
('mjSENS_JOINTVEL', 9),
('mjSENS_TENDONPOS', 10),
('mjSENS_TENDONVEL', 11),
('mjSENS_ACTUATORPOS', 12),
('mjSENS_ACTUATORVEL', 13),
('mjSENS_ACTUATORFRC', 14),
('mjSENS_JOINTACTFRC', 15),
('mjSENS_BALLQUAT', 16),
('mjSENS_BALLANGVEL', 17),
('mjSENS_JOINTLIMITPOS', 18),
('mjSENS_JOINTLIMITVEL', 19),
('mjSENS_JOINTLIMITFRC', 20),
('mjSENS_TENDONLIMITPOS', 21),
('mjSENS_TENDONLIMITVEL', 22),
('mjSENS_TENDONLIMITFRC', 23),
('mjSENS_FRAMEPOS', 24),
('mjSENS_FRAMEQUAT', 25),
('mjSENS_FRAMEXAXIS', 26),
('mjSENS_FRAMEYAXIS', 27),
('mjSENS_FRAMEZAXIS', 28),
('mjSENS_FRAMELINVEL', 29),
('mjSENS_FRAMEANGVEL', 30),
('mjSENS_FRAMELINACC', 31),
('mjSENS_FRAMEANGACC', 32),
('mjSENS_SUBTREECOM', 33),
('mjSENS_SUBTREELINVEL', 34),
('mjSENS_SUBTREEANGMOM', 35),
('mjSENS_CLOCK', 36),
('mjSENS_PLUGIN', 37),
('mjSENS_USER', 38),
('mjSENS_CAMPROJECTION', 8),
('mjSENS_JOINTPOS', 9),
('mjSENS_JOINTVEL', 10),
('mjSENS_TENDONPOS', 11),
('mjSENS_TENDONVEL', 12),
('mjSENS_ACTUATORPOS', 13),
('mjSENS_ACTUATORVEL', 14),
('mjSENS_ACTUATORFRC', 15),
('mjSENS_JOINTACTFRC', 16),
('mjSENS_BALLQUAT', 17),
('mjSENS_BALLANGVEL', 18),
('mjSENS_JOINTLIMITPOS', 19),
('mjSENS_JOINTLIMITVEL', 20),
('mjSENS_JOINTLIMITFRC', 21),
('mjSENS_TENDONLIMITPOS', 22),
('mjSENS_TENDONLIMITVEL', 23),
('mjSENS_TENDONLIMITFRC', 24),
('mjSENS_FRAMEPOS', 25),
('mjSENS_FRAMEQUAT', 26),
('mjSENS_FRAMEXAXIS', 27),
('mjSENS_FRAMEYAXIS', 28),
('mjSENS_FRAMEZAXIS', 29),
('mjSENS_FRAMELINVEL', 30),
('mjSENS_FRAMEANGVEL', 31),
('mjSENS_FRAMELINACC', 32),
('mjSENS_FRAMEANGACC', 33),
('mjSENS_SUBTREECOM', 34),
('mjSENS_SUBTREELINVEL', 35),
('mjSENS_SUBTREEANGMOM', 36),
('mjSENS_CLOCK', 37),
('mjSENS_PLUGIN', 38),
('mjSENS_USER', 39),
]),
)),
('mjtStage',
introspect/structs.py
+7
View File
@@ -1885,6 +1885,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='global orientation in qpos0 (ncam x 9)',
),
StructFieldDecl(
name='cam_resolution',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='[width, height] in pixels (ncam x 2)',
),
StructFieldDecl(
name='cam_fovy',
type=PointerType(
src/engine/engine_io.c
+3
View File
@@ -1612,6 +1612,9 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_CLOCK:
return 1;
case mjSENS_CAMPROJECTION:
return 2;
case mjSENS_ACCELEROMETER:
case mjSENS_VELOCIMETER:
case mjSENS_GYRO:
src/engine/engine_sensor.c
+90
View File
@@ -187,6 +187,91 @@ static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, in
}
static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
const int cam_res[2], mjtNum cam_fovy) {
// translation matrix (4x4)
mjtNum translation[4][4] = {0};
translation[0][0] = 1;
translation[1][1] = 1;
translation[2][2] = 1;
translation[3][3] = 1;
translation[0][3] = -cam_xpos[0];
translation[1][3] = -cam_xpos[1];
translation[2][3] = -cam_xpos[2];
// rotation matrix (4x4)
mjtNum rotation[4][4] = {0};
rotation[0][0] = 1;
rotation[1][1] = 1;
rotation[2][2] = 1;
rotation[3][3] = 1;
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
rotation[i][j] = cam_xmat[j*3+i];
}
}
// focal transformation matrix (3x4)
mjtNum height = (mjtNum) cam_res[1];
mjtNum fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * height;
mjtNum focal[3][4] = {0};
focal[0][0] = -fy;
focal[1][1] = fy;
focal[2][2] = 1.0;
// image matrix (3x3)
mjtNum image[3][3] = {0};
image[0][0] = 1;
image[1][1] = 1;
image[2][2] = 1;
image[0][2] = ((mjtNum)cam_res[0] - 1) / 2.0;
image[1][2] = ((mjtNum)cam_res[1] - 1) / 2.0;
// projection matrix (3x4): product of all 4 matrices
mjtNum proj[3][4] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
for (int k=0; k<4; k++) {
for (int l=0; l<4; l++) {
for (int n=0; n<4; n++) {
proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
}
}
}
}
}
// projection matrix multiplies homogenous [x, y, z, 1] vectors
mjtNum pos_hom[4] = {0, 0, 0, 1};
mju_copy3(pos_hom, target_xpos);
// project world coordinates into pixel space, see:
// https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
mjtNum pixel_coord_hom[3] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<4; j++) {
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
}
}
// avoid dividing by tiny numbers
mjtNum denom = pixel_coord_hom[2];
if (mju_abs(denom) < mjMINVAL) {
if (denom < 0) {
denom = mju_min(denom, -mjMINVAL);
} else {
denom = mju_max(denom, mjMINVAL);
}
}
// compute projection
sensordata[0] = pixel_coord_hom[0] / denom;
sensordata[1] = pixel_coord_hom[1] / denom;
}
//-------------------------------- sensor ----------------------------------------------------------
// position-dependent sensors
@@ -221,6 +306,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
break;
case mjSENS_CAMPROJECTION: // camera projection
cam_project(d->sensordata+adr, d->site_xpos+3*objid, d->cam_xpos+3*refid,
d->cam_xmat+9*refid, m->cam_resolution+2*refid, m->cam_fovy[refid]);
break;
case mjSENS_RANGEFINDER: // rangefinder
rvec[0] = d->site_xmat[9*objid+2];
rvec[1] = d->site_xmat[9*objid+5];
src/user/user_model.cc
+2
View File
@@ -1586,6 +1586,7 @@ void mjCModel::CopyTree(mjModel* m) {
copyvec(m->cam_quat+4*cid, pc->locquat, 4);
m->cam_fovy[cid] = (mjtNum)pc->fovy;
m->cam_ipd[cid] = (mjtNum)pc->ipd;
copyvec(m->cam_resolution+2*cid, pc->resolution, 2);
copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam);
}
@@ -3044,6 +3045,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
cameras[i]->fovy = (double)m->cam_fovy[i];
cameras[i]->ipd = (double)m->cam_ipd[i];
copyvec(cameras[i]->resolution, m->cam_resolution+2*i, 2);
if (nuser_cam) {
copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam);
src/user/user_objects.cc
+22 -1
View File
@@ -1949,6 +1949,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
fovy = 45;
ipd = 0.068;
userdata.clear();
resolution[0] = resolution[1] = 1;
// clear private variables
body = 0;
@@ -2003,6 +2004,12 @@ void mjCCamera::Compile(void) {
if (targetbodyid==body->id) {
throw mjCError(this, "parent-targeting in camera '%s' (id = %d)", name.c_str(), id);
}
// make sure the image size is finite
if (fovy >= 180) {
throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
name.c_str(), id, fovy);
}
}
@@ -4044,6 +4051,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TORQUE:
case mjSENS_MAGNETOMETER:
case mjSENS_RANGEFINDER:
case mjSENS_CAMPROJECTION:
// must be attached to site
if (objtype!=mjOBJ_SITE) {
throw mjCError(this,
@@ -4054,19 +4062,32 @@ void mjCSensor::Compile(void) {
if (type==mjSENS_TOUCH || type==mjSENS_RANGEFINDER) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type==mjSENS_CAMPROJECTION) {
dim = 2;
datatype = mjDATATYPE_REAL;
} else {
dim = 3;
datatype = mjDATATYPE_REAL;
}
// set stage
if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER) {
if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER || type==mjSENS_CAMPROJECTION) {
needstage = mjSTAGE_POS;
} else if (type==mjSENS_GYRO || type==mjSENS_VELOCIMETER) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
// check for camera resolution for camera projection sensor
if (type==mjSENS_CAMPROJECTION) {
mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname);
if (!camref->resolution[0] || !camref->resolution[1]) {
throw mjCError(this,
"camera projection sensor requires camera resolution '%s' (id = %d)",
name.c_str(), id);
}
}
break;
case mjSENS_JOINTPOS:
src/user/user_objects.h
+1
View File
@@ -465,6 +465,7 @@ class mjCCamera : public mjCBase {
double ipd; // inter-pupilary distance
double pos[3]; // position
double quat[4]; // orientation
float resolution[2]; // resolution [pixel]
std::vector<double> userdata; // user data
mjCAlternative alt; // alternative orientation specification
src/xml/xml_native_reader.cc
+14 -3
View File
@@ -80,7 +80,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 203;
static const int nMJCF = 204;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -151,7 +151,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "?", "13", "type", "group", "pos", "quat", "material",
"size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
{"camera", "?", "10", "fovy", "ipd", "pos", "quat",
{"camera", "?", "11", "fovy", "ipd", "pos", "quat", "resolution",
"axisangle", "xyaxes", "zaxis", "euler", "mode", "user"},
{"light", "?", "12", "pos", "dir", "directional", "castshadow", "active",
"attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"},
@@ -264,7 +264,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{">"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
"material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
{"camera", "*", "13", "name", "class", "fovy", "ipd",
{"camera", "*", "14", "name", "class", "fovy", "ipd", "resolution",
"pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"mode", "target", "user"},
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
@@ -398,6 +398,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"force", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"torque", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"magnetometer", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"camprojection", "*", "6", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"jointpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"jointvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -1476,6 +1477,10 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) {
ReadAlternative(elem, pcam->alt);
ReadAttr(elem, "fovy", 1, &pcam->fovy, text);
ReadAttr(elem, "ipd", 1, &pcam->ipd, text);
ReadAttr(elem, "resolution", 2, pcam->resolution, text);
if (pcam->resolution[0] < 0 || pcam->resolution[1] < 0) {
throw mjXError(elem, "camera resolution cannot be negative");
}
// read userdata
ReadVector(elem, "user", pcam->userdata, text);
@@ -3013,6 +3018,12 @@ void mjXReader::Sensor(XMLElement* section) {
psen->type = mjSENS_MAGNETOMETER;
psen->objtype = mjOBJ_SITE;
ReadAttrTxt(elem, "site", psen->objname, true);
} else if (type=="camprojection") {
psen->type = mjSENS_CAMPROJECTION;
psen->objtype = mjOBJ_SITE;
ReadAttrTxt(elem, "site", psen->objname, true);
ReadAttrTxt(elem, "camera", psen->refname, true);
psen->reftype = mjOBJ_CAMERA;
} else if (type=="rangefinder") {
psen->type = mjSENS_RANGEFINDER;
psen->objtype = mjOBJ_SITE;
src/xml/xml_native_writer.cc
+7 -1
View File
@@ -408,6 +408,7 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->camera.ipd);
WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy);
WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->camera.mode);
WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
// userdata
if (writingdefaults) {
@@ -1648,6 +1649,11 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "rangefinder");
WriteAttrTxt(elem, "site", psen->objname);
break;
case mjSENS_CAMPROJECTION:
elem = InsertEnd(section, "camprojection");
WriteAttrTxt(elem, "site", psen->objname);
WriteAttrTxt(elem, "camera", psen->refname);
break;
// sensors related to scalar joints, tendons, actuators
case mjSENS_JOINTPOS:
@@ -1836,7 +1842,7 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteVector(elem, "user", psen->userdata);
// add reference if present
if (psen->reftype != mjOBJ_UNKNOWN) {
if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->refname);
}
test/engine/engine_sensor_test.cc
+46
View File
@@ -432,5 +432,51 @@ TEST_F(SensorTest, Clock) {
mj_deleteModel(model);
}
// ------------------------- camera sensor tests -----------------------------
// test clock sensor
TEST_F(SensorTest, CameraProjection) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="1.1 0 1">
<geom type="box" size=".1 .6 .375"/>
<site name="frontorigin" pos="-.1 .6 .375"/>
<site name="frontcorner" pos="-.1 -.6 -.375"/>
</body>
<body pos="-1.1 0 1">
<geom type="box" size=".1 .6 .375"/>
<site name="backcenter" pos="-.1 0 0"/>
</body>
<camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="41.11209"
resolution="1920 1200" name="fixedcamera"/>
</worldbody>
<sensor>
<camprojection site="frontorigin" camera="fixedcamera"/>
<camprojection site="frontcorner" camera="fixedcamera"/>
<camprojection site="backcenter" camera="fixedcamera"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// call step to update sensors
mj_step(model, data);
mj_step1(model, data); // update values of position-based sensors
EXPECT_THAT(model->cam_resolution[0], 1920);
EXPECT_THAT(model->cam_resolution[1], 1200);
mjtNum eps = 1e-4;
EXPECT_NEAR(data->sensordata[0], -0.5, eps);
EXPECT_NEAR(data->sensordata[1], -0.5, eps);
EXPECT_NEAR(data->sensordata[2], 1919.5, eps);
EXPECT_NEAR(data->sensordata[3], 1199.5, eps);
EXPECT_NEAR(data->sensordata[4], 959.5, eps);
EXPECT_NEAR(data->sensordata[5], 599.5, eps);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/sensor/camprojection.xml
Vendored
+23
View File
@@ -0,0 +1,23 @@
<mujoco>
<default>
<site rgba="1 0 0 1" size="0.03"/>
</default>
<worldbody>
<light pos="0 0 3"/>
<body pos="1.1 0 1">
<geom type="box" size=".1 .6 .375"/>
<site name="frontorigin" pos="-.1 .6 .375"/>
<site name="frontcenter" pos="-.1 0 0"/>
</body>
<body pos="0 0 0">
<joint axis="0 0 1" range="-180 180" limited="false"/>
<geom type="cylinder" size=".2 .05" pos="0 0 0.9"/>
<camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="41.11209"
resolution="1920 1200" name="fixedcamera"/>
</body>
</worldbody>
<sensor>
<camprojection site="frontorigin" camera="fixedcamera"/>
<camprojection site="frontcenter" camera="fixedcamera"/>
</sensor>
</mujoco>
test/user/user_objects_test.cc
+22
View File
@@ -732,6 +732,28 @@ TEST_F(QuatNorm, QuatNotNormalized) {
mj_deleteModel(m);
}
// ------------- test camera specifications ------------------------------------
using CameraSpecTest = MujocoTest;
TEST_F(CameraSpecTest, FovyLimits) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<camera fovy="180"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("fovy too large"));
mj_deleteModel(m);
}
// ------------- test actuator order -------------------------------------------
using ActuatorTest = MujocoTest;
unity/Runtime/Bindings/MjBindings.cs
+33 -31
View File
@@ -324,37 +324,38 @@ public enum mjtSensor : int{
mjSENS_TORQUE = 5,
mjSENS_MAGNETOMETER = 6,
mjSENS_RANGEFINDER = 7,
mjSENS_JOINTPOS = 8,
mjSENS_JOINTVEL = 9,
mjSENS_TENDONPOS = 10,
mjSENS_TENDONVEL = 11,
mjSENS_ACTUATORPOS = 12,
mjSENS_ACTUATORVEL = 13,
mjSENS_ACTUATORFRC = 14,
mjSENS_JOINTACTFRC = 15,
mjSENS_BALLQUAT = 16,
mjSENS_BALLANGVEL = 17,
mjSENS_JOINTLIMITPOS = 18,
mjSENS_JOINTLIMITVEL = 19,
mjSENS_JOINTLIMITFRC = 20,
mjSENS_TENDONLIMITPOS = 21,
mjSENS_TENDONLIMITVEL = 22,
mjSENS_TENDONLIMITFRC = 23,
mjSENS_FRAMEPOS = 24,
mjSENS_FRAMEQUAT = 25,
mjSENS_FRAMEXAXIS = 26,
mjSENS_FRAMEYAXIS = 27,
mjSENS_FRAMEZAXIS = 28,
mjSENS_FRAMELINVEL = 29,
mjSENS_FRAMEANGVEL = 30,
mjSENS_FRAMELINACC = 31,
mjSENS_FRAMEANGACC = 32,
mjSENS_SUBTREECOM = 33,
mjSENS_SUBTREELINVEL = 34,
mjSENS_SUBTREEANGMOM = 35,
mjSENS_CLOCK = 36,
mjSENS_PLUGIN = 37,
mjSENS_USER = 38,
mjSENS_CAMPROJECTION = 8,
mjSENS_JOINTPOS = 9,
mjSENS_JOINTVEL = 10,
mjSENS_TENDONPOS = 11,
mjSENS_TENDONVEL = 12,
mjSENS_ACTUATORPOS = 13,
mjSENS_ACTUATORVEL = 14,
mjSENS_ACTUATORFRC = 15,
mjSENS_JOINTACTFRC = 16,
mjSENS_BALLQUAT = 17,
mjSENS_BALLANGVEL = 18,
mjSENS_JOINTLIMITPOS = 19,
mjSENS_JOINTLIMITVEL = 20,
mjSENS_JOINTLIMITFRC = 21,
mjSENS_TENDONLIMITPOS = 22,
mjSENS_TENDONLIMITVEL = 23,
mjSENS_TENDONLIMITFRC = 24,
mjSENS_FRAMEPOS = 25,
mjSENS_FRAMEQUAT = 26,
mjSENS_FRAMEXAXIS = 27,
mjSENS_FRAMEYAXIS = 28,
mjSENS_FRAMEZAXIS = 29,
mjSENS_FRAMELINVEL = 30,
mjSENS_FRAMEANGVEL = 31,
mjSENS_FRAMELINACC = 32,
mjSENS_FRAMEANGACC = 33,
mjSENS_SUBTREECOM = 34,
mjSENS_SUBTREELINVEL = 35,
mjSENS_SUBTREEANGMOM = 36,
mjSENS_CLOCK = 37,
mjSENS_PLUGIN = 38,
mjSENS_USER = 39,
}
public enum mjtStage : int{
mjSTAGE_NONE = 0,
@@ -2089,6 +2090,7 @@ public unsafe struct mjModel_ {
public double* cam_poscom0;
public double* cam_pos0;
public double* cam_mat0;
public int* cam_resolution;
public double* cam_fovy;
public double* cam_ipd;
public double* cam_user;