From 02d015458c33117c14b3923730293c0207e9aea4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 May 2024 04:51:46 -0700 Subject: [PATCH] Add `mj_geomDistance` API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51 PiperOrigin-RevId: 632458301 Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3 --- doc/APIreference/functions.rst | 30 ++- doc/APIreference/functions_override.rst | 25 ++- doc/XMLreference.rst | 180 ++++++++++++++++++ doc/XMLschema.rst | 27 +++ doc/changelog.rst | 11 +- doc/includes/references.h | 7 + include/mujoco/mjmodel.h | 5 + include/mujoco/mujoco.h | 4 + introspect/enums.py | 9 +- introspect/functions.py | 39 ++++ python/mujoco/bindings_test.py | 8 + python/mujoco/functions.cc | 12 ++ src/engine/engine_io.c | 5 + src/engine/engine_ray.c | 9 +- src/engine/engine_sensor.c | 134 +++++++++++-- src/engine/engine_support.c | 43 +++++ src/engine/engine_support.h | 4 + src/engine/engine_vis_visualize.c | 14 ++ src/user/user_objects.cc | 52 +++-- src/xml/xml_native_reader.cc | 26 +++ src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 57 +++++- test/engine/engine_sensor_test.cc | 89 ++++++++- test/engine/engine_support_test.cc | 77 +++++++- .../testdata/sensor/fromto_body_body.xml | 29 +++ test/engine/testdata/sensor/fromto_convex.xml | 83 ++++++++ .../testdata/sensor/fromto_primitive.xml | 71 +++++++ test/user/user_objects_test.cc | 4 +- unity/Runtime/Bindings/MjBindings.cs | 12 +- 29 files changed, 1002 insertions(+), 66 deletions(-) create mode 100644 test/engine/testdata/sensor/fromto_body_body.xml create mode 100644 test/engine/testdata/sensor/fromto_convex.xml create mode 100644 test/engine/testdata/sensor/fromto_primitive.xml diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 182f708e..f171b635 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -57,8 +57,8 @@ Main simulation These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance -(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be -called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and +(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will +be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and :ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind though that the RK4 solver does not work with mj_step1/2. @@ -72,8 +72,8 @@ be set before calling this function. Given the state (qpos, qvel, act), mj_forwa while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed -with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general -sanity check). +with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a +general sanity check). The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was changed (usually in the context of finite differencing). Then there is no point repeating the computations that only @@ -408,6 +408,28 @@ Compute object 6D acceleration (rot:lin) in object-centered frame, world/local o sensors are not present in the model, :ref:`mj_rnePostConstraint` must be manually called in order to calculate mjData.cacc -- the total body acceleration, including contributions from the constraint solver. +.. _mj_geomDistance: + +mj_geomDistance +~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_geomDistance + +Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``. +Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is +found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0). + +.. admonition:: Positive ``distmax`` values + :class: note + + .. TODO: b/339596989 - Improve mjc_Convex. + + For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + This is considered a bug to be fixed in a future release. + In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of + `engine_collision_driver.c `__. + .. _mj_contactForce: mj_contactForce diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index ea7ec0cc..0724fd4f 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -33,8 +33,8 @@ The model and all files referenced in it can be loaded from disk or from a VFS w These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance -(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be -called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and +(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will +be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and :ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind though that the RK4 solver does not work with mj_step1/2. @@ -48,8 +48,8 @@ be set before calling this function. Given the state (qpos, qvel, act), mj_forwa while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed -with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general -sanity check). +with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a +general sanity check). The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was changed (usually in the context of finite differencing). Then there is no point repeating the computations that only @@ -180,6 +180,23 @@ generalized velocities to subtree angular momentum. More precisely if :math:`h` body index ``body`` in ``mjData.subtree_angmom`` (reported by the :ref:`subtreeangmom` sensor) and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = H \dot q`. +.. _mj_geomDistance: + +Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``. +Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is +found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0). + +.. admonition:: Positive ``distmax`` values + :class: note + + .. TODO: b/339596989 - Improve mjc_Convex. + + For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + This is considered a bug to be fixed in a future release. + In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of + `engine_collision_driver.c `__. + .. _mj_mulM: This function multiplies the joint-space inertia matrix stored in mjData.qM by a vector. qM has a custom sparse format diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 27a911b8..1441d293 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6723,6 +6723,186 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d :at:`body`: :at-val:`string, required` Name of the body where the kinematic subtree is rooted. +.. _collision-sensors: + +collision sensors +^^^^^^^^^^^^^^^^^ + +The following 3 sensor types, :ref:`sensor/distance`, :ref:`sensor/normal` and +:ref:`sensor/fromto`, respectively measure the distance, normal direction and line segment of the +smallest signed distance between the surfaces of two geoms using the narrow-phase geom-geom colliders. The collision +computation is always performed, independently of the standard collision :ref:`selection and filtering` +pipeline. These 3 sensors share some common properties: + +.. _collision-sensors-cutoff: + +:at:`cutoff` + For most sensors, the :at:`cutoff` attribute simply defines a clipping operation on sensor values. For collision + sensors, it defines the maximum distance at which collisions will be detected, corresponding to the ``dismax`` + argument of :ref:`mj_geomDistance`. For example, at the default value of 0, only negative distances (corresponding + to geom-geom penetration) will be reported by :ref:`sensor/distance`. + In order to determine collision properties of non-penetrating geom pairs, a positive :at:`cutoff` is required. + + .. admonition:: Positive cutoff values + :class: note + + .. TODO: b/339596989 - Improve mjc_Convex. + + For some colliders, a positive :at:`cutoff` will result in an accurate measurement. However, for collision + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + This is considered a bug to be fixed in a future release. + In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of + `engine_collision_driver.c `__. + +:at:`geom1`, :at:`geom2`, :at:`body1`, :at:`body2` + For all 3 collision sensor types, the two colliding geoms can be specified explicitly using the :at:`geom1` and + :at:`geom2` attributes or implicitly, using :at:`body1`, :at:`body2`. In the latter case the sensor will iterate over + all geoms of the specified body or bodies (mixed specification like :at:`geom1`, :at:`body2` are allowed), and + select the collision with the smallest signed distance. + +sequential sensors + When multiple collision sensors are defined sequentially and have identical attributes (:at:`geom1`, :at:`body1`, + :at:`geom2`, :at:`body2`, :at:`cutoff`), for example when both distance and normal are queried for the same geom + pair, the collision functions will be called once for the whole sensor block, avoiding repeated computation. + +.. _sensor-distance: + +:el-prefix:`sensor/` |-| **distance** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates a sensor that returns the smallest signed distance between the surfaces of two geoms. +See :ref:`collision-sensors` for more details about sensors of this type. + +.. _sensor-distance-cutoff: + +:at:`cutoff` + See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor catagories. + If no collision is detected, the distance sensor returns the :at:`cutoff` value, so in this case + :at:`cutoff` acts as a maximum clipping value, in addition to the special semantics. + +.. _sensor-distance-geom1: + +:at:`geom1`: :at-val:`string, optional` + Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-distance-geom2: + +:at:`geom2`: :at-val:`string, optional` + Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-distance-body1: + +:at:`body1`: :at-val:`string, optional` + Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-distance-body2: + +:at:`body2`: :at-val:`string, optional` + Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-distance-name: + +.. _sensor-distance-noise: + +.. _sensor-distance-user: + +:at:`name`, :at:`noise`, :at:`user` + See :ref:`CSensor`. + + +.. _sensor-normal: + +:el-prefix:`sensor/` |-| **normal** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates a sensor that returns the normal direction of the smallest signed distance between the surfaces of +two geoms. It is guaranteed to point from the surface of geom1 to the surface of geom2, though note that in the case of +penetration, this direction is generally in the opposite direction to that of the centroids. +See :ref:`collision-sensors` for more details about sensors of this type. + +.. _sensor-normal-cutoff: + +:at:`cutoff` + See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor catagories. + If no collision is detected, the :ref:`normal` sensor returns (0, 0, 0), otherwise it returns a + normalized direction vector. For this sensor, :at:`cutoff` does not lead to any clamping. + +.. _sensor-normal-geom1: + +:at:`geom1`: :at-val:`string, optional` + Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-normal-geom2: + +:at:`geom2`: :at-val:`string, optional` + Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-normal-body1: + +:at:`body1`: :at-val:`string, optional` + Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-normal-body2: + +:at:`body2`: :at-val:`string, optional` + Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-normal-name: + +.. _sensor-normal-noise: + +.. _sensor-normal-user: + +:at:`name`, :at:`noise`, :at:`user` + See :ref:`CSensor`. + + +.. _sensor-fromto: + +:el-prefix:`sensor/` |-| **fromto** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates a sensor that returns the segment defining the smallest signed distance between the surfaces of two +geoms. The segment is defined by 6 numbers (x1, y1, z1, x2, y2, z2) corresponding to two points in the world frame. +(x1, y1, z1) is on the surface of geom1, (x2, y2, z2) is on the surface of geom2. When this sensor is present and the +:ref:`mjVIS_RANGEFINDER` visualization flag is set, segments will be visualized as rangefinder rays. +See :ref:`collision-sensors` for more details about sensors of this type. + +.. _sensor-fromto-cutoff: + +:at:`cutoff` + See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor catagories. + If no collision is detected, the :ref:`fromto` sensor returns 6 zeros. + For this sensor, :at:`cutoff` does not lead to any clamping. + +.. _sensor-fromto-geom1: + +:at:`geom1`: :at-val:`string, optional` + Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-fromto-geom2: + +:at:`geom2`: :at-val:`string, optional` + Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-fromto-body1: + +:at:`body1`: :at-val:`string, optional` + Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified. + +.. _sensor-fromto-body2: + +:at:`body2`: :at-val:`string, optional` + Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified. + +.. _sensor-fromto-name: + +.. _sensor-fromto-noise: + +.. _sensor-fromto-user: + +:at:`name`, :at:`noise`, :at:`user` + See :ref:`CSensor`. .. _sensor-clock: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index cb96f80c..31278134 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1163,6 +1163,33 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`distance | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`geom1` | :ref:`geom2` | :ref:`body1` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body2` | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`normal | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`geom1` | :ref:`geom2` | :ref:`body1` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body2` | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`fromto | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`geom1` | :ref:`geom2` | :ref:`body1` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body2` | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`clock | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 49e04ebc..ad37c39b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,12 +5,19 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ + +1. Added :ref:`mj_geomDistance` for computing the shortest signed distance between two geoms and optionally a segment + connecting them. Relatedly, added the 3 sensors: :ref:`distance`, :ref:`normal`, + :ref:`fromto`. See the function and sensor documentation for details. Fixes :github:issue:`51`. + Bug fixes ^^^^^^^^^ -1. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic` is enabled, as is +2. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic` is enabled, as is often the case for URDF imports. Fixes :github:issue:`1069`, :github:issue:`1577`. -2. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing +3. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing them. Fixes :github:issue:`1539`. Version 3.1.5 (May 7, 2024) diff --git a/doc/includes/references.h b/doc/includes/references.h index 1be65826..18473228 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -643,6 +643,11 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree + // sensors for geometric distance; attached to geoms or bodies + mjSENS_GEOMDIST, // signed distance between two geoms + mjSENS_GEOMNORMAL, // normal direction between two geoms + mjSENS_GEOMFROMTO, // segment between two geoms + // global sensors mjSENS_CLOCK, // simulation time @@ -2542,6 +2547,8 @@ void mj_objectVelocity(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local); void mj_objectAcceleration(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local); +mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, + mjtNum distmax, mjtNum fromto[6]); void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, const mjtNum* qpos1, const mjtNum* qpos2); diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9bad4ef6..f8384c74 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -327,6 +327,11 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree + // sensors for geometric distance; attached to geoms or bodies + mjSENS_GEOMDIST, // signed distance between two geoms + mjSENS_GEOMNORMAL, // normal direction between two geoms + mjSENS_GEOMFROMTO, // segment between two geoms + // global sensors mjSENS_CLOCK, // simulation time diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index ac5ed0b2..72bfc367 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -464,6 +464,10 @@ MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d, MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local); +// Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2. +MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, + mjtNum distmax, mjtNum fromto[6]); + // Extract 6D force:torque given contact id, in the contact frame. MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/introspect/enums.py b/introspect/enums.py index c2efcc25..36a9e3a3 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -338,9 +338,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSENS_SUBTREECOM', 34), ('mjSENS_SUBTREELINVEL', 35), ('mjSENS_SUBTREEANGMOM', 36), - ('mjSENS_CLOCK', 37), - ('mjSENS_PLUGIN', 38), - ('mjSENS_USER', 39), + ('mjSENS_GEOMDIST', 37), + ('mjSENS_GEOMNORMAL', 38), + ('mjSENS_GEOMFROMTO', 39), + ('mjSENS_CLOCK', 40), + ('mjSENS_PLUGIN', 41), + ('mjSENS_USER', 42), ]), )), ('mjtStage', diff --git a/introspect/functions.py b/introspect/functions.py index 300b6c9c..d643828b 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -2743,6 +2743,45 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.', # pylint: disable=line-too-long )), + ('mj_geomDistance', + FunctionDecl( + name='mj_geomDistance', + return_type=ValueType(name='mjtNum'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData', is_const=True), + ), + ), + FunctionParameterDecl( + name='geom1', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='geom2', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='distmax', + type=ValueType(name='mjtNum'), + ), + FunctionParameterDecl( + name='fromto', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(6,), + ), + ), + ), + doc='Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.', # pylint: disable=line-too-long + )), ('mj_contactForce', FunctionDecl( name='mj_contactForce', diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 42065d35..8faa6db6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1194,6 +1194,14 @@ Euler integrator, semi-implicit in velocity. mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered, None, None, None, None, None, None, None) + def test_geom_distance(self): + mujoco.mj_forward(self.model, self.data) + fromto = np.empty(6, np.float64) + dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto) + self.assertEqual(dist, 41.9) + np.testing.assert_array_equal(fromto, + np.array((42., 0., 0., 42., 0., 41.9))) + def test_inverse_fd(self): eps = 1e-6 flg_centered = 0 diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 8cb6b65c..3c0809cb 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -531,6 +531,18 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def( + pymodule, + [](const raw::MjModel* m, const raw::MjData* d, + int geom1, int geom2, mjtNum distmax, + std::optional> fromto) { + if (fromto.has_value() && fromto->size() != 6) { + throw py::type_error("fromto should be of size 6"); + } + return InterceptMjErrors(::mj_geomDistance)( + m, d, geom1, geom2, distmax, + fromto.has_value() ? fromto->data() : nullptr); + }); Def( pymodule, [](const raw::MjModel* m, Eigen::Ref qvel, diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index c00ceb1f..49862691 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1773,6 +1773,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_TENDONLIMITPOS: case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: + case mjSENS_GEOMDIST: case mjSENS_CLOCK: return 1; @@ -1797,8 +1798,12 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_SUBTREECOM: case mjSENS_SUBTREELINVEL: case mjSENS_SUBTREEANGMOM: + case mjSENS_GEOMNORMAL: return 3; + case mjSENS_GEOMFROMTO: + return 6; + case mjSENS_BALLQUAT: case mjSENS_FRAMEQUAT: return 4; diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index b520a818..f3fb2705 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -1137,14 +1137,13 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3], -//---------------------------- main entry point --------------------------------------------------- +//---------------------------- main entry point ---------------------------------------------------- // intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude // return geomid and distance (x) to nearest surface, or -1 if no intersection // geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1]) { + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) { mjtNum dist, newdist; // check vector length @@ -1154,7 +1153,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum // clear result dist = -1; - *geomid = -1; + if (geomid) *geomid = -1; // loop over geoms not eliminated by mask and bodyexclude for (int i=0; i < m->ngeom; i++) { @@ -1177,7 +1176,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum // update if closer intersection found if (newdist >= 0 && (newdist < dist || dist < 0)) { dist = newdist; - *geomid = i; + if (geomid) *geomid = i; } } } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index e1758260..c7150255 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -40,6 +40,11 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) { // process sensors matching stage and having positive cutoff for (int i=0; i < m->nsensor; i++) { if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) { + // skip fromto sensors + if (m->sensor_type[i] == mjSENS_GEOMFROMTO) { + continue; + } + // get sensor info int adr = m->sensor_adr[i]; int dim = m->sensor_dim[i]; @@ -214,9 +219,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], // position-dependent sensors void mj_sensorPos(const mjModel* m, mjData* d) { - int rgeomid, objtype, objid, reftype, refid, adr, offset, nusersensor = 0; - int ne = d->ne, nf = d->nf, nefc = d->nefc; - mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref; + int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor; + int nusersensor = 0; // disabled sensors: return if (mjDISABLED(mjDSBL_SENSOR)) { @@ -224,22 +228,26 @@ void mj_sensorPos(const mjModel* m, mjData* d) { } // process sensors matching stage - for (int i=0; i < m->nsensor; i++) { + for (int i=0; i < nsensor; i++) { + mjtSensor type = (mjtSensor) m->sensor_type[i]; + // skip sensor plugins -- these are handled after builtin sensor types - if (m->sensor_type[i] == mjSENS_PLUGIN) { + if (type == mjSENS_PLUGIN) { continue; } if (m->sensor_needstage[i] == mjSTAGE_POS) { // get sensor info - objtype = m->sensor_objtype[i]; - objid = m->sensor_objid[i]; - refid = m->sensor_refid[i]; - reftype = m->sensor_reftype[i]; - adr = m->sensor_adr[i]; + int objtype = m->sensor_objtype[i]; + int objid = m->sensor_objid[i]; + int refid = m->sensor_refid[i]; + int reftype = m->sensor_reftype[i]; + int adr = m->sensor_adr[i]; + + mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref; // process according to type - switch ((mjtSensor) m->sensor_type[i]) { + switch (type) { case mjSENS_MAGNETOMETER: // magnetometer mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3); break; @@ -255,7 +263,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { rvec[1] = d->site_xmat[9*objid+5]; rvec[2] = d->site_xmat[9*objid+8]; d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1, - m->site_bodyid[objid], &rgeomid); + m->site_bodyid[objid], NULL); + break; case mjSENS_JOINTPOS: // jointpos @@ -303,11 +312,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) { // reference frame unspecified: global frame if (refid == -1) { - if (m->sensor_type[i] == mjSENS_FRAMEPOS) { + if (type == mjSENS_FRAMEPOS) { mju_copy3(d->sensordata+adr, xpos); } else { // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively - offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS; + int offset = type - mjSENS_FRAMEXAXIS; d->sensordata[adr] = xmat[offset]; d->sensordata[adr+1] = xmat[offset+3]; d->sensordata[adr+2] = xmat[offset+6]; @@ -317,12 +326,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) { // reference frame specified else { get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref); - if (m->sensor_type[i] == mjSENS_FRAMEPOS) { + if (type == mjSENS_FRAMEPOS) { mju_sub3(rvec, xpos, xpos_ref); mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref); } else { // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively - offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS; + int offset = type - mjSENS_FRAMEXAXIS; mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]}; mju_rotVecMatT(d->sensordata+adr, axis, xmat_ref); } @@ -354,6 +363,99 @@ void mj_sensorPos(const mjModel* m, mjData* d) { mju_copy3(d->sensordata+adr, d->subtree_com+3*objid); break; + case mjSENS_GEOMDIST: // signed distance between two geoms + case mjSENS_GEOMNORMAL: // normal direction between two geoms + case mjSENS_GEOMFROMTO: // segment between two geoms + { + // use cutoff for collision margin + mjtNum margin = m->sensor_cutoff[i]; + + // initialize outputs + mjtNum dist = margin; // collision distance + mjtNum fromto[6] = {0}; // segment between geoms + + // get lists of geoms to collide + int n1, id1; + if (objtype == mjOBJ_BODY) { + n1 = m->body_geomnum[objid]; + id1 = m->body_geomadr[objid]; + } else { + n1 = 1; + id1 = objid; + } + int n2, id2; + if (reftype == mjOBJ_BODY) { + n2 = m->body_geomnum[refid]; + id2 = m->body_geomadr[refid]; + } else { + n2 = 1; + id2 = refid; + } + + // collide all pairs + for (int geom1=id1; geom1 < id1+n1; geom1++) { + for (int geom2=id2; geom2 < id2+n2; geom2++) { + mjtNum fromto_new[6] = {0}; + mjtNum dist_new = mj_geomDistance(m, d, geom1, geom2, margin, fromto_new); + if (dist_new < dist) { + dist = dist_new; + mju_copy(fromto, fromto_new, 6); + } + } + } + + // write sensordata for this sensor and all subsequent sensors with identical signature + int write_sensor = 1; + while (write_sensor) { + // write geom distance + if (type == mjSENS_GEOMDIST) { + d->sensordata[adr] = dist; + } + + // write distance normal + else if (type == mjSENS_GEOMNORMAL) { + mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]}; + if (normal[0] || normal[1] || normal[2]) { + mju_normalize3(normal); + } + mju_copy3(d->sensordata + adr, normal); + } + + // write distance fromto + else { + mju_copy(d->sensordata + adr, fromto, 6); + } + + // if this is the last sensor, break + if (i+1 == nsensor) { + break; + } + + // type of the next sensor + mjtSensor type_next = m->sensor_type[i+1]; + + // check if signature of next sensor matches this sensor + write_sensor = (type_next == mjSENS_GEOMDIST || + type_next == mjSENS_GEOMNORMAL || + type_next == mjSENS_GEOMFROMTO) && + m->sensor_objtype[i+1] == objtype && + m->sensor_objid[i+1] == objid && + m->sensor_reftype[i+1] == reftype && + m->sensor_refid[i+1] == refid && + m->sensor_cutoff[i+1] == margin; + + // if signature matches, increment external loop variable i + if (write_sensor) { + i++; + + // update adr and type, everything else is the same + adr = m->sensor_adr[i]; + type = type_next; + } + } + } + break; + case mjSENS_CLOCK: // clock d->sensordata[adr] = d->time; break; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index ba2f6a8a..1b3ee7a3 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -20,6 +20,7 @@ #include #include +#include "engine/engine_collision_driver.h" #include "engine/engine_core_constraint.h" #include "engine/engine_crossplatform.h" #include "engine/engine_io.h" @@ -1703,6 +1704,48 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, //-------------------------- miscellaneous --------------------------------------------------------- +// returns the smallest distance between two geoms +mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, mjtNum distmax, + mjtNum fromto[6]) { + mjContact con[mjMAXCONPAIR]; + mjtNum dist = distmax; + if (fromto) mju_zero(fromto, 6); + + // flip geom order if required + int flip = m->geom_type[geom1] > m->geom_type[geom2]; + int g1 = flip ? geom2 : geom1; + int g2 = flip ? geom1 : geom2; + int type1 = m->geom_type[g1]; + int type2 = m->geom_type[g2]; + + // call collision function if it exists + if (!mjCOLLISIONFUNC[type1][type2]) { + return dist; + } + int num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, distmax); + + // find smallest distance + int smallest = -1; + for (int i=0; i < num; i++) { + mjtNum dist_i = con[i].dist; + if (dist_i < dist) { + dist = dist_i; + smallest = i; + } + } + + // write fromto if given and a collision has been found + if (fromto && smallest >= 0) { + mjtNum sign = flip ? -1 : 1; + mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist); + mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist); + } + + return dist; +} + + + // extract 6D force:torque for one contact, in contact frame void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) { mjContact* con; diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index a5d3c7ae..d4b6fd46 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -189,6 +189,10 @@ MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d, //-------------------------- miscellaneous --------------------------------------------------------- +// returns the smallest distance between two geoms +MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, + mjtNum distmax, mjtNum fromto[6]); + // extract 6D force:torque for one contact, in contact frame MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 5de671f1..9dca56bd 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1974,6 +1974,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to); f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4); FINISH + } else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) { + // sensor data + mjtNum* fromto = d->sensordata + m->sensor_adr[i]; + + // null output: nothing to render + if (mju_isZero(fromto, 6)) { + continue; + } + + // make ray + START + mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3); + f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4); + FINISH } } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 8d88b6e3..8ed146e1 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -5388,7 +5388,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) { ((mjCGeom*)obj)->SetNotVisual(); } - // get sensorized object id } else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) { throw mjCError(this, "invalid type in sensor"); } @@ -5402,7 +5401,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) { // find name if (!ref) { - throw mjCError(this, "unrecognized name '%s' of reference frame object", refname_.c_str()); + throw mjCError(this, "unrecognized name '%s' of object", refname_.c_str()); } // must be attached to object with spatial frame @@ -5460,7 +5459,7 @@ void mjCSensor::Compile(void) { case mjSENS_CAMPROJECTION: // must be attached to site if (objtype!=mjOBJ_SITE) { - throw mjCError(this, "sensor must be attached to site: sensor"); + throw mjCError(this, "sensor must be attached to site"); } // set dim and datatype @@ -5498,7 +5497,7 @@ void mjCSensor::Compile(void) { case mjSENS_JOINTACTFRC: // must be attached to joint if (objtype!=mjOBJ_JOINT) { - throw mjCError(this, "sensor must be attached to joint: sensor"); + throw mjCError(this, "sensor must be attached to joint"); } // make sure joint is slide or hinge @@ -5522,7 +5521,7 @@ void mjCSensor::Compile(void) { case mjSENS_TENDONVEL: // must be attached to tendon if (objtype!=mjOBJ_TENDON) { - throw mjCError(this, "sensor must be attached to tendon: sensor"); + throw mjCError(this, "sensor must be attached to tendon"); } // set @@ -5540,7 +5539,7 @@ void mjCSensor::Compile(void) { case mjSENS_ACTUATORFRC: // must be attached to actuator if (objtype!=mjOBJ_ACTUATOR) { - throw mjCError(this, "sensor must be attached to actuator: sensor"); + throw mjCError(this, "sensor must be attached to actuator"); } // set @@ -5559,7 +5558,7 @@ void mjCSensor::Compile(void) { case mjSENS_BALLANGVEL: // must be attached to joint if (objtype!=mjOBJ_JOINT) { - throw mjCError(this, "sensor must be attached to joint: sensor"); + throw mjCError(this, "sensor must be attached to joint"); } // make sure joint is ball @@ -5584,7 +5583,7 @@ void mjCSensor::Compile(void) { case mjSENS_JOINTLIMITFRC: // must be attached to joint if (objtype!=mjOBJ_JOINT) { - throw mjCError(this, "sensor must be attached to joint: sensor"); + throw mjCError(this, "sensor must be attached to joint"); } // make sure joint has limit @@ -5609,7 +5608,7 @@ void mjCSensor::Compile(void) { case mjSENS_TENDONLIMITFRC: // must be attached to tendon if (objtype!=mjOBJ_TENDON) { - throw mjCError(this, "sensor must be attached to tendon: sensor"); + throw mjCError(this, "sensor must be attached to tendon"); } // make sure tendon has limit @@ -5675,7 +5674,7 @@ void mjCSensor::Compile(void) { case mjSENS_SUBTREEANGMOM: // must be attached to body if (objtype!=mjOBJ_BODY) { - throw mjCError(this, "sensor must be attached to body: sensor"); + throw mjCError(this, "sensor must be attached to body"); } // set @@ -5688,6 +5687,34 @@ void mjCSensor::Compile(void) { } break; + case mjSENS_GEOMDIST: + case mjSENS_GEOMNORMAL: + case mjSENS_GEOMFROMTO: + // must be attached to body or geom + if ((objtype!=mjOBJ_BODY && objtype!=mjOBJ_GEOM) || + (reftype!=mjOBJ_BODY && reftype!=mjOBJ_GEOM)) { + throw mjCError(this, "sensor must be attached to body or geom"); + } + + // objects must be different + if (objtype == reftype && obj == ref) { + throw mjCError(this, "1st body/geom must be different from 2nd body/geom"); + } + + // set + needstage = mjSTAGE_POS; + if (type==mjSENS_GEOMDIST) { + dim = 1; + datatype = mjDATATYPE_POSITIVE; + } else if (type==mjSENS_GEOMNORMAL) { + dim = 3; + datatype = mjDATATYPE_AXIS; + } else { + dim = 6; + datatype = mjDATATYPE_REAL; + } + break; + case mjSENS_CLOCK: dim = 1; needstage = mjSTAGE_POS; @@ -5705,7 +5732,7 @@ void mjCSensor::Compile(void) { throw mjCError(this, "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"); } break; @@ -5737,7 +5764,8 @@ void mjCSensor::Compile(void) { } // check cutoff for incompatible data types - if (cutoff>0 && (datatype==mjDATATYPE_AXIS || datatype==mjDATATYPE_QUATERNION)) { + if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION || + (datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) { throw mjCError(this, "cutoff applied to axis or quaternion datatype in sensor"); } } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index da9f41e5..1a6c47a2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -472,6 +472,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"}, {"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"}, {"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"}, + {"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"}, {"clock", "*", "4", "name", "cutoff", "noise", "user"}, {"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage", "dim", "cutoff", "noise", "user"}, @@ -3949,6 +3952,29 @@ void mjXReader::Sensor(XMLElement* section) { ReadAttrTxt(elem, "body", objname, true); } + // sensors for geometric distance; attached to geoms or bodies + else if (type=="distance" || type=="normal" || type=="fromto") { + bool has_body1 = ReadAttrTxt(elem, "body1", objname); + bool has_geom1 = ReadAttrTxt(elem, "geom1", objname); + if (has_body1 == has_geom1) { + throw mjXError(elem, "exactly one of (geom1, body1) must be specified"); + } + psen->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM; + bool has_body2 = ReadAttrTxt(elem, "body2", refname); + bool has_geom2 = ReadAttrTxt(elem, "geom2", refname); + if (has_body2 == has_geom2) { + throw mjXError(elem, "exactly one of (geom2, body2) must be specified"); + } + psen->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM; + if (type=="distance") { + psen->type = mjSENS_GEOMDIST; + } else if (type=="normal") { + psen->type = mjSENS_GEOMNORMAL; + } else { + psen->type = mjSENS_GEOMFROMTO; + } + } + // global sensors else if (type=="clock") { psen->type = mjSENS_CLOCK; diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 69059900..e0f857db 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -99,7 +99,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 227 +#define nMJCF 230 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 3ebc2718..65e21c02 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1875,46 +1875,82 @@ void mjXWriter::Sensor(XMLElement* root) { elem = InsertEnd(section, "framepos"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEQUAT: elem = InsertEnd(section, "framequat"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEXAXIS: elem = InsertEnd(section, "framexaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEYAXIS: elem = InsertEnd(section, "frameyaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEZAXIS: elem = InsertEnd(section, "framezaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMELINVEL: elem = InsertEnd(section, "framelinvel"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEANGVEL: elem = InsertEnd(section, "frameangvel"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMELINACC: elem = InsertEnd(section, "framelinacc"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; case mjSENS_FRAMEANGACC: elem = InsertEnd(section, "frameangacc"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->get_objname()); + if (psen->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); + WriteAttrTxt(elem, "refname", psen->get_refname()); + } break; // sensors related to kinematic subtrees; attached to a body (which is the subtree root) @@ -1930,6 +1966,21 @@ void mjXWriter::Sensor(XMLElement* root) { elem = InsertEnd(section, "subtreeangmom"); WriteAttrTxt(elem, "body", psen->get_objname()); break; + case mjSENS_GEOMDIST: + elem = InsertEnd(section, "distance"); + WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); + WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + break; + case mjSENS_GEOMNORMAL: + elem = InsertEnd(section, "normal"); + WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); + WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + break; + case mjSENS_GEOMFROMTO: + elem = InsertEnd(section, "fromto"); + WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); + WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + break; // global sensors case mjSENS_CLOCK: @@ -1968,12 +2019,6 @@ void mjXWriter::Sensor(XMLElement* root) { WriteAttr(elem, "noise", 1, &psen->noise, &zero); } WriteVector(elem, "user", psen->get_userdata()); - - // add reference if present - if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); - } } // remove section if empty diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 53101197..0667c321 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 @@ -44,7 +46,7 @@ using ::testing::StrEq; using SensorTest = MujocoTest; -// --------------------- test sensor disableflag ----------------------------- +// --------------------- test sensor disableflag ------------------------------ // hand-picked positions and orientations for simple expected values TEST_F(SensorTest, DisableSensors) { @@ -255,7 +257,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) { mj_deleteModel(model); } -// object and reference in the same body, expect angular velocites to be zero +// object and reference in the same body, expect angular velocities to be zero TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { constexpr char xml[] = R"( @@ -279,7 +281,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { data->qvel[0] = 1; mj_forward(model, data); - // obj and ref rotate together, relative angular velocites should be zero + // obj and ref rotate together, relative angular velocities should be zero std::vector angvel = GetSensor(model, data, 0); EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0})); @@ -415,14 +417,14 @@ TEST_F(SensorTest, Clock) { mjData* data = mj_makeData(model); // call step 4 times, checking that clock works as expected - for (int i=0; i<5; i++) { + for (int i=0; i < 5; i++) { mj_step(model, data); - mj_step1(model, data); // update values of position-based sensors + mj_step1(model, data); // update values of position-based sensors EXPECT_EQ(data->sensordata[0], data->time); EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3)); } - // chack names + // check names const char* name0 = mj_id2name(model, mjOBJ_SENSOR, 0); EXPECT_EQ(name0, nullptr); const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1); @@ -432,7 +434,80 @@ TEST_F(SensorTest, Clock) { mj_deleteModel(model); } -// ------------------------- camera sensor tests ----------------------------- +// test clock sensor +TEST_F(SensorTest, CollisionSequential) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + EXPECT_DOUBLE_EQ(data->sensordata[0], 0.8); + EXPECT_DOUBLE_EQ(data->sensordata[1], 0.7); + EXPECT_DOUBLE_EQ(data->sensordata[2], 0.5); + + mjtNum eps = 1e-14; + + EXPECT_THAT(GetSensor(model, data, 3), + Pointwise(DoubleNear(eps), std::vector{0, 0, 1})); + EXPECT_THAT(GetSensor(model, data, 4), + Pointwise(DoubleNear(eps), std::vector{0, 0, -1})); + EXPECT_THAT(GetSensor(model, data, 5), + Pointwise(DoubleNear(eps), std::vector{1, 0, 0})); + EXPECT_THAT(GetSensor(model, data, 6), + Pointwise(DoubleNear(eps), + std::vector{0, 0, 0, 0, 0, .8})); + EXPECT_THAT(GetSensor(model, data, 7), + Pointwise(DoubleNear(eps), + std::vector{1, 0, .7, 1, 0, 0})); + EXPECT_THAT(GetSensor(model, data, 8), + Pointwise(DoubleNear(eps), + std::vector{.2, 0, 1, .7, 0, 1})); + + EXPECT_THAT(GetSensor(model, data, 9), + Pointwise(DoubleNear(eps), GetSensor(model, data, 0))); + EXPECT_THAT(GetSensor(model, data, 10), + Pointwise(DoubleNear(eps), GetSensor(model, data, 6))); + EXPECT_THAT(GetSensor(model, data, 11), + Pointwise(DoubleNear(eps), GetSensor(model, data, 3))); + EXPECT_THAT(GetSensor(model, data, 12), + Pointwise(DoubleNear(eps), GetSensor(model, data, 5))); + EXPECT_THAT(GetSensor(model, data, 13), + Pointwise(DoubleNear(eps), GetSensor(model, data, 8))); + EXPECT_THAT(GetSensor(model, data, 14), + Pointwise(DoubleNear(eps), GetSensor(model, data, 2))); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// ------------------------- camera sensor tests ------------------------------ // test clock sensor TEST_F(SensorTest, CameraProjection) { diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index d6a892c1..52e2c5a7 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -34,7 +34,8 @@ std::vector AsVector(const mjtNum* array, int n) { } using ::testing::DoubleNear; -using ::testing::ContainsRegex; +using ::testing::Eq; +using ::testing::ContainsRegex; // NOLINT using ::testing::MatchesRegex; using ::testing::Pointwise; using ::testing::ElementsAreArray; @@ -647,5 +648,79 @@ TEST_F(SupportTest, MulMIsland) { mj_deleteModel(model); } +static constexpr char GeomDistanceTestingModel[] = R"( + + + + + + + + + + + + +)"; + +TEST_F(SupportTest, GeomDistance) { + mjModel* model = LoadModelFromString(GeomDistanceTestingModel); + mjData* data = mj_makeData(model); + mj_kinematics(model, data); + + // plane-sphere, distmax too small + mjtNum distmax = 0.5; + EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5); + mjtNum fromto[6]; + EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5); + EXPECT_THAT(fromto, Pointwise(Eq(), std::vector{0, 0, 0, 0, 0, 0})); + + // plane-sphere + distmax = 1.0; + EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 0, 1, 1.0, fromto), 0.8); + mjtNum eps = 1e-12; + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{0, 0, 0, 0, 0, 0.8})); + + // sphere-plane + EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 0, 1.0, fromto), 0.8); + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{0, 0, 0.8, 0, 0, 0})); + + // sphere-sphere + EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 2, 1.0, fromto), 0.5); + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{.2, 0, 1, .7, 0, 1})); + + // sphere-sphere, flipped order + EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 2, 1, 1.0, fromto), 0.5); + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{.7, 0, 1, .2, 0, 1})); + + // TODO: b/339596989 - Improve the bounds below (mjc_Convex). + + // mesh-sphere (close distmax) + distmax = 0.701; + eps = 1e-5; + EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), + DoubleNear(0.7, eps)); + eps = 1e-3; + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{0, 0, .1, 0, 0, .8})); + + // mesh-sphere (far distmax) + distmax = 1.0; + eps = 1e-3; + EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), + DoubleNear(0.7, eps)); + eps = 2e-2; + EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), + std::vector{0, 0, .1, 0, 0, .8})); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/sensor/fromto_body_body.xml b/test/engine/testdata/sensor/fromto_body_body.xml new file mode 100644 index 00000000..e185a3ae --- /dev/null +++ b/test/engine/testdata/sensor/fromto_body_body.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sensor/fromto_convex.xml b/test/engine/testdata/sensor/fromto_convex.xml new file mode 100644 index 00000000..e46deceb --- /dev/null +++ b/test/engine/testdata/sensor/fromto_convex.xml @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sensor/fromto_primitive.xml b/test/engine/testdata/sensor/fromto_primitive.xml new file mode 100644 index 00000000..2d39eaca --- /dev/null +++ b/test/engine/testdata/sensor/fromto_primitive.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 93043166..f23a7574 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -542,7 +542,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadRefName) { std::array error; LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(error.data(), - HasSubstr("unrecognized name 'wrong_name' of reference frame")); + HasSubstr("unrecognized name 'wrong_name' of object")); EXPECT_THAT(error.data(), HasSubstr("line 8")); } @@ -601,7 +601,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) { ASSERT_THAT(model, IsNull()); EXPECT_THAT( error.data(), - HasSubstr("unrecognized name 'alessio' of reference frame object")); + HasSubstr("unrecognized name 'alessio' of object")); EXPECT_THAT(error.data(), HasSubstr("name 'tom'")); EXPECT_THAT(error.data(), HasSubstr("line 7")); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 79dac388..ca8a3729 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -358,9 +358,12 @@ public enum mjtSensor : int{ mjSENS_SUBTREECOM = 34, mjSENS_SUBTREELINVEL = 35, mjSENS_SUBTREEANGMOM = 36, - mjSENS_CLOCK = 37, - mjSENS_PLUGIN = 38, - mjSENS_USER = 39, + mjSENS_GEOMDIST = 37, + mjSENS_GEOMNORMAL = 38, + mjSENS_GEOMFROMTO = 39, + mjSENS_CLOCK = 40, + mjSENS_PLUGIN = 41, + mjSENS_USER = 42, } public enum mjtStage : int{ mjSTAGE_NONE = 0, @@ -6668,6 +6671,9 @@ public static unsafe extern void mj_objectVelocity(mjModel_* m, mjData_* d, int [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_objectAcceleration(mjModel_* m, mjData_* d, int objtype, int objid, double* res, int flg_local); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern double mj_geomDistance(mjModel_* m, mjData_* d, int geom1, int geom2, double distmax, double* fromto); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_contactForce(mjModel_* m, mjData_* d, int id, double* result);