diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 08af2d39..76e861e6 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -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: diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 05b67169..90e8f04a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 5b516790..c374cfa0 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -369,11 +369,11 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`directional` | :ref:`castshadow` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`active` | :ref:`pos` | :ref:`dir` | :ref:`attenuation` | | +| | | | :ref:`active` | :ref:`pos` | :ref:`dir` | :ref:`radius` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cutoff` | :ref:`exponent` | :ref:`ambient` | :ref:`diffuse` | | +| | | | :ref:`attenuation` | :ref:`cutoff` | :ref:`exponent` | :ref:`ambient` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`specular` | :ref:`mode` | :ref:`target` | | | +| | | | :ref:`diffuse` | :ref:`specular` | :ref:`mode` | :ref:`target` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | @@ -1378,11 +1378,13 @@ | :ref:`light | ? | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`pos` | :ref:`dir` | :ref:`directional` | :ref:`castshadow` | | +| | | | :ref:`pos` | :ref:`dir` | :ref:`radius` | :ref:`directional` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`active` | :ref:`attenuation` | :ref:`cutoff` | :ref:`exponent` | | +| | | | :ref:`castshadow` | :ref:`active` | :ref:`attenuation` | :ref:`cutoff` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ambient` | :ref:`diffuse` | :ref:`specular` | :ref:`mode` | | +| | | | :ref:`exponent` | :ref:`ambient` | :ref:`diffuse` | :ref:`specular` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`mode` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index b50d64b1..5b9a296f 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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` attribute. +2. Added the :ref:`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 `__, 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 `__. +3. Added support for gmsh format 2.2 , tetrahedral mesh, as generated by e.g. `fTetwild `__. 4. Added :ref:`mju_euler2Quat` for converting an Euler-angle sequence to quaternion. diff --git a/doc/includes/references.h b/doc/includes/references.h index 613dd037..e0a77d94 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 2f0031e2..37e2a9f4 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 7cb4fa7b..6ad3093b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index 980a9f0f..112cb730 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -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', diff --git a/mjx/mujoco/mjx/_src/collision_convex.py b/mjx/mujoco/mjx/_src/collision_convex.py index 1e501097..726ecade 100644 --- a/mjx/mujoco/mjx/_src/collision_convex.py +++ b/mjx/mujoco/mjx/_src/collision_convex.py @@ -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 diff --git a/mjx/mujoco/mjx/_src/collision_driver.py b/mjx/mujoco/mjx/_src/collision_driver.py index f4da34ef..7fd905b4 100644 --- a/mjx/mujoco/mjx/_src/collision_driver.py +++ b/mjx/mujoco/mjx/_src/collision_driver.py @@ -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, diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index d070ed7b..d501be5d 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -465,6 +465,50 @@ class CapsuleCollisionTest(parameterized.TestCase): ) +class CylinderTest(absltest.TestCase): + """Tests the cylinder contact functions.""" + + _CYLINDER_PLANE = """ + + + + + + + + + + """ + + 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( + ' 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 diff --git a/mjx/mujoco/mjx/_src/collision_sdf.py b/mjx/mujoco/mjx/_src/collision_sdf.py index 914a8e5b..999b3e8a 100644 --- a/mjx/mujoco/mjx/_src/collision_sdf.py +++ b/mjx/mujoco/mjx/_src/collision_sdf.py @@ -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] diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 315884ac..0a6f1df3 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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) \ diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index e14bf591..8fee7ebf 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -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); diff --git a/src/user/user_api.h b/src/user/user_api.h index c12a4e4c..dd03c959 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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 diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 3cc78c3a..a01f9a42 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -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> 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"); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index dc106b9d..0a539376 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 5c361a96..17421d9a 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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(this) = static_cast(other); *static_cast(this) = static_cast(other); - std::map 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; imodel = model; - frames.back()->body = this; - fmap[other.frames[i]] = i; - } - - // copy all children - for (int i=0; iframe = other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : np; - } - for (int i=0; iframe = other.frames[i]->frame ? frames[fmap[other.frames[i]->frame]] : np; - } - for (int i=0; ibody = this; - geoms.back()->model = model; - geoms.back()->frame = other.geoms[i]->frame ? frames[fmap[other.geoms[i]->frame]] : np; - } - for (int i=0; ibody = this; - joints.back()->model = model; - joints.back()->frame = other.joints[i]->frame ? frames[fmap[other.joints[i]->frame]] : np; - } - for (int i=0; ibody = this; - sites.back()->model = model; - sites.back()->frame = other.sites[i]->frame ? frames[fmap[other.sites[i]->frame]] : np; - } - for (int i=0; ibody = this; - cameras.back()->model = model; - cameras.back()->frame = other.cameras[i]->frame ? frames[fmap[other.cameras[i]->frame]] : np; - } - for (int i=0; ibody = 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 fmap; + for (int i=0; iframe = + 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 +void mjCBody::CopyList(std::vector& dst, const std::vector& src, + std::map& fmap, const mjCFrame* pframe) { + int nsrc = (int)src.size(); + for (int i=0; iframe != 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(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 diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1b9f0f2d..db4ea58f 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 + void CopyList(std::vector& dst, const std::vector& src, + std::map& 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) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 55e02d03..7427f019 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index c70ee22b..a2fa4ec6 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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); diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 01b6534c..bf2198c0 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -383,7 +383,9 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) { "user/testdata/malformed_shark_22_ascii_missing_num_elements.xml"); std::array 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); } diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 0fc57317..56898655 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -58,6 +58,11 @@ static const char* const kDuplicateOBJPath = static const char* const kMalformedFaceOBJPath = "user/testdata/malformed_face.xml"; +std::vector AsVector(const mjtNum* array, int n) { + return std::vector(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"( + + + + + + + + + + + )"; + 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) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 2e854d89..61253f35 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1868,5 +1868,29 @@ TEST_F(XMLReaderTest, ExtentNegativeNotAllowed) { EXPECT_THAT(error.data(), HasSubstr("line 3")); } +TEST_F(XMLReaderTest, LightRadius) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array 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 diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index cf48889b..0b5f1dec 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -935,6 +935,35 @@ TEST_F(XMLWriterTest, WritesHfield) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, WritesLight) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + 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"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 993f35f1..4b8d6f84 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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;