Merge branch 'google-deepmind:main' into main
This commit is contained in:
Andrew Luo
@@ -250,7 +250,7 @@ jobs:
|
||||
shell: bash
|
||||
run:
|
||||
source ${{ matrix.tmpdir }}/venv/bin/activate &&
|
||||
pytest -n auto -v --pyargs mujoco.mjx
|
||||
pytest -n auto -v -k 'not IntegrationTest' --pyargs mujoco.mjx
|
||||
- name: Notify team chat
|
||||
shell: bash
|
||||
env:
|
||||
|
||||
@@ -3181,6 +3181,12 @@ the direction specified by the dir attribute. It does not have a full spatial fr
|
||||
:at:`dir`: :at-val:`real(3), "0 0 -1"`
|
||||
Direction of the light.
|
||||
|
||||
.. _body-light-radius:
|
||||
|
||||
:at:`radius`: :at-val:`real, "0"`
|
||||
Radius of the light, affects shadow softness. This attribute has no effect in MuJoCo's native renderer, but it can be
|
||||
useful when rendering scenes with an external renderer.
|
||||
|
||||
.. _body-light-attenuation:
|
||||
|
||||
:at:`attenuation`: :at-val:`real(3), "1 0 0"`
|
||||
@@ -7586,6 +7592,8 @@ if omitted.
|
||||
|
||||
.. _default-light-dir:
|
||||
|
||||
.. _default-light-radius:
|
||||
|
||||
.. _default-light-directional:
|
||||
|
||||
.. _default-light-castshadow:
|
||||
|
||||
+8
-6
@@ -369,11 +369,11 @@
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`name<body-light-name>` | :ref:`class<body-light-class>` | :ref:`directional<body-light-directional>` | :ref:`castshadow<body-light-castshadow>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`active<body-light-active>` | :ref:`pos<body-light-pos>` | :ref:`dir<body-light-dir>` | :ref:`attenuation<body-light-attenuation>` | |
|
||||
| | | | :ref:`active<body-light-active>` | :ref:`pos<body-light-pos>` | :ref:`dir<body-light-dir>` | :ref:`radius<body-light-radius>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`cutoff<body-light-cutoff>` | :ref:`exponent<body-light-exponent>` | :ref:`ambient<body-light-ambient>` | :ref:`diffuse<body-light-diffuse>` | |
|
||||
| | | | :ref:`attenuation<body-light-attenuation>` | :ref:`cutoff<body-light-cutoff>` | :ref:`exponent<body-light-exponent>` | :ref:`ambient<body-light-ambient>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`specular<body-light-specular>` | :ref:`mode<body-light-mode>` | :ref:`target<body-light-target>` | | |
|
||||
| | | | :ref:`diffuse<body-light-diffuse>` | :ref:`specular<body-light-specular>` | :ref:`mode<body-light-mode>` | :ref:`target<body-light-target>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| body |br| |_| |L| | | .. table:: |
|
||||
@@ -1378,11 +1378,13 @@
|
||||
| :ref:`light | ? | :class: mjcf-attributes |
|
||||
| <default-light>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`pos<default-light-pos>` | :ref:`dir<default-light-dir>` | :ref:`directional<default-light-directional>` | :ref:`castshadow<default-light-castshadow>` | |
|
||||
| | | | :ref:`pos<default-light-pos>` | :ref:`dir<default-light-dir>` | :ref:`radius<default-light-radius>` | :ref:`directional<default-light-directional>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`active<default-light-active>` | :ref:`attenuation<default-light-attenuation>` | :ref:`cutoff<default-light-cutoff>` | :ref:`exponent<default-light-exponent>` | |
|
||||
| | | | :ref:`castshadow<default-light-castshadow>` | :ref:`active<default-light-active>` | :ref:`attenuation<default-light-attenuation>` | :ref:`cutoff<default-light-cutoff>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`ambient<default-light-ambient>` | :ref:`diffuse<default-light-diffuse>` | :ref:`specular<default-light-specular>` | :ref:`mode<default-light-mode>` | |
|
||||
| | | | :ref:`exponent<default-light-exponent>` | :ref:`ambient<default-light-ambient>` | :ref:`diffuse<default-light-diffuse>` | :ref:`specular<default-light-specular>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`mode<default-light-mode>` | | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| default |br| |_| |L| | | .. table:: |
|
||||
|
||||
+20
-1
@@ -2,6 +2,25 @@
|
||||
Changelog
|
||||
=========
|
||||
|
||||
Upcoming version (not yet released)
|
||||
-----------------------------------
|
||||
|
||||
General
|
||||
^^^^^^^
|
||||
1. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
|
||||
:ref:`scale<asset-mesh-scale>` attribute.
|
||||
2. Added the :ref:`light/radius<body-light-radius>` attribute and corresponding ``mjModel.light_radius`` field. This
|
||||
value has no effect in MuJoCo's native renderer, but can be usful when rendering scenes with an external renderer.
|
||||
|
||||
MJX
|
||||
^^^
|
||||
3. Added cylinder plane collisions.
|
||||
|
||||
Bug fixes
|
||||
^^^^^^^^^
|
||||
4. Defaults of lights were not being saved, now fixed.
|
||||
|
||||
|
||||
Version 3.1.4 (April 10th, 2024)
|
||||
--------------------------------
|
||||
|
||||
@@ -27,7 +46,7 @@ General
|
||||
model
|
||||
`refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__,
|
||||
which demostrates Cartesian actuation of an arm, has been updated to use this attribute.
|
||||
3. Added support for gmsh format 2.2, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
|
||||
3. Added support for gmsh format 2.2 , tetrahedral mesh, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
|
||||
|
||||
4. Added :ref:`mju_euler2Quat` for converting an Euler-angle sequence to quaternion.
|
||||
|
||||
|
||||
+14
-12
@@ -1082,6 +1082,7 @@ struct mjModel_ {
|
||||
mjtByte* light_active; // is light on (nlight x 1)
|
||||
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
||||
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
||||
mjtNum* light_radius; // radius of the light (nlight x 1)
|
||||
mjtNum* light_poscom0; // global position rel. to sub-com in qpos0 (nlight x 3)
|
||||
mjtNum* light_pos0; // global position rel. to body in qpos0 (nlight x 3)
|
||||
mjtNum* light_dir0; // global direction in qpos0 (nlight x 3)
|
||||
@@ -1165,6 +1166,7 @@ struct mjModel_ {
|
||||
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
||||
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
||||
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
||||
mjtNum* mesh_scale; // scaling applied to asset vertices (nmesh x 3)
|
||||
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
||||
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||
@@ -1194,20 +1196,20 @@ struct mjModel_ {
|
||||
int* skin_pathadr; // address of asset path for skin; -1: none (nskin x 1)
|
||||
|
||||
// height fields
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows in grid (nhfield x 1)
|
||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows in grid (nhfield x 1)
|
||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||
|
||||
// textures
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows in texture image (ntex x 1)
|
||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||
int* tex_adr; // address in rgb (ntex x 1)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of asset path for texture; -1: none (ntex x 1)
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows in texture image (ntex x 1)
|
||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||
int* tex_adr; // address in rgb (ntex x 1)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||
|
||||
// materials
|
||||
int* mat_texid; // texture id; -1: none (nmat x 1)
|
||||
|
||||
+14
-12
@@ -797,6 +797,7 @@ struct mjModel_ {
|
||||
mjtByte* light_active; // is light on (nlight x 1)
|
||||
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
||||
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
||||
mjtNum* light_radius; // radius of the light (nlight x 1)
|
||||
mjtNum* light_poscom0; // global position rel. to sub-com in qpos0 (nlight x 3)
|
||||
mjtNum* light_pos0; // global position rel. to body in qpos0 (nlight x 3)
|
||||
mjtNum* light_dir0; // global direction in qpos0 (nlight x 3)
|
||||
@@ -880,6 +881,7 @@ struct mjModel_ {
|
||||
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
||||
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
||||
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
||||
mjtNum* mesh_scale; // scaling applied to asset vertices (nmesh x 3)
|
||||
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
||||
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||
@@ -909,20 +911,20 @@ struct mjModel_ {
|
||||
int* skin_pathadr; // address of asset path for skin; -1: none (nskin x 1)
|
||||
|
||||
// height fields
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows in grid (nhfield x 1)
|
||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows in grid (nhfield x 1)
|
||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||
|
||||
// textures
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows in texture image (ntex x 1)
|
||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||
int* tex_adr; // address in rgb (ntex x 1)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of asset path for texture; -1: none (ntex x 1)
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows in texture image (ntex x 1)
|
||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||
int* tex_adr; // address in rgb (ntex x 1)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||
|
||||
// materials
|
||||
int* mat_texid; // texture id; -1: none (nmat x 1)
|
||||
|
||||
@@ -295,6 +295,7 @@
|
||||
XMJV( mjtByte, light_active, nlight, 1 ) \
|
||||
X ( mjtNum, light_pos, nlight, 3 ) \
|
||||
X ( mjtNum, light_dir, nlight, 3 ) \
|
||||
X ( mjtNum, light_radius, nlight, 1 ) \
|
||||
X ( mjtNum, light_poscom0, nlight, 3 ) \
|
||||
X ( mjtNum, light_pos0, nlight, 3 ) \
|
||||
X ( mjtNum, light_dir0, nlight, 3 ) \
|
||||
@@ -365,6 +366,7 @@
|
||||
XMJV( int, mesh_bvhadr, nmesh, 1 ) \
|
||||
XMJV( int, mesh_bvhnum, nmesh, 1 ) \
|
||||
XMJV( int, mesh_graphadr, nmesh, 1 ) \
|
||||
X ( mjtNum, mesh_scale, nmesh, 3 ) \
|
||||
X ( mjtNum, mesh_pos, nmesh, 3 ) \
|
||||
X ( mjtNum, mesh_quat, nmesh, 4 ) \
|
||||
X ( float, mesh_vert, nmeshvert, 3 ) \
|
||||
|
||||
+26
-12
@@ -2116,6 +2116,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
doc='direction rel. to body frame (nlight x 3)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='light_radius',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtNum'),
|
||||
),
|
||||
doc='radius of the light (nlight x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='light_poscom0',
|
||||
type=PointerType(
|
||||
@@ -2655,6 +2662,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='mesh_scale',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtNum'),
|
||||
),
|
||||
doc='scaling applied to asset vertices (nmesh x 3)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='mesh_pos',
|
||||
type=PointerType(
|
||||
@@ -2835,84 +2849,84 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtNum'),
|
||||
),
|
||||
doc='(x, y, z_top, z_bottom) (nhfield x 4)',
|
||||
doc='(x, y, z_top, z_bottom) (nhfield x 4)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_nrow',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of rows in grid (nhfield x 1)',
|
||||
doc='number of rows in grid (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_ncol',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of columns in grid (nhfield x 1)',
|
||||
doc='number of columns in grid (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_adr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address in hfield_data (nhfield x 1)',
|
||||
doc='address in hfield_data (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_data',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='float'),
|
||||
),
|
||||
doc='elevation data (nhfielddata x 1)', # pylint: disable=line-too-long
|
||||
doc='elevation data (nhfielddata x 1)', # pylint: disable=line-too-long
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_pathadr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address of asset path for hfield; -1: none (nhfield x 1)',
|
||||
doc='address of hfield asset path; -1: none (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_type',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='texture type (mjtTexture) (ntex x 1)',
|
||||
doc='texture type (mjtTexture) (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_height',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of rows in texture image (ntex x 1)',
|
||||
doc='number of rows in texture image (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_width',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of columns in texture image (ntex x 1)',
|
||||
doc='number of columns in texture image (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_adr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address in rgb (ntex x 1)',
|
||||
doc='address in rgb (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_rgb',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtByte'),
|
||||
),
|
||||
doc='rgb (alpha = 1) (ntexdata x 1)', # pylint: disable=line-too-long
|
||||
doc='rgb (alpha = 1) (ntexdata x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_pathadr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address of asset path for texture; -1: none (ntex x 1)',
|
||||
doc='address of texture asset path; -1: none (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='mat_texid',
|
||||
|
||||
@@ -186,7 +186,7 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
plane_pos = convex.mat.T @ (plane.pos - convex.pos)
|
||||
n = convex.mat.T @ plane.mat[:, 2]
|
||||
support = (plane_pos - vert) @ n
|
||||
idx = _manifold_points(vert, support > 0, n)
|
||||
idx = _manifold_points(vert, support > jp.maximum(0, support.max() - 1e-4), n)
|
||||
pos = vert[idx]
|
||||
|
||||
# convert to world frame
|
||||
|
||||
@@ -33,6 +33,7 @@ from mujoco.mjx._src.collision_convex import plane_convex
|
||||
from mujoco.mjx._src.collision_convex import sphere_convex
|
||||
from mujoco.mjx._src.collision_primitive import capsule_capsule
|
||||
from mujoco.mjx._src.collision_primitive import plane_capsule
|
||||
from mujoco.mjx._src.collision_primitive import plane_cylinder
|
||||
from mujoco.mjx._src.collision_primitive import plane_ellipsoid
|
||||
from mujoco.mjx._src.collision_primitive import plane_sphere
|
||||
from mujoco.mjx._src.collision_primitive import sphere_capsule
|
||||
@@ -52,6 +53,7 @@ _COLLISION_FUNC = {
|
||||
(GeomType.PLANE, GeomType.CAPSULE): plane_capsule,
|
||||
(GeomType.PLANE, GeomType.BOX): plane_convex,
|
||||
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
|
||||
(GeomType.PLANE, GeomType.CYLINDER): plane_cylinder,
|
||||
(GeomType.PLANE, GeomType.MESH): plane_convex,
|
||||
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
|
||||
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
|
||||
|
||||
@@ -465,6 +465,50 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
||||
)
|
||||
|
||||
|
||||
class CylinderTest(absltest.TestCase):
|
||||
"""Tests the cylinder contact functions."""
|
||||
|
||||
_CYLINDER_PLANE = """
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom size="40 40 40" type="plane"/>
|
||||
<body pos="0 0 0.04">
|
||||
<joint type="free"/>
|
||||
<geom fromto="-0.1 0 0 0.1 0 0" size="0.05" type="cylinder"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
"""
|
||||
|
||||
def test_cylinder_plane(self):
|
||||
d, dx = _collide(self._CYLINDER_PLANE)
|
||||
|
||||
# cylinder is lying flat
|
||||
np.testing.assert_array_less(dx.contact.dist[:2], 0)
|
||||
np.testing.assert_array_less(-dx.contact.dist[2:], 0)
|
||||
|
||||
# sort position for comparison
|
||||
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1]))
|
||||
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]})
|
||||
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
|
||||
d.contact.pos[:] = d.contact.pos[idx]
|
||||
|
||||
# extract the contact points with penetration
|
||||
c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact)
|
||||
for field in dataclasses.fields(Contact):
|
||||
_assert_attr_eq(c, d.contact, field.name, 'cylinder_plane', 1e-5)
|
||||
|
||||
# cylinder is vertical
|
||||
xml = self._CYLINDER_PLANE.replace(
|
||||
'<geom fromto="-0.1 0 0 0.1 0 0"', '<geom fromto="0 0 -0.1 0 0 0.1"')
|
||||
xml = xml.replace('pos="0 0 0.04"', 'pos="0 0 0.095"')
|
||||
d, dx = _collide(xml)
|
||||
|
||||
np.testing.assert_array_less(dx.contact.dist, 0)
|
||||
for field in dataclasses.fields(Contact):
|
||||
_assert_attr_eq(dx.contact, d.contact, field.name, 'cylinder_plane', 1e-5)
|
||||
|
||||
|
||||
class ConvexTest(absltest.TestCase):
|
||||
"""Tests the convex contact functions."""
|
||||
|
||||
|
||||
@@ -78,6 +78,65 @@ def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Contact:
|
||||
)
|
||||
|
||||
|
||||
def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Contact:
|
||||
"""Calculates one contact between an cylinder and a plane."""
|
||||
n = plane.mat[:, 2]
|
||||
axis = cylinder.mat[:, 2]
|
||||
|
||||
# make sure axis points towards plane
|
||||
prjaxis = jp.dot(n, axis)
|
||||
sign = -math.sign(prjaxis)
|
||||
axis, prjaxis = axis * sign, prjaxis * sign
|
||||
|
||||
# compute normal distance to cylinder center
|
||||
dist0 = jp.dot(cylinder.pos - plane.pos, n)
|
||||
|
||||
# remove component of -normal along axis, compute length
|
||||
vec = axis * prjaxis - n
|
||||
len_ = math.norm(vec)
|
||||
|
||||
vec = jp.where(
|
||||
len_ < 1e-12,
|
||||
# disk parallel to plane: pick x-axis of cylinder, scale by radius
|
||||
cylinder.mat[:, 0] * cylinder.size[0],
|
||||
# general configuration: normalize vector, scale by radius
|
||||
vec / len_ * cylinder.size[0]
|
||||
)
|
||||
|
||||
# project vector on normal
|
||||
prjvec = jp.dot(vec, n)
|
||||
|
||||
# scale axis by half-length
|
||||
axis *= cylinder.size[1]
|
||||
prjaxis *= cylinder.size[1]
|
||||
|
||||
# compute sideways vector: vec1
|
||||
prjvec1 = -prjvec * 0.5
|
||||
vec1 = math.normalize(jp.cross(vec, axis)) * cylinder.size[0]
|
||||
vec1 *= jp.sqrt(3.0) * 0.5
|
||||
|
||||
# disk parallel to plane
|
||||
d1 = dist0 + prjaxis + prjvec
|
||||
d2 = dist0 + prjaxis + prjvec1
|
||||
dist = jp.array([d1, d2, d2])
|
||||
pos = cylinder.pos + axis + jp.array([
|
||||
vec - n * d1 * 0.5,
|
||||
vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||
-vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||
])
|
||||
|
||||
# cylinder parallel to plane
|
||||
cond = jp.abs(prjaxis) < 1e-3
|
||||
d3 = dist0 - prjaxis + prjvec
|
||||
dist = jp.where(cond, dist.at[1].set(d3), dist)
|
||||
pos = jp.where(
|
||||
cond, pos.at[1].set(cylinder.pos + vec - axis - n * d3 * 0.5), pos
|
||||
)
|
||||
|
||||
frame = jp.stack([math.make_frame(n)] * 3, axis=0)
|
||||
return dist, pos, frame
|
||||
|
||||
|
||||
def _sphere_sphere(
|
||||
pos1: jax.Array, radius1: jax.Array, pos2: jax.Array, radius2: jax.Array
|
||||
) -> Contact:
|
||||
@@ -135,6 +194,7 @@ def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Contact:
|
||||
plane_sphere.ncon = 1
|
||||
plane_capsule.ncon = 2
|
||||
plane_ellipsoid.ncon = 1
|
||||
plane_cylinder.ncon = 3
|
||||
sphere_sphere.ncon = 1
|
||||
sphere_capsule.ncon = 1
|
||||
capsule_capsule.ncon = 1
|
||||
|
||||
@@ -33,8 +33,7 @@ from mujoco.mjx._src.collision_base import GeomInfo
|
||||
from mujoco.mjx._src.dataclasses import PyTreeNode
|
||||
# pylint: enable=g-importing-member
|
||||
|
||||
|
||||
# the objective function, inputs: pos (that we optimize for) and size (known)
|
||||
# the SDF function takes position in, and returns a distance or objective
|
||||
SDFFn = Callable[[jax.Array], jax.Array]
|
||||
|
||||
|
||||
|
||||
@@ -534,11 +534,15 @@ mjModel* mj_makeModel(
|
||||
m->nuser_actuator = nuser_actuator;
|
||||
m->nuser_sensor = nuser_sensor;
|
||||
m->nnames = nnames;
|
||||
m->nnames_map = mjLOAD_MULTIPLE
|
||||
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
|
||||
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
|
||||
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
|
||||
+ nkey + nplugin);
|
||||
long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin +
|
||||
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
|
||||
nnumeric + ntext + ntuple + nkey + nplugin;
|
||||
if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
|
||||
mju_free(m);
|
||||
mju_warning("Invalid model: size of nnames_map is larger than INT_MAX");
|
||||
return 0;
|
||||
}
|
||||
m->nnames_map = mjLOAD_MULTIPLE * nnames_map;
|
||||
m->npaths = npaths;
|
||||
|
||||
#define X(name) \
|
||||
|
||||
@@ -1033,11 +1033,31 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
|
||||
fprintf(fp, "CONTACT\n");
|
||||
for (int i=0; i < d->ncon; i++) {
|
||||
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
|
||||
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
|
||||
d->contact[i].geom[0], d->contact[i].flex[0],
|
||||
d->contact[i].elem[0], d->contact[i].vert[0],
|
||||
d->contact[i].geom[1], d->contact[i].flex[1],
|
||||
d->contact[i].elem[1], d->contact[i].vert[1]);
|
||||
int g1 = d->contact[i].geom[0];
|
||||
int g2 = d->contact[i].geom[1];
|
||||
|
||||
// special case for geom-geom contacts
|
||||
if (g1 > -1 && g2 > -1) {
|
||||
fprintf(fp, " geoms ");
|
||||
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, g1);
|
||||
const char* geom2 = mj_id2name(m, mjOBJ_GEOM, g2);
|
||||
if (geom1) {
|
||||
fprintf(fp, "%s : ", geom1);
|
||||
} else {
|
||||
fprintf(fp, "%d : ", g1);
|
||||
}
|
||||
if (geom2) {
|
||||
fprintf(fp, "%s\n", geom2);
|
||||
} else {
|
||||
fprintf(fp, "%d\n", g2);
|
||||
}
|
||||
} else {
|
||||
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
|
||||
d->contact[i].geom[0], d->contact[i].flex[0],
|
||||
d->contact[i].elem[0], d->contact[i].vert[0],
|
||||
d->contact[i].geom[1], d->contact[i].flex[1],
|
||||
d->contact[i].elem[1], d->contact[i].vert[1]);
|
||||
}
|
||||
fprintf(fp, " exclude %d\n efc_address %d\n",
|
||||
d->contact[i].exclude, d->contact[i].efc_address);
|
||||
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
|
||||
|
||||
@@ -347,6 +347,7 @@ typedef struct _mjsLight { // light specification
|
||||
// frame
|
||||
double pos[3]; // position
|
||||
double dir[3]; // direction
|
||||
double radius; // radius
|
||||
mjtCamLight mode; // tracking mode
|
||||
mjString targetbody; // target body for targeting
|
||||
|
||||
|
||||
+97
-26
@@ -17,6 +17,7 @@
|
||||
#include <cstddef>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@@ -41,7 +42,6 @@
|
||||
namespace {
|
||||
namespace mju = ::mujoco::util;
|
||||
using std::vector;
|
||||
using std::string;
|
||||
using std::stringstream;
|
||||
} // namespace
|
||||
|
||||
@@ -62,6 +62,15 @@ static void ReadStrFromBuffer(char* dest, const char* src, int maxlen) {
|
||||
std::strncpy(dest, src, maxlen);
|
||||
}
|
||||
|
||||
bool IsValidElementHeader22(const std::string& line) {
|
||||
// making sure characters are numbers
|
||||
for (char c : line) {
|
||||
if (!std::isdigit(c)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// constructor: set defaults outside mjCDef
|
||||
@@ -879,7 +888,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
|
||||
}
|
||||
|
||||
// get extension and check; must be STL, OBJ or MSH
|
||||
string ext = mjuu_getext(file);
|
||||
std::string ext = mjuu_getext(file);
|
||||
if (strcasecmp(ext.c_str(), ".stl") &&
|
||||
strcasecmp(ext.c_str(), ".obj") &&
|
||||
strcasecmp(ext.c_str(), ".msh")) {
|
||||
@@ -892,8 +901,8 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
|
||||
}
|
||||
|
||||
// load resource
|
||||
string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir),
|
||||
mjs_getString(model->spec.meshdir), file);
|
||||
std::string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir),
|
||||
mjs_getString(model->spec.meshdir), file);
|
||||
mjResource* resource = nullptr;
|
||||
|
||||
try {
|
||||
@@ -998,8 +1007,8 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
|
||||
}
|
||||
|
||||
// open resource
|
||||
string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir),
|
||||
mjs_getString(model->spec.meshdir), file);
|
||||
std::string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir),
|
||||
mjs_getString(model->spec.meshdir), file);
|
||||
mjResource* resource = nullptr;
|
||||
|
||||
try {
|
||||
@@ -1037,7 +1046,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
// ascii nodes
|
||||
if (binary == 0) {
|
||||
// convert node char buffer to stringstream
|
||||
stringstream ss(string(buffer + nodebegin, nodeend - nodebegin));
|
||||
stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));
|
||||
|
||||
// read header
|
||||
ss >> numEntityBlocks >> numNodes >> minNodeTag >> maxNodeTag;
|
||||
@@ -1046,6 +1055,11 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "Error reading Nodes header");
|
||||
}
|
||||
|
||||
// check number of nodes is a positive number
|
||||
if (numNodes < 0) {
|
||||
throw mjCError(NULL, "Invalid number of nodes");
|
||||
}
|
||||
|
||||
// require single block
|
||||
if (numEntityBlocks != 1 || numNodes != numNodesInBlock) {
|
||||
throw mjCError(NULL, "All nodes must be in single block");
|
||||
@@ -1105,6 +1119,11 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "All nodes must be in single block");
|
||||
}
|
||||
|
||||
// check number of nodes is a positive number
|
||||
if (numNodes < 0) {
|
||||
throw mjCError(NULL, "Invalid number of nodes");
|
||||
}
|
||||
|
||||
// check dimensionality and save
|
||||
if (entityDim < 1 || entityDim > 3) {
|
||||
throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
|
||||
@@ -1150,7 +1169,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
if (binary == 0) {
|
||||
// convert element char buffer to stringstream
|
||||
buffer[elemend] = 0;
|
||||
stringstream ss(buffer + elembegin);
|
||||
stringstream ss(std::string(buffer + elembegin, elemend - elembegin));
|
||||
|
||||
// read header
|
||||
ss >> numEntityBlocks >> numElements >> minElementTag >> maxElementTag;
|
||||
@@ -1164,6 +1183,11 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "All elements must be in single block");
|
||||
}
|
||||
|
||||
// check number of elements is a positive number
|
||||
if (numElements < 0) {
|
||||
throw mjCError(NULL, "Invalid number of elements");
|
||||
}
|
||||
|
||||
// dimensionality must be same as nodes
|
||||
if (entityDim != def.spec.flex->dim) {
|
||||
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
|
||||
@@ -1217,6 +1241,11 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "All elements must be in single block");
|
||||
}
|
||||
|
||||
// check number of elements is a positive number
|
||||
if (numElements < 0) {
|
||||
throw mjCError(NULL, "Invalid number of elements");
|
||||
}
|
||||
|
||||
// dimensionality must be same as nodes
|
||||
if (entityDim != def.spec.flex->dim) {
|
||||
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
|
||||
@@ -1270,7 +1299,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
// ascii nodes
|
||||
if (binary == 0) {
|
||||
// convert node char buffer to stringstream
|
||||
stringstream ss(string(buffer + nodebegin, nodeend - nodebegin));
|
||||
stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));
|
||||
|
||||
// read header
|
||||
size_t maxNodeTag = 0;
|
||||
@@ -1322,6 +1351,11 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
size_t maxNodeTag = std::stoi(maxNodeTagChar);
|
||||
size_t numNodes = maxNodeTag;
|
||||
|
||||
// check number of nodes is a positive number
|
||||
if (numNodes < 0) {
|
||||
throw mjCError(NULL, "Invalid number of nodes");
|
||||
}
|
||||
|
||||
// node data: node tag and 3 nodes
|
||||
int nodeSize = sizeof(double);
|
||||
int indexSize = sizeof(int);
|
||||
@@ -1352,20 +1386,25 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
}
|
||||
}
|
||||
|
||||
size_t entityDim = 3;
|
||||
def.spec.flex->dim = entityDim;
|
||||
|
||||
// ascii elements
|
||||
if (binary == 0) {
|
||||
// convert element char buffer to stringstream
|
||||
buffer[elemend] = 0;
|
||||
stringstream ss(buffer + elembegin);
|
||||
stringstream ss(std::string(buffer + elembegin, elemend - elembegin));
|
||||
std::string line;
|
||||
|
||||
// checking header template
|
||||
std::getline(ss, line);
|
||||
if (!IsValidElementHeader22(line)) {
|
||||
throw mjCError(NULL, "Invalid elements header");
|
||||
}
|
||||
ss.seekg(-(line.size()+1), std::ios::cur);
|
||||
// read header
|
||||
size_t maxElementTag = 0;
|
||||
ss >> maxElementTag;
|
||||
if (!ss.good()) {
|
||||
throw mjCError(NULL, "Error reading Elements header");
|
||||
throw mjCError(NULL, "GetMaxElementTag::Error reading Elements header");
|
||||
}
|
||||
size_t numElements = maxElementTag;
|
||||
|
||||
@@ -1373,14 +1412,36 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "Invalid number of elements.");
|
||||
}
|
||||
|
||||
|
||||
// check number of elements is a positive number
|
||||
if (numElements < 0) {
|
||||
throw mjCError(NULL, "Invalid number of elements");
|
||||
}
|
||||
|
||||
// reading first element's type
|
||||
int tag = 0, type = 0, numTags = 0;
|
||||
ss >> tag >> type >> numTags;
|
||||
if (!ss.good()) {
|
||||
throw mjCError(NULL, "Error reading Elements");
|
||||
}
|
||||
int numNodeTags = type;
|
||||
if (numNodeTags < 1 || numNodeTags >4) {
|
||||
throw mjCError(NULL, "Invalid number of node tags");
|
||||
}
|
||||
|
||||
// setting entityDim
|
||||
size_t entityDim = numNodeTags - 1;
|
||||
def.spec.flex->dim = entityDim;
|
||||
|
||||
// read elements, discard all tags
|
||||
element.reserve((entityDim+1)*numElements);
|
||||
for (size_t i=0; i < numElements; i++) {
|
||||
int tag = 0, type = 0, numTags = 0, nodeTag = 0, physicalEntityTag = 0,
|
||||
elmentModelEntityTag = 0;
|
||||
ss >> tag >> type >> numTags;
|
||||
if (!ss.good()) {
|
||||
element.reserve(numNodeTags*numElements);
|
||||
for (size_t i=0; i<numElements; i++) {
|
||||
int nodeTag = 0, physicalEntityTag = 0, elmentModelEntityTag = 0;
|
||||
if (i != 0) {
|
||||
ss >> tag >> type >> numTags;
|
||||
if (!ss.good()) {
|
||||
throw mjCError(NULL, "Error reading Elements");
|
||||
}
|
||||
}
|
||||
if (numTags > 0) {
|
||||
ss >> physicalEntityTag >> elmentModelEntityTag;
|
||||
@@ -1388,7 +1449,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
throw mjCError(NULL, "Error reading Elements");
|
||||
}
|
||||
}
|
||||
for (int k=0; k <= entityDim; k++) {
|
||||
for (int k=0; k < numNodeTags; k++) {
|
||||
ss >> nodeTag;
|
||||
if (!ss.good()) {
|
||||
throw mjCError(NULL, "Error reading Elements");
|
||||
@@ -1419,23 +1480,34 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
||||
int numElements = maxElementTag;
|
||||
int tag, numTags;
|
||||
int nodeTag;
|
||||
int numNodeTags;
|
||||
|
||||
// check number of elements is a positive number
|
||||
if (numElements < 0) {
|
||||
throw mjCError(NULL, "Invalid number of elements");
|
||||
}
|
||||
|
||||
// size of single component in element data
|
||||
int componentSize = sizeof(int);
|
||||
// element buffer
|
||||
const char* elementsBuffer = buffer + elembegin + measuredHeaderSize;
|
||||
|
||||
ReadFromBuffer(&numNodeTags, elementsBuffer);
|
||||
ReadFromBuffer(&numTags, elementsBuffer + componentSize*2);
|
||||
ReadFromBuffer(&tag, elementsBuffer + componentSize*3);
|
||||
|
||||
if (numNodeTags < 1 || numNodeTags >4) {
|
||||
throw mjCError(NULL, "Invalid number of node tags");
|
||||
}
|
||||
size_t entityDim = numNodeTags - 1;
|
||||
def.spec.flex->dim = entityDim;
|
||||
|
||||
// element data(Ftetwild): tag and 4 nodeTag
|
||||
constexpr int numComponentsFtetwild = 5;
|
||||
// element data(gmshApp): 4 Info components, 2 entity tag and 4 nodeTags
|
||||
// element data(gmshApp): 4 Info components, 2 entity tag and entityDim+1 nodeTags
|
||||
constexpr int numInfoComponents = 4;
|
||||
constexpr int numEntityTagComponents = 2;
|
||||
constexpr int numNodeTags = 4;
|
||||
constexpr int numComponentsGmshApp = numInfoComponents +
|
||||
numEntityTagComponents + numNodeTags;
|
||||
|
||||
int numComponentsGmshApp = numInfoComponents + numEntityTagComponents + numNodeTags;
|
||||
|
||||
// single element data size
|
||||
int elementDataSizeFtetwild = numComponentsFtetwild*componentSize;
|
||||
@@ -1524,7 +1596,6 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
|
||||
char* buffer = 0;
|
||||
int buffer_sz = mju_readResource(resource, (const void**) &buffer);
|
||||
|
||||
|
||||
// check buffer
|
||||
if (buffer_sz < 0) {
|
||||
throw mjCError(NULL, "Could not read GMSH file");
|
||||
|
||||
@@ -1985,6 +1985,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
||||
m->light_active[lid] = (mjtByte)pl->active;
|
||||
copyvec(m->light_pos+3*lid, pl->pos, 3);
|
||||
copyvec(m->light_dir+3*lid, pl->dir, 3);
|
||||
m->light_radius[lid] = pl->radius;
|
||||
copyvec(m->light_attenuation+3*lid, pl->attenuation, 3);
|
||||
m->light_cutoff[lid] = pl->cutoff;
|
||||
m->light_exponent[lid] = pl->exponent;
|
||||
@@ -2123,6 +2124,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
|
||||
m->mesh_bvhnum[i] = pme->tree().nbvh;
|
||||
m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1;
|
||||
copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3);
|
||||
copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
|
||||
copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
|
||||
|
||||
|
||||
+66
-62
@@ -581,6 +581,14 @@ mjCBase& mjCBase::operator=(const mjCBase& other) {
|
||||
|
||||
|
||||
|
||||
void mjCBase::NameSpace(const mjCModel* m) {
|
||||
if (!name.empty()) {
|
||||
name = m->prefix + name + m->suffix;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// load resource if found (fallback to OS filesystem)
|
||||
mjResource* mjCBase::LoadResource(string filename, const mjVFS* vfs) {
|
||||
// try reading from provided VFS
|
||||
@@ -675,8 +683,6 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
|
||||
spec = other.spec;
|
||||
*static_cast<mjCBody_*>(this) = static_cast<const mjCBody_&>(other);
|
||||
*static_cast<mjsBody*>(this) = static_cast<const mjsBody&>(other);
|
||||
std::map<mjCFrame*, int> fmap;
|
||||
mjCFrame *np = nullptr;
|
||||
bodies.clear();
|
||||
frames.clear();
|
||||
geoms.clear();
|
||||
@@ -685,52 +691,8 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
|
||||
cameras.clear();
|
||||
lights.clear();
|
||||
|
||||
// create frames and map them to the old ones
|
||||
for (int i=0; i<other.frames.size(); i++) {
|
||||
frames.push_back(new mjCFrame(*other.frames[i]));
|
||||
frames.back()->model = model;
|
||||
frames.back()->body = this;
|
||||
fmap[other.frames[i]] = i;
|
||||
}
|
||||
|
||||
// copy all children
|
||||
for (int i=0; i<other.bodies.size(); i++) {
|
||||
bodies.push_back(new mjCBody(*other.bodies[i], model)); // triggers recursive call
|
||||
bodies.back()->frame = other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.frames.size(); i++) {
|
||||
frames[i]->frame = other.frames[i]->frame ? frames[fmap[other.frames[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.geoms.size(); i++) {
|
||||
geoms.push_back(new mjCGeom(*other.geoms[i]));
|
||||
geoms.back()->body = this;
|
||||
geoms.back()->model = model;
|
||||
geoms.back()->frame = other.geoms[i]->frame ? frames[fmap[other.geoms[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.joints.size(); i++) {
|
||||
joints.push_back(new mjCJoint(*other.joints[i]));
|
||||
joints.back()->body = this;
|
||||
joints.back()->model = model;
|
||||
joints.back()->frame = other.joints[i]->frame ? frames[fmap[other.joints[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.sites.size(); i++) {
|
||||
sites.push_back(new mjCSite(*other.sites[i]));
|
||||
sites.back()->body = this;
|
||||
sites.back()->model = model;
|
||||
sites.back()->frame = other.sites[i]->frame ? frames[fmap[other.sites[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.cameras.size(); i++) {
|
||||
cameras.push_back(new mjCCamera(*other.cameras[i]));
|
||||
cameras.back()->body = this;
|
||||
cameras.back()->model = model;
|
||||
cameras.back()->frame = other.cameras[i]->frame ? frames[fmap[other.cameras[i]->frame]] : np;
|
||||
}
|
||||
for (int i=0; i<other.lights.size(); i++) {
|
||||
lights.push_back(new mjCLight(*other.lights[i]));
|
||||
lights.back()->body = this;
|
||||
lights.back()->model = model;
|
||||
lights.back()->frame = other.lights[i]->frame ? frames[fmap[other.lights[i]->frame]] : np;
|
||||
}
|
||||
// add elements to lists
|
||||
*this += other;
|
||||
}
|
||||
PointToLocal();
|
||||
return *this;
|
||||
@@ -738,13 +700,60 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
|
||||
|
||||
|
||||
|
||||
mjCBody& mjCBody::operator+=(mjCBody& other) {
|
||||
bodies.push_back(&other);
|
||||
// copy children of other body into body
|
||||
mjCBody& mjCBody::operator+=(const mjCBody& other) {
|
||||
// map other frames to indices
|
||||
std::map<mjCFrame*, int> fmap;
|
||||
for (int i=0; i<other.frames.size(); i++) {
|
||||
fmap[other.frames[i]] = i;
|
||||
}
|
||||
|
||||
// copy frames, needs to happen first
|
||||
CopyList(frames, other.frames, fmap);
|
||||
|
||||
// copy all children
|
||||
CopyList(geoms, other.geoms, fmap);
|
||||
CopyList(joints, other.joints, fmap);
|
||||
CopyList(sites, other.sites, fmap);
|
||||
CopyList(cameras, other.cameras, fmap);
|
||||
CopyList(lights, other.lights, fmap);
|
||||
|
||||
for (int i=0; i<other.bodies.size(); i++) {
|
||||
bodies.push_back(new mjCBody(*other.bodies[i], model)); // triggers recursive call
|
||||
bodies.back()->frame =
|
||||
other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : nullptr;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// copy src list of elements into dst; set body, model and frame
|
||||
template <typename T>
|
||||
void mjCBody::CopyList(std::vector<T*>& dst, const std::vector<T*>& src,
|
||||
std::map<mjCFrame*, int>& fmap, const mjCFrame* pframe) {
|
||||
int nsrc = (int)src.size();
|
||||
for (int i=0; i<nsrc; i++) {
|
||||
if (pframe && src[i]->frame != pframe) {
|
||||
continue; // skip if the element is not inside pframe
|
||||
}
|
||||
dst.push_back(new T(*src[i]));
|
||||
dst.back()->body = this;
|
||||
dst.back()->model = model;
|
||||
|
||||
// assign dst frame to src frame
|
||||
dst.back()->frame = src[i]->frame ? frames[fmap[src[i]->frame]] : nullptr;
|
||||
|
||||
// set namespace from pframe if given
|
||||
if (pframe) {
|
||||
dst.back()->NameSpace(pframe->model);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void mjCBody::PointToLocal() {
|
||||
spec.element = static_cast<mjElement*>(this);
|
||||
spec.name = (mjString)&name;
|
||||
@@ -797,21 +806,15 @@ void mjCBody::NameSpace(const mjCModel* m) {
|
||||
}
|
||||
|
||||
for (auto& joint : joints) {
|
||||
if (!joint->name.empty()) {
|
||||
joint->name = prefix + joint->name + suffix;
|
||||
}
|
||||
joint->NameSpace(m);
|
||||
}
|
||||
|
||||
for (auto& geom : geoms) {
|
||||
if (!geom->name.empty()) {
|
||||
geom->name = prefix + geom->name + suffix;
|
||||
}
|
||||
geom->NameSpace(m);
|
||||
}
|
||||
|
||||
for (auto& site : sites) {
|
||||
if (!site->name.empty()) {
|
||||
site->name = prefix + site->name + suffix;
|
||||
}
|
||||
site->NameSpace(m);
|
||||
}
|
||||
|
||||
for (auto& camera : cameras) {
|
||||
@@ -1391,6 +1394,7 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
|
||||
}
|
||||
|
||||
|
||||
|
||||
// attach body to frame
|
||||
mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
|
||||
mjCBody* subtree = new mjCBody(other, model);
|
||||
@@ -1399,10 +1403,10 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
|
||||
subtree->SetFrame(this);
|
||||
subtree->NameSpace(other.model);
|
||||
|
||||
// add to tree
|
||||
*body += *subtree;
|
||||
// add to body children
|
||||
body->bodies.push_back(subtree);
|
||||
|
||||
// TODO: needs to attach only referencing elements
|
||||
// attach referencing elements
|
||||
*model += *other.model;
|
||||
|
||||
// clear suffixes and return
|
||||
|
||||
+12
-7
@@ -198,7 +198,7 @@ class mjCBase : public mjCBase_ {
|
||||
virtual void ResolveReferences(const mjCModel* m) {}
|
||||
|
||||
// Appends prefix and suffix to reference
|
||||
virtual void NameSpace(const mjCModel* m) {}
|
||||
virtual void NameSpace(const mjCModel* m);
|
||||
|
||||
// Copy assignment
|
||||
mjCBase& operator=(const mjCBase& other);
|
||||
@@ -274,7 +274,7 @@ class mjCBody : public mjCBody_, private mjsBody {
|
||||
mjCLight* AddLight(mjCDef* = 0);
|
||||
|
||||
// API for adding existing objects to body
|
||||
mjCBody& operator+=(mjCBody& other);
|
||||
mjCBody& operator+=(const mjCBody& other);
|
||||
|
||||
// API for accessing objects
|
||||
int NumObjects(mjtObj type);
|
||||
@@ -322,6 +322,11 @@ class mjCBody : public mjCBody_, private mjsBody {
|
||||
|
||||
void CopyFromSpec(); // copy spec into attributes
|
||||
void PointToLocal(void);
|
||||
|
||||
// copy src list of elements into dst; set body, model and frame
|
||||
template <typename T>
|
||||
void CopyList(std::vector<T*>& dst, const std::vector<T*>& src,
|
||||
std::map<mjCFrame*, int>& fmap, const mjCFrame* pframe = nullptr);
|
||||
};
|
||||
|
||||
|
||||
@@ -815,12 +820,12 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
|
||||
const mjCBoundingVolumeHierarchy& tree() { return tree_; }
|
||||
|
||||
void Compile(const mjVFS* vfs); // compiler
|
||||
double* GetPosPtr(mjtGeomInertia type); // get position
|
||||
double* GetQuatPtr(mjtGeomInertia type); // get orientation
|
||||
double* GetPosPtr(mjtGeomInertia type); // get position
|
||||
double* GetQuatPtr(mjtGeomInertia type); // get orientation
|
||||
double* GetOffsetPosPtr(); // get position offset for geom
|
||||
double* GetOffsetQuatPtr(); // get orientation offset for geom
|
||||
double* GetInertiaBoxPtr(mjtGeomInertia type); // get inertia box
|
||||
double& GetVolumeRef(mjtGeomInertia type); // get volume
|
||||
double* GetInertiaBoxPtr(mjtGeomInertia type); // get inertia box
|
||||
double& GetVolumeRef(mjtGeomInertia type); // get volume
|
||||
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
|
||||
bool HasTexcoord() const; // texcoord not null
|
||||
void DelTexcoord(); // delete texcoord
|
||||
@@ -852,7 +857,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
|
||||
void ApplyTransformations(); // apply user transformations
|
||||
void ComputeFaceCentroid(double[3]); // compute centroid of all faces
|
||||
void RemoveRepeated(void); // remove repeated vertices
|
||||
void CheckMesh(mjtGeomInertia type); // check if the mesh is valid
|
||||
void CheckMesh(mjtGeomInertia type); // check if the mesh is valid
|
||||
|
||||
// mesh data to be copied into mjModel
|
||||
float* vert_; // vertex data (3*nvert), relative to (pos, quat)
|
||||
|
||||
@@ -153,7 +153,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
{"camera", "?", "16", "fovy", "ipd", "resolution", "pos", "quat", "axisangle", "xyaxes",
|
||||
"zaxis", "euler", "mode", "focal", "focalpixel", "principal", "principalpixel",
|
||||
"sensorsize", "user"},
|
||||
{"light", "?", "12", "pos", "dir", "directional", "castshadow", "active",
|
||||
{"light", "?", "13", "pos", "dir", "radius", "directional", "castshadow", "active",
|
||||
"attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"},
|
||||
{"pair", "?", "7", "condim", "friction", "solref", "solreffriction", "solimp",
|
||||
"gap", "margin"},
|
||||
@@ -263,9 +263,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
||||
{"camera", "*", "19", "name", "class", "fovy", "ipd", "resolution", "pos", "quat",
|
||||
"axisangle", "xyaxes", "zaxis", "euler", "mode", "target", "focal", "focalpixel",
|
||||
"principal", "principalpixel", "sensorsize", "user"},
|
||||
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
|
||||
"pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular",
|
||||
"mode", "target"},
|
||||
{"light", "*", "16", "name", "class", "directional", "castshadow", "active",
|
||||
"pos", "dir", "radius", "attenuation", "cutoff", "exponent", "ambient", "diffuse",
|
||||
"specular", "mode", "target"},
|
||||
{"plugin", "*", "2", "plugin", "instance"},
|
||||
{"<"},
|
||||
{"config", "*", "2", "key", "value"},
|
||||
@@ -1783,6 +1783,7 @@ void mjXReader::OneLight(XMLElement* elem, mjsLight* plight) {
|
||||
}
|
||||
ReadAttr(elem, "pos", 3, plight->pos, text);
|
||||
ReadAttr(elem, "dir", 3, plight->dir, text);
|
||||
ReadAttr(elem, "radius", 1, &plight->radius, text);
|
||||
ReadAttr(elem, "attenuation", 3, plight->attenuation, text);
|
||||
ReadAttr(elem, "cutoff", 1, &plight->cutoff, text);
|
||||
ReadAttr(elem, "exponent", 1, &plight->exponent, text);
|
||||
|
||||
@@ -508,6 +508,7 @@ void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) {
|
||||
}
|
||||
|
||||
// defaults and regular
|
||||
WriteAttr(elem, "radius", 1, &plight->radius, &def->light.radius);
|
||||
WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->light.directional);
|
||||
WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->light.castshadow);
|
||||
WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->light.active);
|
||||
@@ -1172,6 +1173,11 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
|
||||
OneCamera(elem, &def->camera, par);
|
||||
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
||||
|
||||
// light
|
||||
elem = InsertEnd(section, "light");
|
||||
OneLight(elem, &def->light, par);
|
||||
if (!elem->FirstAttribute()) section->DeleteChild(elem);
|
||||
|
||||
// pair
|
||||
elem = InsertEnd(section, "pair");
|
||||
OnePair(elem, &def->pair, par);
|
||||
|
||||
@@ -383,7 +383,9 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
|
||||
"user/testdata/malformed_shark_22_ascii_missing_num_elements.xml");
|
||||
std::array<char, 1024> error;
|
||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||
EXPECT_THAT(error.data(), HasSubstr("XML Error: Error: Invalid node tag"));
|
||||
// TODO(mohammadhamid): Replace with an assertion about the error message. For
|
||||
// some reason, on Windows the error message is different on GH Actions
|
||||
EXPECT_THAT(m, IsNull());
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
|
||||
@@ -58,6 +58,11 @@ static const char* const kDuplicateOBJPath =
|
||||
static const char* const kMalformedFaceOBJPath =
|
||||
"user/testdata/malformed_face.xml";
|
||||
|
||||
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
|
||||
return std::vector<mjtNum>(array, array + n);
|
||||
}
|
||||
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::HasSubstr;
|
||||
using ::testing::IsNull;
|
||||
using ::testing::NotNull;
|
||||
@@ -863,6 +868,27 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(MjCMeshTest, MeshScale) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<asset>
|
||||
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
|
||||
<mesh name="pyramid_scaled" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="0.9 1 -1"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<geom type="mesh" name="geom1" mesh="pyramid"/>
|
||||
<geom type="mesh" name="geom2" mesh="pyramid_scaled"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
ASSERT_THAT(model, NotNull());
|
||||
|
||||
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
|
||||
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// ----------------------------- texcoord -------------------------------------
|
||||
|
||||
TEST_F(MjCMeshTest, CreateFaceTexCoord) {
|
||||
|
||||
@@ -1868,5 +1868,29 @@ TEST_F(XMLReaderTest, ExtentNegativeNotAllowed) {
|
||||
EXPECT_THAT(error.data(), HasSubstr("line 3"));
|
||||
}
|
||||
|
||||
TEST_F(XMLReaderTest, LightRadius) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<default>
|
||||
<default class="r1">
|
||||
<light radius="1"/>
|
||||
</default>
|
||||
</default>
|
||||
<worldbody>
|
||||
<light/>
|
||||
<light class="r1"/>
|
||||
<light class="r1" radius="2"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model, NotNull()) << error.data();
|
||||
EXPECT_EQ(model->light_radius[0], 0);
|
||||
EXPECT_EQ(model->light_radius[1], 1);
|
||||
EXPECT_EQ(model->light_radius[2], 2);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -935,6 +935,35 @@ TEST_F(XMLWriterTest, WritesHfield) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(XMLWriterTest, WritesLight) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<default>
|
||||
<default class="r1">
|
||||
<light radius="1"/>
|
||||
</default>
|
||||
</default>
|
||||
<worldbody>
|
||||
<light/>
|
||||
<light class="r1"/>
|
||||
<light class="r1" radius="2"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
ASSERT_THAT(model, NotNull());
|
||||
|
||||
// save and read, compare data
|
||||
mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model));
|
||||
EXPECT_EQ(mtemp->nlight, 3);
|
||||
EXPECT_EQ(mtemp->light_radius[0], 0);
|
||||
EXPECT_EQ(mtemp->light_radius[1], 1);
|
||||
EXPECT_EQ(mtemp->light_radius[2], 2);
|
||||
|
||||
mj_deleteModel(mtemp);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(XMLWriterTest, SpringlengthOneValue) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
@@ -5297,6 +5297,7 @@ public unsafe struct mjModel_ {
|
||||
public byte* light_active;
|
||||
public double* light_pos;
|
||||
public double* light_dir;
|
||||
public double* light_radius;
|
||||
public double* light_poscom0;
|
||||
public double* light_pos0;
|
||||
public double* light_dir0;
|
||||
@@ -5374,6 +5375,7 @@ public unsafe struct mjModel_ {
|
||||
public int* mesh_facenormal;
|
||||
public int* mesh_facetexcoord;
|
||||
public int* mesh_graph;
|
||||
public double* mesh_scale;
|
||||
public double* mesh_pos;
|
||||
public double* mesh_quat;
|
||||
public int* mesh_pathadr;
|
||||
|
||||
Reference in New Issue
Block a user