From 8064ad59c82e5e83a21b15bf883408cfd97a4f97 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 12 Sep 2023 08:15:14 -0700 Subject: [PATCH] Add camera projection sensor. BEGIN_PUBLIC Add camera projection sensor. END_PUBLIC PiperOrigin-RevId: 564726114 Change-Id: I33b8e5562eff29c21538bf8f38ce3116c7dfc5a9 --- doc/XMLreference.rst | 43 ++++++++- doc/XMLschema.rst | 19 ++-- doc/changelog.rst | 8 +- doc/includes/references.h | 2 + include/mujoco/mjmodel.h | 2 + include/mujoco/mjxmacro.h | 1 + introspect/enums.py | 63 ++++++------- introspect/structs.py | 7 ++ src/engine/engine_io.c | 3 + src/engine/engine_sensor.c | 90 +++++++++++++++++++ src/user/user_model.cc | 2 + src/user/user_objects.cc | 23 ++++- src/user/user_objects.h | 1 + src/xml/xml_native_reader.cc | 17 +++- src/xml/xml_native_writer.cc | 8 +- test/engine/engine_sensor_test.cc | 46 ++++++++++ test/engine/testdata/sensor/camprojection.xml | 23 +++++ test/user/user_objects_test.cc | 22 +++++ unity/Runtime/Bindings/MjBindings.cs | 64 ++++++------- 19 files changed, 369 insertions(+), 75 deletions(-) create mode 100644 test/engine/testdata/sensor/camprojection.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b8518652..8c412513 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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 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` 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: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 2951b722..6cc3590a 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -364,11 +364,11 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`fovy` | :ref:`ipd` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`pos` | :ref:`quat` | :ref:`axisangle` | :ref:`xyaxes` | | +| | | | :ref:`resolution` | :ref:`pos` | :ref:`quat` | :ref:`axisangle` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`zaxis` | :ref:`euler` | :ref:`mode` | :ref:`target` | | +| | | | :ref:`xyaxes` | :ref:`zaxis` | :ref:`euler` | :ref:`mode` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`user` | | | | | +| | | | :ref:`target` | :ref:`user` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | @@ -835,6 +835,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`camprojection | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`site` | :ref:`camera` | :ref:`cutoff` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`noise` | :ref:`user` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`rangefinder | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | @@ -1223,9 +1232,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`fovy` | :ref:`ipd` | :ref:`pos` | :ref:`quat` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | :ref:`euler` | | +| | | | :ref:`resolution` | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`mode` | :ref:`user` | | | | +| | | | :ref:`euler` | :ref:`mode` | :ref:`user` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 5c690f10..61932f04 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -65,16 +65,20 @@ General 12. Added :ref:`ls_iterations` and :ref:`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` attribute and :ref:`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 `__ where calling ``update_scene`` with an invalid +15. Fixed `#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) diff --git a/doc/includes/references.h b/doc/includes/references.h index 7b4ec3e9..a28f77ed 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index ec3bfeac..31031f8f 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 9f64b389..7ec0a7f4 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index c769d225..d9cc2772 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -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', diff --git a/introspect/structs.py b/introspect/structs.py index 4f6a1a6c..abf8b461 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -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( diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index d29cb343..0d2546f0 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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: diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 6be75fc9..fa149f2b 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -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]; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index c69fffcf..4cb2a541 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3f5f6376..9229faf9 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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: diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 812606dc..d8d012a4 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 userdata; // user data mjCAlternative alt; // alternative orientation specification diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index dc5129d6..8e244550 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index bcd0ffff..debe73b5 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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); } diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index ae66fc0e..815c0472 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -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"( + + + + + + + + + + + + + + + + + + + + )"; + 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 diff --git a/test/engine/testdata/sensor/camprojection.xml b/test/engine/testdata/sensor/camprojection.xml new file mode 100644 index 00000000..b805a8ee --- /dev/null +++ b/test/engine/testdata/sensor/camprojection.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 7f4ce069..e7dd3b42 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -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"( + + + + + + + + + )"; + std::array 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; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index aae87bf5..bad1edc6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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;