From d0e4771c8c007346c2d97f7b1e65308254c2482e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 14 Jul 2025 13:01:55 -0700 Subject: [PATCH] Add contact sensor. PiperOrigin-RevId: 783011982 Change-Id: Ica56fe9d520fa1d1ee7338e09148b1a55a049912 --- doc/APIreference/APItypes.rst | 10 + doc/XMLreference.rst | 151 +++++++++- doc/XMLschema.rst | 13 + doc/changelog.rst | 3 + doc/includes/references.h | 14 + include/mujoco/mjmodel.h | 16 ++ python/mujoco/introspect/enums.py | 26 +- src/engine/engine_io.c | 1 + src/engine/engine_sensor.c | 212 +++++++++++++- src/engine/engine_support.c | 24 ++ src/engine/engine_support.h | 7 + src/user/user_objects.cc | 61 +++- src/xml/xml_base.h | 3 + src/xml/xml_native_reader.cc | 95 +++++++ src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 36 ++- src/xml/xml_util.cc | 55 +++- src/xml/xml_util.h | 8 + test/engine/engine_sensor_test.cc | 268 ++++++++++++++++-- test/engine/engine_support_test.cc | 10 + test/engine/testdata/sensor/contact.xml | 35 +++ .../engine/testdata/sensor/contact_sorted.xml | 22 ++ .../testdata/sensor/contact_subtree.xml | 30 ++ test/engine/testdata/sensor/humanoid.xml | 267 +++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 21 +- 25 files changed, 1343 insertions(+), 47 deletions(-) create mode 100644 test/engine/testdata/sensor/contact.xml create mode 100644 test/engine/testdata/sensor/contact_sorted.xml create mode 100644 test/engine/testdata/sensor/contact_subtree.xml create mode 100644 test/engine/testdata/sensor/humanoid.xml diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index d72e1b69..65830fe3 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -367,6 +367,16 @@ These are the possible sensor data types, used in ``mjData.sensor_datatype``. .. mujoco-include:: mjtDataType +.. _mjtConDataField: + +mjtConDataField +~~~~~~~~~~~~~~~ + +Types of data fields returned by contact sensors. + +.. mujoco-include:: mjtConDataField + + .. _mjtSameFrame: mjtSameFrame diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 421e3781..28d97205 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -7240,7 +7240,7 @@ See :ref:`collision-sensors` for more details about sensors of this type. .. _sensor-distance-user: -:at:`name`, :at:`noise`, :at:`user` +:at:`name`, :at:`noise`, :at:`user`: See :ref:`CSensor`. @@ -7287,7 +7287,7 @@ See :ref:`collision-sensors` for more details about sensors of this type. .. _sensor-normal-user: -:at:`name`, :at:`noise`, :at:`user` +:at:`name`, :at:`noise`, :at:`user`: See :ref:`CSensor`. @@ -7335,10 +7335,155 @@ See :ref:`collision-sensors` for more details about sensors of this type. .. _sensor-fromto-user: -:at:`name`, :at:`noise`, :at:`user` +:at:`name`, :at:`noise`, :at:`user`: See :ref:`CSensor`. +.. _sensor-contact: + +:el-prefix:`sensor/` |-| **contact** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +**Motivation:** The array of contacts which occur during the main dynamics pipeline is inherently variable-sized. The +purpose of the contact sensor is to report contact-related information in a fixed-size array. This is useful as +input to learning-based agents and in environment logic. + +Unlike the purely geometric :ref:`collision-sensors` that act independently of the dynamics pipeline, the contact +sensor reports information that was discovered during the collision and constraint steps, extracting data +from ``mjData.{contact, efc_force}``, ignoring contacts that were filtered out by the :ref:`standard` +mechanism and produce no force. + +Contact sensor output involves three stages: **matching**, **reduction** and **extraction**. + +Matching + Selects a set of contacts from ``mjData.contact`` using criteria defined by :ref:`geom1`, + :ref:`geom2`, :ref:`body1`, :ref:`body2`, + :ref:`subtree1`, :ref:`subtree2` and + :ref:`site`. Matching applies an intersection of criteria, for example setting + :ref:`body1` and :ref:`body2` will match contacts that involve both + bodies, while setting only :ref:`geom1` will match any contacts involving that geom. Setting + :ref:`site` will match contacts that are inside the volume defined by the site; this matching + criterion can be used with {geom2, body2, subtree2}. The subtree attributes take a body name and match all contacts + involving the subtree where that body is located. Setting :ref:`subtree1` and + :ref:`subtree2` to the same body or to two bodies in the same subtree will match + self-collisions in the subtree. Specifying no matching criterion will match all contacts. + +Reduction + Reduces the number of matched contacts to exactly :ref:`num` sub-arrays, or "slots". + If less than :at:`num` contacts match, the remaining slots are set to be identically zero. Note that the default, + "unsorted" reduction criterion is potentitally non-deterministic. See :ref:`reduce` below. + +Extraction + Copies the set of fields specified by the user into each slot, see :ref:`data`. + +.. _sensor-contact-geom1: +.. _sensor-contact-geom2: + +:at:`geom1`, :at:`geom2`: :at-val:`string, optional` + Name of a geom participating in a contact. See **matching** :ref:`above `. + +.. _sensor-contact-body1: +.. _sensor-contact-body2: + +:at:`body1`, :at:`body2`: :at-val:`string, optional` + Name of a body participating in a contact. See **matching** :ref:`above `. + +.. _sensor-contact-subtree1: +.. _sensor-contact-subtree2: + +:at:`subtree1`, :at:`subtree2`: :at-val:`string, optional` + Name of a body whose subtree is participating in a contact. See **matching** :ref:`above `. Note + currently only "entire" subtrees are supported, in the sense that the specified body must be a direct child of the + world. General subtrees could be added in the future. + +.. _sensor-contact-site: + +:at:`site`: :at-val:`string, optional` + Name of a site within whose volume the contact position must be found in order to match. + See **matching** :ref:`above `. + +.. _sensor-contact-num: + +:at:`num`: :at-val:`int, "1"` + Number of contacts to report. The sensor will always report :at:`num` sequential data arrays ("slots") per contact. + The order in which contacts are reported depends on the :ref:`reduce` attribute. + +.. _sensor-contact-data: + +:at:`data`: :at-val:`[found, force, torque, dist, pos, normal, tangent], "found"` + Specification of which data field(s) to report from the selected contacts. + + - :at-val:`found` **real(1)**: This field serves two purposes. First, it indicates whether a contact was found in + this slot, 0 means not found while a positive number means found. Second, the positive value equals the number of + *matching* contacts. So if :at:`num = 3` contacts were requested but only 2 were matched, the :at-val:`found` + fields will equal (2, 2, 0); if 6 were matched they will equal (6, 6, 6). + - :at-val:`force` **real(3)**: The contact force, in the contact frame. + - :at-val:`torque` **real(3)**: The contact torque, in the contact frame. + - :at-val:`dist` **real(1)**: The penetration distance. + - :at-val:`pos`: **real(3)**: The contact position, in the global frame. + - :at-val:`normal`: **real(3)**: The contact normal direction, in the global frame. + - :at-val:`tangent`: **real(3)**: The first tangent direction, in the global frame. + In order to complete the full 3x3 contact frame, use tangent2 = cross(normal, tangent). + + Importantly, the :at:`data` attribute can contain **multiple sequential data types**, as long as the relative + order---as listed above---is maintained. For example, :at:`data` = :at-val:`"found force dist"` will return 5 numbers + per contact (the concateneated values of [found, force, dist]), while :at:`data` = :at-val:`"force found dist"` is an + error because :at-val:`found` must come before :at-val:`force`. + + Missing contacts + If less than :at:`num` contacts satisfy the matching criterion, the entire data slot is set to be identically + zero. Because most data types can take 0 as a valid value, only the zero-ness of the :at-val:`normal` and + :at-val:`tangent` unit vectors can be used to unambiguously detect an empty slot. For this reason, the + :at-val:`found` data type is in place to allow for simple detection of missing contacts. + + Size of sensordata block + Unlike other sensors, the size of the corresponding sensordata block depends on the values of its attributes + :ref:`num` and :ref:`data`. The total size of the output of a contact + sensor is the product ``num x size(selected data fields)``. For example, requesting :at:`num = 6` contacts + with :at:`data =` :at-val:`"force dist normal"` (3+1+3=7), will result in a sensordata block of 42 numbers (6 + consecutive slots x 7 numbers per slot). + + Direction convention + Because contacts create two equal-and-opposite forces between contacting bodies, there is freedom in the + choice of which body impinges on which. + + The sensor's convention is for "geom1/body1/subtree1" and "geom2/body2/subtree2" to determine the direction of + the normal. The normal always points from the first to the second. + + In the case that a direction cannot be determined, as when only a :at:`site` is used as the matching criterion, or + when both subtrees are the same, the normal direction is the same as it is in ``mjData.contact``, where the normal + points from the first to the second geom, and the two geoms are sorted according to their order in :ref:`mjtGeom`. + +.. _sensor-contact-reduce: + +:at:`reduce`: :at-val:`[none, mindist, maxforce, netforce], "none"` + Reduction criterion to use. Also see **reduction** :ref:`above `. + + - **none**: Returns the first :at:`num` contacts that satisfy the matching criterion, in the order that they appear + in ``mjData.contact``. Note that while this is the fastest option, it is also potentially non-deterministic: future + changes to collision detection code may cause the identity and order of matching contacts to change. + - **mindist**: Returns :at:`num` contacts with the smallest penetration depth, ascending order. + - **maxforce**: Returns :at:`num` contacts with the largest force norm, descending order. + - **netforce**: This reduction criterion returns one new "synthetic" contact, located at the force-weighted centroid + of all matched contacts. The frame of the contact is the global frame, so normal and tangent directions lose their + natural semantic. The force and torque are computed such that a wrench applied at the computed position will have + the same net effect as all the matching contacts combined. Note that this reduction criterion always returns + exactly one contact. + +.. _sensor-contact-cutoff: + +:at:`cutoff`: + This attribute is ignored. + +.. _sensor-contact-name: + +.. _sensor-contact-user: + +.. _sensor-contact-noise: + +:at:`name`, :at:`noise`, :at:`user`: + See :ref:`CSensor`. + .. _sensor-e_potential: :el-prefix:`sensor/` |-| **e_potential** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index d94e3029..23d05a96 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1239,6 +1239,19 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`contact | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`geom1` | :ref:`geom2` | :ref:`body1` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body2` | :ref:`subtree1` | :ref:`subtree2` | :ref:`site` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`num` | :ref:`data` | :ref:`reduce` | :ref:`cutoff` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`noise` | :ref:`user` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`e_potential | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 98b9ba9c..271496d2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,9 @@ General ^^^^^^^ - Added the :ref:`insidesite` sensor, for checking if an object is inside the volume of a site. It is useful for triggering events in surrounding environment logic. +- Added the :ref:`contact` sensor, for reporting contact information according to user-defined criteria. + The purpose of the :el:`contact` sensor is to report contact-related information in a fixed-size array. This is useful + as input to learning-based agents and in environment logic. - Removed the SdfLib plugin and the dependency on `SdfLib `__. SDFs are now supported natively in mjModel. diff --git a/doc/includes/references.h b/doc/includes/references.h index 094931fb..961326f8 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -731,6 +731,9 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMFROMTO, // segment between two geoms + // sensors for reporting contacts which occurred during the simulation + mjSENS_CONTACT, // contacts which occurred during the simulation + // global sensors mjSENS_E_POTENTIAL, // potential energy mjSENS_E_KINETIC, // kinetic energy @@ -754,6 +757,17 @@ typedef enum mjtDataType_ { // data type for sensors mjDATATYPE_AXIS, // 3D unit vector mjDATATYPE_QUATERNION // unit quaternion } mjtDataType; +typedef enum mjtConDataField_ { // data fields returned by contact sensors + mjCONDATA_FOUND = 0, // whether a contact was found + mjCONDATA_FORCE, // contact force + mjCONDATA_TORQUE, // contact torque + mjCONDATA_DIST, // contact penetration distance + mjCONDATA_POS, // contact position + mjCONDATA_NORMAL, // contact frame normal + mjCONDATA_TANGENT, // contact frame first tangent + + mjNCONDATA = 7 // number of contact sensor data fields +} mjtConDataField; typedef enum mjtSameFrame_ { // frame alignment of bodies with their children mjSAMEFRAME_NONE = 0, // no alignment mjSAMEFRAME_BODY, // frame is same as body frame diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 5a62d521..36d2ba22 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -369,6 +369,9 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMFROMTO, // segment between two geoms + // sensors for reporting contacts which occurred during the simulation + mjSENS_CONTACT, // contacts which occurred during the simulation + // global sensors mjSENS_E_POTENTIAL, // potential energy mjSENS_E_KINETIC, // kinetic energy @@ -398,6 +401,19 @@ typedef enum mjtDataType_ { // data type for sensors } mjtDataType; +typedef enum mjtConDataField_ { // data fields returned by contact sensors + mjCONDATA_FOUND = 0, // whether a contact was found + mjCONDATA_FORCE, // contact force + mjCONDATA_TORQUE, // contact torque + mjCONDATA_DIST, // contact penetration distance + mjCONDATA_POS, // contact position + mjCONDATA_NORMAL, // contact frame normal + mjCONDATA_TANGENT, // contact frame first tangent + + mjNCONDATA = 7 // number of contact sensor data fields +} mjtConDataField; + + typedef enum mjtSameFrame_ { // frame alignment of bodies with their children mjSAMEFRAME_NONE = 0, // no alignment mjSAMEFRAME_BODY, // frame is same as body frame diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index deea1826..bab08383 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -386,11 +386,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSENS_GEOMDIST', 39), ('mjSENS_GEOMNORMAL', 40), ('mjSENS_GEOMFROMTO', 41), - ('mjSENS_E_POTENTIAL', 42), - ('mjSENS_E_KINETIC', 43), - ('mjSENS_CLOCK', 44), - ('mjSENS_PLUGIN', 45), - ('mjSENS_USER', 46), + ('mjSENS_CONTACT', 42), + ('mjSENS_E_POTENTIAL', 43), + ('mjSENS_E_KINETIC', 44), + ('mjSENS_CLOCK', 45), + ('mjSENS_PLUGIN', 46), + ('mjSENS_USER', 47), ]), )), ('mjtStage', @@ -415,6 +416,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDATATYPE_QUATERNION', 3), ]), )), + ('mjtConDataField', + EnumDecl( + name='mjtConDataField', + declname='enum mjtConDataField_', + values=dict([ + ('mjCONDATA_FOUND', 0), + ('mjCONDATA_FORCE', 1), + ('mjCONDATA_TORQUE', 2), + ('mjCONDATA_DIST', 3), + ('mjCONDATA_POS', 4), + ('mjCONDATA_NORMAL', 5), + ('mjCONDATA_TANGENT', 6), + ('mjNCONDATA', 7), + ]), + )), ('mjtSameFrame', EnumDecl( name='mjtSameFrame', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9e51eb74..beb28682 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2159,6 +2159,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_FRAMEQUAT: return 4; + case mjSENS_CONTACT: case mjSENS_USER: return sensor_dim; diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index afd467d0..29f3e009 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -26,6 +26,7 @@ #include "engine/engine_io.h" #include "engine/engine_plugin.h" #include "engine/engine_ray.h" +#include "engine/engine_sort.h" #include "engine/engine_support.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -36,6 +37,26 @@ //-------------------------------- utility --------------------------------------------------------- + +typedef struct { + mjtNum criterion; // criterion for partial sort + int id; // index in d->contact + int flip; // 0: don't flip the normal, 1: flip the normal +} ContactInfo; + +// define ContactSelect: find the k smallest elements of a ContactInfo array +static int ContactInfoCompare(const ContactInfo* a, const ContactInfo* b, void* context) { + if (a->criterion < b->criterion) return -1; + if (a->criterion > b->criterion) return 1; + + if (a->id < b->id) return -1; + if (a->id > b->id) return 1; + + return 0; +} +mjPARTIAL_SORT(ContactSelect, ContactInfo, ContactInfoCompare) + + // apply cutoff after each stage static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) { // process sensors matching stage and having positive cutoff @@ -98,6 +119,8 @@ static void get_xpos_xmat(const mjData* d, mjtObj type, int id, int sensor_id, } } + + // get global quaternion of an object in mjData static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, int sensor_id, mjtNum *quat) { @@ -123,6 +146,7 @@ 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, @@ -216,6 +240,117 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], +// check if a contact body/geom matches a sensor spec (type, id) +static int checkMatch(const mjModel* m, int body, int geom, mjtObj type, int id) { + if (type == mjOBJ_UNKNOWN) return 1; + if (type == mjOBJ_SITE) return 1; // already passed site filter test + if (type == mjOBJ_GEOM) return id == geom; + if (type == mjOBJ_BODY) return id == body; + if (type == mjOBJ_XBODY) return body >= 0 && m->body_rootid[id] == m->body_rootid[body]; + return 0; +} + +// 0: no match +// 1: match, use contact normal +// -1: match, flip contact normal +static int matchContact(const mjModel* m, const mjData* d, int conid, + mjtObj type1, int id1, mjtObj type2, int id2) { + // no criterion: quick match + if (type1 == mjOBJ_UNKNOWN && type2 == mjOBJ_UNKNOWN) { + return 1; + } + + // site filter + if (type1 == mjOBJ_SITE) { + if (!mju_insideGeom(d->site_xpos + 3 * id1, d->site_xmat + 9 * id1, + m->site_size + 3 * id1, m->site_type[id1], d->contact[conid].pos)) { + return 0; + } + } + + // get geom, body ids + int geom1 = d->contact[conid].geom[0]; + int geom2 = d->contact[conid].geom[1]; + int body1 = geom1 >= 0 ? m->geom_bodyid[geom1] : -1; + int body2 = geom2 >= 0 ? m->geom_bodyid[geom2] : -1; + + // check match of sensor objects with contact objects + int match11 = checkMatch(m, body1, geom1, type1, id1); + int match12 = checkMatch(m, body2, geom2, type1, id1); + int match21 = checkMatch(m, body1, geom1, type2, id2); + int match22 = checkMatch(m, body2, geom2, type2, id2); + + // if a sensor object is specified, it must be involved in the contact + if (!match11 && !match12) return 0; + if (!match21 && !match22) return 0; + + // determine direction + if (type1 != mjOBJ_UNKNOWN && type2 != mjOBJ_UNKNOWN) { + // both obj1 and obj2 specified: direction depends on order + int order_regular = match11 && match22; + int order_reverse = match12 && match21; + if (order_regular && !order_reverse) return 1; + if (order_reverse && !order_regular) return -1; + if (order_regular && order_reverse) return 1; // ambiguous, return 1 + } else if (type1 != mjOBJ_UNKNOWN) { + // only obj1 specified: normal points away from obj1 + return match11 ? 1 : -1; + } else if (type2 != mjOBJ_UNKNOWN) { + // only obj2 specified: normal points towards obj2 + return match22 ? 1 : -1; + } + + // should not occur, all conditions are covered above + return 0; +} + + + +// fill in output data for contact sensor for all fields +// if flg_flip > 0, normal/tangent rotate 180 about frame[2] +// force/torque flip-z s.t. force is equal-and-opposite in new contact frame +static void copySensorData(const mjModel* m, const mjData* d, + mjtNum* data[mjNCONDATA], int id, int flg_flip, int nfound) { + // found flag + if (data[mjCONDATA_FOUND]) *data[mjCONDATA_FOUND] = nfound; + + // contact force and torque + if (data[mjCONDATA_FORCE] || data[mjCONDATA_TORQUE]) { + mjtNum forcetorque[6]; + mj_contactForce(m, d, id, forcetorque); + if (data[mjCONDATA_FORCE]) { + mju_copy3(data[mjCONDATA_FORCE], forcetorque); + if (flg_flip) data[mjCONDATA_FORCE][2] *= -1; + } + if (data[mjCONDATA_TORQUE]) { + mju_copy3(data[mjCONDATA_TORQUE], forcetorque+3); + if (flg_flip) data[mjCONDATA_TORQUE][2] *= -1; + } + } + + // contact penetration distance + if (data[mjCONDATA_DIST]) { + *data[mjCONDATA_DIST] = d->contact[id].dist; + } + + // contact position + if (data[mjCONDATA_POS]) { + mju_copy3(data[mjCONDATA_POS], d->contact[id].pos); + } + + // contact normal + if (data[mjCONDATA_NORMAL]) { + mju_copy3(data[mjCONDATA_NORMAL], d->contact[id].frame); + if (flg_flip) mju_scl3(data[mjCONDATA_NORMAL], data[mjCONDATA_NORMAL], -1); + } + + // contact first tangent + if (data[mjCONDATA_TANGENT]) { + mju_copy3(data[mjCONDATA_TANGENT], d->contact[id].frame+3); + if (flg_flip) mju_scl3(data[mjCONDATA_TANGENT], data[mjCONDATA_TANGENT], -1); + } +} + //-------------------------------- sensor ---------------------------------------------------------- // position-dependent sensors @@ -709,7 +844,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { int rootid, bodyid, objtype, objid, adr, nusersensor = 0; int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu; mjtNum tmp[6], conforce[6], conray[3], frc; - mjContact* con; + const mjContact* con; // disabled sensors: return if (mjDISABLED(mjDSBL_SENSOR)) { @@ -792,6 +927,81 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { } break; + case mjSENS_CONTACT: // contact + { + // prepare sizes and indices, check consistency + int dataspec = m->sensor_intprm[i*mjNSENS]; + int size = mju_condataSize(dataspec); // size of each slot + int dim = m->sensor_dim[i]; // total sensor array dimension + int num = dim / size; // number of slots + int reftype = m->sensor_reftype[i]; + int refid = m->sensor_refid[i]; + int reduce = m->sensor_intprm[i*mjNSENS+1]; + + // clear all outputs, prepare data pointers + mjtNum* ptr = d->sensordata + adr; + mju_zero(ptr, dim); + mjtNum* data[mjNCONDATA] = {NULL}; + for (int j=0; j < mjNCONDATA; j++) { + if (dataspec & (1 << j)) { + data[j] = ptr; + ptr += mjCONDATA_SIZE[j]; + } + } + + // prepare for matching loop + int nmatch = 0; + mj_markStack(d); + ContactInfo *match = mjSTACKALLOC(d, d->ncon, ContactInfo); + + // find matching contacts + for (int j=0; j < d->ncon; j++) { + // check match condition + int match_j = matchContact(m, d, j, objtype, objid, reftype, refid); + if (!match_j) { + continue; + } + + // save id and flip flag + match[nmatch].id = j; + match[nmatch].flip = match_j < 0; + + // save sorting criterion, if required + if (reduce) { + if (reduce == 1) { + match[nmatch].criterion = d->contact[j].dist; + } else { + mjtNum forcetorque[6]; + mj_contactForce(m, d, j, forcetorque); + match[nmatch].criterion = -mju_dot3(forcetorque, forcetorque); + } + } + + // increment number of matching contacts + nmatch++; + } + + // number of slots to be filled + int nslot = mjMIN(num, nmatch); + + // partial sort to get bottom nslot contacts given reduction criterion + if (reduce) { + ContactInfo *heap = mjSTACKALLOC(d, nslot, ContactInfo); + ContactSelect(match, heap, nmatch, nslot, NULL); + } + + // copy data into slots, increment pointers + for (int j=0; j < nslot; j++) { + copySensorData(m, d, data, match[j].id, match[j].flip, nmatch); + for (int k=0; k < mjNCONDATA; k++) { + if (data[k]) data[k] += size; + } + } + + mj_freeStack(d); + } + break; + case mjSENS_ACCELEROMETER: // accelerometer // tmp = site acceleration, in site frame mj_objectAcceleration(m, d, mjOBJ_SITE, objid, tmp, 1); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a0a732f6..204aa28a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -97,6 +97,17 @@ const char* mjTIMERSTRING[mjNTIMER]= { }; +// size of contact data fields +const int mjCONDATA_SIZE[mjNCONDATA] = { + 1, // mjCONDATA_FOUND + 3, // mjCONDATA_FORCE + 3, // mjCONDATA_TORQUE + 1, // mjCONDATA_DIST + 3, // mjCONDATA_POS + 3, // mjCONDATA_NORMAL + 3 // mjCONDATA_TANGENT +}; + //-------------------------- get/set state --------------------------------------------------------- @@ -1576,3 +1587,16 @@ const char* mj_versionString(void) { static const char versionstring[] = mjVERSIONSTRING; return versionstring; } + + + +// return total size of data in a contact sensor bitfield specification +int mju_condataSize(int dataspec) { + int size = 0; + for (int i=0; i < mjNCONDATA; i++) { + if (dataspec & (1 << i)) { + size += mjCONDATA_SIZE[i]; + } + } + return size; +} diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 3d5f14c3..5dc8036e 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -29,6 +29,9 @@ MJAPI extern const char* mjDISABLESTRING[mjNDISABLE]; MJAPI extern const char* mjENABLESTRING[mjNENABLE]; MJAPI extern const char* mjTIMERSTRING[mjNTIMER]; +// arrays +MJAPI extern const int mjCONDATA_SIZE[mjNCONDATA]; // TODO(tassa): expose in public header? + //-------------------------- get/set state --------------------------------------------------------- @@ -199,6 +202,10 @@ MJAPI int mj_version(void); // current version of MuJoCo as a null-terminated string MJAPI const char* mj_versionString(void); + +// return total size of data fields in a contact sensor bitfield specification +MJAPI int mju_condataSize(int dataSpec); + #ifdef __cplusplus } #endif diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 13a722de..8922d508 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -37,6 +37,7 @@ #include "lodepng.h" #include "cc/array_safety.h" #include "engine/engine_passive.h" +#include "engine/engine_support.h" #include #include #include "user/user_api.h" @@ -6686,6 +6687,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) { type != mjSENS_E_KINETIC && type != mjSENS_CLOCK && type != mjSENS_PLUGIN && + type != mjSENS_CONTACT && type != mjSENS_USER) { throw mjCError(this, "invalid type in sensor"); } @@ -7043,6 +7045,60 @@ void mjCSensor::Compile(void) { } break; + case mjSENS_CONTACT: + // check first matching criterion + if (objtype != mjOBJ_SITE && + objtype != mjOBJ_BODY && + objtype != mjOBJ_XBODY && + objtype != mjOBJ_GEOM && + objtype != mjOBJ_UNKNOWN) { + throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site"); + } + + // check that subtree1 is a full tree + if (objtype == mjOBJ_XBODY && static_cast(obj)->GetParent()->id != 0) { + throw mjCError(this, "subtree1 must be a child of the world"); + } + + // check second matching criterion + if (reftype != mjOBJ_BODY && + reftype != mjOBJ_XBODY && + reftype != mjOBJ_GEOM && + reftype != mjOBJ_UNKNOWN) { + throw mjCError(this, "second matching criterion: if set, must be (x)body or geom"); + } + + // check that subtree2 is a full tree + if (reftype == mjOBJ_XBODY && static_cast(ref)->GetParent()->id != 0) { + throw mjCError(this, "subtree2 must be a child of the world"); + } + + // check for non-positive dim + if (dim <= 0) { + throw mjCError(this, "dim must be positive in sensor (got %d)", "", dim); + } + + // check for dim correctness + if (dim % mju_condataSize(intprm[0]) != 0) { + throw mjCError(this, "dim %d does not match data spec", "", dim); + } + + // check for reduce correctness + if (intprm[1] < 0 || intprm[1] > 3) { + throw mjCError(this, "unknown reduction criterion. got %d, " + "expected one of {0, 1, 2, 3}", "", intprm[1]); + } + + // netforce not yet implemented + if (intprm[1] == 3) { + throw mjCError(this, "netforce reduction is not yet implemented\n" + "please contact the developers if you need this feature"); + } + + needstage = mjSTAGE_ACC; + datatype = mjDATATYPE_REAL; + break; + case mjSENS_E_POTENTIAL: case mjSENS_E_KINETIC: case mjSENS_CLOCK: @@ -7054,13 +7110,12 @@ void mjCSensor::Compile(void) { case mjSENS_USER: // check for negative dim if (dim < 0) { - throw mjCError(this, "sensor dim must be positive in sensor"); + throw mjCError(this, "sensor dim must be non-negative in sensor"); } // make sure dim is consistent with datatype if (datatype == mjDATATYPE_AXIS && dim != 3) { - throw mjCError(this, - "datatype AXIS requires dim=3 in sensor"); + throw mjCError(this, "datatype AXIS requires dim=3 in sensor"); } if (datatype == mjDATATYPE_QUATERNION && dim != 4) { throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor"); diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index f6e41683..10dda197 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -43,6 +43,7 @@ extern const int gain_sz; extern const int bias_sz; extern const int stage_sz; extern const int datatype_sz; +extern const int reduce_sz; extern const mjMap angle_map[]; extern const mjMap enable_map[]; extern const mjMap bool_map[]; @@ -70,6 +71,8 @@ extern const mjMap gain_map[]; extern const mjMap bias_map[]; extern const mjMap stage_map[]; extern const mjMap datatype_map[]; +extern const mjMap condata_map[]; +extern const mjMap reduce_map[]; extern const mjMap meshtype_map[]; extern const mjMap meshinertia_map[]; extern const mjMap flexself_map[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5acdda86..be465213 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -34,6 +34,7 @@ #include #include #include "engine/engine_plugin.h" +#include "engine/engine_support.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include @@ -481,6 +482,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, + {"contact", "*", "12", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site", + "num", "data", "reduce", "cutoff", "noise", "user"}, {"e_potential", "*", "4", "name", "cutoff", "noise", "user"}, {"e_kinetic", "*", "4", "name", "cutoff", "noise", "user"}, {"clock", "*", "4", "name", "cutoff", "noise", "user"}, @@ -606,6 +609,7 @@ const mjMap texrole_map[texrole_sz] = { {"orm", mjTEXROLE_ORM}, }; + // integrator type const int integrator_sz = 4; const mjMap integrator_map[integrator_sz] = { @@ -615,6 +619,7 @@ const mjMap integrator_map[integrator_sz] = { {"implicitfast", mjINT_IMPLICITFAST} }; + // cone type const int cone_sz = 2; const mjMap cone_map[cone_sz] = { @@ -743,6 +748,28 @@ const mjMap datatype_map[datatype_sz] = { }; +// contact data type +const mjMap condata_map[mjNCONDATA] = { + {"found", mjCONDATA_FOUND}, + {"force", mjCONDATA_FORCE}, + {"torque", mjCONDATA_TORQUE}, + {"dist", mjCONDATA_DIST}, + {"pos", mjCONDATA_POS}, + {"normal", mjCONDATA_NORMAL}, + {"tangent", mjCONDATA_TANGENT} +}; + + +// contact reduction type +const int reduce_sz = 4; +const mjMap reduce_map[reduce_sz] = { + {"none", 0}, + {"mindist", 1}, + {"maxforce", 2}, + {"netforce", 3} +}; + + // LR mode const int lrmode_sz = 4; const mjMap lrmode_map[lrmode_sz] = { @@ -4132,6 +4159,74 @@ void mjXReader::Sensor(XMLElement* section) { } } + // sensor for contacts; attached to geoms or bodies or a site + else if (type == "contact") { + // first matching criterion + bool has_site = ReadAttrTxt(elem, "site", objname); + bool has_body1 = ReadAttrTxt(elem, "body1", objname); + bool has_subtree1 = ReadAttrTxt(elem, "subtree1", objname); + bool has_geom1 = ReadAttrTxt(elem, "geom1", objname); + if (has_site + has_body1 + has_subtree1 + has_geom1 > 1) { + throw mjXError(elem, "at most one of (geom1, body1, subtree1, site) can be specified"); + } + if (has_site) { sensor->objtype = mjOBJ_SITE; } + else if (has_body1) { sensor->objtype = mjOBJ_BODY; } + else if (has_subtree1) { sensor->objtype = mjOBJ_XBODY; } + else if (has_geom1) { sensor->objtype = mjOBJ_GEOM; } + else { sensor->objtype = mjOBJ_UNKNOWN; } + + // second matching criterion + bool has_body2 = ReadAttrTxt(elem, "body2", refname); + bool has_subtree2 = ReadAttrTxt(elem, "subtree2", refname); + bool has_geom2 = ReadAttrTxt(elem, "geom2", refname); + if (has_body2 + has_subtree2 + has_geom2 > 1) { + throw mjXError(elem, "at most one of (geom2, body2, subtree2) can be specified"); + } + if (has_body2) { sensor->reftype = mjOBJ_BODY; } + else if (has_subtree2) { sensor->reftype = mjOBJ_XBODY; } + else if (has_geom2) { sensor->reftype = mjOBJ_GEOM; } + else { sensor->reftype = mjOBJ_UNKNOWN; } + + // process data specification (intprm[0]) + int dataspec = 1 << mjCONDATA_FOUND; + std::vector condata(mjNCONDATA); + int nkeys = MapValues(elem, "data", condata.data(), condata_map, mjNCONDATA); + if (nkeys) { + dataspec = 1 << condata[0]; + + // check ordering while adding bits to dataspec + for (int i = 1; i < nkeys; ++i) { + if (condata[i] <= condata[i-1]) { + std::string correct_order; + for (int j = 0; j < mjNCONDATA; ++j) { + correct_order += condata_map[j].key; + if (j < mjNCONDATA - 1) correct_order += ", "; + } + throw mjXError(elem, "data attributes must be in order: %s", correct_order.c_str()); + } + dataspec |= 1 << condata[i]; + } + } + sensor->intprm[0] = dataspec; + + // number of contacts, sensor dim + sensor->dim = 1; + ReadAttrInt(elem, "num", &sensor->dim); + if (sensor->dim <= 0) { + throw mjXError(elem, "'num' must be positive in sensor"); + } + sensor->dim *= mju_condataSize(dataspec); + + // reduction type (intprm[1]) + sensor->intprm[1] = 0; + if (MapValue(elem, "reduce", &n, reduce_map, reduce_sz)) { + sensor->intprm[1] = n; + } + + // sensor type + sensor->type = mjSENS_CONTACT; + } + // global sensors else if (type == "e_potential") { sensor->type = mjSENS_E_POTENTIAL; diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 4bb3f913..4653b590 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -101,7 +101,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 239 +#define nMJCF 240 extern const char* MJCF[nMJCF][mjXATTRNUM]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 88dd6fed..b7cb6307 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -28,6 +28,7 @@ #include #include "engine/engine_io.h" #include "engine/engine_plugin.h" +#include "engine/engine_support.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_model.h" @@ -76,7 +77,7 @@ static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) { // top level sections std::array sections = { - "", "", "objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname()); break; - + case mjSENS_CONTACT: + { + elem = InsertEnd(section, "contact"); + if (sensor->objtype == mjOBJ_BODY) { + WriteAttrTxt(elem, "body1", sensor->get_objname()); + } else if (sensor->objtype == mjOBJ_XBODY) { + WriteAttrTxt(elem, "subtree1", sensor->get_objname()); + } else if (sensor->objtype == mjOBJ_GEOM) { + WriteAttrTxt(elem, "geom1", sensor->get_objname()); + } else if (sensor->objtype == mjOBJ_SITE) { + WriteAttrTxt(elem, "site", sensor->get_objname()); + } + if (sensor->reftype == mjOBJ_BODY) { + WriteAttrTxt(elem, "body2", sensor->get_refname()); + } else if (sensor->reftype == mjOBJ_XBODY) { + WriteAttrTxt(elem, "subtree2", sensor->get_refname()); + } else if (sensor->reftype == mjOBJ_GEOM) { + WriteAttrTxt(elem, "geom2", sensor->get_refname()); + } + int dataspec = sensor->intprm[0]; + WriteAttrInt(elem, "num", sensor->dim / mju_condataSize(dataspec), 1); + int data[mjNCONDATA]; + int ndata = 0; + for (int i=0; i < mjNCONDATA; i++) { + if (dataspec & (1 << i)) { + data[ndata++] = i; + } + } + WriteAttrKeys(elem, "data", condata_map, mjNCONDATA, data, ndata, 0); + WriteAttrKey(elem, "reduce", reduce_map, reduce_sz, sensor->intprm[1], 0); + } + break; // global sensors case mjSENS_E_POTENTIAL: elem = InsertEnd(section, "potential"); diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index ad1a3546..407fc30a 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -792,7 +792,7 @@ XMLElement* mjXUtil::FindSubElem(XMLElement* elem, std::string name, bool requir -// find attribute, translate key, return int value +// find attribute, translate key into data, return true if found bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data, const mjMap* map, int mapSz, bool required) { // get attribute text @@ -814,6 +814,42 @@ bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data, +// find attribute, translate unique space-separated keys to data, return number of keys found +int mjXUtil::MapValues(XMLElement* elem, const char* attr, int* data, + const mjMap* map, int mapSz, bool required) { + // get attribute text + auto maybe_text = ReadAttrStr(elem, attr, required); + if (!maybe_text.has_value()) { + return 0; + } + + std::string text = maybe_text.value(); + std::istringstream strm(text); + std::string key; + std::set found_keys; + int count = 0; + + while (strm >> key) { + if (found_keys.count(key)) { + throw mjXError(elem, "duplicate keyword: '%s'"); + return 0; + } + + int value = FindKey(map, mapSz, key); + if (value == -1) { + throw mjXError(elem, "invalid keyword: '%s'"); + return 0; + } + + found_keys.insert(key); + data[count++] = value; + } + + return count; +} + + + //---------------------------------- write functions ----------------------------------------------- // check if double is int @@ -970,3 +1006,20 @@ void mjXUtil::WriteAttrKey(XMLElement* elem, std::string name, WriteAttrTxt(elem, name, FindValue(map, mapsz, data)); } + + +// write attribute- space-separated keywords +void mjXUtil::WriteAttrKeys(XMLElement* elem, std::string name, const mjMap* map, + int mapsz, int* data, int ndata, int def) { + // skip default + if (ndata == 1 && data[0] == def) { + return; + } + + std::string text = FindValue(map, mapsz, data[0]); + for (int i = 1; i < ndata; ++i) { + text += " " + FindValue(map, mapsz, data[i]); + } + + WriteAttrTxt(elem, name, text); +} diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index 37cd843b..b0e38c4c 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -183,6 +183,10 @@ class mjXUtil { static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data, const mjMap* map, int mapSz, bool required = false); + // find attribute, translate unique space-separated keys to data, return number of keys found + static int MapValues(tinyxml2::XMLElement* elem, const char* attr, int* data, + const mjMap* map, int mapSz, bool required = false); + // write attribute- any type template static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data, @@ -204,6 +208,10 @@ class mjXUtil { static void WriteAttrKey(tinyxml2::XMLElement* elem, std::string name, const mjMap* map, int mapsz, int data, int def = -12345); + // write attribute- space-separated keywords + static void WriteAttrKeys(XMLElement* elem, std::string name, const mjMap* map, + int mapsz, int* data, int ndata, int def = -12345); + private: template static bool ReadAttrValues(tinyxml2::XMLElement* elem, const char* attr, diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index b6c8faa1..ef9a1eac 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -14,6 +14,8 @@ // Tests for engine/engine_sensor.c. +#include +#include #include #include @@ -28,21 +30,39 @@ namespace mujoco { namespace { +using ::std::string; +using ::std::vector; + +using ::testing::DoubleNear; +using ::testing::ElementsAre; +using ::testing::ElementsAreArray; +using ::testing::HasSubstr; +using ::testing::IsNull; +using ::testing::Not; +using ::testing::NotNull; +using ::testing::Pointwise; +using ::testing::SizeIs; +using ::testing::StrEq; +using ::testing::WhenSorted; + const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers // returns as a vector the measured values from sensor with index `id` -static std::vector GetSensor(const mjModel* model, - const mjData* data, - int id) { - return std::vector( +static vector GetSensor(const mjModel* model, + const mjData* data, int id) { + return vector( data->sensordata + model->sensor_adr[id], data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); } -using ::testing::Pointwise; -using ::testing::DoubleNear; -using ::testing::NotNull; -using ::testing::StrEq; +// returns as a vector the measured values from sensor with name `name +static vector GetSensor(const mjModel* model, + const mjData* data, const char* name) { + int id = mj_name2id(model, mjOBJ_SENSOR, name); + return vector( + data->sensordata + model->sensor_adr[id], + data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); +} using SensorTest = MujocoTest; @@ -112,13 +132,13 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMat) { mj_forward(model, data); // compare actual and expected values - std::vector pos = GetSensor(model, data, 0); + vector pos = GetSensor(model, data, 0); EXPECT_THAT(pos, Pointwise(DoubleNear(tol), {5, 5, 0})); - std::vector xaxis = GetSensor(model, data, 1); + vector xaxis = GetSensor(model, data, 1); EXPECT_THAT(xaxis, Pointwise(DoubleNear(tol), {0, -1, 0})); - std::vector yaxis = GetSensor(model, data, 2); + vector yaxis = GetSensor(model, data, 2); EXPECT_THAT(yaxis, Pointwise(DoubleNear(tol), {1, 0, 0})); mj_deleteData(data); @@ -155,7 +175,7 @@ TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) { mju_mat2Quat(converted_quat, mat); // compare quaternion sensor and quat derived from orientation matrix - std::vector quat = GetSensor(model, data, 3); + vector quat = GetSensor(model, data, 3); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), converted_quat)); mj_deleteData(data); @@ -199,7 +219,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { // call mj_forward, save global sensors (colocated with reference frame) mj_forward(model, data); - std::vector expected_values(data->sensordata, data->sensordata+nsensordata/2); + vector expected_values(data->sensordata, data->sensordata+nsensordata/2); // set qpos to arbitrary values, call mj_forward for (int i=0; i < 7; i++) { @@ -208,7 +228,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { mj_forward(model, data); // get values from relative sensors after moving the object - std::vector actual_values(data->sensordata+nsensordata/2, + vector actual_values(data->sensordata+nsensordata/2, data->sensordata+nsensordata); // object and reference have moved together, we expect values to not change @@ -245,7 +265,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) { mj_forward(model, data); // compare to expected values - std::vector linvel = GetSensor(model, data, 0); + vector linvel = GetSensor(model, data, 0); const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0}; EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel)); @@ -278,7 +298,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { mj_forward(model, data); // obj and ref rotate together, relative angular velocities should be zero - std::vector angvel = GetSensor(model, data, 0); + vector angvel = GetSensor(model, data, 0); EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0})); mj_deleteData(data); @@ -314,7 +334,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) { mj_forward(model, data); // obj and ref rotate on same axis, we can just difference the velocities - std::vector angvel = GetSensor(model, data, 0); + vector angvel = GetSensor(model, data, 0); const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0}; EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel)); @@ -358,8 +378,8 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { mj_forward(model, data); // save measured linear and angular velocities as vectors - std::vector linvel = GetSensor(model, data, 2); - std::vector angvel = GetSensor(model, data, 3); + vector linvel = GetSensor(model, data, 2); + vector angvel = GetSensor(model, data, 3); // save current position, quaternion as arrays mjtNum pos0[3], quat0[4]; @@ -615,20 +635,20 @@ TEST_F(SensorTest, CollisionSequential) { mjtNum eps = 1e-14; EXPECT_THAT(GetSensor(model, data, 3), - Pointwise(DoubleNear(eps), std::vector{0, 0, 1})); + Pointwise(DoubleNear(eps), vector{0, 0, 1})); EXPECT_THAT(GetSensor(model, data, 4), - Pointwise(DoubleNear(eps), std::vector{0, 0, -1})); + Pointwise(DoubleNear(eps), vector{0, 0, -1})); EXPECT_THAT(GetSensor(model, data, 5), - Pointwise(DoubleNear(eps), std::vector{1, 0, 0})); + Pointwise(DoubleNear(eps), vector{1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 6), Pointwise(DoubleNear(eps), - std::vector{0, 0, 0, 0, 0, .8})); + vector{0, 0, 0, 0, 0, .8})); EXPECT_THAT(GetSensor(model, data, 7), Pointwise(DoubleNear(eps), - std::vector{1, 0, .7, 1, 0, 0})); + vector{1, 0, .7, 1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 8), Pointwise(DoubleNear(eps), - std::vector{.2, 0, 1, .7, 0, 1})); + vector{.2, 0, 1, .7, 0, 1})); EXPECT_THAT(GetSensor(model, data, 9), Pointwise(DoubleNear(eps), GetSensor(model, data, 0))); @@ -647,6 +667,202 @@ TEST_F(SensorTest, CollisionSequential) { mj_deleteModel(model); } +TEST_F(SensorTest, BadContact) { + string xml_template = R"( + + + + + + + + + + + + + + + + + )"; + + struct Case { + string bad_attr; + string expected_error; + }; + + Case test_cases[] = { + {"geom1='sphere1' geom2='sphere2' data='dist force normal'", + "must be in order: found, force, torque, dist, pos, normal, tangent"}, + {"geom1='sphere1' geom2='sphere2' num='-3'", + "'num' must be positive in sensor"}, + {"geom1='sphere1' geom2='sphere2' site='site'", + "at most one of (geom1, body1, subtree1, site) can be specified"}, + {"geom2='sphere1' body2='body'", + "at most one of (geom2, body2, subtree2) can be specified"}, + {"subtree1='non_root'", + "must be a child of the world"} + }; + + for (const auto& test : test_cases) { + string xml = xml_template; + size_t pos = xml.find("BAD_ATTR"); + ASSERT_NE(pos, string::npos); + xml.replace(pos, 8, test.bad_attr); + + char error[1024]; + mjModel* model = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(model, IsNull()) << "Test case: " << test.bad_attr; + EXPECT_THAT(error, HasSubstr(test.expected_error)) + << "Test case: " << test.bad_attr; + } +} + +TEST_F(SensorTest, Contact) { + const string xml_path = + GetTestDataFilePath("engine/testdata/sensor/contact.xml"); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + + for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { + model->opt.cone = cone; + + mj_resetData(model, data); + while (data->time < 2) { + mj_step(model, data); + } + + vector all = GetSensor(model, data, "all"); + EXPECT_EQ(all, vector{4}); + + vector world = GetSensor(model, data, "world"); + EXPECT_EQ(world, vector{3}); + + vector b1 = GetSensor(model, data, "b1"); + EXPECT_EQ(b1, vector{3}); + + vector g1 = GetSensor(model, data, "g1"); + EXPECT_EQ(g1, vector{3}); + + vector b1g2 = GetSensor(model, data, "b1:g2"); + EXPECT_EQ(b1g2, vector{1}); + + vector b1world = GetSensor(model, data, "b1:world"); + EXPECT_EQ(b1world, vector{2}); + + vector site = GetSensor(model, data, "site"); + EXPECT_EQ(site, vector{2}); + + vector sitewall = GetSensor(model, data, "site:wall"); + EXPECT_EQ(sitewall, vector{1}); + + mjtNum tol = 1e-4; + vector wall = GetSensor(model, data, "wall"); + EXPECT_THAT(wall, Pointwise(DoubleNear(tol), {1, 8, 0, 0, -1, 0, 0, + 0, 0, 0, 0, 0, 0, 0})); + + // normals points *away* from b2 (towards floor / b1) + vector b2 = GetSensor(model, data, "b2"); + EXPECT_THAT(b2, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, -1, + 4, 0, 0, 1, 0, 0})); + + // normal points *towards* b2 + vector b2f = GetSensor(model, data, "b2_flipped"); + EXPECT_THAT(b2f, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, 1, + 4, 0, 0, -1, 0, 0})); + + vector b2r = GetSensor(model, data, "b2_reduced"); + EXPECT_THAT(b2r, Pointwise(DoubleNear(tol), {4, 0, 0, -1, 0, 0})); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, ContactSorted) { + const string xml_path = + GetTestDataFilePath("engine/testdata/sensor/contact_sorted.xml"); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + while (data->time < .5) { + mj_step(model, data); + } + + vector unsorted = GetSensor(model, data, "unsorted"); + EXPECT_THAT(unsorted, SizeIs(4)); + EXPECT_THAT(unsorted, Not(WhenSorted(ElementsAreArray(unsorted)))); + + vector sorted = GetSensor(model, data, "sorted dist"); + EXPECT_THAT(sorted, SizeIs(4)); + EXPECT_THAT(sorted, WhenSorted(ElementsAreArray(sorted))); + + vector sorted_force = GetSensor(model, data, "sorted force"); + EXPECT_THAT(sorted_force, SizeIs(12)); + vector nnorms; + for (size_t i = 0; i < sorted_force.size(); i += 3) { + nnorms.push_back(-sorted_force[i]*sorted_force[i] + + -sorted_force[i+1]*sorted_force[i+1] + + -sorted_force[i+2]*sorted_force[i+2]); + } + EXPECT_THAT(nnorms, WhenSorted(ElementsAreArray(nnorms))); + + vector smallest = GetSensor(model, data, "smallest dist"); + EXPECT_THAT(smallest, SizeIs(1)); + EXPECT_EQ(smallest[0], sorted[0]); + + vector largest = GetSensor(model, data, "largest force"); + EXPECT_THAT(largest, SizeIs(3)); + EXPECT_THAT(largest, ElementsAre(sorted_force[0], + sorted_force[1], + sorted_force[2])); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, ContactSubtree) { + const string xml_path = + GetTestDataFilePath("engine/testdata/sensor/contact_subtree.xml"); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + + while (data->time < 0.2) { + mj_step(model, data); + + int all = GetSensor(model, data, "all")[0]; + int w_t1 = GetSensor(model, data, "w_t1")[0]; + int w_t2 = GetSensor(model, data, "w_t2")[0]; + int t1 = GetSensor(model, data, "t1")[0]; + int t2 = GetSensor(model, data, "t2")[0]; + int t1_t1 = GetSensor(model, data, "t1_t1")[0]; + int t2_t2 = GetSensor(model, data, "t2_t2")[0]; + int t1_t2 = GetSensor(model, data, "t1_t2")[0]; + int t2_t1 = GetSensor(model, data, "t2_t1")[0]; + + // compute the number of first tree contacts in two different ways + EXPECT_EQ(t1, w_t1 + t1_t1 + t1_t2); + + // compute the number of second tree contacts in two different ways + EXPECT_EQ(t2, w_t2 + t2_t2 + t2_t1); + + // compute the number of all contacts in two different ways + EXPECT_EQ(all, w_t1 + w_t2 + t1_t1 + t2_t2 + t1_t2); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(SensorTest, CameraProjection) { constexpr char xml[] = R"( @@ -725,7 +941,7 @@ TEST_F(SensorTest, InsideSite) { for (int i = 0; i < 5; i++) { data->qpos[0] = hpos[i]; mj_forward(model, data); - std::vector expected(5, 0.0); + vector expected(5, 0.0); expected[i] = 1.0; EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); } diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 71b8e820..0cd50bec 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -947,5 +947,15 @@ TEST_F(SupportTest, SetKeyframe) { mj_deleteModel(model); } +TEST_F(SupportTest, ContactSensorDim) { + int dataSpec = 1 << mjCONDATA_FOUND | + 1 << mjCONDATA_FORCE | + 1 << mjCONDATA_DIST | + 1 << mjCONDATA_POS | + 1 << mjCONDATA_TANGENT; + + EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/sensor/contact.xml b/test/engine/testdata/sensor/contact.xml new file mode 100644 index 00000000..ab2b812b --- /dev/null +++ b/test/engine/testdata/sensor/contact.xml @@ -0,0 +1,35 @@ + + diff --git a/test/engine/testdata/sensor/contact_sorted.xml b/test/engine/testdata/sensor/contact_sorted.xml new file mode 100644 index 00000000..426fc2a9 --- /dev/null +++ b/test/engine/testdata/sensor/contact_sorted.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sensor/contact_subtree.xml b/test/engine/testdata/sensor/contact_subtree.xml new file mode 100644 index 00000000..a6d39e55 --- /dev/null +++ b/test/engine/testdata/sensor/contact_subtree.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sensor/humanoid.xml b/test/engine/testdata/sensor/humanoid.xml new file mode 100644 index 00000000..a2c4a16f --- /dev/null +++ b/test/engine/testdata/sensor/humanoid.xml @@ -0,0 +1,267 @@ + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0321e163..f6bcfb84 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -394,11 +394,12 @@ public enum mjtSensor : int{ mjSENS_GEOMDIST = 39, mjSENS_GEOMNORMAL = 40, mjSENS_GEOMFROMTO = 41, - mjSENS_E_POTENTIAL = 42, - mjSENS_E_KINETIC = 43, - mjSENS_CLOCK = 44, - mjSENS_PLUGIN = 45, - mjSENS_USER = 46, + mjSENS_CONTACT = 42, + mjSENS_E_POTENTIAL = 43, + mjSENS_E_KINETIC = 44, + mjSENS_CLOCK = 45, + mjSENS_PLUGIN = 46, + mjSENS_USER = 47, } public enum mjtStage : int{ mjSTAGE_NONE = 0, @@ -412,6 +413,16 @@ public enum mjtDataType : int{ mjDATATYPE_AXIS = 2, mjDATATYPE_QUATERNION = 3, } +public enum mjtConDataField : int{ + mjCONDATA_FOUND = 0, + mjCONDATA_FORCE = 1, + mjCONDATA_TORQUE = 2, + mjCONDATA_DIST = 3, + mjCONDATA_POS = 4, + mjCONDATA_NORMAL = 5, + mjCONDATA_TANGENT = 6, + mjNCONDATA = 7, +} public enum mjtSameFrame : int{ mjSAMEFRAME_NONE = 0, mjSAMEFRAME_BODY = 1,