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

Add contact sensor.

Browse Source 
PiperOrigin-RevId: 783011982
Change-Id: Ica56fe9d520fa1d1ee7338e09148b1a55a049912
This commit is contained in:
Yuval Tassa
2025-07-14 13:01:55 -07:00
committed by Copybara-Service
parent e441868dad
commit d0e4771c8c
25 changed files with 1343 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APItypes.rst
+10
View File
@@ -367,6 +367,16 @@ These are the possible sensor data types, used in ``mjData.sensor_datatype``.
.. mujoco-include:: mjtDataType .. mujoco-include:: mjtDataType
.. _mjtConDataField:
mjtConDataField
~~~~~~~~~~~~~~~
Types of data fields returned by contact sensors.
.. mujoco-include:: mjtConDataField
.. _mjtSameFrame: .. _mjtSameFrame:
mjtSameFrame mjtSameFrame
doc/XMLreference.rst
+148 -3
View File
@@ -7240,7 +7240,7 @@ See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-distance-user: .. _sensor-distance-user:
:at:`name`, :at:`noise`, :at:`user` :at:`name`, :at:`noise`, :at:`user`:
See :ref:`CSensor`. See :ref:`CSensor`.
@@ -7287,7 +7287,7 @@ See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-normal-user: .. _sensor-normal-user:
:at:`name`, :at:`noise`, :at:`user` :at:`name`, :at:`noise`, :at:`user`:
See :ref:`CSensor`. See :ref:`CSensor`.
@@ -7335,10 +7335,155 @@ See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-fromto-user: .. _sensor-fromto-user:
:at:`name`, :at:`noise`, :at:`user` :at:`name`, :at:`noise`, :at:`user`:
See :ref:`CSensor`. 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<coSelection>`
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<sensor-contact-geom1>`,
:ref:`geom2<sensor-contact-geom2>`, :ref:`body1<sensor-contact-body1>`, :ref:`body2<sensor-contact-body2>`,
:ref:`subtree1<sensor-contact-subtree1>`, :ref:`subtree2<sensor-contact-subtree2>` and
:ref:`site<sensor-contact-site>`. Matching applies an intersection of criteria, for example setting
:ref:`body1<sensor-contact-body1>` and :ref:`body2<sensor-contact-body2>` will match contacts that involve both
bodies, while setting only :ref:`geom1<sensor-contact-geom1>` will match any contacts involving that geom. Setting
:ref:`site<sensor-contact-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<sensor-contact-subtree1>` and
:ref:`subtree2<sensor-contact-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<sensor-contact-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<sensor-contact-reduce>` below.
Extraction
Copies the set of fields specified by the user into each slot, see :ref:`data<sensor-contact-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>`.
.. _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>`.
.. _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 <sensor-contact>`. 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>`.
.. _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<sensor-contact-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<sensor-contact-num>` and :ref:`data<sensor-contact-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 <sensor-contact>`.
- **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: .. _sensor-e_potential:
:el-prefix:`sensor/` |-| **e_potential** (*) :el-prefix:`sensor/` |-| **e_potential** (*)
doc/XMLschema.rst
+13
View File
@@ -1239,6 +1239,19 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: | | |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`contact | \* | :class: mjcf-attributes |
| <sensor-contact>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-contact-name>` | :ref:`geom1<sensor-contact-geom1>` | :ref:`geom2<sensor-contact-geom2>` | :ref:`body1<sensor-contact-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-contact-body2>` | :ref:`subtree1<sensor-contact-subtree1>` | :ref:`subtree2<sensor-contact-subtree2>` | :ref:`site<sensor-contact-site>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`num<sensor-contact-num>` | :ref:`data<sensor-contact-data>` | :ref:`reduce<sensor-contact-reduce>` | :ref:`cutoff<sensor-contact-cutoff>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`noise<sensor-contact-noise>` | :ref:`user<sensor-contact-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`e_potential | \* | :class: mjcf-attributes | | :ref:`e_potential | \* | :class: mjcf-attributes |
| <sensor-e_potential>` | | | | <sensor-e_potential>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
doc/changelog.rst
+3
View File
@@ -9,6 +9,9 @@ General
^^^^^^^ ^^^^^^^
- Added the :ref:`insidesite<sensor-insidesite>` sensor, for checking if an object is inside the volume of a site. - Added the :ref:`insidesite<sensor-insidesite>` sensor, for checking if an object is inside the volume of a site.
It is useful for triggering events in surrounding environment logic. It is useful for triggering events in surrounding environment logic.
- Added the :ref:`contact<sensor-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 <https://github.com/UPC-ViRVIG/SdfLib>`__. SDFs are now - Removed the SdfLib plugin and the dependency on `SdfLib <https://github.com/UPC-ViRVIG/SdfLib>`__. SDFs are now
supported natively in mjModel. supported natively in mjModel.
doc/includes/references.h
+14
View File
@@ -731,6 +731,9 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMNORMAL, // normal direction between two geoms
mjSENS_GEOMFROMTO, // segment 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 // global sensors
mjSENS_E_POTENTIAL, // potential energy mjSENS_E_POTENTIAL, // potential energy
mjSENS_E_KINETIC, // kinetic energy mjSENS_E_KINETIC, // kinetic energy
@@ -754,6 +757,17 @@ typedef enum mjtDataType_ { // data type for sensors
mjDATATYPE_AXIS, // 3D unit vector mjDATATYPE_AXIS, // 3D unit vector
mjDATATYPE_QUATERNION // unit quaternion mjDATATYPE_QUATERNION // unit quaternion
} mjtDataType; } 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 typedef enum mjtSameFrame_ { // frame alignment of bodies with their children
mjSAMEFRAME_NONE = 0, // no alignment mjSAMEFRAME_NONE = 0, // no alignment
mjSAMEFRAME_BODY, // frame is same as body frame mjSAMEFRAME_BODY, // frame is same as body frame
include/mujoco/mjmodel.h
+16
View File
@@ -369,6 +369,9 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMNORMAL, // normal direction between two geoms
mjSENS_GEOMFROMTO, // segment 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 // global sensors
mjSENS_E_POTENTIAL, // potential energy mjSENS_E_POTENTIAL, // potential energy
mjSENS_E_KINETIC, // kinetic energy mjSENS_E_KINETIC, // kinetic energy
@@ -398,6 +401,19 @@ typedef enum mjtDataType_ { // data type for sensors
} mjtDataType; } 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 typedef enum mjtSameFrame_ { // frame alignment of bodies with their children
mjSAMEFRAME_NONE = 0, // no alignment mjSAMEFRAME_NONE = 0, // no alignment
mjSAMEFRAME_BODY, // frame is same as body frame mjSAMEFRAME_BODY, // frame is same as body frame
python/mujoco/introspect/enums.py
+21 -5
View File
@@ -386,11 +386,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSENS_GEOMDIST', 39), ('mjSENS_GEOMDIST', 39),
('mjSENS_GEOMNORMAL', 40), ('mjSENS_GEOMNORMAL', 40),
('mjSENS_GEOMFROMTO', 41), ('mjSENS_GEOMFROMTO', 41),
('mjSENS_E_POTENTIAL', 42), ('mjSENS_CONTACT', 42),
('mjSENS_E_KINETIC', 43), ('mjSENS_E_POTENTIAL', 43),
('mjSENS_CLOCK', 44), ('mjSENS_E_KINETIC', 44),
('mjSENS_PLUGIN', 45), ('mjSENS_CLOCK', 45),
('mjSENS_USER', 46), ('mjSENS_PLUGIN', 46),
('mjSENS_USER', 47),
]), ]),
)), )),
('mjtStage', ('mjtStage',
@@ -415,6 +416,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDATATYPE_QUATERNION', 3), ('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', ('mjtSameFrame',
EnumDecl( EnumDecl(
name='mjtSameFrame', name='mjtSameFrame',
src/engine/engine_io.c
+1
View File
@@ -2159,6 +2159,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_FRAMEQUAT: case mjSENS_FRAMEQUAT:
return 4; return 4;
case mjSENS_CONTACT:
case mjSENS_USER: case mjSENS_USER:
return sensor_dim; return sensor_dim;
src/engine/engine_sensor.c
+211 -1
View File
@@ -26,6 +26,7 @@
#include "engine/engine_io.h" #include "engine/engine_io.h"
#include "engine/engine_plugin.h" #include "engine/engine_plugin.h"
#include "engine/engine_ray.h" #include "engine/engine_ray.h"
#include "engine/engine_sort.h"
#include "engine/engine_support.h" #include "engine/engine_support.h"
#include "engine/engine_util_blas.h" #include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
@@ -36,6 +37,26 @@
//-------------------------------- utility --------------------------------------------------------- //-------------------------------- 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 // apply cutoff after each stage
static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) { static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process sensors matching stage and having positive cutoff // 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 // 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, static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, int sensor_id,
mjtNum *quat) { 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], static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
const int cam_res[2], mjtNum cam_fovy, 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 ---------------------------------------------------------- //-------------------------------- sensor ----------------------------------------------------------
// position-dependent sensors // position-dependent sensors
@@ -709,7 +844,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
int rootid, bodyid, objtype, objid, adr, nusersensor = 0; int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu; int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
mjtNum tmp[6], conforce[6], conray[3], frc; mjtNum tmp[6], conforce[6], conray[3], frc;
mjContact* con; const mjContact* con;
// disabled sensors: return // disabled sensors: return
if (mjDISABLED(mjDSBL_SENSOR)) { if (mjDISABLED(mjDSBL_SENSOR)) {
@@ -792,6 +927,81 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
} }
break; 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 case mjSENS_ACCELEROMETER: // accelerometer
// tmp = site acceleration, in site frame // tmp = site acceleration, in site frame
mj_objectAcceleration(m, d, mjOBJ_SITE, objid, tmp, 1); mj_objectAcceleration(m, d, mjOBJ_SITE, objid, tmp, 1);
src/engine/engine_support.c
+24
View File
@@ -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 --------------------------------------------------------- //-------------------------- get/set state ---------------------------------------------------------
@@ -1576,3 +1587,16 @@ const char* mj_versionString(void) {
static const char versionstring[] = mjVERSIONSTRING; static const char versionstring[] = mjVERSIONSTRING;
return versionstring; 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;
}
src/engine/engine_support.h
+7
View File
@@ -29,6 +29,9 @@ MJAPI extern const char* mjDISABLESTRING[mjNDISABLE];
MJAPI extern const char* mjENABLESTRING[mjNENABLE]; MJAPI extern const char* mjENABLESTRING[mjNENABLE];
MJAPI extern const char* mjTIMERSTRING[mjNTIMER]; MJAPI extern const char* mjTIMERSTRING[mjNTIMER];
// arrays
MJAPI extern const int mjCONDATA_SIZE[mjNCONDATA]; // TODO(tassa): expose in public header?
//-------------------------- get/set state --------------------------------------------------------- //-------------------------- get/set state ---------------------------------------------------------
@@ -199,6 +202,10 @@ MJAPI int mj_version(void);
// current version of MuJoCo as a null-terminated string // current version of MuJoCo as a null-terminated string
MJAPI const char* mj_versionString(void); 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 #ifdef __cplusplus
} }
#endif #endif
src/user/user_objects.cc
+58 -3
View File
@@ -37,6 +37,7 @@
#include "lodepng.h" #include "lodepng.h"
#include "cc/array_safety.h" #include "cc/array_safety.h"
#include "engine/engine_passive.h" #include "engine/engine_passive.h"
#include "engine/engine_support.h"
#include <mujoco/mjspec.h> #include <mujoco/mjspec.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "user/user_api.h" #include "user/user_api.h"
@@ -6686,6 +6687,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
type != mjSENS_E_KINETIC && type != mjSENS_E_KINETIC &&
type != mjSENS_CLOCK && type != mjSENS_CLOCK &&
type != mjSENS_PLUGIN && type != mjSENS_PLUGIN &&
type != mjSENS_CONTACT &&
type != mjSENS_USER) { type != mjSENS_USER) {
throw mjCError(this, "invalid type in sensor"); throw mjCError(this, "invalid type in sensor");
} }
@@ -7043,6 +7045,60 @@ void mjCSensor::Compile(void) {
} }
break; 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<mjCBody*>(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<mjCBody*>(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_POTENTIAL:
case mjSENS_E_KINETIC: case mjSENS_E_KINETIC:
case mjSENS_CLOCK: case mjSENS_CLOCK:
@@ -7054,13 +7110,12 @@ void mjCSensor::Compile(void) {
case mjSENS_USER: case mjSENS_USER:
// check for negative dim // check for negative dim
if (dim < 0) { 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 // make sure dim is consistent with datatype
if (datatype == mjDATATYPE_AXIS && dim != 3) { if (datatype == mjDATATYPE_AXIS && dim != 3) {
throw mjCError(this, throw mjCError(this, "datatype AXIS requires dim=3 in sensor");
"datatype AXIS requires dim=3 in sensor");
} }
if (datatype == mjDATATYPE_QUATERNION && dim != 4) { if (datatype == mjDATATYPE_QUATERNION && dim != 4) {
throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor"); throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor");
src/xml/xml_base.h
+3
View File
@@ -43,6 +43,7 @@ extern const int gain_sz;
extern const int bias_sz; extern const int bias_sz;
extern const int stage_sz; extern const int stage_sz;
extern const int datatype_sz; extern const int datatype_sz;
extern const int reduce_sz;
extern const mjMap angle_map[]; extern const mjMap angle_map[];
extern const mjMap enable_map[]; extern const mjMap enable_map[];
extern const mjMap bool_map[]; extern const mjMap bool_map[];
@@ -70,6 +71,8 @@ extern const mjMap gain_map[];
extern const mjMap bias_map[]; extern const mjMap bias_map[];
extern const mjMap stage_map[]; extern const mjMap stage_map[];
extern const mjMap datatype_map[]; extern const mjMap datatype_map[];
extern const mjMap condata_map[];
extern const mjMap reduce_map[];
extern const mjMap meshtype_map[]; extern const mjMap meshtype_map[];
extern const mjMap meshinertia_map[]; extern const mjMap meshinertia_map[];
extern const mjMap flexself_map[]; extern const mjMap flexself_map[];
src/xml/xml_native_reader.cc
+95
View File
@@ -34,6 +34,7 @@
#include <mujoco/mjtnum.h> #include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h> #include <mujoco/mjvisualize.h>
#include "engine/engine_plugin.h" #include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
#include <mujoco/mjspec.h> #include <mujoco/mjspec.h>
@@ -481,6 +482,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"normal", "*", "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"}, {"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_potential", "*", "4", "name", "cutoff", "noise", "user"},
{"e_kinetic", "*", "4", "name", "cutoff", "noise", "user"}, {"e_kinetic", "*", "4", "name", "cutoff", "noise", "user"},
{"clock", "*", "4", "name", "cutoff", "noise", "user"}, {"clock", "*", "4", "name", "cutoff", "noise", "user"},
@@ -606,6 +609,7 @@ const mjMap texrole_map[texrole_sz] = {
{"orm", mjTEXROLE_ORM}, {"orm", mjTEXROLE_ORM},
}; };
// integrator type // integrator type
const int integrator_sz = 4; const int integrator_sz = 4;
const mjMap integrator_map[integrator_sz] = { const mjMap integrator_map[integrator_sz] = {
@@ -615,6 +619,7 @@ const mjMap integrator_map[integrator_sz] = {
{"implicitfast", mjINT_IMPLICITFAST} {"implicitfast", mjINT_IMPLICITFAST}
}; };
// cone type // cone type
const int cone_sz = 2; const int cone_sz = 2;
const mjMap cone_map[cone_sz] = { 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 // LR mode
const int lrmode_sz = 4; const int lrmode_sz = 4;
const mjMap lrmode_map[lrmode_sz] = { 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<int> 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 // global sensors
else if (type == "e_potential") { else if (type == "e_potential") {
sensor->type = mjSENS_E_POTENTIAL; sensor->type = mjSENS_E_POTENTIAL;
src/xml/xml_native_reader.h
+1 -1
View File
@@ -101,7 +101,7 @@ class mjXReader : public mjXBase {
}; };
// MJCF schema // MJCF schema
#define nMJCF 239 #define nMJCF 240
extern const char* MJCF[nMJCF][mjXATTRNUM]; extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+34 -2
View File
@@ -28,6 +28,7 @@
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "engine/engine_io.h" #include "engine/engine_io.h"
#include "engine/engine_plugin.h" #include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
#include "user/user_model.h" #include "user/user_model.h"
@@ -76,7 +77,7 @@ static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) {
// top level sections // top level sections
std::array<string, 17> sections = { std::array<string, 17> sections = {
"<actuator", "<asset", "<compiler", "<contact", "<custom", "<actuator", "<asset", "<compiler", "<contact>", "<custom",
"<default>", "<deformable", "<equality", "<extension", "<keyframe", "<default>", "<deformable", "<equality", "<extension", "<keyframe",
"<option", "<sensor", "<size", "<statistic", "<tendon", "<option", "<sensor", "<size", "<statistic", "<tendon",
"<visual", "<worldbody"}; "<visual", "<worldbody"};
@@ -2205,7 +2206,38 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname());
WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname()); WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname());
break; 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 // global sensors
case mjSENS_E_POTENTIAL: case mjSENS_E_POTENTIAL:
elem = InsertEnd(section, "potential"); elem = InsertEnd(section, "potential");
src/xml/xml_util.cc
+54 -1
View File
@@ -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, bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required) { const mjMap* map, int mapSz, bool required) {
// get attribute text // 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<std::string> 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 ----------------------------------------------- //---------------------------------- write functions -----------------------------------------------
// check if double is int // check if double is int
@@ -970,3 +1006,20 @@ void mjXUtil::WriteAttrKey(XMLElement* elem, std::string name,
WriteAttrTxt(elem, name, FindValue(map, mapsz, data)); 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);
}
src/xml/xml_util.h
+8
View File
@@ -183,6 +183,10 @@ class mjXUtil {
static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data, static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required = false); 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 // write attribute- any type
template<typename T> template<typename T>
static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data, 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, static void WriteAttrKey(tinyxml2::XMLElement* elem, std::string name,
const mjMap* map, int mapsz, int data, int def = -12345); 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: private:
template<typename T> template<typename T>
static bool ReadAttrValues(tinyxml2::XMLElement* elem, const char* attr, static bool ReadAttrValues(tinyxml2::XMLElement* elem, const char* attr,
test/engine/engine_sensor_test.cc
+242 -26
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_sensor.c. // Tests for engine/engine_sensor.c.
#include <cstddef>
#include <string>
#include <vector> #include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
@@ -28,21 +30,39 @@
namespace mujoco { namespace mujoco {
namespace { 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 const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers
// returns as a vector the measured values from sensor with index `id` // returns as a vector the measured values from sensor with index `id`
static std::vector<mjtNum> GetSensor(const mjModel* model, static vector<mjtNum> GetSensor(const mjModel* model,
const mjData* data, const mjData* data, int id) {
int id) { return vector<mjtNum>(
return std::vector<mjtNum>(
data->sensordata + model->sensor_adr[id], data->sensordata + model->sensor_adr[id],
data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
} }
using ::testing::Pointwise; // returns as a vector the measured values from sensor with name `name
using ::testing::DoubleNear; static vector<mjtNum> GetSensor(const mjModel* model,
using ::testing::NotNull; const mjData* data, const char* name) {
using ::testing::StrEq; int id = mj_name2id(model, mjOBJ_SENSOR, name);
return vector<mjtNum>(
data->sensordata + model->sensor_adr[id],
data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
}
using SensorTest = MujocoTest; using SensorTest = MujocoTest;
@@ -112,13 +132,13 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMat) {
mj_forward(model, data); mj_forward(model, data);
// compare actual and expected values // 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})); 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})); 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})); EXPECT_THAT(yaxis, Pointwise(DoubleNear(tol), {1, 0, 0}));
mj_deleteData(data); mj_deleteData(data);
@@ -155,7 +175,7 @@ TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) {
mju_mat2Quat(converted_quat, mat); mju_mat2Quat(converted_quat, mat);
// compare quaternion sensor and quat derived from orientation matrix // 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)); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), converted_quat));
mj_deleteData(data); mj_deleteData(data);
@@ -199,7 +219,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
// call mj_forward, save global sensors (colocated with reference frame) // call mj_forward, save global sensors (colocated with reference frame)
mj_forward(model, data); 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 // set qpos to arbitrary values, call mj_forward
for (int i=0; i < 7; i++) { for (int i=0; i < 7; i++) {
@@ -208,7 +228,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
mj_forward(model, data); mj_forward(model, data);
// get values from relative sensors after moving the object // 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); data->sensordata+nsensordata);
// object and reference have moved together, we expect values to not change // object and reference have moved together, we expect values to not change
@@ -245,7 +265,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) {
mj_forward(model, data); mj_forward(model, data);
// compare to expected values // 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}; const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0};
EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel)); EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel));
@@ -278,7 +298,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
mj_forward(model, data); mj_forward(model, data);
// obj and ref rotate together, relative angular velocities should be zero // 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})); EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0}));
mj_deleteData(data); mj_deleteData(data);
@@ -314,7 +334,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) {
mj_forward(model, data); mj_forward(model, data);
// obj and ref rotate on same axis, we can just difference the velocities // 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}; const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0};
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel)); EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel));
@@ -358,8 +378,8 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) {
mj_forward(model, data); mj_forward(model, data);
// save measured linear and angular velocities as vectors // save measured linear and angular velocities as vectors
std::vector linvel = GetSensor(model, data, 2); vector linvel = GetSensor(model, data, 2);
std::vector angvel = GetSensor(model, data, 3); vector angvel = GetSensor(model, data, 3);
// save current position, quaternion as arrays // save current position, quaternion as arrays
mjtNum pos0[3], quat0[4]; mjtNum pos0[3], quat0[4];
@@ -615,20 +635,20 @@ TEST_F(SensorTest, CollisionSequential) {
mjtNum eps = 1e-14; mjtNum eps = 1e-14;
EXPECT_THAT(GetSensor(model, data, 3), EXPECT_THAT(GetSensor(model, data, 3),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, 1})); Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 4), EXPECT_THAT(GetSensor(model, data, 4),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, -1})); Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, -1}));
EXPECT_THAT(GetSensor(model, data, 5), EXPECT_THAT(GetSensor(model, data, 5),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{1, 0, 0})); Pointwise(DoubleNear(eps), vector<mjtNum>{1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 6), EXPECT_THAT(GetSensor(model, data, 6),
Pointwise(DoubleNear(eps), Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, 0, 0, 0, .8})); vector<mjtNum>{0, 0, 0, 0, 0, .8}));
EXPECT_THAT(GetSensor(model, data, 7), EXPECT_THAT(GetSensor(model, data, 7),
Pointwise(DoubleNear(eps), Pointwise(DoubleNear(eps),
std::vector<mjtNum>{1, 0, .7, 1, 0, 0})); vector<mjtNum>{1, 0, .7, 1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 8), EXPECT_THAT(GetSensor(model, data, 8),
Pointwise(DoubleNear(eps), Pointwise(DoubleNear(eps),
std::vector<mjtNum>{.2, 0, 1, .7, 0, 1})); vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 9), EXPECT_THAT(GetSensor(model, data, 9),
Pointwise(DoubleNear(eps), GetSensor(model, data, 0))); Pointwise(DoubleNear(eps), GetSensor(model, data, 0)));
@@ -647,6 +667,202 @@ TEST_F(SensorTest, CollisionSequential) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(SensorTest, BadContact) {
string xml_template = R"(
<mujoco>
<worldbody>
<geom name="sphere1" pos="0 0 1" size="0.2"/>
<body name="body">
<freejoint/>
<geom name="sphere2" pos="1 0 1" size="0.3"/>
<site name="site" pos="1 0 1" size="0.3"/>
<body name="non_root">
<geom name="sphere3" pos="1 0 1" size="0.3"/>
</body>
</body>
</worldbody>
<sensor>
<contact BAD_ATTR/>
</sensor>
</mujoco>
)";
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<mjtNum>{4});
vector world = GetSensor(model, data, "world");
EXPECT_EQ(world, vector<mjtNum>{3});
vector b1 = GetSensor(model, data, "b1");
EXPECT_EQ(b1, vector<mjtNum>{3});
vector g1 = GetSensor(model, data, "g1");
EXPECT_EQ(g1, vector<mjtNum>{3});
vector b1g2 = GetSensor(model, data, "b1:g2");
EXPECT_EQ(b1g2, vector<mjtNum>{1});
vector b1world = GetSensor(model, data, "b1:world");
EXPECT_EQ(b1world, vector<mjtNum>{2});
vector site = GetSensor(model, data, "site");
EXPECT_EQ(site, vector<mjtNum>{2});
vector sitewall = GetSensor(model, data, "site:wall");
EXPECT_EQ(sitewall, vector<mjtNum>{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<mjtNum> 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) { TEST_F(SensorTest, CameraProjection) {
constexpr char xml[] = R"( constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -725,7 +941,7 @@ TEST_F(SensorTest, InsideSite) {
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
data->qpos[0] = hpos[i]; data->qpos[0] = hpos[i];
mj_forward(model, data); mj_forward(model, data);
std::vector<mjtNum> expected(5, 0.0); vector<mjtNum> expected(5, 0.0);
expected[i] = 1.0; expected[i] = 1.0;
EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected);
} }
test/engine/engine_support_test.cc
+10
View File
@@ -947,5 +947,15 @@ TEST_F(SupportTest, SetKeyframe) {
mj_deleteModel(model); 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
} // namespace mujoco } // namespace mujoco
test/engine/testdata/sensor/contact.xml
Vendored
+35
View File
@@ -0,0 +1,35 @@
<mujoco model="contact">
<option gravity="4 0 -3"/>
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size=".5 1 .01"/>
<geom name="wall" type="plane" size=".25 1 .05" zaxis="-1 0 0" pos=".5 0 .25"/>
<site name="site" type="box" size=".15 .15 .15" pos=".5 0 0" rgba=".5 .7 .5 .3"/>
<body name="b1" pos="0 0 .2">
<freejoint/>
<geom name="g1" size=".1" rgba=".8 .2 .1 1" mass="1"/>
</body>
<body name="b2" pos="-.3 0 .2">
<freejoint/>
<geom name="g2" size=".1" rgba=".8 .2 .1 1" mass="1"/>
</body>
</worldbody>
<sensor>
<contact name="all"/>
<contact name="world" body1="world"/>
<contact name="b1" body1="b1"/>
<contact name="g1" geom1="g1"/>
<contact name="b1:g2" body1="b1" geom2="g2"/>
<contact name="b1:world" body1="b1" body2="world"/>
<contact name="site" site="site"/>
<contact name="site:wall" site="site" geom2="wall"/>
<contact name="wall" geom1="wall" num="2" data="found force normal"/>
<contact name="b2" body1="b2" data="force normal" num="2"/>
<contact name="b2_flipped" body2="b2" data="force normal" num="2"/>
<contact name="b2_reduced" body2="b2" data="force normal" reduce="maxforce"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/contact_sorted.xml
Vendored
+22
View File
@@ -0,0 +1,22 @@
<mujoco model="contact sorted">
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size=".5 1 .01"/>
<body name="spheres">
<joint axis="0 -1 0"/>
<geom size=".1" pos="-.1 0 0" rgba=".8 .2 .1 1"/>
<geom size=".1" pos=".1 0 0" rgba=".8 .2 .1 1"/>
<geom size=".1" pos="-.2 0 0" rgba=".8 .2 .1 1"/>
<geom size=".1" pos=".3 0 0" rgba=".8 .2 .1 1"/>
</body>
</worldbody>
<sensor>
<contact name="unsorted" body1="spheres" num="4" data="dist"/>
<contact name="sorted dist" body1="spheres" num="4" data="dist" reduce="mindist"/>
<contact name="sorted force" body1="spheres" num="4" data="force" reduce="maxforce"/>
<contact name="smallest dist" body1="spheres" num="1" data="dist" reduce="mindist"/>
<contact name="largest force" body1="spheres" num="1" data="force" reduce="maxforce"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/contact_subtree.xml
Vendored
+30
View File
@@ -0,0 +1,30 @@
<mujoco model="contact subtree">
<asset>
<model file="humanoid.xml"/>
</asset>
<worldbody>
<light pos="0 0 5"/>
<geom name="floor" type="plane" size="2 2 .01"/>
<!-- two horizontal humanoids, in the air, one above the other -->
<frame pos="-.5 0 1.2" euler="0 90 0">
<attach model="Humanoid" body="torso" prefix="1"/>
</frame>
<frame pos="-.5 0 0.5" euler="0 90 -90">
<attach model="Humanoid" body="torso" prefix="2"/>
</frame>
</worldbody>
<sensor>
<contact name="all"/>
<contact name="w_t1" body1="world" subtree2="1torso"/>
<contact name="w_t2" body1="world" subtree2="2torso"/>
<contact name="t1" subtree1="1torso"/>
<contact name="t2" subtree1="2torso"/>
<contact name="t1_t1" subtree1="1torso" subtree2="1torso"/>
<contact name="t2_t2" subtree1="2torso" subtree2="2torso"/>
<contact name="t1_t2" subtree1="1torso" subtree2="2torso"/>
<contact name="t2_t1" subtree1="2torso" subtree2="1torso"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/humanoid.xml
Vendored
+267
View File
@@ -0,0 +1,267 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Humanoid">
<option timestep="0.005"/>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" group="1"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="torso" pos="0 0 1.282" childclass="body">
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
<body name="pelvis" pos="0 0 -.165">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
<joint name="hip_y_right" class="hip_y"/>
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<joint name="knee_right" class="knee"/>
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<joint name="ankle_y_right" class="ankle_y"/>
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
<joint name="hip_y_left" class="hip_y"/>
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<joint name="knee_left" class="knee"/>
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<joint name="ankle_y_left" class="ankle_y"/>
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
<body name="lower_arm_right" pos=".18 -.18 -.18">
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
<body name="hand_right" pos=".18 .18 .18">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
<body name="lower_arm_left" pos=".18 .18 -.18">
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
<body name="hand_left" pos=".18 -.18 .18">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1="waist_lower" body2="thigh_right"/>
<exclude body1="waist_lower" body2="thigh_left"/>
</contact>
<tendon>
<fixed name="hamstring_right" limited="true" range="-0.3 2">
<joint joint="hip_y_right" coef=".5"/>
<joint joint="knee_right" coef="-.5"/>
</fixed>
<fixed name="hamstring_left" limited="true" range="-0.3 2">
<joint joint="hip_y_left" coef=".5"/>
<joint joint="knee_left" coef="-.5"/>
</fixed>
</tendon>
<actuator>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_right"/>
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_left"/>
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_right"/>
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
<motor name="elbow_left" gear="40" joint="elbow_left"/>
</actuator>
<keyframe>
<!--
The values below are split into rows for readibility:
torso position
torso orientation
spinal
right leg
left leg
arms
-->
<key name="squat"
qpos="0 0 0.596
0.988015 0 0.154359 0
0 0.4 0
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
0 0 0 0 0 0"/>
<key name="stand_on_left_leg"
qpos="0 0 1.21948
0.971588 -0.179973 0.135318 -0.0729076
-0.0516 -0.202 0.23
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
<key name="prone"
qpos="0.4 0 0.0757706
0.7325 0 0.680767 0
0 0.0729 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.0077 0.0019 -0.026 -0.351 -0.27 0
0.56 -0.62 -1.752
0.56 -0.62 -1.752"/>
<key name="supine"
qpos="-0.4 0 0.08122
0.722788 0 -0.69107 0
0 -0.25 0
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.0182 0.0142 0.3 0.042 -0.44 -0.02
0.186 -0.73 -1.73
0.186 -0.73 -1.73"/>
</keyframe>
</mujoco>
unity/Runtime/Bindings/MjBindings.cs
+16 -5
View File
@@ -394,11 +394,12 @@ public enum mjtSensor : int{
mjSENS_GEOMDIST = 39, mjSENS_GEOMDIST = 39,
mjSENS_GEOMNORMAL = 40, mjSENS_GEOMNORMAL = 40,
mjSENS_GEOMFROMTO = 41, mjSENS_GEOMFROMTO = 41,
mjSENS_E_POTENTIAL = 42, mjSENS_CONTACT = 42,
mjSENS_E_KINETIC = 43, mjSENS_E_POTENTIAL = 43,
mjSENS_CLOCK = 44, mjSENS_E_KINETIC = 44,
mjSENS_PLUGIN = 45, mjSENS_CLOCK = 45,
mjSENS_USER = 46, mjSENS_PLUGIN = 46,
mjSENS_USER = 47,
} }
public enum mjtStage : int{ public enum mjtStage : int{
mjSTAGE_NONE = 0, mjSTAGE_NONE = 0,
@@ -412,6 +413,16 @@ public enum mjtDataType : int{
mjDATATYPE_AXIS = 2, mjDATATYPE_AXIS = 2,
mjDATATYPE_QUATERNION = 3, 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{ public enum mjtSameFrame : int{
mjSAMEFRAME_NONE = 0, mjSAMEFRAME_NONE = 0,
mjSAMEFRAME_BODY = 1, mjSAMEFRAME_BODY = 1,