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

Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

Browse Source 
PiperOrigin-RevId: 632458301
Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3
This commit is contained in:
Yuval Tassa
2024-05-10 04:51:46 -07:00
committed by Copybara-Service
parent 31de1dad9d
commit 02d015458c
29 changed files with 1002 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+26 -4
View File
@@ -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 <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
.. _mj_contactForce:
mj_contactForce
doc/APIreference/functions_override.rst
+21 -4
View File
@@ -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-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 <https://github.com/google-deepmind/mujoco/blob/main/src/engine/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
doc/XMLreference.rst
+180
View File
@@ -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<sensor-distance>`, :ref:`sensor/normal<sensor-normal>` and
:ref:`sensor/fromto<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<coSelection>`
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<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 <https://github.com/google-deepmind/mujoco/blob/main/src/engine/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-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<mjtVisFlag>` 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-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:
doc/XMLschema.rst
+27
View File
@@ -1163,6 +1163,33 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`distance | \* | :class: mjcf-attributes |
| <sensor-distance>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-distance-name>` | :ref:`geom1<sensor-distance-geom1>` | :ref:`geom2<sensor-distance-geom2>` | :ref:`body1<sensor-distance-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-distance-body2>` | :ref:`cutoff<sensor-distance-cutoff>` | :ref:`noise<sensor-distance-noise>` | :ref:`user<sensor-distance-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`normal | \* | :class: mjcf-attributes |
| <sensor-normal>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-normal-name>` | :ref:`geom1<sensor-normal-geom1>` | :ref:`geom2<sensor-normal-geom2>` | :ref:`body1<sensor-normal-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-normal-body2>` | :ref:`cutoff<sensor-normal-cutoff>` | :ref:`noise<sensor-normal-noise>` | :ref:`user<sensor-normal-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`fromto | \* | :class: mjcf-attributes |
| <sensor-fromto>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-fromto-name>` | :ref:`geom1<sensor-fromto-geom1>` | :ref:`geom2<sensor-fromto-geom2>` | :ref:`body1<sensor-fromto-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-fromto-body2>` | :ref:`cutoff<sensor-fromto-cutoff>` | :ref:`noise<sensor-fromto-noise>` | :ref:`user<sensor-fromto-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`clock | \* | :class: mjcf-attributes |
| <sensor-clock>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
doc/changelog.rst
+9 -2
View File
@@ -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<sensor-distance>`, :ref:`normal<sensor-normal>`,
:ref:`fromto<sensor-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<compiler-fusestatic>` is enabled, as is
2. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-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)
doc/includes/references.h
+7
View File
@@ -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);
include/mujoco/mjmodel.h
+5
View File
@@ -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
include/mujoco/mujoco.h
+4
View File
@@ -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]);
introspect/enums.py
+6 -3
View File
@@ -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',
introspect/functions.py
+39
View File
@@ -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',
python/mujoco/bindings_test.py
+8
View File
@@ -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
python/mujoco/functions.cc
+12
View File
@@ -531,6 +531,18 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mj_objectVelocity>(pymodule);
Def<traits::mj_objectAcceleration>(pymodule);
Def<traits::mj_contactForce>(pymodule);
Def<traits::mj_geomDistance>(
pymodule,
[](const raw::MjModel* m, const raw::MjData* d,
int geom1, int geom2, mjtNum distmax,
std::optional<Eigen::Ref<EigenArrayXX>> 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<traits::mj_differentiatePos>(
pymodule,
[](const raw::MjModel* m, Eigen::Ref<EigenVectorX> qvel,
src/engine/engine_io.c
+5
View File
@@ -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;
src/engine/engine_ray.c
+4 -5
View File
@@ -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;
}
}
}
src/engine/engine_sensor.c
+118 -16
View File
@@ -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;
src/engine/engine_support.c
+43
View File
@@ -20,6 +20,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#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;
src/engine/engine_support.h
+4
View File
@@ -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]);
src/engine/engine_vis_visualize.c
+14
View File
@@ -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
}
}
}
src/user/user_objects.cc
+40 -12
View File
@@ -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");
}
}
src/xml/xml_native_reader.cc
+26
View File
@@ -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;
src/xml/xml_native_reader.h
+1 -1
View File
@@ -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_
src/xml/xml_native_writer.cc
+51 -6
View File
@@ -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
test/engine/engine_sensor_test.cc
+82 -7
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_sensor.c.
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
@@ -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"(
<mujoco>
@@ -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"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="1 1 1"/>
<geom name="sphere1" pos="0 0 1" size="0.2"/>
<geom name="sphere2" pos="1 0 1" size="0.3"/>
</worldbody>
<sensor>
<distance name="0" geom1="plane" geom2="sphere1" cutoff="1"/>
<distance name="1" geom1="sphere2" geom2="plane" cutoff="1"/>
<distance name="2" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<normal name="3" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="4" geom1="sphere2" geom2="plane" cutoff="1"/>
<normal name="5" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="6" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="7" geom1="sphere2" geom2="plane" cutoff="1"/>
<fromto name="8" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<!-- sequential sensors with identical signature -->
<distance name="9" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="10" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="11" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="12" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="13" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<distance name="14" geom1="sphere1" geom2="sphere2" cutoff="1"/>
</sensor>
</mujoco>
)";
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<mjtNum>{0, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 4),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, -1}));
EXPECT_THAT(GetSensor(model, data, 5),
Pointwise(DoubleNear(eps), std::vector<mjtNum>{1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 6),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{0, 0, 0, 0, 0, .8}));
EXPECT_THAT(GetSensor(model, data, 7),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{1, 0, .7, 1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 8),
Pointwise(DoubleNear(eps),
std::vector<mjtNum>{.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) {
test/engine/engine_support_test.cc
+76 -1
View File
@@ -34,7 +34,8 @@ std::vector<mjtNum> 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"(
<mujoco>
<asset>
<mesh name="box" scale=".1 .1 .1" vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 1 1 1"/>
</asset>
<worldbody>
<geom type="plane" size="1 1 1"/>
<geom pos="0 0 1" size="0.2"/>
<geom pos="1 0 1" size="0.3"/>
<geom type="mesh" mesh="box"/>
</worldbody>
</mujoco>
)";
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<mjtNum>{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<mjtNum>{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<mjtNum>{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<mjtNum>{.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<mjtNum>{.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<mjtNum>{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<mjtNum>{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/sensor/fromto_body_body.xml
Vendored
+29
View File
@@ -0,0 +1,29 @@
<mujoco model="Body-Body collisions fromto">
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom type="plane" size="2 2 .01"/>
<body name="1" pos="0 0 .3">
<freejoint/>
<replicate count="50" offset="0 0 0.01" euler="0 0 25">
<geom type="capsule" size=".01 .05" pos="0 -.23 -.2" euler="0 84.5 0" rgba="0 0 1 1"/>
</replicate>
</body>
<body name="2" pos=".6 0 .3" euler="0 0 90">
<freejoint/>
<replicate count="50" offset="0 0 0.01" euler="0 0 15">
<geom type="capsule" pos=".025 -.2 -.2" size=".032 .04" euler="-24 -12 2" rgba=".8 0 0 1"/>
</replicate>
</body>
</worldbody>
<sensor>
<fromto body1="1" body2="2" cutoff="1"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/fromto_convex.xml
Vendored
+83
View File
@@ -0,0 +1,83 @@
<mujoco model="Convex-Convex collisions">
<option>
<flag gravity="disable"/>
</option>
<asset>
<mesh name="halfbox1" scale=".2 .3 .1"
vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
<mesh name="halfbox2" scale=".2 .3 .1"
vertex="1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
<hfield name="hfield" nrow="3" ncol="4" size="1 1 .1 .08"
elevation="1 0 1 2
0 2 1 1
1 0 1 0"/>
</asset>
<default>
<geom rgba=".2 .2 .6 1"/>
</default>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom name="hfield" type="hfield" hfield="hfield" rgba="0.5 0.5 0.5 1"/>
<body pos=".2 0 .6">
<freejoint/>
<geom name="sphere" type="sphere" size=".15"/>
</body>
<body pos="0 0 .3">
<freejoint/>
<geom name="ellipsoid" type="ellipsoid" size=".1 .1 .2"/>
</body>
<body pos=".4 0 .3">
<freejoint/>
<geom name="cylinder" type="cylinder" size=".1 .15"/>
</body>
<body pos="-.5 0 .3">
<freejoint/>
<geom name="box" type="box" size=".2 .15 .1"/>
</body>
<body pos="-.5 0 .6">
<freejoint/>
<geom name="mesh1" type="mesh" mesh="halfbox1"/>
</body>
<body pos="-.5 -.6 .3">
<freejoint/>
<geom name="mesh2" type="mesh" mesh="halfbox2"/>
</body>
</worldbody>
<sensor>
<fromto geom1="ellipsoid" geom2="box" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="cylinder" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="mesh2" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="hfield" cutoff="1"/>
<fromto geom1="mesh1" geom2="sphere" cutoff="1"/>
<fromto geom1="mesh1" geom2="box" cutoff="1"/>
<fromto geom1="mesh1" geom2="mesh2" cutoff="1"/>
<fromto geom1="hfield" geom2="sphere" cutoff="1"/>
<fromto geom1="hfield" geom2="box" cutoff="1"/>
<fromto geom1="hfield" geom2="mesh1" cutoff="1"/>
<fromto geom1="ellipsoid" geom2="box" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="cylinder" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="mesh2" cutoff="0.2"/>
<fromto geom1="ellipsoid" geom2="hfield" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="sphere" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="box" cutoff="0.2"/>
<fromto geom1="mesh1" geom2="mesh2" cutoff="0.2"/>
<fromto geom1="hfield" geom2="sphere" cutoff="0.2"/>
<fromto geom1="hfield" geom2="box" cutoff="0.2"/>
<fromto geom1="hfield" geom2="mesh1" cutoff="0.2"/>
</sensor>
</mujoco>
test/engine/testdata/sensor/fromto_primitive.xml
Vendored
+71
View File
@@ -0,0 +1,71 @@
<mujoco model="Primitive collisions">
<asset>
<mesh name="halfbox" scale=".2 .3 .1"
vertex="0 0 0 1 0 0 0 1 0 1 1 0
0 0 1 1 0 1 0 1 1 "/>
</asset>
<default>
<geom rgba=".2 .2 .6 1"/>
</default>
<worldbody>
<light pos=".5 0 3"/>
<light pos="-.5 0 3"/>
<geom name="plane" type="plane" size="2 2 .01" rgba="0.5 0.5 0.5 1"/>
<body pos=".2 0 .6">
<freejoint/>
<geom name="sphere" type="sphere" size=".15"/>
</body>
<body pos="0 0 .3">
<freejoint/>
<geom name="capsule1" type="capsule" size=".1 .1"/>
</body>
<body pos="-.2 0 .4" euler="90 0 0">
<freejoint/>
<geom name="capsule2" type="capsule" size=".05 .1"/>
</body>
<body pos=".4 0 .3">
<freejoint/>
<geom name="cylinder" type="cylinder" size=".1 .15"/>
</body>
<body pos="-.5 0 .3">
<freejoint/>
<geom name="box1" type="box" size=".2 .15 .1"/>
</body>
<body pos="-.5 0 .6">
<freejoint/>
<geom name="box2" type="box" size=".1 .05 .1"/>
</body>
<body pos="-.5 -.6 .3">
<freejoint/>
<geom name="mesh" type="mesh" mesh="halfbox"/>
</body>
</worldbody>
<sensor>
<fromto geom1="plane" geom2="sphere" cutoff="1"/>
<fromto geom1="plane" geom2="capsule1" cutoff="1"/>
<fromto geom1="plane" geom2="box1" cutoff="1"/>
<fromto geom1="plane" geom2="cylinder" cutoff="1"/>
<fromto geom1="plane" geom2="mesh" cutoff="1"/>
<fromto geom1="capsule2" geom2="capsule1" cutoff="1"/>
<fromto geom1="capsule1" geom2="sphere" cutoff="1"/>
<fromto geom1="sphere" geom2="cylinder" cutoff="1"/>
<fromto geom1="box1" geom2="sphere" cutoff="1"/>
<fromto geom1="box2" geom2="capsule2" cutoff="1"/>
<fromto geom1="box1" geom2="box2" cutoff="1"/>
</sensor>
</mujoco>
test/user/user_objects_test.cc
+2 -2
View File
@@ -542,7 +542,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadRefName) {
std::array<char, 1024> 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"));
}
unity/Runtime/Bindings/MjBindings.cs
+9 -3
View File
@@ -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);