From 6147c2fc6e331aef417f5b446e488e59c5520dc2 Mon Sep 17 00:00:00 2001 From: stonfute Date: Wed, 5 Jun 2024 15:56:23 +0200 Subject: [PATCH 001/426] Fix MjHingeJoint.cs range with MakeLocaleInvariant --- unity/Runtime/Components/Joints/MjHingeJoint.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unity/Runtime/Components/Joints/MjHingeJoint.cs b/unity/Runtime/Components/Joints/MjHingeJoint.cs index 8549c738..8d84d649 100644 --- a/unity/Runtime/Components/Joints/MjHingeJoint.cs +++ b/unity/Runtime/Components/Joints/MjHingeJoint.cs @@ -83,7 +83,7 @@ namespace Mujoco { if (RangeLower > RangeUpper) { throw new ArgumentException("Lower range value can't be bigger than Higher"); } - mjcf.SetAttribute("range", $"{RangeLower} {RangeUpper}"); + mjcf.SetAttribute("range", MjEngineTool.MakeLocaleInvariant($"{RangeLower} {RangeUpper}")); mjcf.SetAttribute("ref", $"{Configuration}"); return mjcf; From 9504a9718279bfd9c2c7292c0a3fb426378d5a54 Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 20 Oct 2024 23:03:21 -0400 Subject: [PATCH 002/426] add citation and fix math symbol text color --- mjx/training_apg.ipynb | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/mjx/training_apg.ipynb b/mjx/training_apg.ipynb index 4a7192dc..fa9bc8a5 100644 --- a/mjx/training_apg.ipynb +++ b/mjx/training_apg.ipynb @@ -66,10 +66,10 @@ "$$\n", "\n", "$$\n", - "\\frac{\\partial x_t}{\\partial \\theta} = \\textcolor{Navy}{\\frac{\\partial f(x_{t-1}, a_{t-1})}{\\partial x_{t-1}}}\\frac{\\partial x_{t-1}}{\\partial \\theta} + \\textcolor{Navy}{\\frac{\\partial f(x_{t-1}, a_{t-1})}{\\partial a_{t-1}}} \\frac{\\partial a_{t-1}}{\\partial \\theta}\n", + "\\frac{\\partial x_t}{\\partial \\theta} = \\color{blue}{\\frac{\\partial f(x_{t-1}, a_{t-1})}{\\partial x_{t-1}}}\\frac{\\partial x_{t-1}}{\\partial \\theta} + \\color{blue}{\\frac{\\partial f(x_{t-1}, a_{t-1})}{\\partial a_{t-1}}} \\frac{\\partial a_{t-1}}{\\partial \\theta}\n", "$$\n", "\n", - "The navy-colored terms in the above expression are enabled by MJX's differentiability and are the key difference between FoPG's and ZoPG's. An important consideration is what these jacobians look like near contact points. To see why certain gradients within the jacobian can be pathological, imagine a hard sphere falling toward a block of marble. How does its velocity change with respect to distance ($\\frac{\\partial \\dot{z}_t}{\\partial z_t}$), the instant before it touches the ground? This is the case of an **uninformative gradient**, due to [hard contact](https://arxiv.org/html/2404.02887v1). Fortunately, the default contact settings in Mujoco are sufficiently [soft](https://mujoco.readthedocs.io/en/stable/computation/index.html#soft-contact-model) for learning via FoPG's. With soft contacts, the ground applies an increasing force on the ball as it penetrates it, unlike rigid contacts, which instantly provide enough force for deflection.\n", + "The blue-colored terms in the above expression are enabled by MJX's differentiability and are the key difference between FoPG's and ZoPG's. An important consideration is what these jacobians look like near contact points. To see why certain gradients within the jacobian can be pathological, imagine a hard sphere falling toward a block of marble. How does its velocity change with respect to distance ($\\frac{\\partial \\dot{z}_t}{\\partial z_t}$), the instant before it touches the ground? This is the case of an **uninformative gradient**, due to [hard contact](https://arxiv.org/html/2404.02887v1). Fortunately, the default contact settings in Mujoco are sufficiently [soft](https://mujoco.readthedocs.io/en/stable/computation/index.html#soft-contact-model) for learning via FoPG's. With soft contacts, the ground applies an increasing force on the ball as it penetrates it, unlike rigid contacts, which instantly provide enough force for deflection.\n", "\n", "A helpful way to think about FoPG's is via the chain rule and computation graphs, as illustrated below for how $r_2$ influences the policy gradient, again for the case that the reward does not depend on action:\n", "\n", @@ -85,7 +85,29 @@ "\n", "Last, despite the sample efficiency, FoPG methods can still struggle with wall-clock time. Because the gradients have low variance, they do not benefit significantly from massive parallelization of data collection - unlike [RL](https://arxiv.org/abs/2109.11978). Additionally, the policy gradient is typically calculated via autodifferentiation. This can be 3-5x slower than unrolling the simulation forward, and memory intensive, with memory requirements scaling with $O(m \\cdot (m+n) \\cdot T)$, where m and n are the state and control dimensions, $m \\cdot (m+n)$ is the jacobian dimension, and T is the number of steps propogated through.\n", "\n", - "Note that with certain models, using autodifferentiation through mjx.step currently causes [nan gradients](https://github.com/google-deepmind/mujoco/issues/1517). For now, we address this issue by using double-precision floats, at the cost of doubling the memory requirements and training time." + "Note that with certain models, using autodifferentiation through mjx.step currently causes [nan gradients](https://github.com/google-deepmind/mujoco/issues/1517). For now, we address this issue by using double-precision floats, at the cost of doubling the memory requirements and training time.\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "---\n", + "**Publications**\n", + "\n", + "If you use this work in an academic context, please cite the following publication:\n", + "\n", + "```\n", + "@misc{luo2024residual,\n", + " title={Residual Policy Learning for Perceptive Quadruped Control Using Differentiable Simulation},\n", + " author={Luo, Jing Yuan and Song, Yunlong and Klemm, Victor and Shi, Fan and Scaramuzza, Davide and Hutter, Marco},\n", + " year={2024},\n", + " eprint={2410.03076},\n", + " archivePrefix={arXiv},\n", + " primaryClass={cs.RO},\n", + " url={https://doi.org/10.48550/arXiv.2410.03076}\n", + "}\n", + "```" ] }, { From 7620aef530d41eb5553f364a84793212004e78a3 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 23 Oct 2024 12:16:39 -0700 Subject: [PATCH 003/426] Fix typo in the changelog. PiperOrigin-RevId: 689058361 Change-Id: I479efa8fc0b8d31030d3f6444b874a14419f3518 --- doc/changelog.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index d3ec0db6..4cb748dd 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -17,7 +17,7 @@ MJX Bug fixes ^^^^^^^^^ -- Fixed a bug in slider-crank ref:`transmission`. The bug was introduced in 3.0.0. +- Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. Version 3.2.4 (Oct 15, 2024) ---------------------------- From ca348632a9b04ae12d488be811e916f7d9a7cacc Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 24 Oct 2024 02:40:56 -0700 Subject: [PATCH 004/426] Change tolerances for ellipsoid-capsule and ellipsoid-sphere collision tests in MJX. PiperOrigin-RevId: 689301610 Change-Id: I86be65524d13a19abd0b378b0933a5fbbe0ecab8 --- mjx/mujoco/mjx/_src/collision_driver_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index 611703f1..d5c893d0 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -263,7 +263,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-4) + dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3) _ELLIPSOID_CAPSULE = """ @@ -285,7 +285,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-4) + dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3) _ELLIPSOID_CYLINDER = """ From 1043633cc366a4a563ab74b0bf6c07a306a47ca7 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 24 Oct 2024 05:36:15 -0700 Subject: [PATCH 005/426] Remove arbitrary limit of a 1000 elements from certain attributes. Fixes #2166. PiperOrigin-RevId: 689343609 Change-Id: I7cbf5007884433807c76c6c309cb4a858479d34d --- src/xml/xml_native_reader.cc | 38 +++++++++++++++++------------------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 17fe2609..f145afbd 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -4258,8 +4258,6 @@ void mjXReader::Sensor(XMLElement* section) { // keyframe section parser void mjXReader::Keyframe(XMLElement* section) { XMLElement* elem; - int n; - double data[1000]; // iterate over child elements elem = FirstChildElement(section); @@ -4275,39 +4273,39 @@ void mjXReader::Keyframe(XMLElement* section) { ReadAttr(elem, "time", 1, &key->time, text); // read qpos - n = ReadAttr(elem, "qpos", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->qpos, data, n); + auto maybe_data = ReadAttrVec(elem, "qpos", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->qpos, maybe_data->data(), maybe_data->size()); } // read qvel - n = ReadAttr(elem, "qvel", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->qvel, data, n); + maybe_data = ReadAttrVec(elem, "qvel", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->qvel, maybe_data->data(), maybe_data->size()); } // read act - n = ReadAttr(elem, "act", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->act, data, n); + maybe_data = ReadAttrVec(elem, "act", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->act, maybe_data->data(), maybe_data->size()); } // read mpos - n = ReadAttr(elem, "mpos", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->mpos, data, n); + maybe_data = ReadAttrVec(elem, "mpos", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->mpos, maybe_data->data(), maybe_data->size()); } // read mquat - n = ReadAttr(elem, "mquat", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->mquat, data, n); + maybe_data = ReadAttrVec(elem, "mquat", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->mquat, maybe_data->data(), maybe_data->size()); } // read ctrl - n = ReadAttr(elem, "ctrl", 1000, data, text, false, false); - if (n) { - mjs_setDouble(key->ctrl, data, n); + maybe_data = ReadAttrVec(elem, "ctrl", false); + if (maybe_data.has_value()) { + mjs_setDouble(key->ctrl, maybe_data->data(), maybe_data->size()); } // advance to next element From 159f23e6b40d3554022bce24ed97283abe373531 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 24 Oct 2024 08:29:03 -0700 Subject: [PATCH 006/426] Remember compiler options during attach. Note: - Move compiler options into `mjsCompiler` struct. - The compiler options from the attached model are not written by mj_SaveXML. PiperOrigin-RevId: 689391440 Change-Id: I1d63c146a32f87c737b7a55b64a54b0ffe3aecc9 --- doc/APIreference/APItypes.rst | 10 +++ doc/includes/references.h | 19 +++-- include/mujoco/mjspec.h | 21 +++-- introspect/structs.py | 84 ++++++++++-------- .../mujoco/codegen/generate_spec_bindings.py | 8 +- python/mujoco/raw.h | 1 + python/mujoco/specs.cc | 1 + python/mujoco/specs_test.py | 40 ++++----- src/user/user_api.cc | 11 ++- src/user/user_flexcomp.cc | 7 +- src/user/user_flexcomp.h | 2 +- src/user/user_init.c | 22 ++--- src/user/user_mesh.cc | 4 +- src/user/user_model.cc | 46 ++++------ src/user/user_objects.cc | 85 +++++++++++++------ src/user/user_objects.h | 3 +- src/xml/xml.cc | 4 +- src/xml/xml_native_reader.cc | 38 ++++----- src/xml/xml_native_writer.cc | 20 ++--- src/xml/xml_urdf.cc | 4 +- test/user/user_api_test.cc | 48 ++++++++--- test/user/user_model_test.cc | 4 +- test/user/user_objects_test.cc | 4 +- unity/Runtime/Bindings/MjBindings.cs | 20 +++++ 24 files changed, 302 insertions(+), 204 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index d9ca5fd3..fba959b2 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1140,6 +1140,16 @@ behavior. .. mujoco-include:: mjsElement +.. _mjsCompiler: + +mjsCompiler +~~~~~~~~~~ + +Compiler options. + +.. mujoco-include:: mjsCompiler + + .. _mjsBody: mjsBody diff --git a/doc/includes/references.h b/doc/includes/references.h index 9f348881..f1cd64a7 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1683,22 +1683,15 @@ typedef enum mjtOrientation_ { // type of orientation specifier typedef struct mjsElement_ { // element type, do not modify mjtObj elemtype; // element type } mjsElement; -typedef struct mjSpec_ { // model specification - mjsElement* element; // element type - mjString* modelname; // model name - - // compiler settings +typedef struct mjsCompiler_ { // compiler options mjtByte autolimits; // infer "limited" attribute based on range double boundmass; // enforce minimum body mass double boundinertia; // enforce minimum body diagonal inertia double settotalmass; // rescale masses and inertias; <=0: ignore mjtByte balanceinertia; // automatically impose A + B >= C rule - mjtByte strippath; // automatically strip paths from mesh files mjtByte fitaabb; // meshfit to aabb instead of inertia box mjtByte degree; // angles in radians or degrees char eulerseq[3]; // sequence for euler rotations - mjString* meshdir; // mesh and hfield directory - mjString* texturedir; // texture directory mjtByte discardvisual; // discard visual geoms in parser mjtByte convexhull; // compute mesh convex hulls mjtByte usethread; // use multiple threads to speed up compiler @@ -1707,6 +1700,16 @@ typedef struct mjSpec_ { // model specification int inertiagrouprange[2]; // range of geom groups used to compute inertia int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation +} mjsCompiler; +typedef struct mjSpec_ { // model specification + mjsElement* element; // element type + mjString* modelname; // model name + + // compiler data + mjsCompiler compiler; // compiler options + mjtByte strippath; // automatically strip paths from mesh files + mjString* meshdir; // mesh and hfield directory + mjString* texturedir; // texture directory // engine data mjOption option; // physics options diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index c6d38c79..58feb8f8 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -121,22 +121,15 @@ typedef struct mjsElement_ { // element type, do not modify } mjsElement; -typedef struct mjSpec_ { // model specification - mjsElement* element; // element type - mjString* modelname; // model name - - // compiler settings +typedef struct mjsCompiler_ { // compiler options mjtByte autolimits; // infer "limited" attribute based on range double boundmass; // enforce minimum body mass double boundinertia; // enforce minimum body diagonal inertia double settotalmass; // rescale masses and inertias; <=0: ignore mjtByte balanceinertia; // automatically impose A + B >= C rule - mjtByte strippath; // automatically strip paths from mesh files mjtByte fitaabb; // meshfit to aabb instead of inertia box mjtByte degree; // angles in radians or degrees char eulerseq[3]; // sequence for euler rotations - mjString* meshdir; // mesh and hfield directory - mjString* texturedir; // texture directory mjtByte discardvisual; // discard visual geoms in parser mjtByte convexhull; // compute mesh convex hulls mjtByte usethread; // use multiple threads to speed up compiler @@ -145,6 +138,18 @@ typedef struct mjSpec_ { // model specification int inertiagrouprange[2]; // range of geom groups used to compute inertia int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation +} mjsCompiler; + + +typedef struct mjSpec_ { // model specification + mjsElement* element; // element type + mjString* modelname; // model name + + // compiler data + mjsCompiler compiler; // compiler options + mjtByte strippath; // automatically strip paths from mesh files + mjString* meshdir; // mesh and hfield directory + mjString* texturedir; // texture directory // engine data mjOption option; // physics options diff --git a/introspect/structs.py b/introspect/structs.py index 449ac230..9f8c8b4c 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -8800,25 +8800,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), ), )), - ('mjSpec', + ('mjsCompiler', StructDecl( - name='mjSpec', - declname='struct mjSpec_', + name='mjsCompiler', + declname='struct mjsCompiler_', fields=( - StructFieldDecl( - name='element', - type=PointerType( - inner_type=ValueType(name='mjsElement'), - ), - doc='element type', - ), - StructFieldDecl( - name='modelname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='model name', - ), StructFieldDecl( name='autolimits', type=ValueType(name='mjtByte'), @@ -8844,11 +8830,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtByte'), doc='automatically impose A + B >= C rule', ), - StructFieldDecl( - name='strippath', - type=ValueType(name='mjtByte'), - doc='automatically strip paths from mesh files', - ), StructFieldDecl( name='fitaabb', type=ValueType(name='mjtByte'), @@ -8867,20 +8848,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='sequence for euler rotations', ), - StructFieldDecl( - name='meshdir', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='mesh and hfield directory', - ), - StructFieldDecl( - name='texturedir', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='texture directory', - ), StructFieldDecl( name='discardvisual', type=ValueType(name='mjtByte'), @@ -8924,6 +8891,51 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjLROpt'), doc='options for lengthrange computation', ), + ), + )), + ('mjSpec', + StructDecl( + name='mjSpec', + declname='struct mjSpec_', + fields=( + StructFieldDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + doc='element type', + ), + StructFieldDecl( + name='modelname', + type=PointerType( + inner_type=ValueType(name='mjString'), + ), + doc='model name', + ), + StructFieldDecl( + name='compiler', + type=ValueType(name='mjsCompiler'), + doc='compiler options', + ), + StructFieldDecl( + name='strippath', + type=ValueType(name='mjtByte'), + doc='automatically strip paths from mesh files', + ), + StructFieldDecl( + name='meshdir', + type=PointerType( + inner_type=ValueType(name='mjString'), + ), + doc='mesh and hfield directory', + ), + StructFieldDecl( + name='texturedir', + type=PointerType( + inner_type=ValueType(name='mjString'), + ), + doc='texture directory', + ), StructFieldDecl( name='option', type=ValueType(name='mjOption'), diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index e02733c1..700ff883 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -39,8 +39,12 @@ def _value_binding_code( fullvarname = 'ptr->' + varname if field.name.startswith('mjs'): # all other mjs are raw structs fulltype = field.name.replace('mjs', 'raw::Mjs') - if field.name == 'mjsPlugin' or field.name == 'mjsOrientation': - fulltype = fulltype + '&' # plugin and orientation are not pointers + if ( + field.name == 'mjsPlugin' + or field.name == 'mjsOrientation' + or field.name == 'mjsCompiler' + ): + fulltype = fulltype + '&' # plugin, orientation, compiler are not pointers else: fulltype = fulltype + '*' # non-mjs structs diff --git a/python/mujoco/raw.h b/python/mujoco/raw.h index 3d481a12..7776e88c 100644 --- a/python/mujoco/raw.h +++ b/python/mujoco/raw.h @@ -58,6 +58,7 @@ using MjsText = ::mjsText; using MjsTuple = ::mjsTuple; using MjsKey = ::mjsKey; using MjsDefault = ::mjsDefault; +using MjsCompiler = ::mjsCompiler; using MjOption = ::mjOption; using MjSolverStat = ::mjSolverStat; using MjStatistic = ::mjStatistic; diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index b70ac9a7..2359de42 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -194,6 +194,7 @@ PYBIND11_MODULE(_specs, m) { py::class_ mjOption(m, "MjOption"); py::class_ mjStatistic(m, "MjStatistic"); py::class_ mjVisual(m, "MjVisual"); + py::class_ mjsCompiler(m, "MjsCompiler"); DefineArray(m, "MjCharVec"); DefineArray(m, "MjStringVec"); DefineArray(m, "MjByteVec"); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index e04292cc..b5955e5c 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -34,28 +34,28 @@ class SpecsTest(absltest.TestCase): spec = mujoco.MjSpec() # Check that euler sequence order is set correctly. - self.assertEqual(spec.eulerseq[0], 'x') - spec.eulerseq = ['z', 'y', 'x'] - self.assertEqual(spec.eulerseq[0], 'z') + self.assertEqual(spec.compiler.eulerseq[0], 'x') + spec.compiler.eulerseq = ['z', 'y', 'x'] + self.assertEqual(spec.compiler.eulerseq[0], 'z') # Change single elements of euler sequence. - spec.eulerseq[0] = 'y' - spec.eulerseq[1] = 'z' - self.assertEqual(spec.eulerseq[0], 'y') - self.assertEqual(spec.eulerseq[1], 'z') + spec.compiler.eulerseq[0] = 'y' + spec.compiler.eulerseq[1] = 'z' + self.assertEqual(spec.compiler.eulerseq[0], 'y') + self.assertEqual(spec.compiler.eulerseq[1], 'z') # eulerseq is iterable - self.assertEqual('yzx', ''.join(spec.eulerseq)) + self.assertEqual('yzx', ''.join(spec.compiler.eulerseq)) # supports `len` - self.assertLen(spec.eulerseq, 3) + self.assertLen(spec.compiler.eulerseq, 3) # field checks for out-of-bound access on read and on write with self.assertRaises(IndexError): - spec.eulerseq[3] = 'x' + spec.compiler.eulerseq[3] = 'x' with self.assertRaises(IndexError): - spec.eulerseq[-1] = 'x' + spec.compiler.eulerseq[-1] = 'x' # Add a body, check that it has default orientation. body = spec.worldbody.add_body() @@ -843,19 +843,15 @@ class SpecsTest(absltest.TestCase): with self.assertRaises(IndexError): material.textures[-1] = 'x' - def test_attach_error(self): + def test_attach_units(self): child = mujoco.MjSpec() parent = mujoco.MjSpec() - parent.degree = not child.degree - body = parent.worldbody.add_body() - frame = child.worldbody.add_frame() - with self.assertRaises(ValueError) as cm: - body.attach_frame(frame, '_', '') - self.assertEqual( - str(cm.exception), - 'Error: cannot attach mjSpecs with incompatible compiler/angle' - ' attribute', - ) + parent.compiler.degree = not child.compiler.degree + body = child.worldbody.add_body(euler=[90, 0, 0]) + frame = parent.worldbody.add_frame(euler=[-mujoco.mjPI / 2, 0, 0]) + frame.attach_body(body, 'child-', '') + model = parent.compile() + np.testing.assert_almost_equal(model.body_quat[1], [1, 0, 0, 0]) def test_attach_body_to_site(self): child = mujoco.MjSpec() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index a1412146..6b5a7769 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -69,7 +69,13 @@ mjSpec* mj_makeSpec() { // copy model mjSpec* mj_copySpec(const mjSpec* s) { - mjCModel* modelC = new mjCModel(*static_cast(s->element)); + mjCModel* modelC = nullptr; + try { + modelC = new mjCModel(*static_cast(s->element)); + } catch (mjCError& e) { + mju_error("Failed to copy spec: %s", e.message); + return nullptr; + } return &modelC->spec; } @@ -177,7 +183,8 @@ mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, frame->spec.quat[1] = site->spec.quat[1]; frame->spec.quat[2] = site->spec.quat[2]; frame->spec.quat[3] = site->spec.quat[3]; - mjs_resolveOrientation(frame->spec.quat, spec->degree, spec->eulerseq, &site->spec.alt); + mjs_resolveOrientation(frame->spec.quat, spec->compiler.degree, + spec->compiler.eulerseq, &site->spec.alt); return mjs_attachBody(&frame->spec, child, prefix, suffix); } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 2f4aa428..732d87ba 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -98,8 +98,9 @@ mjCFlexcomp::mjCFlexcomp(void) { // make flexcomp object -bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { - mjCModel* model = (mjCModel*)spec->element; +bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { + mjCModel* model = static_cast(body->element)->model; + mjsCompiler* compiler = static_cast(body->element)->compiler; mjsFlex* dflex = def.spec.flex; bool radial = (type == mjFCOMPTYPE_BOX || @@ -147,7 +148,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // compute orientation - const char* alterr = mjs_resolveOrientation(quat, model->spec.degree, model->spec.eulerseq, &alt); + const char* alterr = mjs_resolveOrientation(quat, compiler->degree, compiler->eulerseq, &alt); if (alterr) { return comperr(error, alterr, error_sz); } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index d090022b..92ce884c 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -42,7 +42,7 @@ typedef enum _mjtFcompType { class mjCFlexcomp { public: mjCFlexcomp(void); - bool Make(mjSpec* spec, mjsBody* body, char* error, int error_sz); + bool Make(mjsBody* body, char* error, int error_sz); bool MakeGrid(char* error, int error_sz); bool MakeBox(char* error, int error_sz); diff --git a/src/user/user_init.c b/src/user/user_init.c index d2ef5798..b5d15413 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -33,17 +33,17 @@ void mjs_defaultSpec(mjSpec* spec) { spec->stat.center[0] = mjNAN; // compiler settings - spec->autolimits = 1; - spec->settotalmass = -1; - spec->degree = 1; - spec->eulerseq[0] = 'x'; - spec->eulerseq[1] = 'y'; - spec->eulerseq[2] = 'z'; - spec->convexhull = 1; - spec->usethread = 1; - spec->inertiafromgeom = mjINERTIAFROMGEOM_AUTO; - spec->inertiagrouprange[1] = mjNGROUP-1; - mj_defaultLROpt(&spec->LRopt); + spec->compiler.autolimits = 1; + spec->compiler.settotalmass = -1; + spec->compiler.degree = 1; + spec->compiler.eulerseq[0] = 'x'; + spec->compiler.eulerseq[1] = 'y'; + spec->compiler.eulerseq[2] = 'z'; + spec->compiler.convexhull = 1; + spec->compiler.usethread = 1; + spec->compiler.inertiafromgeom = mjINERTIAFROMGEOM_AUTO; + spec->compiler.inertiagrouprange[1] = mjNGROUP-1; + mj_defaultLROpt(&spec->compiler.LRopt); // engine data mj_defaultOption(&spec->option); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index fbe2383c..d1e20f46 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -563,7 +563,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { } // make graph describing convex hull - if ((model->convexhull && needhull_) || face_.empty()) { + if ((model->compiler.convexhull && needhull_) || face_.empty()) { MakeGraph(); } @@ -739,7 +739,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3); // use inertial box - if (!model->fitaabb) { + if (!model->compiler.fitaabb) { // get inertia box type (shell or volume) double* boxsz = GetInertiaBoxPtr(geom->typeinertia); switch (geom->type) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 82e3d941..99ab9a11 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -227,8 +227,10 @@ void mjCModel::CopyList(std::vector& dest, } // copy the element from the other model to this model source[i]->ForgetKeyframes(); + mjSpec* origin = FindSpec(mjs_getString(source[i]->model->spec.modelname)); dest.push_back(candidate); dest.back()->model = this; + dest.back()->compiler = origin ? &origin->compiler : &spec.compiler; dest.back()->id = -1; } if (!dest.empty()) { @@ -354,20 +356,6 @@ static bool IsPluginActive( mjCModel& mjCModel::operator+=(const mjCModel& other) { - // TODO: use compiler settings stored in specs_ during compilation - std::string msg = "cannot attach mjSpecs with incompatible compiler/"; - if (other.spec.degree != spec.degree) { - throw mjCError(nullptr, (msg + "angle attribute").c_str()); - } - if (other.spec.autolimits != spec.autolimits) { - throw mjCError(nullptr, (msg + "autolimits attribute").c_str()); - } - if (other.spec.eulerseq[0] != spec.eulerseq[0] || - other.spec.eulerseq[1] != spec.eulerseq[1] || - other.spec.eulerseq[2] != spec.eulerseq[2]) { - throw mjCError(nullptr, (msg + "eulerseq attribute").c_str()); - } - // create global lists mjCBody *world = bodies_[0]; ResetTreeLists(); @@ -1784,8 +1772,8 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { mjOption saveopt = m->opt; m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_PASSIVE | mjDSBL_GRAVITY | mjDSBL_ACTUATION; - if (LRopt.timestep>0) { - m->opt.timestep = LRopt.timestep; + if (compiler.LRopt.timestep>0) { + m->opt.timestep = compiler.LRopt.timestep; } // number of threads available @@ -1800,14 +1788,14 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { m->actuator_biastype[i]==mjBIAS_MUSCLE); int isuser = (m->actuator_gaintype[i]==mjGAIN_USER || m->actuator_biastype[i]==mjBIAS_USER); - if ((LRopt.mode==mjLRMODE_NONE) || - (LRopt.mode==mjLRMODE_MUSCLE && !ismuscle) || - (LRopt.mode==mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) { + if ((compiler.LRopt.mode==mjLRMODE_NONE) || + (compiler.LRopt.mode==mjLRMODE_MUSCLE && !ismuscle) || + (compiler.LRopt.mode==mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) { continue; } // use existing length range if available - if (LRopt.useexisting && + if (compiler.LRopt.useexisting && (m->actuator_lengthrange[2*i] < m->actuator_lengthrange[2*i+1])) { continue; } @@ -1817,10 +1805,10 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { } // single thread - if (!usethread || cnt<2 || nthread<2) { + if (!compiler.usethread || cnt<2 || nthread<2) { char err[200]; for (int i=0; inu; i++) { - if (!mj_setLengthRange(m, data, i, &LRopt, err, 200)) { + if (!mj_setLengthRange(m, data, i, &compiler.LRopt, err, 200)) { throw mjCError(0, "%s", err); } } @@ -1844,7 +1832,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { // prepare thread function arguments, clear errors LRThreadArg arg[kMaxCompilerThreads]; for (int i=0; i 1) { + if (compiler.usethread && meshes_.size() > 1) { // multi-threaded mesh compile CompileMeshes(vfs); } else { @@ -3981,10 +3969,10 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { reassignid(excludes_); // resolve asset references, compute sizes - IndexAssets(discardvisual); + IndexAssets(compiler.discardvisual); SetSizes(); // fuse static if enabled - if (fusestatic) { + if (compiler.fusestatic) { FuseStatic(); } @@ -4144,8 +4132,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { CopyObjects(m); // scale mass - if (settotalmass>0) { - mj_setTotalmass(m, settotalmass); + if (compiler.settotalmass>0) { + mj_setTotalmass(m, compiler.settotalmass); } // set arena size into m->narena diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4c084b22..e795f49a 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -749,6 +749,7 @@ void mjCBase::SetFrame(mjCFrame* _frame) { mjCBody::mjCBody(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; mjs_defaultBody(&spec); elemtype = mjOBJ_BODY; @@ -785,6 +786,8 @@ mjCBody::mjCBody(mjCModel* _model) { mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) { model = _model; + mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); + compiler = origin ? &origin->compiler : &model->spec.compiler; *this = other; } @@ -868,9 +871,11 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { } // copy input frame + mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); frames.push_back(new mjCFrame(other)); frames.back()->body = this; frames.back()->model = model; + frames.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; frames.back()->frame = other.frame; frames.back()->NameSpace(other.model); int i = frames.size(); @@ -931,9 +936,11 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, if (pframe && !pframe->IsAncestor(src[i]->frame)) { continue; // skip if the element is not inside pframe } + mjSpec* origin = model->FindSpec(mjs_getString(src[i]->model->spec.modelname)); dst.push_back(new T(*src[i])); dst.back()->body = this; dst.back()->model = model; + dst.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; dst.back()->id = -1; dst.back()->classname = src[i]->classname; @@ -1413,8 +1420,8 @@ void mjCBody::InertiaFromGeom(void) { // select geoms based on group sel.clear(); for (int i=0; igroup>=model->inertiagrouprange[0] && - geoms[i]->group<=model->inertiagrouprange[1]) { + if (geoms[i]->group>=compiler->inertiagrouprange[0] && + geoms[i]->group<=compiler->inertiagrouprange[1]) { sel.push_back(geoms[i]); } } @@ -1559,7 +1566,7 @@ void mjCBody::Compile(void) { // check and process orientation alternatives for body if (alt.type != mjORIENTATION_QUAT) { - const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); + const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt); if (err) { throw mjCError(this, "error '%s' in frame alternative", err); } @@ -1582,7 +1589,7 @@ void mjCBody::Compile(void) { } if (ialt.type != mjORIENTATION_QUAT) { - const char* err = ResolveOrientation(iquat, model->degree, model->eulerseq, ialt); + const char* err = ResolveOrientation(iquat, compiler->degree, compiler->eulerseq, ialt); if (err) { throw mjCError(this, "error '%s' in inertia alternative", err); } @@ -1591,15 +1598,15 @@ void mjCBody::Compile(void) { // compile all geoms for (int i=0; iinferinertia = id>0 && - (!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) && - geoms[i]->spec.group >= model->inertiagrouprange[0] && - geoms[i]->spec.group <= model->inertiagrouprange[1]; + (!explicitinertial || compiler->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) && + geoms[i]->spec.group >= compiler->inertiagrouprange[0] && + geoms[i]->spec.group <= compiler->inertiagrouprange[1]; geoms[i]->Compile(); } // set inertial frame from geoms if necessary - if (id>0 && (model->inertiafromgeom==mjINERTIAFROMGEOM_TRUE || - (!mjuu_defined(ipos[0]) && model->inertiafromgeom==mjINERTIAFROMGEOM_AUTO))) { + if (id>0 && (compiler->inertiafromgeom==mjINERTIAFROMGEOM_TRUE || + (!mjuu_defined(ipos[0]) && compiler->inertiafromgeom==mjINERTIAFROMGEOM_AUTO))) { InertiaFromGeom(); } @@ -1612,10 +1619,10 @@ void mjCBody::Compile(void) { // check and correct mass and inertia if (id>0) { // fix minimum - mass = std::max(mass, model->boundmass); - inertia[0] = std::max(inertia[0], model->boundinertia); - inertia[1] = std::max(inertia[1], model->boundinertia); - inertia[2] = std::max(inertia[2], model->boundinertia); + mass = std::max(mass, compiler->boundmass); + inertia[0] = std::max(inertia[0], compiler->boundinertia); + inertia[1] = std::max(inertia[1], compiler->boundinertia); + inertia[2] = std::max(inertia[2], compiler->boundinertia); // check for negative values if (mass<0 || inertia[0]<0 || inertia[1]<0 ||inertia[2]<0) { @@ -1626,7 +1633,7 @@ void mjCBody::Compile(void) { if (inertia[0] + inertia[1] < inertia[2] || inertia[0] + inertia[2] < inertia[1] || inertia[1] + inertia[2] < inertia[0]) { - if (model->balanceinertia) { + if (compiler->balanceinertia) { inertia[0] = inertia[1] = inertia[2] = (inertia[0] + inertia[1] + inertia[2])/3.0; } else { throw mjCError(this, "inertia must satisfy A + B >= C; use 'balanceinertia' to fix"); @@ -1654,7 +1661,7 @@ void mjCBody::Compile(void) { bodies.empty() && // no child bodies AND (joints[0]->spec.align == 1 || // either joint.align="true" (joints[0]->spec.align == 2 && // or joint.align="auto" - model->alignfree))); // and compiler.align="true" + compiler->alignfree))); // and compiler->align="true" // free-joint alignment, phase 1 (this body + child geoms) double ipos_inverse[3], iquat_inverse[4]; @@ -1740,7 +1747,7 @@ void mjCBody::Compile(void) { } // if discarding visual geoms, use explicit inertias - if (model->discardvisual) { + if (compiler->discardvisual) { for (int j=0; jIsVisual()) { explicitinertial = true; @@ -1782,6 +1789,7 @@ mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) { elemtype = mjOBJ_FRAME; compiled = false; model = _model; + if (_model) compiler = &_model->spec.compiler; body = NULL; frame = _frame ? _frame : NULL; last_attached = nullptr; @@ -1905,7 +1913,7 @@ void mjCFrame::Compile() { } CopyFromSpec(); - const char* err = ResolveOrientation(quat, model->spec.degree, model->spec.eulerseq, alt); + const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -1940,6 +1948,7 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -2069,7 +2078,7 @@ int mjCJoint::Compile(void) { // otherwise if limited is auto, check consistency wrt auto-limits else if (limited == mjLIMITED_AUTO) { bool hasrange = !(range[0]==0 && range[1]==0); - checklimited(this, model->autolimits, "joint", "", limited, hasrange); + checklimited(this, compiler->autolimits, "joint", "", limited, hasrange); } // resolve limits @@ -2083,7 +2092,7 @@ int mjCJoint::Compile(void) { } // convert limits to radians - if (model->degree && (type==mjJNT_HINGE || type==mjJNT_BALL)) { + if (compiler->degree && (type==mjJNT_HINGE || type==mjJNT_BALL)) { if (range[0]) { range[0] *= mjPI/180.0; } @@ -2100,7 +2109,7 @@ int mjCJoint::Compile(void) { // otherwise if actfrclimited is auto, check consistency wrt auto-limits else if (actfrclimited == mjLIMITED_AUTO) { bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0); - checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange); + checklimited(this, compiler->autolimits, "joint", "", actfrclimited, hasrange); } // resolve actuator force range limits @@ -2144,7 +2153,7 @@ int mjCJoint::Compile(void) { } // convert reference angles to radians for hinge joints - if (type==mjJNT_HINGE && model->degree) { + if (type==mjJNT_HINGE && compiler->degree) { ref *= mjPI/180.0; springref *= mjPI/180.0; } @@ -2193,6 +2202,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -2892,7 +2902,7 @@ void mjCGeom::Compile(void) { // not 'fromto': try alternative else { - const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); + const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in geom %d", err, id); } @@ -3034,6 +3044,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; } @@ -3140,7 +3151,7 @@ void mjCSite::Compile(void) { // alternative orientation else { - const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); + const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -3179,6 +3190,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -3248,7 +3260,7 @@ void mjCCamera::Compile(void) { userdata_.resize(model->nuser_cam); // process orientation specifications - const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); + const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in camera %d", err, id); } @@ -3330,6 +3342,7 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; PointToLocal(); @@ -3423,6 +3436,7 @@ mjCHField::mjCHField(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear variables data.clear(); @@ -3678,6 +3692,7 @@ mjCTexture::mjCTexture(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear user settings: single file spec_file_.clear(); @@ -4511,6 +4526,7 @@ mjCMaterial::mjCMaterial(mjCModel* _model, mjCDef* _def) { } model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; PointToLocal(); @@ -4597,6 +4613,7 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -4817,6 +4834,7 @@ void mjCPair::Compile(void) { mjCBodyPair::mjCBodyPair(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; elemtype = mjOBJ_EXCLUDE; // set defaults @@ -4955,6 +4973,7 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -5116,6 +5135,7 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // point to local @@ -5201,6 +5221,7 @@ mjCTendon::~mjCTendon() { void mjCTendon::SetModel(mjCModel* _model) { model = _model; + if (_model) compiler = &_model->spec.compiler; for (int i=0; imodel = _model; } @@ -5423,7 +5444,7 @@ void mjCTendon::Compile(void) { // if limited is auto, set to 1 if range is specified, otherwise unlimited if (limited == mjLIMITED_AUTO) { bool hasrange = !(range[0]==0 && range[1]==0); - checklimited(this, model->autolimits, "tendon", "", limited, hasrange); + checklimited(this, compiler->autolimits, "tendon", "", limited, hasrange); } // check limits @@ -5447,6 +5468,7 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) { // set model and tendon pointer model = _model; + if (_model) compiler = &_model->spec.compiler; tendon = _tendon; // clear variables @@ -5591,6 +5613,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = _def ? _def->name : "main"; // in case this actuator is not compiled @@ -5846,15 +5869,15 @@ void mjCActuator::Compile(void) { // if limited is auto, check for inconsistency wrt to autolimits if (forcelimited == mjLIMITED_AUTO) { bool hasrange = !(forcerange[0]==0 && forcerange[1]==0); - checklimited(this, model->autolimits, "actuator", "force", forcelimited, hasrange); + checklimited(this, compiler->autolimits, "actuator", "force", forcelimited, hasrange); } if (ctrllimited == mjLIMITED_AUTO) { bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0); - checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange); + checklimited(this, compiler->autolimits, "actuator", "ctrl", ctrllimited, hasrange); } if (actlimited == mjLIMITED_AUTO) { bool hasrange = !(actrange[0]==0 && actrange[1]==0); - checklimited(this, model->autolimits, "actuator", "act", actlimited, hasrange); + checklimited(this, compiler->autolimits, "actuator", "act", actlimited, hasrange); } // check limits @@ -5951,6 +5974,7 @@ mjCSensor::mjCSensor(mjCModel* _model) { // set model model = _model; + if (_model) compiler = &_model->spec.compiler; // clear private variables spec_objname_.clear(); @@ -6477,6 +6501,7 @@ mjCNumeric::mjCNumeric(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear variables spec_data_.clear(); @@ -6566,6 +6591,7 @@ mjCText::mjCText(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear variables spec_data_.clear(); @@ -6643,6 +6669,7 @@ mjCTuple::mjCTuple(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear variables spec_objtype_.clear(); @@ -6776,6 +6803,7 @@ mjCKey::mjCKey(mjCModel* _model) { // set model pointer model = _model; + if (_model) compiler = &_model->spec.compiler; // clear variables spec_qpos_.clear(); @@ -6952,6 +6980,7 @@ mjCPlugin::mjCPlugin(mjCModel* _model) { plugin_slot = -1; parent = this; model = _model; + if (_model) compiler = &_model->spec.compiler; name.clear(); plugin_name.clear(); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 85db8da8..1018fb8b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -212,7 +212,8 @@ class mjCBase : public mjCBase_ { mjCBase& operator=(const mjCBase& other); mjCFrame* frame; // pointer to frame transformation - mjCModel* model; // pointer to model that created object + mjCModel* model; // pointer to model that owns object + mjsCompiler* compiler; // pointer to the compiler options virtual ~mjCBase() = default; // destructor diff --git a/src/xml/xml.cc b/src/xml/xml.cc index abcd8c13..5277a534 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -365,8 +365,8 @@ mjSpec* ParseXML(const char* filename, const mjVFS* vfs, // set reasonable default for parsing a URDF // this is separate from the Parser to allow multiple URDFs to be loaded. spec->strippath = true; - spec->fusestatic = true; - spec->discardvisual = true; + spec->compiler.fusestatic = true; + spec->compiler.discardvisual = true; parser.SetModel(spec); parser.Parse(root); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index f145afbd..854459cb 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -972,19 +972,19 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { // top-level attributes if (MapValue(section, "autolimits", &n, bool_map, 2)) { - spec->autolimits = (n==1); + spec->compiler.autolimits = (n==1); } - ReadAttr(section, "boundmass", 1, &spec->boundmass, text); - ReadAttr(section, "boundinertia", 1, &spec->boundinertia, text); - ReadAttr(section, "settotalmass", 1, &spec->settotalmass, text); + ReadAttr(section, "boundmass", 1, &spec->compiler.boundmass, text); + ReadAttr(section, "boundinertia", 1, &spec->compiler.boundinertia, text); + ReadAttr(section, "settotalmass", 1, &spec->compiler.settotalmass, text); if (MapValue(section, "balanceinertia", &n, bool_map, 2)) { - spec->balanceinertia = (n==1); + spec->compiler.balanceinertia = (n==1); } if (MapValue(section, "strippath", &n, bool_map, 2)) { spec->strippath = (n==1); } if (MapValue(section, "fitaabb", &n, bool_map, 2)) { - spec->fitaabb = (n==1); + spec->compiler.fitaabb = (n==1); } if (MapValue(section, "coordinate", &n, coordinate_map, 2)) { if (n==1) { @@ -993,13 +993,13 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { } } if (MapValue(section, "angle", &n, angle_map, 2)) { - spec->degree = (n==1); + spec->compiler.degree = (n==1); } if (ReadAttrTxt(section, "eulerseq", text)) { if (text.size()!=3) { throw mjXError(section, "euler format must have length 3"); } - memcpy(spec->eulerseq, text.c_str(), 3); + memcpy(spec->compiler.eulerseq, text.c_str(), 3); } if (ReadAttrTxt(section, "assetdir", text)) { mjs_setString(spec->meshdir, text.c_str()); @@ -1014,27 +1014,27 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { mjs_setString(spec->texturedir, texturedir.c_str()); } if (MapValue(section, "discardvisual", &n, bool_map, 2)) { - spec->discardvisual = (n==1); + spec->compiler.discardvisual = (n==1); } if (MapValue(section, "convexhull", &n, bool_map, 2)) { - spec->convexhull = (n==1); + spec->compiler.convexhull = (n==1); } if (MapValue(section, "usethread", &n, bool_map, 2)) { - spec->usethread = (n==1); + spec->compiler.usethread = (n==1); } if (MapValue(section, "fusestatic", &n, bool_map, 2)) { - spec->fusestatic = (n==1); + spec->compiler.fusestatic = (n==1); } - MapValue(section, "inertiafromgeom", &spec->inertiafromgeom, TFAuto_map, 3); - ReadAttr(section, "inertiagrouprange", 2, spec->inertiagrouprange, text); + MapValue(section, "inertiafromgeom", &spec->compiler.inertiafromgeom, TFAuto_map, 3); + ReadAttr(section, "inertiagrouprange", 2, spec->compiler.inertiagrouprange, text); if (MapValue(section, "alignfree", &n, bool_map, 2)) { - spec->alignfree = (n==1); + spec->compiler.alignfree = (n==1); } // lengthrange subelement XMLElement* elem = FindSubElem(section, "lengthrange"); if (elem) { - mjLROpt* opt = &(spec->LRopt); + mjLROpt* opt = &(spec->compiler.LRopt); // flags MapValue(elem, "mode", &opt->mode, lrmode_map, lrmode_sz); @@ -2759,7 +2759,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { // make flexcomp char error[200]; - bool res = fcomp.Make(spec, body, error, 200); + bool res = fcomp.Make(body, error, 200); // throw error if (!res) { @@ -3575,7 +3575,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, alt.type = mjORIENTATION_EULER; mjuu_copyvec(alt.euler, euler, 3); double rotation[4] = {1, 0, 0, 0}; - mjs_resolveOrientation(rotation, spec->degree, spec->eulerseq, &alt); + mjs_resolveOrientation(rotation, spec->compiler.degree, spec->compiler.eulerseq, &alt); // read childdef mjsDefault* childdef = 0; @@ -3610,7 +3610,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, alt.euler[0] = i*euler[0]; alt.euler[1] = i*euler[1]; alt.euler[2] = i*euler[2]; - mjs_resolveOrientation(quat, spec->degree, spec->eulerseq, &alt); + mjs_resolveOrientation(quat, spec->compiler.degree, spec->compiler.eulerseq, &alt); mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]); // process suffix diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 02c265a3..907920a6 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -920,8 +920,8 @@ void mjXWriter::Compiler(XMLElement* root) { XMLElement* section = InsertEnd(root, "compiler"); // settings - if (!model->convexhull) { - WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->convexhull)); + if (!model->compiler.convexhull) { + WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->compiler.convexhull)); } WriteAttrTxt(section, "angle", "radian"); if (!model->get_meshdir().empty()) { @@ -930,20 +930,20 @@ void mjXWriter::Compiler(XMLElement* root) { if (!model->get_texturedir().empty()) { WriteAttrTxt(section, "texturedir", model->get_texturedir()); } - if (!model->usethread) { + if (!model->compiler.usethread) { WriteAttrTxt(section, "usethread", "false"); } - if (model->boundmass) { - WriteAttr(section, "boundmass", 1, &model->boundmass); + if (model->compiler.boundmass) { + WriteAttr(section, "boundmass", 1, &model->compiler.boundmass); } - if (model->boundinertia) { - WriteAttr(section, "boundinertia", 1, &model->boundinertia); + if (model->compiler.boundinertia) { + WriteAttr(section, "boundinertia", 1, &model->compiler.boundinertia); } - if (model->alignfree) { + if (model->compiler.alignfree) { WriteAttrTxt(section, "alignfree", "true"); } - if (!model->autolimits) { + if (!model->compiler.autolimits) { WriteAttrTxt(section, "autolimits", "false"); } } @@ -1611,7 +1611,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_vi WriteVector(elem, "user", body->get_userdata()); // write inertial - if (body->explicitinertial && model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { + if (body->explicitinertial && model->compiler.inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { XMLElement* inertial = InsertEnd(elem, "inertial"); WriteAttr(inertial, "pos", 3, body->ipos); WriteAttr(inertial, "quat", 4, body->iquat, unitq); diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 3dc98b65..301ac22a 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -109,7 +109,7 @@ void mjXURDF::Parse( } // enforce required compiler defaults for URDF - spec->degree = false; + spec->compiler.degree = false; // get model name std::string modelname; @@ -316,7 +316,7 @@ void mjXURDF::Body(XMLElement* body_elem) { } } // create geom if not discarded - if (!spec->discardvisual) { + if (!spec->compiler.discardvisual) { pgeom = Geom(elem, pbody, false); // save color diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 7922d1d8..ab824637 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1839,23 +1839,43 @@ TEST_F(MujocoTest, RepeatedAttachKeyframe) { mj_deleteModel(model_2); } -TEST_F(MujocoTest, DifferentUnitsNotAllowed) { - mjSpec* spec_1 = mj_makeSpec(); - mjSpec* spec_2 = mj_makeSpec(); - spec_1->degree = 1; - spec_2->degree = 0; +TEST_F(MujocoTest, DifferentUnitsAllowed) { + mjSpec* child = mj_makeSpec(); + child->compiler.degree = 1; + mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0); + body->alt.type = mjORIENTATION_EULER; + body->alt.euler[0] = 90; - mjsBody* body = mjs_addBody(mjs_findBody(spec_1, "world"), 0); - mjsFrame* frame = mjs_addFrame(mjs_findBody(spec_2, "world"), 0); + mjSpec* parent = mj_makeSpec(); + parent->compiler.degree = 0; + mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0); + frame->alt.type = mjORIENTATION_EULER; + frame->alt.euler[0] = -mjPI / 2; - constexpr char msg[] = "mjSpecs with incompatible compiler/angle"; - EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), IsNull()); - EXPECT_THAT(mjs_attachFrame(body, frame, "child-", ""), IsNull()); - EXPECT_THAT(mjs_getError(spec_1), HasSubstr(msg)); - EXPECT_THAT(mjs_getError(spec_2), HasSubstr(msg)); + EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), NotNull()); + mjModel* model = mj_compile(parent, 0); + EXPECT_THAT(model, NotNull()); + EXPECT_NEAR(model->body_quat[4], 1, 1e-12); + EXPECT_NEAR(model->body_quat[5], 0, 1e-12); + EXPECT_NEAR(model->body_quat[6], 0, 1e-12); + EXPECT_NEAR(model->body_quat[7], 0, 1e-12); - mj_deleteSpec(spec_1); - mj_deleteSpec(spec_2); + mjSpec* copy = mj_copySpec(parent); + EXPECT_THAT(copy, NotNull()); + mj_deleteModel(model); + mj_deleteSpec(child); + mj_deleteSpec(parent); + + // check that deleting `parent` or `child` does not invalidate the copy + mjModel* copy_model = mj_compile(copy, 0); + EXPECT_THAT(copy_model, NotNull()); + EXPECT_NEAR(copy_model->body_quat[0], 1, 1e-12); + EXPECT_NEAR(copy_model->body_quat[1], 0, 1e-12); + EXPECT_NEAR(copy_model->body_quat[2], 0, 1e-12); + EXPECT_NEAR(copy_model->body_quat[3], 0, 1e-12); + + mj_deleteModel(copy_model); + mj_deleteSpec(copy); } TEST_F(MujocoTest, CopyAttachedSpec) { diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 74a14f77..24467d97 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -480,8 +480,8 @@ TEST_F(LengthRangeTest, LengthRangeThreading) { DoubleNear(std::sqrt(5.0), 1e-3)); // recompile without threads - ASSERT_EQ(spec->usethread, 1); - spec->usethread = 0; + ASSERT_EQ(spec->compiler.usethread, 1); + spec->compiler.usethread = 0; mjModel* model2 = mj_compile(spec, 0); EXPECT_THAT(model2, NotNull()) << error; diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 2a97ec58..5d6891c3 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1176,7 +1176,7 @@ TEST_F(MjCJointTest, AlignFree) { std::array err; mjSpec* s = mj_parseXML(xml_path.c_str(), nullptr, err.data(), err.size()); ASSERT_THAT(s, NotNull()) << err.data(); - s->alignfree = 1; // auto-aligned free joint + s->compiler.alignfree = 1; // auto-aligned free joint mjModel* m = mj_compile(s, nullptr); ASSERT_THAT(m, NotNull()); @@ -1186,7 +1186,7 @@ TEST_F(MjCJointTest, AlignFree) { EXPECT_EQ(m->dof_simplenum[0], 6); // make unaligned model - s->alignfree = 0; // unaligned free joint + s->compiler.alignfree = 0; // unaligned free joint mjModel* m_u = mj_compile(s, nullptr); ASSERT_THAT(m_u, NotNull()); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 9bd3c6bb..dbc5364b 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5716,6 +5716,26 @@ public unsafe struct mjrContext_ { public int readDepthMap; } +[StructLayout(LayoutKind.Sequential)] +public unsafe struct mjsCompiler_ { + public byte autolimits; + public double boundmass; + public double boundinertia; + public double settotalmass; + public byte balanceinertia; + public byte fitaabb; + public byte degree; + public fixed sbyte eulerseq[3]; + public byte discardvisual; + public byte convexhull; + public byte usethread; + public byte fusestatic; + public int inertiafromgeom; + public fixed int inertiagrouprange[2]; + public int alignfree; + public mjLROpt_ LRopt; +} + [StructLayout(LayoutKind.Sequential)] public unsafe struct mjuiState_ { public int nrect; From a3ea01e57e07750b83b54c1387c4c3dc61930030 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 24 Oct 2024 09:14:13 -0700 Subject: [PATCH 007/426] Remove deprecated `mju_rotVecMat` and `mju_rotVecMatT` functions. PiperOrigin-RevId: 689405782 Change-Id: I0281376d5d6f5c31ea2603d61c03c7b94aaa6312 --- doc/APIreference/functions.rst | 18 --------- doc/changelog.rst | 3 +- doc/includes/references.h | 2 - include/mujoco/mujoco.h | 6 --- introspect/functions.py | 58 ---------------------------- python/mujoco/functions.cc | 2 - src/engine/engine_util_blas.c | 14 ------- src/engine/engine_util_blas.h | 6 --- unity/Runtime/Bindings/MjBindings.cs | 6 --- 9 files changed, 2 insertions(+), 113 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 0ce805ce..38568b35 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3142,24 +3142,6 @@ mju_mulMatTVec3 Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. -.. _mju_rotVecMat: - -mju_rotVecMat -~~~~~~~~~~~~~ - -.. mujoco-include:: mju_rotVecMat - -Deprecated, use mju_mulMatVec3(res, mat, vec). - -.. _mju_rotVecMatT: - -mju_rotVecMatT -~~~~~~~~~~~~~~ - -.. mujoco-include:: mju_rotVecMatT - -Deprecated, use mju_mulMatTVec3(res, mat, vec). - .. _mju_cross: mju_cross diff --git a/doc/changelog.rst b/doc/changelog.rst index 4cb748dd..8f69bb07 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,7 @@ General ^^^^^^^ - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. +- Removed the deprecated ``mju_rotVecMat`` and ``mju_rotVecMatT`` functions. MJX ^^^ @@ -214,7 +215,7 @@ General 3. Calls to :ref:`mj_defaultVFS` may allocate memory inside VFS, and the corresponding :ref:`mj_deleteVFS` must be called to deallocate any internal allocated memory. - 4. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and + 4. Deprecated ``mju_rotVecMat`` and ``mju_rotVecMatT`` in favor of :ref:`mju_mulMatVec3` and :ref:`mju_mulMatTVec3`. These function names and argument order are more consistent with the rest of the API. The older functions have been removed from the Python bindings and will be removed from the C API in the next release. diff --git a/doc/includes/references.h b/doc/includes/references.h index f1cd64a7..e19503d4 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3432,8 +3432,6 @@ mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); -void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); -void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); void mju_zero4(mjtNum res[4]); void mju_unit4(mjtNum res[4]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 751c6cdf..849462fe 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -975,12 +975,6 @@ MJAPI void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3 // Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. MJAPI void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); -// Deprecated, use mju_mulMatVec3(res, mat, vec). -MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); - -// Deprecated, use mju_mulMatTVec3(res, mat, vec). -MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); - // Compute cross-product: res = cross(a, b). MJAPI void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); diff --git a/introspect/functions.py b/introspect/functions.py index c0a3a196..4f86485d 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6187,64 +6187,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc="Multiply transposed 3-by-3 matrix by vector: res = mat' * vec.", )), - ('mju_rotVecMat', - FunctionDecl( - name='mju_rotVecMat', - return_type=ValueType(name='void'), - parameters=( - FunctionParameterDecl( - name='res', - type=ArrayType( - inner_type=ValueType(name='mjtNum'), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='vec', - type=ArrayType( - inner_type=ValueType(name='mjtNum', is_const=True), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='mat', - type=ArrayType( - inner_type=ValueType(name='mjtNum', is_const=True), - extents=(9,), - ), - ), - ), - doc='Deprecated, use mju_mulMatVec3(res, mat, vec).', - )), - ('mju_rotVecMatT', - FunctionDecl( - name='mju_rotVecMatT', - return_type=ValueType(name='void'), - parameters=( - FunctionParameterDecl( - name='res', - type=ArrayType( - inner_type=ValueType(name='mjtNum'), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='vec', - type=ArrayType( - inner_type=ValueType(name='mjtNum', is_const=True), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='mat', - type=ArrayType( - inner_type=ValueType(name='mjtNum', is_const=True), - extents=(9,), - ), - ), - ), - doc='Deprecated, use mju_mulMatTVec3(res, mat, vec).', - )), ('mju_cross', FunctionDecl( name='mju_cross', diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 0770611d..6965957d 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -720,8 +720,6 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); - // skipped: mju_rotVecMat - // skipped: mju_rotVecMatT Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index af1a0447..c870c6fb 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -189,20 +189,6 @@ void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) { -// multiply vector by 3D rotation matrix (deprecated) -void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) { - mju_mulMatVec3(res, mat, vec); -} - - - -// multiply vector by transposed 3D rotation matrix (deprecated) -void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) { - mju_mulMatTVec3(res, mat, vec); -} - - - // multiply 3x3 matrices, void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6]; diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index 77f9a6a7..619a3225 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -112,12 +112,6 @@ MJAPI void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3 // multiply transposed 3-by-3 matrix by vector MJAPI void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); -// multiply vector by 3D rotation matrix (deprecated) -MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); - -// multiply vector by transposed 3D rotation matrix (deprecated) -MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]); - // multiply 3x3 matrices MJAPI void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index dbc5364b..4eab533b 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7141,12 +7141,6 @@ public static unsafe extern void mju_mulMatVec3(double* res, double* mat, double [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_mulMatTVec3(double* res, double* mat, double* vec); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mju_rotVecMat(double* res, double* vec, double* mat); - -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mju_rotVecMatT(double* res, double* vec, double* mat); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_cross(double* res, double* a, double* b); From bebec52869af5d3182c998803dd73c8b7744ee4b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 24 Oct 2024 13:00:38 -0700 Subject: [PATCH 008/426] Add find_all with string input. PiperOrigin-RevId: 689487165 Change-Id: I48511aec1610b2112f9456e8f410d077c57c7a95 --- python/mujoco/specs.cc | 104 +++++++++++++++++++++++------------- python/mujoco/specs_test.py | 30 ++++++----- 2 files changed, 82 insertions(+), 52 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 2359de42..96bb7baf 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -155,6 +155,48 @@ void DefineArray(py::module& m, const std::string& typestr) { }, py::keep_alive<0, 1>(), py::return_value_policy::reference_internal); }; +py::list FindAllImpl(raw::MjsBody& body, mjtObj objtype) { + py::list list; + raw::MjsElement* el = mjs_firstChild(&body, objtype, true); + std::string error = mjs_getError(mjs_getSpec(body.element)); + if (!el && !error.empty()) { + throw pybind11::value_error(error); + } + while (el) { + switch (objtype) { + case mjOBJ_BODY: + list.append(mjs_asBody(el)); + break; + case mjOBJ_CAMERA: + list.append(mjs_asCamera(el)); + break; + case mjOBJ_FRAME: + list.append(mjs_asFrame(el)); + break; + case mjOBJ_GEOM: + list.append(mjs_asGeom(el)); + break; + case mjOBJ_JOINT: + list.append(mjs_asJoint(el)); + break; + case mjOBJ_LIGHT: + list.append(mjs_asLight(el)); + break; + case mjOBJ_SITE: + list.append(mjs_asSite(el)); + break; + default: + // this should never happen + throw pybind11::value_error( + "body.find_all supports the types: body, frame, geom, site, " + "light, camera."); + break; + } + el = mjs_nextChild(&body, el, true); + } + return list; // list of pointers, so they can be copied +} + PYBIND11_MODULE(_specs, m) { auto structs_m = py::module::import("mujoco._structs"); py::function mjmodel_from_spec_ptr = @@ -419,45 +461,31 @@ PYBIND11_MODULE(_specs, m) { mjsBody.def( "find_all", [](raw::MjsBody& self, mjtObj objtype) -> py::list { - py::list list; - raw::MjsElement* el = mjs_firstChild(&self, objtype, true); - std::string error = mjs_getError(mjs_getSpec(self.element)); - if (!el && !error.empty()) { - throw pybind11::value_error(error); + return FindAllImpl(self, objtype); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "find_all", + [](raw::MjsBody& self, std::string& name) -> py::list { + mjtObj objtype = mjOBJ_UNKNOWN; + if (name == "body") { + objtype = mjOBJ_BODY; + } else if (name == "frame") { + objtype = mjOBJ_FRAME; + } else if (name == "geom") { + objtype = mjOBJ_GEOM; + } else if (name == "site") { + objtype = mjOBJ_SITE; + } else if (name == "light") { + objtype = mjOBJ_LIGHT; + } else if (name == "camera") { + objtype = mjOBJ_CAMERA; + } else { + throw pybind11::value_error( + "body.find_all supports the types: body, frame, geom, site, " + "light, camera."); } - while (el) { - switch (objtype) { - case mjOBJ_BODY: - list.append(mjs_asBody(el)); - break; - case mjOBJ_CAMERA: - list.append(mjs_asCamera(el)); - break; - case mjOBJ_FRAME: - list.append(mjs_asFrame(el)); - break; - case mjOBJ_GEOM: - list.append(mjs_asGeom(el)); - break; - case mjOBJ_JOINT: - list.append(mjs_asJoint(el)); - break; - case mjOBJ_LIGHT: - list.append(mjs_asLight(el)); - break; - case mjOBJ_SITE: - list.append(mjs_asSite(el)); - break; - default: - // this should never happen - throw pybind11::value_error( - "body.find_all supports the types: body, frame, geom, site, " - "light, camera."); - break; - } - el = mjs_nextChild(&self, el, true); - } - return list; + return FindAllImpl(self, objtype); }, py::return_value_policy::reference_internal); mjsBody.def( diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index b5955e5c..450c1f78 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -611,7 +611,6 @@ class SpecsTest(absltest.TestCase): """ spec = mujoco.MjSpec.from_string(main_xml) bodytype = mujoco.mjtObj.mjOBJ_BODY - sitetype = mujoco.mjtObj.mjOBJ_SITE self.assertLen(spec.bodies, 5) self.assertEqual(spec.bodies[1].name, 'body1') self.assertEqual(spec.bodies[2].name, 'body2') @@ -620,23 +619,26 @@ class SpecsTest(absltest.TestCase): self.assertLen(spec.worldbody.find_all(bodytype), 4) self.assertLen(spec.bodies[1].find_all(bodytype), 2) self.assertLen(spec.bodies[3].find_all(bodytype), 1) - self.assertEqual(spec.worldbody.find_all(bodytype)[0].name, 'body1') - self.assertEqual(spec.worldbody.find_all(bodytype)[1].name, 'body2') - self.assertEqual(spec.worldbody.find_all(bodytype)[2].name, 'body3') - self.assertEqual(spec.worldbody.find_all(bodytype)[3].name, 'body4') - self.assertEqual(spec.bodies[1].find_all(bodytype)[0].name, 'body3') - self.assertEqual(spec.bodies[1].find_all(bodytype)[1].name, 'body4') - self.assertEqual(spec.bodies[3].find_all(bodytype)[0].name, 'body4') - self.assertEmpty(spec.bodies[2].find_all(bodytype)) - self.assertEmpty(spec.bodies[4].find_all(bodytype)) - self.assertEqual(spec.worldbody.find_all(sitetype)[0].name, 'site') + self.assertEqual(spec.worldbody.find_all('body')[0].name, 'body1') + self.assertEqual(spec.worldbody.find_all('body')[1].name, 'body2') + self.assertEqual(spec.worldbody.find_all('body')[2].name, 'body3') + self.assertEqual(spec.worldbody.find_all('body')[3].name, 'body4') + self.assertEqual(spec.bodies[1].find_all('body')[0].name, 'body3') + self.assertEqual(spec.bodies[1].find_all('body')[1].name, 'body4') + self.assertEqual(spec.bodies[3].find_all('body')[0].name, 'body4') + self.assertEmpty(spec.bodies[2].find_all('body')) + self.assertEmpty(spec.bodies[4].find_all('body')) + self.assertEqual(spec.worldbody.find_all('site')[0].name, 'site') with self.assertRaises(ValueError) as cm: - spec.worldbody.find_all(mujoco.mjtObj.mjOBJ_ACTUATOR) + spec.worldbody.find_all('actuator') self.assertEqual( str(cm.exception), - 'Error: Body.NextChild supports the types: body, frame, geom, site,' - ' light, camera\nElement name \'world\', id 0', + 'body.find_all supports the types: body, frame, geom, site,' + ' light, camera.', ) + body4 = spec.worldbody.find_all('body')[3] + body4.name = 'body4_new' + self.assertEqual(spec.bodies[4].name, 'body4_new') def test_iterators(self): spec = mujoco.MjSpec() From 2de430a61f4bb2ebd9c12d90e983cc35925fbea4 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Thu, 24 Oct 2024 15:34:07 -0700 Subject: [PATCH 009/426] Use newton by default in benchmark. PiperOrigin-RevId: 689538944 Change-Id: I99b5588ae5980eee43fb33ac390122ce38000dd3 --- mjx/mujoco/mjx/_src/test_util.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 349cab16..a5a3af01 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -53,7 +53,7 @@ def benchmark( nstep: int = 1000, batch_size: int = 1024, unroll_steps: int = 1, - solver: str = 'cg', + solver: str = 'newton', iterations: int = 1, ls_iterations: int = 4, ) -> Tuple[float, float, int]: From f24de91cc9d6724b838bafc7883a0d463d7f76c3 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Thu, 24 Oct 2024 16:07:28 -0700 Subject: [PATCH 010/426] Use eq_active in MJX. Fixes #2173. PiperOrigin-RevId: 689549368 Change-Id: I14d9817c5ca5dfc2a735ab8bacfb11b75c9feadc --- doc/changelog.rst | 1 + mjx/mujoco/mjx/_src/constraint.py | 38 +++++++++++++++++++------- mjx/mujoco/mjx/_src/constraint_test.py | 14 ++++++++-- 3 files changed, 41 insertions(+), 12 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 8f69bb07..109c54f9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,6 +15,7 @@ MJX ^^^ - Added ``apply_ft``, ``jac``, and ``xfrc_accumulate`` as public functions. - Added ``TOUCH`` sensor. +- Added support for ``eq_active``. Fixes :github:issue:`2173`. Bug fixes ^^^^^^^^^ diff --git a/mjx/mujoco/mjx/_src/constraint.py b/mjx/mujoco/mjx/_src/constraint.py index 26b684e8..158d9157 100644 --- a/mjx/mujoco/mjx/_src/constraint.py +++ b/mjx/mujoco/mjx/_src/constraint.py @@ -107,7 +107,9 @@ def _efc_equality_connect(m: Model, d: Data) -> Optional[_Efc]: return None @jax.vmap - def rows(is_site, obj1id, obj2id, body1id, body2id, data, solref, solimp): + def rows( + is_site, obj1id, obj2id, body1id, body2id, data, solref, solimp, active + ): anchor1, anchor2 = data[0:3], data[3:6] pos1 = d.xmat[body1id] @ anchor1 + d.xpos[body1id] @@ -128,7 +130,8 @@ def _efc_equality_connect(m: Model, d: Data) -> Optional[_Efc]: invweight = m.body_invweight0[body1id, 0] + m.body_invweight0[body2id, 0] zero = jp.zeros_like(pos) - return _row(j, pos, pos_imp, invweight, solref, solimp, zero, zero) + efc = _row(j, pos, pos_imp, invweight, solref, solimp, zero, zero) + return jax.tree_util.tree_map(lambda x: x * active, efc) is_site = m.eq_objtype == ObjType.SITE body1id = np.copy(m.eq_obj1id) @@ -147,6 +150,7 @@ def _efc_equality_connect(m: Model, d: Data) -> Optional[_Efc]: m.eq_data, m.eq_solref, m.eq_solimp, + d.eq_active, ) args = jax.tree_util.tree_map(lambda x: x[eq_id], args) # concatenate to drop row grouping @@ -161,7 +165,9 @@ def _efc_equality_weld(m: Model, d: Data) -> Optional[_Efc]: return None @jax.vmap - def rows(is_site, obj1id, obj2id, body1id, body2id, data, solref, solimp): + def rows( + is_site, obj1id, obj2id, body1id, body2id, data, solref, solimp, active + ): anchor1, anchor2 = data[0:3], data[3:6] relpose, torquescale = data[6:10], data[10] @@ -208,7 +214,8 @@ def _efc_equality_weld(m: Model, d: Data) -> Optional[_Efc]: invweight = jp.repeat(invweight, 3, axis=0) zero = jp.zeros_like(pos) - return _row(j, pos, pos_imp, invweight, solref, solimp, zero, zero) + efc = _row(j, pos, pos_imp, invweight, solref, solimp, zero, zero) + return jax.tree_util.tree_map(lambda x: x * active, efc) is_site = m.eq_objtype == ObjType.SITE body1id = np.copy(m.eq_obj1id) @@ -227,6 +234,7 @@ def _efc_equality_weld(m: Model, d: Data) -> Optional[_Efc]: m.eq_data, m.eq_solref, m.eq_solimp, + d.eq_active, ) args = jax.tree_util.tree_map(lambda x: x[eq_id], args) # concatenate to drop row grouping @@ -242,7 +250,9 @@ def _efc_equality_joint(m: Model, d: Data) -> Optional[_Efc]: return None @jax.vmap - def rows(obj2id, data, solref, solimp, dofadr1, dofadr2, qposadr1, qposadr2): + def rows( + obj2id, data, solref, solimp, active, dofadr1, dofadr2, qposadr1, qposadr2 + ): pos1, pos2 = d.qpos[qposadr1], d.qpos[qposadr2] ref1, ref2 = m.qpos0[qposadr1], m.qpos0[qposadr2] dif = (pos2 - ref2) * (obj2id > -1) @@ -255,9 +265,11 @@ def _efc_equality_joint(m: Model, d: Data) -> Optional[_Efc]: invweight += m.dof_invweight0[dofadr2] * (obj2id > -1) zero = jp.zeros_like(pos) - return _row(j, pos, pos, invweight, solref, solimp, zero, zero) + efc = _row(j, pos, pos, invweight, solref, solimp, zero, zero) + return jax.tree_util.tree_map(lambda x: x * active, efc) args = (m.eq_obj1id, m.eq_obj2id, m.eq_data, m.eq_solref, m.eq_solimp) + args += (d.eq_active,) args = jax.tree_util.tree_map(lambda x: x[eq_id], args) dofadr1, dofadr2 = m.jnt_dofadr[args[0]], m.jnt_dofadr[args[1]] qposadr1, qposadr2 = m.jnt_qposadr[args[0]], m.jnt_qposadr[args[1]] @@ -274,7 +286,7 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]: if (m.opt.disableflags & DisableBit.EQUALITY) or eq_id.size == 0: return None - obj1id, obj2id, data, solref, solimp = jax.tree_util.tree_map( + obj1id, obj2id, data, solref, solimp, active = jax.tree_util.tree_map( lambda x: x[eq_id], ( m.eq_obj1id, @@ -282,11 +294,14 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]: m.eq_data, m.eq_solref, m.eq_solimp, + d.eq_active, ), ) @jax.vmap - def rows(obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2): + def rows( + obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2, active + ): dif = pos2 * (obj2id > -1) dif_power = jp.power(dif, jp.arange(0, 5)) pos = pos1 - jp.dot(data[:5], dif_power) @@ -294,7 +309,8 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]: j = jac1 + jac2 * -deriv zero = jp.zeros_like(pos) - return _row(j, pos, pos, invweight, solref, solimp, zero, zero) + efc = _row(j, pos, pos, invweight, solref, solimp, zero, zero) + return jax.tree_util.tree_map(lambda x: x * active, efc) inv1, inv2 = m.tendon_invweight0[obj1id], m.tendon_invweight0[obj2id] jac1, jac2 = d.ten_J[obj1id], d.ten_J[obj2id] @@ -302,7 +318,9 @@ def _efc_equality_tendon(m: Model, d: Data) -> Optional[_Efc]: pos2 = d.ten_length[obj2id] - m.tendon_length0[obj2id] invweight = inv1 + inv2 * (obj2id > -1) - return rows(obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2) + return rows( + obj2id, data, solref, solimp, invweight, jac1, jac2, pos1, pos2, active + ) def _efc_friction(m: Model, d: Data) -> Optional[_Efc]: diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index ceb43147..76d9bc43 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -41,10 +41,17 @@ def _assert_attr_eq(a, b, attr): class ConstraintTest(parameterized.TestCase): + def setUp(self): + super().setUp() + np.random.seed(42) + @parameterized.parameters( - mujoco.mjtCone.mjCONE_PYRAMIDAL, mujoco.mjtCone.mjCONE_ELLIPTIC + {'cone': mujoco.mjtCone.mjCONE_PYRAMIDAL, 'rand_eq_active': False}, + {'cone': mujoco.mjtCone.mjCONE_ELLIPTIC, 'rand_eq_active': False}, + {'cone': mujoco.mjtCone.mjCONE_PYRAMIDAL, 'rand_eq_active': True}, + {'cone': mujoco.mjtCone.mjCONE_ELLIPTIC, 'rand_eq_active': True}, ) - def test_constraints(self, cone): + def test_constraints(self, cone, rand_eq_active): """Test constraints.""" m = test_util.load_test_file('constraints.xml') m.opt.cone = cone @@ -53,6 +60,8 @@ class ConstraintTest(parameterized.TestCase): # sample a mix of active/inactive constraints at different timesteps for key in range(3): mujoco.mj_resetDataKeyframe(m, d, key) + if rand_eq_active: + d.eq_active[:] = np.random.randint(0, 2, size=m.neq) mujoco.mj_forward(m, d) mx = mjx.put_model(m) dx = mjx.put_data(m, d) @@ -66,6 +75,7 @@ class ConstraintTest(parameterized.TestCase): _assert_eq(0, dx.efc_aref[order][d.nefc :], 'efc_aref') _assert_eq(d.efc_D, dx.efc_D[order][: d.nefc], 'efc_D') _assert_eq(d.efc_pos, dx.efc_pos[order][: d.nefc], 'efc_pos') + _assert_eq(dx.efc_pos[order][d.nefc:], 0, 'efc_pos') _assert_eq( d.efc_frictionloss, dx.efc_frictionloss[order][: d.nefc], From 455116466b972a208b72d32f33d30ea4299ba148 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 21 Oct 2024 11:25:28 +0200 Subject: [PATCH 011/426] Fix running tests out of a installed mujoco python package --- python/mujoco/specs_test.py | 9 +- python/mujoco/testdata/model.xml | 178 +++++++++++++++++++++++++++++++ 2 files changed, 183 insertions(+), 4 deletions(-) create mode 100644 python/mujoco/testdata/model.xml diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 84f10995..96f3d2fd 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -18,6 +18,7 @@ import inspect import textwrap from absl.testing import absltest +from etils import epath import mujoco import numpy as np @@ -433,10 +434,11 @@ class SpecsTest(absltest.TestCase): ) def test_load_xml(self): - filename = '../../test/testdata/model.xml' state_type = mujoco.mjtState.mjSTATE_INTEGRATION # Load from file. + file_path = epath.resource_path("mujoco") / "testdata" / "model.xml" + filename = file_path.as_posix() spec1 = mujoco.MjSpec.from_file(filename) model1 = spec1.compile() data1 = mujoco.MjData(model1) @@ -692,9 +694,8 @@ class SpecsTest(absltest.TestCase): mujoco.mjtGeom.mjGEOM_BOX) def test_delete(self): - filename = '../../test/testdata/model.xml' - - spec = mujoco.MjSpec.from_file(filename) + file_path = epath.resource_path("mujoco") / "testdata" / "model.xml" + spec = mujoco.MjSpec.from_file(file_path.as_posix()) model = spec.compile() self.assertIsNotNone(model) diff --git a/python/mujoco/testdata/model.xml b/python/mujoco/testdata/model.xml new file mode 100644 index 00000000..98274616 --- /dev/null +++ b/python/mujoco/testdata/model.xml @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 1201db8050b4800d436cce545ffd4dd723d7602b Mon Sep 17 00:00:00 2001 From: Silvio Date: Fri, 25 Oct 2024 09:33:17 +0200 Subject: [PATCH 012/426] Do not test python bindings in python/dist directory This will permit to early catch tests that rely on files not installed in the wheel, i.e. catch if pytest --pyargs mujoco fails in a vanilla environment. --- .github/workflows/build.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1f6364d9..b22215b6 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -239,7 +239,6 @@ jobs: - name: Test Python bindings if: ${{ runner.os != 'Windows' }} shell: bash - working-directory: python/dist env: MUJOCO_GL: disable run: > From 078c7bb42eda6a9678782ca67f7d4a8f5156e4c6 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 25 Oct 2024 15:28:29 -0700 Subject: [PATCH 013/426] internal change PiperOrigin-RevId: 689931872 Change-Id: Icc7b261b6d98e181a6bf0845f44a050af2833db2 --- mjx/mujoco/mjx/_src/collision_driver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/collision_driver.py b/mjx/mujoco/mjx/_src/collision_driver.py index 796ef3fa..129bcf2a 100644 --- a/mjx/mujoco/mjx/_src/collision_driver.py +++ b/mjx/mujoco/mjx/_src/collision_driver.py @@ -377,11 +377,11 @@ def collision(m: Model, d: Data) -> Data: if d.ncon == 0: return d - groups = _contact_groups(m, d) max_geom_pairs = _numeric(m, 'max_geom_pairs') max_contact_points = _numeric(m, 'max_contact_points') # run collision functions on groups + groups = _contact_groups(m, d) for key, contact in groups.items(): # determine which contacts we'll use for collision testing by running a # broad phase cull if requested From 7cf457286d045870dd46a0d23317a3be884abb5a Mon Sep 17 00:00:00 2001 From: Philipp Schmutz <2059887+pschmutz@users.noreply.github.com> Date: Sat, 26 Oct 2024 16:22:47 +0200 Subject: [PATCH 014/426] Print error instead of failing silently when loading plugins Previously the return value of dlopen was ignored, meaning a failed load of a shared library would stay unnoticed --- src/engine/engine_plugin.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_plugin.cc b/src/engine/engine_plugin.cc index 93ea4233..20a64bf1 100644 --- a/src/engine/engine_plugin.cc +++ b/src/engine/engine_plugin.cc @@ -387,7 +387,15 @@ void mj_loadPluginLibrary(const char* path) { #if defined(_WIN32) || defined(__CYGWIN__) LoadLibraryA(path); #else - dlopen(path, RTLD_NOW | RTLD_LOCAL); + void* handle = dlopen(path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + const char* error = dlerror(); + if (error) { + mju_error("Error loading plugin library '%s': %s\n", path, error); + } else { + mju_error("Unknown error loading plugin library '%s'\n", path); + } + } #endif } From 61cb552f8ad0ea377c831ac53dc559576f0e6908 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 26 Oct 2024 07:40:49 -0700 Subject: [PATCH 015/426] Refactor PBR texture layers from separate sub-elements to a single layer sub-element. Add new element to PyMJCF schema. PiperOrigin-RevId: 690115286 Change-Id: I7bb3f184cb321ca96037b2ca0ee160efef37469d --- doc/APIreference/APItypes.rst | 2 +- doc/XMLreference.rst | 166 +++++++++-------------------- doc/XMLschema.rst | 62 +---------- doc/changelog.rst | 5 +- src/xml/xml_native_reader.cc | 29 ++--- src/xml/xml_native_reader.h | 2 +- test/xml/xml_native_reader_test.cc | 14 +-- 7 files changed, 80 insertions(+), 200 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index fba959b2..3f11ee5e 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1143,7 +1143,7 @@ behavior. .. _mjsCompiler: mjsCompiler -~~~~~~~~~~ +~~~~~~~~~~~ Compiler options. diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8c3e72de..f2b030db 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1247,19 +1247,16 @@ The full list of processing steps applied by the compiler to each mesh is as fol :at:`inertia`: :at-val:`[convex, exact, legacy], "legacy"` This attribute controls how the mesh is used when mass and inertia are - :ref:`inferred from geometry<_compiler-inertiafromgeom>`. The current default value :at-val:`legacy` will be changed + :ref:`inferred from geometry`. The current default value :at-val:`legacy` will be changed to :at-val:`convex` in a future release. - :at-val:`convex` - Use the mesh's convex hull to compute volume and inertia. + :at-val:`convex`: Use the mesh's convex hull to compute volume and inertia. - :at-val:`exact` - Use an exact algorithm to compute volume and inertia. This algorithm requires a well-oriented, watertight mesh and - will error otherwise. + :at-val:`exact`: Use an exact algorithm to compute volume and inertia. This algorithm requires a well-oriented, + watertight mesh and will error otherwise. - :at-val:`legacy` - Use the legacy algorithm, which is similar to :at-val:`convex`, but leads to volume overcounting for non-convex - meshes. + :at-val:`legacy`: Use the legacy algorithm, which is similar to :at-val:`convex`, but leads to volume overcounting + for non-convex meshes. .. _asset-mesh-smoothnormal: @@ -1710,9 +1707,9 @@ properties are grouped together. loaded explicitly via the :ref:`texture ` element and then referenced here. The texture referenced here is used for specifying the RGB values. For advanced rendering (e.g., Physics-Based Rendering), more texture types need to be specified (e.g., roughness, metallic). In this case, this texture attribute should be omitted, and - the texture types should be specified explicitly via the specific role child elements, e.g., - :ref:`texture `. Note however that the built-in renderer does not support PBR properties, so these - advanced rendering features are only available when using an external renderer. + the texture types should be specified using :ref:`layer ` child elements. Note however that the + built-in renderer does not support PBR properties, so these advanced rendering features are only available when using + an external renderer. .. _asset-material-texrepeat: @@ -1784,116 +1781,59 @@ properties are grouped together. model element which defines its own local rgba attribute, the local definition has precedence. Note that this "local" definition could in fact come from a defaults class. The remaining material properties always apply. -.. _material-rgb: +.. _material-layer: -:el-prefix:`material/` |-| **rgb** (?) -'''''''''''''''''''''''''''''''''''''' +:el-prefix:`material/` |-| **layer** (?) +'''''''''''''''''''''''''''''''''''''''' -This element references a texture asset used to specify base color / albedo values. +If multiple textures are needed to specify the appearance of a material, the :ref:`texture ` +attribute cannot be used, and :el:`layer` child elements must be used instead. Specifying both the :at:`texture` +attribute and :el:`layer` child elements is an error. -.. _material-rgb-texture: +.. _material-layer-texture: :at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly 3 channels. + Name of the texture, like the :ref:`texture ` attribute. -.. _material-normal: +.. _material-layer-role: -:el-prefix:`material/` |-| **normal** (?) -''''''''''''''''''''''''''''''''''''''''' +:at:`role`: :at-val:`string, required` + Role of the texture. The valid values, expected number of channels, and the role semantics are: -This element references a texture asset used to specify the bump map (surface normals). - -.. _material-normal-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly 3 channels. - -.. _material-occlusion: - -:el-prefix:`material/` |-| **occlusion** (?) -'''''''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify ambient occlusion. - -.. _material-occlusion-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly one channel. - -.. _material-roughness: - -:el-prefix:`material/` |-| **roughness** (?) -'''''''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify the roughness map. - -.. _material-roughness-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly one channel. - -.. _material-metallic: - -:el-prefix:`material/` |-| **metallic** (?) -''''''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify the metallic map. - -.. _material-metallic-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly one channel. - -.. _material-opacity: - -:el-prefix:`material/` |-| **opacity** (?) -'''''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify the opacity map (alpha channel, transparency). - -.. _material-opacity-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly one channel. - -.. _material-emissive: - -:el-prefix:`material/` |-| **emissive** (?) -''''''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify light emission. - -.. _material-emissive-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly 4 channels. - -.. _material-orm: - -:el-prefix:`material/` |-| **orm** (?) -'''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify a packed ORM map, where occlusion, roughness, and metallic -are joined into the corresponding RGB values of a single texture. - -.. _material-orm-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly 3 channels. - -.. _material-rgba: - -:el-prefix:`material/` |-| **rgba** (?) -''''''''''''''''''''''''''''''''''''''' - -This element references a texture asset used to specify a packed map where albedo and opacity are joined into the same -4-channel texture. - -.. _material-rgba-texture: - -:at:`texture`: :at-val:`string, required` - Name of the texture, expected to have exactly 4 channels. + .. list-table:: + :widths: 1 1 8 + :header-rows: 1 + * - value + - channels + - description + * - :at:`rgb` + - 3 + - base color / albedo [red, green, blue] + * - :at:`normal` + - 3 + - bump map (surface normals) + * - :at:`occlusion` + - 1 + - ambient occlusion + * - :at:`roughness` + - 1 + - roughness + * - :at:`metallic` + - 1 + - metallicity + * - :at:`opacity` + - 1 + - opacity (alpha channel) + * - :at:`emissive` + - 4 + - RGB light emmision intensity, exposure weight in 4th channel + * - :at:`orm` + - 3 + - packed 3 channel [occlusion, roughness, metallic] + * - :at:`rgba` + - 4 + - packed 4 channel [red, green, blue, alpha] .. _asset-model: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 4b45b68b..72df2be3 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -178,66 +178,10 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`rgb | ? | :class: mjcf-attributes | -| ` | | | +| :ref:`layer | \* | :class: mjcf-attributes | +| ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`occlusion | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`roughness | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`metallic | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`normal | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`opacity | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`emissive | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`rgba | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| |_2| material |br| |_2| |L| | | .. table:: | -| :ref:`orm | ? | :class: mjcf-attributes | -| ` | | | -| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`texture` | | | | | +| | | | :ref:`texture` | :ref:`role` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| asset |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 109c54f9..920cff46 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,9 @@ General - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. - Removed the deprecated ``mju_rotVecMat`` and ``mju_rotVecMatT`` functions. +- Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). +- The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single + :ref:`layer` sub-element. MJX ^^^ @@ -169,7 +172,7 @@ General 2. Added a new :ref:`autoreset` flag to disable automatic reset when NaNs or infinities are detected. 3. Added sub-elements to the MJCF :ref:`material` element, to allow specification of multiple textures - for rendering (e.g., :ref:`occlusion-roughness-metallic`). Note that the MuJoCo renderer doesn't + for rendering (e.g., ``occlusion, roughness, metallic``). Note that the MuJoCo renderer doesn't support these new features, and they are made available for use with external renderers. 4. Sorting (``mjQUICKSORT``) now calls ``std::sort`` when building with C++ (:github:issue:`1638`). diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 854459cb..6fc675c5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -247,15 +247,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"material", "*", "12", "name", "class", "texture", "texrepeat", "texuniform", "emission", "specular", "shininess", "reflectance", "metallic", "roughness", "rgba"}, {"<"}, - {"rgb", "?", "1", "texture"}, - {"occlusion", "?", "1", "texture"}, - {"roughness", "?", "1", "texture"}, - {"metallic", "?", "1", "texture"}, - {"normal", "?", "1", "texture"}, - {"opacity", "?", "1", "texture"}, - {"emissive", "?", "1", "texture"}, - {"rgba", "?", "1", "texture"}, - {"orm", "?", "1", "texture"}, + {"layer", "*", "2", "texture", "role"}, {">"}, {"model", "*", "3", "name", "file", "content_type"}, {">"}, @@ -1590,17 +1582,18 @@ void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* material) { tex_attributes_found = true; } - XMLElement* tex_elem = FirstChildElement(elem); - while (tex_elem) { + XMLElement* layer = FirstChildElement(elem); + while (layer) { if (tex_attributes_found) { - throw mjXError(tex_elem, "A material with a texture attribute cannot have texture sub-elements"); + throw mjXError(layer, "A material with a texture attribute cannot have layer sub-elements"); } - // texture sub-element - int role = FindKey(texrole_map, texrole_sz, tex_elem->Name()); - string texmat; - ReadAttrTxt(tex_elem, "texture", texmat, true); - mjs_setInStringVec(material->textures, role, texmat.c_str()); - tex_elem = NextSiblingElement(tex_elem); + + // layer sub-element + ReadAttrTxt(layer, "role", text, true); + int role = FindKey(texrole_map, texrole_sz, text); + ReadAttrTxt(layer, "texture", text, true); + mjs_setInStringVec(material->textures, role, text.c_str()); + layer = NextSiblingElement(layer); } if (MapValue(elem, "texuniform", &n, bool_map, 2)) { diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index c9f456bf..62488b97 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -101,7 +101,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 245 +#define nMJCF 237 extern const char* MJCF[nMJCF][mjXATTRNUM]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 8f902565..4eed4735 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -805,10 +805,10 @@ TEST_F(XMLReaderTest, MaterialTextureTest) { - - - - + + + + @@ -873,8 +873,8 @@ TEST_F(XMLReaderTest, MaterialTextureFailTest) { - - + + @@ -887,7 +887,7 @@ TEST_F(XMLReaderTest, MaterialTextureFailTest) { mjModel* m = LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(m, IsNull()); EXPECT_THAT(error.data(), HasSubstr("A material with a texture attribute " - "cannot have texture sub-elements")); + "cannot have layer sub-elements")); } TEST_F(XMLReaderTest, LargeTextureTest) { From 7dc8aef807792ff214d230b5fd6e4bbe1e914b6c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 27 Oct 2024 04:30:12 -0700 Subject: [PATCH 016/426] Remove the `convexhull` compiler option. PiperOrigin-RevId: 690307675 Change-Id: I62d8213579ad07296ca96de004d39ae012174aa4 --- doc/XMLreference.rst | 14 +-- doc/XMLschema.rst | 4 +- doc/changelog.rst | 1 + doc/includes/references.h | 1 - include/mujoco/mjspec.h | 1 - introspect/structs.py | 5 - src/user/user_init.c | 1 - src/user/user_mesh.cc | 2 +- src/xml/xml_base.h | 1 - src/xml/xml_native_reader.cc | 7 +- src/xml/xml_native_writer.cc | 3 - .../testdata/collision_driver/midphase.xml | 2 +- test/user/user_mesh_test.cc | 102 +++++++++--------- unity/Runtime/Bindings/MjBindings.cs | 1 - 14 files changed, 58 insertions(+), 87 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index f2b030db..523b0d43 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -772,23 +772,11 @@ has any effect. The settings here are global and apply to the entire model. models compiled with this flag, it is important to remember that collision geoms are often placed in a :ref:`group` which is invisible by default. -.. _compiler-convexhull: - -:at:`convexhull`: :at-val:`[false, true], "true"` - If this attribute is "true", the compiler will automatically generate a convex hull for every mesh that is used in at - least one non-visual geom (in the sense of the discardvisual attribute above). This is done to speed up collision - detection; recall :ref:`Collision` section in the Computation chapter. Even if the mesh is already convex, the hull - contains edge information that is not present in the mesh file, so it needs to be constructed. The only reason to - disable this feature is to speed up re-loading of a model with large meshes during model editing (since the convex - hull computation is the slowest operation performed by the compiler). However once model design is finished, this - feature should be enabled, because the availability of convex hulls substantially speeds up collision detection with - large meshes. - .. _compiler-usethread: :at:`usethread`: :at-val:`[false, true], "true"` If this attribute is "true", the model compiler will run in multi-threaded mode. Currently multi-threading is used - for computing the length ranges of actuators and for loading meshes. + for computing the length ranges of actuators and for parallel loading of meshes. .. _compiler-fusestatic: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 72df2be3..caf07b20 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -52,9 +52,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`fitaabb` | :ref:`eulerseq` | :ref:`meshdir` | :ref:`texturedir` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`discardvisual` | :ref:`convexhull` | :ref:`usethread` | :ref:`fusestatic` | | +| | | | :ref:`discardvisual` | :ref:`usethread` | :ref:`fusestatic` | :ref:`inertiafromgeom` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`inertiafromgeom` | :ref:`inertiagrouprange` | :ref:`assetdir` | :ref:`alignfree` | | +| | | | :ref:`inertiagrouprange` | :ref:`assetdir` | :ref:`alignfree` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| compiler |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 920cff46..6aa3b745 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,7 @@ General ^^^^^^^ - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. +- The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. - Removed the deprecated ``mju_rotVecMat`` and ``mju_rotVecMatT`` functions. - Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). - The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single diff --git a/doc/includes/references.h b/doc/includes/references.h index e19503d4..e8b6f6e7 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1693,7 +1693,6 @@ typedef struct mjsCompiler_ { // compiler options mjtByte degree; // angles in radians or degrees char eulerseq[3]; // sequence for euler rotations mjtByte discardvisual; // discard visual geoms in parser - mjtByte convexhull; // compute mesh convex hulls mjtByte usethread; // use multiple threads to speed up compiler mjtByte fusestatic; // fuse static bodies with parent int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 58feb8f8..2dc3177b 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -131,7 +131,6 @@ typedef struct mjsCompiler_ { // compiler options mjtByte degree; // angles in radians or degrees char eulerseq[3]; // sequence for euler rotations mjtByte discardvisual; // discard visual geoms in parser - mjtByte convexhull; // compute mesh convex hulls mjtByte usethread; // use multiple threads to speed up compiler mjtByte fusestatic; // fuse static bodies with parent int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) diff --git a/introspect/structs.py b/introspect/structs.py index 9f8c8b4c..0776273e 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -8853,11 +8853,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtByte'), doc='discard visual geoms in parser', ), - StructFieldDecl( - name='convexhull', - type=ValueType(name='mjtByte'), - doc='compute mesh convex hulls', - ), StructFieldDecl( name='usethread', type=ValueType(name='mjtByte'), diff --git a/src/user/user_init.c b/src/user/user_init.c index b5d15413..08fceb59 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -39,7 +39,6 @@ void mjs_defaultSpec(mjSpec* spec) { spec->compiler.eulerseq[0] = 'x'; spec->compiler.eulerseq[1] = 'y'; spec->compiler.eulerseq[2] = 'z'; - spec->compiler.convexhull = 1; spec->compiler.usethread = 1; spec->compiler.inertiafromgeom = mjINERTIAFROMGEOM_AUTO; spec->compiler.inertiagrouprange[1] = mjNGROUP-1; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index d1e20f46..695de30f 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -563,7 +563,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { } // make graph describing convex hull - if ((model->compiler.convexhull && needhull_) || face_.empty()) { + if (needhull_ || face_.empty()) { MakeGraph(); } diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index a022831c..bf2c92d6 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -42,7 +42,6 @@ extern const int gain_sz; extern const int bias_sz; extern const int stage_sz; extern const int datatype_sz; -extern const mjMap coordinate_map[]; extern const mjMap angle_map[]; extern const mjMap enable_map[]; extern const mjMap bool_map[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6fc675c5..6a70d13e 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -97,9 +97,9 @@ static void UpdateString(string& psuffix, int count, int i) { const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, - {"compiler", "*", "20", "autolimits", "boundmass", "boundinertia", "settotalmass", + {"compiler", "*", "19", "autolimits", "boundmass", "boundinertia", "settotalmass", "balanceinertia", "strippath", "coordinate", "angle", "fitaabb", "eulerseq", - "meshdir", "texturedir", "discardvisual", "convexhull", "usethread", + "meshdir", "texturedir", "discardvisual", "usethread", "fusestatic", "inertiafromgeom", "inertiagrouprange", "assetdir", "alignfree"}, {"<"}, {"lengthrange", "?", "10", "mode", "useexisting", "uselimit", @@ -1008,9 +1008,6 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { if (MapValue(section, "discardvisual", &n, bool_map, 2)) { spec->compiler.discardvisual = (n==1); } - if (MapValue(section, "convexhull", &n, bool_map, 2)) { - spec->compiler.convexhull = (n==1); - } if (MapValue(section, "usethread", &n, bool_map, 2)) { spec->compiler.usethread = (n==1); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 907920a6..e9a47920 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -920,9 +920,6 @@ void mjXWriter::Compiler(XMLElement* root) { XMLElement* section = InsertEnd(root, "compiler"); // settings - if (!model->compiler.convexhull) { - WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->compiler.convexhull)); - } WriteAttrTxt(section, "angle", "radian"); if (!model->get_meshdir().empty()) { WriteAttrTxt(section, "meshdir", model->get_meshdir()); diff --git a/test/engine/testdata/collision_driver/midphase.xml b/test/engine/testdata/collision_driver/midphase.xml index 3939de92..49bc5dbf 100644 --- a/test/engine/testdata/collision_driver/midphase.xml +++ b/test/engine/testdata/collision_driver/midphase.xml @@ -1,6 +1,6 @@ - + )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -458,8 +457,9 @@ TEST_F(MjCMeshTest, FaceNormalAutogenerated) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -480,9 +480,9 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -510,18 +510,18 @@ TEST_F(MjCMeshTest, TinyInertiaFails) { TEST_F(MjCMeshTest, FlippedFaceAllowedLegacyInertia) { const std::string xml_path = GetTestDataFilePath(kMalformedFaceOBJPath); - std::array error; - mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(model, testing::NotNull()) << error.data(); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(model->nmeshface, 4); mj_deleteModel(model); } TEST_F(MjCMeshTest, MissingFaceAllowedConvexInertia) { const std::string xml_path = GetTestDataFilePath(kCompareInertiaPath); - std::array error; - mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(model, testing::NotNull()) << error.data(); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(model->nmeshface, 10); EXPECT_THAT(model->body_inertia[3], model->body_inertia[9]); EXPECT_THAT(model->body_inertia[4], model->body_inertia[10]); @@ -583,9 +583,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedWorld) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; CheckTetrahedronWasRescaled(model); mj_deleteModel(model); } @@ -605,9 +605,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; CheckTetrahedronWasRescaled(model); mj_deleteModel(model); } @@ -628,9 +628,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; CheckTetrahedronWasRescaled(model); mj_deleteModel(model); } @@ -650,9 +650,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; CheckTetrahedronWasRescaled(model); mj_deleteModel(model); } @@ -706,9 +706,9 @@ TEST_F(MjCMeshTest, AreaTooSmallAllowedWorld) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -736,10 +736,9 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { static constexpr char xml[] = R"( - @@ -749,9 +748,9 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; EXPECT_LE(mju_abs(model->geom_size[0]), 1); EXPECT_LE(mju_abs(model->geom_size[1]), 1); EXPECT_LE(mju_abs(model->geom_size[2]), 1); @@ -815,9 +814,9 @@ TEST_F(MjCMeshTest, VolumeTooSmallAllowedWorld) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -895,8 +894,9 @@ TEST_F(MjCMeshTest, MeshPosQuat) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; // Loading the mesh results in an offset of the geom's pos and quat due to the // fact that the geom's center is not the volumetric center of the mesh. To // recover the geom's originally specified pose, the offset used is stored in @@ -953,8 +953,9 @@ TEST_F(MjCMeshTest, MeshScale) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; 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)); @@ -972,9 +973,9 @@ TEST_F(MjCMeshTest, CreateFaceTexCoord) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; mj_deleteModel(model); } @@ -1057,13 +1058,10 @@ TEST_F(MjCMeshTest, InvalidIndexInFace) { )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, IsNull()); - EXPECT_THAT( - error.data(), - HasSubstr( - "in face 0, vertex index 6 does not exist")); + EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist")); mj_deleteModel(model); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4eab533b..f456e3f6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5727,7 +5727,6 @@ public unsafe struct mjsCompiler_ { public byte degree; public fixed sbyte eulerseq[3]; public byte discardvisual; - public byte convexhull; public byte usethread; public byte fusestatic; public int inertiafromgeom; From 1c424644dd68875dba95e973e171b358c4f0f5a4 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 28 Oct 2024 03:38:52 -0700 Subject: [PATCH 017/426] Fix MJX touch sensor. PiperOrigin-RevId: 690544118 Change-Id: Ic909c6f6ce0e31237cbe317fbbfbb1fd267bd462 --- mjx/mujoco/mjx/_src/sensor.py | 21 ++++++++--------- mjx/mujoco/mjx/_src/sensor_test.py | 3 --- mjx/mujoco/mjx/test_data/sensor/sensor.xml | 27 ++++++++++++++-------- 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/mjx/mujoco/mjx/_src/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index e514bdd2..5d040052 100644 --- a/mjx/mujoco/mjx/_src/sensor.py +++ b/mjx/mujoco/mjx/_src/sensor.py @@ -461,7 +461,7 @@ def sensor_acc(m: Model, d: Data) -> Data: force, condim_id = support.contact_force_dim(m, d, dim) forces.append(force) condim_ids.append(condim_id) - forces = jp.concatenate(forces)[jp.concatenate(condim_ids)] + forces = jp.concatenate(forces)[np.argsort(np.concatenate(condim_ids))] # get bodies of contact geoms conbody = jp.array(m.geom_bodyid)[d.contact.geom] @@ -483,14 +483,14 @@ def sensor_acc(m: Model, d: Data) -> Data: conray = jp.where(conbody1[..., None], -conray, conray) # compute distance, mapping over sites and contacts - def _distance( - site_size, site_xpos, site_xmat, site_type, contact_pos, conray - ): - return jax.vmap( - lambda site_size, site_xpos, site_xmat, conray: jax.vmap( - lambda pnt, vec: ray.ray_geom(site_size, pnt, vec, site_type) - )((contact_pos - site_xpos) @ site_xmat, conray @ site_xmat) - )(site_size, site_xpos, site_xmat, conray) + def _distance(site_size, site_xpos, site_xmat, site_type, pos, conray): + def dist(size, xpos, xmat, conray): + pnt = (pos - xpos) @ xmat + vec = conray @ xmat + ray_geom_ = lambda pnt, vec: ray.ray_geom(size, pnt, vec, site_type) + return jax.vmap(ray_geom_)(pnt, vec) + + return jax.vmap(dist)(site_size, site_xpos, site_xmat, conray) dist = [] dist_id = [] @@ -506,8 +506,7 @@ def sensor_acc(m: Model, d: Data) -> Data: ) dist.append(jp.where(jp.isinf(dist_site), 0, dist_site)) dist_id.append(dist_id_site) - - dist = jp.vstack(dist)[np.concatenate(dist_id)] + dist = jp.vstack(dist)[np.argsort(np.concatenate(dist_id))] # accumulate normal forces for each site sensor = jp.dot((dist > 0) & contacts, forces[:, 0]) diff --git a/mjx/mujoco/mjx/_src/sensor_test.py b/mjx/mujoco/mjx/_src/sensor_test.py index 9eedac13..19d4ef46 100644 --- a/mjx/mujoco/mjx/_src/sensor_test.py +++ b/mjx/mujoco/mjx/_src/sensor_test.py @@ -101,17 +101,14 @@ class SensorTest(parameterized.TestCase): - - - """) diff --git a/mjx/mujoco/mjx/test_data/sensor/sensor.xml b/mjx/mujoco/mjx/test_data/sensor/sensor.xml index c8257831..71532743 100644 --- a/mjx/mujoco/mjx/test_data/sensor/sensor.xml +++ b/mjx/mujoco/mjx/test_data/sensor/sensor.xml @@ -102,15 +102,22 @@ - - - - - + + + + - - + + + + + + + + + + @@ -141,7 +148,8 @@ - + + @@ -152,7 +160,8 @@ - + + From a36f2cccb6218c5d069e21236f24d3bb21110778 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 28 Oct 2024 03:46:00 -0700 Subject: [PATCH 018/426] Add nnz argument for size of allocated memory to `mju_dense2sparse` and check if this number is too small for number of non-zeros. PiperOrigin-RevId: 690545825 Change-Id: I0e31cb907a1151cb2e4766d5f0cdfecb5629a2a6 --- src/engine/engine_util_sparse.c | 15 ++++++-- src/engine/engine_util_sparse.h | 5 +-- test/engine/engine_island_test.cc | 10 +++--- test/engine/engine_util_sparse_test.cc | 48 ++++++++++++++++++++++---- 4 files changed, 63 insertions(+), 15 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 89403bca..f6d05cfd 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -148,8 +148,13 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const in // convert matrix from dense to sparse -void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, - int* rownnz, int* rowadr, int* colind) { +// nnz is size of res and colind, return 1 if too small, 0 otherwise +int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, + int* rownnz, int* rowadr, int* colind, int nnz) { + if (nnz <= 0) { + return 1; + } + int adr = 0; // find non-zeros and construct sparse @@ -161,6 +166,11 @@ void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, // find non-zeros for (int c=0; c < nc; c++) { if (mat[r*nc+c]) { + // check for out of bounds + if (adr >= nnz) { + return 1; + } + // record index and count colind[adr] = c; rownnz[r]++; @@ -170,6 +180,7 @@ void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, } } } + return 0; } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index ae14cf70..fa2647c3 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -34,8 +34,9 @@ MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, co int nnz2, const int* ind2, int flg_unc2); // convert matrix from dense to sparse -MJAPI void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, - int* rownnz, int* rowadr, int* colind); +// nnz is size of res and colind, return 1 if too small, 0 otherwise +MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, + int* rownnz, int* rowadr, int* colind, int nnz); // convert matrix from sparse to dense MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index c0f5afb8..3729e3f0 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -55,7 +55,7 @@ TEST_F(IslandTest, FloodFillSingleton) { int rowadr[nr]; int colind[nnz]; mjtNum res[nnz]; // unused - mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind); + mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind, nnz); // outputs / scratch int island[nr]; @@ -82,7 +82,7 @@ TEST_F(IslandTest, FloodFill1) { int rowadr[nr]; int colind[nnz]; mjtNum res[nnz]; // unused - mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind); + mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind, nnz); // outputs / stack int island[nr]; @@ -112,7 +112,7 @@ TEST_F(IslandTest, FloodFill2) { int rowadr[nr]; int colind[nnz]; mjtNum res[nnz]; // unused - mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind); + mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind, nnz); // outputs / stack int island[nr]; @@ -140,7 +140,7 @@ TEST_F(IslandTest, FloodFill3a) { int rowadr[nr]; int colind[nnz]; mjtNum res[nnz]; // unused - mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind); + mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind, nnz); // outputs / stack int island[nr]; @@ -174,7 +174,7 @@ TEST_F(IslandTest, FloodFill3b) { int rowadr[nr]; int colind[nnz]; mjtNum res[nnz]; // unused - mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind); + mju_dense2sparse(res, mat, nr, nr, rownnz, rowadr, colind, nnz); // outputs / stack int island[nr]; diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 92b8fc8d..ade96c0b 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -984,7 +984,7 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int rowadrA[2]; int colindA[4]; int rownnzA_factor[2]; - mju_dense2sparse(sparseA, matA, nA, nA, rownnzA, rowadrA, colindA); + mju_dense2sparse(sparseA, matA, nA, nA, rownnzA, rowadrA, colindA, 4); int nnzA = mju_cholFactorNNZ(rownnzA_factor, rownnzA, rowadrA, colindA, nA, d); @@ -1000,7 +1000,7 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int rowadrB[3]; int colindB[9]; int rownnzB_factor[3]; - mju_dense2sparse(sparseB, matB, nB, nB, rownnzB, rowadrB, colindB); + mju_dense2sparse(sparseB, matB, nB, nB, rownnzB, rowadrB, colindB, 9); int nnzB = mju_cholFactorNNZ(rownnzB_factor, rownnzB, rowadrB, colindB, nB, d); @@ -1016,7 +1016,7 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int rowadrC[3]; int colindC[9]; int rownnzC_factor[3]; - mju_dense2sparse(sparseC, matC, nC, nC, rownnzC, rowadrC, colindC); + mju_dense2sparse(sparseC, matC, nC, nC, rownnzC, rowadrC, colindC, 9); int nnzC = mju_cholFactorNNZ(rownnzC_factor, rownnzC, rowadrC, colindC, nC, d); @@ -1033,7 +1033,7 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int rowadrD[4]; int colindD[16]; int rownnzD_factor[4]; - mju_dense2sparse(sparseD, matD, nD, nD, rownnzD, rowadrD, colindD); + mju_dense2sparse(sparseD, matD, nD, nD, rownnzD, rowadrD, colindD, 16); int nnzD = mju_cholFactorNNZ(rownnzD_factor, rownnzD, rowadrD, colindD, nD, d); @@ -1050,11 +1050,11 @@ TEST_F(EngineUtilSparseTest, MjuMulMatTVec) { mjtNum mat[] = {1, 2, 0, 0, 3, 4}; - mjtNum mat_sparse[6]; + mjtNum mat_sparse[4]; int rownnz[2]; int rowadr[2]; int colind[4]; - mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind); + mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind, 4); // multiply: res = mat' * vec mjtNum vec[] = {5, 6}; @@ -1064,5 +1064,41 @@ TEST_F(EngineUtilSparseTest, MjuMulMatTVec) { EXPECT_THAT(AsVector(res, 3), ElementsAre(5, 28, 24)); } +TEST_F(EngineUtilSparseTest, MjuDenseToSparse) { + int nr = 2; + int nc = 2; + mjtNum mat[] = {1, 2, + 0, 3}; + + mjtNum mat_sparse[4]; + int rownnz[2]; + int rowadr[2]; + int colind[4]; + + // nnz == number of non-zeros + int status3 = + mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind, 3); + + EXPECT_EQ(status3, 0); + + // nnz > number of non-zeros + int status4 = + mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind, 4); + + EXPECT_EQ(status4, 0); + + // nnz < number of non-zeros + int status2 = + mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind, 2); + + EXPECT_EQ(status2, 1); + + // nnz == 0 + int status0 = + mju_dense2sparse(mat_sparse, mat, nr, nc, rownnz, rowadr, colind, 0); + + EXPECT_EQ(status0, 1); +} + } // namespace } // namespace mujoco From 3c21abc0e544bfafb140f8ef5506f4fa8f6aa616 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 28 Oct 2024 04:24:11 -0700 Subject: [PATCH 019/426] Add ray intersection with ellipsoid to MJX. PiperOrigin-RevId: 690557087 Change-Id: Icd2f0be1243574720baeac82b8bdd92e57c6a61c --- doc/changelog.rst | 1 + mjx/mujoco/mjx/_src/ray.py | 24 ++++++++++++++++++++++ mjx/mujoco/mjx/test_data/sensor/sensor.xml | 6 ++++++ 3 files changed, 31 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index 6aa3b745..436ffce3 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -20,6 +20,7 @@ MJX - Added ``apply_ft``, ``jac``, and ``xfrc_accumulate`` as public functions. - Added ``TOUCH`` sensor. - Added support for ``eq_active``. Fixes :github:issue:`2173`. +- Added ray intersection with ellipsoid. Bug fixes ^^^^^^^^^ diff --git a/mjx/mujoco/mjx/_src/ray.py b/mjx/mujoco/mjx/_src/ray.py index 2591385a..9335f4e9 100644 --- a/mjx/mujoco/mjx/_src/ray.py +++ b/mjx/mujoco/mjx/_src/ray.py @@ -108,6 +108,29 @@ def _ray_capsule( return x +def _ray_ellipsoid( + size: jax.Array, + pnt: jax.Array, + vec: jax.Array, +) -> jax.Array: + """Returns the distance at which a ray intersects with an ellipsoid.""" + + # invert size^2 + s = 1 / jp.square(size) + + # (x*lvec+lpnt)' * diag(1/size^2) * (x*lvec+lpnt) = 1 + svec = s * vec + a = svec @ vec + b = svec @ pnt + c = (s * pnt) @ pnt - 1 + + # solve a*x^2 + 2*b*x + c = 0 + x0, x1 = _ray_quad(a, b, c) + x = jp.where(jp.isinf(x0), x1, x0) + + return x + + def _ray_box( size: jax.Array, pnt: jax.Array, @@ -201,6 +224,7 @@ _RAY_FUNC = { GeomType.PLANE: _ray_plane, GeomType.SPHERE: _ray_sphere, GeomType.CAPSULE: _ray_capsule, + GeomType.ELLIPSOID: _ray_ellipsoid, GeomType.BOX: _ray_box, GeomType.MESH: _ray_mesh, } diff --git a/mjx/mujoco/mjx/test_data/sensor/sensor.xml b/mjx/mujoco/mjx/test_data/sensor/sensor.xml index 71532743..b6d3fa2a 100644 --- a/mjx/mujoco/mjx/test_data/sensor/sensor.xml +++ b/mjx/mujoco/mjx/test_data/sensor/sensor.xml @@ -119,6 +119,11 @@ + + + + + @@ -178,6 +183,7 @@ + From d8494fef3bc67bb7f0029d7dd5ad3f73418d6d65 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 28 Oct 2024 04:50:41 -0700 Subject: [PATCH 020/426] Remove deprecated mjv_makeConnector function. PiperOrigin-RevId: 690563333 Change-Id: Ice9c6b6cf41de8a55fa361477f9aee9181d50184 --- doc/APIreference/functions.rst | 12 -------- doc/changelog.rst | 4 +-- doc/includes/references.h | 3 -- doc/python.rst | 2 +- include/mujoco/mujoco.h | 8 ----- introspect/functions.py | 46 ---------------------------- python/mujoco/functions.cc | 1 - python/tutorial.ipynb | 9 +++--- src/engine/engine_vis_visualize.c | 26 ++++++---------- src/engine/engine_vis_visualize.h | 6 ---- unity/Runtime/Bindings/MjBindings.cs | 3 -- 11 files changed, 17 insertions(+), 103 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 38568b35..878bbf00 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -2107,18 +2107,6 @@ mjv_initGeom Initialize given geom fields when not NULL, set the rest to their default values. -.. _mjv_makeConnector: - -mjv_makeConnector -~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjv_makeConnector - -Set (type, size, pos, mat) for connector-type geom between given points. -Assume that mjv_initGeom was already called to set all other properties. -Width of mjGEOM_LINE is denominated in pixels. -Deprecated: use mjv_connector. - .. _mjv_connector: mjv_connector diff --git a/doc/changelog.rst b/doc/changelog.rst index 436ffce3..f3c9b25e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,7 +10,7 @@ General - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. - The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. -- Removed the deprecated ``mju_rotVecMat`` and ``mju_rotVecMatT`` functions. +- Removed the deprecated ``mju_rotVecMat``, ``mju_rotVecMatT`` and ``mjv_makeConnector`` functions. - Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). - The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single :ref:`layer` sub-element. @@ -892,7 +892,7 @@ General #. Added analytic derivatives for quaternion :ref:`subtraction` and :ref:`integration` (rotation with an angular velocity). Derivatives are in the 3D tangent space. #. Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient - "from-to" argument parametrization. :ref:`mjv_makeConnector` is now deprecated. + "from-to" argument parametrization. ``mjv_makeConnector`` is now deprecated. #. Bumped oldest supported MacOS from version 10.12 to 11. MacOS 11 is the oldest version still maintained by Apple. Python bindings diff --git a/doc/includes/references.h b/doc/includes/references.h index e8b6f6e7..9a777763 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3337,9 +3337,6 @@ void mjv_defaultOption(mjvOption* opt); void mjv_defaultFigure(mjvFigure* fig); void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3], const mjtNum pos[3], const mjtNum mat[9], const float rgba[4]); -void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, - mjtNum a0, mjtNum a1, mjtNum a2, - mjtNum b0, mjtNum b1, mjtNum b2); void mjv_connector(mjvGeom* geom, int type, mjtNum width, const mjtNum from[3], const mjtNum to[3]); void mjv_defaultScene(mjvScene* scn); diff --git a/doc/python.rst b/doc/python.rst index e3e8d2e8..cef23eb5 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -126,7 +126,7 @@ attributes: - ``user_scn``: an :ref:`mjvScene` object that allows users to add change rendering flags and add custom visualization geoms to the rendered scene. This is separate from the ``mjvScene`` that the viewer uses internally to render the final scene, and is entirely under the user's control. User scripts can call e.g. :ref:`mjv_initGeom` or - :ref:`mjv_makeConnector` to add visualization geoms to ``user_scn``, and upon the next call to ``sync()``, the viewer + :ref:`mjv_connector` to add visualization geoms to ``user_scn``, and upon the next call to ``sync()``, the viewer will incorporate these geoms to future rendered images. Similarly, user scripts can make changes to ``user_scn.flags`` which would be picked up at the next call to ``sync()``. The ``sync()`` call also copies changes to rendering flags made via the GUI back into ``user_scn`` to preserve consistency. For example: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 849462fe..fae4600f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -668,14 +668,6 @@ MJAPI void mjv_defaultFigure(mjvFigure* fig); MJAPI void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3], const mjtNum pos[3], const mjtNum mat[9], const float rgba[4]); -// Set (type, size, pos, mat) for connector-type geom between given points. -// Assume that mjv_initGeom was already called to set all other properties. -// Width of mjGEOM_LINE is denominated in pixels. -// Deprecated: use mjv_connector. -MJAPI void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, - mjtNum a0, mjtNum a1, mjtNum a2, - mjtNum b0, mjtNum b1, mjtNum b2); - // Set (type, size, pos, mat) for connector-type geom between given points. // Assume that mjv_initGeom was already called to set all other properties. // Width of mjGEOM_LINE is denominated in pixels. diff --git a/introspect/functions.py b/introspect/functions.py index 4f86485d..2afb3798 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -4372,52 +4372,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Initialize given geom fields when not NULL, set the rest to their default values.', # pylint: disable=line-too-long )), - ('mjv_makeConnector', - FunctionDecl( - name='mjv_makeConnector', - return_type=ValueType(name='void'), - parameters=( - FunctionParameterDecl( - name='geom', - type=PointerType( - inner_type=ValueType(name='mjvGeom'), - ), - ), - FunctionParameterDecl( - name='type', - type=ValueType(name='int'), - ), - FunctionParameterDecl( - name='width', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='a0', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='a1', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='a2', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='b0', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='b1', - type=ValueType(name='mjtNum'), - ), - FunctionParameterDecl( - name='b2', - type=ValueType(name='mjtNum'), - ), - ), - doc='Set (type, size, pos, mat) for connector-type geom between given points. Assume that mjv_initGeom was already called to set all other properties. Width of mjGEOM_LINE is denominated in pixels. Deprecated: use mjv_connector.', # pylint: disable=line-too-long - )), ('mjv_connector', FunctionDecl( name='mjv_connector', diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 6965957d..47bf5314 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -675,7 +675,6 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); - Def(pymodule); Def(pymodule); // Skipped: mjv_defaultScene (have MjvScene.__init__, memory managed by // MjvScene). diff --git a/python/tutorial.ipynb b/python/tutorial.ipynb index 15b5ad39..c3d4193d 100644 --- a/python/tutorial.ipynb +++ b/python/tutorial.ipynb @@ -1872,14 +1872,13 @@ " if scene.ngeom >= scene.maxgeom:\n", " return\n", " scene.ngeom += 1 # increment ngeom\n", - " # initialise a new capsule, add it to the scene using mjv_makeConnector\n", + " # initialise a new capsule, add it to the scene using mjv_connector\n", " mujoco.mjv_initGeom(scene.geoms[scene.ngeom-1],\n", " mujoco.mjtGeom.mjGEOM_CAPSULE, np.zeros(3),\n", " np.zeros(3), np.zeros(9), rgba.astype(np.float32))\n", - " mujoco.mjv_makeConnector(scene.geoms[scene.ngeom-1],\n", - " mujoco.mjtGeom.mjGEOM_CAPSULE, radius,\n", - " point1[0], point1[1], point1[2],\n", - " point2[0], point2[1], point2[2])\n", + " mujoco.mjv_connector(scene.geoms[scene.ngeom-1],\n", + " mujoco.mjtGeom.mjGEOM_CAPSULE, radius,\n", + " point1, point2)\n", "\n", " # traces of time, position and speed\n", "times = []\n", diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index d3638970..10396a0b 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -346,10 +346,9 @@ static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* // set (type, size, pos, mat) connector-type geom between given points // assume that mjv_initGeom was already called to set all other properties -void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, - mjtNum a0, mjtNum a1, mjtNum a2, - mjtNum b0, mjtNum b1, mjtNum b2) { - mjtNum quat[4], mat[9], dif[3] = {b0-a0, b1-a1, b2-a2}; +void mjv_connector(mjvGeom* geom, int type, mjtNum width, + const mjtNum from[3], const mjtNum to[3]) { + mjtNum quat[4], mat[9], dif[3] = {to[0]-from[0], to[1]-from[1], to[2]-from[2]}; // require connector-compatible type if (type != mjGEOM_CAPSULE && type != mjGEOM_CYLINDER && @@ -367,17 +366,17 @@ void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, // cylinder and capsule are centered, and size[0] is "radius" if (type == mjGEOM_CAPSULE || type == mjGEOM_CYLINDER) { - geom->pos[0] = 0.5*(a0 + b0); - geom->pos[1] = 0.5*(a1 + b1); - geom->pos[2] = 0.5*(a2 + b2); + geom->pos[0] = 0.5*(from[0] + to[0]); + geom->pos[1] = 0.5*(from[1] + to[1]); + geom->pos[2] = 0.5*(from[2] + to[2]); geom->size[2] *= 0.5; } // arrow is not centered else { - geom->pos[0] = a0; - geom->pos[1] = a1; - geom->pos[2] = a2; + geom->pos[0] = from[0]; + geom->pos[1] = from[1]; + geom->pos[2] = from[2]; } // set mat to minimal rotation aligning b-a with z axis @@ -386,12 +385,7 @@ void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, mju_n2f(geom->mat, mat, 9); } -// set (type, size, pos, mat) connector-type geom between given points -// assume that mjv_initGeom was already called to set all other properties -void mjv_connector(mjvGeom* geom, int type, mjtNum width, - const mjtNum from[3], const mjtNum to[3]) { - mjv_makeConnector(geom, type, width, from[0], from[1], from[2], to[0], to[1], to[2]); -} + // initialize given fields when not NULL, set the rest to their default values void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, diff --git a/src/engine/engine_vis_visualize.h b/src/engine/engine_vis_visualize.h index 41c2050d..bf548360 100644 --- a/src/engine/engine_vis_visualize.h +++ b/src/engine/engine_vis_visualize.h @@ -24,12 +24,6 @@ extern "C" { #endif -// set (type, size, pos, mat) connector-type geom between given points -// assume that mjv_initGeom was already called to set all other properties -MJAPI void mjv_makeConnector(mjvGeom* geom, int type, mjtNum width, - mjtNum a0, mjtNum a1, mjtNum a2, - mjtNum b0, mjtNum b1, mjtNum b2); - // set (type, size, pos, mat) connector-type geom between given points // assume that mjv_initGeom was already called to set all other properties MJAPI void mjv_connector(mjvGeom* geom, int type, mjtNum width, diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index f456e3f6..51a13917 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6921,9 +6921,6 @@ public static unsafe extern void mjv_defaultFigure(mjvFigure_* fig); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mjv_initGeom(mjvGeom_* geom, int type, double* size, double* pos, double* mat, float* rgba); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mjv_makeConnector(mjvGeom_* geom, int type, double width, double a0, double a1, double a2, double b0, double b1, double b2); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mjv_connector(mjvGeom_* geom, int type, double width, double* from, double* to); From ca45041049003d55fc030a254d548edd86b3d54c Mon Sep 17 00:00:00 2001 From: Joss Moore Date: Mon, 28 Oct 2024 04:54:42 -0700 Subject: [PATCH 021/426] MjSpecs.copy should return an MjSpec rather than a raw::MjSpec* raw::MjSpec* is not a good python object. PiperOrigin-RevId: 690564198 Change-Id: I3a5dd525a3a9acc67bb0e598dd508572dc4c687f --- python/mujoco/specs.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 96bb7baf..fe7e22fb 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -323,10 +323,9 @@ PYBIND11_MODULE(_specs, m) { return mjmodel_mjdata_from_spec_ptr(reinterpret_cast(self.ptr), m, d); }); - mjSpec.def( - "copy", - [](const MjSpec& self) -> raw::MjSpec* { return mj_copySpec(self.ptr); }, - py::return_value_policy::reference_internal); + mjSpec.def("copy", [](const MjSpec& self) -> MjSpec { + return MjSpec(mj_copySpec(self.ptr)); + }); mjSpec.def_property_readonly( "worldbody", [](MjSpec& self) -> raw::MjsBody* { From a4a6248a069b23f25694f870a380e8eccb3f584c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 28 Oct 2024 05:51:05 -0700 Subject: [PATCH 022/426] Move AsVector utility to fixture.h PiperOrigin-RevId: 690577546 Change-Id: I2cebcffa1f2e3b0789f43e772e364c4762719ab3 --- test/benchmark/engine_core_smooth_benchmark_test.cc | 7 +------ test/benchmark/engine_util_sparse_benchmark_test.cc | 6 ------ test/engine/engine_core_constraint_test.cc | 5 ----- test/engine/engine_derivative_test.cc | 5 ----- test/engine/engine_forward_test.cc | 4 ---- test/engine/engine_solver_test.cc | 5 ----- test/engine/engine_support_test.cc | 4 ---- test/engine/engine_util_solve_test.cc | 4 ---- test/engine/engine_util_spatial_test.cc | 5 ----- test/fixture.h | 5 +++++ test/pipeline_test.cc | 4 ---- test/user/user_mesh_test.cc | 4 ---- test/user/user_model_test.cc | 5 ----- test/user/user_objects_test.cc | 8 ++------ test/xml/xml_native_reader_test.cc | 4 ---- 15 files changed, 8 insertions(+), 67 deletions(-) diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index 2262874a..fc6c6a89 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -14,8 +14,8 @@ // A benchmark for comparing different implementations of mj_solveLD. +#include #include -#include #include #include #include @@ -30,11 +30,6 @@ static const int kNumWarmupSteps = 200; // number of steps to benchmark static const int kNumBenchmarkSteps = 50; -// copy array into vector -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - // ----------------------------- old functions -------------------------------- void ABSL_ATTRIBUTE_NOINLINE solveLD_baseline(const mjModel* m, mjtNum* x, diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index e3850d07..d77dc067 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -37,11 +36,6 @@ using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse); // number of steps to roll out before benchmarking static const int kNumWarmupSteps = 500; -// copy array into vector -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - // ----------------------------- old functions -------------------------------- void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 6a7d3159..b9a779fd 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -34,10 +33,6 @@ using ::testing::DoubleNear; using ::testing::Pointwise; using CoreConstraintTest = MujocoTest; -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - // compute rotation residual following formula in mj_instantiateEquality void RotationResidual(const mjModel *model, mjData *data, const mjtNum qpos[7], const mjtNum dqpos[6], diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 778ddadb..4984a311 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -86,11 +86,6 @@ static void PrintMatrix(mjtNum* mat, int nrow, int ncol) { } } - -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - static const char* const kEnergyConservingPendulumPath = "engine/testdata/derivative/energy_conserving_pendulum.xml"; static const char* const kTumblingThinObjectPath = diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 6c269b77..9c930bc4 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -40,10 +40,6 @@ namespace mujoco { namespace { -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - static const char* const kEnergyConservingPendulumPath = "engine/testdata/derivative/energy_conserving_pendulum.xml"; static const char* const kDampedActuatorsPath = diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 47a642ed..5a8e6b2e 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -15,7 +15,6 @@ // Tests for engine/engine_solver.c #include -#include #include #include @@ -25,10 +24,6 @@ namespace mujoco { namespace { -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - using ::testing::DoubleNear; using ::testing::NotNull; using ::testing::Pointwise; diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 4393f02b..1f95c660 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -37,10 +37,6 @@ using ::testing::Pointwise; using ::testing::ElementsAreArray; using ::testing::Pointwise; -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - using AngMomMatTest = MujocoTest; static constexpr char AngMomTestingModel[] = R"( diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index dabc6024..7d56e525 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/test/engine/engine_util_solve_test.cc @@ -36,10 +36,6 @@ using ::std::string; using ::std::setw; using QCQP2Test = MujocoTest; -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - TEST_F(QCQP2Test, DegenerateAMatrix) { // A 2x2 matrix with determinant zero. const mjtNum Ain[9] { 6, -15, 2, -5 }; diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 678252f0..7356a91c 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -15,7 +15,6 @@ // Tests for engine/engine_util_spatial.c #include -#include #include #include @@ -36,10 +35,6 @@ using ::testing::Pointwise; using Quat2MatTest = MujocoTest; -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - TEST_F(Quat2MatTest, NoRotation) { mjtNum result[9] = {0}; mjtNum quat[] = {1, 0, 0, 0}; diff --git a/test/fixture.h b/test/fixture.h index 6eda2fa4..8da8ef5c 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -105,6 +105,11 @@ std::vector GetCtrlNoise(const mjModel* m, int nsteps, // Returns the name of the different field and the max difference. mjtNum CompareModel(const mjModel* m1, const mjModel* m2, std::string& field); +// Returns a vector containing the elements of the array. +inline std::vector AsVector(const mjtNum* array, int n) { + return std::vector(array, array + n); +} + // Installs a mock filesystem via a resource provider. To obtain thread safety, // each filesystem is scoped for individual unit tests with destructive // operations not permitted. diff --git a/test/pipeline_test.cc b/test/pipeline_test.cc index fbc50030..fb7c347a 100644 --- a/test/pipeline_test.cc +++ b/test/pipeline_test.cc @@ -27,10 +27,6 @@ namespace mujoco { namespace { -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - static const char* const kDefaultModel = "testdata/model.xml"; using ::testing::Pointwise; diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 0a57109c..cac1b7ed 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -67,10 +67,6 @@ 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; diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 24467d97..0874a14c 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -26,7 +26,6 @@ #include #include #include -#include "src/cc/array_safety.h" #include "test/fixture.h" namespace mujoco { @@ -44,10 +43,6 @@ static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { array + ncolumn * (row + 1)); } -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - // ----------------------------- test mjCModel -------------------------------- using UserCModelTest = MujocoTest; diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 5d6891c3..b6fbaf5f 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -34,10 +33,6 @@ namespace { constexpr double kInertiaTol = 1e-6; -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - using std::string; using ::testing::DoubleNear; using ::testing::ElementsAre; @@ -2402,7 +2397,8 @@ TEST_F(UserObjectsTest, BadWeld) { int len = 4; // good model using body semantic - string xml = base.replace(pos, len, ""); + string xml = base.replace(pos, len, + ""); char error[1024]; mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); ASSERT_THAT(m, NotNull()) << error; diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 4eed4735..4a287b6e 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -33,10 +33,6 @@ namespace mujoco { namespace { -std::vector AsVector(const mjtNum* array, int n) { - return std::vector(array, array + n); -} - using ::std::string; using ::testing::AllOf; using ::testing::ElementsAre; From 3d920e6a70ddeaad5729f71ecdea0c58098976f6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 28 Oct 2024 08:16:09 -0700 Subject: [PATCH 023/426] Update build documentation to include instructions on how to build the docs locally. PiperOrigin-RevId: 690616910 Change-Id: I717988e3adc67cb6d65488d39bc804c3d4f0e3b5 --- doc/changelog.rst | 2 +- doc/programming/index.rst | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index f3c9b25e..caf32f5f 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -891,7 +891,7 @@ General used to determine the type of the asset file without resorting to pulling the type from the file extension. #. Added analytic derivatives for quaternion :ref:`subtraction` and :ref:`integration` (rotation with an angular velocity). Derivatives are in the 3D tangent space. -#. Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient +#. Added :ref:`mjv_connector` which has identical functionality to ``mjv_makeConnector``, but with more convenient "from-to" argument parametrization. ``mjv_makeConnector`` is now deprecated. #. Bumped oldest supported MacOS from version 10.12 to 11. MacOS 11 is the oldest version still maintained by Apple. diff --git a/doc/programming/index.rst b/doc/programming/index.rst index 47039cb8..f6e4cb3e 100644 --- a/doc/programming/index.rst +++ b/doc/programming/index.rst @@ -94,8 +94,8 @@ Building from source To build MuJoCo from source, you will need CMake and a working C++17 compiler installed. The steps are: -#. Clone the ``mujoco`` repository from GitHub. -#. Create a new build directory somewhere, and ``cd`` into it. +#. Clone the ``mujoco`` repository: ``git clone https://github.com/deepmind/mujoco.git`` +#. Create a new build directory and ``cd`` into it. #. Run :shell:`cmake $PATH_TO_CLONED_REPO` to configure the build. #. Run ``cmake --build .`` to build. @@ -120,6 +120,19 @@ installed (see :github:issue:`862` for more details). `continuous integration setup `_ on GitHub. +.. _inBuildDocs: + +Building the docs +~~~~~~~~~~~~~~~~~ + +If you wish to build the documentation locally, for example to test pull-requests that improve it, do: + +1. Clone the ``mujoco`` repository: ``git clone https://github.com/deepmind/mujoco.git`` +2. Go to the ``doc/`` directory: ``cd mujoco/doc`` +3. Install the dependencies: ``pip install -r requirements.txt`` +4. Build the HTML: ``make html`` +5. Open ``_build/html/index.html`` in your browser of choice. + .. _inHeader: Header files From 864b805a6eb557aa2676d3d0184cdc1afaacb2a7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 28 Oct 2024 10:43:09 -0700 Subject: [PATCH 024/426] Fix multiple bugs related to connect and weld constraints with site semantics. Fixes #2179 The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints. PiperOrigin-RevId: 690670420 Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6 --- doc/changelog.rst | 5 ++ src/engine/engine_core_constraint.c | 30 +++++++++- src/engine/engine_core_smooth.c | 26 ++++++--- src/engine/engine_island.c | 13 ++++- test/engine/engine_core_constraint_test.cc | 36 ++++++++++++ test/engine/engine_core_smooth_test.cc | 43 ++++++++++++-- test/engine/engine_island_test.cc | 13 ++--- test/engine/engine_solver_test.cc | 39 ++++++++++--- .../testdata/equality_site_body_compare.xml | 58 +++++++++++++++++++ test/engine/testdata/island/island_efc.xml | 5 +- 10 files changed, 234 insertions(+), 34 deletions(-) create mode 100644 test/engine/testdata/equality_site_body_compare.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index caf32f5f..ba4cae6d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -24,6 +24,11 @@ MJX Bug fixes ^^^^^^^^^ +- Fixed several bugs related to connect and weld constraints with site semantics (fixes :github:issue:`2179`, reported + by :github:user:`yinfanyi`). The introduction of site specification to connects and welds in 3.2.3 conditionally + changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`, but these changes were not properly propagated in + several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and + runtime enabling/disabling of such constraints. - Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. Version 3.2.4 (Oct 15, 2024) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 87bba06e..dc2a7658 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1117,16 +1117,30 @@ void mj_diagApprox(const mjModel* m, mjData* d) { // process according to equality-constraint type switch (m->eq_type[id]) { case mjEQ_CONNECT: - // body translation b1 = m->eq_obj1id[id]; b2 = m->eq_obj2id[id]; + + // get body ids if using site semantics + if (m->eq_objtype[id] == mjOBJ_SITE) { + b1 = m->site_bodyid[b1]; + b2 = m->site_bodyid[b2]; + } + + // body translation dA[i] = m->body_invweight0[2*b1] + m->body_invweight0[2*b2]; break; case mjEQ_WELD: // distinguish translation and rotation inertia - // body translation or rotation depending on weldcnt b1 = m->eq_obj1id[id]; b2 = m->eq_obj2id[id]; + + // get body ids if using site semantics + if (m->eq_objtype[id] == mjOBJ_SITE) { + b1 = m->site_bodyid[b1]; + b2 = m->site_bodyid[b2]; + } + + // body translation or rotation depending on weldcnt dA[i] = m->body_invweight0[2*b1 + (weldcnt > 2)] + m->body_invweight0[2*b2 + (weldcnt > 2)]; weldcnt = (weldcnt + 1) % 6; @@ -1650,6 +1664,12 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; } + // get body ids if using site semantics + if (m->eq_objtype[i] == mjOBJ_SITE) { + id[0] = m->site_bodyid[id[0]]; + id[1] = m->site_bodyid[id[1]]; + } + NV = mj_jacDifPairCount(m, chain, id[1], id[0], issparse); break; @@ -1659,6 +1679,12 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; } + // get body ids if using site semantics + if (m->eq_objtype[i] == mjOBJ_SITE) { + id[0] = m->site_bodyid[id[0]]; + id[1] = m->site_bodyid[id[1]]; + } + NV = mj_jacDifPairCount(m, chain, id[1], id[0], issparse); break; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index be191d37..1bdcfda1 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1902,7 +1902,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } } - // cfrc_ext += connect and weld constraints + // cfrc_ext += connect, weld, flex constraints int i = 0, ne = d->ne; while (i < ne) { if (d->efc_type[i] != mjCNSTR_EQUALITY) @@ -1910,8 +1910,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { int id = d->efc_id[i]; mjtNum* eq_data = m->eq_data + mjNEQDATA*id; - mjtNum pos[3]; - int k; + mjtNum pos[3], *offset; + int k, obj1, obj2, body_semantic; switch ((mjtEq) m->eq_type[id]) { case mjEQ_CONNECT: case mjEQ_WELD: @@ -1923,10 +1923,17 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { mju_zero3(cfrc); // no torque from connect } + body_semantic = m->eq_objtype[id] == mjOBJ_BODY; + // body 1 - if ((k = m->eq_obj1id[id])) { + obj1 = m->eq_obj1id[id]; + k = body_semantic ? obj1 : m->site_bodyid[obj1]; + if (k) { + offset = body_semantic ? eq_data + 3 * (m->eq_type[id] == mjEQ_WELD) : + m->site_pos + 3 * obj1; + // transform point on body1: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0); + mj_local2Global(d, pos, 0, offset, 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); @@ -1936,9 +1943,14 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } // body 2 - if ((k = m->eq_obj2id[id])) { + obj2 = m->eq_obj2id[id]; + k = body_semantic ? obj2 : m->site_bodyid[obj2]; + if (k) { + offset = body_semantic ? eq_data + 3 * (m->eq_type[id] == mjEQ_CONNECT) : + m->site_pos + 3 * obj2; + // transform point on body2: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0); + mj_local2Global(d, pos, 0, offset, 0, k, 0); // tmp = subtree CoM-based torque_force vector mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 286af4c7..26d67213 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -257,8 +257,17 @@ static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) { if (efc_type == mjCNSTR_EQUALITY) { mjtEq eq_type = m->eq_type[efc_id]; if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) { - tree[0] = m->body_treeid[m->eq_obj1id[efc_id]]; - tree[1] = m->body_treeid[m->eq_obj2id[efc_id]]; + int b1 = m->eq_obj1id[efc_id]; + int b2 = m->eq_obj2id[efc_id]; + + // get body ids if using site semantics + if (m->eq_objtype[efc_id] == mjOBJ_SITE) { + b1 = m->site_bodyid[b1]; + b2 = m->site_bodyid[b2]; + } + + tree[0] = m->body_treeid[b1]; + tree[1] = m->body_treeid[b2]; // handle static bodies if (tree[0] < 0) { diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index b9a779fd..8baeeadc 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -249,6 +250,41 @@ TEST_F(CoreConstraintTest, JacobianPreAllocate) { } } +TEST_F(CoreConstraintTest, EqualityBodySite) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/equality_site_body_compare.xml"); + + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + // simulate, get diag(A) + while (data->time < 0.1) { + mj_step(model, data); + } + int nefc_site = data->nefc; + std::vector dA = AsVector(data->efc_diagApprox, nefc_site); + + // reset + mj_resetData(model, data); + + // turn site-defined equalities off, equivalent body-defined equalities on + for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; + + // simulate again, get diag(A) + while (data->time < 0.1) { + mj_step(model, data); + } + + // compare + EXPECT_EQ(nefc_site, data->nefc); + EXPECT_THAT(AsVector(data->efc_diagApprox, data->nefc), + Pointwise(DoubleNear(1e-12), dA)); + + mj_deleteData(data); + mj_deleteModel(model); +} + + static const char* const kIlslandEfcPath = "engine/testdata/island/island_efc.xml"; diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index dcb123d0..8b2b4f2a 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -39,9 +39,6 @@ using ::testing::DoubleNear; using ::testing::NotNull; using CoreSmoothTest = MujocoTest; -std::vector GetVector(const mjtNum* array, int length) { - return std::vector(array, array + length); -} constexpr bool EndsWith(std::string_view str, std::string_view suffix) { return str.size() >= suffix.size() && @@ -104,7 +101,7 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { mj_resetDataDebug(model, data, 'd'); mj_kinematics(model, data); - EXPECT_THAT(GetVector(&data->xipos[0], 3), ElementsAre(0, 0, 0)); + EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0)); mj_deleteData(data); mj_deleteModel(model); @@ -241,16 +238,50 @@ TEST_F(CoreSmoothTest, WeldRatioTorqueFree) { TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) { constexpr char kModelFilePath[] = - "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml"; + "engine/testdata/core_smooth/rne_post/weld/" + "tfratio0_force_slide_rotated.xml"; TestConnect(kModelFilePath); } TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) { constexpr char kModelFilePath[] = - "engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml"; + "engine/testdata/core_smooth/rne_post/weld/" + "tfratio0_multiple_constraints.xml"; TestConnect(kModelFilePath); } +TEST_F(CoreSmoothTest, EqualityBodySite) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/equality_site_body_compare.xml"); + + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + // simulate, get sensordata + while (data->time < 0.1) { + mj_step(model, data); + } + std::vector sdata = AsVector(data->sensordata, model->nsensordata); + + // reset + mj_resetData(model, data); + + // turn site-defined equalities off, equivalent body-defined equalities on + for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; + + // simulate again, get sensordata + while (data->time < 0.1) { + mj_step(model, data); + } + + // compare + EXPECT_THAT(AsVector(data->sensordata, model->nsensordata), + Pointwise(DoubleNear(1e-8), sdata)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- site actuators ---------------------------------- // Test Cartesian position control using site transmission with refsite diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 3729e3f0..3688c15b 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -14,20 +14,15 @@ // Tests for engine/engine_island.c. -#include -#include #include #include #include #include #include -#include -#include #include #include "src/engine/engine_island.h" #include "src/engine/engine_util_sparse.h" -#include "src/thread/thread_pool.h" #include "test/fixture.h" namespace mujoco { @@ -357,10 +352,10 @@ TEST_F(IslandTest, IslandEfc) { // expect island structure to correspond to comment at top of xml EXPECT_EQ(data->nisland, 4); - EXPECT_EQ(data->ne, 4); + EXPECT_EQ(data->ne, 7); EXPECT_EQ(data->nf, 2); EXPECT_EQ(data->nl, 1); - EXPECT_EQ(data->nefc, 27); + EXPECT_EQ(data->nefc, 30); mj_deleteData(data); mj_deleteModel(model); @@ -378,10 +373,10 @@ TEST_F(IslandTest, IslandEfcElliptic) { mj_forward(model, data); EXPECT_EQ(data->nisland, 4); - EXPECT_EQ(data->ne, 4); + EXPECT_EQ(data->ne, 7); EXPECT_EQ(data->nf, 2); EXPECT_EQ(data->nl, 1); - EXPECT_EQ(data->nefc, 22); + EXPECT_EQ(data->nefc, 25); mj_deleteData(data); mj_deleteModel(model); diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 5a8e6b2e..d20c3d38 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -14,7 +14,10 @@ // Tests for engine/engine_solver.c +#include +#include #include +#include #include #include @@ -27,6 +30,28 @@ namespace { using ::testing::DoubleNear; using ::testing::NotNull; using ::testing::Pointwise; +using ::std::vector; +using ::std::abs; +using ::std::max; + +// compare two vectors, relative error (reduces size of large vector elements) +inline void ExpectEqRel(vector v1, vector v2, mjtNum rtol) { + ASSERT_TRUE(v1.size() == v2.size()); + + // make scale vector + int n = v1.size(); + vector scale(n); + for (int i = 0; i < n; i++) { + scale[i] = max(1.0, abs(v1[i]) + abs(v2[i])); + } + + // scale and compare + for (int i = 0; i < n; i++) { + v1[i] /= scale[i]; + v2[i] /= scale[i]; + } + EXPECT_THAT(v1, Pointwise(DoubleNear(rtol), v2)); +} using SolverTest = MujocoTest; @@ -51,7 +76,7 @@ TEST_F(SolverTest, IslandsEquivalent) { mjData* data_island = mj_makeData(model); mjData* data_noisland = mj_makeData(model); - mjtNum tol = 2e-4; + mjtNum rtol = 1e-5; for (bool warmstart : {true, false}) { if (warmstart) { @@ -73,8 +98,8 @@ TEST_F(SolverTest, IslandsEquivalent) { mj_forward(model, data_island); model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands - EXPECT_THAT(AsVector(data_noisland->qacc, nv), - Pointwise(DoubleNear(tol), AsVector(data_island->qacc, nv))); + ExpectEqRel(AsVector(data_noisland->qacc, nv), + AsVector(data_island->qacc, nv), rtol); } } @@ -102,7 +127,7 @@ TEST_F(SolverTest, OneBigIsland) { mjData* data_noisland = mj_makeData(model); int nv = model->nv; - mjtNum tol = 1e-8; + mjtNum rtol = 1e-7; // save current (default) iterations int iterations_default = model->opt.iterations; @@ -144,9 +169,9 @@ TEST_F(SolverTest, OneBigIsland) { model->opt.enableflags &= ~mjENBL_ISLAND; model->opt.iterations = iterations_default; - // compare accelerations - EXPECT_THAT(AsVector(data_noisland->qacc, nv), - Pointwise(DoubleNear(tol), AsVector(data_island->qacc, nv))); + // compare accelerations (relative error) + ExpectEqRel(AsVector(data_noisland->qacc, nv), + AsVector(data_island->qacc, nv), rtol); } mj_deleteData(data_noisland); diff --git a/test/engine/testdata/equality_site_body_compare.xml b/test/engine/testdata/equality_site_body_compare.xml new file mode 100644 index 00000000..cae4ff2d --- /dev/null +++ b/test/engine/testdata/equality_site_body_compare.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/island/island_efc.xml b/test/engine/testdata/island/island_efc.xml index b500f57c..32201839 100644 --- a/test/engine/testdata/island/island_efc.xml +++ b/test/engine/testdata/island/island_efc.xml @@ -1,6 +1,6 @@ \n" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "sJFuNetilv4m" + }, + "source": [ + "## All imports" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "ArmOx3cBDvFR" + }, + "outputs": [], + "source": [ + "!pip install mujoco\n", + "\n", + "# Set up GPU rendering.\n", + "from google.colab import files\n", + "import distutils.util\n", + "import os\n", + "import subprocess\n", + "if subprocess.run('nvidia-smi').returncode:\n", + " raise RuntimeError(\n", + " 'Cannot communicate with GPU. '\n", + " 'Make sure you are using a GPU Colab runtime. '\n", + " 'Go to the Runtime menu and select Choose runtime type.')\n", + "\n", + "# Add an ICD config so that glvnd can pick up the Nvidia EGL driver.\n", + "# This is usually installed as part of an Nvidia driver package, but the Colab\n", + "# kernel doesn't install its driver via APT, and as a result the ICD is missing.\n", + "# (https://github.com/NVIDIA/libglvnd/blob/master/src/EGL/icd_enumeration.md)\n", + "NVIDIA_ICD_CONFIG_PATH = '/usr/share/glvnd/egl_vendor.d/10_nvidia.json'\n", + "if not os.path.exists(NVIDIA_ICD_CONFIG_PATH):\n", + " with open(NVIDIA_ICD_CONFIG_PATH, 'w') as f:\n", + " f.write(\"\"\"{\n", + " \"file_format_version\" : \"1.0.0\",\n", + " \"ICD\" : {\n", + " \"library_path\" : \"libEGL_nvidia.so.0\"\n", + " }\n", + "}\n", + "\"\"\")\n", + "\n", + "# Configure MuJoCo to use the EGL rendering backend (requires GPU)\n", + "print('Setting environment variable to use GPU rendering:')\n", + "%env MUJOCO_GL=egl\n", + "\n", + "# Check if installation was succesful.\n", + "try:\n", + " print('Checking that the installation succeeded:')\n", + " import mujoco\n", + " mujoco.MjModel.from_xml_string('')\n", + "except Exception as e:\n", + " raise e from RuntimeError(\n", + " 'Something went wrong during installation. Check the shell output above '\n", + " 'for more information.\\n'\n", + " 'If using a hosted Colab runtime, make sure you enable GPU acceleration '\n", + " 'by going to the Runtime menu and selecting \"Choose runtime type\".')\n", + "\n", + "print('Installation successful.')\n", + "\n", + "# Other imports and helper functions\n", + "import time\n", + "import itertools\n", + "import numpy as np\n", + "\n", + "# Graphics and plotting.\n", + "print('Installing mediapy:')\n", + "!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)\n", + "!pip install -q mediapy\n", + "import mediapy as media\n", + "import matplotlib.pyplot as plt\n", + "\n", + "# More legible printing from numpy.\n", + "np.set_printoptions(precision=3, suppress=True, linewidth=100)\n", + "\n", + "from IPython.display import clear_output\n", + "clear_output()\n", + "\n", + "# Get MuJoCo's humanoid model and a Franka arm from the MuJoCo Menagerie.\n", + "print('Getting MuJoCo humanoid XML description from GitHub:')\n", + "!git clone https://github.com/google-deepmind/mujoco\n", + "humanoid_file = 'mujoco/model/humanoid/humanoid.xml'\n", + "humanoid100_file = 'mujoco/model/humanoid/humanoid100.xml'\n", + "print('Getting MuJoCo Menagerie Franka XML description from GitHub:')\n", + "!git clone https://github.com/google-deepmind/mujoco_menagerie\n", + "franka_file = 'mujoco_menagerie/franka_fr3/fr3.xml'\n", + "\n", + "def render(model, data=None, height=250):\n", + " if data is None:\n", + " data = mj.MjData(model)\n", + " with mj.Renderer(model, 480, 640) as renderer:\n", + " mj.mj_forward(model, data)\n", + " renderer.update_scene(data)\n", + " media.show_image(renderer.render(), height=height)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "iJRNczuyHbuc" + }, + "source": [ + "# Parsing XML to `mjSpec` and compiling to `mjModel`\n" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "MYPbnl3mxvDj" + }, + "source": [ + "Unlike `mj_loadXML` which combines parsing and compiling, when using `mjSpec`, parsing and compiling are separate, allowing for editing steps:" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "oummB7I7EfSq" + }, + "outputs": [], + "source": [ + "#@title A static model, from string {vertical-output: true}\n", + "\n", + "static_model = \"\"\"\n", + "\n", + " \n", + " \n", + " \n", + " \n", + " \n", + "\n", + "\"\"\"\n", + "spec = mj.MjSpec.from_string(static_model)\n", + "model = spec.compile()\n", + "render(model)\n", + "\n", + "# Change the mjSpec, re-compile and re-render\n", + "geoms = spec.worldbody.find_all('geom')\n", + "geoms[0].name = 'blue_box'\n", + "geoms[0].rgba = [0, 0, 1, 1]\n", + "geoms[1].name = 'yellow_sphere'\n", + "geoms[1].rgba = [1, 1, 0, 1]\n", + "spec.worldbody.add_geom(name='magenta cylinder',\n", + " type=mj.mjtGeom.mjGEOM_CYLINDER,\n", + " rgba=[1, 0, 1, 1],\n", + " pos=[-.2, 0, .2],\n", + " size=[.1, .1, 0])\n", + "\n", + "model = spec.compile()\n", + "render(model)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Tw_yUwqxKwCI" + }, + "source": [ + "`mjSpec` can save XML to string, saving all modifications." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "_7HAcWqNGwyw" + }, + "outputs": [], + "source": [ + "#@title Print an XML from an `mjSpec` {vertical-output: true}\n", + "\n", + "print(spec.to_xml())" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "A6EHNulmFHFI" + }, + "outputs": [], + "source": [ + "#@title Building an `mjSpec` from scratch {vertical-output: true}\n", + "\n", + "spec = mj.MjSpec()\n", + "spec.worldbody.add_light(name=\"top\", pos=[0, 0, 1])\n", + "body = spec.worldbody.add_body(name=\"box_and_sphere\", euler=[0, 0, -30])\n", + "body.add_joint(name=\"swing\", type=mj.mjtJoint.mjJNT_HINGE,\n", + " axis=[1, -1, 0], pos=[-.2, -.2, -.2])\n", + "body.add_geom(name=\"red_box\", type=mj.mjtGeom.mjGEOM_BOX,\n", + " size=[.2, .2, .2], rgba=[1, 0, 0, 1])\n", + "body.add_geom(name=\"green_sphere\", pos=[.2, .2, .2],\n", + " size=[.1, 0, 0], rgba=[0, 1, 0, 1])\n", + "model = spec.compile()\n", + "\n", + "duration = 2 # (seconds)\n", + "framerate = 30 # (Hz)\n", + "\n", + "# enable joint visualization option:\n", + "scene_option = mj.MjvOption()\n", + "scene_option.flags[mj.mjtVisFlag.mjVIS_JOINT] = True\n", + "\n", + "# Simulate and display video.\n", + "frames = []\n", + "data = mj.MjData(model)\n", + "mj.mj_resetData(model, data)\n", + "with mj.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " mj.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data, scene_option=scene_option)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", + "\n", + "media.show_video(frames, fps=framerate)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "3N4YEIVt75_T" + }, + "source": [ + "# Control example" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "TcQuv56BwaJf" + }, + "source": [ + "A key feature of this library is the ability to easily attach multiple models into a larger one. Disambiguation of duplicated names from different\n", + "models, or multiple instances of the same model, is handled via user-defined namespacing.\n", + "\n", + "One example use case is when we want robots with a variable number of joints, as is a fundamental change to the kinematic structure. The following snippets realise this scenario." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "7C-hfbtj8nRV" + }, + "outputs": [], + "source": [ + "leg_model = \"\"\"\n", + "\n", + " \n", + "\n", + " \n", + " \n", + " \n", + " \n", + "\n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + " \n", + "\n", + " \n", + " \n", + " \n", + " \n", + "\n", + "\"\"\"\n", + "\n", + "class Leg(object):\n", + " \"\"\"A 2-DoF leg with position actuators.\"\"\"\n", + " def __init__(self, length, rgba):\n", + " self.spec = mj.MjSpec.from_string(leg_model)\n", + "\n", + " # Thigh:\n", + " thigh = self.spec.find_body('thigh')\n", + " thigh.add_geom(fromto=[0, 0, 0, length, 0, 0], size=[length/4, 0, 0], rgba=rgba)\n", + "\n", + " # Hip:\n", + " shin = self.spec.find_body('shin')\n", + " shin.add_geom(fromto=[0, 0, 0, 0, 0, -length], size=[length/5, 0, 0], rgba=rgba)\n", + " shin.pos[0] = length" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "MQGsxnIB_RLO" + }, + "source": [ + "The `Leg` class describes an abstract articulated leg, with two joints and corresponding proportional-derivative actuators.\n", + "\n", + "Note that:\n", + "\n", + "- MJCF attributes correspond directly to arguments of the `add_()` methods.\n", + "- When referencing elements, e.g when specifying the joint to which an actuator is attached, the name string of the MJCF elements is used." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "kMiuMyZW_XoB" + }, + "outputs": [], + "source": [ + "BODY_RADIUS = 0.1\n", + "random_state = np.random.RandomState(42)\n", + "creature_model = \"\"\"\n", + "\n", + " \n", + "\n", + " \n", + " \n", + " \n", + "\n", + "\"\"\".format(BODY_RADIUS, BODY_RADIUS, BODY_RADIUS / 2)\n", + "\n", + "def make_creature(num_legs):\n", + " \"\"\"Constructs a creature with `num_legs` legs.\"\"\"\n", + " rgba = random_state.uniform([0, 0, 0, 1], [1, 1, 1, 1])\n", + " spec = mj.MjSpec.from_string(creature_model)\n", + "\n", + " # Attach legs to equidistant sites on the circumference.\n", + " spec.worldbody.first_geom().rgba = rgba\n", + " leg = Leg(length=BODY_RADIUS, rgba=rgba)\n", + " for i in range(num_legs):\n", + " theta = 2 * i * np.pi / num_legs\n", + " hip_pos = BODY_RADIUS * np.array([np.cos(theta), np.sin(theta), 0])\n", + " hip_site = spec.worldbody.add_site(pos=hip_pos, euler=[0, 0, theta])\n", + " hip_site.attach(leg.spec.find_body('thigh'), '', '-' + str(i))\n", + "\n", + " return spec" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "QMQ3jc6-_toj" + }, + "source": [ + "The `make_creature` function uses the `attach()` method to procedurally attach legs to the torso. Note that at this stage both the torso and hip attachment sites are children of the `worldbody`, since their parent body has yet to be instantiated. We'll now make an arena with a chequered floor and two lights, and place our creatures in a grid." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "vt2JwXd__1cT" + }, + "outputs": [], + "source": [ + "#@title Six Creatures on a floor.{vertical-output: true}\n", + "\n", + "arena = mj.MjSpec()\n", + "arena.compiler.degree = False # Use radians.\n", + "\n", + "# Make arena with textured floor.\n", + "chequered = arena.add_texture(\n", + " name=\"chequered\", type=mj.mjtTexture.mjTEXTURE_2D,\n", + " builtin=mj.mjtBuiltin.mjBUILTIN_CHECKER,\n", + " width=300, height=300, rgb1=[.2, .3, .4], rgb2=[.3, .4, .5])\n", + "grid = arena.add_material(\n", + " name='grid', texrepeat=[5, 5], reflectance=.2\n", + " ).textures[mj.mjtTextureRole.mjTEXROLE_RGB] = 'chequered'\n", + "arena.worldbody.add_geom(\n", + " type=mj.mjtGeom.mjGEOM_PLANE, size=[2, 2, .1], material='grid')\n", + "for x in [-2, 2]:\n", + " arena.worldbody.add_light(pos=[x, -1, 3], dir=[-x, 1, -2])\n", + "\n", + "# Instantiate 6 creatures with 3 to 8 legs.\n", + "creatures = [make_creature(num_legs=num_legs) for num_legs in range(3, 9)]\n", + "\n", + "# Place them on a grid in the arena.\n", + "height = .15\n", + "grid = 5 * BODY_RADIUS\n", + "xpos, ypos, zpos = np.meshgrid([-grid, 0, grid], [0, grid], [height])\n", + "for i, spec in enumerate(creatures):\n", + " # Place spawn sites on a grid.\n", + " spawn_pos = (xpos.flat[i], ypos.flat[i], zpos.flat[i])\n", + " spawn_site = arena.worldbody.add_site(pos=spawn_pos, group=3)\n", + " # Attach to the arena at the spawn sites, with a free joint.\n", + " spawn_body = spawn_site.attach(spec.worldbody, '', '-' + str(i))\n", + " spawn_body.add_freejoint()\n", + "\n", + "# Instantiate the physics and render.\n", + "model = arena.compile()\n", + "render(model)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "mPUGkrCzAFMg" + }, + "source": [ + "Multi-legged creatures, ready to roam! Let's inject some controls and watch them move. We'll generate a sinusoidal open-loop control signal of fixed frequency and random phase, recording both video frames and the horizontal positions of the torso geoms, in order to plot the movement trajectories." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "7gz9FfNzGxPO" + }, + "outputs": [], + "source": [ + "#@title Video of the movement{vertical-output: true}\n", + "#@test {\"timeout\": 600}\n", + "\n", + "duration = 10 # (Seconds)\n", + "framerate = 30 # (Hz)\n", + "video = []\n", + "pos_x = []\n", + "pos_y = []\n", + "geoms = arena.worldbody.find_all(\"geom\")\n", + "torsos = [geom.id for geom in geoms if 'torso' in geom.name]\n", + "actuators = [actuator.id for actuator in arena.actuators]\n", + "\n", + "# Control signal frequency, phase, amplitude.\n", + "freq = 5\n", + "phase = 2 * np.pi * random_state.rand(len(arena.actuators))\n", + "amp = 0.9\n", + "\n", + "# Simulate, saving video frames and torso locations.\n", + "data = mj.MjData(model)\n", + "mj.mj_resetData(model, data)\n", + "with mj.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " # Inject controls and step the physics.\n", + " data.ctrl[actuators] = amp * np.sin(freq * data.time + phase)\n", + " mj.mj_step(model, data)\n", + "\n", + " # Save torso horizontal positions using name indexing.\n", + " pos_x.append(data.geom_xpos[torsos, 0].copy())\n", + " pos_y.append(data.geom_xpos[torsos, 1].copy())\n", + "\n", + " # Save video frames.\n", + " if len(video) < data.time * framerate:\n", + " renderer.update_scene(data)\n", + " pixels = renderer.render()\n", + " video.append(pixels.copy())\n", + "\n", + "media.show_video(video, fps=framerate)" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "qt2L52e_Tcgt" + }, + "outputs": [], + "source": [ + "#@title Movement trajectories{vertical-output: true}\n", + "\n", + "creature_colors = model.geom_rgba[torsos][:, :3]\n", + "fig, ax = plt.subplots(figsize=(4, 4))\n", + "ax.set_prop_cycle(color=creature_colors)\n", + "_ = ax.plot(pos_x, pos_y, linewidth=4)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "kSEUoxifxYJ4" + }, + "source": [ + "The plot above shows the corresponding movement trajectories of creature positions. Note how `mjSpec` attribute `id` were used to access both `xpos` and `rgba` values. This attribute is valid only after a model is compiled." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "QZ8alJZz8cB1" + }, + "source": [ + "# Model editing" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "JN3Z4v0PyXKa" + }, + "source": [ + "`mjSpec` elements can be traversed in two ways:\n", + "- For elements inside the kinematic tree, the tree can be traversed using the `first` and `next` functions.\n", + "- For all other elements, we provide a list.\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "8IcB7nezblyT" + }, + "outputs": [], + "source": [ + "#@title Traversing the spec.{vertical-output: true}\n", + "\n", + "spec = mj.MjSpec.from_file(humanoid_file)\n", + "\n", + "# Function that recursively prints all body names\n", + "def print_bodies(parent, level=0):\n", + " body = parent.first_body()\n", + " while body:\n", + " print(''.join(['-' for i in range(level)]) + body.name)\n", + " print_bodies(body, level + 1)\n", + " body = parent.next_body(body)\n", + "\n", + "print(\"The spec has the following actuators:\")\n", + "for actuator in spec.actuators:\n", + " print(actuator.name)\n", + "\n", + "print(\"\\nThe spec has the following bodies:\")\n", + "print_bodies(spec.worldbody)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "hcGI4orhyzvc" + }, + "source": [ + "An `mjSpec` can be compiled multiple times. If the state has to be preserved between different compilations, then the function `recompile()` must be used, which returns a new `mjData` that contains the mapped state, possibly having a different dimension from the origin." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "uh_N1Fkqk-Mi" + }, + "outputs": [], + "source": [ + "#@title Model re-compilation with state preservation.{vertical-output: true}\n", + "\n", + "spec = mj.MjSpec.from_file(humanoid100_file)\n", + "model = spec.compile()\n", + "data = mj.MjData(model)\n", + "\n", + "# Run for 5 seconds\n", + "for i in range(1000):\n", + " mj.mj_step(model, data)\n", + "\n", + "# Show result\n", + "render(model, data)\n", + "\n", + "# Create list of all bodies we want to delete\n", + "body = spec.worldbody.first_body()\n", + "delete_list = []\n", + "while body:\n", + " geom_type = body.first_geom().type\n", + " if (geom_type == mj.mjtGeom.mjGEOM_BOX or\n", + " geom_type == mj.mjtGeom.mjGEOM_ELLIPSOID):\n", + " delete_list.append(body)\n", + " body = spec.worldbody.next_body(body)\n", + "\n", + "# Remove all bodies in the list from the spec\n", + "for body in delete_list:\n", + " spec.detach_body(body)\n", + "\n", + "# # Add another humanoid\n", + "spec_humanoid = mj.MjSpec.from_file(humanoid_file)\n", + "attachment_frame = spec.worldbody.add_frame(pos=[0, -1, 2])\n", + "attachment_frame.attach_body(spec_humanoid.find_body('torso'), 'a', 'b')\n", + "\n", + "# Recompile preserving the state\n", + "new_model, new_data = spec.recompile(model, data)\n", + "\n", + "# Show result\n", + "render(new_model, new_data)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "XmSlXirVzLqt" + }, + "source": [ + "Let us load the humanoid model and inspect it." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "UywMzsp5Hnk2" + }, + "outputs": [], + "source": [ + "#@title Humanoid model.{vertical-output: true}\n", + "\n", + "spec = mj.MjSpec.from_file(humanoid_file)\n", + "\n", + "model = spec.compile()\n", + "render(model)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "owcmKeuSzQRy" + }, + "source": [ + "We wish to remove the arms and replace them with the legs. This can be done by first storing the arm positions into frames attaches to the torso. Then we can detach the arms and self-attach the legs into the frames." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "qZCyv-B0IGiG" + }, + "outputs": [], + "source": [ + "#@title Humanoid with arms replaced by legs.{vertical-output: true}\n", + "\n", + "spec = mj.MjSpec.from_file(humanoid_file)\n", + "\n", + "# Get the torso, arm, and leg bodies\n", + "arm_left = spec.find_body('upper_arm_left')\n", + "arm_right = spec.find_body('upper_arm_right')\n", + "leg_left = spec.find_body('thigh_left')\n", + "leg_right = spec.find_body('thigh_right')\n", + "torso = spec.find_body('torso')\n", + "\n", + "# Attach frames at the arm positions\n", + "shoulder_left = torso.add_frame(pos=arm_left.pos)\n", + "shoulder_right = torso.add_frame(pos=arm_right.pos)\n", + "\n", + "# Remove the arms\n", + "spec.detach_body(arm_left)\n", + "spec.detach_body(arm_right)\n", + "\n", + "# Add new legs\n", + "shoulder_left.attach_body(leg_left, 'shoulder', 'left')\n", + "shoulder_right.attach_body(leg_right, 'shoulder', 'right')\n", + "\n", + "model = spec.compile()\n", + "render(model, height=400)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "LnEwEjW3zdua" + }, + "source": [ + "Similarly, different models can be attach together. Here, the right arm is detached and a robot arm from a different model is attached in its place." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "w-NdFhSIIrLL" + }, + "outputs": [], + "source": [ + "#@title Humanoid with Franka arm.{vertical-output: true}\n", + "\n", + "spec = mj.MjSpec.from_file(humanoid_file)\n", + "franka = mj.MjSpec.from_file(franka_file)\n", + "spec.compiler.degree = False # This has no effect on the humanoid as it does not contain angles\n", + "\n", + "# Replace right arm with frame\n", + "arm_right = spec.find_body('upper_arm_right')\n", + "torso = spec.find_body('torso')\n", + "shoulder_right = torso.add_frame(pos=arm_right.pos, quat=[0, 0.8509035, 0, 0.525322])\n", + "spec.detach_body(arm_right)\n", + "\n", + "# Attach Franka arm to humanoid\n", + "franka_arm = franka.find_body('fr3_link2')\n", + "shoulder_right.attach_body(franka_arm, 'franka', '')\n", + "\n", + "model = spec.compile()\n", + "render(model, height=400)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "e_idaggAznXu" + }, + "source": [ + "When doing this, the actuators and all other objects referenced by the attached sub-tree are imported in the new model. All assets are currently imported, referenced or not." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "50lOgJ7mQ2bV" + }, + "outputs": [], + "source": [ + "#@title Imported actuators.{vertical-output: true}\n", + "\n", + "for actuator in spec.actuators:\n", + " print(actuator.name)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "APDoWK4mz0aJ" + }, + "source": [ + "Domain randomization can be performed by attaching multiple times the same spec, edited each time with a new instance of randomized parameters." + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "oHjdgkISNLKy" + }, + "outputs": [], + "source": [ + "#@title Humanoid with randomized heads and arm poses.{vertical-output: true}\n", + "\n", + "humanoid = mj.MjSpec.from_file(humanoid_file)\n", + "spec = mj.MjSpec()\n", + "\n", + "# Delete all key frames to avoid name conflicts\n", + "while humanoid.keys:\n", + " humanoid.keys[-1].delete()\n", + "\n", + "# Create a grid of humanoids by attaching humanoid to spec multiple times\n", + "for i in range(4):\n", + " for j in range(4):\n", + " humanoid.materials[0].rgba = [\n", + " np.random.uniform(), np.random.uniform(),\n", + " np.random.uniform(), 1] # Randomize color\n", + " humanoid.find_body('head').first_geom().size = [\n", + " .18*np.random.uniform(), 0, 0] # Randomize head size\n", + " humanoid.find_body('upper_arm_left').quat = [\n", + " np.random.uniform(), np.random.uniform(),\n", + " np.random.uniform(), np.random.uniform()] # Randomize left arm orientation\n", + " humanoid.find_body('upper_arm_right').quat = [\n", + " np.random.uniform(), np.random.uniform(),\n", + " np.random.uniform(), np.random.uniform()] # Randomize right arm orientation\n", + "\n", + " # attach randomized humanoid to parent spec\n", + " frame = spec.worldbody.add_frame(pos=[i, j, 0])\n", + " frame.attach_body(humanoid.find_body('torso'), str(i), str(j))\n", + "\n", + "spec.worldbody.add_light(mode=mj.mjtCamLight.mjCAMLIGHT_TARGETBODYCOM,\n", + " targetbody='3torso3', diffuse=[.8, .8, .8],\n", + " specular=[0.3, 0.3, 0.3], pos=[0, -6, 4], cutoff=30)\n", + "model = spec.compile()\n", + "render(model, height=400)" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": { + "id": "pyy2q_mSVAX1" + }, + "outputs": [], + "source": [] + } + ], + "metadata": { + "colab": { + "collapsed_sections": [ + "sJFuNetilv4m" + ], + "private_outputs": true + }, + "kernelspec": { + "display_name": "Python 3", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} From 13f11d4c45528f1f71e809f3a314c475776266a9 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 28 Oct 2024 10:59:13 -0700 Subject: [PATCH 026/426] Fit typo in mjSpec tutorial link. PiperOrigin-RevId: 690676376 Change-Id: I5f082f5703488403aa65be76d208def68ccc3eb9 --- python/mjspec.ipynb | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index f59cac69..6f9455fd 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -8,7 +8,7 @@ "source": [ "![MuJoCo banner](https://raw.githubusercontent.com/google-deepmind/mujoco/main/banner.png)\n", "\n", - "#

Model Editing

\n", + "#

Model Editing

\n", "\n", "This notebook provides an introductory tutorial for model editing in MuJoCo using the `mjSpec` API. This notebook assumes that the reader is already familiar with MuJoCo basic concepts, as demostrated in the [introductory tutorial](https://github.com/google-deepmind/mujoco?tab=readme-ov-file#getting-started). Documentation for this API can be found in the [Model Editing](https://mujoco.readthedocs.io/en/latest/programming/modeledit.html) chapter in the documentation (C API) and in the [Python chapter](https://mujoco.readthedocs.io/en/latest/python.html#model-editing). Here we use the Python API.\n", "\n", @@ -83,8 +83,8 @@ "# Check if installation was succesful.\n", "try:\n", " print('Checking that the installation succeeded:')\n", - " import mujoco\n", - " mujoco.MjModel.from_xml_string('')\n", + " import mujoco as mj\n", + " mj.MjModel.from_xml_string('')\n", "except Exception as e:\n", " raise e from RuntimeError(\n", " 'Something went wrong during installation. Check the shell output above '\n", @@ -806,9 +806,6 @@ ], "metadata": { "colab": { - "collapsed_sections": [ - "sJFuNetilv4m" - ], "private_outputs": true }, "kernelspec": { From d01fb81b50f6c6d19909b81554ade93eda41bf76 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 28 Oct 2024 13:20:09 -0700 Subject: [PATCH 027/426] Use eq_active0 in MJX. PiperOrigin-RevId: 690727109 Change-Id: If60a99d7644e0376a27039478319ff5300b15cc1 --- mjx/mujoco/mjx/_src/io.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 19f92b55..893e7cb1 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -233,7 +233,6 @@ def make_data( 'ctrl': (m.nu, float), 'qfrc_applied': (m.nv, float), 'xfrc_applied': (m.nbody, 6, float), - 'eq_active': (m.neq, jp.uint8), 'mocap_pos': (m.nmocap, 3, float), 'mocap_quat': (m.nmocap, 4, float), 'qacc': (m.nv, float), @@ -345,6 +344,7 @@ def make_data( qpos=jp.array(m.qpos0), contact=contact, efc_type=efc_type, + eq_active=m.eq_active0, **zero_fields ) From f1d4b23e48a3c71e15a0f36c56e6e04c06886d60 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 28 Oct 2024 14:17:51 -0700 Subject: [PATCH 028/426] mjSpec tutorial backwards compatibility with the latest MuJoCo release. PiperOrigin-RevId: 690747714 Change-Id: Ib8894a1dc2a68e30a110e773c77fbd2baee87efd --- python/mjspec.ipynb | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index 6f9455fd..6d54be72 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -172,7 +172,7 @@ "render(model)\n", "\n", "# Change the mjSpec, re-compile and re-render\n", - "geoms = spec.worldbody.find_all('geom')\n", + "geoms = spec.worldbody.find_all(mj.mjtObj.mjOBJ_GEOM)\n", "geoms[0].name = 'blue_box'\n", "geoms[0].rgba = [0, 0, 1, 1]\n", "geoms[1].name = 'yellow_sphere'\n", @@ -392,7 +392,11 @@ "#@title Six Creatures on a floor.{vertical-output: true}\n", "\n", "arena = mj.MjSpec()\n", - "arena.compiler.degree = False # Use radians.\n", + "\n", + "if hasattr(arena, 'compiler'):\n", + " arena.compiler.degree = False # MuJoCo dev (next release).\n", + "else:\n", + " arena.degree = False # MuJoCo release\n", "\n", "# Make arena with textured floor.\n", "chequered = arena.add_texture(\n", @@ -452,7 +456,7 @@ "video = []\n", "pos_x = []\n", "pos_y = []\n", - "geoms = arena.worldbody.find_all(\"geom\")\n", + "geoms = arena.worldbody.find_all(mj.mjtObj.mjOBJ_GEOM)\n", "torsos = [geom.id for geom in geoms if 'torso' in geom.name]\n", "actuators = [actuator.id for actuator in arena.actuators]\n", "\n", @@ -703,7 +707,11 @@ "\n", "spec = mj.MjSpec.from_file(humanoid_file)\n", "franka = mj.MjSpec.from_file(franka_file)\n", - "spec.compiler.degree = False # This has no effect on the humanoid as it does not contain angles\n", + "\n", + "if hasattr(spec, 'compiler'):\n", + " spec.compiler.degree = False # MuJoCo dev (next release).\n", + "else:\n", + " spec.degree = False # MuJoCo release\n", "\n", "# Replace right arm with frame\n", "arm_right = spec.find_body('upper_arm_right')\n", @@ -805,15 +813,18 @@ } ], "metadata": { + "accelerator": "GPU", "colab": { + "collapsed_sections": [ + "sJFuNetilv4m" + ], + "gpuClass": "premium", "private_outputs": true }, + "gpuClass": "premium", "kernelspec": { "display_name": "Python 3", "name": "python3" - }, - "language_info": { - "name": "python" } }, "nbformat": 4, From ce5f8b472841d2e02d9441d008355e2057f5b3a8 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 29 Oct 2024 03:30:38 -0700 Subject: [PATCH 029/426] Fix typo in `mju_muscleDynamics` comments. PiperOrigin-RevId: 690949399 Change-Id: I8102fb72cce909786eab0771467c7f6f751792bf --- src/engine/engine_util_misc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 4aada4b9..15bad4cc 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -592,8 +592,8 @@ mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) { mjtNum actclamp = mju_clip(act, 0, 1); // compute timescales as in Millard et al. (2013) https://doi.org/10.1115/1.4023390 - mjtNum tau_act = prm[0] * (0.5 + 1.5*actclamp); // activation timscale - mjtNum tau_deact = prm[1] / (0.5 + 1.5*actclamp); // deactivation timscale + mjtNum tau_act = prm[0] * (0.5 + 1.5*actclamp); // activation timescale + mjtNum tau_deact = prm[1] / (0.5 + 1.5*actclamp); // deactivation timescale mjtNum smoothing_width = prm[2]; // width of smoothing sigmoid mjtNum dctrl = ctrlclamp - act; // excess excitation From 6703a40fcfd4b50e51530ebaa395b7bfc5239c28 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 29 Oct 2024 05:06:19 -0700 Subject: [PATCH 030/426] Improve plugin documentation. PiperOrigin-RevId: 690974299 Change-Id: Iadaabafb7434ada9579d6e2f64aae435cdfedbd8 --- doc/programming/extension.rst | 94 ++++++++++++++++++++--------------- 1 file changed, 54 insertions(+), 40 deletions(-) diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index e6dba3c4..8e777718 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -201,22 +201,22 @@ the ``copy`` callback from :ref:`mjpPlugin` for each plugin instance present. .. _exActuatorAct: -Actuator activations -"""""""""""""""""""" +Actuator states +""""""""""""""" When writing stateful actuator plugins, there are two choices for where to save the actuator state. One option is using -``plugin_state`` as described above, and the other is to use ``mjData.act`` by implementing the ``actuator_actdim`` and -``actuator_act_dot`` callbacks on :ref:`mjpPlugin`. +``plugin_state`` as described above, and the other is to use ``mjData.act`` by implementing the callback on +:ref:`mjpPlugin`. When using the latter option, the actuator plugin's state will be added to ``mjData.act``, and MuJoCo will automatically integrate ``mjData.act_dot`` values between timesteps. One advantage of this approach is that finite-differencing functions like :ref:`mjd_transitionFD` will work as they do for native actuators. The ``mjpPlugin.advance`` callback will be called after ``act_dot`` is integrated, and actuator plugins may overwrite -the ``act`` values at that point, if Euler integration isn't appropriate. +the ``act`` values at that point, if the built-in integrator is not appropriate. Users may specify the :ref:`dyntype` attribute on actuator plugins, to introduce a filter or -an integrator between user inputs and actuator activations. When they do, the activation variable introduced by -``dyntype`` will be placed *after* the plugin's activation variables in the ``act`` array. +an integrator between user inputs and actuator states. When they do, the state variable introduced by +``dyntype`` will be placed *after* the plugin's state variables in the ``act`` array. .. _exRegistration: @@ -269,43 +269,57 @@ A future version of this section will include: There are several first-party plugin directories: -* **actuator:** The plugins in the `actuator/ `__ - directory implement custom actuators, so far only a PID controller. See the - `README `__ for details. -* **elasticity:** The plugins in the `elasticity/ - `__ directory are passive forces based on - continuum mechanics for 1-dimensional and 2-dimensional bodies. The 1D model is invariant under rotations and captures - the large deformation of elastic cables, decoupling twisting and bending strains. The 2D model is a suitable for - computing the bending stiffness of thin elastic plates (i.e. shells having a flat stress-free configuration). In this - case, the elastic energy is quadratic and therefore the stiffness matrix is constant. For more information, please see - the `README `__. -* **sensor:** The plugins in the `sensor/ `__ - directory implement custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the - `README `__ for details. -* **sdf:** The plugins in the `sdf/ `__ directory - specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its gradient at - query points. This shape then acts as a new geom type in the collision table at the top of `engine_collision_driver.c - `__. For more information - concerning the available SDFs and how to write your own implicit geometry, please see the `README - `__. The rest of this section will give more - detail concerning the collision algorithm and the plugin engine interface. +actuator +"""""""" +The plugins in the `actuator/ `__ directory +implement custom actuators, so far only a PID controller. See the `README +`__ for details. - Collision points are found by minimizing the function A + B + abs(max(A, B)), where A and B are the two colliding - SDFs, via gradient descent. Because SDFs are non-convex, multiple starting points are required in order to converge to - multiple local minima. The number of starting points is set using :ref:`sdf_initpoints`, and - are initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes. The number of - gradient descent iterations is set using :ref:`sdf_iterations`. - While *exact* SDFs---encoding the precise signed distance to the surface---are preferred, collisions are possible with - any function whose value vanishes at the surface and grows monotonically away from it, with a negative sign in the - interior. For such functions, it is still possible to find collisons, albeit with a possibly - increased number of starting points. +elasticity +"""""""""" +The plugins in the `elasticity/ `__ directory are +passive forces based on continuum mechanics for 1-dimensional and 2-dimensional bodies. The 1D model is invariant under +rotations and captures the large deformation of elastic cables, decoupling twisting and bending strains. The 2D model is +a suitable for computing the bending stiffness of thin elastic plates (i.e. shells having a flat stress-free +configuration). In this case, the elastic energy is quadratic and therefore the stiffness matrix is constant. For more +information, please see the `README +`__. - The ``sdf_distance`` method is called by the compiler to produce a visual mesh for rendering using the marching cubes - algorithm implemented by `MarchingCubeCpp `__. - Future improvement to the gradient descent algorithm, such as a line search which takes advantage of the properties of - SDFs, might reduce the number of iterations and/or starting points. +sensor +"""""" +The plugins in the `sensor/ `__ directory implement +custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the `README +`__ for details. + + +sdf +""" +The plugins in the `sdf/ `__ directory +specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its gradient at +query points. This shape then acts as a new geom type in the collision table at the top of `engine_collision_driver.c +`__. For more information +concerning the available SDFs and how to write your own implicit geometry, please see the `README +`__. The rest of this section will give more +detail concerning the collision algorithm and the plugin engine interface. + +Collision points are found by minimizing the function A + B + abs(max(A, B)), where A and B are the two colliding +SDFs, via gradient descent. Because SDFs are non-convex, multiple starting points are required in order to converge to +multiple local minima. The number of starting points is set using :ref:`sdf_initpoints`, and +are initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes. The number of +gradient descent iterations is set using :ref:`sdf_iterations`. + +While *exact* SDFs---encoding the precise signed distance to the surface---are preferred, collisions are possible with +any function whose value vanishes at the surface and grows monotonically away from it, with a negative sign in the +interior. For such functions, it is still possible to find collisons, albeit with a possibly +increased number of starting points. + +The ``sdf_distance`` method is called by the compiler to produce a visual mesh for rendering using the marching cubes +algorithm implemented by `MarchingCubeCpp `__. + +Future improvement to the gradient descent algorithm, such as a line search which takes advantage of the properties of +SDFs, might reduce the number of iterations and/or starting points. For the sdf plugin, the following methods need to be specified From 71cfcc54326ca23ea163f023c494b11232a6f8d5 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Tue, 29 Oct 2024 05:20:07 -0700 Subject: [PATCH 031/426] Use assertIn instead of assertContainsSubsequence for substring checks. A contained subsequence must not necessarily be continuous, so the check only checked if the individual letters were appearing in order within the message, possibly interspersed with other characters. PiperOrigin-RevId: 690977874 Change-Id: I36bd5a0105b979a9b0d6d04ca5239245d4f8d041 --- python/mujoco/minimize_test.py | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/python/mujoco/minimize_test.py b/python/mujoco/minimize_test.py index 6d96f52c..6f4cd3cc 100644 --- a/python/mujoco/minimize_test.py +++ b/python/mujoco/minimize_test.py @@ -32,7 +32,7 @@ class MinimizeTest(absltest.TestCase): x, _ = minimize.least_squares(x0, residual, output=out) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol') + self.assertIn('norm(dx) < tol', out.getvalue()) def test_start_at_minimum(self) -> None: def residual(x): @@ -43,8 +43,8 @@ class MinimizeTest(absltest.TestCase): x, _ = minimize.least_squares(x0, residual, output=out) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol') - self.assertContainsSubsequence(out.getvalue(), 'exact minimum found') + self.assertIn('norm(dx) < tol', out.getvalue()) + self.assertIn('exact minimum found', out.getvalue()) def test_jac_callback(self) -> None: def residual(x): @@ -60,8 +60,8 @@ class MinimizeTest(absltest.TestCase): check_derivatives=True) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol') - self.assertContainsSubsequence(out.getvalue(), 'Jacobian matches') + self.assertIn('norm(dx) < tol', out.getvalue()) + self.assertIn('Jacobian matches', out.getvalue()) # Try with bad Jacobian, ask least_squares to check it. def bad_jacobian(x, r): @@ -83,7 +83,7 @@ class MinimizeTest(absltest.TestCase): x0 = np.zeros(dim) out = io.StringIO() minimize.least_squares(x0, residual, max_iter=20, output=out) - self.assertContainsSubsequence(out.getvalue(), 'maximum iterations') + self.assertIn('maximum iterations', out.getvalue()) # Succeed after 100 iterations (default). x, _ = minimize.least_squares(x0, residual) @@ -106,7 +106,7 @@ class MinimizeTest(absltest.TestCase): x, _ = minimize.least_squares(x0, residual, bounds=bounds_types['inbounds'], output=out) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol') + self.assertIn('norm(dx) < tol', out.getvalue()) # Test different bounds conditions. for bounds in bounds_types.values(): @@ -118,7 +118,7 @@ class MinimizeTest(absltest.TestCase): output=out, verbose=minimize.Verbosity.FULLITER, ) - self.assertContainsSubsequence(out.getvalue(), ' < tol') + self.assertIn(' < tol', out.getvalue()) grad = trace[-2].jacobian.T @ trace[-2].residual # If x_i is on the boundary, gradient points out, otherwise it is 0. for i, xi in enumerate(x): @@ -161,7 +161,7 @@ class MinimizeTest(absltest.TestCase): iter_callback=iter_callback) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'Hello iteration 3!') + self.assertIn('Hello iteration 3!', out.getvalue()) def test_norm(self) -> None: def residual(x): @@ -187,11 +187,9 @@ class MinimizeTest(absltest.TestCase): check_derivatives=True) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) - self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol') - self.assertContainsSubsequence(out.getvalue(), - 'User-provided norm gradient matches') - self.assertContainsSubsequence(out.getvalue(), - 'User-provided norm Hessian matches') + self.assertIn('norm(dx) < tol', out.getvalue()) + self.assertIn('User-provided norm gradient matches', out.getvalue()) + self.assertIn('User-provided norm Hessian matches', out.getvalue()) class SmoothL2BadGrad(minimize.Norm): def value(self, r): From 0995af83f970cb218069a2ace2bc56321c0177f1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 29 Oct 2024 05:31:13 -0700 Subject: [PATCH 032/426] Add documentation for SdfLib plugin. Fixes #2182. PiperOrigin-RevId: 690980373 Change-Id: Ifc94f928e9cfbdbef3cb6f11ad4bffed542bf49a --- plugin/sdf/README.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/plugin/sdf/README.md b/plugin/sdf/README.md index 11ca621b..4646e1d9 100644 --- a/plugin/sdf/README.md +++ b/plugin/sdf/README.md @@ -62,6 +62,16 @@ Parameters: - `radius1` [m]: major radius (default `0.35`). - `radius1` [m]: minor radius (default `0.15`). +### SdfLib + +Implemented in [sdflib.cc](sdflib.cc). Example usage in [cow.xml](../../model/plugin/sdf/cow.xml). + +This plugin uses the library [SdfLib](https://github.com/UPC-ViRVIG/SdfLib) to compute a voxel-based approximation of a +user-specified mesh. The mesh can be arbitrary and not necessarily convex. This offers an alternative to +convex-decomposed meshes. The performance is likely to be slower than that of analytic SDFs, since a cubic +approximation has to be evaluated on the convex grid. However, the SDF generation is done automatically, simplifying the +task of creating an SDF, which can be difficult for complex shapes. + ### How to make your own SDF Create your `MySDF.h` and `MySDF.cc` files in the SDF folder, where this README is located. Implement your SDF using the @@ -98,3 +108,10 @@ class MySDF { MySDF(const mjModel* m, mjData* d, int instance); }; ``` + + + + + + + From 1336e99d8837c8951ad478bc49804b8942fffae9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 29 Oct 2024 06:01:01 -0700 Subject: [PATCH 033/426] Remove test annotation from mjspec notebook PiperOrigin-RevId: 690987516 Change-Id: Ia99b65278609166ecec9002e5290c44a54255d80 --- python/mjspec.ipynb | 1 - 1 file changed, 1 deletion(-) diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index 6d54be72..772faf30 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -449,7 +449,6 @@ "outputs": [], "source": [ "#@title Video of the movement{vertical-output: true}\n", - "#@test {\"timeout\": 600}\n", "\n", "duration = 10 # (Seconds)\n", "framerate = 30 # (Hz)\n", From e889cffe23c9110f74f6a2303f6d8cdd906032ca Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 29 Oct 2024 08:54:39 -0700 Subject: [PATCH 034/426] Clarify in mju_boxQP test that for the synthetic test set, the average number of factorizations is expected to be 5 or less, while the worst case is expected to be 6 or less. PiperOrigin-RevId: 691037804 Change-Id: Id0bc8bedf08d623efcacbe696775f0ef0f6781b9 --- test/engine/engine_util_solve_test.cc | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index 7d56e525..d4bea9a1 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/test/engine/engine_util_solve_test.cc @@ -16,8 +16,12 @@ #include "src/engine/engine_util_solve.h" +#include +#include #include #include +#include +#include #include #include @@ -332,12 +336,21 @@ TEST_F(BoxQPTest, BoundedQPvariations) { string slog(log); string factorstr = "factorizations="; std::size_t index = slog.find(factorstr) + factorstr.length(); - factorizations += std::stoi(slog.substr(index, 3)); + int num_factor = std::stoi(slog.substr(index, 3)); + + // never more than 6 factorizations + EXPECT_LE(num_factor, 6); + + factorizations += num_factor; count++; } } } double meanfactor = ((double)factorizations) / count; + + // average of 4.5 factorizations is expected + EXPECT_LE(meanfactor, 5.0); + std::cerr << "n=" << setw(3) << n << ": average of " << meanfactor << " factorizations\n"; } From b941e994d9b8a0e482d750ee15a578e501fed72b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 29 Oct 2024 09:16:54 -0700 Subject: [PATCH 035/426] Move keyframe resizing from compilation to attach (mjCModel::operator+=). This allows to resize parent keyframes also when the child has no keyframes. PiperOrigin-RevId: 691045299 Change-Id: I3ab6509b13b5754e937ec77daf9efe697cabf1bc --- src/user/user_model.cc | 42 +++++++++---------- src/user/user_model.h | 4 +- test/user/user_api_test.cc | 85 +++++++++++++++++++++++++++++++++----- 3 files changed, 96 insertions(+), 35 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 99ab9a11..257a394a 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -401,6 +401,16 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) { } } + // resize keyframes in the parent model + if (!keys_.empty()) { + SaveDofOffsets(/*computesize=*/true); + ComputeReference(); + for (auto* key : keys_) { + ResizeKeyframe(key, qpos0.data(), body_pos0.data(), body_quat0.data()); + } + nq = nv = na = nu = nmocap = 0; + } + // restore to the original state if (!compiled) { ResetTreeLists(); @@ -3173,10 +3183,7 @@ void mjCModel::StoreKeyframes(mjCModel* dest) { // do not change compilation quantities in case the user wants to recompile preserving the state if (!compiled) { SaveDofOffsets(/*computesize=*/true); - qpos0.resize(nq); - body_pos0.resize(3*bodies_.size()); - body_quat0.resize(4*bodies_.size()); - ComputeReference(qpos0, body_pos0, body_quat0); + ComputeReference(); } // save keyframe info and resize keyframes @@ -3729,26 +3736,28 @@ void mjCModel::CompileMeshes(const mjVFS* vfs) { // compute qpos0 -template -void mjCModel::ComputeReference(std::vector& q0, std::vector& bpos, std::vector& bquat) { +void mjCModel::ComputeReference() { int b = 0; + qpos0.resize(nq); + body_pos0.resize(3*bodies_.size()); + body_quat0.resize(4*bodies_.size()); for (auto body : bodies_) { - mjuu_copyvec(bpos.data()+3*b, body->spec.pos, 3); - mjuu_copyvec(bquat.data()+4*b, body->spec.quat, 4); + mjuu_copyvec(body_pos0.data()+3*b, body->spec.pos, 3); + mjuu_copyvec(body_quat0.data()+4*b, body->spec.quat, 4); for (auto joint : body->joints) { switch (joint->type) { case mjJNT_FREE: - mjuu_copyvec(q0.data()+joint->qposadr_, body->spec.pos, 3); - mjuu_copyvec(q0.data()+joint->qposadr_+3, body->spec.quat, 4); + mjuu_copyvec(qpos0.data()+joint->qposadr_, body->spec.pos, 3); + mjuu_copyvec(qpos0.data()+joint->qposadr_+3, body->spec.quat, 4); break; case mjJNT_BALL: - mjuu_setvec(q0.data()+joint->qposadr_, 1, 0, 0, 0); + mjuu_setvec(qpos0.data()+joint->qposadr_, 1, 0, 0, 0); break; case mjJNT_SLIDE: case mjJNT_HINGE: - q0[joint->qposadr_] = (T)joint->spec.ref; + qpos0[joint->qposadr_] = (mjtNum)joint->spec.ref; break; default: @@ -3811,18 +3820,9 @@ void mjCModel::ResizeKeyframe(mjCKey* key, const mjtNum* qpos0_, // convert pending keyframes info to actual keyframes void mjCModel::ResolveKeyframes(const mjModel* m) { - if (key_pending_.empty()) { - return; - } - // store dof offsets in joints and actuators SaveDofOffsets(); - // resize existing keyframes to the new state, fill in missing default values - for (auto* key : keys_) { - ResizeKeyframe(key, m->qpos0, m->body_pos, m->body_quat); - } - // create new keyframes, fill in missing default values for (const auto& info : key_pending_) { mjCKey* key = (mjCKey*)FindObject(mjOBJ_KEY, info.name); diff --git a/src/user/user_model.h b/src/user/user_model.h index a6e9462d..d65d93d9 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -393,9 +393,7 @@ class mjCModel : public mjCModel_, private mjSpec { void ResizeKeyframe(mjCKey* key, const mjtNum* qpos0_, const mjtNum* bpos, const mjtNum* bquat); // compute qpos0 - template - void ComputeReference(std::vector& q0, std::vector& bpos, - std::vector& bquat); + void ComputeReference(); mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index ab824637..93c0c1ab 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1489,7 +1489,7 @@ TEST_F(MujocoTest, AttachMocap) { - + )"; @@ -1497,11 +1497,11 @@ TEST_F(MujocoTest, AttachMocap) { - + - - + + )"; @@ -1519,13 +1519,6 @@ TEST_F(MujocoTest, AttachMocap) { mjsBody* attached_body = mjs_findBody(spec, "attached-mocap-1"); EXPECT_THAT(attached_body, NotNull()); - attached_body->pos[0] = 3; - attached_body->pos[1] = 3; - attached_body->pos[2] = 3; - attached_body->quat[0] = 0; - attached_body->quat[1] = 0; - attached_body->quat[2] = 1; - attached_body->quat[3] = 0; mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); @@ -1839,6 +1832,76 @@ TEST_F(MujocoTest, RepeatedAttachKeyframe) { mj_deleteModel(model_2); } +TEST_F(MujocoTest, ResizeParentKeyframe) { + static constexpr char xml_parent[] = R"( + + + + + + + + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + )"; + + static constexpr char xml_expected[] = R"( + + + + + + + + + + + + + + + + + )"; + + std::array er; + mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); + + mjs_attachBody(mjs_findFrame(parent, "frame"), mjs_findBody(child, "body"), + "child-", ""); + + mjModel* model = mj_compile(parent, 0); + EXPECT_THAT(model, NotNull()); + + mjtNum tol = 0; + std::string field = ""; + mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(expected, NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected, field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + + mj_deleteSpec(parent); + mj_deleteSpec(child); + mj_deleteModel(model); + mj_deleteModel(expected); +} + TEST_F(MujocoTest, DifferentUnitsAllowed) { mjSpec* child = mj_makeSpec(); child->compiler.degree = 1; From e835f139c83b3b94a1629e5470c5e2465d903354 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 29 Oct 2024 09:54:07 -0700 Subject: [PATCH 036/426] Add sibling traversal for tree elements. PiperOrigin-RevId: 691057957 Change-Id: I3f30cf175b5ad0842f3fbbf7fdb42ba7a2212682 --- python/mujoco/specs.cc | 52 +++++++++++++++++++++++++++++++++---- python/mujoco/specs_test.py | 23 ++++++++++++++-- 2 files changed, 68 insertions(+), 7 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index fe7e22fb..b2beaa3f 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -155,9 +155,9 @@ void DefineArray(py::module& m, const std::string& typestr) { }, py::keep_alive<0, 1>(), py::return_value_policy::reference_internal); }; -py::list FindAllImpl(raw::MjsBody& body, mjtObj objtype) { +py::list FindAllImpl(raw::MjsBody& body, mjtObj objtype, bool recursive) { py::list list; - raw::MjsElement* el = mjs_firstChild(&body, objtype, true); + raw::MjsElement* el = mjs_firstChild(&body, objtype, recursive); std::string error = mjs_getError(mjs_getSpec(body.element)); if (!el && !error.empty()) { throw pybind11::value_error(error); @@ -192,7 +192,7 @@ py::list FindAllImpl(raw::MjsBody& body, mjtObj objtype) { "light, camera."); break; } - el = mjs_nextChild(&body, el, true); + el = mjs_nextChild(&body, el, recursive); } return list; // list of pointers, so they can be copied } @@ -460,7 +460,7 @@ PYBIND11_MODULE(_specs, m) { mjsBody.def( "find_all", [](raw::MjsBody& self, mjtObj objtype) -> py::list { - return FindAllImpl(self, objtype); + return FindAllImpl(self, objtype, true); }, py::return_value_policy::reference_internal); mjsBody.def( @@ -484,7 +484,7 @@ PYBIND11_MODULE(_specs, m) { "body.find_all supports the types: body, frame, geom, site, " "light, camera."); } - return FindAllImpl(self, objtype); + return FindAllImpl(self, objtype, true); }, py::return_value_policy::reference_internal); mjsBody.def( @@ -505,6 +505,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asBody(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "bodies", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_BODY, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_camera", [](raw::MjsBody& self) -> raw::MjsCamera* { @@ -517,6 +523,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asCamera(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "cameras", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_CAMERA, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_light", [](raw::MjsBody& self) -> raw::MjsLight* { @@ -529,6 +541,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asLight(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "lights", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_LIGHT, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_joint", [](raw::MjsBody& self) -> raw::MjsJoint* { @@ -541,6 +559,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asJoint(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "joints", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_JOINT, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_geom", [](raw::MjsBody& self) -> raw::MjsGeom* { @@ -553,6 +577,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asGeom(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "geoms", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_GEOM, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_site", [](raw::MjsBody& self) -> raw::MjsSite* { @@ -565,6 +595,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asSite(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "sites", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_SITE, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "first_frame", [](raw::MjsBody& self) -> raw::MjsFrame* { @@ -577,6 +613,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_asFrame(mjs_nextChild(&self, child.element, false)); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "frames", + [](raw::MjsBody& self) -> py::list { + return FindAllImpl(self, mjOBJ_FRAME, false); + }, + py::return_value_policy::reference_internal); mjsBody.def( "spec", [](raw::MjsBody& self) -> raw::MjSpec* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index fc325a4a..7bca48f0 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -602,8 +602,12 @@ class SpecsTest(absltest.TestCase): + + + + - + @@ -614,6 +618,9 @@ class SpecsTest(absltest.TestCase): spec = mujoco.MjSpec.from_string(main_xml) bodytype = mujoco.mjtObj.mjOBJ_BODY self.assertLen(spec.bodies, 5) + self.assertLen(spec.sites, 5) + self.assertLen(spec.worldbody.find_all('body'), 4) + self.assertLen(spec.worldbody.find_all('site'), 5) self.assertEqual(spec.bodies[1].name, 'body1') self.assertEqual(spec.bodies[2].name, 'body2') self.assertEqual(spec.bodies[3].name, 'body3') @@ -630,7 +637,19 @@ class SpecsTest(absltest.TestCase): self.assertEqual(spec.bodies[3].find_all('body')[0].name, 'body4') self.assertEmpty(spec.bodies[2].find_all('body')) self.assertEmpty(spec.bodies[4].find_all('body')) - self.assertEqual(spec.worldbody.find_all('site')[0].name, 'site') + self.assertEqual(spec.worldbody.find_all('site')[0].name, 'site1') + self.assertEqual(spec.worldbody.find_all('site')[1].name, 'site2') + self.assertEqual(spec.worldbody.find_all('site')[2].name, 'site3') + self.assertEqual(spec.worldbody.find_all('site')[3].name, 'site4') + self.assertEqual(spec.worldbody.find_all('site')[4].name, 'site5') + self.assertEmpty(spec.bodies[2].sites) + self.assertLen(spec.bodies[3].sites, 4) + self.assertLen(spec.bodies[4].sites, 1) + self.assertEqual(spec.bodies[3].sites[0].name, 'site1') + self.assertEqual(spec.bodies[3].sites[1].name, 'site2') + self.assertEqual(spec.bodies[3].sites[2].name, 'site3') + self.assertEqual(spec.bodies[3].sites[3].name, 'site4') + self.assertEqual(spec.bodies[4].sites[0].name, 'site5') with self.assertRaises(ValueError) as cm: spec.worldbody.find_all('actuator') self.assertEqual( From f47840e57c80b3782da0d39eb4fba1fc45122760 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 29 Oct 2024 12:47:03 -0700 Subject: [PATCH 037/426] Add `mju_dense2sparse` to public API. PiperOrigin-RevId: 691125900 Change-Id: Id9b7c739ee14bff2317168da258b1a3acbdafdbe --- doc/APIreference/functions.rst | 12 ++++++- doc/includes/references.h | 2 ++ include/mujoco/mujoco.h | 7 +++- introspect/functions.py | 52 +++++++++++++++++++++++++++- python/mujoco/bindings_test.py | 17 +++++++++ python/mujoco/functions.cc | 19 ++++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 ++ 7 files changed, 109 insertions(+), 3 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 878bbf00..4ef49615 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3414,6 +3414,16 @@ rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motio Sparse math ^^^^^^^^^^^ +.. _mju_dense2sparse: + +mju_dense2sparse +~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_dense2sparse + +Convert matrix from dense to sparse. + nnz is size of res and colind, return 1 if too small, 0 otherwise. + .. _mju_sparse2dense: mju_sparse2dense @@ -4372,7 +4382,7 @@ mjs_setFrame .. mujoco-include:: mjs_setFrame -Set element's enlcosing frame. +Set element's enclosing frame. .. _mjs_resolveOrientation: diff --git a/doc/includes/references.h b/doc/includes/references.h index 9a777763..6c0c1d41 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3464,6 +3464,8 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, const mjtNum newpos[3], const mjtNum oldpos[3], const mjtNum rotnew2old[9]); +int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, + int* rownnz, int* rowadr, int* colind, int nnz); void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fae4600f..54b807e9 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1069,6 +1069,11 @@ MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_forc //---------------------------------- Sparse math --------------------------------------------------- +// Convert matrix from dense to sparse. +// nnz is size of res and colind, return 1 if too small, 0 otherwise. +MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, + int* rownnz, int* rowadr, int* colind, int nnz); + // Convert matrix from sparse to dense. MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); @@ -1611,7 +1616,7 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); // Set element's default. MJAPI void mjs_setDefault(mjsElement* element, mjsDefault* def); -// Set element's enlcosing frame. +// Set element's enclosing frame. MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); // Resolve alternative orientations to quat, return error if any. diff --git a/introspect/functions.py b/introspect/functions.py index 2afb3798..6c3a5b9e 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6958,6 +6958,56 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Coordinate transform of 6D motion or force vector in rotation:translation format. rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motion type.', # pylint: disable=line-too-long )), + ('mju_dense2sparse', + FunctionDecl( + name='mju_dense2sparse', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='res', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='mat', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='nr', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='nc', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='rownnz', + type=PointerType( + inner_type=ValueType(name='int'), + ), + ), + FunctionParameterDecl( + name='rowadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + ), + FunctionParameterDecl( + name='colind', + type=PointerType( + inner_type=ValueType(name='int'), + ), + ), + FunctionParameterDecl( + name='nnz', + type=ValueType(name='int'), + ), + ), + doc='Convert matrix from dense to sparse. nnz is size of res and colind, return 1 if too small, 0 otherwise.', # pylint: disable=line-too-long + )), ('mju_sparse2dense', FunctionDecl( name='mju_sparse2dense', @@ -10236,7 +10286,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc="Set element's enlcosing frame.", + doc="Set element's enclosing frame.", )), ('mjs_resolveOrientation', FunctionDecl( diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 5448a609..ffdfb75b 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1337,6 +1337,23 @@ Euler integrator, semi-implicit in velocity. mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]]) self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.) + def test_mju_dense_to_sparse(self): + mat = np.array([[0., 1., 0.], [2., 0., 3.]]) + expected_vals = np.array([1., 2., 3.]) + expected_rownnz = np.array([1, 2]) + expected_rowadr = np.array([0, 1]) + expected_colind = np.array([1, 0, 2]) + vals = np.zeros(3) + row_nnz = np.zeros(2, np.int32) + row_adr = np.zeros(2, np.int32) + col_ind = np.zeros(3, np.int32) + status = mujoco.mju_dense2sparse(vals, mat, row_nnz, row_adr, col_ind) + np.testing.assert_equal(status, 0) + np.testing.assert_array_equal(vals, expected_vals) + np.testing.assert_array_equal(row_nnz, expected_rownnz) + np.testing.assert_array_equal(row_adr, expected_rowadr) + np.testing.assert_array_equal(col_ind, expected_colind) + def test_mju_sparse_to_dense(self): expected = np.array([[0., 1., 0.], [2., 0., 3.]]) mat = np.array((1., 2., 3.)) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 47bf5314..5bb00ba9 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1019,6 +1019,25 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); // Sparse math + DEF_WITH_OMITTED_PY_ARGS(traits::mju_dense2sparse, "nr", "nc", "nnz")( + pymodule, + [](Eigen::Ref res, Eigen::Ref mat, + Eigen::Ref rownnz, Eigen::Ref rowadr, + Eigen::Ref colind) { + if (mat.rows() != rownnz.size()) { + throw py::type_error("#rows in mat should equal size of rownnz"); + } + if (mat.rows() != rowadr.size()) { + throw py::type_error("#rows in mat should equal size of rowadr"); + } + if (res.size() != colind.size()) { + throw py::type_error("#size of res should equal size of colind"); + } + return ::mju_dense2sparse(res.data(), mat.data(), mat.rows(), + mat.cols(), rownnz.data(), rowadr.data(), + colind.data(), res.size()); + }); + DEF_WITH_OMITTED_PY_ARGS(traits::mju_sparse2dense, "nr", "nc")( pymodule, [](Eigen::Ref res, diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 51a13917..e963ab1c 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7230,6 +7230,9 @@ public static unsafe extern void mju_sqrMatTD(double* res, double* mat, double* [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_transformSpatial(double* res, double* vec, int flg_force, double* newpos, double* oldpos, double* rotnew2old); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mju_dense2sparse(double* res, double* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind, int nnz); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_sparse2dense(double* res, double* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind); From 03d04f60968bb65d1dd436d9f4651f10f4af4b32 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Tue, 29 Oct 2024 14:48:10 -0700 Subject: [PATCH 038/426] Add texture fields to mjx.Model for madrona collaborators. PiperOrigin-RevId: 691170517 Change-Id: Icb8e2d6ed65a13d17dc2565d3d84de629a10c37a --- mjx/mujoco/mjx/_src/types.py | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index f8fdbd23..c36fa66c 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -514,6 +514,8 @@ class Model(PyTreeNode): nmeshgraph: number of ints in mesh auxiliary data nhfield: number of heightfields nhfielddata: number of data points in all heightfields + ntex: number of textures + ntexdata: number of bytes in texture rgb data nmat: number of materials npair: number of predefined geom pairs nexclude: number of excluded geom pairs @@ -635,6 +637,7 @@ class Model(PyTreeNode): light_mode: light tracking mode (mjtCamLight) (nlight,) light_bodyid: id of light's body (nlight,) light_targetbodyid: id of targeted body; -1: none (nlight,) + light_directional: directional light (nlight,) light_pos: position rel. to body frame (nlight, 3) light_dir: direction rel. to body frame (nlight, 3) light_poscom0: global position rel. to sub-com in qpos0 (nlight, 3) @@ -692,12 +695,22 @@ class Model(PyTreeNode): mesh_pos: translation applied to asset vertices (nmesh, 3) mesh_quat: rotation applied to asset vertices (nmesh, 4) mesh_convex: pre-compiled convex mesh info for MJX (nmesh,) + mesh_texcoordadr: texcoord data address; -1: no texcoord (nmesh,) + mesh_texcoordnum: number of texcoord (nmesh,) + mesh_texcoord: vertex texcoords for all meshes (nmeshtexcoord, 2) hfield_size: (x, y, z_top, z_bottom) (nhfield,) hfield_nrow: number of rows in grid (nhfield,) hfield_ncol: number of columns in grid (nhfield,) hfield_adr: address in hfield_data (nhfield,) hfield_data: elevation data (nhfielddata,) + tex_type: texture type (mjtTexture) (ntex,) + tex_height: number of rows in texture image (ntex,) + tex_width: number of columns in texture image (ntex,) + tex_nchannel: number of channels in texture image (ntex,) + tex_adr: start address in tex_data (ntex,) + tex_data: pixel values (ntexdata,) mat_rgba: rgba (nmat, 4) + mat_texid: indices of textures; -1: none (nmat, mjNTEXROLE) pair_dim: contact dimensionality (npair,) pair_geom1: id of geom1 (npair,) pair_geom2: id of geom2 (npair,) @@ -820,6 +833,8 @@ class Model(PyTreeNode): nmeshgraph: int nhfield: int nhfielddata: int + ntex: int + ntexdata: int nmat: int npair: int nexclude: int @@ -944,14 +959,15 @@ class Model(PyTreeNode): cam_resolution: np.ndarray cam_sensorsize: np.ndarray cam_intrinsic: np.ndarray - light_mode: np.ndarray = _restricted_to('mujoco') + light_mode: np.ndarray light_bodyid: np.ndarray = _restricted_to('mujoco') light_targetbodyid: np.ndarray = _restricted_to('mujoco') + light_directional: np.ndarray light_pos: np.ndarray = _restricted_to('mujoco') light_dir: np.ndarray = _restricted_to('mujoco') light_poscom0: np.ndarray = _restricted_to('mujoco') - light_pos0: np.ndarray = _restricted_to('mujoco') - light_dir0: np.ndarray = _restricted_to('mujoco') + light_pos0: np.ndarray + light_dir0: np.ndarray flex_contype: np.ndarray = _restricted_to('mujoco') flex_conaffinity: np.ndarray = _restricted_to('mujoco') flex_condim: np.ndarray = _restricted_to('mujoco') @@ -1004,12 +1020,22 @@ class Model(PyTreeNode): mesh_pos: np.ndarray mesh_quat: np.ndarray mesh_convex: Tuple[ConvexMesh, ...] = _restricted_to('mjx') + mesh_texcoordadr: np.ndarray + mesh_texcoordnum: np.ndarray + mesh_texcoord: np.ndarray hfield_size: np.ndarray hfield_nrow: np.ndarray hfield_ncol: np.ndarray hfield_adr: np.ndarray hfield_data: jax.Array + tex_type: np.ndarray + tex_height: np.ndarray + tex_width: np.ndarray + tex_nchannel: np.ndarray + tex_adr: np.ndarray + tex_data: jax.Array mat_rgba: np.ndarray + mat_texid: np.ndarray pair_dim: np.ndarray pair_geom1: np.ndarray pair_geom2: np.ndarray From e1cb43819a8343c18aacd213079b734a5992da57 Mon Sep 17 00:00:00 2001 From: Yuxuan Jiang Date: Wed, 30 Oct 2024 14:27:32 +0800 Subject: [PATCH 039/426] Fix typo in MJDATA_VECTOR definition --- include/mujoco/mjxmacro.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index fc41b0a1..3ca67c16 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -766,7 +766,7 @@ X( size_t, maxuse_threadstack, mjMAXTHREAD, 1 ) \ X( mjWarningStat, warning, mjNWARNING, 1 ) \ X( mjTimerStat, timer, mjNTIMER, 1 ) \ - X( mjSolverStat, solver, mjNILSAND, mjNSOLVER ) \ + X( mjSolverStat, solver, mjNISLAND, mjNSOLVER ) \ X( int, solver_niter, mjNISLAND, 1 ) \ X( int, solver_nnz, mjNISLAND, 1 ) \ X( mjtNum, solver_fwdinv, 2, 1 ) \ From 32df8a515a63eb770b6f1a81ea25d0ce96c5f77e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 30 Oct 2024 08:33:38 -0700 Subject: [PATCH 040/426] Fix typos. fixes #2139 PiperOrigin-RevId: 691429879 Change-Id: I929b7a387ec41f68b8d85fe20de83083216da36a --- doc/programming/extension.rst | 6 +++--- doc/programming/index.rst | 2 +- doc/programming/modeledit.rst | 4 ++-- doc/programming/samples.rst | 6 +++--- doc/programming/visualization.rst | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 8e777718..1963b213 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -312,7 +312,7 @@ gradient descent iterations is set using :ref:`sdf_iterations`: This callback is optional and is used to check if an existing - opened resource has been modifed from its orginal source. + opened resource has been modified from its original source. .. _exProviderUsage: diff --git a/doc/programming/index.rst b/doc/programming/index.rst index f6e4cb3e..6486ab61 100644 --- a/doc/programming/index.rst +++ b/doc/programming/index.rst @@ -143,7 +143,7 @@ links below, to make this documentation self-contained. `mujoco.h `__ This is the main header file and must be included in all programs using MuJoCo. It defines all API functions and - global variables, and includes the all other header files except mjxmacro.h. + global variables, and includes all other header files except mjxmacro.h. `mjmodel.h `__ Defines the C structure :ref:`mjModel` which is the runtime representation of the model being simulated. It also defines a number of primitive types and other structures needed to define mjModel. diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index 8968d4cc..d7752964 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -18,7 +18,7 @@ Overview ~~~~~~~~ The new API augments the traditional workflow of creating and editing models using XML files, breaking up the *parse* and -*compile* steps. As summarized in the the :ref:`Overview chapter`, the traditional workflow is: +*compile* steps. As summarized in the :ref:`Overview chapter`, the traditional workflow is: 1. Create an XML model description file (MJCF or URDF) and associated assets. |br| 2. Call :ref:`mj_loadXML`, obtain an :ref:`mjModel` instance. @@ -29,7 +29,7 @@ The new workflow is: :ref:`mjSpec`. 2. Edit the mutable :ref:`mjSpec` datastructure adding, changing and removing elements. 3. Compile the :ref:`mjSpec` at any point, obtaining an updated :ref:`mjModel` instance. After compilation, the - :ref:`mjSpec` remains editable, so steps 2 and 3 are interchangable. + :ref:`mjSpec` remains editable, so steps 2 and 3 are interchangeable. .. _meUsage: diff --git a/doc/programming/samples.rst b/doc/programming/samples.rst index 40cbd434..019cc899 100644 --- a/doc/programming/samples.rst +++ b/doc/programming/samples.rst @@ -61,8 +61,8 @@ Where the command line arguments are - The ``ctrlnoise`` argument prevents models from settling into a static state where, due to warmstarts, one can measure artificially faster simulation. - When ``npoolthread > 1`` is specified, an engine-internal :ref:`mjThreadPool` is created with the specified number of - threads, to speed up simulation of large scenes. Note that while it is possible to to use both ``nthread`` and - ``npoolthread``, the scenarios for which one would want these different type of multithreading are usually mutually + threads, to speed up simulation of large scenes. Note that while it is possible to use both ``nthread`` and + ``npoolthread``, the scenarios for which one would want these different types of multithreading are usually mutually exclusive. - For more repeatable performance statistics, run the tool with the ``performance`` `governor `__ on Linux, or the @@ -203,7 +203,7 @@ data file into a playable movie file: ffmpeg -f rawvideo -pixel_format rgb24 -video_size 2560x1440 -framerate 60 -i rgb.out -vf "vflip,format=yuv420p" video.mp4 -Note that the offscreen rendering resolution of the model and ffmpeg's video_size must be the identical. +Note that the offscreen rendering resolution of the model and ffmpeg's video_size must be identical. This sample can be compiled in three ways which differ in how the OpenGL context is created: using GLFW with an invisible window, using OSMesa, or using EGL. The latter two options are only available on Linux and are envoked by diff --git a/doc/programming/visualization.rst b/doc/programming/visualization.rst index ce60ba40..e8caa12c 100644 --- a/doc/programming/visualization.rst +++ b/doc/programming/visualization.rst @@ -151,7 +151,7 @@ The low-level mjvGLCamera is what determines the actual rendering. There are two for each eye. Each has position, forward and up directions. Forward corresponds to the negative Z axis of the camera frame, while up corresponds to the positive Y axis. There is also a frustum in the sense of OpenGL, except we store the average of the left and right frustum edges and then during rendering compute the actual edges from the viewport aspect -ratio assuming 1:1 pixel aspect ratio. The distance between the two camera positions corresponds to the inter-pupilary +ratio assuming 1:1 pixel aspect ratio. The distance between the two camera positions corresponds to the inter-pupillary distance (ipd). When the low-level camera parameters are computed automatically from an abstract camera, the ipd as well as vertical field of view (fovy) are taken from ``mjModel.vis.global.ipd``/``fovy`` for free and tracking cameras, and from the camera-specific ``mjModel.cam_ipd/fovy`` for cameras defined in the model. When stereoscopic mode is not From 70d944b4588920e68cad5aeef3e3e26758d6691a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 30 Oct 2024 08:38:12 -0700 Subject: [PATCH 041/426] micro-optimization to `mju_mulMatTVecSparse` PiperOrigin-RevId: 691431137 Change-Id: I95f94ff38ae911194b21aa220af35c7374af9739 --- src/engine/engine_util_sparse.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index f6d05cfd..434a0ce4 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -224,13 +224,16 @@ void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int mju_zero(res, nc); for (int i=0; i < nr; i++) { + mjtNum scl = vec[i]; + + // skip if 0 + if (!scl) continue; + + // add row scaled by the corresponding vector element int nnz = rownnz[i]; int adr = rowadr[i]; const int* ind = colind + adr; const mjtNum* row = mat + adr; - mjtNum scl = vec[i]; - - // add row scaled by the corresponding vector element for (int j=0; j < nnz; j++) { res[ind[j]] += row[j] * scl; } From a9737c60e8f21ec22b15db729448bb7ab0005144 Mon Sep 17 00:00:00 2001 From: Kevin Zakka Date: Wed, 30 Oct 2024 11:40:17 -0700 Subject: [PATCH 042/426] Copybara import of the project: -- 5b525ccaef3d7184d3167815be63ed630781a064 by Kevin Zakka : Add command line scripts to MJX docs. COPYBARA_INTEGRATE_REVIEW=https://github.com/google-deepmind/mujoco/pull/2196 from kevinzakka:typo-fixes 5b525ccaef3d7184d3167815be63ed630781a064 PiperOrigin-RevId: 691497203 Change-Id: I7c5d6a8fcbd2a42939673cee5eadb30bebdad833 --- doc/mjx.rst | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/doc/mjx.rst b/doc/mjx.rst index c1e0fc16..49435bc6 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -165,6 +165,30 @@ Minimal example pos = jax.jit(batched_step)(vel) print(pos) +.. _MjxCli: + +Helpful Command Line Scripts +---------------------------- + +We provide two command line scripts with the ``mujoco-mjx`` package: + +.. code-block:: shell + + mjx-testspeed --mjcf=/PATH/TO/MJCF/ --base_path=. + +This command takes in a path to an MJCF file along with optional arguments (use ``--help`` for more information) +and computes helpful metrics for performance tuning. The command will output, among other things, the total +simulation time, the total steps per second and the total realtime factor (here total is across all available +devices). + +.. code-block:: shell + + mjx-viewer --help + +This command launches the MJX model in the simulate viewer, allowing you to visualize and interact with the model. +Note this steps the simulation using MJX physics (not C MuJoCo) so it can be helpful for example for debugging +solver parameters. + .. _MjxFeatureParity: Feature Parity From 26ccaeb78ec81ada50002138023dbd0c93aa7beb Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Wed, 30 Oct 2024 13:42:10 -0700 Subject: [PATCH 043/426] Don't populate _qM_sparse, _qLD_sparse, _qLDiagInv_sparse fields if they were never set via _full_compat. Fixes #2188 PiperOrigin-RevId: 691538016 Change-Id: Ib0385166af2dfbbb46cf1986cb692e1fda467a0b --- mjx/mujoco/mjx/_src/smooth.py | 7 +++++-- mjx/mujoco/mjx/_src/smooth_test.py | 3 +++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index 3a168221..f2e3b721 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -291,7 +291,7 @@ def crb(m: Model, d: Data) -> Data: crb_cdof = jax.vmap(math.inert_mul)(crb_dof, d.cdof) qm = support.make_m(m, crb_cdof, d.cdof, m.dof_armature) d = d.replace(qM=qm) - if support.is_sparse(m): + if support.is_sparse(m) and d._qM_sparse.size > 0: # pylint: disable=protected-access d = d.replace(_qM_sparse=qm) return d @@ -353,7 +353,10 @@ def factor_m(m: Model, d: Data) -> Data: qld = (qld / qld[jp.array(madr_ds)]).at[m.dof_Madr].set(qld_diag) d = d.replace(qLD=qld, qLDiagInv=1 / qld_diag) - d = d.replace(_qLD_sparse=d.qLD, _qLDiagInv_sparse=d.qLDiagInv) + if d._qLD_sparse.size > 0: # pylint: disable=protected-access + d = d.replace(_qLD_sparse=d.qLD) + if d._qLDiagInv_sparse.size > 0: # pylint: disable=protected-access + d = d.replace(_qLDiagInv_sparse=d.qLDiagInv) return d diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 46f1fa2e..8b5218e5 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -87,10 +87,13 @@ class SmoothTest(absltest.TestCase): dx = jax.jit(mjx.crb)(mx, mjx.put_data(m, d)) _assert_attr_eq(d, dx, 'crb') _assert_attr_eq(d, dx, 'qM') + _assert_eq(dx._qM_sparse, np.zeros(0), '_qM_sparse') # factor_m dx = jax.jit(mjx.factor_m)(mx, mjx.put_data(m, d)) _assert_attr_eq(d, dx, 'qLD') _assert_attr_eq(d, dx, 'qLDiagInv') + _assert_eq(dx._qLD_sparse, np.zeros(0), '_qLD_sparse') + _assert_eq(dx._qLDiagInv_sparse, np.zeros(0), '_qLDiagInv_sparse') # com_vel dx = jax.jit(mjx.com_vel)(mx, mjx.put_data(m, d)) _assert_attr_eq(d, dx, 'cvel') From 94bbf297fcce7819dc9ff32c521cb492f20f7976 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 31 Oct 2024 06:29:39 -0700 Subject: [PATCH 044/426] Replace `flex_xvert0` with `flex_vert0` `flex_xvert0` stored the Cartesian position of the vertices in `qpos0`. This array was unused, so it is now replaced with `flex_vert0`, which still contains the positions of the vertices, but normalized on the bounding box of the flex so that the coordinates are in `[0,1]^m->flex_dim`. This will be useful in the future for using different interpolation methods for computing flex deformations. PiperOrigin-RevId: 691779361 Change-Id: I5c4223103cd4558f4e268fe3e8d8177541e4754f --- doc/includes/references.h | 2 +- include/mujoco/mjmodel.h | 2 +- include/mujoco/mjxmacro.h | 2 +- introspect/structs.py | 4 +- src/engine/engine_setconst.c | 13 ++++- test/user/user_flex_test.cc | 71 +++++++++++++++++++++++----- unity/Runtime/Bindings/MjBindings.cs | 2 +- 7 files changed, 77 insertions(+), 19 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 6c0c1d41..b7a32e40 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1170,7 +1170,7 @@ struct mjModel_ { int* flex_shell; // shell fragment vertex ids (dim per frag) (nflexshelldata x 1) int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2) mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3) - mjtNum* flex_xvert0; // Cartesian vertex positions in qpos0 (nflexvert x 3) + mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3) mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1) mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 7893a8b3..9bea5917 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -877,7 +877,7 @@ struct mjModel_ { int* flex_shell; // shell fragment vertex ids (dim per frag) (nflexshelldata x 1) int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2) mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3) - mjtNum* flex_xvert0; // Cartesian vertex positions in qpos0 (nflexvert x 3) + mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3) mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1) mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 3ca67c16..a47a0a32 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -346,7 +346,7 @@ XMJV( int, flex_shell, nflexshelldata,1 ) \ X ( int, flex_evpair, nflexevpair, 2 ) \ X ( mjtNum, flex_vert, nflexvert, 3 ) \ - X ( mjtNum, flex_xvert0, nflexvert, 3 ) \ + X ( mjtNum, flex_vert0, nflexvert, 3 ) \ X ( mjtNum, flexedge_length0, nflexedge, 1 ) \ X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ XMJV( mjtNum, flex_radius, nflex, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index 0776273e..c8993c31 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -2603,11 +2603,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ array_extent=('nflexvert', 3), ), StructFieldDecl( - name='flex_xvert0', + name='flex_vert0', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='Cartesian vertex positions in qpos0', + doc='vertex positions in qpos0 on [0, 1]^d', array_extent=('nflexvert', 3), ), StructFieldDecl( diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 64d35ff8..f451353b 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -118,8 +118,19 @@ static void set0(mjModel* m, mjData* d) { m->light_mode[i] = lightmode[i]; } + // compute bounding box coordinates + for (int i=0; i < m->nflex; i++) { + int bvhadr = m->flex_bvhadr[i]; + const mjtNum* bvh = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic); + for (int j=0; j < m->nflexvert; j++) { + for (int k=0; k < 3; k++) { + mjtNum size = 2*(bvh[3+k] - m->flex_radius[i]); + m->flex_vert0[3*j+k] = (d->flexvert_xpos[3*j+k] - bvh[k]) / size + 0.5; + } + } + } + // copy fields - mju_copy(m->flex_xvert0, d->flexvert_xpos, 3*m->nflexvert); mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge); mju_copy(m->tendon_length0, d->ten_length, m->ntendon); mju_copy(m->actuator_length0, d->actuator_length, m->nu); diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 1e2c168d..2f6414d2 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -242,6 +242,45 @@ TEST_F(UserFlexTest, RigidFlex) { mj_deleteModel(m); mj_deleteData(d); } +TEST_F(UserFlexTest, BoundingBoxCoordinates) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + mj_kinematics(m, d); + mj_flex(m, d); + + EXPECT_EQ(m->nflexvert, 5*5*5); + EXPECT_EQ(m->nflexelem, 4*4*4*6); + EXPECT_EQ(m->flex_dim[0], 3); + + // Cartesian coordinates + EXPECT_EQ(d->flexvert_xpos[0], -1); + EXPECT_EQ(d->flexvert_xpos[1], -2); + EXPECT_EQ(d->flexvert_xpos[2], -3); + EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-3], 3); + EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-2], 2); + EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-1], 1); + + // bounding box coordinates + EXPECT_EQ(m->flex_vert0[0], 0); + EXPECT_EQ(m->flex_vert0[1], 0); + EXPECT_EQ(m->flex_vert0[2], 0); + EXPECT_EQ(m->flex_vert0[3*m->nflexvert-3], 1); + EXPECT_EQ(m->flex_vert0[3*m->nflexvert-2], 1); + EXPECT_EQ(m->flex_vert0[3*m->nflexvert-1], 1); + + mj_deleteModel(m); + mj_deleteData(d); +} TEST_F(UserFlexTest, LoadMSHBinary_41_Success) { const std::string xml_path = @@ -280,14 +319,16 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) { mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); + mj_kinematics(m, d); + mj_flex(m, d); EXPECT_EQ(m->nflexvert, 14); EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(m->flex_xvert0[0], -0.5 ); - EXPECT_EQ(m->flex_xvert0[1], -0.5 ); - EXPECT_EQ(m->flex_xvert0[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[2], 0 ); // first element EXPECT_EQ(m->flex_elem[0], 9-1 ); @@ -306,14 +347,16 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) { mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); + mj_kinematics(m, d); + mj_flex(m, d); EXPECT_EQ(m->nflexvert, 14); EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(m->flex_xvert0[0], -0.5 ); - EXPECT_EQ(m->flex_xvert0[1], -0.5 ); - EXPECT_EQ(m->flex_xvert0[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[2], 0 ); // first element EXPECT_EQ(m->flex_elem[0], 9-1 ); @@ -377,14 +420,16 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) { mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); + mj_kinematics(m, d); + mj_flex(m, d); EXPECT_EQ(m->nflexvert, 14); EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(m->flex_xvert0[0], -0.5 ); - EXPECT_EQ(m->flex_xvert0[1], -0.5 ); - EXPECT_EQ(m->flex_xvert0[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[2], 0 ); // first element EXPECT_EQ(m->flex_elem[0], 9-1 ); @@ -403,14 +448,16 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) { mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); + mj_kinematics(m, d); + mj_flex(m, d); EXPECT_EQ(m->nflexvert, 14); EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(m->flex_xvert0[0], -0.5 ); - EXPECT_EQ(m->flex_xvert0[1], -0.5 ); - EXPECT_EQ(m->flex_xvert0[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); + EXPECT_EQ(d->flexvert_xpos[2], 0 ); // first element EXPECT_EQ(m->flex_elem[0], 9-1 ); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e963ab1c..262be737 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5428,7 +5428,7 @@ public unsafe struct mjModel_ { public int* flex_shell; public int* flex_evpair; public double* flex_vert; - public double* flex_xvert0; + public double* flex_vert0; public double* flexedge_length0; public double* flexedge_invweight0; public double* flex_radius; From 18682aac6a92c08a9afef139770982997e35eda1 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 31 Oct 2024 06:44:35 -0700 Subject: [PATCH 045/426] Fix function definition formatting. PiperOrigin-RevId: 691782530 Change-Id: Ib1611285e77e274b17074d5f09ede00cdc0c6f0f --- src/engine/engine_derivative.c | 24 ++++++++---------------- src/engine/engine_util_solve.c | 7 +++---- src/engine/engine_vis_interact.c | 2 +- src/engine/engine_vis_state.c | 12 ++++++------ src/user/user_api.cc | 2 +- 5 files changed, 19 insertions(+), 28 deletions(-) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index c49e9484..3f7eb63f 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -63,8 +63,7 @@ static void mjd_cross(const mjtNum a[3], const mjtNum b[3], // derivative of mju_crossMotion w.r.t velocity -static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) -{ +static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) { mju_zero(D, 36); // res[0] = -vel[2]*v[1] + vel[1]*v[2] @@ -101,8 +100,7 @@ static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) // derivative of mju_crossForce w.r.t. velocity -static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6]) -{ +static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6]) { mju_zero(D, 36); // res[0] = -vel[2]*f[1] + vel[1]*f[2] - vel[5]*f[4] + vel[4]*f[5] @@ -139,8 +137,7 @@ static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6]) // derivative of mju_crossForce w.r.t. force -static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6]) -{ +static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6]) { mju_zero(D, 36); // res[0] = -vel[2]*f[1] + vel[1]*f[2] - vel[5]*f[4] + vel[4]*f[5] @@ -177,8 +174,7 @@ static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6]) // derivative of mju_mulInertVec w.r.t vel -static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10]) -{ +static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10]) { mju_zero(D, 36); // res[0] = i[0]*v[0] + i[3]*v[1] + i[4]*v[2] - i[8]*v[4] + i[7]*v[5] @@ -221,8 +217,7 @@ static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10]) // derivative of mju_subQuat w.r.t inputs -void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]) -{ +void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]) { // no outputs, quick return if (!Da && !Db) { return; @@ -330,8 +325,7 @@ void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, // no longer used, except in tests // derivative of cvel, cdof_dot w.r.t qvel (dense version) -static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) -{ +static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) { int nv = m->nv, nbody = m->nbody; mjtNum mat[36]; @@ -969,8 +963,7 @@ static void mjd_addedMassForces( static inline void mjd_viscous_torque( mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, const mjtNum fluid_viscosity, const mjtNum size[3], - const mjtNum slender_drag_coef, const mjtNum ang_drag_coef) -{ + const mjtNum slender_drag_coef, const mjtNum ang_drag_coef) { const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]); const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]); const mjtNum d_mid = size[0] + size[1] + size[2] - d_max - d_min; @@ -1280,8 +1273,7 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { // fluid forces based on inertia-box approximation -void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) -{ +void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { mj_markStack(d); int nv = m->nv; diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index dd6c7843..10b50157 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -1046,10 +1046,9 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, // R must have allocatd size n*(n+7), but only nfree*nfree values are used in output // index (if given) must have allocated size n, but only nfree values are used in output // only lower triangles of H and R and read from and written to, respectively -int mju_boxQP(mjtNum* res, mjtNum* R, int* index, // outputs - const mjtNum* H, const mjtNum* g, int n, // QP definition - const mjtNum* lower, const mjtNum* upper) // bounds -{ +int mju_boxQP(mjtNum* res, mjtNum* R, int* index, + const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper) { // algorithm options int maxiter = 100; // maximum number of iterations mjtNum mingrad = 1E-16; // minimum squared norm of (unclamped) gradient diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 7786d405..cea65167 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -254,7 +254,7 @@ mjtNum mjv_frustumHeight(const mjvScene* scn) { // rotate 3D vec in horizontal plane by angle between (0,1) and (forward_x,forward_y) -MJAPI void mjv_alignToCamera(mjtNum* res, const mjtNum* vec, const mjtNum* forward) { +void mjv_alignToCamera(mjtNum* res, const mjtNum* vec, const mjtNum* forward) { mjtNum xaxis[2], yaxis[2]; // forward-aligned y-axis diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 55d35497..9a0ea210 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -394,9 +394,9 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, // move camera with mouse given a scene state; action is mjtMouse -MJAPI void mjv_moveCameraFromState(const mjvSceneState* scnstate, int action, - mjtNum reldx, mjtNum reldy, - const mjvScene* scn, mjvCamera* cam) { +void mjv_moveCameraFromState(const mjvSceneState* scnstate, int action, + mjtNum reldx, mjtNum reldy, + const mjvScene* scn, mjvCamera* cam) { mjModel m; mjv_assignFromSceneState(scnstate, &m, NULL); mjv_moveCamera(&m, action, reldx, reldy, scn, cam); @@ -405,9 +405,9 @@ MJAPI void mjv_moveCameraFromState(const mjvSceneState* scnstate, int action, // move perturb object with mouse given a scene state; action is mjtMouse -MJAPI void mjv_movePerturbFromState(const mjvSceneState* scnstate, int action, - mjtNum reldx, mjtNum reldy, - const mjvScene* scn, mjvPerturb* pert) { +void mjv_movePerturbFromState(const mjvSceneState* scnstate, int action, + mjtNum reldx, mjtNum reldy, + const mjvScene* scn, mjvPerturb* pert) { mjModel m; mjData d; mjv_assignFromSceneState(scnstate, &m, &d); diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 6b5a7769..62dd2717 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -440,7 +440,7 @@ mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* defspec) { // wrap site using tendon -MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendonspec, const char* name) { +mjsWrap* mjs_wrapSite(mjsTendon* tendonspec, const char* name) { mjCTendon* tendon = static_cast(tendonspec->element); tendon->WrapSite(name); return &tendon->path.back()->spec; From 554641060b569be8f55237a3ceee2f61007587cf Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Thu, 31 Oct 2024 09:22:02 -0700 Subject: [PATCH 046/426] Add support for computing elliptic cone contact forces to MJX. Fixes #2153. PiperOrigin-RevId: 691829630 Change-Id: I862ac9c7db07d0a74b718d2a41e19d5ae636a260 --- mjx/mujoco/mjx/_src/sensor_test.py | 10 +++++++-- mjx/mujoco/mjx/_src/smooth_test.py | 26 +++++++++++++++++----- mjx/mujoco/mjx/_src/support.py | 19 +++++++++------- mjx/mujoco/mjx/test_data/sensor/sensor.xml | 8 +++---- 4 files changed, 44 insertions(+), 19 deletions(-) diff --git a/mjx/mujoco/mjx/_src/sensor_test.py b/mjx/mujoco/mjx/_src/sensor_test.py index 19d4ef46..37b1e13e 100644 --- a/mjx/mujoco/mjx/_src/sensor_test.py +++ b/mjx/mujoco/mjx/_src/sensor_test.py @@ -22,6 +22,8 @@ from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import test_util +from mujoco.mjx._src.types import ConeType + import numpy as np # tolerance for difference between MuJoCo and MJX smooth calculations - mostly @@ -41,10 +43,14 @@ def _assert_attr_eq(a, b, attr): class SensorTest(parameterized.TestCase): - @parameterized.parameters('sensor/model.xml', 'sensor/sensor.xml') - def test_sensor(self, filename): + @parameterized.product( + filename=['sensor/model.xml', 'sensor/sensor.xml'], + cone_type=list(ConeType), + ) + def test_sensor(self, filename, cone_type): """Tests MJX sensor functions match MuJoCo sensor functions.""" m = test_util.load_test_file(filename) + m.opt.cone = cone_type d = mujoco.MjData(m) # give the system a little kick to ensure we have non-identity rotations d.qvel = 0.1 * np.random.random(m.nv) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 8b5218e5..fc14ac65 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -20,6 +20,7 @@ import jax import mujoco from mujoco import mjx from mujoco.mjx._src import test_util +from mujoco.mjx._src.types import ConeType import numpy as np # tolerance for difference between MuJoCo and MJX smooth calculations - mostly @@ -198,27 +199,42 @@ class SmoothTest(absltest.TestCase): _assert_attr_eq(d, dx, 'subtree_linvel') _assert_attr_eq(d, dx, 'subtree_angmom') - def test_rnepostconstraint(self): + +class RnePostConstraintTest(parameterized.TestCase): + + @parameterized.parameters(ConeType) + def test_rnepostconstraint(self, cone_type): """Tests MJX rne_postconstraint function to match MuJoCo mj_rnePostConstraint.""" m = mujoco.MjModel.from_xml_string(""" - + - + - + + + + + + + + + - + """) + # set cone type + m.opt.cone = cone_type + # create data and set to keyframe d = mujoco.MjData(m) mujoco.mj_resetDataKeyframe(m, d, 0) # apply external forces diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 622c6659..f065d9d8 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -21,6 +21,7 @@ import mujoco from mujoco.mjx._src import math from mujoco.mjx._src import scan # pylint: disable=g-importing-member +from mujoco.mjx._src.types import ConeType from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import JacobianType from mujoco.mjx._src.types import Model @@ -307,12 +308,13 @@ def contact_force( """Extract 6D force:torque for one contact, in contact frame by default.""" efc_address = d.contact.efc_address[contact_id] condim = d.contact.dim[contact_id] - if m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: + if m.opt.cone == ConeType.PYRAMIDAL: force = _decode_pyramid( d.efc_force[efc_address:], d.contact.friction[contact_id], condim ) - elif m.opt.cone == mujoco.mjtCone.mjCONE_ELLIPTIC: - raise NotImplementedError('Elliptic cone force is not implemented yet.') + elif m.opt.cone == ConeType.ELLIPTIC: + force = d.efc_force[efc_address : efc_address + condim] + force = jp.concatenate([force, jp.zeros((6 - condim))]) else: raise ValueError(f'Unknown cone type: {m.opt.cone}') @@ -331,7 +333,7 @@ def contact_force_dim( idx_dim = (d.contact.efc_address >= 0) & (d.contact.dim == dim) # contact force from efc - if m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: + if m.opt.cone == ConeType.PYRAMIDAL: efc_address = ( d.contact.efc_address[idx_dim, None] + np.arange(np.where(dim == 1, 1, 2 * (dim - 1)))[None] @@ -340,12 +342,13 @@ def contact_force_dim( force = jax.vmap(_decode_pyramid, in_axes=(0, 0, None))( efc_force, d.contact.friction[idx_dim], dim ) - return force, np.where(idx_dim)[0] - elif m.opt.cone == mujoco.mjtCone.mjCONE_ELLIPTIC: - # TODO(taylorhowell): add support for elliptic cone - raise NotImplementedError('Elliptic cone force is not implemented yet.') + elif m.opt.cone == ConeType.ELLIPTIC: + efc_address = d.contact.efc_address[idx_dim, None] + np.arange(dim)[None] + force = d.efc_force[efc_address] + force = jp.hstack([force, jp.zeros((force.shape[0], 6 - dim))]) else: raise ValueError(f'Unknown cone type: {m.opt.cone}.') + return force, np.where(idx_dim)[0] def _length_circle( diff --git a/mjx/mujoco/mjx/test_data/sensor/sensor.xml b/mjx/mujoco/mjx/test_data/sensor/sensor.xml index b6d3fa2a..efd855e1 100644 --- a/mjx/mujoco/mjx/test_data/sensor/sensor.xml +++ b/mjx/mujoco/mjx/test_data/sensor/sensor.xml @@ -106,22 +106,22 @@ - + - + - + - + From c9f78ad8a88c1689a0869eca9419e4f748c6994f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Oct 2024 10:42:29 -0700 Subject: [PATCH 047/426] Rename `nnzJ` to `nJ` in `mjData`. This naming is more consistent. PiperOrigin-RevId: 691858223 Change-Id: Ife3db822afa0b51b30d0792548b35b3742da388a --- doc/includes/references.h | 10 +++++----- include/mujoco/mjdata.h | 10 +++++----- include/mujoco/mjxmacro.h | 10 +++++----- introspect/structs.py | 10 +++++----- mjx/mujoco/mjx/_src/io.py | 2 +- python/mujoco/structs.cc | 4 ++-- src/engine/engine_core_constraint.c | 15 +++++++-------- src/engine/engine_io.c | 2 +- test/engine/engine_io_test.cc | 2 +- unity/Runtime/Bindings/MjBindings.cs | 2 +- 10 files changed, 33 insertions(+), 34 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index b7a32e40..546299d7 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -169,7 +169,7 @@ struct mjData_ { int nf; // number of friction constraints int nl; // number of limit constraints int nefc; // number of constraints - int nnzJ; // number of non-zeros in constraint Jacobian + int nJ; // number of non-zeros in constraint Jacobian int nisland; // number of detected constraint islands // global properties @@ -361,13 +361,13 @@ struct mjData_ { int* efc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) int* efc_J_rowadr; // row start address in colind array (nefc x 1) int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) - int* efc_J_colind; // column indices in constraint Jacobian (nnzJ x 1) + int* efc_J_colind; // column indices in constraint Jacobian (nJ x 1) int* efc_JT_rownnz; // number of non-zeros in constraint Jacobian row T (nv x 1) int* efc_JT_rowadr; // row start address in colind array T (nv x 1) int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) - int* efc_JT_colind; // column indices in constraint Jacobian T (nnzJ x 1) - mjtNum* efc_J; // constraint Jacobian (nnzJ x 1) - mjtNum* efc_JT; // constraint Jacobian transposed (nnzJ x 1) + int* efc_JT_colind; // column indices in constraint Jacobian T (nJ x 1) + mjtNum* efc_J; // constraint Jacobian (nJ x 1) + mjtNum* efc_JT; // constraint Jacobian transposed (nJ x 1) mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 44342c90..96b37acc 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -197,7 +197,7 @@ struct mjData_ { int nf; // number of friction constraints int nl; // number of limit constraints int nefc; // number of constraints - int nnzJ; // number of non-zeros in constraint Jacobian + int nJ; // number of non-zeros in constraint Jacobian int nisland; // number of detected constraint islands // global properties @@ -389,13 +389,13 @@ struct mjData_ { int* efc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) int* efc_J_rowadr; // row start address in colind array (nefc x 1) int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) - int* efc_J_colind; // column indices in constraint Jacobian (nnzJ x 1) + int* efc_J_colind; // column indices in constraint Jacobian (nJ x 1) int* efc_JT_rownnz; // number of non-zeros in constraint Jacobian row T (nv x 1) int* efc_JT_rowadr; // row start address in colind array T (nv x 1) int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) - int* efc_JT_colind; // column indices in constraint Jacobian T (nnzJ x 1) - mjtNum* efc_J; // constraint Jacobian (nnzJ x 1) - mjtNum* efc_JT; // constraint Jacobian transposed (nnzJ x 1) + int* efc_JT_colind; // column indices in constraint Jacobian T (nJ x 1) + mjtNum* efc_J; // constraint Jacobian (nJ x 1) + mjtNum* efc_JT; // constraint Jacobian transposed (nJ x 1) mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index a47a0a32..796b6c5a 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -689,13 +689,13 @@ X( int, efc_J_rownnz, MJ_D(nefc), 1 ) \ X( int, efc_J_rowadr, MJ_D(nefc), 1 ) \ X( int, efc_J_rowsuper, MJ_D(nefc), 1 ) \ - X( int, efc_J_colind, MJ_D(nnzJ), 1 ) \ + X( int, efc_J_colind, MJ_D(nJ), 1 ) \ X( int, efc_JT_rownnz, MJ_M(nv), 1 ) \ X( int, efc_JT_rowadr, MJ_M(nv), 1 ) \ X( int, efc_JT_rowsuper, MJ_M(nv), 1 ) \ - X( int, efc_JT_colind, MJ_D(nnzJ), 1 ) \ - X( mjtNum, efc_J, MJ_D(nnzJ), 1 ) \ - X( mjtNum, efc_JT, MJ_D(nnzJ), 1 ) \ + X( int, efc_JT_colind, MJ_D(nJ), 1 ) \ + X( mjtNum, efc_J, MJ_D(nJ), 1 ) \ + X( mjtNum, efc_JT, MJ_D(nJ), 1 ) \ X( mjtNum, efc_pos, MJ_D(nefc), 1 ) \ X( mjtNum, efc_margin, MJ_D(nefc), 1 ) \ X( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \ @@ -755,7 +755,7 @@ X( int, nf ) \ X( int, nl ) \ X( int, nefc ) \ - X( int, nnzJ ) \ + X( int, nJ ) \ X( int, nisland ) \ X( mjtNum, time ) \ X( uintptr_t, threadpool ) diff --git a/introspect/structs.py b/introspect/structs.py index c8993c31..0d5107fd 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4692,7 +4692,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='number of constraints', ), StructFieldDecl( - name='nnzJ', + name='nJ', type=ValueType(name='int'), doc='number of non-zeros in constraint Jacobian', ), @@ -5588,7 +5588,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='column indices in constraint Jacobian', - array_extent=('nnzJ',), + array_extent=('nJ',), ), StructFieldDecl( name='efc_JT_rownnz', @@ -5616,7 +5616,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='int'), ), - doc='column indices in constraint Jacobian T (nnzJ x 1)', # pylint: disable=line-too-long + doc='column indices in constraint Jacobian T (nJ x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='efc_J', @@ -5624,7 +5624,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='constraint Jacobian', - array_extent=('nnzJ',), + array_extent=('nJ',), ), StructFieldDecl( name='efc_JT', @@ -5632,7 +5632,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='constraint Jacobian transposed', - array_extent=('nnzJ',), + array_extent=('nJ',), ), StructFieldDecl( name='efc_pos', diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 893e7cb1..00ba75e5 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -408,7 +408,7 @@ def get_data_into( ncon = (d_i.contact.dist <= 0).sum() efc_active = (d_i.efc_J != 0).any(axis=1) nefc = int(efc_active.sum()) - result_i.nnzJ = nefc * m.nv + result_i.nJ = nefc * m.nv if ncon != result_i.ncon or nefc != result_i.nefc: mujoco._functions._realloc_con_efc(result_i, ncon=ncon, nefc=nefc) # pylint: disable=protected-access result_i.efc_J_rownnz[:] = np.repeat(m.nv, nefc) diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 0c97081e..56a6c8ee 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -747,7 +747,7 @@ void MjDataWrapper::Serialize(std::ostream& output) const { X(ncon); X(ne); X(nf); - X(nnzJ); + X(nJ); X(nefc); X(nisland); X(time); @@ -824,7 +824,7 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { X(ncon); X(ne); X(nf); - X(nnzJ); + X(nJ); X(nefc); X(nisland); X(time); diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index dc2a7658..2c62248a 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1943,7 +1943,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { // driver: call all functions above void mj_makeConstraint(const mjModel* m, mjData* d) { // clear sizes - d->ne = d->nf = d->nl = d->nefc = d->nnzJ = 0; + d->ne = d->nf = d->nl = d->nefc = d->nJ = 0; // disabled or Jacobian not allocated: return if (mjDISABLED(mjDSBL_CONSTRAINT)) { @@ -1951,13 +1951,13 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { } // precount sizes for constraint Jacobian matrices - int *nnz = mj_isSparse(m) ? &(d->nnzJ) : NULL; + int *nnz = mj_isSparse(m) ? &(d->nJ) : NULL; int ne_allocated = mj_ne(m, d, nnz); int nf_allocated = mj_nf(m, d, nnz); int nl_allocated = mj_nl(m, d, nnz); int nefc_allocated = ne_allocated + nf_allocated + nl_allocated + mj_nc(m, d, nnz); if (!mj_isSparse(m)) { - d->nnzJ = nefc_allocated * m->nv; + d->nJ = nefc_allocated * m->nv; } d->nefc = nefc_allocated; @@ -1998,12 +1998,11 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { mjERROR("nefc mis-allocation: found nefc=%d but allocated %d", d->nefc, nefc_allocated); } - // check that nnzJ was computed correctly + // check that nJ was computed correctly if (d->nefc > 0) { - int nnzJ = d->efc_J_rownnz[d->nefc - 1] + d->efc_J_rowadr[d->nefc - 1]; - if (d->nnzJ != nnzJ) { - mjERROR("constraint Jacobian mis-allocation: found nnzJ=%d but allocated %d", - nnzJ, d->nnzJ); + int nJ = d->efc_J_rownnz[d->nefc - 1] + d->efc_J_rowadr[d->nefc - 1]; + if (d->nJ != nJ) { + mjERROR("constraint Jacobian mis-allocation: found nJ=%d but allocated %d", nJ, d->nJ); } } } else if (d->nefc > nefc_allocated) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 88ca9bd2..6891da68 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1815,7 +1815,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { d->nf = 0; d->nl = 0; d->nefc = 0; - d->nnzJ = 0; + d->nJ = 0; d->nisland = 0; // clear global properties diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index fc38adc7..7b65de4a 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -156,7 +156,7 @@ TEST_F(EngineIoTest, ResetVariableSizes) { EXPECT_EQ(data->ne, 0); EXPECT_EQ(data->nf, 0); EXPECT_EQ(data->nefc, 0); - EXPECT_EQ(data->nnzJ, 0); + EXPECT_EQ(data->nJ, 0); EXPECT_EQ(data->ncon, 0); mj_deleteData(data); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 262be737..906c2c7f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4851,7 +4851,7 @@ public unsafe struct mjData_ { public int nf; public int nl; public int nefc; - public int nnzJ; + public int nJ; public int nisland; public double time; public fixed double energy[2]; From 1d58576d284f2cfb33ba872de5fd23fe1e54e47e Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 1 Nov 2024 03:28:01 -0700 Subject: [PATCH 048/426] Fetch source code line links for API functions on docs client side. PiperOrigin-RevId: 692115441 Change-Id: Ifbedf6a4998000f99e2275cb43293b3050daf0ee --- doc/APIreference/APIfunctions.rst | 5 + doc/APIreference/functions.rst | 1860 ++++++++++++++--------------- doc/changelog.rst | 6 + doc/conf.py | 4 + doc/js/linenumbers.js | 108 ++ 5 files changed, 1053 insertions(+), 930 deletions(-) create mode 100644 doc/js/linenumbers.js diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index 824221b6..e64a33ca 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -1,3 +1,8 @@ +.. raw:: html + +
+ + .. _API: ========= diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 4ef49615..4406e5c5 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -13,8 +13,8 @@ The model and all files referenced in it can be loaded from disk or from a VFS w .. _mj_loadXML: -mj_loadXML -~~~~~~~~~~ +`mj_loadXML <#mj_loadXML>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_loadXML @@ -24,8 +24,8 @@ If error is not NULL, it must have size error_sz. .. _mj_parseXML: -mj_parseXML -~~~~~~~~~~~ +`mj_parseXML <#mj_parseXML>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_parseXML @@ -33,8 +33,8 @@ Parse spec from XML file. .. _mj_parseXMLString: -mj_parseXMLString -~~~~~~~~~~~~~~~~~ +`mj_parseXMLString <#mj_parseXMLString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_parseXMLString @@ -42,8 +42,8 @@ Parse spec from XML string. .. _mj_compile: -mj_compile -~~~~~~~~~~ +`mj_compile <#mj_compile>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_compile @@ -53,8 +53,8 @@ If compilation fails, :ref:`mj_compile` returns ``NULL``; the error can be read .. _mj_recompile: -mj_recompile -~~~~~~~~~~~~ +`mj_recompile <#mj_recompile>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_recompile @@ -70,8 +70,8 @@ instances will be deleted; as in :ref:`mj_compile`, the compilation error can be .. _mj_saveLastXML: -mj_saveLastXML -~~~~~~~~~~~~~~ +`mj_saveLastXML <#mj_saveLastXML>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_saveLastXML @@ -80,8 +80,8 @@ If error is not NULL, it must have size error_sz. .. _mj_freeLastXML: -mj_freeLastXML -~~~~~~~~~~~~~~ +`mj_freeLastXML <#mj_freeLastXML>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_freeLastXML @@ -89,8 +89,8 @@ Free last XML model if loaded. Called internally at each load. .. _mj_saveXMLString: -mj_saveXMLString -~~~~~~~~~~~~~~~~ +`mj_saveXMLString <#mj_saveXMLString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_saveXMLString @@ -98,8 +98,8 @@ Save spec to XML string, return 1 on success, 0 otherwise. XML saving requires t .. _mj_saveXML: -mj_saveXML -~~~~~~~~~~ +`mj_saveXML <#mj_saveXML>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_saveXML @@ -136,8 +136,8 @@ depend on qpos. Calling the dynamics with skipstage = :ref:`mjSTAGE_POS`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_step @@ -145,8 +145,8 @@ Advance simulation, use control callback to obtain external force and control. .. _mj_step1: -mj_step1 -~~~~~~~~ +`mj_step1 <#mj_step1>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_step1 @@ -154,8 +154,8 @@ Advance simulation in two steps: before external force and control is set by use .. _mj_step2: -mj_step2 -~~~~~~~~ +`mj_step2 <#mj_step2>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_step2 @@ -163,8 +163,8 @@ Advance simulation in two steps: after external force and control is set by user .. _mj_forward: -mj_forward -~~~~~~~~~~ +`mj_forward <#mj_forward>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_forward @@ -172,8 +172,8 @@ Forward dynamics: same as mj_step but do not integrate in time. .. _mj_inverse: -mj_inverse -~~~~~~~~~~ +`mj_inverse <#mj_inverse>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_inverse @@ -181,8 +181,8 @@ Inverse dynamics: qacc must be set before calling. .. _mj_forwardSkip: -mj_forwardSkip -~~~~~~~~~~~~~~ +`mj_forwardSkip <#mj_forwardSkip>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_forwardSkip @@ -190,8 +190,8 @@ Forward dynamics with skip; skipstage is mjtStage. .. _mj_inverseSkip: -mj_inverseSkip -~~~~~~~~~~~~~~ +`mj_inverseSkip <#mj_inverseSkip>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_inverseSkip @@ -208,8 +208,8 @@ computations, and are documented in more detail below. .. _mj_stateSize: -mj_stateSize -~~~~~~~~~~~~ +`mj_stateSize <#mj_stateSize>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_stateSize @@ -218,8 +218,8 @@ correspond to element fields of :ref:`mjtState`. .. _mj_getState: -mj_getState -~~~~~~~~~~~ +`mj_getState <#mj_getState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_getState @@ -228,8 +228,8 @@ Copy concatenated state components specified by ``spec`` from ``d`` into ``state .. _mj_setState: -mj_setState -~~~~~~~~~~~ +`mj_setState <#mj_setState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_setState @@ -238,8 +238,8 @@ Copy concatenated state components specified by ``spec`` from ``state`` into `` .. _mj_setKeyframe: -mj_setKeyframe -~~~~~~~~~~~~~~ +`mj_setKeyframe <#mj_setKeyframe>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_setKeyframe @@ -247,8 +247,8 @@ Copy current state to the k-th model keyframe. .. _mj_addContact: -mj_addContact -~~~~~~~~~~~~~ +`mj_addContact <#mj_addContact>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_addContact @@ -256,8 +256,8 @@ Add contact to d->contact list; return 0 if success; 1 if buffer full. .. _mj_isPyramidal: -mj_isPyramidal -~~~~~~~~~~~~~~ +`mj_isPyramidal <#mj_isPyramidal>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_isPyramidal @@ -265,8 +265,8 @@ Determine type of friction cone. .. _mj_isSparse: -mj_isSparse -~~~~~~~~~~~ +`mj_isSparse <#mj_isSparse>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_isSparse @@ -274,8 +274,8 @@ Determine type of constraint Jacobian. .. _mj_isDual: -mj_isDual -~~~~~~~~~ +`mj_isDual <#mj_isDual>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_isDual @@ -283,8 +283,8 @@ Determine type of solver (PGS is dual, CG and Newton are primal). .. _mj_mulJacVec: -mj_mulJacVec -~~~~~~~~~~~~ +`mj_mulJacVec <#mj_mulJacVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_mulJacVec @@ -293,8 +293,8 @@ sparse; the function is aware of this setting. Multiplication by J maps velociti .. _mj_mulJacTVec: -mj_mulJacTVec -~~~~~~~~~~~~~ +`mj_mulJacTVec <#mj_mulJacTVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_mulJacTVec @@ -303,8 +303,8 @@ space. .. _mj_jac: -mj_jac -~~~~~~ +`mj_jac <#mj_jac>`__ +~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jac @@ -320,8 +320,8 @@ by :ref:`mj_comPos`. .. _mj_jacBody: -mj_jacBody -~~~~~~~~~~ +`mj_jacBody <#mj_jacBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacBody @@ -330,8 +330,8 @@ site. They are just shortcuts; the same can be achieved by calling mj_jac direct .. _mj_jacBodyCom: -mj_jacBodyCom -~~~~~~~~~~~~~ +`mj_jacBodyCom <#mj_jacBodyCom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacBodyCom @@ -339,8 +339,8 @@ Compute body center-of-mass end-effector Jacobian. .. _mj_jacSubtreeCom: -mj_jacSubtreeCom -~~~~~~~~~~~~~~~~ +`mj_jacSubtreeCom <#mj_jacSubtreeCom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacSubtreeCom @@ -348,8 +348,8 @@ Compute subtree center-of-mass end-effector Jacobian. .. _mj_jacGeom: -mj_jacGeom -~~~~~~~~~~ +`mj_jacGeom <#mj_jacGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacGeom @@ -357,8 +357,8 @@ Compute geom end-effector Jacobian. .. _mj_jacSite: -mj_jacSite -~~~~~~~~~~ +`mj_jacSite <#mj_jacSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacSite @@ -366,8 +366,8 @@ Compute site end-effector Jacobian. .. _mj_jacPointAxis: -mj_jacPointAxis -~~~~~~~~~~~~~~~ +`mj_jacPointAxis <#mj_jacPointAxis>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacPointAxis @@ -375,8 +375,8 @@ Compute translation end-effector Jacobian of point, and rotation Jacobian of axi .. _mj_jacDot: -mj_jacDot -~~~~~~~~~ +`mj_jacDot <#mj_jacDot>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_jacDot @@ -387,8 +387,8 @@ consistent with the current generalized positions and velocities ``mjData.{qpos, .. _mj_angmomMat: -mj_angmomMat -~~~~~~~~~~~~ +`mj_angmomMat <#mj_angmomMat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_angmomMat @@ -399,8 +399,8 @@ and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = .. _mj_name2id: -mj_name2id -~~~~~~~~~~ +`mj_name2id <#mj_name2id>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_name2id @@ -408,8 +408,8 @@ Get id of object with the specified mjtObj type and name, returns -1 if id not f .. _mj_id2name: -mj_id2name -~~~~~~~~~~ +`mj_id2name <#mj_id2name>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_id2name @@ -417,8 +417,8 @@ Get name of object with the specified mjtObj type and id, returns NULL if name n .. _mj_fullM: -mj_fullM -~~~~~~~~ +`mj_fullM <#mj_fullM>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fullM @@ -426,8 +426,8 @@ Convert sparse inertia matrix M into full (i.e. dense) matrix. .. _mj_mulM: -mj_mulM -~~~~~~~ +`mj_mulM <#mj_mulM>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_mulM @@ -438,8 +438,8 @@ sparsity. .. _mj_mulM2: -mj_mulM2 -~~~~~~~~ +`mj_mulM2 <#mj_mulM2>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_mulM2 @@ -447,8 +447,8 @@ Multiply vector by (inertia matrix)^(1/2). .. _mj_addM: -mj_addM -~~~~~~~ +`mj_addM <#mj_addM>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_addM @@ -457,8 +457,8 @@ Destination can be sparse uncompressed, or dense when all int* are NULL .. _mj_applyFT: -mj_applyFT -~~~~~~~~~~ +`mj_applyFT <#mj_applyFT>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_applyFT @@ -468,8 +468,8 @@ we want to add the result to a different vector. .. _mj_objectVelocity: -mj_objectVelocity -~~~~~~~~~~~~~~~~~ +`mj_objectVelocity <#mj_objectVelocity>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_objectVelocity @@ -477,8 +477,8 @@ Compute object 6D velocity (rot:lin) in object-centered frame, world/local orien .. _mj_objectAcceleration: -mj_objectAcceleration -~~~~~~~~~~~~~~~~~~~~~ +`mj_objectAcceleration <#mj_objectAcceleration>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_objectAcceleration @@ -488,8 +488,8 @@ mjData.cacc -- the total body acceleration, including contributions from the con .. _mj_geomDistance: -mj_geomDistance -~~~~~~~~~~~~~~~ +`mj_geomDistance <#mj_geomDistance>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_geomDistance @@ -510,8 +510,8 @@ found, the function will return ``distmax`` and ``fromto``, if given, will be se .. _mj_contactForce: -mj_contactForce -~~~~~~~~~~~~~~~ +`mj_contactForce <#mj_contactForce>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_contactForce @@ -519,8 +519,8 @@ Extract 6D force:torque given contact id, in the contact frame. .. _mj_differentiatePos: -mj_differentiatePos -~~~~~~~~~~~~~~~~~~~ +`mj_differentiatePos <#mj_differentiatePos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_differentiatePos @@ -532,8 +532,8 @@ inputs have dimensionality nq. .. _mj_integratePos: -mj_integratePos -~~~~~~~~~~~~~~~ +`mj_integratePos <#mj_integratePos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_integratePos @@ -542,8 +542,8 @@ format of qpos. .. _mj_normalizeQuat: -mj_normalizeQuat -~~~~~~~~~~~~~~~~ +`mj_normalizeQuat <#mj_normalizeQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_normalizeQuat @@ -551,8 +551,8 @@ Normalize all quaternions in qpos-type vector. .. _mj_local2Global: -mj_local2Global -~~~~~~~~~~~~~~~ +`mj_local2Global <#mj_local2Global>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_local2Global @@ -560,8 +560,8 @@ Map from body local to global Cartesian coordinates, sameframe takes values from .. _mj_getTotalmass: -mj_getTotalmass -~~~~~~~~~~~~~~~ +`mj_getTotalmass <#mj_getTotalmass>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_getTotalmass @@ -569,8 +569,8 @@ Sum all body masses. .. _mj_setTotalmass: -mj_setTotalmass -~~~~~~~~~~~~~~~ +`mj_setTotalmass <#mj_setTotalmass>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_setTotalmass @@ -578,8 +578,8 @@ Scale body masses and inertias to achieve specified total mass. .. _mj_getPluginConfig: -mj_getPluginConfig -~~~~~~~~~~~~~~~~~~ +`mj_getPluginConfig <#mj_getPluginConfig>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_getPluginConfig @@ -588,8 +588,8 @@ NULL: invalid plugin instance ID or attribute name .. _mj_loadPluginLibrary: -mj_loadPluginLibrary -~~~~~~~~~~~~~~~~~~~~ +`mj_loadPluginLibrary <#mj_loadPluginLibrary>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_loadPluginLibrary @@ -597,8 +597,8 @@ Load a dynamic library. The dynamic library is assumed to register one or more p .. _mj_loadAllPluginLibraries: -mj_loadAllPluginLibraries -~~~~~~~~~~~~~~~~~~~~~~~~~ +`mj_loadAllPluginLibraries <#mj_loadAllPluginLibraries>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_loadAllPluginLibraries @@ -608,8 +608,8 @@ for each dynamic library encountered that registers plugins. .. _mj_version: -mj_version -~~~~~~~~~~ +`mj_version <#mj_version>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_version @@ -617,8 +617,8 @@ Return version number: 1.0.2 is encoded as 102. .. _mj_versionString: -mj_versionString -~~~~~~~~~~~~~~~~ +`mj_versionString <#mj_versionString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_versionString @@ -634,8 +634,8 @@ These are components of the simulation pipeline, called internally from :ref:`mj .. _mj_fwdPosition: -mj_fwdPosition -~~~~~~~~~~~~~~ +`mj_fwdPosition <#mj_fwdPosition>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fwdPosition @@ -643,8 +643,8 @@ Run position-dependent computations. .. _mj_fwdVelocity: -mj_fwdVelocity -~~~~~~~~~~~~~~ +`mj_fwdVelocity <#mj_fwdVelocity>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fwdVelocity @@ -652,8 +652,8 @@ Run velocity-dependent computations. .. _mj_fwdActuation: -mj_fwdActuation -~~~~~~~~~~~~~~~ +`mj_fwdActuation <#mj_fwdActuation>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fwdActuation @@ -661,8 +661,8 @@ Compute actuator force qfrc_actuator. .. _mj_fwdAcceleration: -mj_fwdAcceleration -~~~~~~~~~~~~~~~~~~ +`mj_fwdAcceleration <#mj_fwdAcceleration>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fwdAcceleration @@ -670,8 +670,8 @@ Add up all non-constraint forces, compute qacc_smooth. .. _mj_fwdConstraint: -mj_fwdConstraint -~~~~~~~~~~~~~~~~ +`mj_fwdConstraint <#mj_fwdConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_fwdConstraint @@ -679,8 +679,8 @@ Run selected constraint solver. .. _mj_Euler: -mj_Euler -~~~~~~~~ +`mj_Euler <#mj_Euler>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_Euler @@ -688,8 +688,8 @@ Euler integrator, semi-implicit in velocity. .. _mj_RungeKutta: -mj_RungeKutta -~~~~~~~~~~~~~ +`mj_RungeKutta <#mj_RungeKutta>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_RungeKutta @@ -697,8 +697,8 @@ Runge-Kutta explicit order-N integrator. .. _mj_implicit: -mj_implicit -~~~~~~~~~~~ +`mj_implicit <#mj_implicit>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_implicit @@ -708,8 +708,8 @@ Integrates the simulation state using an implicit-in-velocity integrator (either .. _mj_invPosition: -mj_invPosition -~~~~~~~~~~~~~~ +`mj_invPosition <#mj_invPosition>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_invPosition @@ -717,8 +717,8 @@ Run position-dependent computations in inverse dynamics. .. _mj_invVelocity: -mj_invVelocity -~~~~~~~~~~~~~~ +`mj_invVelocity <#mj_invVelocity>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_invVelocity @@ -726,8 +726,8 @@ Run velocity-dependent computations in inverse dynamics. .. _mj_invConstraint: -mj_invConstraint -~~~~~~~~~~~~~~~~ +`mj_invConstraint <#mj_invConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_invConstraint @@ -735,8 +735,8 @@ Apply the analytical formula for inverse constraint dynamics. .. _mj_compareFwdInv: -mj_compareFwdInv -~~~~~~~~~~~~~~~~ +`mj_compareFwdInv <#mj_compareFwdInv>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_compareFwdInv @@ -752,8 +752,8 @@ that the user will need to call them. .. _mj_sensorPos: -mj_sensorPos -~~~~~~~~~~~~ +`mj_sensorPos <#mj_sensorPos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_sensorPos @@ -761,8 +761,8 @@ Evaluate position-dependent sensors. .. _mj_sensorVel: -mj_sensorVel -~~~~~~~~~~~~ +`mj_sensorVel <#mj_sensorVel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_sensorVel @@ -770,8 +770,8 @@ Evaluate velocity-dependent sensors. .. _mj_sensorAcc: -mj_sensorAcc -~~~~~~~~~~~~ +`mj_sensorAcc <#mj_sensorAcc>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_sensorAcc @@ -779,8 +779,8 @@ Evaluate acceleration and force-dependent sensors. .. _mj_energyPos: -mj_energyPos -~~~~~~~~~~~~ +`mj_energyPos <#mj_energyPos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_energyPos @@ -788,8 +788,8 @@ Evaluate position-dependent energy (potential). .. _mj_energyVel: -mj_energyVel -~~~~~~~~~~~~ +`mj_energyVel <#mj_energyVel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_energyVel @@ -797,8 +797,8 @@ Evaluate velocity-dependent energy (kinetic). .. _mj_checkPos: -mj_checkPos -~~~~~~~~~~~ +`mj_checkPos <#mj_checkPos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_checkPos @@ -806,8 +806,8 @@ Check qpos, reset if any element is too big or nan. .. _mj_checkVel: -mj_checkVel -~~~~~~~~~~~ +`mj_checkVel <#mj_checkVel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_checkVel @@ -815,8 +815,8 @@ Check qvel, reset if any element is too big or nan. .. _mj_checkAcc: -mj_checkAcc -~~~~~~~~~~~ +`mj_checkAcc <#mj_checkAcc>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_checkAcc @@ -824,8 +824,8 @@ Check qacc, reset if any element is too big or nan. .. _mj_kinematics: -mj_kinematics -~~~~~~~~~~~~~ +`mj_kinematics <#mj_kinematics>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_kinematics @@ -833,8 +833,8 @@ Run forward kinematics. .. _mj_comPos: -mj_comPos -~~~~~~~~~ +`mj_comPos <#mj_comPos>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_comPos @@ -842,8 +842,8 @@ Map inertias and motion dofs to global frame centered at CoM. .. _mj_camlight: -mj_camlight -~~~~~~~~~~~ +`mj_camlight <#mj_camlight>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_camlight @@ -851,8 +851,8 @@ Compute camera and light positions and orientations. .. _mj_flex: -mj_flex -~~~~~~~ +`mj_flex <#mj_flex>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_flex @@ -860,8 +860,8 @@ Compute flex-related quantities. .. _mj_tendon: -mj_tendon -~~~~~~~~~ +`mj_tendon <#mj_tendon>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_tendon @@ -869,8 +869,8 @@ Compute tendon lengths, velocities and moment arms. .. _mj_transmission: -mj_transmission -~~~~~~~~~~~~~~~ +`mj_transmission <#mj_transmission>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_transmission @@ -878,8 +878,8 @@ Compute actuator transmission lengths and moments. .. _mj_crb: -mj_crb -~~~~~~ +`mj_crb <#mj_crb>`__ +~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_crb @@ -887,8 +887,8 @@ Run composite rigid body inertia algorithm (CRB). .. _mj_factorM: -mj_factorM -~~~~~~~~~~ +`mj_factorM <#mj_factorM>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_factorM @@ -896,8 +896,8 @@ Compute sparse :math:`L^T D L` factorizaton of inertia matrix. .. _mj_solveM: -mj_solveM -~~~~~~~~~ +`mj_solveM <#mj_solveM>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_solveM @@ -905,8 +905,8 @@ Solve linear system :math:`M x = y` using factorization: :math:`x = (L^T D L)^{- .. _mj_solveM2: -mj_solveM2 -~~~~~~~~~~ +`mj_solveM2 <#mj_solveM2>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_solveM2 @@ -914,8 +914,8 @@ Half of linear solve: :math:`x = \sqrt{D^{-1}} (L^T)^{-1} y` .. _mj_comVel: -mj_comVel -~~~~~~~~~ +`mj_comVel <#mj_comVel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_comVel @@ -923,8 +923,8 @@ Compute cvel, cdof_dot. .. _mj_passive: -mj_passive -~~~~~~~~~~ +`mj_passive <#mj_passive>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_passive @@ -932,8 +932,8 @@ Compute qfrc_passive from spring-dampers, gravity compensation and fluid forces. .. _mj_subtreeVel: -mj_subtreeVel -~~~~~~~~~~~~~ +`mj_subtreeVel <#mj_subtreeVel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_subtreeVel @@ -944,8 +944,8 @@ It is also triggered for :ref:`user sensors` of :ref:`stage`__ +~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_rne @@ -954,8 +954,8 @@ assumes :math:`\ddot q = 0`). .. _mj_rnePostConstraint: -mj_rnePostConstraint -~~~~~~~~~~~~~~~~~~~~ +`mj_rnePostConstraint <#mj_rnePostConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_rnePostConstraint @@ -977,8 +977,8 @@ the effect of spatial tendons, see :github:issue:`832`. .. _mj_collision: -mj_collision -~~~~~~~~~~~~ +`mj_collision <#mj_collision>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_collision @@ -986,8 +986,8 @@ Run collision detection. .. _mj_makeConstraint: -mj_makeConstraint -~~~~~~~~~~~~~~~~~ +`mj_makeConstraint <#mj_makeConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_makeConstraint @@ -995,8 +995,8 @@ Construct constraints. .. _mj_island: -mj_island -~~~~~~~~~ +`mj_island <#mj_island>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_island @@ -1004,8 +1004,8 @@ Find constraint islands. .. _mj_projectConstraint: -mj_projectConstraint -~~~~~~~~~~~~~~~~~~~~ +`mj_projectConstraint <#mj_projectConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_projectConstraint @@ -1013,8 +1013,8 @@ Compute inverse constraint inertia efc_AR. .. _mj_referenceConstraint: -mj_referenceConstraint -~~~~~~~~~~~~~~~~~~~~~~ +`mj_referenceConstraint <#mj_referenceConstraint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_referenceConstraint @@ -1022,8 +1022,8 @@ Compute efc_vel, efc_aref. .. _mj_constraintUpdate: -mj_constraintUpdate -~~~~~~~~~~~~~~~~~~~ +`mj_constraintUpdate <#mj_constraintUpdate>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_constraintUpdate @@ -1047,8 +1047,8 @@ rays from a single point. .. _mj_multiRay: -mj_multiRay -~~~~~~~~~~~ +`mj_multiRay <#mj_multiRay>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_multiRay @@ -1057,8 +1057,8 @@ Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. .. _mj_ray: -mj_ray -~~~~~~ +`mj_ray <#mj_ray>`__ +~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_ray @@ -1075,8 +1075,8 @@ bodyexclude=-1 can be used to indicate that all bodies are included. .. _mj_rayHfield: -mj_rayHfield -~~~~~~~~~~~~ +`mj_rayHfield <#mj_rayHfield>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_rayHfield @@ -1084,8 +1084,8 @@ Intersect ray with hfield, return nearest distance or -1 if no intersection. .. _mj_rayMesh: -mj_rayMesh -~~~~~~~~~~ +`mj_rayMesh <#mj_rayMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_rayMesh @@ -1093,8 +1093,8 @@ Intersect ray with mesh, return nearest distance or -1 if no intersection. .. _mju_rayGeom: -mju_rayGeom -~~~~~~~~~~~ +`mju_rayGeom <#mju_rayGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_rayGeom @@ -1102,8 +1102,8 @@ Intersect ray with pure geom, return nearest distance or -1 if no intersection. .. _mju_rayFlex: -mju_rayFlex -~~~~~~~~~~~ +`mju_rayFlex <#mju_rayFlex>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_rayFlex @@ -1112,8 +1112,8 @@ and also output nearest vertex id. .. _mju_raySkin: -mju_raySkin -~~~~~~~~~~~ +`mju_raySkin <#mju_raySkin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_raySkin @@ -1129,8 +1129,8 @@ These functions can be used to print various quantities to the screen for debugg .. _mj_printFormattedModel: -mj_printFormattedModel -~~~~~~~~~~~~~~~~~~~~~~ +`mj_printFormattedModel <#mj_printFormattedModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_printFormattedModel @@ -1139,8 +1139,8 @@ float_format must be a valid printf-style format string for a single float value .. _mj_printModel: -mj_printModel -~~~~~~~~~~~~~ +`mj_printModel <#mj_printModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_printModel @@ -1148,8 +1148,8 @@ Print model to text file. .. _mj_printFormattedData: -mj_printFormattedData -~~~~~~~~~~~~~~~~~~~~~ +`mj_printFormattedData <#mj_printFormattedData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_printFormattedData @@ -1158,8 +1158,8 @@ float_format must be a valid printf-style format string for a single float value .. _mj_printData: -mj_printData -~~~~~~~~~~~~ +`mj_printData <#mj_printData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_printData @@ -1167,8 +1167,8 @@ Print data to text file. .. _mju_printMat: -mju_printMat -~~~~~~~~~~~~ +`mju_printMat <#mju_printMat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_printMat @@ -1176,8 +1176,8 @@ Print matrix to screen. .. _mju_printMatSparse: -mju_printMatSparse -~~~~~~~~~~~~~~~~~~ +`mju_printMatSparse <#mju_printMatSparse>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_printMatSparse @@ -1185,8 +1185,8 @@ Print sparse matrix to screen. .. _mj_printSchema: -mj_printSchema -~~~~~~~~~~~~~~ +`mj_printSchema <#mj_printSchema>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_printSchema @@ -1209,8 +1209,8 @@ The VFS must first be allocated using :ref:`mj_defaultVFS` and must be freed wit .. _mj_defaultVFS: -mj_defaultVFS -~~~~~~~~~~~~~ +`mj_defaultVFS <#mj_defaultVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_defaultVFS @@ -1218,8 +1218,8 @@ Initialize an empty VFS, :ref:`mj_deleteVFS` must be called to deallocate the VF .. _mj_addFileVFS: -mj_addFileVFS -~~~~~~~~~~~~~ +`mj_addFileVFS <#mj_addFileVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_addFileVFS @@ -1228,8 +1228,8 @@ Add file to VFS. The directory argument is optional and can be NULL or empty. Re .. _mj_addBufferVFS: -mj_addBufferVFS -~~~~~~~~~~~~~~~ +`mj_addBufferVFS <#mj_addBufferVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_addBufferVFS @@ -1237,8 +1237,8 @@ Add file to VFS from buffer, return 0: success, 2: repeated name, -1: failed to .. _mj_deleteFileVFS: -mj_deleteFileVFS -~~~~~~~~~~~~~~~~ +`mj_deleteFileVFS <#mj_deleteFileVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_deleteFileVFS @@ -1246,8 +1246,8 @@ Delete file from VFS, return 0: success, -1: not found in VFS. .. _mj_deleteVFS: -mj_deleteVFS -~~~~~~~~~~~~ +`mj_deleteVFS <#mj_deleteVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_deleteVFS @@ -1263,8 +1263,8 @@ in the code samples. .. _mj_defaultLROpt: -mj_defaultLROpt -~~~~~~~~~~~~~~~ +`mj_defaultLROpt <#mj_defaultLROpt>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_defaultLROpt @@ -1272,8 +1272,8 @@ Set default options for length range computation. .. _mj_defaultSolRefImp: -mj_defaultSolRefImp -~~~~~~~~~~~~~~~~~~~ +`mj_defaultSolRefImp <#mj_defaultSolRefImp>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_defaultSolRefImp @@ -1281,8 +1281,8 @@ Set solver parameters to default values. .. _mj_defaultOption: -mj_defaultOption -~~~~~~~~~~~~~~~~ +`mj_defaultOption <#mj_defaultOption>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_defaultOption @@ -1290,8 +1290,8 @@ Set physics options to default values. .. _mj_defaultVisual: -mj_defaultVisual -~~~~~~~~~~~~~~~~ +`mj_defaultVisual <#mj_defaultVisual>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_defaultVisual @@ -1299,8 +1299,8 @@ Set visual options to default values. .. _mj_copyModel: -mj_copyModel -~~~~~~~~~~~~ +`mj_copyModel <#mj_copyModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_copyModel @@ -1308,8 +1308,8 @@ Copy mjModel, allocate new if dest is NULL. .. _mj_saveModel: -mj_saveModel -~~~~~~~~~~~~ +`mj_saveModel <#mj_saveModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_saveModel @@ -1317,8 +1317,8 @@ Save model to binary MJB file or memory buffer; buffer has precedence when given .. _mj_loadModel: -mj_loadModel -~~~~~~~~~~~~ +`mj_loadModel <#mj_loadModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_loadModel @@ -1327,8 +1327,8 @@ If vfs is not NULL, look up file in vfs before reading from disk. .. _mj_deleteModel: -mj_deleteModel -~~~~~~~~~~~~~~ +`mj_deleteModel <#mj_deleteModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_deleteModel @@ -1336,8 +1336,8 @@ Free memory allocation in model. .. _mj_sizeModel: -mj_sizeModel -~~~~~~~~~~~~ +`mj_sizeModel <#mj_sizeModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_sizeModel @@ -1345,8 +1345,8 @@ Return size of buffer needed to hold model. .. _mj_makeData: -mj_makeData -~~~~~~~~~~~ +`mj_makeData <#mj_makeData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_makeData @@ -1355,8 +1355,8 @@ If the model buffer is unallocated the initial configuration will not be set. .. _mj_copyData: -mj_copyData -~~~~~~~~~~~ +`mj_copyData <#mj_copyData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_copyData @@ -1365,8 +1365,8 @@ m is only required to contain the size fields from MJMODEL_INTS. .. _mj_resetData: -mj_resetData -~~~~~~~~~~~~ +`mj_resetData <#mj_resetData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_resetData @@ -1374,8 +1374,8 @@ Reset data to defaults. .. _mj_resetDataDebug: -mj_resetDataDebug -~~~~~~~~~~~~~~~~~ +`mj_resetDataDebug <#mj_resetDataDebug>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_resetDataDebug @@ -1383,8 +1383,8 @@ Reset data to defaults, fill everything else with debug_value. .. _mj_resetDataKeyframe: -mj_resetDataKeyframe -~~~~~~~~~~~~~~~~~~~~ +`mj_resetDataKeyframe <#mj_resetDataKeyframe>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_resetDataKeyframe @@ -1392,8 +1392,8 @@ Reset data. If 0 <= key < nkey, set fields from specified keyframe. .. _mj_markStack: -mj_markStack -~~~~~~~~~~~~ +`mj_markStack <#mj_markStack>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_markStack @@ -1401,8 +1401,8 @@ Mark a new frame on the mjData stack. .. _mj_freeStack: -mj_freeStack -~~~~~~~~~~~~ +`mj_freeStack <#mj_freeStack>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_freeStack @@ -1411,8 +1411,8 @@ to mj_markStack must no longer be used afterwards. .. _mj_stackAllocByte: -mj_stackAllocByte -~~~~~~~~~~~~~~~~~ +`mj_stackAllocByte <#mj_stackAllocByte>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_stackAllocByte @@ -1421,8 +1421,8 @@ Call mju_error on stack overflow. .. _mj_stackAllocNum: -mj_stackAllocNum -~~~~~~~~~~~~~~~~ +`mj_stackAllocNum <#mj_stackAllocNum>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_stackAllocNum @@ -1430,8 +1430,8 @@ Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow. .. _mj_stackAllocInt: -mj_stackAllocInt -~~~~~~~~~~~~~~~~ +`mj_stackAllocInt <#mj_stackAllocInt>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_stackAllocInt @@ -1439,8 +1439,8 @@ Allocate array of ints on mjData stack. Call mju_error on stack overflow. .. _mj_deleteData: -mj_deleteData -~~~~~~~~~~~~~ +`mj_deleteData <#mj_deleteData>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_deleteData @@ -1448,8 +1448,8 @@ Free memory allocation in mjData. .. _mj_resetCallbacks: -mj_resetCallbacks -~~~~~~~~~~~~~~~~~ +`mj_resetCallbacks <#mj_resetCallbacks>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_resetCallbacks @@ -1457,8 +1457,8 @@ Reset all callbacks to NULL pointers (NULL is the default). .. _mj_setConst: -mj_setConst -~~~~~~~~~~~ +`mj_setConst <#mj_setConst>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_setConst @@ -1466,8 +1466,8 @@ Set constant fields of mjModel, corresponding to qpos0 configuration. .. _mj_setLengthRange: -mj_setLengthRange -~~~~~~~~~~~~~~~~~ +`mj_setLengthRange <#mj_setLengthRange>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_setLengthRange @@ -1475,8 +1475,8 @@ Set actuator_lengthrange for specified actuator; return 1 if ok, 0 if error. .. _mj_makeSpec: -mj_makeSpec -~~~~~~~~~~~ +`mj_makeSpec <#mj_makeSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_makeSpec @@ -1484,8 +1484,8 @@ Create empty spec. .. _mj_copySpec: -mj_copySpec -~~~~~~~~~~~ +`mj_copySpec <#mj_copySpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_copySpec @@ -1493,8 +1493,8 @@ Copy spec. .. _mj_deleteSpec: -mj_deleteSpec -~~~~~~~~~~~~~ +`mj_deleteSpec <#mj_deleteSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_deleteSpec @@ -1502,8 +1502,8 @@ Free memory allocation in mjSpec. .. _mjs_activatePlugin: -mjs_activatePlugin -~~~~~~~~~~~~~~~~~~ +`mjs_activatePlugin <#mjs_activatePlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_activatePlugin @@ -1516,8 +1516,8 @@ Error and memory .. _mju_error: -mju_error -~~~~~~~~~ +`mju_error <#mju_error>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_error @@ -1525,8 +1525,8 @@ Main error function; does not return to caller. .. _mju_error_i: -mju_error_i -~~~~~~~~~~~ +`mju_error_i <#mju_error_i>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_error_i @@ -1534,8 +1534,8 @@ Deprecated: use mju_error. .. _mju_error_s: -mju_error_s -~~~~~~~~~~~ +`mju_error_s <#mju_error_s>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_error_s @@ -1543,8 +1543,8 @@ Deprecated: use mju_error. .. _mju_warning: -mju_warning -~~~~~~~~~~~ +`mju_warning <#mju_warning>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_warning @@ -1552,8 +1552,8 @@ Main warning function; returns to caller. .. _mju_warning_i: -mju_warning_i -~~~~~~~~~~~~~ +`mju_warning_i <#mju_warning_i>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_warning_i @@ -1561,8 +1561,8 @@ Deprecated: use mju_warning. .. _mju_warning_s: -mju_warning_s -~~~~~~~~~~~~~ +`mju_warning_s <#mju_warning_s>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_warning_s @@ -1570,8 +1570,8 @@ Deprecated: use mju_warning. .. _mju_clearHandlers: -mju_clearHandlers -~~~~~~~~~~~~~~~~~ +`mju_clearHandlers <#mju_clearHandlers>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_clearHandlers @@ -1579,8 +1579,8 @@ Clear user error and memory handlers. .. _mju_malloc: -mju_malloc -~~~~~~~~~~ +`mju_malloc <#mju_malloc>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_malloc @@ -1588,8 +1588,8 @@ Allocate memory; byte-align on 64; pad size to multiple of 64. .. _mju_free: -mju_free -~~~~~~~~ +`mju_free <#mju_free>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_free @@ -1597,8 +1597,8 @@ Free memory, using free() by default. .. _mj_warning: -mj_warning -~~~~~~~~~~ +`mj_warning <#mj_warning>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mj_warning @@ -1606,8 +1606,8 @@ High-level warning function: count warnings in mjData, print only the first. .. _mju_writeLog: -mju_writeLog -~~~~~~~~~~~~ +`mju_writeLog <#mju_writeLog>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_writeLog @@ -1615,8 +1615,8 @@ Write [datetime, type: message] to MUJOCO_LOG.TXT. .. _mjs_getError: -mjs_getError -~~~~~~~~~~~~ +`mjs_getError <#mjs_getError>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getError @@ -1624,8 +1624,8 @@ Get compiler error message from spec. .. _mjs_isWarning: -mjs_isWarning -~~~~~~~~~~~~~ +`mjs_isWarning <#mjs_isWarning>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_isWarning @@ -1638,8 +1638,8 @@ Miscellaneous .. _mju_muscleGain: -mju_muscleGain -~~~~~~~~~~~~~~ +`mju_muscleGain <#mju_muscleGain>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_muscleGain @@ -1647,8 +1647,8 @@ Muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvm .. _mju_muscleBias: -mju_muscleBias -~~~~~~~~~~~~~~ +`mju_muscleBias <#mju_muscleBias>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_muscleBias @@ -1656,8 +1656,8 @@ Muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fv .. _mju_muscleDynamics: -mju_muscleDynamics -~~~~~~~~~~~~~~~~~~ +`mju_muscleDynamics <#mju_muscleDynamics>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_muscleDynamics @@ -1665,8 +1665,8 @@ Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width). .. _mju_encodePyramid: -mju_encodePyramid -~~~~~~~~~~~~~~~~~ +`mju_encodePyramid <#mju_encodePyramid>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_encodePyramid @@ -1674,8 +1674,8 @@ Convert contact force to pyramid representation. .. _mju_decodePyramid: -mju_decodePyramid -~~~~~~~~~~~~~~~~~ +`mju_decodePyramid <#mju_decodePyramid>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_decodePyramid @@ -1683,8 +1683,8 @@ Convert pyramid representation to contact force. .. _mju_springDamper: -mju_springDamper -~~~~~~~~~~~~~~~~ +`mju_springDamper <#mju_springDamper>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_springDamper @@ -1692,8 +1692,8 @@ Integrate spring-damper analytically, return pos(dt). .. _mju_min: -mju_min -~~~~~~~ +`mju_min <#mju_min>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_min @@ -1701,8 +1701,8 @@ Return min(a,b) with single evaluation of a and b. .. _mju_max: -mju_max -~~~~~~~ +`mju_max <#mju_max>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_max @@ -1710,8 +1710,8 @@ Return max(a,b) with single evaluation of a and b. .. _mju_clip: -mju_clip -~~~~~~~~ +`mju_clip <#mju_clip>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_clip @@ -1719,8 +1719,8 @@ Clip x to the range [min, max]. .. _mju_sign: -mju_sign -~~~~~~~~ +`mju_sign <#mju_sign>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sign @@ -1728,8 +1728,8 @@ Return sign of x: +1, -1 or 0. .. _mju_round: -mju_round -~~~~~~~~~ +`mju_round <#mju_round>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_round @@ -1737,8 +1737,8 @@ Round x to nearest integer. .. _mju_type2Str: -mju_type2Str -~~~~~~~~~~~~ +`mju_type2Str <#mju_type2Str>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_type2Str @@ -1746,8 +1746,8 @@ Convert type id (mjtObj) to type name. .. _mju_str2Type: -mju_str2Type -~~~~~~~~~~~~ +`mju_str2Type <#mju_str2Type>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_str2Type @@ -1755,8 +1755,8 @@ Convert type name to type id (mjtObj). .. _mju_writeNumBytes: -mju_writeNumBytes -~~~~~~~~~~~~~~~~~ +`mju_writeNumBytes <#mju_writeNumBytes>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_writeNumBytes @@ -1764,8 +1764,8 @@ Return human readable number of bytes using standard letter suffix. .. _mju_warningText: -mju_warningText -~~~~~~~~~~~~~~~ +`mju_warningText <#mju_warningText>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_warningText @@ -1773,8 +1773,8 @@ Construct a warning message given the warning type and info. .. _mju_isBad: -mju_isBad -~~~~~~~~~ +`mju_isBad <#mju_isBad>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_isBad @@ -1782,8 +1782,8 @@ Return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise. Used by check functions. .. _mju_isZero: -mju_isZero -~~~~~~~~~~ +`mju_isZero <#mju_isZero>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_isZero @@ -1791,8 +1791,8 @@ Return 1 if all elements are 0. .. _mju_standardNormal: -mju_standardNormal -~~~~~~~~~~~~~~~~~~ +`mju_standardNormal <#mju_standardNormal>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_standardNormal @@ -1800,8 +1800,8 @@ Standard normal random number generator (optional second number). .. _mju_f2n: -mju_f2n -~~~~~~~ +`mju_f2n <#mju_f2n>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_f2n @@ -1809,8 +1809,8 @@ Convert from float to mjtNum. .. _mju_n2f: -mju_n2f -~~~~~~~ +`mju_n2f <#mju_n2f>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_n2f @@ -1818,8 +1818,8 @@ Convert from mjtNum to float. .. _mju_d2n: -mju_d2n -~~~~~~~ +`mju_d2n <#mju_d2n>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_d2n @@ -1827,8 +1827,8 @@ Convert from double to mjtNum. .. _mju_n2d: -mju_n2d -~~~~~~~ +`mju_n2d <#mju_n2d>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_n2d @@ -1836,8 +1836,8 @@ Convert from mjtNum to double. .. _mju_insertionSort: -mju_insertionSort -~~~~~~~~~~~~~~~~~ +`mju_insertionSort <#mju_insertionSort>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_insertionSort @@ -1845,8 +1845,8 @@ Insertion sort, resulting list is in increasing order. .. _mju_insertionSortInt: -mju_insertionSortInt -~~~~~~~~~~~~~~~~~~~~ +`mju_insertionSortInt <#mju_insertionSortInt>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_insertionSortInt @@ -1854,8 +1854,8 @@ Integer insertion sort, resulting list is in increasing order. .. _mju_Halton: -mju_Halton -~~~~~~~~~~ +`mju_Halton <#mju_Halton>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_Halton @@ -1863,8 +1863,8 @@ Generate Halton sequence. .. _mju_strncpy: -mju_strncpy -~~~~~~~~~~~ +`mju_strncpy <#mju_strncpy>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_strncpy @@ -1872,8 +1872,8 @@ Call strncpy, then set dst[n-1] = 0. .. _mju_sigmoid: -mju_sigmoid -~~~~~~~~~~~ +`mju_sigmoid <#mju_sigmoid>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sigmoid @@ -1897,8 +1897,8 @@ illustrated in :ref:`simulate`. .. _mjv_defaultCamera: -mjv_defaultCamera -~~~~~~~~~~~~~~~~~ +`mjv_defaultCamera <#mjv_defaultCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultCamera @@ -1906,8 +1906,8 @@ Set default camera. .. _mjv_defaultFreeCamera: -mjv_defaultFreeCamera -~~~~~~~~~~~~~~~~~~~~~ +`mjv_defaultFreeCamera <#mjv_defaultFreeCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultFreeCamera @@ -1915,8 +1915,8 @@ Set default free camera. .. _mjv_defaultPerturb: -mjv_defaultPerturb -~~~~~~~~~~~~~~~~~~ +`mjv_defaultPerturb <#mjv_defaultPerturb>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultPerturb @@ -1924,8 +1924,8 @@ Set default perturbation. .. _mjv_room2model: -mjv_room2model -~~~~~~~~~~~~~~ +`mjv_room2model <#mjv_room2model>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_room2model @@ -1933,8 +1933,8 @@ Transform pose from room to model space. .. _mjv_model2room: -mjv_model2room -~~~~~~~~~~~~~~ +`mjv_model2room <#mjv_model2room>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_model2room @@ -1942,8 +1942,8 @@ Transform pose from model to room space. .. _mjv_cameraInModel: -mjv_cameraInModel -~~~~~~~~~~~~~~~~~ +`mjv_cameraInModel <#mjv_cameraInModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_cameraInModel @@ -1951,8 +1951,8 @@ Get camera info in model space; average left and right OpenGL cameras. .. _mjv_cameraInRoom: -mjv_cameraInRoom -~~~~~~~~~~~~~~~~ +`mjv_cameraInRoom <#mjv_cameraInRoom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_cameraInRoom @@ -1960,8 +1960,8 @@ Get camera info in room space; average left and right OpenGL cameras. .. _mjv_frustumHeight: -mjv_frustumHeight -~~~~~~~~~~~~~~~~~ +`mjv_frustumHeight <#mjv_frustumHeight>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_frustumHeight @@ -1969,8 +1969,8 @@ Get frustum height at unit distance from camera; average left and right OpenGL c .. _mjv_alignToCamera: -mjv_alignToCamera -~~~~~~~~~~~~~~~~~ +`mjv_alignToCamera <#mjv_alignToCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_alignToCamera @@ -1978,8 +1978,8 @@ Rotate 3D vec in horizontal plane by angle between (0,1) and (forward_x,forward_ .. _mjv_moveCamera: -mjv_moveCamera -~~~~~~~~~~~~~~ +`mjv_moveCamera <#mjv_moveCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_moveCamera @@ -1987,8 +1987,8 @@ Move camera with mouse; action is mjtMouse. .. _mjv_moveCameraFromState: -mjv_moveCameraFromState -~~~~~~~~~~~~~~~~~~~~~~~ +`mjv_moveCameraFromState <#mjv_moveCameraFromState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_moveCameraFromState @@ -1996,8 +1996,8 @@ Move camera with mouse given a scene state; action is mjtMouse. .. _mjv_movePerturb: -mjv_movePerturb -~~~~~~~~~~~~~~~ +`mjv_movePerturb <#mjv_movePerturb>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_movePerturb @@ -2005,8 +2005,8 @@ Move perturb object with mouse; action is mjtMouse. .. _mjv_movePerturbFromState: -mjv_movePerturbFromState -~~~~~~~~~~~~~~~~~~~~~~~~ +`mjv_movePerturbFromState <#mjv_movePerturbFromState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_movePerturbFromState @@ -2014,8 +2014,8 @@ Move perturb object with mouse given a scene state; action is mjtMouse. .. _mjv_moveModel: -mjv_moveModel -~~~~~~~~~~~~~ +`mjv_moveModel <#mjv_moveModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_moveModel @@ -2023,8 +2023,8 @@ Move model with mouse; action is mjtMouse. .. _mjv_initPerturb: -mjv_initPerturb -~~~~~~~~~~~~~~~ +`mjv_initPerturb <#mjv_initPerturb>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_initPerturb @@ -2032,8 +2032,8 @@ Copy perturb pos,quat from selected body; set scale for perturbation. .. _mjv_applyPerturbPose: -mjv_applyPerturbPose -~~~~~~~~~~~~~~~~~~~~ +`mjv_applyPerturbPose <#mjv_applyPerturbPose>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_applyPerturbPose @@ -2042,8 +2042,8 @@ Write d->qpos only if flg_paused and subtree root for selected body has free joi .. _mjv_applyPerturbForce: -mjv_applyPerturbForce -~~~~~~~~~~~~~~~~~~~~~ +`mjv_applyPerturbForce <#mjv_applyPerturbForce>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_applyPerturbForce @@ -2051,8 +2051,8 @@ Set perturb force,torque in d->xfrc_applied, if selected body is dynamic. .. _mjv_averageCamera: -mjv_averageCamera -~~~~~~~~~~~~~~~~~ +`mjv_averageCamera <#mjv_averageCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_averageCamera @@ -2060,8 +2060,8 @@ Return the average of two OpenGL cameras. .. _mjv_select: -mjv_select -~~~~~~~~~~ +`mjv_select <#mjv_select>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_select @@ -2082,8 +2082,8 @@ Unity or Unreal Engine. See :ref:`simulate` for illustration of how .. _mjv_defaultOption: -mjv_defaultOption -~~~~~~~~~~~~~~~~~ +`mjv_defaultOption <#mjv_defaultOption>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultOption @@ -2091,8 +2091,8 @@ Set default visualization options. .. _mjv_defaultFigure: -mjv_defaultFigure -~~~~~~~~~~~~~~~~~ +`mjv_defaultFigure <#mjv_defaultFigure>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultFigure @@ -2100,8 +2100,8 @@ Set default figure. .. _mjv_initGeom: -mjv_initGeom -~~~~~~~~~~~~ +`mjv_initGeom <#mjv_initGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_initGeom @@ -2109,8 +2109,8 @@ Initialize given geom fields when not NULL, set the rest to their default values .. _mjv_connector: -mjv_connector -~~~~~~~~~~~~~ +`mjv_connector <#mjv_connector>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_connector @@ -2120,8 +2120,8 @@ Width of mjGEOM_LINE is denominated in pixels. .. _mjv_defaultScene: -mjv_defaultScene -~~~~~~~~~~~~~~~~ +`mjv_defaultScene <#mjv_defaultScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultScene @@ -2129,8 +2129,8 @@ Set default abstract scene. .. _mjv_makeScene: -mjv_makeScene -~~~~~~~~~~~~~ +`mjv_makeScene <#mjv_makeScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_makeScene @@ -2138,8 +2138,8 @@ Allocate resources in abstract scene. .. _mjv_freeScene: -mjv_freeScene -~~~~~~~~~~~~~ +`mjv_freeScene <#mjv_freeScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_freeScene @@ -2147,8 +2147,8 @@ Free abstract scene. .. _mjv_updateScene: -mjv_updateScene -~~~~~~~~~~~~~~~ +`mjv_updateScene <#mjv_updateScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_updateScene @@ -2156,8 +2156,8 @@ Update entire scene given model state. .. _mjv_updateSceneFromState: -mjv_updateSceneFromState -~~~~~~~~~~~~~~~~~~~~~~~~ +`mjv_updateSceneFromState <#mjv_updateSceneFromState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_updateSceneFromState @@ -2165,8 +2165,8 @@ Update entire scene from a scene state, return the number of new mjWARN_VGEOMFUL .. _mjv_defaultSceneState: -mjv_defaultSceneState -~~~~~~~~~~~~~~~~~~~~~ +`mjv_defaultSceneState <#mjv_defaultSceneState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_defaultSceneState @@ -2174,8 +2174,8 @@ Set default scene state. .. _mjv_makeSceneState: -mjv_makeSceneState -~~~~~~~~~~~~~~~~~~ +`mjv_makeSceneState <#mjv_makeSceneState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_makeSceneState @@ -2183,8 +2183,8 @@ Allocate resources and initialize a scene state object. .. _mjv_freeSceneState: -mjv_freeSceneState -~~~~~~~~~~~~~~~~~~ +`mjv_freeSceneState <#mjv_freeSceneState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_freeSceneState @@ -2192,8 +2192,8 @@ Free scene state. .. _mjv_updateSceneState: -mjv_updateSceneState -~~~~~~~~~~~~~~~~~~~~ +`mjv_updateSceneState <#mjv_updateSceneState>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_updateSceneState @@ -2201,8 +2201,8 @@ Update a scene state from model and data. .. _mjv_addGeoms: -mjv_addGeoms -~~~~~~~~~~~~ +`mjv_addGeoms <#mjv_addGeoms>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_addGeoms @@ -2210,8 +2210,8 @@ Add geoms from selected categories. .. _mjv_makeLights: -mjv_makeLights -~~~~~~~~~~~~~~ +`mjv_makeLights <#mjv_makeLights>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_makeLights @@ -2219,8 +2219,8 @@ Make list of lights. .. _mjv_updateCamera: -mjv_updateCamera -~~~~~~~~~~~~~~~~ +`mjv_updateCamera <#mjv_updateCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_updateCamera @@ -2228,8 +2228,8 @@ Update camera. .. _mjv_updateSkin: -mjv_updateSkin -~~~~~~~~~~~~~~ +`mjv_updateSkin <#mjv_updateSkin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjv_updateSkin @@ -2245,8 +2245,8 @@ of how to use these functions. .. _mjr_defaultContext: -mjr_defaultContext -~~~~~~~~~~~~~~~~~~ +`mjr_defaultContext <#mjr_defaultContext>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_defaultContext @@ -2254,8 +2254,8 @@ Set default mjrContext. .. _mjr_makeContext: -mjr_makeContext -~~~~~~~~~~~~~~~ +`mjr_makeContext <#mjr_makeContext>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_makeContext @@ -2263,8 +2263,8 @@ Allocate resources in custom OpenGL context; fontscale is mjtFontScale. .. _mjr_changeFont: -mjr_changeFont -~~~~~~~~~~~~~~ +`mjr_changeFont <#mjr_changeFont>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_changeFont @@ -2272,8 +2272,8 @@ Change font of existing context. .. _mjr_addAux: -mjr_addAux -~~~~~~~~~~ +`mjr_addAux <#mjr_addAux>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_addAux @@ -2281,8 +2281,8 @@ Add Aux buffer with given index to context; free previous Aux buffer. .. _mjr_freeContext: -mjr_freeContext -~~~~~~~~~~~~~~~ +`mjr_freeContext <#mjr_freeContext>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_freeContext @@ -2290,8 +2290,8 @@ Free resources in custom OpenGL context, set to default. .. _mjr_resizeOffscreen: -mjr_resizeOffscreen -~~~~~~~~~~~~~~~~~~~ +`mjr_resizeOffscreen <#mjr_resizeOffscreen>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_resizeOffscreen @@ -2299,8 +2299,8 @@ Resize offscreen buffers. .. _mjr_uploadTexture: -mjr_uploadTexture -~~~~~~~~~~~~~~~~~ +`mjr_uploadTexture <#mjr_uploadTexture>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_uploadTexture @@ -2308,8 +2308,8 @@ Upload texture to GPU, overwriting previous upload if any. .. _mjr_uploadMesh: -mjr_uploadMesh -~~~~~~~~~~~~~~ +`mjr_uploadMesh <#mjr_uploadMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_uploadMesh @@ -2317,8 +2317,8 @@ Upload mesh to GPU, overwriting previous upload if any. .. _mjr_uploadHField: -mjr_uploadHField -~~~~~~~~~~~~~~~~ +`mjr_uploadHField <#mjr_uploadHField>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_uploadHField @@ -2326,8 +2326,8 @@ Upload height field to GPU, overwriting previous upload if any. .. _mjr_restoreBuffer: -mjr_restoreBuffer -~~~~~~~~~~~~~~~~~ +`mjr_restoreBuffer <#mjr_restoreBuffer>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_restoreBuffer @@ -2335,8 +2335,8 @@ Make con->currentBuffer current again. .. _mjr_setBuffer: -mjr_setBuffer -~~~~~~~~~~~~~ +`mjr_setBuffer <#mjr_setBuffer>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_setBuffer @@ -2345,8 +2345,8 @@ If only one buffer is available, set that buffer and ignore framebuffer argument .. _mjr_readPixels: -mjr_readPixels -~~~~~~~~~~~~~~ +`mjr_readPixels <#mjr_readPixels>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_readPixels @@ -2355,8 +2355,8 @@ Viewport is in OpenGL framebuffer; client buffer starts at (0,0). .. _mjr_drawPixels: -mjr_drawPixels -~~~~~~~~~~~~~~ +`mjr_drawPixels <#mjr_drawPixels>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_drawPixels @@ -2365,8 +2365,8 @@ Viewport is in OpenGL framebuffer; client buffer starts at (0,0). .. _mjr_blitBuffer: -mjr_blitBuffer -~~~~~~~~~~~~~~ +`mjr_blitBuffer <#mjr_blitBuffer>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_blitBuffer @@ -2375,8 +2375,8 @@ If src, dst have different size and flg_depth==0, color is interpolated with GL_ .. _mjr_setAux: -mjr_setAux -~~~~~~~~~~ +`mjr_setAux <#mjr_setAux>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_setAux @@ -2384,8 +2384,8 @@ Set Aux buffer for custom OpenGL rendering (call restoreBuffer when done). .. _mjr_blitAux: -mjr_blitAux -~~~~~~~~~~~ +`mjr_blitAux <#mjr_blitAux>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_blitAux @@ -2393,8 +2393,8 @@ Blit from Aux buffer to con->currentBuffer. .. _mjr_text: -mjr_text -~~~~~~~~ +`mjr_text <#mjr_text>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_text @@ -2402,8 +2402,8 @@ Draw text at (x,y) in relative coordinates; font is mjtFont. .. _mjr_overlay: -mjr_overlay -~~~~~~~~~~~ +`mjr_overlay <#mjr_overlay>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_overlay @@ -2411,8 +2411,8 @@ Draw text overlay; font is mjtFont; gridpos is mjtGridPos. .. _mjr_maxViewport: -mjr_maxViewport -~~~~~~~~~~~~~~~ +`mjr_maxViewport <#mjr_maxViewport>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_maxViewport @@ -2420,8 +2420,8 @@ Get maximum viewport for active buffer. .. _mjr_rectangle: -mjr_rectangle -~~~~~~~~~~~~~ +`mjr_rectangle <#mjr_rectangle>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_rectangle @@ -2429,8 +2429,8 @@ Draw rectangle. .. _mjr_label: -mjr_label -~~~~~~~~~ +`mjr_label <#mjr_label>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_label @@ -2438,8 +2438,8 @@ Draw rectangle with centered text. .. _mjr_figure: -mjr_figure -~~~~~~~~~~ +`mjr_figure <#mjr_figure>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_figure @@ -2447,8 +2447,8 @@ Draw 2D figure. .. _mjr_render: -mjr_render -~~~~~~~~~~ +`mjr_render <#mjr_render>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_render @@ -2456,8 +2456,8 @@ Render 3D scene. .. _mjr_finish: -mjr_finish -~~~~~~~~~~ +`mjr_finish <#mjr_finish>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_finish @@ -2465,8 +2465,8 @@ Call glFinish. .. _mjr_getError: -mjr_getError -~~~~~~~~~~~~ +`mjr_getError <#mjr_getError>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_getError @@ -2474,8 +2474,8 @@ Call glGetError and return result. .. _mjr_findRect: -mjr_findRect -~~~~~~~~~~~~ +`mjr_findRect <#mjr_findRect>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjr_findRect @@ -2490,8 +2490,8 @@ For a high-level description of the UI framework, see :ref:`UI`. .. _mjui_themeSpacing: -mjui_themeSpacing -~~~~~~~~~~~~~~~~~ +`mjui_themeSpacing <#mjui_themeSpacing>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_themeSpacing @@ -2499,8 +2499,8 @@ Get builtin UI theme spacing (ind: 0-1). .. _mjui_themeColor: -mjui_themeColor -~~~~~~~~~~~~~~~ +`mjui_themeColor <#mjui_themeColor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_themeColor @@ -2508,8 +2508,8 @@ Get builtin UI theme color (ind: 0-3). .. _mjui_add: -mjui_add -~~~~~~~~ +`mjui_add <#mjui_add>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_add @@ -2522,8 +2522,8 @@ of the UI. Keep in mind that there is a maximum preallocated number of sections .. _mjui_addToSection: -mjui_addToSection -~~~~~~~~~~~~~~~~~ +`mjui_addToSection <#mjui_addToSection>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_addToSection @@ -2531,8 +2531,8 @@ Add definitions to UI section. .. _mjui_resize: -mjui_resize -~~~~~~~~~~~ +`mjui_resize <#mjui_resize>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_resize @@ -2540,8 +2540,8 @@ Compute UI sizes. .. _mjui_update: -mjui_update -~~~~~~~~~~~ +`mjui_update <#mjui_update>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_update @@ -2554,8 +2554,8 @@ specifies the section and the item that was modified. A value of -1 means all it .. _mjui_event: -mjui_event -~~~~~~~~~~ +`mjui_event <#mjui_event>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_event @@ -2566,8 +2566,8 @@ depending on which UI item was modified and what the state of that item is after .. _mjui_render: -mjui_render -~~~~~~~~~~~ +`mjui_render <#mjui_render>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjui_render @@ -2586,8 +2586,8 @@ outputs of derivative functions are the trailing rather than leading arguments. .. _mjd_transitionFD: -mjd_transitionFD -~~~~~~~~~~~~~~~~ +`mjd_transitionFD <#mjd_transitionFD>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjd_transitionFD @@ -2635,8 +2635,8 @@ These matrices and their dimensions are: .. _mjd_inverseFD: -mjd_inverseFD -~~~~~~~~~~~~~ +`mjd_inverseFD <#mjd_inverseFD>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjd_inverseFD @@ -2678,8 +2678,8 @@ using finite-differencing. These matrices and their dimensions are: .. _mjd_subQuat: -mjd_subQuat -~~~~~~~~~~~ +`mjd_subQuat <#mjd_subQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjd_subQuat @@ -2687,8 +2687,8 @@ Derivatives of :ref:`mju_subQuat` (quaternion difference). .. _mjd_quatIntegrate: -mjd_quatIntegrate -~~~~~~~~~~~~~~~~~ +`mjd_quatIntegrate <#mjd_quatIntegrate>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjd_quatIntegrate @@ -2718,8 +2718,8 @@ Plugins ^^^^^^^ .. _mjp_defaultPlugin: -mjp_defaultPlugin -~~~~~~~~~~~~~~~~~ +`mjp_defaultPlugin <#mjp_defaultPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_defaultPlugin @@ -2727,8 +2727,8 @@ Set default plugin definition. .. _mjp_registerPlugin: -mjp_registerPlugin -~~~~~~~~~~~~~~~~~~ +`mjp_registerPlugin <#mjp_registerPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_registerPlugin @@ -2741,8 +2741,8 @@ need not be the same. .. _mjp_pluginCount: -mjp_pluginCount -~~~~~~~~~~~~~~~ +`mjp_pluginCount <#mjp_pluginCount>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_pluginCount @@ -2750,8 +2750,8 @@ Return the number of globally registered plugins. .. _mjp_getPlugin: -mjp_getPlugin -~~~~~~~~~~~~~ +`mjp_getPlugin <#mjp_getPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_getPlugin @@ -2759,8 +2759,8 @@ Look up a plugin by name. If slot is not NULL, also write its registered slot nu .. _mjp_getPluginAtSlot: -mjp_getPluginAtSlot -~~~~~~~~~~~~~~~~~~~ +`mjp_getPluginAtSlot <#mjp_getPluginAtSlot>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_getPluginAtSlot @@ -2768,8 +2768,8 @@ Look up a plugin by the registered slot number that was returned by mjp_register .. _mjp_defaultResourceProvider: -mjp_defaultResourceProvider -~~~~~~~~~~~~~~~~~~~~~~~~~~~ +`mjp_defaultResourceProvider <#mjp_defaultResourceProvider>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_defaultResourceProvider @@ -2777,8 +2777,8 @@ Set default resource provider definition. .. _mjp_registerResourceProvider: -mjp_registerResourceProvider -~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +`mjp_registerResourceProvider <#mjp_registerResourceProvider>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_registerResourceProvider @@ -2788,8 +2788,8 @@ returns a slot number > 0 on success. .. _mjp_resourceProviderCount: -mjp_resourceProviderCount -~~~~~~~~~~~~~~~~~~~~~~~~~ +`mjp_resourceProviderCount <#mjp_resourceProviderCount>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_resourceProviderCount @@ -2797,8 +2797,8 @@ Return the number of globally registered resource providers. .. _mjp_getResourceProvider: -mjp_getResourceProvider -~~~~~~~~~~~~~~~~~~~~~~~ +`mjp_getResourceProvider <#mjp_getResourceProvider>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_getResourceProvider @@ -2807,8 +2807,8 @@ If no match, return NULL. .. _mjp_getResourceProviderAtSlot: -mjp_getResourceProviderAtSlot -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +`mjp_getResourceProviderAtSlot <#mjp_getResourceProviderAtSlot>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjp_getResourceProviderAtSlot @@ -2821,8 +2821,8 @@ Threads ^^^^^^^ .. _mju_threadPoolCreate: -mju_threadPoolCreate -~~~~~~~~~~~~~~~~~~~~ +`mju_threadPoolCreate <#mju_threadPoolCreate>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_threadPoolCreate @@ -2830,8 +2830,8 @@ Create a thread pool with the specified number of threads running. .. _mju_bindThreadPool: -mju_bindThreadPool -~~~~~~~~~~~~~~~~~~ +`mju_bindThreadPool <#mju_bindThreadPool>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_bindThreadPool @@ -2839,8 +2839,8 @@ Adds a thread pool to mjData and configures it for multi-threaded use. .. _mju_threadPoolEnqueue: -mju_threadPoolEnqueue -~~~~~~~~~~~~~~~~~~~~~ +`mju_threadPoolEnqueue <#mju_threadPoolEnqueue>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_threadPoolEnqueue @@ -2848,8 +2848,8 @@ Enqueue a task in a thread pool. .. _mju_threadPoolDestroy: -mju_threadPoolDestroy -~~~~~~~~~~~~~~~~~~~~~ +`mju_threadPoolDestroy <#mju_threadPoolDestroy>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_threadPoolDestroy @@ -2857,8 +2857,8 @@ Destroy a thread pool. .. _mju_defaultTask: -mju_defaultTask -~~~~~~~~~~~~~~~ +`mju_defaultTask <#mju_defaultTask>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_defaultTask @@ -2866,8 +2866,8 @@ Initialize an mjTask. .. _mju_taskJoin: -mju_taskJoin -~~~~~~~~~~~~ +`mju_taskJoin <#mju_taskJoin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_taskJoin @@ -2997,8 +2997,8 @@ Vector math .. _mju_zero3: -mju_zero3 -~~~~~~~~~ +`mju_zero3 <#mju_zero3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_zero3 @@ -3006,8 +3006,8 @@ Set res = 0. .. _mju_copy3: -mju_copy3 -~~~~~~~~~ +`mju_copy3 <#mju_copy3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_copy3 @@ -3015,8 +3015,8 @@ Set res = vec. .. _mju_scl3: -mju_scl3 -~~~~~~~~ +`mju_scl3 <#mju_scl3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_scl3 @@ -3024,8 +3024,8 @@ Set res = vec*scl. .. _mju_add3: -mju_add3 -~~~~~~~~ +`mju_add3 <#mju_add3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_add3 @@ -3033,8 +3033,8 @@ Set res = vec1 + vec2. .. _mju_sub3: -mju_sub3 -~~~~~~~~ +`mju_sub3 <#mju_sub3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sub3 @@ -3042,8 +3042,8 @@ Set res = vec1 - vec2. .. _mju_addTo3: -mju_addTo3 -~~~~~~~~~~ +`mju_addTo3 <#mju_addTo3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addTo3 @@ -3051,8 +3051,8 @@ Set res = res + vec. .. _mju_subFrom3: -mju_subFrom3 -~~~~~~~~~~~~ +`mju_subFrom3 <#mju_subFrom3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_subFrom3 @@ -3060,8 +3060,8 @@ Set res = res - vec. .. _mju_addToScl3: -mju_addToScl3 -~~~~~~~~~~~~~ +`mju_addToScl3 <#mju_addToScl3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addToScl3 @@ -3069,8 +3069,8 @@ Set res = res + vec*scl. .. _mju_addScl3: -mju_addScl3 -~~~~~~~~~~~ +`mju_addScl3 <#mju_addScl3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addScl3 @@ -3078,8 +3078,8 @@ Set res = vec1 + vec2*scl. .. _mju_normalize3: -mju_normalize3 -~~~~~~~~~~~~~~ +`mju_normalize3 <#mju_normalize3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_normalize3 @@ -3087,8 +3087,8 @@ Normalize vector, return length before normalization. .. _mju_norm3: -mju_norm3 -~~~~~~~~~ +`mju_norm3 <#mju_norm3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_norm3 @@ -3096,8 +3096,8 @@ Return vector length (without normalizing the vector). .. _mju_dot3: -mju_dot3 -~~~~~~~~ +`mju_dot3 <#mju_dot3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_dot3 @@ -3105,8 +3105,8 @@ Return dot-product of vec1 and vec2. .. _mju_dist3: -mju_dist3 -~~~~~~~~~ +`mju_dist3 <#mju_dist3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_dist3 @@ -3114,8 +3114,8 @@ Return Cartesian distance between 3D vectors pos1 and pos2. .. _mju_mulMatVec3: -mju_mulMatVec3 -~~~~~~~~~~~~~~ +`mju_mulMatVec3 <#mju_mulMatVec3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatVec3 @@ -3123,8 +3123,8 @@ Multiply 3-by-3 matrix by vector: res = mat * vec. .. _mju_mulMatTVec3: -mju_mulMatTVec3 -~~~~~~~~~~~~~~~ +`mju_mulMatTVec3 <#mju_mulMatTVec3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatTVec3 @@ -3132,8 +3132,8 @@ Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. .. _mju_cross: -mju_cross -~~~~~~~~~ +`mju_cross <#mju_cross>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cross @@ -3141,8 +3141,8 @@ Compute cross-product: res = cross(a, b). .. _mju_zero4: -mju_zero4 -~~~~~~~~~ +`mju_zero4 <#mju_zero4>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_zero4 @@ -3150,8 +3150,8 @@ Set res = 0. .. _mju_unit4: -mju_unit4 -~~~~~~~~~ +`mju_unit4 <#mju_unit4>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_unit4 @@ -3159,8 +3159,8 @@ Set res = (1,0,0,0). .. _mju_copy4: -mju_copy4 -~~~~~~~~~ +`mju_copy4 <#mju_copy4>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_copy4 @@ -3168,8 +3168,8 @@ Set res = vec. .. _mju_normalize4: -mju_normalize4 -~~~~~~~~~~~~~~ +`mju_normalize4 <#mju_normalize4>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_normalize4 @@ -3177,8 +3177,8 @@ Normalize vector, return length before normalization. .. _mju_zero: -mju_zero -~~~~~~~~ +`mju_zero <#mju_zero>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_zero @@ -3186,8 +3186,8 @@ Set res = 0. .. _mju_fill: -mju_fill -~~~~~~~~ +`mju_fill <#mju_fill>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_fill @@ -3195,8 +3195,8 @@ Set res = val. .. _mju_copy: -mju_copy -~~~~~~~~ +`mju_copy <#mju_copy>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_copy @@ -3204,8 +3204,8 @@ Set res = vec. .. _mju_sum: -mju_sum -~~~~~~~ +`mju_sum <#mju_sum>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sum @@ -3213,8 +3213,8 @@ Return sum(vec). .. _mju_L1: -mju_L1 -~~~~~~ +`mju_L1 <#mju_L1>`__ +~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_L1 @@ -3222,8 +3222,8 @@ Return L1 norm: sum(abs(vec)). .. _mju_scl: -mju_scl -~~~~~~~ +`mju_scl <#mju_scl>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_scl @@ -3231,8 +3231,8 @@ Set res = vec*scl. .. _mju_add: -mju_add -~~~~~~~ +`mju_add <#mju_add>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_add @@ -3240,8 +3240,8 @@ Set res = vec1 + vec2. .. _mju_sub: -mju_sub -~~~~~~~ +`mju_sub <#mju_sub>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sub @@ -3249,8 +3249,8 @@ Set res = vec1 - vec2. .. _mju_addTo: -mju_addTo -~~~~~~~~~ +`mju_addTo <#mju_addTo>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addTo @@ -3258,8 +3258,8 @@ Set res = res + vec. .. _mju_subFrom: -mju_subFrom -~~~~~~~~~~~ +`mju_subFrom <#mju_subFrom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_subFrom @@ -3267,8 +3267,8 @@ Set res = res - vec. .. _mju_addToScl: -mju_addToScl -~~~~~~~~~~~~ +`mju_addToScl <#mju_addToScl>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addToScl @@ -3276,8 +3276,8 @@ Set res = res + vec*scl. .. _mju_addScl: -mju_addScl -~~~~~~~~~~ +`mju_addScl <#mju_addScl>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_addScl @@ -3285,8 +3285,8 @@ Set res = vec1 + vec2*scl. .. _mju_normalize: -mju_normalize -~~~~~~~~~~~~~ +`mju_normalize <#mju_normalize>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_normalize @@ -3294,8 +3294,8 @@ Normalize vector, return length before normalization. .. _mju_norm: -mju_norm -~~~~~~~~ +`mju_norm <#mju_norm>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_norm @@ -3303,8 +3303,8 @@ Return vector length (without normalizing vector). .. _mju_dot: -mju_dot -~~~~~~~ +`mju_dot <#mju_dot>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_dot @@ -3312,8 +3312,8 @@ Return dot-product of vec1 and vec2. .. _mju_mulMatVec: -mju_mulMatVec -~~~~~~~~~~~~~ +`mju_mulMatVec <#mju_mulMatVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatVec @@ -3321,8 +3321,8 @@ Multiply matrix and vector: res = mat * vec. .. _mju_mulMatTVec: -mju_mulMatTVec -~~~~~~~~~~~~~~ +`mju_mulMatTVec <#mju_mulMatTVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatTVec @@ -3330,8 +3330,8 @@ Multiply transposed matrix and vector: res = mat' * vec. .. _mju_mulVecMatVec: -mju_mulVecMatVec -~~~~~~~~~~~~~~~~ +`mju_mulVecMatVec <#mju_mulVecMatVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulVecMatVec @@ -3339,8 +3339,8 @@ Multiply square matrix with vectors on both sides: returns vec1' * mat * vec2. .. _mju_transpose: -mju_transpose -~~~~~~~~~~~~~ +`mju_transpose <#mju_transpose>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_transpose @@ -3348,8 +3348,8 @@ Transpose matrix: res = mat'. .. _mju_symmetrize: -mju_symmetrize -~~~~~~~~~~~~~~ +`mju_symmetrize <#mju_symmetrize>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_symmetrize @@ -3357,8 +3357,8 @@ Symmetrize square matrix :math:`R = \frac{1}{2}(M + M^T)`. .. _mju_eye: -mju_eye -~~~~~~~ +`mju_eye <#mju_eye>`__ +~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_eye @@ -3366,8 +3366,8 @@ Set mat to the identity matrix. .. _mju_mulMatMat: -mju_mulMatMat -~~~~~~~~~~~~~ +`mju_mulMatMat <#mju_mulMatMat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatMat @@ -3375,8 +3375,8 @@ Multiply matrices: res = mat1 * mat2. .. _mju_mulMatMatT: -mju_mulMatMatT -~~~~~~~~~~~~~~ +`mju_mulMatMatT <#mju_mulMatMatT>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatMatT @@ -3384,8 +3384,8 @@ Multiply matrices, second argument transposed: res = mat1 * mat2'. .. _mju_mulMatTMat: -mju_mulMatTMat -~~~~~~~~~~~~~~ +`mju_mulMatTMat <#mju_mulMatTMat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulMatTMat @@ -3393,8 +3393,8 @@ Multiply matrices, first argument transposed: res = mat1' * mat2. .. _mju_sqrMatTD: -mju_sqrMatTD -~~~~~~~~~~~~ +`mju_sqrMatTD <#mju_sqrMatTD>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sqrMatTD @@ -3402,8 +3402,8 @@ Set res = mat' * diag * mat if diag is not NULL, and res = mat' * mat otherwise. .. _mju_transformSpatial: -mju_transformSpatial -~~~~~~~~~~~~~~~~~~~~ +`mju_transformSpatial <#mju_transformSpatial>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_transformSpatial @@ -3416,8 +3416,8 @@ Sparse math ^^^^^^^^^^^ .. _mju_dense2sparse: -mju_dense2sparse -~~~~~~~~~~~~~~~~ +`mju_dense2sparse <#mju_dense2sparse>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_dense2sparse @@ -3426,8 +3426,8 @@ Convert matrix from dense to sparse. .. _mju_sparse2dense: -mju_sparse2dense -~~~~~~~~~~~~~~~~ +`mju_sparse2dense <#mju_sparse2dense>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_sparse2dense @@ -3440,8 +3440,8 @@ Quaternions .. _mju_rotVecQuat: -mju_rotVecQuat -~~~~~~~~~~~~~~ +`mju_rotVecQuat <#mju_rotVecQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_rotVecQuat @@ -3449,8 +3449,8 @@ Rotate vector by quaternion. .. _mju_negQuat: -mju_negQuat -~~~~~~~~~~~ +`mju_negQuat <#mju_negQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_negQuat @@ -3458,8 +3458,8 @@ Conjugate quaternion, corresponding to opposite rotation. .. _mju_mulQuat: -mju_mulQuat -~~~~~~~~~~~ +`mju_mulQuat <#mju_mulQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulQuat @@ -3467,8 +3467,8 @@ Multiply quaternions. .. _mju_mulQuatAxis: -mju_mulQuatAxis -~~~~~~~~~~~~~~~ +`mju_mulQuatAxis <#mju_mulQuatAxis>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulQuatAxis @@ -3476,8 +3476,8 @@ Multiply quaternion and axis. .. _mju_axisAngle2Quat: -mju_axisAngle2Quat -~~~~~~~~~~~~~~~~~~ +`mju_axisAngle2Quat <#mju_axisAngle2Quat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_axisAngle2Quat @@ -3485,8 +3485,8 @@ Convert axisAngle to quaternion. .. _mju_quat2Vel: -mju_quat2Vel -~~~~~~~~~~~~ +`mju_quat2Vel <#mju_quat2Vel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_quat2Vel @@ -3494,8 +3494,8 @@ Convert quaternion (corresponding to orientation difference) to 3D velocity. .. _mju_subQuat: -mju_subQuat -~~~~~~~~~~~ +`mju_subQuat <#mju_subQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_subQuat @@ -3503,8 +3503,8 @@ Subtract quaternions, express as 3D velocity: qb*quat(res) = qa. .. _mju_quat2Mat: -mju_quat2Mat -~~~~~~~~~~~~ +`mju_quat2Mat <#mju_quat2Mat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_quat2Mat @@ -3512,8 +3512,8 @@ Convert quaternion to 3D rotation matrix. .. _mju_mat2Quat: -mju_mat2Quat -~~~~~~~~~~~~ +`mju_mat2Quat <#mju_mat2Quat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mat2Quat @@ -3521,8 +3521,8 @@ Convert 3D rotation matrix to quaternion. .. _mju_derivQuat: -mju_derivQuat -~~~~~~~~~~~~~ +`mju_derivQuat <#mju_derivQuat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_derivQuat @@ -3530,8 +3530,8 @@ Compute time-derivative of quaternion, given 3D rotational velocity. .. _mju_quatIntegrate: -mju_quatIntegrate -~~~~~~~~~~~~~~~~~ +`mju_quatIntegrate <#mju_quatIntegrate>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_quatIntegrate @@ -3539,8 +3539,8 @@ Integrate quaternion given 3D angular velocity. .. _mju_quatZ2Vec: -mju_quatZ2Vec -~~~~~~~~~~~~~ +`mju_quatZ2Vec <#mju_quatZ2Vec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_quatZ2Vec @@ -3548,8 +3548,8 @@ Construct quaternion performing rotation from z-axis to given vector. .. _mju_euler2Quat: -mju_euler2Quat -~~~~~~~~~~~~~~ +`mju_euler2Quat <#mju_euler2Quat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_euler2Quat @@ -3563,8 +3563,8 @@ Poses .. _mju_mulPose: -mju_mulPose -~~~~~~~~~~~ +`mju_mulPose <#mju_mulPose>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_mulPose @@ -3572,8 +3572,8 @@ Multiply two poses. .. _mju_negPose: -mju_negPose -~~~~~~~~~~~ +`mju_negPose <#mju_negPose>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_negPose @@ -3581,8 +3581,8 @@ Conjugate pose, corresponding to the opposite spatial transformation. .. _mju_trnVecPose: -mju_trnVecPose -~~~~~~~~~~~~~~ +`mju_trnVecPose <#mju_trnVecPose>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_trnVecPose @@ -3595,8 +3595,8 @@ Decompositions / Solvers .. _mju_cholFactor: -mju_cholFactor -~~~~~~~~~~~~~~ +`mju_cholFactor <#mju_cholFactor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cholFactor @@ -3604,8 +3604,8 @@ Cholesky decomposition: mat = L*L'; return rank, decomposition performed in-plac .. _mju_cholSolve: -mju_cholSolve -~~~~~~~~~~~~~ +`mju_cholSolve <#mju_cholSolve>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cholSolve @@ -3613,8 +3613,8 @@ Solve (mat*mat') * res = vec, where mat is a Cholesky factor. .. _mju_cholUpdate: -mju_cholUpdate -~~~~~~~~~~~~~~ +`mju_cholUpdate <#mju_cholUpdate>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cholUpdate @@ -3622,8 +3622,8 @@ Cholesky rank-one update: L*L' +/- x*x'; return rank. .. _mju_cholFactorBand: -mju_cholFactorBand -~~~~~~~~~~~~~~~~~~ +`mju_cholFactorBand <#mju_cholFactorBand>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cholFactorBand @@ -3672,8 +3672,8 @@ Band-dense Cholesky decomposition. .. _mju_cholSolveBand: -mju_cholSolveBand -~~~~~~~~~~~~~~~~~ +`mju_cholSolveBand <#mju_cholSolveBand>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_cholSolveBand @@ -3681,8 +3681,8 @@ Solve (mat*mat')*res = vec where mat is a band-dense Cholesky factor. .. _mju_band2Dense: -mju_band2Dense -~~~~~~~~~~~~~~ +`mju_band2Dense <#mju_band2Dense>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_band2Dense @@ -3690,8 +3690,8 @@ Convert banded matrix to dense matrix, fill upper triangle if flg_sym>0. .. _mju_dense2Band: -mju_dense2Band -~~~~~~~~~~~~~~ +`mju_dense2Band <#mju_dense2Band>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_dense2Band @@ -3699,8 +3699,8 @@ Convert dense matrix to banded matrix. .. _mju_bandMulMatVec: -mju_bandMulMatVec -~~~~~~~~~~~~~~~~~ +`mju_bandMulMatVec <#mju_bandMulMatVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_bandMulMatVec @@ -3708,8 +3708,8 @@ Multiply band-diagonal matrix with nvec vectors, include upper triangle if flg_s .. _mju_bandDiag: -mju_bandDiag -~~~~~~~~~~~~ +`mju_bandDiag <#mju_bandDiag>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_bandDiag @@ -3717,8 +3717,8 @@ Address of diagonal element i in band-dense matrix representation. .. _mju_eig3: -mju_eig3 -~~~~~~~~ +`mju_eig3 <#mju_eig3>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_eig3 @@ -3726,8 +3726,8 @@ Eigenvalue decomposition of symmetric 3x3 matrix, mat = eigvec * diag(eigval) * .. _mju_boxQP: -mju_boxQP -~~~~~~~~~ +`mju_boxQP <#mju_boxQP>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_boxQP @@ -3766,8 +3766,8 @@ notes: .. _mju_boxQPmalloc: -mju_boxQPmalloc -~~~~~~~~~~~~~~~ +`mju_boxQPmalloc <#mju_boxQPmalloc>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mju_boxQPmalloc @@ -3781,8 +3781,8 @@ Attachment ^^^^^^^^^^ .. _mjs_attachBody: -mjs_attachBody -~~~~~~~~~~~~~~ +`mjs_attachBody <#mjs_attachBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_attachBody @@ -3790,8 +3790,8 @@ Attach child body to a parent frame, return the attached body if success or NULL .. _mjs_attachFrame: -mjs_attachFrame -~~~~~~~~~~~~~~~ +`mjs_attachFrame <#mjs_attachFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_attachFrame @@ -3799,8 +3799,8 @@ Attach child frame to a parent body, return the attached frame if success or NUL .. _mjs_attachToSite: -mjs_attachToSite -~~~~~~~~~~~~~~~~ +`mjs_attachToSite <#mjs_attachToSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_attachToSite @@ -3808,8 +3808,8 @@ Attach child body to a parent site, return the attached body if success or NULL .. _mjs_detachBody: -mjs_detachBody -~~~~~~~~~~~~~~ +`mjs_detachBody <#mjs_detachBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_detachBody @@ -3821,8 +3821,8 @@ Tree elements ^^^^^^^^^^^^^ .. _mjs_addBody: -mjs_addBody -~~~~~~~~~~~ +`mjs_addBody <#mjs_addBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addBody @@ -3830,8 +3830,8 @@ Add child body to body, return child. .. _mjs_addSite: -mjs_addSite -~~~~~~~~~~~ +`mjs_addSite <#mjs_addSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addSite @@ -3839,8 +3839,8 @@ Add site to body, return site spec. .. _mjs_addJoint: -mjs_addJoint -~~~~~~~~~~~~ +`mjs_addJoint <#mjs_addJoint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addJoint @@ -3848,8 +3848,8 @@ Add joint to body. .. _mjs_addFreeJoint: -mjs_addFreeJoint -~~~~~~~~~~~~~~~~ +`mjs_addFreeJoint <#mjs_addFreeJoint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addFreeJoint @@ -3857,8 +3857,8 @@ Add freejoint to body. .. _mjs_addGeom: -mjs_addGeom -~~~~~~~~~~~ +`mjs_addGeom <#mjs_addGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addGeom @@ -3866,8 +3866,8 @@ Add geom to body. .. _mjs_addCamera: -mjs_addCamera -~~~~~~~~~~~~~ +`mjs_addCamera <#mjs_addCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addCamera @@ -3875,8 +3875,8 @@ Add camera to body. .. _mjs_addLight: -mjs_addLight -~~~~~~~~~~~~ +`mjs_addLight <#mjs_addLight>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addLight @@ -3884,8 +3884,8 @@ Add light to body. .. _mjs_addFrame: -mjs_addFrame -~~~~~~~~~~~~ +`mjs_addFrame <#mjs_addFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addFrame @@ -3893,8 +3893,8 @@ Add frame to body. .. _mjs_delete: -mjs_delete -~~~~~~~~~~ +`mjs_delete <#mjs_delete>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_delete @@ -3906,8 +3906,8 @@ Non-tree elements ^^^^^^^^^^^^^^^^^ .. _mjs_addActuator: -mjs_addActuator -~~~~~~~~~~~~~~~ +`mjs_addActuator <#mjs_addActuator>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addActuator @@ -3915,8 +3915,8 @@ Add actuator. .. _mjs_addSensor: -mjs_addSensor -~~~~~~~~~~~~~ +`mjs_addSensor <#mjs_addSensor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addSensor @@ -3924,8 +3924,8 @@ Add sensor. .. _mjs_addFlex: -mjs_addFlex -~~~~~~~~~~~ +`mjs_addFlex <#mjs_addFlex>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addFlex @@ -3933,8 +3933,8 @@ Add flex. .. _mjs_addPair: -mjs_addPair -~~~~~~~~~~~ +`mjs_addPair <#mjs_addPair>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addPair @@ -3942,8 +3942,8 @@ Add contact pair. .. _mjs_addExclude: -mjs_addExclude -~~~~~~~~~~~~~~ +`mjs_addExclude <#mjs_addExclude>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addExclude @@ -3951,8 +3951,8 @@ Add excluded body pair. .. _mjs_addEquality: -mjs_addEquality -~~~~~~~~~~~~~~~ +`mjs_addEquality <#mjs_addEquality>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addEquality @@ -3960,8 +3960,8 @@ Add equality. .. _mjs_addTendon: -mjs_addTendon -~~~~~~~~~~~~~ +`mjs_addTendon <#mjs_addTendon>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addTendon @@ -3969,8 +3969,8 @@ Add tendon. .. _mjs_wrapSite: -mjs_wrapSite -~~~~~~~~~~~~ +`mjs_wrapSite <#mjs_wrapSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_wrapSite @@ -3978,8 +3978,8 @@ Wrap site using tendon. .. _mjs_wrapGeom: -mjs_wrapGeom -~~~~~~~~~~~~ +`mjs_wrapGeom <#mjs_wrapGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_wrapGeom @@ -3987,8 +3987,8 @@ Wrap geom using tendon. .. _mjs_wrapJoint: -mjs_wrapJoint -~~~~~~~~~~~~~ +`mjs_wrapJoint <#mjs_wrapJoint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_wrapJoint @@ -3996,8 +3996,8 @@ Wrap joint using tendon. .. _mjs_wrapPulley: -mjs_wrapPulley -~~~~~~~~~~~~~~ +`mjs_wrapPulley <#mjs_wrapPulley>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_wrapPulley @@ -4005,8 +4005,8 @@ Wrap pulley using tendon. .. _mjs_addNumeric: -mjs_addNumeric -~~~~~~~~~~~~~~ +`mjs_addNumeric <#mjs_addNumeric>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addNumeric @@ -4014,8 +4014,8 @@ Add numeric. .. _mjs_addText: -mjs_addText -~~~~~~~~~~~ +`mjs_addText <#mjs_addText>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addText @@ -4023,8 +4023,8 @@ Add text. .. _mjs_addTuple: -mjs_addTuple -~~~~~~~~~~~~ +`mjs_addTuple <#mjs_addTuple>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addTuple @@ -4032,8 +4032,8 @@ Add tuple. .. _mjs_addKey: -mjs_addKey -~~~~~~~~~~ +`mjs_addKey <#mjs_addKey>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addKey @@ -4041,8 +4041,8 @@ Add keyframe. .. _mjs_addPlugin: -mjs_addPlugin -~~~~~~~~~~~~~ +`mjs_addPlugin <#mjs_addPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addPlugin @@ -4050,8 +4050,8 @@ Add plugin. .. _mjs_addDefault: -mjs_addDefault -~~~~~~~~~~~~~~ +`mjs_addDefault <#mjs_addDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addDefault @@ -4063,8 +4063,8 @@ Assets ^^^^^^ .. _mjs_addMesh: -mjs_addMesh -~~~~~~~~~~~ +`mjs_addMesh <#mjs_addMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addMesh @@ -4072,8 +4072,8 @@ Add mesh. .. _mjs_addHField: -mjs_addHField -~~~~~~~~~~~~~ +`mjs_addHField <#mjs_addHField>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addHField @@ -4081,8 +4081,8 @@ Add height field. .. _mjs_addSkin: -mjs_addSkin -~~~~~~~~~~~ +`mjs_addSkin <#mjs_addSkin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addSkin @@ -4090,8 +4090,8 @@ Add skin. .. _mjs_addTexture: -mjs_addTexture -~~~~~~~~~~~~~~ +`mjs_addTexture <#mjs_addTexture>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addTexture @@ -4099,8 +4099,8 @@ Add texture. .. _mjs_addMaterial: -mjs_addMaterial -~~~~~~~~~~~~~~~ +`mjs_addMaterial <#mjs_addMaterial>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_addMaterial @@ -4112,8 +4112,8 @@ Find and get utilities ^^^^^^^^^^^^^^^^^^^^^^ .. _mjs_getSpec: -mjs_getSpec -~~~~~~~~~~~ +`mjs_getSpec <#mjs_getSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getSpec @@ -4121,8 +4121,8 @@ Get spec from body. .. _mjs_findSpec: -mjs_findSpec -~~~~~~~~~~~~ +`mjs_findSpec <#mjs_findSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findSpec @@ -4130,8 +4130,8 @@ Find spec (model asset) by name. .. _mjs_findBody: -mjs_findBody -~~~~~~~~~~~~ +`mjs_findBody <#mjs_findBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findBody @@ -4139,8 +4139,8 @@ Find body in spec by name. .. _mjs_findElement: -mjs_findElement -~~~~~~~~~~~~~~~ +`mjs_findElement <#mjs_findElement>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findElement @@ -4148,8 +4148,8 @@ Find element in spec by name. .. _mjs_findChild: -mjs_findChild -~~~~~~~~~~~~~ +`mjs_findChild <#mjs_findChild>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findChild @@ -4157,8 +4157,8 @@ Find child body by name. .. _mjs_findFrame: -mjs_findFrame -~~~~~~~~~~~~~ +`mjs_findFrame <#mjs_findFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findFrame @@ -4166,8 +4166,8 @@ Find frame by name. .. _mjs_getDefault: -mjs_getDefault -~~~~~~~~~~~~~~ +`mjs_getDefault <#mjs_getDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getDefault @@ -4175,8 +4175,8 @@ Get default corresponding to an element. .. _mjs_findDefault: -mjs_findDefault -~~~~~~~~~~~~~~~ +`mjs_findDefault <#mjs_findDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_findDefault @@ -4184,8 +4184,8 @@ Find default in model by class name. .. _mjs_getSpecDefault: -mjs_getSpecDefault -~~~~~~~~~~~~~~~~~~ +`mjs_getSpecDefault <#mjs_getSpecDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getSpecDefault @@ -4193,8 +4193,8 @@ Get global default from model. .. _mjs_getId: -mjs_getId -~~~~~~~~~ +`mjs_getId <#mjs_getId>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getId @@ -4202,8 +4202,8 @@ Get element id. .. _mjs_firstChild: -mjs_firstChild -~~~~~~~~~~~~~~ +`mjs_firstChild <#mjs_firstChild>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_firstChild @@ -4211,8 +4211,8 @@ Return body's first child of given type. If recurse is nonzero, also search the .. _mjs_nextChild: -mjs_nextChild -~~~~~~~~~~~~~ +`mjs_nextChild <#mjs_nextChild>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_nextChild @@ -4221,8 +4221,8 @@ If recurse is nonzero, also search the body's subtree. .. _mjs_firstElement: -mjs_firstElement -~~~~~~~~~~~~~~~~ +`mjs_firstElement <#mjs_firstElement>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_firstElement @@ -4230,8 +4230,8 @@ Return spec's first element of selected type. .. _mjs_nextElement: -mjs_nextElement -~~~~~~~~~~~~~~~ +`mjs_nextElement <#mjs_nextElement>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_nextElement @@ -4243,8 +4243,8 @@ Attribute setters ^^^^^^^^^^^^^^^^^ .. _mjs_setBuffer: -mjs_setBuffer -~~~~~~~~~~~~~ +`mjs_setBuffer <#mjs_setBuffer>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setBuffer @@ -4252,8 +4252,8 @@ Copy buffer. .. _mjs_setString: -mjs_setString -~~~~~~~~~~~~~ +`mjs_setString <#mjs_setString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setString @@ -4261,8 +4261,8 @@ Copy text to string. .. _mjs_setStringVec: -mjs_setStringVec -~~~~~~~~~~~~~~~~ +`mjs_setStringVec <#mjs_setStringVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setStringVec @@ -4270,8 +4270,8 @@ Split text to entries and copy to string vector. .. _mjs_setInStringVec: -mjs_setInStringVec -~~~~~~~~~~~~~~~~~~ +`mjs_setInStringVec <#mjs_setInStringVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setInStringVec @@ -4279,8 +4279,8 @@ Set entry in string vector. .. _mjs_appendString: -mjs_appendString -~~~~~~~~~~~~~~~~ +`mjs_appendString <#mjs_appendString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_appendString @@ -4288,8 +4288,8 @@ Append text entry to string vector. .. _mjs_setInt: -mjs_setInt -~~~~~~~~~~ +`mjs_setInt <#mjs_setInt>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setInt @@ -4297,8 +4297,8 @@ Copy int array to vector. .. _mjs_appendIntVec: -mjs_appendIntVec -~~~~~~~~~~~~~~~~ +`mjs_appendIntVec <#mjs_appendIntVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_appendIntVec @@ -4306,8 +4306,8 @@ Append int array to vector of arrays. .. _mjs_setFloat: -mjs_setFloat -~~~~~~~~~~~~ +`mjs_setFloat <#mjs_setFloat>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setFloat @@ -4315,8 +4315,8 @@ Copy float array to vector. .. _mjs_appendFloatVec: -mjs_appendFloatVec -~~~~~~~~~~~~~~~~~~ +`mjs_appendFloatVec <#mjs_appendFloatVec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_appendFloatVec @@ -4324,8 +4324,8 @@ Append float array to vector of arrays. .. _mjs_setDouble: -mjs_setDouble -~~~~~~~~~~~~~ +`mjs_setDouble <#mjs_setDouble>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setDouble @@ -4333,8 +4333,8 @@ Copy double array to vector. .. _mjs_setPluginAttributes: -mjs_setPluginAttributes -~~~~~~~~~~~~~~~~~~~~~~~ +`mjs_setPluginAttributes <#mjs_setPluginAttributes>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setPluginAttributes @@ -4346,8 +4346,8 @@ Attribute getters ^^^^^^^^^^^^^^^^^ .. _mjs_getString: -mjs_getString -~~~~~~~~~~~~~ +`mjs_getString <#mjs_getString>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getString @@ -4355,8 +4355,8 @@ Get string contents. .. _mjs_getDouble: -mjs_getDouble -~~~~~~~~~~~~~ +`mjs_getDouble <#mjs_getDouble>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_getDouble @@ -4368,8 +4368,8 @@ Spec utilities ^^^^^^^^^^^^^^ .. _mjs_setDefault: -mjs_setDefault -~~~~~~~~~~~~~~ +`mjs_setDefault <#mjs_setDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setDefault @@ -4377,8 +4377,8 @@ Set element's default. .. _mjs_setFrame: -mjs_setFrame -~~~~~~~~~~~~ +`mjs_setFrame <#mjs_setFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_setFrame @@ -4386,8 +4386,8 @@ Set element's enclosing frame. .. _mjs_resolveOrientation: -mjs_resolveOrientation -~~~~~~~~~~~~~~~~~~~~~~ +`mjs_resolveOrientation <#mjs_resolveOrientation>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_resolveOrientation @@ -4395,8 +4395,8 @@ Resolve alternative orientations to quat, return error if any. .. _mjs_bodyToFrame: -mjs_bodyToFrame -~~~~~~~~~~~~~~~ +`mjs_bodyToFrame <#mjs_bodyToFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_bodyToFrame @@ -4408,8 +4408,8 @@ Element initialization ^^^^^^^^^^^^^^^^^^^^^^ .. _mjs_defaultSpec: -mjs_defaultSpec -~~~~~~~~~~~~~~~ +`mjs_defaultSpec <#mjs_defaultSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultSpec @@ -4417,8 +4417,8 @@ Default spec attributes. .. _mjs_defaultOrientation: -mjs_defaultOrientation -~~~~~~~~~~~~~~~~~~~~~~ +`mjs_defaultOrientation <#mjs_defaultOrientation>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultOrientation @@ -4426,8 +4426,8 @@ Default orientation attributes. .. _mjs_defaultBody: -mjs_defaultBody -~~~~~~~~~~~~~~~ +`mjs_defaultBody <#mjs_defaultBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultBody @@ -4435,8 +4435,8 @@ Default body attributes. .. _mjs_defaultFrame: -mjs_defaultFrame -~~~~~~~~~~~~~~~~ +`mjs_defaultFrame <#mjs_defaultFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultFrame @@ -4444,8 +4444,8 @@ Default frame attributes. .. _mjs_defaultJoint: -mjs_defaultJoint -~~~~~~~~~~~~~~~~ +`mjs_defaultJoint <#mjs_defaultJoint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultJoint @@ -4453,8 +4453,8 @@ Default joint attributes. .. _mjs_defaultGeom: -mjs_defaultGeom -~~~~~~~~~~~~~~~ +`mjs_defaultGeom <#mjs_defaultGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultGeom @@ -4462,8 +4462,8 @@ Default geom attributes. .. _mjs_defaultSite: -mjs_defaultSite -~~~~~~~~~~~~~~~ +`mjs_defaultSite <#mjs_defaultSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultSite @@ -4471,8 +4471,8 @@ Default site attributes. .. _mjs_defaultCamera: -mjs_defaultCamera -~~~~~~~~~~~~~~~~~ +`mjs_defaultCamera <#mjs_defaultCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultCamera @@ -4480,8 +4480,8 @@ Default camera attributes. .. _mjs_defaultLight: -mjs_defaultLight -~~~~~~~~~~~~~~~~ +`mjs_defaultLight <#mjs_defaultLight>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultLight @@ -4489,8 +4489,8 @@ Default light attributes. .. _mjs_defaultFlex: -mjs_defaultFlex -~~~~~~~~~~~~~~~ +`mjs_defaultFlex <#mjs_defaultFlex>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultFlex @@ -4498,8 +4498,8 @@ Default flex attributes. .. _mjs_defaultMesh: -mjs_defaultMesh -~~~~~~~~~~~~~~~ +`mjs_defaultMesh <#mjs_defaultMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultMesh @@ -4507,8 +4507,8 @@ Default mesh attributes. .. _mjs_defaultHField: -mjs_defaultHField -~~~~~~~~~~~~~~~~~ +`mjs_defaultHField <#mjs_defaultHField>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultHField @@ -4516,8 +4516,8 @@ Default height field attributes. .. _mjs_defaultSkin: -mjs_defaultSkin -~~~~~~~~~~~~~~~ +`mjs_defaultSkin <#mjs_defaultSkin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultSkin @@ -4525,8 +4525,8 @@ Default skin attributes. .. _mjs_defaultTexture: -mjs_defaultTexture -~~~~~~~~~~~~~~~~~~ +`mjs_defaultTexture <#mjs_defaultTexture>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultTexture @@ -4534,8 +4534,8 @@ Default texture attributes. .. _mjs_defaultMaterial: -mjs_defaultMaterial -~~~~~~~~~~~~~~~~~~~ +`mjs_defaultMaterial <#mjs_defaultMaterial>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultMaterial @@ -4543,8 +4543,8 @@ Default material attributes. .. _mjs_defaultPair: -mjs_defaultPair -~~~~~~~~~~~~~~~ +`mjs_defaultPair <#mjs_defaultPair>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultPair @@ -4552,8 +4552,8 @@ Default pair attributes. .. _mjs_defaultEquality: -mjs_defaultEquality -~~~~~~~~~~~~~~~~~~~ +`mjs_defaultEquality <#mjs_defaultEquality>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultEquality @@ -4561,8 +4561,8 @@ Default equality attributes. .. _mjs_defaultTendon: -mjs_defaultTendon -~~~~~~~~~~~~~~~~~ +`mjs_defaultTendon <#mjs_defaultTendon>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultTendon @@ -4570,8 +4570,8 @@ Default tendon attributes. .. _mjs_defaultActuator: -mjs_defaultActuator -~~~~~~~~~~~~~~~~~~~ +`mjs_defaultActuator <#mjs_defaultActuator>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultActuator @@ -4579,8 +4579,8 @@ Default actuator attributes. .. _mjs_defaultSensor: -mjs_defaultSensor -~~~~~~~~~~~~~~~~~ +`mjs_defaultSensor <#mjs_defaultSensor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultSensor @@ -4588,8 +4588,8 @@ Default sensor attributes. .. _mjs_defaultNumeric: -mjs_defaultNumeric -~~~~~~~~~~~~~~~~~~ +`mjs_defaultNumeric <#mjs_defaultNumeric>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultNumeric @@ -4597,8 +4597,8 @@ Default numeric attributes. .. _mjs_defaultText: -mjs_defaultText -~~~~~~~~~~~~~~~ +`mjs_defaultText <#mjs_defaultText>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultText @@ -4606,8 +4606,8 @@ Default text attributes. .. _mjs_defaultTuple: -mjs_defaultTuple -~~~~~~~~~~~~~~~~ +`mjs_defaultTuple <#mjs_defaultTuple>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultTuple @@ -4615,8 +4615,8 @@ Default tuple attributes. .. _mjs_defaultKey: -mjs_defaultKey -~~~~~~~~~~~~~~ +`mjs_defaultKey <#mjs_defaultKey>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultKey @@ -4624,8 +4624,8 @@ Default keyframe attributes. .. _mjs_defaultPlugin: -mjs_defaultPlugin -~~~~~~~~~~~~~~~~~ +`mjs_defaultPlugin <#mjs_defaultPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_defaultPlugin @@ -4637,8 +4637,8 @@ Element casting ^^^^^^^^^^^^^^^ .. _mjs_asBody: -mjs_asBody -~~~~~~~~~~ +`mjs_asBody <#mjs_asBody>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asBody @@ -4646,8 +4646,8 @@ Safely cast an element as mjsBody, or return NULL if the element is not an mjsBo .. _mjs_asGeom: -mjs_asGeom -~~~~~~~~~~ +`mjs_asGeom <#mjs_asGeom>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asGeom @@ -4655,8 +4655,8 @@ Safely cast an element as mjsGeom, or return NULL if the element is not an mjsGe .. _mjs_asJoint: -mjs_asJoint -~~~~~~~~~~~ +`mjs_asJoint <#mjs_asJoint>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asJoint @@ -4664,8 +4664,8 @@ Safely cast an element as mjsJoint, or return NULL if the element is not an mjsJ .. _mjs_asSite: -mjs_asSite -~~~~~~~~~~ +`mjs_asSite <#mjs_asSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asSite @@ -4673,8 +4673,8 @@ Safely cast an element as mjsSite, or return NULL if the element is not an mjsSi .. _mjs_asCamera: -mjs_asCamera -~~~~~~~~~~~~ +`mjs_asCamera <#mjs_asCamera>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asCamera @@ -4682,8 +4682,8 @@ Safely cast an element as mjsCamera, or return NULL if the element is not an mjs .. _mjs_asLight: -mjs_asLight -~~~~~~~~~~~ +`mjs_asLight <#mjs_asLight>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asLight @@ -4691,8 +4691,8 @@ Safely cast an element as mjsLight, or return NULL if the element is not an mjsL .. _mjs_asFrame: -mjs_asFrame -~~~~~~~~~~~ +`mjs_asFrame <#mjs_asFrame>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asFrame @@ -4700,8 +4700,8 @@ Safely cast an element as mjsFrame, or return NULL if the element is not an mjsF .. _mjs_asActuator: -mjs_asActuator -~~~~~~~~~~~~~~ +`mjs_asActuator <#mjs_asActuator>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asActuator @@ -4709,8 +4709,8 @@ Safely cast an element as mjsActuator, or return NULL if the element is not an m .. _mjs_asSensor: -mjs_asSensor -~~~~~~~~~~~~ +`mjs_asSensor <#mjs_asSensor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asSensor @@ -4718,8 +4718,8 @@ Safely cast an element as mjsSensor, or return NULL if the element is not an mjs .. _mjs_asFlex: -mjs_asFlex -~~~~~~~~~~ +`mjs_asFlex <#mjs_asFlex>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asFlex @@ -4727,8 +4727,8 @@ Safely cast an element as mjsFlex, or return NULL if the element is not an mjsFl .. _mjs_asPair: -mjs_asPair -~~~~~~~~~~ +`mjs_asPair <#mjs_asPair>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asPair @@ -4736,8 +4736,8 @@ Safely cast an element as mjsPair, or return NULL if the element is not an mjsPa .. _mjs_asEquality: -mjs_asEquality -~~~~~~~~~~~~~~ +`mjs_asEquality <#mjs_asEquality>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asEquality @@ -4745,8 +4745,8 @@ Safely cast an element as mjsEquality, or return NULL if the element is not an m .. _mjs_asExclude: -mjs_asExclude -~~~~~~~~~~~~~ +`mjs_asExclude <#mjs_asExclude>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asExclude @@ -4754,8 +4754,8 @@ Safely cast an element as mjsExclude, or return NULL if the element is not an mj .. _mjs_asTendon: -mjs_asTendon -~~~~~~~~~~~~ +`mjs_asTendon <#mjs_asTendon>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asTendon @@ -4763,8 +4763,8 @@ Safely cast an element as mjsTendon, or return NULL if the element is not an mjs .. _mjs_asNumeric: -mjs_asNumeric -~~~~~~~~~~~~~ +`mjs_asNumeric <#mjs_asNumeric>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asNumeric @@ -4772,8 +4772,8 @@ Safely cast an element as mjsNumeric, or return NULL if the element is not an mj .. _mjs_asText: -mjs_asText -~~~~~~~~~~ +`mjs_asText <#mjs_asText>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asText @@ -4781,8 +4781,8 @@ Safely cast an element as mjsText, or return NULL if the element is not an mjsTe .. _mjs_asTuple: -mjs_asTuple -~~~~~~~~~~~ +`mjs_asTuple <#mjs_asTuple>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asTuple @@ -4790,8 +4790,8 @@ Safely cast an element as mjsTuple, or return NULL if the element is not an mjsT .. _mjs_asKey: -mjs_asKey -~~~~~~~~~ +`mjs_asKey <#mjs_asKey>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asKey @@ -4799,8 +4799,8 @@ Safely cast an element as mjsKey, or return NULL if the element is not an mjsKey .. _mjs_asMesh: -mjs_asMesh -~~~~~~~~~~ +`mjs_asMesh <#mjs_asMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asMesh @@ -4808,8 +4808,8 @@ Safely cast an element as mjsMesh, or return NULL if the element is not an mjsMe .. _mjs_asHField: -mjs_asHField -~~~~~~~~~~~~ +`mjs_asHField <#mjs_asHField>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asHField @@ -4817,8 +4817,8 @@ Safely cast an element as mjsHField, or return NULL if the element is not an mjs .. _mjs_asSkin: -mjs_asSkin -~~~~~~~~~~ +`mjs_asSkin <#mjs_asSkin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asSkin @@ -4826,8 +4826,8 @@ Safely cast an element as mjsSkin, or return NULL if the element is not an mjsSk .. _mjs_asTexture: -mjs_asTexture -~~~~~~~~~~~~~ +`mjs_asTexture <#mjs_asTexture>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asTexture @@ -4835,8 +4835,8 @@ Safely cast an element as mjsTexture, or return NULL if the element is not an mj .. _mjs_asMaterial: -mjs_asMaterial -~~~~~~~~~~~~~~ +`mjs_asMaterial <#mjs_asMaterial>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asMaterial @@ -4844,8 +4844,8 @@ Safely cast an element as mjsMaterial, or return NULL if the element is not an m .. _mjs_asPlugin: -mjs_asPlugin -~~~~~~~~~~~~ +`mjs_asPlugin <#mjs_asPlugin>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. mujoco-include:: mjs_asPlugin diff --git a/doc/changelog.rst b/doc/changelog.rst index ba4cae6d..cdc66f8d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -31,6 +31,12 @@ Bug fixes runtime enabling/disabling of such constraints. - Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. + +Documentation +^^^^^^^^^^^^^ +- Function headers in the :doc:`API reference <../APIreference/APIfunctions>` now link to their source definitions + in GitHub. + Version 3.2.4 (Oct 15, 2024) ---------------------------- diff --git a/doc/conf.py b/doc/conf.py index e41542fa..cecc6c96 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -159,10 +159,14 @@ pygments_dark_style = 'monokai' html_static_path = [ '_static', 'css', + 'js', ] html_css_files = [ 'theme_overrides.css', ] +html_js_files = [ + 'linenumbers.js', +] favicons = [ { diff --git a/doc/js/linenumbers.js b/doc/js/linenumbers.js new file mode 100644 index 00000000..cdea9af8 --- /dev/null +++ b/doc/js/linenumbers.js @@ -0,0 +1,108 @@ +const SRCS = [ + 'xml/xml_api.cc', + 'user/user_api.cc', + 'user/user_init.c', + 'user/user_vfs.cc', + 'thread/thread_task.cc', + 'thread/thread_pool.cc', + 'render/render_context.c', + 'render/render_gl2.c', + 'render/render_gl3.c', + 'render/render_util.c', + 'engine/engine_derivative.c', + 'engine/engine_io.c', + 'engine/engine_sensor.c', + 'engine/engine_callback.c', + 'engine/engine_collision_driver.c', + 'engine/engine_core_constraint.c', + 'engine/engine_core_smooth.c', + 'engine/engine_derivative_fd.c', + 'engine/engine_forward.c', + 'engine/engine_inverse.c', + 'engine/engine_island.c', + 'engine/engine_name.c', + 'engine/engine_passive.c', + 'engine/engine_plugin.cc', + 'engine/engine_print.c', + 'engine/engine_ray.c', + 'engine/engine_setconst.c', + 'engine/engine_solver.c', + 'engine/engine_support.c', + 'engine/engine_util_blas.c', + 'engine/engine_util_container.c', + 'engine/engine_util_errmem.c', + 'engine/engine_util_misc.c', + 'engine/engine_util_solve.c', + 'engine/engine_util_spatial.c', + 'engine/engine_util_sparse.c', + 'engine/engine_vis_init.c', + 'engine/engine_vis_interact.c', + 'engine/engine_vis_state.c', + 'engine/engine_vis_visualize.c', + 'ui/ui_main.c', +]; + +class LineNumbers { + constructor() { + this.map = new Map(); + } + + static fetch(src) { + const url = `https://raw.githubusercontent.com/google-deepmind/mujoco/refs/heads/main/src/${src}`; + const request = new XMLHttpRequest(); + return new Promise((resolve, reject) => { + request.onreadystatechange = () => { + if (request.readyState === 4) { + if (request.status === 200) { + resolve(request.responseText); + } else { + reject(request.status); + } + } + }; + request.open('GET', url, true); + request.send(); + }); + } + + fetchAll() { + let requests = []; + for (const src of SRCS) { + requests.push(LineNumbers.fetch(src).then(contents => { + this.processSrc(src, contents); + }, reason => {/* swallow error */})); + } + Promise.all(requests).then(() => { + const anchors = document.querySelectorAll('h3 a.reference.external'); + for (const anchor of anchors) { + const url = anchor.getAttribute('href'); + if (url.startsWith('#')) { + const key = url.substring(1); + if (this.map.has(key)) { + anchor.href = this.map.get(key); + } else { + console.log(`No line number found for ${key}`); + } + } + } + }); + } + + processSrc(src, contents) { + const lines = contents.split('\n'); + const re = /^(const )?[a-zA-Z0-9_*]+\s(.+)\(.+[{,]$/; + for (let i = 0; i < lines.length; i++) { + if (lines[i].match(re)) { + const key = lines[i].match(re)[2]; + this.map.set(key, `https://github.com/google-deepmind/mujoco/blob/main/src/${src}#L${i+1}`); + } + } + } +} + +window.onload = () => { + if (document.getElementById('fetchlines')) { + let lines = new LineNumbers(); + lines.fetchAll(); + } +}; From a51f346059bbefad31b6c00572a4f137e053a6cd Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 1 Nov 2024 08:05:14 -0700 Subject: [PATCH 049/426] Use sparse (uncompressed) actuator_moment in mj_transmission. PiperOrigin-RevId: 692179704 Change-Id: Ic30ac5a98dc13de2028e378df65dc88ba3912bf5 --- mjx/mujoco/mjx/_src/io.py | 47 ++++++- mjx/mujoco/mjx/_src/smooth_test.py | 24 +++- mjx/mujoco/mjx/_src/types.py | 6 + .../mjx/integration_test/smooth_test.py | 25 +++- python/LQR.ipynb | 10 +- src/engine/engine_core_smooth.c | 128 +++++++++++------- src/engine/engine_derivative.c | 11 +- src/engine/engine_forward.c | 12 +- src/engine/engine_io.c | 3 - src/engine/engine_print.c | 9 +- src/engine/engine_setconst.c | 28 +++- src/engine/engine_util_sparse.h | 4 +- test/engine/engine_derivative_test.cc | 4 +- 13 files changed, 230 insertions(+), 81 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 00ba75e5..28b84bbe 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -274,6 +274,9 @@ def make_data( 'wrap_obj': (m.nwrap, 2, jp.int32), 'wrap_xpos': (m.nwrap, 6, float), 'actuator_length': (m.nu, float), + 'moment_rownnz': (m.nu, jp.int32), + 'moment_rowadr': (m.nu, jp.int32), + 'moment_colind': (m.nu, m.nv, jp.int32), 'actuator_moment': (m.nu, m.nv, float), 'crb': (m.nbody, 10, float), 'qM': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), @@ -427,6 +430,25 @@ def get_data_into( result_i.contact.efc_address[:] = efc_map[result_i.contact.efc_address] continue + # MuJoCo actuator_moment is sparse, MJX uses a dense representation. + if field.name == 'actuator_moment' and m.nu: + moment_rownnz = np.zeros(m.nu, dtype=int) + moment_rowadr = np.zeros(m.nu, dtype=int) + moment_colind = np.zeros(m.nu * m.nv, dtype=int) + actuator_moment = np.zeros(m.nu * m.nv) + mujoco.mju_dense2sparse( + actuator_moment, + d.actuator_moment, + moment_rownnz, + moment_rowadr, + moment_colind, + ) + result_i.moment_rownnz[:] = moment_rownnz + result_i.moment_rowadr[:] = moment_rowadr + result_i.moment_colind[:] = moment_colind.reshape((m.nu, m.nv)) + result_i.actuator_moment[:] = actuator_moment.reshape((m.nu, m.nv)) + continue + value = getattr(d_i, field.name) if field.name in ('nefc', 'ncon'): @@ -532,6 +554,17 @@ def put_data( # MJX does not support islanding, so only transfer the first solver_niter fields['solver_niter'] = fields['solver_niter'][0] + # convert sparse representation of actuator_moment to dense matrix + moment = np.zeros((m.nu, m.nv)) + mujoco.mju_sparse2dense( + moment, + d.actuator_moment.reshape(-1), + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind.reshape(-1), + ) + fields['actuator_moment'] = moment + contact, contact_map = _make_contact(d.contact, dim, efc_address) # pad efc fields: MuJoCo efc arrays are sparse for inactive constraints. @@ -539,12 +572,14 @@ def put_data( # neither: it contains zeros for inactive constraints, and efc_J is always # (nefc, nv). this may change in the future. if mujoco.mj_isSparse(m): - nr = d.efc_J_rownnz.shape[0] - efc_j = np.zeros((nr, m.nv)) - for i in range(nr): - rowadr = d.efc_J_rowadr[i] - for j in range(d.efc_J_rownnz[i]): - efc_j[i, d.efc_J_colind[rowadr + j]] = fields['efc_J'][rowadr + j] + efc_j = np.zeros((d.efc_J_rownnz.shape[0], m.nv)) + mujoco.mju_sparse2dense( + efc_j, + fields['efc_J'], + d.efc_J_rownnz, + d.efc_J_rowadr, + d.efc_J_colind, + ) fields['efc_J'] = efc_j else: fields['efc_J'] = fields['efc_J'].reshape((-1 if m.nv else 0, m.nv)) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index fc14ac65..39339297 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -117,7 +117,17 @@ class SmoothTest(absltest.TestCase): # transmission dx = jax.jit(mjx.transmission)(mx, dx) _assert_attr_eq(d, dx, 'actuator_length') - _assert_attr_eq(d, dx, 'actuator_moment') + + # convert sparse actuator_moment to dense representation + moment = np.zeros((m.nu, m.nv)) + mujoco.mju_sparse2dense( + moment, + d.actuator_moment.reshape(-1), + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind.reshape(-1), + ) + _assert_eq(moment, dx.actuator_moment, 'actuator_moment') def test_disable_gravity(self): m = mujoco.MjModel.from_xml_string(""" @@ -178,7 +188,17 @@ class SmoothTest(absltest.TestCase): mujoco.mj_transmission(m, d) dx = jax.jit(mjx.transmission)(mx, dx) _assert_attr_eq(d, dx, 'actuator_length') - _assert_attr_eq(d, dx, 'actuator_moment') + + # convert sparse actuator_moment to dense representation + moment = np.zeros((m.nu, m.nv)) + mujoco.mju_sparse2dense( + moment, + d.actuator_moment.reshape(-1), + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind.reshape(-1), + ) + _assert_eq(moment, dx.actuator_moment, 'actuator_moment') def test_subtree_vel(self): """Tests MJX subtree_vel function matches MuJoCo mj_subtreeVel.""" diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index c36fa66c..e1df9681 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -1228,6 +1228,9 @@ class Data(PyTreeNode): wrap_obj: geom id; -1: site; -2: pulley (nwrap*2,) wrap_xpos: Cartesian 3D points in all path (nwrap*2, 3) actuator_length: actuator lengths (nu,) + moment_rownnz: number of non-zeros in actuator_moment row (nu,) + moment_rowadr: row start address in colind array (nu,) + moment_colind: column indices in sparse Jacobian (nu, nv) actuator_moment: actuator moments (nu, nv) crb: com-based composite inertia and mass (nbody, 10) qM: total inertia if sparse: (nM,) @@ -1350,6 +1353,9 @@ class Data(PyTreeNode): wrap_obj: jax.Array wrap_xpos: jax.Array actuator_length: jax.Array + moment_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + moment_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + moment_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name actuator_moment: jax.Array crb: jax.Array qM: jax.Array # pylint:disable=invalid-name diff --git a/mjx/mujoco/mjx/integration_test/smooth_test.py b/mjx/mujoco/mjx/integration_test/smooth_test.py index 5359245e..8d6b2137 100644 --- a/mjx/mujoco/mjx/integration_test/smooth_test.py +++ b/mjx/mujoco/mjx/integration_test/smooth_test.py @@ -68,10 +68,27 @@ class TransmissionIntegrationTest(parameterized.TestCase): mujoco.mj_transmission(m, d) dx = transmission_jit_fn(mx, dx) - for field in ['actuator_length', 'actuator_moment']: - _assert_attr_eq( - d, dx, field, seed, f'transmission{seed}', atol=1e-4 - ) + _assert_attr_eq( + d, dx, 'actuator_length', seed, f'transmission{seed}', atol=1e-4 + ) + + # convert sparse actuator_moment to dense representation + moment = np.zeros((m.nu, m.nv)) + mujoco.mju_sparse2dense( + moment, + d.actuator_moment.reshape(-1), + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind.reshape(-1), + ) + _assert_eq( + moment, + dx.actuator_moment, + 'actuator_moment', + seed, + f'transmission{seed}', + atol=1e-4, + ) if __name__ == '__main__': diff --git a/python/LQR.ipynb b/python/LQR.ipynb index b683444a..adee43ed 100644 --- a/python/LQR.ipynb +++ b/python/LQR.ipynb @@ -491,7 +491,15 @@ }, "outputs": [], "source": [ - "ctrl0 = np.atleast_2d(qfrc0) @ np.linalg.pinv(data.actuator_moment)\n", + "actuator_moment = np.zeros((model.nu, model.nv))\n", + "mujoco.mju_sparse2dense(\n", + " actuator_moment,\n", + " data.actuator_moment,\n", + " data.moment_rownnz,\n", + " data.moment_rowadr,\n", + " data.moment_colind,\n", + ")\n", + "ctrl0 = np.atleast_2d(qfrc0) @ np.linalg.pinv(actuator_moment)\n", "ctrl0 = ctrl0.flatten() # Save the ctrl setpoint.\n", "print('control setpoint:', ctrl0)" ] diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 1bdcfda1..e9bf33cc 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -857,6 +857,9 @@ void mj_transmission(const mjModel* m, mjData* d) { // outputs mjtNum* length = d->actuator_length; mjtNum* moment = d->actuator_moment; + int *rownnz = d->moment_rownnz; + int *rowadr = d->moment_rowadr; + int *colind = d->moment_colind; // allocate Jacbians mj_markStack(d); @@ -875,6 +878,10 @@ void mj_transmission(const mjModel* m, mjData* d) { // compute lengths and moments for (int i=0; i < nu; i++) { + rownnz[i] = 0; + rowadr[i] = i == 0 ? 0 : rowadr[i-1] + rownnz[i-1]; + int adr = rowadr[i]; + // extract info int id = m->actuator_trnid[2*i]; mjtNum* gear = m->actuator_gear+6*i; @@ -885,18 +892,19 @@ void mj_transmission(const mjModel* m, mjData* d) { case mjTRN_JOINTINPARENT: // joint, force in parent frame // slide and hinge joint: scalar gear if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) { + // sparsity + rownnz[i]++; + colind[adr] = m->jnt_dofadr[id]; + length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0]; - moment[i*nv + m->jnt_dofadr[id]] = gear[0]; + moment[adr] = gear[0]; } // ball joint: 3D wrench gear else if (m->jnt_type[id] == mjJNT_BALL) { - // j: qpos start address - int j = m->jnt_qposadr[id]; - // axis: expmap representation of quaternion mjtNum axis[3], quat[4]; - mju_copy4(quat, d->qpos+j); + mju_copy4(quat, d->qpos+m->jnt_qposadr[id]); mju_normalize4(quat); mju_quat2Vel(axis, quat, 1); @@ -912,11 +920,17 @@ void mj_transmission(const mjModel* m, mjData* d) { // length: axis*gearAxis length[i] = mju_dot3(axis, gearAxis); - // j: dof start address - j = m->jnt_dofadr[id]; + // dof start address + int jnt_dofadr = m->jnt_dofadr[id]; + + // sparsity + for (int j = 0; j < 3; j++) { + colind[adr+j] = jnt_dofadr + j; + } + rownnz[i] += 3; // moment: gearAxis - mju_copy3(moment+i*nv+j, gearAxis); + mju_copy3(moment+adr, gearAxis); } // free joint: 6D wrench gear @@ -924,35 +938,30 @@ void mj_transmission(const mjModel* m, mjData* d) { // cannot compute meaningful length, set to 0 length[i] = 0; - // j: qpos start address - int j = m->jnt_qposadr[id]; - - // vec: translational components - mjtNum vec[3]; - mju_copy3(vec, d->qpos+j); - - // axis: expmap representation of quaternion - mjtNum axis[3], quat[4]; - mju_quat2Vel(axis, d->qpos+j+3, 1); - mju_copy4(quat, d->qpos+j+3); - mju_normalize4(quat); - mju_quat2Vel(axis, quat, 1); - // gearAxis: rotate to world frame if necessary mjtNum gearAxis[3]; if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear+3); } else { + mjtNum quat[4]; + mju_copy4(quat, d->qpos+m->jnt_qposadr[id]+3); + mju_normalize4(quat); mju_negQuat(quat, quat); mju_rotVecQuat(gearAxis, gear+3, quat); } - // j: dof start address - j = m->jnt_dofadr[id]; + // dof start address + int jnt_dofadr = m->jnt_dofadr[id]; + + // sparsity + for (int j = 0; j < 6; j++) { + colind[adr+j] = jnt_dofadr + j; + } + rownnz[i] += 6; // moment: gear(tran), gearAxis - mju_copy3(moment+i*nv+j, gear); - mju_copy3(moment+i*nv+j+3, gearAxis); + mju_copy3(moment+adr, gear); + mju_copy3(moment+adr+3, gearAxis); } break; @@ -1000,20 +1009,26 @@ void mj_transmission(const mjModel* m, mjData* d) { mj_jacSite(m, d, jac, 0, id); mju_subFrom(jac, jacS, 3*nv); + // sparsity + for (int j = 0; j < nv; j++) { + colind[adr+j] = j; + } + rownnz[i] += nv; + // clear moment - mju_zero(moment+i*nv, nv); + mju_zero(moment + adr, nv); // apply chain rule for (int j=0; j < nv; j++) { for (int k=0; k < 3; k++) { - moment[i*nv+j] += dlda[k]*jacA[k*nv+j] + dldv[k]*jac[k*nv+j]; + moment[adr+j] += dlda[k]*jacA[k*nv+j] + dldv[k]*jac[k*nv+j]; } } // scale by gear ratio length[i] *= gear[0]; for (int j = 0; j < nv; j++) { - moment[i*nv + j] *= gear[0]; + moment[adr+j] *= gear[0]; } } break; @@ -1022,20 +1037,32 @@ void mj_transmission(const mjModel* m, mjData* d) { length[i] = d->ten_length[id]*gear[0]; // moment: sparse or dense - if (mj_isSparse(m)) { - // clear moment - mju_zero(moment+i*nv, nv); + if (issparse) { + // sparsity + int ten_J_rownnz = d->ten_J_rownnz[id]; + int ten_J_rowadr = d->ten_J_rowadr[id]; + rownnz[i] += ten_J_rownnz; + mju_copyInt(colind + adr, d->ten_J_colind + ten_J_rowadr, ten_J_rownnz); - int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id]; - for (int j=d->ten_J_rowadr[id]; j < end; j++) { - moment[i*nv + d->ten_J_colind[j]] = d->ten_J[j] * gear[0]; - } + mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz); } else { - mju_scl(moment + i*nv, d->ten_J + id*nv, gear[0], nv); + // sparsity + for (int j = 0; j < nv; j++) { + colind[adr+j] = j; + } + rownnz[i] += nv; + + mju_scl(moment+adr, d->ten_J + id*nv, gear[0], nv); } break; case mjTRN_SITE: // site + // sparsity + for (int j = 0; j < nv; j++) { + colind[adr+j] = j; + } + rownnz[i] += nv; + // get site translation (jac) and rotation (jacS) Jacobians in global frame mj_jacSite(m, d, jac, jacS, id); @@ -1050,9 +1077,9 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_mulMatVec3(wrench+3, d->site_xmat+9*id, gear+3); // rotation // moment: global Jacobian projected on wrench - mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation - mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation - mju_addTo(moment+i*nv, jac, nv); // add the two + mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); // translation + mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation + mju_addTo(moment+adr, jac, nv); // add the two } // reference site defined @@ -1089,7 +1116,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } // clear moment - mju_zero(moment+i*nv, nv); + mju_zero(moment+adr, nv); // translational transmission if (!mju_isZero(gear, 3)) { @@ -1121,7 +1148,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear); // moment: global Jacobian projected on wrench - mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); + mju_mulMatTVec(moment+adr, jac, wrench, 3, nv); } // rotational transmission @@ -1162,18 +1189,24 @@ void mj_transmission(const mjModel* m, mjData* d) { // moment_tmp: global Jacobian projected on wrench, add to moment if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv); mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv); - mju_addTo(moment+i*nv, moment_tmp, nv); + mju_addTo(moment+adr, moment_tmp, nv); } } break; case mjTRN_BODY: // body (adhesive contacts) + // sparsity + for (int j = 0; j < nv; j++) { + colind[adr+j] = j; + } + rownnz[i] += nv; + // cannot compute meaningful length, set to 0 length[i] = 0; // clear moment - mju_zero(moment+i*nv, nv); + mju_zero(moment+adr, nv); // moment is average of all contact normal Jacobians { @@ -1257,15 +1290,16 @@ void mj_transmission(const mjModel* m, mjData* d) { // moment is average over contact normal Jacobians, make negative for adhesion if (counter) { // accumulate active contact Jacobians into moment - mj_mulJacTVec(m, d, moment+i*nv, efc_force); + mj_mulJacTVec(m, d, moment+adr, efc_force); // add Jacobians from excluded contacts - mju_addTo(moment+i*nv, moment_exclude, nv); + mju_addTo(moment+adr, moment_exclude, nv); // normalize by total contacts, flip sign - mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv); + mju_scl(moment+adr, moment+adr, -1.0/counter, nv); } } + break; default: diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 3f7eb63f..15d48149 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -827,6 +827,10 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { return; } + // allocate dense actuator_moment row + mj_markStack(d); + mjtNum* moment = mj_stackAllocNum(d, nv); + // process actuators for (int i=0; i < nu; i++) { // skip if disabled @@ -870,9 +874,14 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // add if (bias_vel != 0) { - addJTBJ(m, d, d->actuator_moment+i*nv, &bias_vel, 1); + mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + i, + d->moment_rowadr + i, d->moment_colind); + addJTBJ(m, d, moment, &bias_vel, 1); } } + + // free space + mj_freeStack(d); } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index ae4b7c2d..e77639e5 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -208,8 +208,11 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv); } - // actuator velocity: always dense - mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv); + // actuator velocity: always sparse + if (!mjDISABLED(mjDSBL_ACTUATION)) { + mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, + d->moment_rownnz, d->moment_rowadr, d->moment_colind, NULL); + } // com-based velocities, passive forces, constraint references mj_comVel(m, d); @@ -270,7 +273,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; int nv = m->nv, nu = m->nu; mjtNum gain, bias, tau; - mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force; + mjtNum *prm, *force = d->actuator_force; // clear actuator_force mju_zero(force, nu); @@ -475,7 +478,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); // qfrc_actuator = moment' * force - mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv); + mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv, + d->moment_rownnz, d->moment_rowadr, d->moment_colind); // actuator-level gravity compensation if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 6891da68..57684692 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1857,9 +1857,6 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_zero(d->mocap_pos, 3*m->nmocap); mju_zero(d->mocap_quat, 4*m->nmocap); - // zero out actuator_moment, mj_transmission touches it selectively - mju_zero(d->actuator_moment, m->nv*m->nu); - // copy qpos0 from model if (m->qpos0) { memcpy(d->qpos, m->qpos0, m->nq*sizeof(mjtNum)); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 626ced94..96b41546 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -93,7 +93,7 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, const int* colind, FILE* fp, const char* float_format) { // if no data, or too many rows to be visually useful, return - if (!mat || nr > 300) { + if (!mat || !nr || nr > 300) { return; } fprintf(fp, "%s\n", str); @@ -147,7 +147,7 @@ static void printSparsity(const char* str, int nr, int nc, // print vector static void printVector(const char* str, const mjtNum* data, int n, FILE* fp, const char* float_format) { - if (!data) { + if (!data || !n) { return; } // print str @@ -1005,7 +1005,10 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); - printArray("ACTUATOR_MOMENT", m->nu, m->nv, d->actuator_moment, fp, float_format); + printSparsity("actuator_moments", m->nu, m->nv, + d->moment_rowadr, d->moment_rownnz, d->moment_colind, fp); + printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, + d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); if (M) { diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index f451353b..c1926673 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -29,6 +29,7 @@ #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" +#include "engine/engine_util_sparse.h" #include "engine/engine_util_spatial.h" @@ -66,6 +67,7 @@ static void set0(mjModel* m, mjData* d) { mj_markStack(d); mjtNum* jac = mj_stackAllocNum(d, 6*nv); mjtNum* tmp = mj_stackAllocNum(d, 6*nv); + mjtNum* moment = mj_stackAllocNum(d, nv); int* cammode = 0; int* lightmode = 0; @@ -278,7 +280,9 @@ static void set0(mjModel* m, mjData* d) { // compute actuator_acc0 for (int i=0; i < m->nu; i++) { - mj_solveM(m, d, tmp, d->actuator_moment+i*nv, 1); + mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + i, + d->moment_rowadr + i, d->moment_colind); + mj_solveM(m, d, tmp, moment, 1); m->actuator_acc0[i] = mju_norm(tmp, nv); } } else { @@ -395,13 +399,16 @@ static void set0(mjModel* m, mjData* d) { // === interpret biasprm[2] > 0 as dampratio for position-like actuators // "reflected" inertia (inversely scaled by transmission squared) - mjtNum* transmission = d->actuator_moment + i*nv; + int rownnz = d->moment_rownnz[i]; + int rowadr = d->moment_rowadr[i]; + mjtNum* transmission = d->actuator_moment + rowadr; mjtNum mass = 0; - for (int j=0; j < nv; j++) { + for (int j=0; j < rownnz; j++) { mjtNum trn = mju_abs(transmission[j]); mjtNum trn2 = trn*trn; // transmission squared if (trn2 > mjMINVAL) { - mass += m->dof_M0[j] / trn2; + int dof = d->moment_colind[rowadr + j]; + mass += m->dof_M0[dof] / trn2; } } @@ -598,11 +605,16 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, // step1: compute inertia and actuator moments mj_step1(m, d); + // dense actuator_moment row + mj_markStack(d); + mjtNum* moment = mj_stackAllocNum(d, nv); + mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + index, + d->moment_rowadr + index, d->moment_colind); + // set force to generate desired acceleration - mj_solveM(m, d, d->qfrc_applied, d->actuator_moment+index*nv, 1); + mj_solveM(m, d, d->qfrc_applied, moment, 1); mjtNum nrm = mju_norm(d->qfrc_applied, nv); - mju_scl(d->qfrc_applied, d->actuator_moment+index*nv, - (2*side-1)*opt->accel/mjMAX(mjMINVAL, nrm), nv); + mju_scl(d->qfrc_applied, moment, (2*side-1)*opt->accel/mjMAX(mjMINVAL, nrm), nv); // impose maxforce nrm = mju_norm(d->qfrc_applied, nv); @@ -613,6 +625,8 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, // step2: apply force mj_step2(m, d); + mj_freeStack(d); + // return actuator length return d->actuator_length[index]; } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index fa2647c3..d1dc2d5d 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -39,8 +39,8 @@ MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind, int nnz); // convert matrix from sparse to dense -MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, - const int* rownnz, const int* rowadr, const int* colind); +MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, + const int* rowadr, const int* colind); // multiply sparse matrix and dense vector: res = mat * vec MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 4984a311..51dc26b7 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -31,6 +31,7 @@ #include "src/engine/engine_io.h" #include "src/engine/engine_util_blas.h" #include "src/engine/engine_util_errmem.h" +#include "src/engine/engine_util_sparse.h" #include "test/fixture.h" namespace mujoco { @@ -475,7 +476,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { if (B) { mjtNum *Bc = mj_stackAllocNum(d, nu*nv); mjtNum *BcT = mj_stackAllocNum(d, nv*nu); - mju_copy(Bc, d->actuator_moment, nv*nu); + mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, + d->moment_rowadr, d->moment_colind); mj_solveLD(m, Bc, nu, d->qH, d->qHDiagInv); mju_transpose(BcT, Bc, nu, nv); mju_scl(B, BcT, dt*dt, nu*nv); From b6037d1759065fdeb331fd0682f340f049bf3bb9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 1 Nov 2024 12:05:26 -0700 Subject: [PATCH 050/426] Improve mjSpec documentation. Fixes #2074. PiperOrigin-RevId: 692251710 Change-Id: Ia46bbbe5b7eaa890433b38fed66824f716cc9ea0 --- doc/changelog.rst | 3 + doc/modeling.rst | 26 +++--- doc/programming/modeledit.rst | 150 ++++++++++++++++++++++++---------- doc/python.rst | 88 ++++++++++++++++---- 4 files changed, 196 insertions(+), 71 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index cdc66f8d..8c2ccf60 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,6 +7,9 @@ Upcoming version (not yet released) General ^^^^^^^ + +- The :doc:`Model Editing` framework afforded by :ref:`mjSpec`, introduced in 3.2.0 as an + in-development feature, is now stable and recommended for general use. - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. - The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. diff --git a/doc/modeling.rst b/doc/modeling.rst index f0d3a0e6..066c5b95 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -136,21 +136,21 @@ the model. We start with an example. .. code-block:: xml - - - - - + + + + + - - - - - - - - + + + + + + + + This example will not actually compile because some required information is missing, but here we are only interested diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index d7752964..9c95e7c5 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -1,13 +1,13 @@ Model Editing ------------- -.. admonition:: Unstable API - :class: attention +.. admonition:: New API + :class: note - The API described below is new and unstable. There may be latent bugs and function signatures may change. Early - adopters are welcome (indeed, encouraged) to try it out and report any issues on GitHub. + The API described below is new but feature complete. It is recommended for general use, but latent bugs are still + possible. Please report any issues on GitHub. -As of MuJoCo 3.2, it is possible to create and modify models using the :ref:`mjSpec` struct and related API. +As of MuJoCo 3.2.0, it is possible to create and modify models using the :ref:`mjSpec` struct and related API. This datastructure is in one-to-one correspondence with MJCF and indeed, MuJoCo's own XML parsers (both MJCF and URDF) use this API when loading a model. @@ -21,25 +21,28 @@ The new API augments the traditional workflow of creating and editing models usi *compile* steps. As summarized in the :ref:`Overview chapter`, the traditional workflow is: 1. Create an XML model description file (MJCF or URDF) and associated assets. |br| - 2. Call :ref:`mj_loadXML`, obtain an :ref:`mjModel` instance. + 2. Call :ref:`mj_loadXML`, obtain an mjModel instance. -The new workflow is: +The new workflow using :ref:`mjSpec` is: - 1. :ref:`Create` an empty :ref:`mjSpec` or :ref:`parse` an existing XML file to an - :ref:`mjSpec`. - 2. Edit the mutable :ref:`mjSpec` datastructure adding, changing and removing elements. - 3. Compile the :ref:`mjSpec` at any point, obtaining an updated :ref:`mjModel` instance. After compilation, the - :ref:`mjSpec` remains editable, so steps 2 and 3 are interchangeable. + 1. :ref:`Create` an empty mjSpec or :ref:`parse` an existing XML file. + 2. Programmatically edit the mjSpec datastructure by adding, modifying and removing elements. + 3. :ref:`Compile` the mjSpec to an mjModel instance. + + After compilation, the mjSpec remains editable, so steps 2 and 3 are interchangeable. .. _meUsage: Usage ~~~~~ -Here we describe the C API for procedural model editing, but it is also exposed in the -:ref:`Python bindings`. -After creating a new :ref:`mjSpec` or parsing an existing XML file to an :ref:`mjSpec`, procedural editing corresponds -to setting attributes. For example, in order to change the timestep, one can do: + +Here we describe the C API for procedural model editing, but it is also exposed in the :ref:`Python +bindings`. Advanced users can refer to `user_api_test.cc +`__ and the MJCF parser in +`xml_native_reader.cc `__ for more +usage examples. After creating a new :ref:`mjSpec` or parsing an existing XML file to an :ref:`mjSpec`, procedural +editing corresponds to setting attributes. For example, in order to change the timestep, one can do: .. code-block:: C @@ -55,61 +58,122 @@ In C one uses the provided :ref:`getters` and :ref:`settersmodelname, "my_model"); -In C++ one can use these directly: +In C++, one can use vectors and strings directly: .. code-block:: C++ std::string modelname = "my_model"; *spec->modelname = modelname; +Loading a spec from XML can be done as follows: + +.. code-block:: C + + std::array error; + mjSpec* s = mj_parseXML(filename, vfs, error.data(), error.size()); + .. _meMjsElements: Model elements ^^^^^^^^^^^^^^ +Model elements coresponding to MJCF are exposed to the user as C structs with the ``mjs`` prefix, the definitions are +listed under the :ref:`Model Editing` section of the struct reference. For example, an MJCF +:ref:`geom` corresponds to an :ref:`mjsGeom`. -Model elements corresponding to MJCF are added to the spec using the corresponding functions. For example, to add a box -geom to the world body, one would do +Global defaults for all elements are set by :ref:`initializers` like :ref:`mjs_defaultGeom`. +These functions are defined in `user_init.c +`__ and are the source of truth for all +default values. + +Elements cannot be created directly; they are returned to the user by the corresponding constructor function, e.g. +:ref:`mjs_addGeom`. For example, to add a box geom to the world body, one would do .. code-block:: C - mjSpec* spec = mj_makeSpec(); - mjsBody* world = mjs_findBody(spec, "world"); - mjsGeom* my_geom = mjs_addGeom(world, NULL); - my_geom->type = mjGEOM_BOX; - my_geom->size[0] = my_geom->size[1] = my_geom->size[2] = 0.5; - mjModel* model = mj_compile(spec); + mjSpec* spec = mj_makeSpec(); // make an empty spec + mjsBody* world = mjs_findBody(spec, "world"); // find the world body + mjsGeom* my_geom = mjs_addGeom(world, NULL); // add a geom to the world + my_geom->type = mjGEOM_BOX; // set geom type + my_geom->size[0] = my_geom->size[1] = my_geom->size[2] = 0.5; // set box size + mjModel* model = mj_compile(spec); // compile to mjModel The ``NULL`` second argument to :ref:`mjs_addGeom` is the optional default class pointer. When using defaults procedurally, default classes are passed in explicitly to element constructors. The global defaults of all elements (used when no default class is passed in) can be inspected in `user_init.c `__. - .. _meAttachment: Attachment ^^^^^^^^^^ -The new framework introduces a powerful new feature: attaching and detaching model subtrees. Attachment allows the user +This framework introduces a powerful new feature: attaching and detaching model subtrees. Attachment allows the user copy a subtree from one model into another, while also copying related referenced assets and referencing elements from outside the kinematic tree (e.g., actuators and sensors). Similarly, detaching a subtree will remove all associated -elements from the model. +elements from the model. This feature is already used to power the :ref:`attach` and +:ref:`replicate` meta-elements in MJCF. It is possible to :ref:`attach a body to a frame` and +to :ref:`attach a body to a site`: -This feature is incomplete and will be described in detail once it is fully implemented, but it is already used to power -the :ref:`attach` and :ref:`replicate` meta-elements in MJCF. +.. code-block:: C + mjSpec* parent = mj_makeSpec(); + mjSpec* child = mj_makeSpec(); + mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), NULL); + mjsSite* site = mjs_addSite(mjs_findBody(parent, "world"), NULL); + mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), NULL); + mjsBody* attached_body_1 = mjs_attachBody(frame, body, "attached-", "-1"); + mjsBody* attached_body_2 = mjs_attachToSite(site, body, "attached-", "-2"); -.. _meKnownIssues: +or :ref:`attach a frame to a body`: -Known issues -~~~~~~~~~~~~ +.. code-block:: C -- Better documentation is still missing and will be added in the future. In the meantime, advanced users can refer - to `user_api_test.cc `__ and the MJCF - parser in `xml_native_reader.cc `__, - which is already using this API. -- One of the central design considerations of the new API is incremental compilation, meaning that after making small - changes to a spec that has already been compiled, subsequent re-compilation will be very fast. While the code is - written to support incremental compilation, this functionality is not fully implemented and will be added in the - future, resulting in faster re-compilation times. -- Since the main test for the new API is the MJCF parser, which always constructs a model from scratch, there - might be latent bugs related to model editing. Please report such bugs if you encounter them. + mjSpec* parent = mj_makeSpec(); + mjSpec* child = mj_makeSpec(); + mjsBody* body = mjs_addBody(mjs_findBody(parent, "world"), NULL); + mjsFrame* frame = mjs_addFrame(mjs_findBody(child, "world"), NULL); + mjsFrame* attached_frame = mjs_attachFrame(body, frame, "attached-", "-1"); + +.. _meDefault: + +Default classes +^^^^^^^^^^^^^^^ +Default classes are fully supported in the new API, however using them requires an understanding of how defaults +are implemented. As explained in the :ref:`Default settings ` section, default classes are first loaded as a +tree of dummy elements, which are then used to initialize elements which reference them. When editing models with +defaults, this initialization is explicit: + +.. code-block:: C + + mjSpec* spec = mj_makeSpec(); + mjsDefault* main = mjs_getSpecDefault(spec); + main->geom.type = mjGEOM_BOX; + mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), main); + +Importantly, changing a default class after it has been used to initialize elements will not change the properties of +already initialized elements. + +.. admonition:: Possible future change + :class: note + + The behaviour described above, where defaults are only applied at initialization, is a remnant of the old, XML-only + loading pipeline. A future API change could allow defaults to be changed and applied after initialization. If you + think this feature is important to you, please let us know on GitHub. + +.. _meSaving: + +XML saving +^^^^^^^^^^ +Specs can be saved to an XML file or string using :ref:`mj_saveXML` or :ref:`mj_saveXMLString`, respectively. +Saving requires that the spec first be compiled. +Importantly, the saved XML will take into account any defined defaults. This is useful when a model has many repeated +values, for example if loaded from URDF, which does not support defaults. In such a case one can add default classes, +set the class of the relevant elements, and save; the resulting XML will use the defaults and be more human-readable. + +.. _meRecompilation: + +In-place recompilation +^^^^^^^^^^^^^^^^^^^^^^ + +Compilation with :ref:`mj_compile` can be called at any point to obtain a new mjModel instance. In contrast, +:ref:`mj_recompile` updates an existing mjModel and mjData pair in-place, while preserving the simulation state. This +allows model editing to occur **during simulation**, for example adding or removing bodies. diff --git a/doc/python.rst b/doc/python.rst index cef23eb5..9c824b86 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -469,14 +469,11 @@ the raw callback pointer, and the GIL will **not** be acquired each time the cal Model editing ============= -The :doc:`Model Editing` framework which allows for procedural model manipulation is exposed -via Python. In many ways this API is conceptually similar to ``dm_control``'s -`PyMJCF module `__, where ``MjSpec`` -plays the role of ``mjcf_model``. The largest difference between these two APIs is speed. Native model manipulation via -``MjSpec`` is around ~100x faster than PyMJCF. +The C API for model editing is documented in the :doc:`Programming<../programming/modeledit>` chapter. +This functionality is mirrored in the Python API, with the addition of several convenience methods. +Below is a minimal usage example, more examples can be found in the Model Editing +`colab notebook `__. -Below is a simple example of how to use the model editing API. For more examples, please refer to -`specs_test.py `__. .. code-block:: python @@ -495,18 +492,79 @@ Below is a simple example of how to use the model editing API. For more examples ... model = spec.compile() -.. admonition:: Missing features - :class: attention +Construction +------------ - We are aware of multiple missing features in the Python API, including: +The ``MjSpec`` object wraps the :ref:`mjSpec` struct and can be constructed in three ways: - - Better tree traversal utilities like :python:`children = body.children()` etc. - - PyMJCF's notion of "binding", allowing access to :ref:`mjModel` and :ref:`mjData` values via the associated ``mjs`` - elements. +1. Create an empty spec: ``spec = mujoco.MjSpec()`` +2. Load the spec from XML string: ``spec = mujoco.MjSpec.from_string(xml_string)`` +3. Load the spec from XML file: ``spec = mujoco.MjSpec.from_file(file_path)`` - There are certainly other missing features that we are not aware of. Please contact us on GitHub with feature - requests or bug reports and we will prioritize accordingly. +Note the ``from_string()`` and ``from_file()`` methods can only be called at construction time. +Convenience methods +------------------- + +The Python bindings provide a number of convenience methods and attributes not directly available in the C API in order +to make model editing easier: + +Element lists +^^^^^^^^^^^^^ +Lists of all elements in a spec can be accessed using named properties, using the plural form. For example, +``spec.meshes`` returns a list of all meshes in the spec. + +The following properties are implemented: ``sites``, ``geoms``, ``joints``, ``lights``, ``cameras``, ``bodies``, +``frames``, ``materials``, ``meshes``, ``pairs``, ``equalities``, ``tendons``, ``actuators``, ``skins``, ``textures``, +``texts``, ``tuples``, ``flexes``, ``hfields``, ``keys``, ``numerics``, ``excludes``, ``sensors``, ``plugins``. + +Tree traversal +^^^^^^^^^^^^^^ +Traversal of the kinematic tree is aided by the following methods which return tree-related lists of elements: + +Direct children: + Like the spec-level element lists described above, bodies have properties which return lists of all direct children. + For example, ``body.geoms`` returns a list of all geoms that are direct children of the body. This works for all + in tree elements namely ``bodies``, ``joints``, ``geoms``, ``sites``, ``cameras``, ``lights`` and ``frames``. + +Recursive search: + ``body.find_all()`` returns a list of all elements of the given type which are in the subtree of the given body. + Element types can be specified with the :ref:`mjtObj` enum, or with the corresponding string. For example either + ``body.find_all(mujoco.mjtObj.mjOBJ_SITE)`` or ``body.find_all('site')`` will return a list of all sites under the + body. + + +Relationship to ``PyMJCF`` +-------------------------- + +`dm_control `__'s +`PyMJCF `__ module provides similar +functionality to the native model editing API described here, but is roughly two orders of magnitude slower due to its +reliance on Python manipulation of strings. + +For users familiar with ``PyMJCF``, the ``MjSpec`` object is conceptually similar to ``dm_control``'s +``mjcf_model``. A more detailed migration guide could be added here in the future; in the meantime, note that the +Model Editing +`colab notebook `__ +includes a reimplementation of the ``PyMJCF`` example in the ``dm_control`` +`tutorial notebook `__. + +``PyMJCF`` provides a notion of "binding", giving access to :ref:`mjModel` and :ref:`mjData` values via the constructing +elements. In the native API, this is done with object ids. For example, say we have multiple geoms containing the string +"torso" in their name. We want to get their Cartesian positions in the XY plane from ``mjData``. This can be done as +follows: + +.. code-block:: python + + torsos = [geom.id for geom in spec.geoms if 'torso' in geom.name] + pos_x = data.geom_xpos[torsos, 0] + pos_y = data.geom_xpos[torsos, 1] + +Notes +----- + +- :ref:`mj_recompile` works differently than in the C API. In the C API, it modifies the model and the data in place, + while in the Python API it returns new :ref:`MjModel` and :ref:`MjData` objects. This is to avoid dangling references. .. _PyBuild: From 47ebb0614e90ea99295b65ca383f04e564318b8c Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 1 Nov 2024 13:23:22 -0700 Subject: [PATCH 051/426] Set some visual params as jax.Arrays for visual domain rando. PiperOrigin-RevId: 692273879 Change-Id: I80475aa65744de06a6bfebbfb95a44faa751595f --- mjx/mujoco/mjx/_src/types.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index e1df9681..ec17efed 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -963,8 +963,8 @@ class Model(PyTreeNode): light_bodyid: np.ndarray = _restricted_to('mujoco') light_targetbodyid: np.ndarray = _restricted_to('mujoco') light_directional: np.ndarray - light_pos: np.ndarray = _restricted_to('mujoco') - light_dir: np.ndarray = _restricted_to('mujoco') + light_pos: jax.Array + light_dir: jax.Array light_poscom0: np.ndarray = _restricted_to('mujoco') light_pos0: np.ndarray light_dir0: np.ndarray From ebe60b9ad5e0ecd32ec506f44d1fef286fd310e8 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 1 Nov 2024 18:17:42 -0700 Subject: [PATCH 052/426] Add const to mjsDefault when used as function argument. Fixes #2197. PiperOrigin-RevId: 692346264 Change-Id: Iddb9be1377a7d59c86911802d8c5563f6bf6b043 --- doc/includes/references.h | 28 +++++++-------- include/mujoco/mujoco.h | 28 +++++++-------- introspect/functions.py | 28 +++++++-------- python/mujoco/specs.cc | 2 +- src/user/user_api.cc | 28 +++++++-------- src/user/user_api.h | 28 +++++++-------- src/xml/xml_native_reader.cc | 67 ++++++++++++++++-------------------- src/xml/xml_native_reader.h | 4 +-- 8 files changed, 102 insertions(+), 111 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 546299d7..0f8d869a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3567,22 +3567,22 @@ mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, const char* prefix, const char* suffix); int mjs_detachBody(mjSpec* s, mjsBody* b); -mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def); -mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def); -mjsJoint* mjs_addJoint(mjsBody* body, mjsDefault* def); +mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); +mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); +mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); mjsJoint* mjs_addFreeJoint(mjsBody* body); -mjsGeom* mjs_addGeom(mjsBody* body, mjsDefault* def); -mjsCamera* mjs_addCamera(mjsBody* body, mjsDefault* def); -mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def); +mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); +mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); +mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); void mjs_delete(mjsElement* element); -mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def); +mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); mjsSensor* mjs_addSensor(mjSpec* s); mjsFlex* mjs_addFlex(mjSpec* s); -mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def); +mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* def); mjsExclude* mjs_addExclude(mjSpec* s); -mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def); -mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def); +mjsEquality* mjs_addEquality(mjSpec* s, const mjsDefault* def); +mjsTendon* mjs_addTendon(mjSpec* s, const mjsDefault* def); mjsWrap* mjs_wrapSite(mjsTendon* tendon, const char* name); mjsWrap* mjs_wrapGeom(mjsTendon* tendon, const char* name, const char* sidesite); mjsWrap* mjs_wrapJoint(mjsTendon* tendon, const char* name, double coef); @@ -3593,11 +3593,11 @@ mjsTuple* mjs_addTuple(mjSpec* s); mjsKey* mjs_addKey(mjSpec* s); mjsPlugin* mjs_addPlugin(mjSpec* s); mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); -mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def); +mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); mjsTexture* mjs_addTexture(mjSpec* s); -mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); +mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); mjSpec* mjs_getSpec(mjsElement* element); mjSpec* mjs_findSpec(mjSpec* spec, const char* name); mjsBody* mjs_findBody(mjSpec* s, const char* name); @@ -3605,7 +3605,7 @@ mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); mjsBody* mjs_findChild(mjsBody* body, const char* name); mjsFrame* mjs_findFrame(mjSpec* s, const char* name); mjsDefault* mjs_getDefault(mjsElement* element); -mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); mjsDefault* mjs_getSpecDefault(mjSpec* s); int mjs_getId(mjsElement* element); mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); @@ -3625,7 +3625,7 @@ void mjs_setDouble(mjDoubleVec* dest, const double* array, int size); void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes); const char* mjs_getString(const mjString* source); const double* mjs_getDouble(const mjDoubleVec* source, int* size); -void mjs_setDefault(mjsElement* element, mjsDefault* def); +void mjs_setDefault(mjsElement* element, const mjsDefault* def); void mjs_setFrame(mjsElement* dest, mjsFrame* frame); const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 54b807e9..2fedda8f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1421,25 +1421,25 @@ MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); //---------------------------------- Tree elements ------------------------------------------------- // Add child body to body, return child. -MJAPI mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def); +MJAPI mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); // Add site to body, return site spec. -MJAPI mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def); +MJAPI mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); // Add joint to body. -MJAPI mjsJoint* mjs_addJoint(mjsBody* body, mjsDefault* def); +MJAPI mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); // Add freejoint to body. MJAPI mjsJoint* mjs_addFreeJoint(mjsBody* body); // Add geom to body. -MJAPI mjsGeom* mjs_addGeom(mjsBody* body, mjsDefault* def); +MJAPI mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); // Add camera to body. -MJAPI mjsCamera* mjs_addCamera(mjsBody* body, mjsDefault* def); +MJAPI mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); // Add light to body. -MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def); +MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); @@ -1451,7 +1451,7 @@ MJAPI void mjs_delete(mjsElement* element); //---------------------------------- Non-tree elements --------------------------------------------- // Add actuator. -MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def); +MJAPI mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); // Add sensor. MJAPI mjsSensor* mjs_addSensor(mjSpec* s); @@ -1460,16 +1460,16 @@ MJAPI mjsSensor* mjs_addSensor(mjSpec* s); MJAPI mjsFlex* mjs_addFlex(mjSpec* s); // Add contact pair. -MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def); +MJAPI mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* def); // Add excluded body pair. MJAPI mjsExclude* mjs_addExclude(mjSpec* s); // Add equality. -MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def); +MJAPI mjsEquality* mjs_addEquality(mjSpec* s, const mjsDefault* def); // Add tendon. -MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def); +MJAPI mjsTendon* mjs_addTendon(mjSpec* s, const mjsDefault* def); // Wrap site using tendon. MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendon, const char* name); @@ -1505,7 +1505,7 @@ MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefa //---------------------------------- Assets -------------------------------------------------------- // Add mesh. -MJAPI mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def); +MJAPI mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); // Add height field. MJAPI mjsHField* mjs_addHField(mjSpec* s); @@ -1517,7 +1517,7 @@ MJAPI mjsSkin* mjs_addSkin(mjSpec* s); MJAPI mjsTexture* mjs_addTexture(mjSpec* s); // Add material. -MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); +MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); //---------------------------------- Find and get utilities ---------------------------------------- @@ -1544,7 +1544,7 @@ MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); MJAPI mjsDefault* mjs_getDefault(mjsElement* element); // Find default in model by class name. -MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); // Get global default from model. MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); @@ -1614,7 +1614,7 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); //---------------------------------- Spec utilities ------------------------------------------------ // Set element's default. -MJAPI void mjs_setDefault(mjsElement* element, mjsDefault* def); +MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def); // Set element's enclosing frame. MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); diff --git a/introspect/functions.py b/introspect/functions.py index 6c3a5b9e..89b28e4a 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -9072,7 +9072,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9094,7 +9094,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9116,7 +9116,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9154,7 +9154,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9176,7 +9176,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9198,7 +9198,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9256,7 +9256,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9310,7 +9310,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9348,7 +9348,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9370,7 +9370,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9596,7 +9596,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9666,7 +9666,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9822,7 +9822,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionDecl( name='mjs_findDefault', return_type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), parameters=( FunctionParameterDecl( @@ -10262,7 +10262,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='def', type=PointerType( - inner_type=ValueType(name='mjsDefault'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index b2beaa3f..d88a0544 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -346,7 +346,7 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjSpec.def( "find_default", - [](MjSpec& self, std::string& classname) -> raw::MjsDefault* { + [](MjSpec& self, std::string& classname) -> const raw::MjsDefault* { return mjs_findDefault(self.ptr, classname.c_str()); }, py::return_value_policy::reference_internal); diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 62dd2717..af465e75 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -263,7 +263,7 @@ void mjs_delete(mjsElement* element) { // add child body to body, return child spec -mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) { +mjsBody* mjs_addBody(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element)->AddBody(def); return &body->spec; @@ -272,7 +272,7 @@ mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) { // add site to body, return site spec -mjsSite* mjs_addSite(mjsBody* bodyspec, mjsDefault* defspec) { +mjsSite* mjs_addSite(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element); mjCSite* site = body->AddSite(def); @@ -282,7 +282,7 @@ mjsSite* mjs_addSite(mjsBody* bodyspec, mjsDefault* defspec) { // add joint to body -mjsJoint* mjs_addJoint(mjsBody* bodyspec, mjsDefault* defspec) { +mjsJoint* mjs_addJoint(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element); mjCJoint* joint = body->AddJoint(def); @@ -301,7 +301,7 @@ mjsJoint* mjs_addFreeJoint(mjsBody* bodyspec) { // add geom to body -mjsGeom* mjs_addGeom(mjsBody* bodyspec, mjsDefault* defspec) { +mjsGeom* mjs_addGeom(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element); mjCGeom* geom = body->AddGeom(def); @@ -311,7 +311,7 @@ mjsGeom* mjs_addGeom(mjsBody* bodyspec, mjsDefault* defspec) { // add camera to body -mjsCamera* mjs_addCamera(mjsBody* bodyspec, mjsDefault* defspec) { +mjsCamera* mjs_addCamera(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element); mjCCamera* camera = body->AddCamera(def); @@ -321,7 +321,7 @@ mjsCamera* mjs_addCamera(mjsBody* bodyspec, mjsDefault* defspec) { // add light to body -mjsLight* mjs_addLight(mjsBody* bodyspec, mjsDefault* defspec) { +mjsLight* mjs_addLight(mjsBody* bodyspec, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCBody* body = static_cast(bodyspec->element); mjCLight* light = body->AddLight(def); @@ -354,7 +354,7 @@ mjsFrame* mjs_addFrame(mjsBody* bodyspec, mjsFrame* parentframe) { // add mesh to model -mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* defspec) { +mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* defspec) { mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCModel* modelC = static_cast(s->element); mjCMesh* mesh = modelC->AddMesh(def); @@ -391,7 +391,7 @@ mjsTexture* mjs_addTexture(mjSpec* s) { // add material to model -mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* defspec) { +mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* defspec) { mjCModel* modelC = static_cast(s->element); mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCMaterial* material = modelC->AddMaterial(def); @@ -401,7 +401,7 @@ mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* defspec) { // add pair to model -mjsPair* mjs_addPair(mjSpec* s, mjsDefault* defspec) { +mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* defspec) { mjCModel* modelC = static_cast(s->element); mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCPair* pair = modelC->AddPair(def); @@ -420,7 +420,7 @@ mjsExclude* mjs_addExclude(mjSpec* s) { // add equality to model -mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* defspec) { +mjsEquality* mjs_addEquality(mjSpec* s, const mjsDefault* defspec) { mjCModel* modelC = static_cast(s->element); mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCEquality* equality = modelC->AddEquality(def); @@ -430,7 +430,7 @@ mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* defspec) { // add tendon to model -mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* defspec) { +mjsTendon* mjs_addTendon(mjSpec* s, const mjsDefault* defspec) { mjCModel* modelC = static_cast(s->element); mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCTendon* tendon = modelC->AddTendon(def); @@ -476,7 +476,7 @@ mjsWrap* mjs_wrapPulley(mjsTendon* tendonspec, double divisor) { // add actuator to model -mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* defspec) { +mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* defspec) { mjCModel* modelC = static_cast(s->element); mjCDef* def = defspec ? static_cast(defspec->element) : 0; mjCActuator* actuator = modelC->AddActuator(def); @@ -580,7 +580,7 @@ mjsDefault* mjs_getDefault(mjsElement* element) { // Find default with given name in model. -mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) { +const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) { mjCModel* modelC = static_cast(s->element); mjCDef* cdef = modelC->FindDefault(classname); if (!cdef) { @@ -692,7 +692,7 @@ int mjs_getId(mjsElement* element) { // set default -void mjs_setDefault(mjsElement* element, mjsDefault* defspec) { +void mjs_setDefault(mjsElement* element, const mjsDefault* defspec) { mjCBase* baseC = static_cast(element); baseC->classname = static_cast(defspec->element)->name; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 25d6106c..70eb6af2 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -85,25 +85,25 @@ MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); //---------------------------------- Add tree elements --------------------------------------------- // Add child body to body, return child. -MJAPI mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def); +MJAPI mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); // Add site to body, return site spec. -MJAPI mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def); +MJAPI mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); // Add joint to body. -MJAPI mjsJoint* mjs_addJoint(mjsBody* body, mjsDefault* def); +MJAPI mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); // Add freejoint to body. MJAPI mjsJoint* mjs_addFreeJoint(mjsBody* body); // Add geom to body. -MJAPI mjsGeom* mjs_addGeom(mjsBody* body, mjsDefault* def); +MJAPI mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); // Add camera to body. -MJAPI mjsCamera* mjs_addCamera(mjsBody* body, mjsDefault* def); +MJAPI mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); // Add light to body. -MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def); +MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); @@ -115,7 +115,7 @@ MJAPI void mjs_delete(mjsElement* element); //---------------------------------- Add non-tree elements ----------------------------------------- // Add actuator. -MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def); +MJAPI mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); // Add sensor. MJAPI mjsSensor* mjs_addSensor(mjSpec* s); @@ -124,16 +124,16 @@ MJAPI mjsSensor* mjs_addSensor(mjSpec* s); MJAPI mjsFlex* mjs_addFlex(mjSpec* s); // Add contact pair. -MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def); +MJAPI mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* def); // Add excluded body pair. MJAPI mjsExclude* mjs_addExclude(mjSpec* s); // Add equality. -MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def); +MJAPI mjsEquality* mjs_addEquality(mjSpec* s, const mjsDefault* def); // Add tendon. -MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def); +MJAPI mjsTendon* mjs_addTendon(mjSpec* s, const mjsDefault* def); // Wrap site using tendon. MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendon, const char* name); @@ -169,7 +169,7 @@ MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefa //---------------------------------- Add assets ---------------------------------------------------- // Add mesh. -MJAPI mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def); +MJAPI mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); // Add height field. MJAPI mjsHField* mjs_addHField(mjSpec* s); @@ -181,7 +181,7 @@ MJAPI mjsSkin* mjs_addSkin(mjSpec* s); MJAPI mjsTexture* mjs_addTexture(mjSpec* s); // Add material. -MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); +MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); //---------------------------------- Find/get utilities -------------------------------------------- @@ -208,7 +208,7 @@ MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); MJAPI mjsDefault* mjs_getDefault(mjsElement* element); // Find default in model by class name. -MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); // Get global default from model. MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); @@ -353,7 +353,7 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); //---------------------------------- Other utilities ----------------------------------------------- // Set element's default. -MJAPI void mjs_setDefault(mjsElement* element, mjsDefault* def); +MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def); // Set element's enlcosing frame. MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6a70d13e..0037a77a 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2383,7 +2383,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // make composite -void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { +void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, const mjsDefault* def) { string text; int n; @@ -3188,7 +3188,7 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { name = elem->Value(); // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -3408,7 +3408,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, name = elem->Value(); // get class if specified, otherwise use body - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getDefault(frame ? frame->element : body->element); } @@ -3519,13 +3519,10 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, // frame sub-element else if (name=="frame") { // read childdef - mjsDefault* childdef = 0; - if (ReadAttrTxt(elem, "childclass", text)) { - childdef = mjs_findDefault(spec, text.c_str()); - mjs_findDefault(spec, text.c_str()); - if (!childdef) { - throw mjXError(elem, "unknown default childclass"); - } + bool has_childclass = ReadAttrTxt(elem, "childclass", text); + const mjsDefault* childdef = has_childclass ? mjs_findDefault(spec, text.c_str()) : nullptr; + if (has_childclass && !childdef) { + throw mjXError(elem, "unknown default childclass"); } // create frame @@ -3568,13 +3565,10 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, mjs_resolveOrientation(rotation, spec->compiler.degree, spec->compiler.eulerseq, &alt); // read childdef - mjsDefault* childdef = 0; - if (ReadAttrTxt(elem, "childclass", text)) { - childdef = mjs_findDefault(spec, text.c_str()); - mjs_findDefault(spec, text.c_str()); - if (!childdef) { - throw mjXError(elem, "unknown default childclass"); - } + bool has_childclass = ReadAttrTxt(elem, "childclass", text); + const mjsDefault* childdef = has_childclass ? mjs_findDefault(spec, text.c_str()) : nullptr; + if (has_childclass && !childdef) { + throw mjXError(elem, "unknown default childclass"); } // create subtree @@ -3622,13 +3616,10 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, // body sub-element else if (name=="body") { // read childdef - mjsDefault* childdef = 0; - if (ReadAttrTxt(elem, "childclass", text)) { - childdef = mjs_findDefault(spec, text.c_str()); - mjs_findDefault(spec, text.c_str()); - if (!childdef) { - throw mjXError(elem, "unknown default childclass"); - } + bool has_childclass = ReadAttrTxt(elem, "childclass", text); + const mjsDefault* childdef = has_childclass ? mjs_findDefault(spec, text.c_str()) : nullptr; + if (has_childclass && !childdef) { + throw mjXError(elem, "unknown default childclass"); } // create child body @@ -3720,7 +3711,7 @@ void mjXReader::Contact(XMLElement* section) { name = elem->Value(); // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -3765,7 +3756,7 @@ void mjXReader::Equality(XMLElement* section) { elem = FirstChildElement(section); while (elem) { // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -3793,7 +3784,7 @@ void mjXReader::Deformable(XMLElement* section, const mjVFS* vfs) { name = elem->Value(); // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -3829,7 +3820,7 @@ void mjXReader::Tendon(XMLElement* section) { elem = FirstChildElement(section); while (elem) { // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -3895,7 +3886,7 @@ void mjXReader::Actuator(XMLElement* section) { elem = FirstChildElement(section); while (elem) { // get class if specified, otherwise use default0 - mjsDefault* def = GetClass(elem); + const mjsDefault* def = GetClass(elem); if (!def) { def = mjs_getSpecDefault(spec); } @@ -4306,19 +4297,19 @@ void mjXReader::Keyframe(XMLElement* section) { // get defaults class -mjsDefault* mjXReader::GetClass(XMLElement* section) { +const mjsDefault* mjXReader::GetClass(XMLElement* section) { string text; - mjsDefault* def = nullptr; - if (ReadAttrTxt(section, "class", text)) { - def = mjs_findDefault(spec, text.c_str()); - if (!def) { - throw mjXError( - section, - string("unknown default class name '" + text + "'").c_str()); - } + if (!ReadAttrTxt(section, "class", text)) { + return nullptr; } + const mjsDefault* def = mjs_findDefault(spec, text.c_str()); + if (!def) { + throw mjXError( + section, + string("unknown default class name '" + text + "'").c_str()); + } return def; } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 62488b97..41aacbbb 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -80,12 +80,12 @@ class mjXReader : public mjXBase { void OneEquality(tinyxml2::XMLElement* elem, mjsEquality* pequality); void OneTendon(tinyxml2::XMLElement* elem, mjsTendon* ptendon); void OneActuator(tinyxml2::XMLElement* elem, mjsActuator* pactuator); - void OneComposite(tinyxml2::XMLElement* elem, mjsBody* pbody, mjsDefault* def); + void OneComposite(tinyxml2::XMLElement* elem, mjsBody* pbody, const mjsDefault* def); void OneFlexcomp(tinyxml2::XMLElement* elem, mjsBody* pbody, const mjVFS* vfs); void OnePlugin(tinyxml2::XMLElement* elem, mjsPlugin* plugin); mjXSchema schema; // schema used for validation - mjsDefault* GetClass(tinyxml2::XMLElement* section); // get default class name + const mjsDefault* GetClass(tinyxml2::XMLElement* section); // get default class name bool readingdefaults; // true while reading defaults From 831d9881d3c2e453536425c5b4eeb65c1aeff464 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sat, 2 Nov 2024 11:13:16 -0700 Subject: [PATCH 053/426] Remove box, cylinder and ellipsoid composite types. Also, fix a bug with Flex textures that was causing an incorrect allocation of the textures in mjModel. Fixes #2013. PiperOrigin-RevId: 692510858 Change-Id: If781716216974e085244da27bc3aa57d91c3458c --- doc/XMLreference.rst | 7 +- doc/changelog.rst | 3 + doc/modeling.rst | 57 +--- model/composite/softbox.xml | 17 +- src/user/user_composite.cc | 453 ----------------------------- src/user/user_composite.h | 11 - src/user/user_flexcomp.cc | 25 ++ src/user/user_model.cc | 1 + src/xml/xml_native_reader.cc | 5 +- test/engine/testdata/skingroup.xml | 10 +- test/user/user_composite_test.cc | 2 +- test/xml/xml_native_reader_test.cc | 10 +- 12 files changed, 54 insertions(+), 547 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 523b0d43..69c8751a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3566,9 +3566,10 @@ saving the XML: :at:`texcoord`: :at-val:`real(2*npoint), optional` Texture coordinates of each point, passed through to the automatically-generated flex. Note that flexcomp does not - generate texture coordinates automatically, except for 2D grids. For all other types, the user can specify explicit - texture coordinates here, even if the points themselves were generated automatically. This requires understanding of - the layout of the automatically-generated points and how they correspond to the texture referenced by the material. + generate texture coordinates automatically, except for 2D grids, box, cylinder and ellipsoid. For all other types, + the user can specify explicit texture coordinates here, even if the points themselves were generated automatically. + This requires understanding of the layout of the automatically-generated points and how they correspond to the + texture referenced by the material. .. _body-flexcomp-mass: diff --git a/doc/changelog.rst b/doc/changelog.rst index 8c2ccf60..a414cd39 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -17,6 +17,8 @@ General - Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). - The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single :ref:`layer` sub-element. +- The composite types box, cylinder, and sphere have been removed. Users should instead use the equivalent types + available in :ref:`flexcomp`. MJX ^^^ @@ -33,6 +35,7 @@ Bug fixes several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints. - Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. +- Fixed a bug in flex texture coordinates that prevented the correct allocation of textures in mjModel. Documentation diff --git a/doc/modeling.rst b/doc/modeling.rst index 066c5b95..9f3bc202 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1280,62 +1280,11 @@ scenario (e.g. a stretched rubber band). The cloth is deprecated. It is recommended to use 2D flex :ref:`deformable objects ` for simulating thin elastic structures. -**Box**. +**Box, cylinder and ellipsoid**. -|image14| |image15| -.. code-block:: xml - - - - - - - - - -The box type, as well as the cylinder and ellipsoid types below, are used to model soft 3D objects. The element bodies -form a grid along the outer shell, thus the number of element bodies scales with the square of the linear dimension. -This is much more efficient than simulating a 3D grid. The parent body within which :el:`composite` appears is at the -center of the soft object. All element bodies are children of the parent. Each element body has a single sliding joint -pointing away from the parent. These joints allow the surface of the soft object to compress and expand at any point. -The joints are equality-constrained to their initial position, so as to maintain the shape. In addition each joint is -equality-constrained to its neighbor joints, so that when the soft objects deforms, the deformation is smooth. -Finally, there is a tendon equality constraint specifying that the sum of all joints should remain constant. This -attempts to preserve the volume of the soft object approximately. If the object is squeezed from all sides it will -compress and the volume will decrease, but otherwise some element bodies will stick out to compensate for squeezing -elsewhere. The plot on the left shows this effect; we are using the capsule probe to compress one corner, and the -opposite sides of the cube expand a bit, while the deformations remain smooth. The :at:`count` attribute determines -the number of element bodies in each dimension, so if the counts are different the resulting object will be a -rectangular box and not a cube. The geoms attached to the element bodies can be spheres, capsules or ellipsoids. -Spheres are faster for collision detection, but they result in a thin shell, allowing other bodies to "get under the -skin" of the soft object. When capsules or ellipsoids are used, they are automatically oriented so that the long axis -points to the outside, thus creating a thicker shell which is harder to penetrate. - -**Cylinder and ellipsoid**. - -|image16| |image17| - -.. code-block:: xml - - - - - - - - - -Cylinders and ellipsoids are created in the same way as boxes. The only difference is that the reference positions of -the element bodies (relative to the parent) are projected on a cylinder or ellipsoid, with size implied by the -:at:`count` attribute. The automatic skin generator is aware of the smooth surfaces, and adjusts the skin normals -accordingly. In the plots we have used the capsule probe to press on each body, then paused the simulation and moved the -probe away (which is possible because the probe is a mocap body which can move independent of the physics). In this way -we can see the indentation made by the probe, and the resulting deformation in the rest of the body. By changing the -solref and solimp attributes of the equality constraints that hold the soft object together, one can adjust the behavior -of the system making it softer or harder, damped or springy, etc. Note that box, cylinder and ellipsoid objects do not -involve long kinematic chains, and can be simulated at large timesteps -- similar to particle and grid, and unlike rope -and cloth. +The box type, as well as the cylinder and ellipsoid types, are now deprecated in favor of 3D flex :ref:`deformable +objects ``. element. .. _CDeformable: diff --git a/model/composite/softbox.xml b/model/composite/softbox.xml index ab129dfd..55d708de 100644 --- a/model/composite/softbox.xml +++ b/model/composite/softbox.xml @@ -14,10 +14,6 @@ --> - diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 1330d886..73c1227d 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -192,25 +192,6 @@ void mjCComposite::SetDefault(void) { case mjCOMPTYPE_CLOTH: // cloth break; - case mjCOMPTYPE_BOX: // 3D - case mjCOMPTYPE_CYLINDER: - case mjCOMPTYPE_ELLIPSOID: - - // no self-collisions - def[0].spec.geom->contype = 0; - - // soft smoothing - AdjustSoft(solrefsmooth, solimpsmooth, 1); - - // soft fix everywhere - for (int i=0; isolref, def[i].spec.equality->solimp, 1); - } - - // hard main tendon fix - AdjustSoft(def[mjCOMPKIND_TENDON].spec.equality->solref, - def[mjCOMPKIND_TENDON].spec.equality->solimp, 0); - break; default: // SHOULD NOT OCCUR mju_error("Invalid composite type: %d", type); @@ -347,11 +328,6 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) "\"shell\" instead.", error_sz); - case mjCOMPTYPE_BOX: - case mjCOMPTYPE_CYLINDER: - case mjCOMPTYPE_ELLIPSOID: - return MakeBox(model, body, error, error_sz); - default: return comperr(error, "Unknown shape in composite", error_sz); } @@ -919,166 +895,6 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i -// project from box to other shape -void mjCComposite::BoxProject(double* pos) { - // determine sizes - double size[3] = { - 0.5*spacing*(count[0]-1), - 0.5*spacing*(count[1]-1), - 0.5*spacing*(count[2]-1) - }; - - // box - if (type==mjCOMPTYPE_BOX) { - pos[0] *= size[0]; - pos[1] *= size[1]; - pos[2] *= size[2]; - } - - // cylinder - else if (type==mjCOMPTYPE_CYLINDER) { - double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); - mjuu_normvec(pos, 2); - pos[0] *= size[0]*L0; - pos[1] *= size[1]*L0; - pos[2] *= size[2]; - } - - // ellipsoid - else if (type==mjCOMPTYPE_ELLIPSOID) { - mjuu_normvec(pos, 3); - pos[0] *= size[0]; - pos[1] *= size[1]; - pos[2] *= size[2]; - } -} - - - -// make 3d box, ellipsoid or cylinder -bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz) { - char txt[100]; - - // check dim - if (dim!=3) { - return comperr(error, "Box and ellipsoid must be three-dimensional", error_sz); - } - - // center geom: two times bigger - mjsGeom* geom = mjs_addGeom(body, &def[0].spec); - mjs_setDefault(geom->element, mjs_getDefault(body->element)); - geom->type = mjGEOM_SPHERE; - mju::sprintf_arr(txt, "%sGcenter", prefix.c_str()); - mjs_setString(geom->name, txt); - mjuu_setvec(geom->pos, 0, 0, 0); - geom->size[0] *= 2; - geom->size[1] = 0; - geom->size[2] = 0; - - // fixed tendon for all joints - mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON); - ten->classname = model->Default()->name; - mju::sprintf_arr(txt, "%sT", prefix.c_str()); - ten->name = txt; - - // create bodies, geoms and joints: outside shell only - for (int ix=0; ixname, txt); - - // set body position (+/- 1) - b->pos[0] = 2.0*ix/(count[0]-1) - 1; - b->pos[1] = 2.0*iy/(count[1]-1) - 1; - b->pos[2] = 2.0*iz/(count[2]-1) - 1; - - // reshape - BoxProject(b->pos); - - // reorient body - b->alt.type = mjORIENTATION_ZAXIS; - mjuu_copyvec(b->alt.zaxis, b->pos, 3); - mjuu_normvec(b->alt.zaxis, 3); - - // add geom - mjsGeom* g = mjs_addGeom(b, &def[0].spec); - mjs_setDefault(g->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz); - mjs_setString(g->name, txt); - - // offset inwards, enforce sphere or capsule - if (g->type==mjGEOM_CAPSULE) { - g->pos[2] = -(g->size[0] + g->size[1]); - } else { - g->type = mjGEOM_SPHERE; - g->pos[2] = -g->size[0]; - } - - // add slider joint - mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec); - mjs_setDefault(jnt->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz); - mjs_setString(jnt->name, txt); - jnt->type = mjJNT_SLIDE; - mjuu_setvec(jnt->pos, 0, 0, 0); - mjuu_setvec(jnt->axis, 0, 0, 1); - - // add fix constraint - mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec); - mjs_setDefault(eq->element, &model->Default()->spec); - eq->type = mjEQ_JOINT; - mjs_setString(eq->name1, mjs_getString(jnt->name)); - - // add joint to tendon - ten->WrapJoint(std::string(mjs_getString(jnt->name)), 1); - - // add neighbor constraints - for (int i=0; i<3; i++) { - int ix1 = mjMIN(ix+(i==0), count[0]-1); - int iy1 = mjMIN(iy+(i==1), count[1]-1); - int iz1 = mjMIN(iz+(i==2), count[2]-1); - if ((ix1==0 || ix1==count[0]-1 || - iy1==0 || iy1==count[1]-1 || - iz1==0 || iz1==count[2]-1) && - (ix!=ix1 || iy!=iy1 || iz!=iz1)) { - char txt2[200]; - mju::sprintf_arr(txt2, - "%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1); - mjsEquality* eqn = mjs_addEquality(&model->spec, 0); - mju_copy(eqn->solref, solrefsmooth, mjNREF); - mju_copy(eqn->solimp, solimpsmooth, mjNIMP); - eqn->type = mjEQ_JOINT; - mjs_setString(eqn->name1, txt); - mjs_setString(eqn->name2, txt2); - } - } - } - } - } - } - - // finalize fixed tendon - mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec); - mjs_setDefault(eqt->element, &model->Default()->spec); - eqt->type = mjEQ_TENDON; - mjs_setString(eqt->name1, ten->name.c_str()); - - // skin - if (skin) { - MakeSkin3(model); - } - - return true; -} - - - // add shear tendons to 2D void mjCComposite::MakeShear(mjCModel* model) { char txt[100], txt1[100], txt2[100]; @@ -1838,272 +1654,3 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) { mju_free(D); } - - -// add skin to 3D -void mjCComposite::MakeSkin3(mjCModel* model) { - int vcnt = 0; - std::map vmap; - char txt[100], cnt0[10], cnt1[10], cnt2[10]; - std::string fmt; - - // string counts - mju::sprintf_arr(cnt0, "%d", count[0]-1); - mju::sprintf_arr(cnt1, "%d", count[1]-1); - mju::sprintf_arr(cnt2, "%d", count[2]-1); - - // add skin, set name and material - mjsSkin* skin = mjs_addSkin(&model->spec); - mju::sprintf_arr(txt, "%sSkin", prefix.c_str()); - mjs_setString(skin->name, txt); - mjs_setString(skin->material, skinmaterial.c_str()); - mjuu_copyvec(skin->rgba, skinrgba, 4); - skin->inflate = skininflate; - skin->group = skingroup; - - // box - if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) { - // z-faces - MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0"); - fmt = "%sB%d_%d_" + std::string(cnt2); - MakeSkin3Box(skin, count[0], count[1], 0, vcnt, fmt.c_str()); - - // y-faces - MakeSkin3Box(skin, count[0], count[2], 0, vcnt, "%sB%d_0_%d"); - fmt = "%sB%d_" + std::string(cnt1) + "_%d"; - MakeSkin3Box(skin, count[0], count[2], 1, vcnt, fmt.c_str()); - - // x-faces - MakeSkin3Box(skin, count[1], count[2], 1, vcnt, "%sB0_%d_%d"); - fmt = "%sB" + std::string(cnt0) + "_%d_%d"; - MakeSkin3Box(skin, count[1], count[2], 0, vcnt, fmt.c_str()); - } - - // cylinder - else if (type==mjCOMPTYPE_CYLINDER) { - // generate vertices in map - for (int ix=0; ixbodyname, txt); - bindpos.push_back(0); - bindpos.push_back(0); - bindpos.push_back(0); - bindquat.push_back(1); - bindquat.push_back(0); - bindquat.push_back(0); - bindquat.push_back(0); - - // vertid and vertweight - vertid.push_back({vcnt + i0*c1+i1}); - vertweight.push_back({1}); - } - } - - // update vertex count - vcnt += c0*c1; -} - - - -// make one face of 3D skin, smooth -void mjCComposite::MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side, - const std::map& vmap, - const char* format) { - char txt00[100], txt01[100], txt10[100], txt11[100]; - - // loop over bodies/vertices of specified face - for (int i0=0; i0second); - face.push_back(vmap.find(txt10)->second); - face.push_back(vmap.find(txt11)->second); - - face.push_back(vmap.find(txt00)->second); - face.push_back(vmap.find(txt11)->second); - face.push_back(vmap.find(txt01)->second); - } else { - face.push_back(vmap.find(txt00)->second); - face.push_back(vmap.find(txt01)->second); - face.push_back(vmap.find(txt11)->second); - - face.push_back(vmap.find(txt00)->second); - face.push_back(vmap.find(txt11)->second); - face.push_back(vmap.find(txt10)->second); - } - } - - // bind pose: origin - mjs_appendString(skin->bodyname, txt00); - bindpos.push_back(0); - bindpos.push_back(0); - bindpos.push_back(0); - bindquat.push_back(1); - bindquat.push_back(0); - bindquat.push_back(0); - bindquat.push_back(0); - - // vertid and vertweight - vertid.push_back({vmap.find(txt00)->second}); - vertweight.push_back({1}); - } - } -} diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 446ef1d4..add71636 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -31,9 +31,6 @@ typedef enum _mjtCompType { mjCOMPTYPE_ROPE, mjCOMPTYPE_LOOP, mjCOMPTYPE_CLOTH, - mjCOMPTYPE_BOX, - mjCOMPTYPE_CYLINDER, - mjCOMPTYPE_ELLIPSOID, mjNCOMPTYPES } mjtCompType; @@ -75,7 +72,6 @@ class mjCComposite { bool MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz); bool MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz); bool MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz); - bool MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz); void MakeShear(mjCModel* model); void MakeSkin2(mjCModel* model, mjtNum inflate); @@ -85,13 +81,6 @@ class mjCComposite { void MakeCableBones(mjCModel* model, mjsSkin* skin); void MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin); - void MakeSkin3(mjCModel* model); - void MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side, int& vcnt, const char* format); - void MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side, - const std::map& vmap, const char* format); - - void BoxProject(double* pos); - // common properties std::string prefix; // name prefix mjtCompType type; // composite type diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 732d87ba..67443e22 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -763,6 +763,7 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { // make 3d box, ellipsoid or cylinder bool mjCFlexcomp::MakeBox(char* error, int error_sz) { double pos[3]; + bool needtex = texcoord.empty() && !std::string(mjs_getString(def.spec.flex->material)).empty(); // set 3D def.spec.flex->dim = 3; @@ -772,6 +773,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { point.push_back(0); point.push_back(0); + // add texture coordinates, if not specified explicitly + if (needtex) { + texcoord.push_back(0); + texcoord.push_back(0); + } + // iz=0/max for (int iz=0; iz < count[2]; iz+=count[2]-1) { for (int ix=0; ix < count[0]; ix++) { @@ -782,6 +789,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { point.push_back(pos[1]); point.push_back(pos[2]); + // add texture coordinates, if not specified explicitly + if (needtex) { + texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); + texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); + } + // add elements if (ix < count[0]-1 && iy < count[1]-1) { element.push_back(0); @@ -808,6 +821,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(pos[2]); + + // add texture coordinates + if (needtex) { + texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); + texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); + } } // add elements @@ -836,6 +855,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(pos[2]); + + // add texture coordinates + if (needtex) { + texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); + texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); + } } // add elements diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 257a394a..94b3ec3d 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1613,6 +1613,7 @@ void mjCModel::SetSizes() { nflexelemedge += flexes_[i]->nelem * mjCFlex::kNumEdges[flexes_[i]->dim - 1]; nflexshelldata += (int)flexes_[i]->shell.size(); nflexevpair += (int)flexes_[i]->evpair.size()/2; + nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); } // mesh counts diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 0037a77a..9a8ad20d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -742,10 +742,7 @@ const mjMap comp_map[mjNCOMPTYPES] = { {"rope", mjCOMPTYPE_ROPE}, {"loop", mjCOMPTYPE_LOOP}, {"cable", mjCOMPTYPE_CABLE}, - {"cloth", mjCOMPTYPE_CLOTH}, - {"box", mjCOMPTYPE_BOX}, - {"cylinder", mjCOMPTYPE_CYLINDER}, - {"ellipsoid", mjCOMPTYPE_ELLIPSOID} + {"cloth", mjCOMPTYPE_CLOTH} }; diff --git a/test/engine/testdata/skingroup.xml b/test/engine/testdata/skingroup.xml index 93053736..6a2da654 100644 --- a/test/engine/testdata/skingroup.xml +++ b/test/engine/testdata/skingroup.xml @@ -27,17 +27,15 @@ - - + - + - - + - + diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc index 1d0dabf1..72f2297a 100644 --- a/test/user/user_composite_test.cc +++ b/test/user/user_composite_test.cc @@ -43,7 +43,7 @@ TEST_F(UserCompositeTest, MultipleJointsNotAllowedUnlessParticle) { - + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 4a287b6e..219ef5e1 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1681,13 +1681,13 @@ TEST_F(XMLReaderTest, ReadsSkinGroups) { - + - + @@ -1698,8 +1698,8 @@ TEST_F(XMLReaderTest, ReadsSkinGroups) { std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, NotNull()); - int geomid1 = mj_name2id(model, mjOBJ_GEOM, "B0G0_0_0"); - int geomid2 = mj_name2id(model, mjOBJ_GEOM, "B1G0_0_0"); + int geomid1 = mj_name2id(model, mjOBJ_GEOM, "B0G0_0"); + int geomid2 = mj_name2id(model, mjOBJ_GEOM, "B1G0_0"); EXPECT_THAT(model->geom_group[geomid1], 2); EXPECT_THAT(model->skin_group[0], 4); EXPECT_THAT(model->geom_group[geomid2], 4); @@ -1712,7 +1712,7 @@ TEST_F(XMLReaderTest, InvalidSkinGroup) { - + From efa5709c9410fd70528387b58d78ab921a68f9d0 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 3 Nov 2024 04:17:43 -0800 Subject: [PATCH 054/426] Fix broken link in `XMLreference`. PiperOrigin-RevId: 692669798 Change-Id: I8f61d1032d11c63945264a1b9dc1c034b7401d98 --- doc/XMLreference.rst | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 69c8751a..16ce21a0 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6184,13 +6184,12 @@ excluded; this is because sensor calculations are independent of the visualizer. :el-prefix:`sensor/` |-| **camprojection** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates a camprojection sensor, which returns the location of a target site, projected onto a camera image -in pixel coordinates. The origin of this system is located at the top-left corner of the first pixel, so a target -which projects exactly onto the corner of the image, will have value (0, 0). Values are not clipped, so targets which -fall outside the camera image will take values above or below the pixel limits. Moreover, points behind the camera -are also projected onto the image, so it is up to the user to filter out such points, if desired. This can be done using -a `framepos` sensor with the camera as reference frame, then a negative/positive value in the -z-coordinate indicates (respectively) a location in the front/back of the camera. +This element creates a camera projection sensor: the location of a target site, projected onto a camera image in pixel +coordinates. The pixel origin (0, 0) is located at the top-left corner. Values are not clipped, so targets which fall +outside the camera image will take values above or below the pixel range limits. Moreover, points behind the camera are +also projected onto the image, so it is up to the user to filter out such points, if desired. This can be done using a +:ref:`framepos` sensor with the camera as a reference frame: a negative/positive value in the +z-coordinate indicates a location in front of/behind the camera plane, respectively. .. _sensor-camprojection-site: From c168ac3397d36f7949aeb5194d0da9145fde7aab Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Sun, 3 Nov 2024 11:09:03 -0800 Subject: [PATCH 055/426] Add source code links in docs for remaining API functions. PiperOrigin-RevId: 692728907 Change-Id: I145eb47453e6aa017505d53d3e17215959252e3d --- doc/js/linenumbers.js | 20 ++++++++++++++++++++ src/render/render_context.c | 2 +- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/doc/js/linenumbers.js b/doc/js/linenumbers.js index cdea9af8..818728e1 100644 --- a/doc/js/linenumbers.js +++ b/doc/js/linenumbers.js @@ -97,6 +97,26 @@ class LineNumbers { this.map.set(key, `https://github.com/google-deepmind/mujoco/blob/main/src/${src}#L${i+1}`); } } + + // edge cases + if (src == 'user/user_api.cc') { + for (let i = 0; i < lines.length; i++) { + if (lines[i].startsWith('[[nodiscard]] int mj_recompile(')) { + const key = 'mj_recompile'; + this.map.set(key, `https://github.com/google-deepmind/mujoco/blob/main/src/${src}#L${i+1}`); + } + } + } else if (src == 'engine/engine_io.c') { + for (let i = 0; i < lines.length; i++) { + if (lines[i].startsWith('void mj_freeStack(')) { + const key = 'mj_freeStack'; + this.map.set(key, `https://github.com/google-deepmind/mujoco/blob/main/src/${src}#L${i+1}`); + } else if (lines[i].startsWith('void mj_markStack(')) { + const key = 'mj_markStack'; + this.map.set(key, `https://github.com/google-deepmind/mujoco/blob/main/src/${src}#L${i+1}`); + } + } + } } } diff --git a/src/render/render_context.c b/src/render/render_context.c index 653ab3ac..4c6c3879 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -1864,7 +1864,7 @@ void mjr_freeContext(mjrContext* con) { // resize offscreen buffers -MJAPI void mjr_resizeOffscreen(int width, int height, mjrContext* con) { +void mjr_resizeOffscreen(int width, int height, mjrContext* con) { if (con->offWidth == width && con->offHeight == height) { return; } From 298ce31e398cc5cb6aec980aeb14fe79e98ccd48 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Nov 2024 04:33:59 -0800 Subject: [PATCH 056/426] Update docs to highlight `nativeccd` PiperOrigin-RevId: 692918061 Change-Id: I9e44bc3123fe6b8a41b769c7de01b20e3c56d758 --- doc/changelog.rst | 10 ++++++++-- doc/modeling.rst | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index a414cd39..bee62085 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,11 +5,17 @@ Changelog Upcoming version (not yet released) ----------------------------------- +Feature promotion +^^^^^^^^^^^^^^^^^ +- The :doc:`Model Editing` framework afforded by :ref:`mjSpec`, introduced in 3.2.0 as an + in-development feature, is now stable and recommended for general use. +- The native convex collision detection pipeline introduced in 3.2.3 and enabled by the + :ref:`nativeccd` flag, is not yet the default but is already recommended for general use. + Please try it when encountering collision-related problems and report any issues you encounter. + General ^^^^^^^ -- The :doc:`Model Editing` framework afforded by :ref:`mjSpec`, introduced in 3.2.0 as an - in-development feature, is now stable and recommended for general use. - The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific :ref:`inertia` attribute. - The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. diff --git a/doc/modeling.rst b/doc/modeling.rst index 9f3bc202..b68d5e99 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1672,6 +1672,8 @@ better visualize and understand the contact configuration and resulting forces. geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components. b. If contacts are between flat surfaces, try enabling the :ref:`multiccd` flag, which allows the detector to find more contacts than the single contact returned by the convex-convex collider. + c. Try enabling the native collision detection pipeline by setting the :ref:`nativeccd` flag, + which uses a more accurate and efficient convex collision detection algorithm. **High-frequency vibration** High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in From b7bbb2b25c7bd6c87683e2f6340978ef9860b9d1 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 4 Nov 2024 11:18:59 -0800 Subject: [PATCH 057/426] Update the MuJoCo changelog for the 3.2.5 release PiperOrigin-RevId: 693032354 Change-Id: I675424a3feeda8037a9559753dc93c05b812257c --- doc/changelog.rst | 58 +++++++++++++++++++++++------------------------ 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index bee62085..ccad2ea6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,52 +2,52 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.2.5 (Nov 4, 2024) +--------------------------- Feature promotion ^^^^^^^^^^^^^^^^^ -- The :doc:`Model Editing` framework afforded by :ref:`mjSpec`, introduced in 3.2.0 as an - in-development feature, is now stable and recommended for general use. -- The native convex collision detection pipeline introduced in 3.2.3 and enabled by the - :ref:`nativeccd` flag, is not yet the default but is already recommended for general use. - Please try it when encountering collision-related problems and report any issues you encounter. +1. The :doc:`Model Editing` framework afforded by :ref:`mjSpec`, introduced in 3.2.0 as an + in-development feature, is now stable and recommended for general use. +2. The native convex collision detection pipeline introduced in 3.2.3 and enabled by the + :ref:`nativeccd` flag, is not yet the default but is already recommended for general use. + Please try it when encountering collision-related problems and report any issues you encounter. General ^^^^^^^ -- The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific - :ref:`inertia` attribute. -- The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. -- Removed the deprecated ``mju_rotVecMat``, ``mju_rotVecMatT`` and ``mjv_makeConnector`` functions. -- Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). -- The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single - :ref:`layer` sub-element. -- The composite types box, cylinder, and sphere have been removed. Users should instead use the equivalent types - available in :ref:`flexcomp`. +3. The global compiler flag ``exactmeshinertia`` has been removed and replaced with the mesh-specific + :ref:`inertia` attribute. +4. The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. +5. Removed the deprecated ``mju_rotVecMat``, ``mju_rotVecMatT`` and ``mjv_makeConnector`` functions. +6. Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). +7. The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single + :ref:`layer` sub-element. +8. The composite types box, cylinder, and sphere have been removed. Users should instead use the equivalent types + available in :ref:`flexcomp`. MJX ^^^ -- Added ``apply_ft``, ``jac``, and ``xfrc_accumulate`` as public functions. -- Added ``TOUCH`` sensor. -- Added support for ``eq_active``. Fixes :github:issue:`2173`. -- Added ray intersection with ellipsoid. +9. Added ``apply_ft``, ``jac``, and ``xfrc_accumulate`` as public functions. +10. Added ``TOUCH`` sensor. +11. Added support for ``eq_active``. Fixes :github:issue:`2173`. +12. Added ray intersection with ellipsoid. Bug fixes ^^^^^^^^^ -- Fixed several bugs related to connect and weld constraints with site semantics (fixes :github:issue:`2179`, reported - by :github:user:`yinfanyi`). The introduction of site specification to connects and welds in 3.2.3 conditionally - changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`, but these changes were not properly propagated in - several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and - runtime enabling/disabling of such constraints. -- Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. -- Fixed a bug in flex texture coordinates that prevented the correct allocation of textures in mjModel. +13. Fixed several bugs related to connect and weld constraints with site semantics (fixes :github:issue:`2179`, reported + by :github:user:`yinfanyi`). The introduction of site specification to connects and welds in 3.2.3 conditionally + changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`, but these changes were not properly propagated in + several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and + runtime enabling/disabling of such constraints. +14. Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. +15. Fixed a bug in flex texture coordinates that prevented the correct allocation of textures in mjModel. Documentation ^^^^^^^^^^^^^ -- Function headers in the :doc:`API reference <../APIreference/APIfunctions>` now link to their source definitions - in GitHub. +16. Function headers in the :doc:`API reference <../APIreference/APIfunctions>` now link to their source definitions + in GitHub. Version 3.2.4 (Oct 15, 2024) ---------------------------- From f9569cdab0d3f56d37f7ede598563becf464c9e7 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Tue, 5 Nov 2024 04:16:20 -0800 Subject: [PATCH 058/426] Fix #2212. PiperOrigin-RevId: 693294401 Change-Id: Ifef2b268f61b90f557580f61316786c50dc9698d --- doc/changelog.rst | 8 ++++++++ mjx/mujoco/mjx/_src/io.py | 6 +++--- mjx/mujoco/mjx/_src/io_test.py | 20 ++++++++++++++++++++ 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index ccad2ea6..4d787323 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,14 @@ Changelog ========= + +Upcoming version (not yet released) +----------------------------------- + +Bug fixes +^^^^^^^^^ +1. Fixed :github:issue:`2212`, type error in ```mjx.get_data``. + Version 3.2.5 (Nov 4, 2024) --------------------------- diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 28b84bbe..200d742e 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -432,9 +432,9 @@ def get_data_into( # MuJoCo actuator_moment is sparse, MJX uses a dense representation. if field.name == 'actuator_moment' and m.nu: - moment_rownnz = np.zeros(m.nu, dtype=int) - moment_rowadr = np.zeros(m.nu, dtype=int) - moment_colind = np.zeros(m.nu * m.nv, dtype=int) + moment_rownnz = np.zeros(m.nu, dtype=np.int32) + moment_rowadr = np.zeros(m.nu, dtype=np.int32) + moment_colind = np.zeros(m.nu * m.nv, dtype=np.int32) actuator_moment = np.zeros(m.nu * m.nv) mujoco.mju_dense2sparse( actuator_moment, diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index d5905b86..cbb10ebe 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -422,6 +422,26 @@ class DataIOTest(parameterized.TestCase): np.testing.assert_allclose(d_2.efc_aref, d.efc_aref) np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address) + def test_get_data_runs(self): + xml = """ + + + + + + + + + + + + + """ + m = mujoco.MjModel.from_xml_string(xml) + d = mujoco.MjData(m) + dx = mjx.put_data(m, d) + mjx.get_data(m, dx) + def test_get_data_batched(self): """Test that get_data makes correct List[MjData] for batched Data.""" From 276f5b04cd32ae585c3c08fcabe9e2a1b716d7ff Mon Sep 17 00:00:00 2001 From: Kevin Sayed Date: Tue, 5 Nov 2024 14:12:19 -0800 Subject: [PATCH 059/426] Bump MuJoCo version to 3.2.6. PiperOrigin-RevId: 693472991 Change-Id: Ic202f52dd42c55c43ba05ca21b5cc6a4024b0c72 --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f5e3df1..4040915e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.2.5 + VERSION 3.2.6 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 9fec766a..dc746b79 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,2,5,0 -PRODUCTVERSION 3,2,5,0 +FILEVERSION 3,2,6,0 +PRODUCTVERSION 3,2,6,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.5" + VALUE "ProductVersion", "3.2.6" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.5" + VALUE "FileVersion", "3.2.6" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 2fbbf3aa..3da13b43 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,2,5,0 -PRODUCTVERSION 3,2,5,0 +FILEVERSION 3,2,6,0 +PRODUCTVERSION 3,2,6,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.5" + VALUE "ProductVersion", "3.2.6" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.5" + VALUE "FileVersion", "3.2.6" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 91a506b7..408aee24 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -517,7 +517,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - Maximum number of UI rectangles. Defined in `mjui.h `_. * - ``mjVERSION_HEADER`` - - 325 + - 326 - The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a number with the same meaning but for the compiled library. diff --git a/doc/unity.rst b/doc/unity.rst index b709d0a1..cb2c2517 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -30,14 +30,14 @@ _____ The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as a trusted binary. Then, copy the dynamic library file from -``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.5.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.6.dylib`` (it can be found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``. Linux _____ Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from -``~/.mujoco/mujoco-3.2.5/lib/libmujoco.so.3.2.5`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.2.6/lib/libmujoco.so.3.2.6`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 2fedda8f..7a38575d 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -16,7 +16,7 @@ #define MUJOCO_MUJOCO_H_ // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 325 +#define mjVERSION_HEADER 326 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index 403f38cb..df714f31 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.2.5" +version = "3.2.6" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.2.5.dev0", + "mujoco>=3.2.6.dev0", "scipy", "trimesh", ] @@ -41,6 +41,6 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.2.5" +Documentation = "https://mujoco.readthedocs.io/en/3.2.6" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.2.5/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.6/changelog.html" diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index e71b1f97..546cdf63 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -84,7 +84,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.5.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.6.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -92,7 +92,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.2.5 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.2.6 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 7468eeac..98e2686e 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.2.5 + 3.2.6 CFBundleGetInfoString - 3.2.5 + 3.2.6 CFBundleLongVersionString - 3.2.5 + 3.2.6 CFBundleShortVersionString - 3.2.5 + 3.2.6 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 887f78e2..49827899 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.2.5" +version = "3.2.6" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.2.5" +Documentation = "https://mujoco.readthedocs.io/en/3.2.6" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.2.5/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.6/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index d3579bbd..70120f7f 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.2.5 + VERSION 3.2.6 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 487b1b16..99af5676 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.2.5 + VERSION 3.2.6 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 2d743006..5b90a705 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -42,8 +42,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 325 -#define mjVERSIONSTRING "3.2.5" + #define mjVERSION 326 +#define mjVERSIONSTRING "3.2.6" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index edf4df86..aba4932d 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.2.5.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.2.6.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.2.5/lib/libmujoco.so.3.2.5", + "/.mujoco/mujoco-3.2.6/lib/libmujoco.so.3.2.6", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 906c2c7f..32e58267 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -109,7 +109,7 @@ public const int mjMAXLINEPNT = 1000; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 325; +public const int mjVERSION_HEADER = 326; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 4c8dc9a0..ee54ac8c 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.2.5", + "version": "3.2.6", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From a2a0b95de01eebf08fa83e5f895a8212d3410a90 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 6 Nov 2024 04:49:45 -0800 Subject: [PATCH 060/426] Print arena memory usage in testspeed PiperOrigin-RevId: 693681047 Change-Id: Ica5b516d0141199f701567d64e2ea1446491e232 --- sample/testspeed.cc | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 09757888..6d8f55bc 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -186,13 +186,19 @@ int main(int argc, char** argv) { mjcb_time = gettm; // print start - std::printf("\nRolling out %d steps%s, at dt = %g", + std::printf("\nRolling out %d steps%s at dt = %g", nstep, nthread > 1 ? " per thread" : "", m->opt.timestep); + + // print precision if (sizeof(mjtNum) == 4) { - std::printf(", using single-precision"); + std::printf(", using single precision"); + } else { + std::printf(", using double precision"); } + + // print threadpool size if (npoolthread > 1) { std::printf(", using %d threads", npoolthread); } @@ -231,7 +237,10 @@ int main(int argc, char** argv) { std::printf(" Time per step : %.1f %ss\n\n", 1e6*simtime[0]/nstep, mu_str); std::printf(" Contacts per step : %.2f\n", static_cast(contacts[0])/nstep); std::printf(" Constraints per step : %.2f\n", static_cast(constraints[0])/nstep); - std::printf(" Degrees of freedom : %d\n\n", m->nv); + std::printf(" Degrees of freedom : %d\n", m->nv); + std::printf(" Memory usage : %.1f%% of %s\n\n", + 100 * d[0]->maxuse_arena / (double)(d[0]->narena), + mju_writeNumBytes(d[0]->narena)); // profiler, top-level printf(" Internal profiler%s, %ss per step\n", nthread > 1 ? " for thread 0" : "", mu_str); From 18f88efe17e3c30ca34892729e3ba86bc3c04f1a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 6 Nov 2024 14:45:40 -0800 Subject: [PATCH 061/426] Contact visualization: if penetration is deeper than height, extend cylinder to show computed depth. PiperOrigin-RevId: 693869892 Change-Id: I1c873e3803286d6a636336246f498552617e978b --- src/engine/engine_vis_visualize.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 10396a0b..5f619fdd 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -141,7 +141,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, START thisgeom->type = mjGEOM_CYLINDER; thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl; - thisgeom->size[2] = m->vis.scale.contactheight * scl; + float halfheight = m->vis.scale.contactheight * scl; + float halfdepth = -con->dist / 2; + thisgeom->size[2] = mjMAX(halfheight, halfdepth); mju_n2f(thisgeom->pos, con->pos, 3); mju_n2f(thisgeom->mat, mat, 9); From d6d3d6bb25bdb7c142f4ee29d586ce1d3cc28f62 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 7 Nov 2024 00:18:41 -0800 Subject: [PATCH 062/426] Fix typo in changelog. PiperOrigin-RevId: 694009875 Change-Id: I0ecafe467347a54b54c29c486b98516d1a53e5ea --- doc/changelog.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 4d787323..3ccaccc1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,7 +8,7 @@ Upcoming version (not yet released) Bug fixes ^^^^^^^^^ -1. Fixed :github:issue:`2212`, type error in ```mjx.get_data``. +- Fixed :github:issue:`2212`, type error in ``mjx.get_data``. Version 3.2.5 (Nov 4, 2024) --------------------------- @@ -45,9 +45,9 @@ Bug fixes ^^^^^^^^^ 13. Fixed several bugs related to connect and weld constraints with site semantics (fixes :github:issue:`2179`, reported by :github:user:`yinfanyi`). The introduction of site specification to connects and welds in 3.2.3 conditionally - changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`, but these changes were not properly propagated in - several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and - runtime enabling/disabling of such constraints. + changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`, but these changes were not properly propagated + in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors + and runtime enabling/disabling of such constraints. 14. Fixed a bug in slider-crank :ref:`transmission`. The bug was introduced in 3.0.0. 15. Fixed a bug in flex texture coordinates that prevented the correct allocation of textures in mjModel. From 414b677eb8983e2d20e6f9cb6b23ef2923922583 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 7 Nov 2024 05:55:04 -0800 Subject: [PATCH 063/426] print memory footprint of model and data PiperOrigin-RevId: 694088011 Change-Id: I3b743df2a59f554414b440aebfa6bf0d0e29b2d8 --- src/engine/engine_print.c | 98 +++++++++++++++++++++++++++++++++++---- 1 file changed, 88 insertions(+), 10 deletions(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 96b41546..2ffd4c24 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -27,6 +27,7 @@ #include "engine/engine_core_constraint.h" #include "engine/engine_io.h" #include "engine/engine_name.h" +#include "engine/engine_macro.h" #include "engine/engine_support.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -163,7 +164,58 @@ static void printVector(const char* str, const mjtNum* data, int n, FILE* fp, -//------------------------------ printing functions ------------------------------------------------ +// print human readable memory size +static const char* memorySize(size_t nbytes) { + static mjTHREADLOCAL char message[20]; + int k = 1024; + + if (nbytes < k) { + snprintf(message, sizeof(message), "%5zu bytes", nbytes); + } else if (nbytes < k*k) { + snprintf(message, sizeof(message), "%5.1f KB", (double)nbytes / k); + } else if (nbytes < k*k*k) { + snprintf(message, sizeof(message), "%5.1f MB", (double)nbytes / (k*k)); + } else { + snprintf(message, sizeof(message), "%5.1f GB", (double)nbytes / (k*k*k)); + } + + return message; +} + + + +// return memory footprint of all significant mesh-related arrays +static size_t sizeMesh(const mjModel* m) { + size_t nbytes = 0; + nbytes += sizeof(float) * 3*m->nmeshvert; // mesh_vert + nbytes += sizeof(float) * 3*m->nmeshnormal; // mesh_normal + nbytes += sizeof(float) * 2*m->nmeshtexcoord; // mesh_texcoord + nbytes += sizeof(int) * 3*m->nmeshface; // mesh_face + nbytes += sizeof(int) * 3*m->nmeshface; // mesh_facenormal + nbytes += sizeof(int) * 3*m->nmeshface; // mesh_facetexcoord + nbytes += sizeof(int) * m->nmeshgraph; // mesh_graph + return nbytes; +} + + + +// return memory footprint of all significant skin-related arrays +static size_t sizeSkin(const mjModel* m) { + size_t nbytes = 0; + nbytes += sizeof(float) * 3*m->nskinvert; // skin_vert + nbytes += sizeof(float) * 2*m->nskintexvert; // skin_texcoord + nbytes += sizeof(int) * 3*m->nskinface; // skin_face + nbytes += sizeof(int) * m->nskinbone; // skin_bonevertadr + nbytes += sizeof(int) * m->nskinbone; // skin_bonevertnum + nbytes += sizeof(float) * 3*m->nskinbone; // skin_bonebindpos + nbytes += sizeof(float) * 4*m->nskinbone; // skin_bonebindquat + nbytes += sizeof(int) * m->nskinbone; // skin_bonebodyid + nbytes += sizeof(int) * m->nskinbonevert; // skin_bonevertid + nbytes += sizeof(float) * m->nskinbonevert; // skin_bonevertweight + return nbytes; +} + + // return whether float_format is a valid format string for a single float static bool validateFloatFormat(const char* float_format) { @@ -240,6 +292,7 @@ static bool validateFloatFormat(const char* float_format) { #pragma clang diagnostic ignored "-Wuninitialized" #endif +//------------------------------ printing functions ------------------------------------------------ // print mjModel to text file, specifying format. float_format must be a // valid printf-style format string for a single float value @@ -274,14 +327,30 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* fprintf(fp, "MuJoCo version %s\n", mj_versionString()); fprintf(fp, "model name %s\n\n", m->names); + // memory footprint + fprintf(fp, "MEMORY\n"); + fprintf(fp, " total %s\n", memorySize(mj_sizeModel(m))); + if (m->nmesh) { + fprintf(fp, " meshes %s\n", memorySize(sizeMesh(m))); + } + if (m->ntex) { + fprintf(fp, " textures %s\n", memorySize(m->ntexdata)); + } + if (m->nskin) { + fprintf(fp, " skins %s\n", memorySize(sizeSkin(m))); + } + fprintf(fp, "\n"); + + // sizes + fprintf(fp, "SIZES\n"); #define X( name ) \ if (m->name) { \ const char* format = _Generic( \ m->name, \ size_t : SIZE_T_FORMAT, \ default : INT_FORMAT); \ - fprintf(fp, NAME_FORMAT, #name); \ + fprintf(fp, NAME_FORMAT, " " #name); \ fprintf(fp, format, m->name); \ fprintf(fp, "\n"); \ } @@ -291,8 +360,9 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* fprintf(fp, "\n"); // scalar options + fprintf(fp, "OPTION\n"); #define X( type, name ) \ - fprintf(fp, NAME_FORMAT, #name); \ + fprintf(fp, NAME_FORMAT, " " #name); \ fprintf(fp, float_format, m->opt.name); \ fprintf(fp, "\n"); @@ -300,7 +370,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* #undef X #define X( type, name ) \ - fprintf(fp, NAME_FORMAT, #name); \ + fprintf(fp, NAME_FORMAT, " " #name); \ fprintf(fp, INT_FORMAT "\n", m->opt.name); MJOPTION_INTS @@ -308,7 +378,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // vector options #define X( name, sz ) \ - fprintf(fp, NAME_FORMAT, #name); \ + fprintf(fp, NAME_FORMAT, " " #name); \ for (int i=0; i < sz; i++) { \ fprintf(fp, float_format, m->opt.name[i]); \ fprintf(fp, " "); \ @@ -325,19 +395,20 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* fprintf(fp, "\n\n"); // statistics - fprintf(fp, NAME_FORMAT, "meaninertia"); + fprintf(fp, "STATISTIC\n"); + fprintf(fp, NAME_FORMAT, " meaninertia"); fprintf(fp, float_format, m->stat.meaninertia); fprintf(fp, "\n"); - fprintf(fp, NAME_FORMAT, "meanmass"); + fprintf(fp, NAME_FORMAT, " meanmass"); fprintf(fp, float_format, m->stat.meanmass); fprintf(fp, "\n"); - fprintf(fp, NAME_FORMAT, "meansize"); + fprintf(fp, NAME_FORMAT, " meansize"); fprintf(fp, float_format, m->stat.meansize); fprintf(fp, "\n"); - fprintf(fp, NAME_FORMAT, "extent"); + fprintf(fp, NAME_FORMAT, " extent"); fprintf(fp, float_format, m->stat.extent); fprintf(fp, "\n"); - fprintf(fp, NAME_FORMAT, "center"); + fprintf(fp, NAME_FORMAT, " center"); fprintf(fp, float_format, m->stat.center[0]); fprintf(fp, float_format, m->stat.center[1]); fprintf(fp, float_format, m->stat.center[2]); @@ -828,6 +899,13 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, __msan_copy_shadow(shadow, d->buffer, d->nbuffer); __msan_unpoison(d->buffer, d->nbuffer); #endif + + fprintf(fp, "MEMORY\n"); + fprintf(fp, " total %s\n", memorySize(sizeof(mjData) + d->nbuffer + d->narena)); + fprintf(fp, " struct %s\n", memorySize(sizeof(mjData))); + fprintf(fp, " buffer %s\n", memorySize(d->nbuffer)); + fprintf(fp, " arena %s\n\n", memorySize(d->narena)); + // ---------------------------------- print mjData fields fprintf(fp, "SIZES\n"); From 0f381a9ebd6beca19cff5e42298b2b86e2940880 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 11 Nov 2024 07:45:11 -0800 Subject: [PATCH 064/426] Add muscle actuators to MJX. PiperOrigin-RevId: 695334356 Change-Id: I6590ed6bdd4a8adc53d5c4e2641f472f7439cedc --- doc/changelog.rst | 4 + doc/mjx.rst | 12 +- mjx/mujoco/mjx/_src/forward.py | 12 +- mjx/mujoco/mjx/_src/forward_test.py | 41 ++-- mjx/mujoco/mjx/_src/support.py | 147 +++++++++++++ mjx/mujoco/mjx/_src/support_test.py | 205 ++++++++++++++++++ mjx/mujoco/mjx/_src/types.py | 12 +- mjx/mujoco/mjx/test_data/actuator/arm21.xml | 37 ++++ mjx/mujoco/mjx/test_data/actuator/arm26.xml | 118 ++++++++++ .../test_data/actuator/general_dyntype.xml | 18 ++ 10 files changed, 566 insertions(+), 40 deletions(-) create mode 100644 mjx/mujoco/mjx/test_data/actuator/arm21.xml create mode 100644 mjx/mujoco/mjx/test_data/actuator/arm26.xml create mode 100644 mjx/mujoco/mjx/test_data/actuator/general_dyntype.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index 3ccaccc1..87225bea 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,6 +6,10 @@ Changelog Upcoming version (not yet released) ----------------------------------- +MJX +^^^ +- Added muscle actuators. + Bug fixes ^^^^^^^^^ - Fixed :github:issue:`2212`, type error in ``mjx.get_data``. diff --git a/doc/mjx.rst b/doc/mjx.rst index 49435bc6..87dfd9e3 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -214,11 +214,11 @@ The following features are **fully supported** in MJX: * - :ref:`Transmission ` - ``JOINT``, ``JOINTINPARENT``, ``SITE``, ``TENDON`` * - :ref:`Actuator Dynamics ` - - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT`` + - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT``, ``MUSCLE`` * - :ref:`Actuator Gain ` - - ``FIXED``, ``AFFINE`` + - ``FIXED``, ``AFFINE``, ``MUSCLE`` * - :ref:`Actuator Bias ` - - ``NONE``, ``AFFINE`` + - ``NONE``, ``AFFINE``, ``MUSCLE`` * - :ref:`Tendon Wrapping ` - ``JOINT``, ``SITE``, ``PULLEY`` * - :ref:`Geom ` @@ -264,12 +264,6 @@ The following features are **in development** and coming soon: - ``IMPLICIT`` * - Dynamics - :ref:`Inverse ` - * - :ref:`Actuator Dynamics ` - - ``MUSCLE`` - * - :ref:`Actuator Gain ` - - ``MUSCLE`` - * - :ref:`Actuator Bias ` - - ``MUSCLE`` * - :ref:`Tendon Wrapping ` - ``SPHERE``, ``CYLINDER`` * - Fluid Model diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index c187d685..07f33a90 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -115,6 +115,8 @@ def fwd_actuation(m: Model, d: Data) -> Data: act_dot = ctrl elif dyn_typ in (DynType.FILTER, DynType.FILTEREXACT): act_dot = (ctrl - act) / jp.clip(dyn_prm[0], mujoco.mjMINVAL) + elif dyn_typ == DynType.MUSCLE: + act_dot = support.muscle_dynamics(ctrl, act, dyn_prm) else: raise NotImplementedError(f'dyntype {dyn_typ.name} not implemented.') return act_dot @@ -139,13 +141,15 @@ def fwd_actuation(m: Model, d: Data) -> Data: ctrl_act = jp.where(m.actuator_actadr == -1, ctrl, act_last_dim) def get_force(*args): - gain_t, gain_p, bias_t, bias_p, len_, vel, ctrl_act = args + gain_t, gain_p, bias_t, bias_p, len_, vel, ctrl_act, len_range, acc0 = args typ, prm = GainType(gain_t), gain_p if typ == GainType.FIXED: gain = prm[0] elif typ == GainType.AFFINE: gain = prm[0] + prm[1] * len_ + prm[2] * vel + elif typ == GainType.MUSCLE: + gain = support.muscle_gain(len_, vel, len_range, acc0, prm) else: raise RuntimeError(f'unrecognized gaintype {typ.name}.') @@ -153,13 +157,15 @@ def fwd_actuation(m: Model, d: Data) -> Data: bias = jp.array(0.0) if typ == BiasType.AFFINE: bias = prm[0] + prm[1] * len_ + prm[2] * vel + elif typ == BiasType.MUSCLE: + bias = support.muscle_bias(len_, len_range, acc0, prm) return gain * ctrl_act + bias force = scan.flat( m, get_force, - 'uuuuuuu', + 'uuuuuuuuu', 'u', m.actuator_gaintype, m.actuator_gainprm, @@ -168,6 +174,8 @@ def fwd_actuation(m: Model, d: Data) -> Data: d.actuator_length, d.actuator_velocity, ctrl_act, + jp.array(m.actuator_lengthrange), + jp.array(m.actuator_acc0), group_by='u', ) forcerange = jp.where( diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index 7c669062..ebe94ebe 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -15,6 +15,7 @@ """Tests for forward functions.""" from absl.testing import absltest +from absl.testing import parameterized import jax import mujoco from mujoco import mjx @@ -167,40 +168,28 @@ class ForwardTest(absltest.TestCase): np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) -class ActuatorTest(absltest.TestCase): - _DYN_XML = """ - - - - - - - - - - - - - - - - - - - """ +class ActuatorTest(parameterized.TestCase): - def test_dyntype(self): - m = mujoco.MjModel.from_xml_string(self._DYN_XML) + @parameterized.parameters( + 'actuator/arm21.xml', + 'actuator/arm26.xml', + 'actuator/general_dyntype.xml', + ) + def test_actuator(self, fname): + m = test_util.load_test_file(fname) d = mujoco.MjData(m) - d.ctrl = np.array([1.5, 1.5, 1.5, 1.5]) - d.act = np.array([0.5, 0.5, 0.5]) - + mujoco.mj_step(m, d) + d.ctrl = 1.5 * np.random.random(m.nu) + d.act = 0.5 * np.random.random(m.na) mx = mjx.put_model(m) dx = mjx.put_data(m, d) mujoco.mj_fwdActuation(m, d) dx = jax.jit(mjx.fwd_actuation)(mx, dx) + _assert_attr_eq(d, dx, 'act_dot') + _assert_attr_eq(d, dx, 'qfrc_actuator') + _assert_attr_eq(d, dx, 'actuator_force') mujoco.mj_Euler(m, d) dx = jax.jit(mjx.euler)(mx, dx) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index f065d9d8..1f3c8137 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -560,3 +560,150 @@ def wrap( wpnt1 = jp.where(invalid, jp.zeros(3), wpnt1) return wlen, wpnt0, wpnt1 + + +def muscle_gain_length( + length: jax.Array, lmin: jax.Array, lmax: jax.Array +) -> jax.Array: + """Normalized muscle length-gain curve.""" + # mid-ranges (maximum is at 1.0) + a = 0.5 * (lmin + 1) + b = 0.5 * (1 + lmax) + + out0 = 0.5 * jp.square( + (length - lmin) / jp.maximum(mujoco.mjMINVAL, a - lmin) + ) + out1 = 1 - 0.5 * jp.square((1 - length) / jp.maximum(mujoco.mjMINVAL, 1 - a)) + out2 = 1 - 0.5 * jp.square((length - 1) / jp.maximum(mujoco.mjMINVAL, b - 1)) + out3 = 0.5 * jp.square( + (lmax - length) / jp.maximum(mujoco.mjMINVAL, lmax - b) + ) + + out = jp.where(length <= b, out2, out3) + out = jp.where(length <= 1, out1, out) + out = jp.where(length <= a, out0, out) + out = jp.where((lmin <= length) & (length <= lmax), out, 0.0) + + return out + + +def muscle_gain( + length: jax.Array, + vel: jax.Array, + lengthrange: jax.Array, + acc0: jax.Array, + prm: jax.Array, +) -> jax.Array: + """Muscle active force.""" + # unpack parameters + lrange = prm[:2] + force, scale, lmin, lmax, vmax, _, fvmax = prm[2:9] + + force = jp.where(force < 0, scale / jp.maximum(mujoco.mjMINVAL, acc0), force) + + # optimum length + L0 = (lengthrange[1] - lengthrange[0]) / jp.maximum( # pylint:disable=invalid-name + mujoco.mjMINVAL, lrange[1] - lrange[0] + ) + + # normalized length and velocity + L = lrange[0] + (length - lengthrange[0]) / jp.maximum(mujoco.mjMINVAL, L0) # pylint:disable=invalid-name + V = vel / jp.maximum(mujoco.mjMINVAL, L0 * vmax) # pylint:disable=invalid-name + + # length curve + FL = muscle_gain_length(L, lmin, lmax) # pylint:disable=invalid-name + + # velocity curve + y = fvmax - 1 + FV = fvmax # pylint:disable=invalid-name + FV = jp.where( # pylint:disable=invalid-name + V <= y, fvmax - jp.square(y - V) / jp.maximum(mujoco.mjMINVAL, y), FV + ) + FV = jp.where(V <= 0, jp.square(V + 1), FV) # pylint:disable=invalid-name + FV = jp.where(V <= -1, 0, FV) # pylint:disable=invalid-name + + # compute FVL and scale, make it negative + return -force * FL * FV + + +def muscle_bias( + length: jax.Array, lengthrange: jax.Array, acc0: jax.Array, prm: jax.Array +) -> jax.Array: + """Muscle passive force.""" + # unpack parameters + lrange = prm[:2] + force, scale, _, lmax, _, fpmax = prm[2:8] + + force = jp.where(force < 0, scale / jp.maximum(mujoco.mjMINVAL, acc0), force) + + # optimum length + L0 = (lengthrange[1] - lengthrange[0]) / jp.maximum( # pylint:disable=invalid-name + mujoco.mjMINVAL, lrange[1] - lrange[0] + ) + + # normalized length + L = lrange[0] + (length - lengthrange[0]) / jp.maximum(mujoco.mjMINVAL, L0) # pylint:disable=invalid-name + + # half-quadratic to (L0 + lmax) / 2, linear beyond + b = 0.5 * (1 + lmax) + + out1 = ( + -force + * fpmax + * 0.5 + * jp.square((L - 1) / jp.maximum(mujoco.mjMINVAL, b - 1)) + ) + out2 = -force * fpmax * (0.5 + (L - b) / jp.maximum(mujoco.mjMINVAL, b - 1)) + + out = jp.where(L <= b, out1, out2) + out = jp.where(L <= 1, 0.0, out) + + return out + + +def muscle_dynamics_timescale( + dctrl: jax.Array, + tau_act: jax.Array, + tau_deact: jax.Array, + smoothing_width: jax.Array, +) -> jax.Array: + """Muscle time constant with optional smoothing.""" + # hard switching + tau_hard = jp.where(dctrl > 0, tau_act, tau_deact) + + def _sigmoid(x): + # sigmoid function over 0 <= x <= 1 using quintic polynomial + # sigmoid: f(x) = 6 * x^5 - 15 * x^4 + 10 * x^3 + # solution of f(0) = f'(0) = f''(0) = 0, f(1) = 1, f'(1) = f''(1) = 0 + return jp.clip(x**3 * (3 * x * (2 * x - 5) + 10), 0, 1) + + # smooth switching + # scale by width, center around 0.5 midpoint, rescale to bounds + tau_smooth = tau_deact + (tau_act - tau_deact) * _sigmoid( + dctrl / smoothing_width + 0.5 + ) + + return jp.where(smoothing_width < mujoco.mjMINVAL, tau_hard, tau_smooth) + + +def muscle_dynamics( + ctrl: jax.Array, act: jax.Array, prm: jax.Array +) -> jax.Array: + """Muscle activation dynamics.""" + # clamp control + ctrlclamp = jp.clip(ctrl, 0, 1) + + # clamp activation + actclamp = jp.clip(act, 0, 1) + + # compute timescales as in Millard et at. (2013) + # https://doi.org/10.1115/1.4023390 + tau_act = prm[0] * (0.5 + 1.5 * actclamp) # activation timescale + tau_deact = prm[1] / (0.5 + 1.5 * actclamp) # deactivation timescale + smoothing_width = prm[2] # width of smoothing sigmoid + dctrl = ctrlclamp - act # excess excitation + + tau = muscle_dynamics_timescale(dctrl, tau_act, tau_deact, smoothing_width) + + # filter output + return dctrl / jp.maximum(mujoco.mjMINVAL, tau) diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 1263f1a7..c955ba72 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -221,6 +221,211 @@ class SupportTest(parameterized.TestCase): force = force.at[3:].set(dx.contact.frame[j] @ force[3:]) np.testing.assert_allclose(result, force, rtol=1e-5, atol=2) + def test_muscle_gain_length(self): + lmin = 0.5 + lmax = 1.5 + np.testing.assert_allclose( + support.muscle_gain_length(0, lmin, lmax), + jp.zeros(1), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(0.5, lmin, lmax), + jp.zeros(1), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(0.6, lmin, lmax), + jp.array([0.08]), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(0.75, lmin, lmax), + jp.array([0.5]), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(1.0, lmin, lmax), + jp.ones(1), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(1.25, lmin, lmax), + jp.array([0.5]), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(1.5, lmin, lmax), + jp.zeros(1), + rtol=1e-5, + atol=1e-5, + ) + np.testing.assert_allclose( + support.muscle_gain_length(2.0, lmin, lmax), + jp.zeros(1), + rtol=1e-5, + atol=1e-5, + ) + + def test_muscle_gain(self): + length = jp.array([1.0]) + lengthrange = jp.array([0.0, 1.0]) + acc0 = jp.array([1.0]) + prm = jp.array([0.0, 1.0, 1.0, 200.0, 0.5, 3.0, 1.0, 0.0, 2.0, 0.0]) + + # V <= -1 + vel = jp.array([-1.5]) + np.testing.assert_allclose( + support.muscle_gain(length, vel, lengthrange, acc0, prm), + jp.array([-0.0]), + rtol=1e-5, + atol=1e-5, + ) + + # V <= 0 + vel = jp.array([-0.5]) + np.testing.assert_allclose( + support.muscle_gain(length, vel, lengthrange, acc0, prm), + jp.array([-0.25]), + rtol=1e-5, + atol=1e-5, + ) + + # V <= y + vel = jp.array([0.5]) + np.testing.assert_allclose( + support.muscle_gain(length, vel, lengthrange, acc0, prm), + jp.array([-1.75]), + rtol=1e-5, + atol=1e-5, + ) + + # V > y + vel = jp.array([1.5]) + np.testing.assert_allclose( + support.muscle_gain(length, vel, lengthrange, acc0, prm), + jp.array([-2.0]), + rtol=1e-5, + atol=1e-5, + ) + + # force < 0 + prm = prm.at[2].set(-1.0) + np.testing.assert_allclose( + support.muscle_gain(length, vel, lengthrange, acc0, prm), + jp.array([-400.0]), + rtol=1e-5, + atol=1e-5, + ) + + def test_muscle_bias(self): + lengthrange = jp.array([0.0, 1.0]) + acc0 = jp.array([1.0]) + prm = jp.array([0.0, 1.0, 1.0, 200.0, 0.5, 3.0, 1.5, 1.3, 1.2, 0.0]) + + # L <= 1 + length = jp.array([0.5]) + np.testing.assert_allclose( + support.muscle_bias(length, lengthrange, acc0, prm), + jp.array([0.0]), + rtol=1e-5, + atol=1e-5, + ) + + # L <= b + length = jp.array([1.5]) + np.testing.assert_allclose( + support.muscle_bias(length, lengthrange, acc0, prm), + jp.array([-0.1625]), + rtol=1e-5, + atol=1e-5, + ) + + # L > b + length = jp.array([2.5]) + np.testing.assert_allclose( + support.muscle_bias(length, lengthrange, acc0, prm), + jp.array([-1.3]), + rtol=1e-5, + atol=1e-5, + ) + + # force < 0 + prm = prm.at[2].set(-1.0) + np.testing.assert_allclose( + support.muscle_bias(length, lengthrange, acc0, prm), + jp.array([-260.0]), + rtol=1e-5, + atol=1e-5, + ) + + def test_smooth_muscle_dynamics(self): + # compute time constant as in Millard et al. (2013) + # https://doi.org/10.1115/1.4023390 + def _muscle_dynamics_millard(ctrl, act, prm): + ctrlclamp = jp.clip(ctrl, 0, 1) + actclamp = jp.clip(act, 0, 1) + + tau0 = prm[0] * (0.5 + 1.5 * actclamp) + tau1 = prm[1] / (0.5 + 1.5 * actclamp) + tau = jp.where(ctrlclamp > act, tau0, tau1) + + return (ctrlclamp - act) / jp.maximum(mujoco.mjMINVAL, tau) + + prm = jp.array([0.01, 0.04, 0.0]) + + # exact equality if tau_smooth = 0 + for ctrl in [-0.1, 0.0, 0.4, 0.5, 1.0, 1.0]: + for act in [-0.1, 0.0, 0.4, 0.5, 1.0, 1.1]: + actdot_old = _muscle_dynamics_millard(ctrl, act, prm) + actdot_new = support.muscle_dynamics(ctrl, act, prm) + np.testing.assert_allclose(actdot_old, actdot_new, rtol=1e-5, atol=1e-5) + + # positive tau_smooth + tau_smooth = 0.2 + prm = prm.at[2].set(tau_smooth) + act = 0.5 + eps = 1.0e-6 + + ctrl = 0.4 - eps # smaller than act by just over 0.5 * tau_smooth + np.testing.assert_allclose( + _muscle_dynamics_millard(ctrl, act, prm), + support.muscle_dynamics(ctrl, act, prm), + rtol=1e-5, + atol=1e-5, + ) + + ctrl = 0.6 + eps # larger than act by just over 0.5 * tau_smooth + np.testing.assert_allclose( + _muscle_dynamics_millard(ctrl, act, prm), + support.muscle_dynamics(ctrl, act, prm), + rtol=1e-5, + atol=1e-5, + ) + + # right in the middle should give average of time constants + tau_act = 0.2 + tau_deact = 0.3 + for dctrl in [0.0, 0.1, 0.2, 1.0, 1.1]: + lower = support.muscle_dynamics_timescale( + -dctrl, tau_act, tau_deact, tau_smooth + ) + upper = support.muscle_dynamics_timescale( + dctrl, tau_act, tau_deact, tau_smooth + ) + np.testing.assert_allclose( + 0.5 * (upper + lower), + 0.5 * (tau_act + tau_deact), + rtol=1e-5, + atol=1e-5, + ) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index ec17efed..a940d44e 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -234,12 +234,14 @@ class DynType(enum.IntEnum): INTEGRATOR: integrator: da/dt = u FILTER: linear filter: da/dt = (u-a) / tau FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration + MUSCLE: piece-wise linear filter with two time constants """ NONE = mujoco.mjtDyn.mjDYN_NONE INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR FILTER = mujoco.mjtDyn.mjDYN_FILTER FILTEREXACT = mujoco.mjtDyn.mjDYN_FILTEREXACT - # unsupported: MUSCLE, USER + MUSCLE = mujoco.mjtDyn.mjDYN_MUSCLE + # unsupported: USER class GainType(enum.IntEnum): @@ -248,10 +250,12 @@ class GainType(enum.IntEnum): Members: FIXED: fixed gain AFFINE: const + kp*length + kv*velocity + MUSCLE: muscle FLV curve computed by muscle_gain """ FIXED = mujoco.mjtGain.mjGAIN_FIXED AFFINE = mujoco.mjtGain.mjGAIN_AFFINE - # unsupported: MUSCLE, USER + MUSCLE = mujoco.mjtGain.mjGAIN_MUSCLE + # unsupported: USER class BiasType(enum.IntEnum): @@ -260,10 +264,12 @@ class BiasType(enum.IntEnum): Members: NONE: no bias AFFINE: const + kp*length + kv*velocity + MUSCLE: muscle passive force computed by muscle_bias """ NONE = mujoco.mjtBias.mjBIAS_NONE AFFINE = mujoco.mjtBias.mjBIAS_AFFINE - # unsupported: MUSCLE, USER + MUSCLE = mujoco.mjtBias.mjBIAS_MUSCLE + # unsupported: USER class ConstraintType(enum.IntEnum): diff --git a/mjx/mujoco/mjx/test_data/actuator/arm21.xml b/mjx/mujoco/mjx/test_data/actuator/arm21.xml new file mode 100644 index 00000000..57ac7990 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/actuator/arm21.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjx/mujoco/mjx/test_data/actuator/arm26.xml b/mjx/mujoco/mjx/test_data/actuator/arm26.xml new file mode 100644 index 00000000..0875358a --- /dev/null +++ b/mjx/mujoco/mjx/test_data/actuator/arm26.xml @@ -0,0 +1,118 @@ + + + + diff --git a/mjx/mujoco/mjx/test_data/actuator/general_dyntype.xml b/mjx/mujoco/mjx/test_data/actuator/general_dyntype.xml new file mode 100644 index 00000000..01d0a503 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/actuator/general_dyntype.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + From f7cc1c2e5bbb802aab133724e9214feaaea6bbf2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 11 Nov 2024 11:14:11 -0800 Subject: [PATCH 065/426] Read solref from jnt_solref when the user calls model.joint.solref. Same for solimp. Fixes #2221. PiperOrigin-RevId: 695416121 Change-Id: I962dc12b13fa4d02dc349ecad1a1ddc7085565db --- python/mujoco/indexer_xmacro.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 199c46c7..6f9d2f9d 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -140,8 +140,8 @@ X( int, dof_, parentid, nv, 1 ) \ X( int, dof_, Madr, nv, 1 ) \ X( int, dof_, simplenum, nv, 1 ) \ - X( mjtNum, dof_, solref, nv, mjNREF ) \ - X( mjtNum, dof_, solimp, nv, mjNIMP ) \ + X( mjtNum, jnt_, solref, njnt, mjNREF ) \ + X( mjtNum, jnt_, solimp, njnt, mjNIMP ) \ X( mjtNum, dof_, frictionloss, nv, 1 ) \ X( mjtNum, dof_, armature, nv, 1 ) \ X( mjtNum, dof_, damping, nv, 1 ) \ From eb77d5b9334e2a4c710a164b31e25d32261612e5 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 12 Nov 2024 06:35:46 -0800 Subject: [PATCH 066/426] Fix bug in mesh hillclimbing support function. PiperOrigin-RevId: 695710616 Change-Id: I2127aee584c13efb44d70cc4e267354ca0b80bf0 --- src/engine/engine_collision_convex.c | 87 ++++++++++++++-------------- 1 file changed, 42 insertions(+), 45 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index e2ddb3e8..30e6d07f 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -14,6 +14,7 @@ #include "engine/engine_collision_convex.h" +#include #include #include @@ -284,6 +285,13 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { +// dot product between mjtNum and float +static inline mjtNum dot3f(const mjtNum a[3], const float b[3]) { + return a[0]*(mjtNum)b[0] + a[1]*(mjtNum)b[1] + a[2]*(mjtNum)b[2]; +} + + + // mesh support function via exhaustive search static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -299,34 +307,32 @@ static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { mjtNum local_dir[3]; mulMatTVec3(local_dir, mat, dir); - mjtNum tmp = -1E+10; - int ibest = -1; + mjtNum max = -FLT_MAX; + int imax = 0; + + // used cached results from previous search if (obj->meshindex >= 0) { - ibest = obj->meshindex; - tmp = local_dir[0] * (mjtNum)verts[3*ibest + 0] + - local_dir[1] * (mjtNum)verts[3*ibest + 1] + - local_dir[2] * (mjtNum)verts[3*ibest + 2]; + imax = obj->meshindex; + max = dot3f(local_dir, verts + 3*imax); } - // search all vertices, find best + // search all vertices, find maximum dot product for (int i=0; i < nverts; i++) { - mjtNum vdot = local_dir[0] * (mjtNum)verts[3*i + 0] + - local_dir[1] * (mjtNum)verts[3*i + 1] + - local_dir[2] * (mjtNum)verts[3*i + 2]; + mjtNum vdot = dot3f(local_dir, verts + 3*i); - // update best - if (vdot > tmp) { - tmp = vdot; - ibest = i; + // update max + if (vdot > max) { + max = vdot; + imax = i; } } - // record best vertex index - obj->meshindex = ibest; + // record vertex index of maximum + obj->meshindex = imax; - local_dir[0] = (mjtNum)verts[3*ibest + 0]; - local_dir[1] = (mjtNum)verts[3*ibest + 1]; - local_dir[2] = (mjtNum)verts[3*ibest + 2]; + local_dir[0] = (mjtNum)verts[3*imax + 0]; + local_dir[1] = (mjtNum)verts[3*imax + 1]; + local_dir[2] = (mjtNum)verts[3*imax + 2]; // transform result to global frame localToGlobal(res, mat, local_dir, pos); @@ -354,38 +360,29 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[ mjtNum local_dir[3]; mulMatTVec3(local_dir, mat, dir); - mjtNum tmp = -1E+10; - int ibest= -1, prev = -1; - // hill-climb until no change + // hillclimb until no change + mjtNum max = -FLT_MAX; + int prev = -1, imax = obj->meshindex < 0 ? 0 : obj->meshindex; do { - prev = ibest; - for (int i = vert_edgeadr[ibest]; edge_localid[i] >= 0; i++) { + prev = imax; + for (int i = vert_edgeadr[imax]; edge_localid[i] >= 0; i++) { int idx = 3*vert_globalid[edge_localid[i]]; - mjtNum vdot = local_dir[0] * (mjtNum)verts[idx + 0] + - local_dir[1] * (mjtNum)verts[idx + 1] + - local_dir[2] * (mjtNum)verts[idx + 2]; - if (vdot > tmp) { - tmp = vdot; - ibest = edge_localid[i]; // update best + mjtNum vdot = dot3f(local_dir, verts + idx); + if (vdot > max) { + max = vdot; + imax = edge_localid[i]; // update maximum vertex index } } - } while (ibest != prev); + } while (imax != prev); - // record best vertex index (local id) - obj->meshindex = ibest; + // record vertex index of maximum (local id) + obj->meshindex = imax; - // map best index to globalid - ibest = vert_globalid[ibest]; - - // sanity check, SHOULD NOT OCCUR - if (ibest < 0) { - mju_warning("mesh_support could not find support vertex"); - mju_zero3(res); - } else { - local_dir[0] = (mjtNum)verts[3*ibest + 0]; - local_dir[1] = (mjtNum)verts[3*ibest + 1]; - local_dir[2] = (mjtNum)verts[3*ibest + 2]; - } + // get resulting support vertex + imax = 3*vert_globalid[imax]; + local_dir[0] = (mjtNum)verts[imax + 0]; + local_dir[1] = (mjtNum)verts[imax + 1]; + local_dir[2] = (mjtNum)verts[imax + 2]; // transform result to global frame localToGlobal(res, mat, local_dir, pos); From 45f908c165cf2cc48d2ccf3e60ae25f191b1e3b7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 12 Nov 2024 09:33:32 -0800 Subject: [PATCH 067/426] Enable attaching plugins not instantiated in the `extension` section. Fixes #2217. PiperOrigin-RevId: 695766524 Change-Id: I534b63400810bd6ef888f39f6665213280fd93ac --- src/user/user_mesh.cc | 8 +++- src/user/user_model.cc | 40 ++++++++++++++---- src/user/user_model.h | 6 ++- src/user/user_objects.cc | 34 +++++++++++++-- src/user/user_objects.h | 8 ++++ test/user/user_api_test.cc | 84 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 164 insertions(+), 16 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 695de30f..766e4976 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -268,10 +268,16 @@ void mjCMesh::CopyFromSpec() { +void mjCMesh::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + mjCMesh::~mjCMesh() { if (center_) mju_free(center_); if (graph_) mju_free(graph_); - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 94b3ec3d..e01fbc5c 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -232,6 +232,7 @@ void mjCModel::CopyList(std::vector& dest, dest.back()->model = this; dest.back()->compiler = origin ? &origin->compiler : &spec.compiler; dest.back()->id = -1; + dest.back()->CopyPlugin(); } if (!dest.empty()) { processlist(ids, dest, dest[0]->elemtype); @@ -309,8 +310,28 @@ void mjCModel::SaveDofOffsets(bool computesize) { template -void mjCModel::CopyPlugin(std::vector& dest, - const std::vector& source, +void mjCModel::CopyExplicitPlugin(T* obj) { + if (!obj->plugin.active || !obj->plugin_instance_name.empty() || !obj->spec.plugin.element) { + return; + } + mjCPlugin* origin = static_cast(obj->spec.plugin.element); + mjCPlugin* candidate = new mjCPlugin(*origin); + candidate->id = plugins_.size(); + candidate->model = this; + plugins_.push_back(candidate); + obj->spec.plugin.element = candidate; +} + +template void mjCModel::CopyExplicitPlugin(mjCBody* obj); +template void mjCModel::CopyExplicitPlugin(mjCGeom* obj); +template void mjCModel::CopyExplicitPlugin(mjCMesh* obj); +template void mjCModel::CopyExplicitPlugin(mjCActuator* obj); +template void mjCModel::CopyExplicitPlugin(mjCSensor* obj); + + + +template +void mjCModel::CopyPlugin(const std::vector& source, const std::vector& list) { // store elements that reference a plugin instance std::unordered_map instances; @@ -330,9 +351,10 @@ void mjCModel::CopyPlugin(std::vector& dest, candidate->NameSpace(plugin->model); bool referenced = instances.find(candidate->name) != instances.end(); auto same_name = [candidate](const mjCPlugin* dest) { return dest->name == candidate->name; }; - bool instance_exists = std::find_if(dest.begin(), dest.end(), same_name) != dest.end(); + bool instance_exists = std::find_if(plugins_.begin(), plugins_.end(), + same_name) != plugins_.end(); if (referenced && !instance_exists) { - dest.push_back(candidate); + plugins_.push_back(candidate); instances.at(candidate->name)->spec.plugin.element = candidate; } else { delete candidate; @@ -390,11 +412,11 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) { CopyList(tuples_, other.tuples_); // create new plugins and map them - CopyPlugin(plugins_, other.plugins_, bodies_); - CopyPlugin(plugins_, other.plugins_, geoms_); - CopyPlugin(plugins_, other.plugins_, meshes_); - CopyPlugin(plugins_, other.plugins_, actuators_); - CopyPlugin(plugins_, other.plugins_, sensors_); + CopyPlugin(other.plugins_, bodies_); + CopyPlugin(other.plugins_, geoms_); + CopyPlugin(other.plugins_, meshes_); + CopyPlugin(other.plugins_, actuators_); + CopyPlugin(other.plugins_, sensors_); for (const auto& [plugin, slot] : other.active_plugins_) { if (!IsPluginActive(plugin, active_plugins_)) { active_plugins_.emplace_back(std::make_pair(plugin, slot)); diff --git a/src/user/user_model.h b/src/user/user_model.h index d65d93d9..33341228 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -366,9 +366,11 @@ class mjCModel : public mjCModel_, private mjSpec { template void CopyList(std::vector& dest, const std::vector& sources); + // copy plugins that are explicitly instantiated by the argument object to this model + template void CopyExplicitPlugin(T* obj); + // copy vector of plugins to this model - template void CopyPlugin(std::vector& dest, - const std::vector& sources, + template void CopyPlugin(const std::vector& sources, const std::vector& list); // delete from list the elements that cause an error diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e795f49a..35659e3e 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -789,6 +789,7 @@ mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) { mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); compiler = origin ? &origin->compiler : &model->spec.compiler; *this = other; + CopyPlugin(); } @@ -942,6 +943,7 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, dst.back()->model = model; dst.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; dst.back()->id = -1; + dst.back()->CopyPlugin(); dst.back()->classname = src[i]->classname; // assign dst frame to src frame @@ -992,6 +994,12 @@ void mjCBody::CopyFromSpec() { +void mjCBody::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + // destructor mjCBody::~mjCBody() { // delete objects allocated here @@ -1011,7 +1019,7 @@ mjCBody::~mjCBody() { cameras.clear(); lights.clear(); - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -2221,7 +2229,7 @@ mjCGeom::mjCGeom(const mjCGeom& other) { mjCGeom::~mjCGeom() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -2273,6 +2281,12 @@ void mjCGeom::CopyFromSpec() { +void mjCGeom::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCGeom::NameSpace(const mjCModel* m) { mjCBase::NameSpace(m); if (!spec_material_.empty() && model != m) { @@ -5636,7 +5650,7 @@ mjCActuator::mjCActuator(const mjCActuator& other) { mjCActuator::~mjCActuator() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -5731,6 +5745,12 @@ void mjCActuator::CopyFromSpec() { +void mjCActuator::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCActuator::ResolveReferences(const mjCModel* m) { switch (trntype) { case mjTRN_JOINT: @@ -6000,7 +6020,7 @@ mjCSensor::mjCSensor(const mjCSensor& other) { mjCSensor::~mjCSensor() { - if (spec.plugin.active && spec.plugin.name->empty()) { + if (spec.plugin.active && spec.plugin.name->empty() && model) { model->DeleteElement(spec.plugin.element); } } @@ -6063,6 +6083,12 @@ void mjCSensor::CopyFromSpec() { +void mjCSensor::CopyPlugin() { + model->CopyExplicitPlugin(this); +} + + + void mjCSensor::ResolveReferences(const mjCModel* m) { objname_ = prefix + objname_ + suffix; refname_ = prefix + refname_ + suffix; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1018fb8b..5b031d6b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -208,6 +208,9 @@ class mjCBase : public mjCBase_ { // Appends prefix and suffix to reference virtual void NameSpace(const mjCModel* m); + // Copy plugins instantiated in this object + virtual void CopyPlugin() {} + // Copy assignment mjCBase& operator=(const mjCBase& other); @@ -360,6 +363,7 @@ class mjCBody : public mjCBody_, private mjsBody { void CopyFromSpec(); // copy spec into attributes void PointToLocal(void); void NameSpace_(const mjCModel* m, bool propagate = true); + void CopyPlugin(); // copy src list of elements into dst; set body, model and frame template @@ -557,6 +561,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void CopyPlugin(); // inherited using mjCBase::info; @@ -935,6 +940,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void ApplyTransformations(); // apply user transformations void ComputeFaceCentroid(double[3]); // compute centroid of all faces void CheckMesh(mjtGeomInertia type); // check if the mesh is valid + void CopyPlugin(); // mesh data to be copied into mjModel double* center_; // face circumcenter data (3*nface) @@ -1503,6 +1509,7 @@ class mjCActuator : public mjCActuator_, private mjsActuator { void PointToLocal(); void ResolveReferences(const mjCModel* m); void NameSpace(const mjCModel* m); + void CopyPlugin(); // reset keyframe references for allowing self-attach void ForgetKeyframes(); @@ -1556,6 +1563,7 @@ class mjCSensor : public mjCSensor_, private mjsSensor { void PointToLocal(); void ResolveReferences(const mjCModel* m); void NameSpace(const mjCModel* m); + void CopyPlugin(); mjCBase* obj; // sensorized object mjCBase* ref; // sensorized reference diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 93c0c1ab..e376d046 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -293,6 +293,56 @@ TEST_F(PluginTest, AttachPlugin) { mj_deleteSpec(spec_2); } +TEST_F(PluginTest, AttachExplicitPlugin) { + static constexpr char xml_parent[] = R"( + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + std::array err; + mjSpec* parent = mj_parseXMLString(xml_parent, 0, err.data(), err.size()); + ASSERT_THAT(parent, NotNull()) << err.data(); + mjSpec* child = mj_parseXMLString(xml_child, 0, err.data(), err.size()); + ASSERT_THAT(child, NotNull()) << err.data(); + + mjsBody* body_parent = mjs_findBody(parent, "body"); + EXPECT_THAT(body_parent, NotNull()); + mjsFrame* attachment_frame = mjs_addFrame(body_parent, 0); + EXPECT_THAT(attachment_frame, NotNull()); + + mjs_attachBody(attachment_frame, mjs_findBody(child, "body"), "child-", ""); + mjModel* model = mj_compile(parent, nullptr); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->nplugin, 1); + + mj_deleteSpec(parent); + mj_deleteSpec(child); + mj_deleteModel(model); +} + TEST_F(PluginTest, ReplicatePlugin) { static constexpr char xml[] = R"( @@ -326,6 +376,40 @@ TEST_F(PluginTest, ReplicatePlugin) { mj_deleteModel(model); } +TEST_F(PluginTest, ReplicateExplicitPlugin) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + + std::array err; + mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size()); + ASSERT_THAT(spec, NotNull()) << err.data(); + mjModel* model = mj_compile(spec, nullptr); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->nplugin, 1); + mj_deleteSpec(spec); + mj_deleteModel(model); +} + TEST_F(MujocoTest, RecompileFails) { mjSpec* spec = mj_makeSpec(); mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0); From c68ee8055ef8aaaf136a25dfa63348de619b2230 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 13 Nov 2024 03:11:14 -0800 Subject: [PATCH 068/426] Fix ordering in `mjxmacro.h`. PiperOrigin-RevId: 696058596 Change-Id: I6359c082e528c1f53591033ea61c352ba5e13763 --- include/mujoco/mjxmacro.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 796b6c5a..33b83f7b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -137,11 +137,11 @@ X ( nB ) \ X ( nC ) \ X ( nD ) \ + XMJV( ntree ) \ + X ( ngravcomp ) \ X ( nemax ) \ X ( njmax ) \ X ( nconmax ) \ - XMJV( ntree ) \ - X ( ngravcomp ) \ X ( nuserdata ) \ XMJV( nsensordata ) \ X ( npluginstate ) \ From 0e8cca93e185c0ca7dfdd15b52db591c591a9c14 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 13 Nov 2024 10:27:54 -0800 Subject: [PATCH 069/426] Refactor nativeccd interface. - Update mj_ccd interface for multi-contacts, - Support GJK cutoff distance, and - Remove mjc_fixNormal from nativeccd (causes bug with cylindar box collisions). PiperOrigin-RevId: 696185362 Change-Id: I4828b7ee1bde078268220172a4937cd553f6187e --- src/engine/engine_collision_convex.c | 79 ++++++++++++++---------- src/engine/engine_collision_convex.h | 2 +- src/engine/engine_collision_gjk.c | 77 +++++++++++++++-------- src/engine/engine_collision_gjk.h | 34 +++++----- src/engine/engine_support.c | 10 +-- test/engine/engine_collision_gjk_test.cc | 69 +++++++++++++-------- 6 files changed, 162 insertions(+), 109 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 30e6d07f..1fac0a77 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -30,7 +30,7 @@ #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" -// call LibCCD or GJK to recover penetration info +// call libccd or nativeccd to recover penetration info static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { if (mjENABLED(mjENBL_NATIVECCD)) { @@ -40,8 +40,8 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, // set config config.max_iterations = ccd->max_iterations, config.tolerance = ccd->mpr_tolerance, - config.contacts = 1; - config.distances = 0; // no geom distances needed + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { @@ -68,6 +68,7 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, } + // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { mjc_center(center->v, (const mjCCDObj*) obj); @@ -744,12 +745,46 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find single convex-convex collision -static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, - const mjModel* m, const mjData* d, +static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, mjtNum margin) { + if (mjENABLED(mjENBL_NATIVECCD)) { + mjCCDConfig config; + mjCCDStatus status; + + // set config + config.max_iterations = m->opt.ccd_iterations; + config.tolerance = m->opt.ccd_tolerance; + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed + + mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + if (dist < 0) { + con->dist = margin + dist; + mju_sub3(con->frame, status.x1, status.x2); + mju_normalize3(con->frame); + con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]); + con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]); + con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]); + mju_zero3(con->frame+3); + return 1; + } + return 0; + } + + // init libccd structure + ccd_t ccd; + mjc_initCCD(&ccd, m); + ccd.first_dir = ccdFirstDirDefault; + ccd.center1 = mjccd_center; + ccd.center2 = mjccd_center; + ccd.support1 = mjccd_support; + ccd.support2 = mjccd_support; + ccd_vec3_t dir, pos; ccd_real_t depth; - if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) { + + // call MPR from libccd + if (ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos) == 0) { // contact is found but normal is undefined if (ccdVec3Eq(&dir, ccd_vec3_origin)) { return 0; @@ -768,11 +803,7 @@ static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, return 1; } - - // no contact found - else { - return 0; - } + return 0; } @@ -819,17 +850,8 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj2, m, d, g2, margin); - // init libccd structure - ccd_t ccd; - mjc_initCCD(&ccd, m); - ccd.first_dir = ccdFirstDirDefault; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - // find initial contact - int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); + int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); // look for additional contacts if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) @@ -878,7 +900,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2); // search for new contact - int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin); + int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { @@ -1525,19 +1547,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, mjc_setCCDObjFlex(&obj1, f1, e1, v1); mjc_setCCDObjFlex(&obj2, f2, e2, -1); - // init libccd structure - ccd_t ccd; - mjc_initCCD(&ccd, m); - ccd.first_dir = ccdFirstDirDefault; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - // find contacts - int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); - - return ncon; + return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); } diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 1f2e347f..c402c9c6 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -85,7 +85,7 @@ int mjc_Convex (const mjModel* m, const mjData* d, int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin); -// heighfield-elem collision function using ccd +// heightfield-elem collision function using ccd int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con, int g, int f, int e, mjtNum margin); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index f513dc19..59b4380f 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -77,8 +77,9 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]); // attaches a face to the polytope with the given vertex indices; returns non-zero on error static void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); -// returns 1 if objects are in contact, 0 otherwise; status must have initial tetrahedrons -static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2); +// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +// status must have initial tetrahedrons +static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); // returns the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); @@ -145,17 +146,18 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - int get_dist = status->has_distances; // need to recover geom distances if not in contact - mjtNum *simplex1 = status->simplex1; // simplex for obj1 - mjtNum *simplex2 = status->simplex2; // simplex for obj2 - mjtNum *simplex = status->simplex; // simplex in Minkowski difference - int n = 0; // number of vertices in the simplex - int k = 0; // current iteration - int kmax = status->max_iterations; // max number of iterations - mjtNum* x1_k = status->x1; // the kth approximation point for obj1 - mjtNum* x2_k = status->x2; // the kth approximation point for obj2 - mjtNum x_k[3]; // the kth approximation point in Minkowski difference - mjtNum lambda[4]; // barycentric coordinates for x_k + int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact + mjtNum *simplex1 = status->simplex1; // simplex for obj1 + mjtNum *simplex2 = status->simplex2; // simplex for obj2 + mjtNum *simplex = status->simplex; // simplex in Minkowski difference + int n = 0; // number of vertices in the simplex + int k = 0; // current iteration + int kmax = status->max_iterations; // max number of iterations + mjtNum* x1_k = status->x1; // the kth approximation point for obj1 + mjtNum* x2_k = status->x2; // the kth approximation point for obj2 + mjtNum x_k[3]; // the kth approximation point in Minkowski difference + mjtNum lambda[4]; // barycentric coordinates for x_k + mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff; // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : status->tolerance * status->tolerance; @@ -182,14 +184,29 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // if the hyperplane separates the Minkowski difference and origin, the objects don't collide // if geom distance isn't requested, return early - if (!get_dist && dot3(x_k, s_k) > 0) { - return mjMAXVAL; + if (!get_dist) { + if (dot3(x_k, s_k) > 0) { + status->gjk_iterations = k; + status->nsimplex = 0; + status->nx = 0; + return mjMAXVAL; + } + } else if (status->dist_cutoff < mjMAXVAL) { + mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k); + if (mju_dot3(x_k, s_k) > 0 && (vs*vs / vv) >= cutoff2) { + status->gjk_iterations = k; + status->nsimplex = 0; + status->nx = 0; + return mjMAXVAL; + } } // tetrahedron is generated and only need contact info; fallback to gjkIntersect to // determine contact if (!get_dist && n == 3) { - return gjkIntersect(status, k, obj1, obj2) ? 0 : mjMAXVAL; + status->gjk_iterations = k; + status->nx = 0; + return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL; } // run the distance subalgorithm to compute the barycentric coordinates @@ -228,10 +245,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { lincomb(x1_k, lambda, simplex1, n); lincomb(x2_k, lambda, simplex2, n); + status->nx = 1; status->gjk_iterations = k; status->nsimplex = n; - status->gjk_dist = mju_norm3(x_k); - return status->gjk_dist; + status->dist = mju_norm3(x_k); + return status->dist; } @@ -329,16 +347,16 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const -// returns 0 if objects are in contact, mjMAXVAL otherwise; status must have initial tetrahedrons -static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2) { +// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex1[12], simplex2[12], simplex[12]; memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12); memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12); memcpy(simplex, status->simplex, sizeof(mjtNum) * 12); int s[4] = {0, 3, 6, 9}; - int kmax = status->max_iterations; - for (int k = start; k < kmax; k++) { + int k = status->gjk_iterations, kmax = status->max_iterations; + for (; k < kmax; k++) { // compute the signed distance to each face in the simplex along with normals mjtNum dist[4], normals[12]; dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]); @@ -359,6 +377,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj copy3(status->simplex1 + 3*n, simplex1 + s[n]); copy3(status->simplex2 + 3*n, simplex2 + s[n]); } + status->gjk_iterations = k; return 1; } @@ -368,6 +387,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj // found origin outside the Minkowski difference (return no collision) if (dot3(&normals[3*index], simplex + s[index]) < 0) { + status->gjk_iterations = k; return 0; } @@ -378,7 +398,8 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj s[i] = s[j]; s[j] = swap; } - return 0; // never found origin + status->gjk_iterations = k; + return -1; // never found origin } @@ -993,7 +1014,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // TODO(kylebayes): It's possible for GJK to return a 2-simplex with the origin not contained in // it but within tolerance from it. In that case the hexahedron could possibly be constructed // that doesn't contain the origin, but nonetheless there is penetration depth. - if (status->gjk_dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { + if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { return 7; } @@ -1296,6 +1317,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o mj_freeStack(d); epaWitness(pt, face, status->x1, status->x2); status->epa_iterations = k; + status->nx = 1; return dist; } @@ -1306,16 +1328,17 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m // set up obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); + status->gjk_iterations = 0; status->epa_iterations = -1; status->tolerance = config->tolerance; status->max_iterations = config->max_iterations; - status->has_contacts = config->contacts; - status->has_distances = config->distances; + status->max_contacts = config->max_contacts; + status->dist_cutoff = config->dist_cutoff; mjtNum dist = gjk(status, obj1, obj2); // penetration recovery for contacts not needed - if (!config->contacts) { + if (!config->max_contacts) { return dist; } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 80417b01..f04ca77a 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -16,7 +16,9 @@ #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ #include +#include #include + #include "engine/engine_collision_convex.h" #ifdef __cplusplus @@ -27,30 +29,32 @@ extern "C" { struct _mjCCDConfig { int max_iterations; // the maximum number of iterations for GJK and EPA mjtNum tolerance; // tolerance used by GJK and EPA - int contacts; // set to true to recover contact (pendetration) info - int distances; // set to true to recover distance info + int max_contacts; // set to max number of contact points to recover + mjtNum dist_cutoff; // set to max geom distance to recover }; typedef struct _mjCCDConfig mjCCDConfig; // data produced from running GJK and EPA struct _mjCCDStatus { - mjtNum x1[3]; // witness point for geom 1 - mjtNum x2[3]; // witness point for geom 2 + // geom distance information + mjtNum dist; // distance between geoms + mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1 + mjtNum x2[3 * mjMAXCONPAIR]; // witness points for geom 2 + int nx; // number of witness points // configurations used - int max_iterations; // the maximum number of iterations for GJK and EPA - mjtNum tolerance; // tolerance used by GJK and EPA - int has_contacts; // set to true if attempted to recover contact (pendetration) info - int has_distances; // set to true if attempted to recover distance info + int max_iterations; // the maximum number of iterations for GJK and EPA + mjtNum tolerance; // tolerance used by GJK and EPA + int max_contacts; // set to max number of contact points to recover + mjtNum dist_cutoff; // set to max geom distance to recover // statistics for debugging purposes - mjtNum gjk_dist; // the distance returned by GJK - int gjk_iterations; // number of iterations that GJK ran - int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run) - mjtNum simplex1[12]; // the simplex that GJK returned for obj1 - mjtNum simplex2[12]; // the simplex that GJK returned for obj2 - mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference - int nsimplex; // size of simplex 1 & 2 + int gjk_iterations; // number of iterations that GJK ran + int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run) + mjtNum simplex1[12]; // the simplex that GJK returned for obj1 + mjtNum simplex2[12]; // the simplex that GJK returned for obj2 + mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference + int nsimplex; // size of simplex 1 & 2 }; typedef struct _mjCCDStatus mjCCDStatus; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 5b90a705..630431fd 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1409,15 +1409,15 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, // returns the smallest distance between two geoms (using nativeccd) static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int g2, - mjtNum fromto[6]) { + mjtNum distmax, mjtNum fromto[6]) { mjCCDConfig config; mjCCDStatus status; // set config config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; - config.contacts = 1; // want contacts - config.distances = 1; // want geom distances + config.max_contacts = 1; // want contacts + config.dist_cutoff = distmax; // want geom distances mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); @@ -1425,7 +1425,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); - if (fromto) { + if (fromto && status.nx > 0) { mju_copy3(fromto, status.x1); mju_copy3(fromto+3, status.x2); } @@ -1459,7 +1459,7 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, // use nativecdd if flag is enabled if (mjENABLED(mjENBL_NATIVECCD)) { if (func == mjc_Convex || func == mjc_BoxBox) { - return mj_geomDistanceCCD(m, d, g1, g2, fromto); + return mj_geomDistanceCCD(m, d, g1, g2, distmax, fromto); } } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index a8ff146f..73137be1 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -56,23 +56,25 @@ constexpr char kEllipoid[] = R"( )"; mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], - mjtNum x2[3]) { + mjtNum x2[3], mjtNum cutoff = mjMAXVAL) { mjCCDConfig config; mjCCDStatus status; // set config config.max_iterations = kMaxIterations, config.tolerance = kTolerance, - config.contacts = 0; // no geom contacts needed - config.distances = 1; + config.max_contacts = 0; // no geom contacts needed + config.dist_cutoff = cutoff; mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); mjc_initCCDObj(&obj2, m, d, g2, 0); mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); - if (x1 != nullptr) mju_copy3(x1, status.x1); - if (x2 != nullptr) mju_copy3(x2, status.x2); + if (status.nx > 0) { + if (x1 != nullptr) mju_copy3(x1, status.x1); + if (x2 != nullptr) mju_copy3(x2, status.x2); + } return dist; } @@ -85,8 +87,8 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, // set config config.max_iterations = ccd->max_iterations, config.tolerance = ccd->mpr_tolerance, - config.contacts = 1; - config.distances = 0; // no geom distances needed + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { @@ -141,16 +143,10 @@ using MjGjkTest = MujocoTest; TEST_F(MjGjkTest, SphereSphereDist) { static constexpr char xml[] = R"( - - - - - - - - - - + + + + )"; std::array error; @@ -172,19 +168,38 @@ TEST_F(MjGjkTest, SphereSphereDist) { mj_deleteModel(model); } +TEST_F(MjGjkTest, SphereSphereDistCutoff) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr, .999999); + + EXPECT_EQ(dist, mjMAXVAL); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SphereSphereNoDist) { static constexpr char xml[] = R"( - - - - - - - - - - + + + + )"; std::array error; From 9f51ba50db503b98b978d132b229ba5ec03d0a01 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 14 Nov 2024 03:44:42 -0800 Subject: [PATCH 070/426] Fix field ordering in `mjModel`. `nnames_map` is not required for `mjModel` construction, it is set during construction. PiperOrigin-RevId: 696462793 Change-Id: Ie722e2a04d7fd7ed96634d3304cc94e895ed713b --- doc/includes/references.h | 8 ++++---- include/mujoco/mjmodel.h | 8 ++++---- introspect/structs.py | 14 +++++++------- unity/Runtime/Bindings/MjBindings.cs | 2 +- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 0f8d869a..4d4c6b98 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -948,10 +948,10 @@ struct mjModel_ { int nuser_actuator; // number of mjtNums in actuator_user int nuser_sensor; // number of mjtNums in sensor_user int nnames; // number of chars in all names - int nnames_map; // number of slots in the names hash map int npaths; // number of chars in all paths - // sizes set after mjModel construction (only affect mjData) + // sizes set after mjModel construction + int nnames_map; // number of slots in the names hash map int nM; // number of non-zeros in sparse inertia matrix int nB; // number of non-zeros in sparse body-dof matrix int nC; // number of non-zeros in sparse reduced dof-dof matrix @@ -959,8 +959,8 @@ struct mjModel_ { int ntree; // number of kinematic trees under world body int ngravcomp; // number of bodies with nonzero gravcomp int nemax; // number of potential equality-constraint rows - int njmax; // number of available rows in constraint Jacobian - int nconmax; // number of potential contacts in contact list + int njmax; // number of available rows in constraint Jacobian (legacy) + int nconmax; // number of potential contacts in contact list (legacy) int nuserdata; // number of mjtNums reserved for the user int nsensordata; // number of mjtNums in sensor data vector int npluginstate; // number of mjtNums in plugin state vector diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9bea5917..3cc59311 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -655,10 +655,10 @@ struct mjModel_ { int nuser_actuator; // number of mjtNums in actuator_user int nuser_sensor; // number of mjtNums in sensor_user int nnames; // number of chars in all names - int nnames_map; // number of slots in the names hash map int npaths; // number of chars in all paths - // sizes set after mjModel construction (only affect mjData) + // sizes set after mjModel construction + int nnames_map; // number of slots in the names hash map int nM; // number of non-zeros in sparse inertia matrix int nB; // number of non-zeros in sparse body-dof matrix int nC; // number of non-zeros in sparse reduced dof-dof matrix @@ -666,8 +666,8 @@ struct mjModel_ { int ntree; // number of kinematic trees under world body int ngravcomp; // number of bodies with nonzero gravcomp int nemax; // number of potential equality-constraint rows - int njmax; // number of available rows in constraint Jacobian - int nconmax; // number of potential contacts in contact list + int njmax; // number of available rows in constraint Jacobian (legacy) + int nconmax; // number of potential contacts in contact list (legacy) int nuserdata; // number of mjtNums reserved for the user int nsensordata; // number of mjtNums in sensor data vector int npluginstate; // number of mjtNums in plugin state vector diff --git a/introspect/structs.py b/introspect/structs.py index 0d5107fd..bed95997 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1148,16 +1148,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='number of chars in all names', ), - StructFieldDecl( - name='nnames_map', - type=ValueType(name='int'), - doc='number of slots in the names hash map', - ), StructFieldDecl( name='npaths', type=ValueType(name='int'), doc='number of chars in all paths', ), + StructFieldDecl( + name='nnames_map', + type=ValueType(name='int'), + doc='number of slots in the names hash map', + ), StructFieldDecl( name='nM', type=ValueType(name='int'), @@ -1196,12 +1196,12 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='njmax', type=ValueType(name='int'), - doc='number of available rows in constraint Jacobian', + doc='number of available rows in constraint Jacobian (legacy)', ), StructFieldDecl( name='nconmax', type=ValueType(name='int'), - doc='number of potential contacts in contact list', + doc='number of potential contacts in contact list (legacy)', ), StructFieldDecl( name='nuserdata', diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 32e58267..358787c1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5238,8 +5238,8 @@ public unsafe struct mjModel_ { public int nuser_actuator; public int nuser_sensor; public int nnames; - public int nnames_map; public int npaths; + public int nnames_map; public int nM; public int nB; public int nC; From 536c60a0ac3fe075c427e37ff4815040de4cc006 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 14 Nov 2024 03:51:39 -0800 Subject: [PATCH 071/426] Print arena usage percentage in `mjData` print. PiperOrigin-RevId: 696464274 Change-Id: Ib794148e037521118138bd72c021dbe086c9681a --- src/engine/engine_print.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 2ffd4c24..f7132bc4 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -904,7 +904,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, " total %s\n", memorySize(sizeof(mjData) + d->nbuffer + d->narena)); fprintf(fp, " struct %s\n", memorySize(sizeof(mjData))); fprintf(fp, " buffer %s\n", memorySize(d->nbuffer)); - fprintf(fp, " arena %s\n\n", memorySize(d->narena)); + double arena_percent = 100 * d->maxuse_arena/(double)(d->narena); + fprintf(fp, " arena %s, used %.1f%%\n\n", memorySize(d->narena), arena_percent); // ---------------------------------- print mjData fields From 0a0de6615d4e6b9bdf34ffcea016e31ec585f42d Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 14 Nov 2024 05:11:30 -0800 Subject: [PATCH 072/426] Back out of gjkIntersect when convergence fails due to algorithm oscillating between two support points. Also, fix related bug in box support function. PiperOrigin-RevId: 696482893 Change-Id: Icedaf5489b761b8bf03ec30558b5d1128668a647 --- src/engine/engine_collision_convex.c | 6 +++--- src/engine/engine_collision_gjk.c | 18 +++++++++++++++--- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 1fac0a77..35211837 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -276,9 +276,9 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { mjtNum local_dir[3], tmp[3]; mulMatTVec3(local_dir, mat, dir); - tmp[0] = mju_sign(local_dir[0]) * size[0]; - tmp[1] = mju_sign(local_dir[1]) * size[1]; - tmp[2] = mju_sign(local_dir[2]) * size[2]; + tmp[0] = (local_dir[0] >= 0 ? 1 : -1) * size[0]; + tmp[1] = (local_dir[1] >= 0 ? 1 : -1) * size[1]; + tmp[2] = (local_dir[2] >= 0 ? 1 : -1) * size[2]; // transform result to global frame localToGlobal(res, mat, tmp, pos); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 59b4380f..3db7aabb 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -147,6 +147,7 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact + int backup_gjk = !get_dist; // use gjkIntersect if no geom distances needed mjtNum *simplex1 = status->simplex1; // simplex for obj1 mjtNum *simplex2 = status->simplex2; // simplex for obj2 mjtNum *simplex = status->simplex; // simplex in Minkowski difference @@ -203,10 +204,15 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // tetrahedron is generated and only need contact info; fallback to gjkIntersect to // determine contact - if (!get_dist && n == 3) { + if (n == 3 && backup_gjk) { status->gjk_iterations = k; - status->nx = 0; - return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL; + int ret = gjkIntersect(status, obj1, obj2); + if (ret != -1) { + status->nx = 0; + return ret > 0 ? 0 : mjMAXVAL; + } + k = status->gjk_iterations; + backup_gjk = 0; } // run the distance subalgorithm to compute the barycentric coordinates @@ -364,6 +370,12 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { dist[2] = signedDistance(&normals[6], simplex + s[1], simplex + s[0], simplex + s[3]); dist[3] = signedDistance(&normals[9], simplex + s[0], simplex + s[1], simplex + s[2]); + // if origin is on any affine hull, convergence will fail + if (!dist[3] || !dist[2] || !dist[1] || !dist[0]) { + status->gjk_iterations = k; + return -1; + } + // find the face with the smallest distance to the origin int i = (dist[0] < dist[1]) ? 0 : 1; int j = (dist[2] < dist[3]) ? 2 : 3; From 6b3d34d9be39b38409580ab71cc2b3854cb3d03e Mon Sep 17 00:00:00 2001 From: Balint-H Date: Thu, 14 Nov 2024 17:32:28 +0000 Subject: [PATCH 073/426] Fix swapped params in tendons on XML import --- unity/Runtime/Components/Tendons/MjBaseTendon.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unity/Runtime/Components/Tendons/MjBaseTendon.cs b/unity/Runtime/Components/Tendons/MjBaseTendon.cs index 89a505d1..5d6815f9 100644 --- a/unity/Runtime/Components/Tendons/MjBaseTendon.cs +++ b/unity/Runtime/Components/Tendons/MjBaseTendon.cs @@ -42,8 +42,8 @@ public abstract class MjBaseTendon : MjComponent { protected override void OnParseMjcf(XmlElement mjcf) { Solver.FromMjcf(mjcf); SpringLength = mjcf.GetFloatAttribute("springlength", defaultValue: -1.0f); - Stiffness = mjcf.GetFloatAttribute("damping"); - Damping = mjcf.GetFloatAttribute("stiffness"); + Stiffness = mjcf.GetFloatAttribute("stiffness"); + Damping = mjcf.GetFloatAttribute("damping"); FromMjcf(mjcf); } From 386662106b52ed8d1ffb2df46d220b6a39736855 Mon Sep 17 00:00:00 2001 From: Silvia Cruciani Date: Thu, 14 Nov 2024 09:40:29 -0800 Subject: [PATCH 074/426] Reshape arrays for LQR notebooks for Mujoco sparse2dense Without reshape the collabs results in an error due to incompatible function arguments PiperOrigin-RevId: 696554177 Change-Id: Id4317ee6fccbe2ac912af7a8b07228ddebebbbcf --- python/LQR.ipynb | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/LQR.ipynb b/python/LQR.ipynb index adee43ed..89767ac9 100644 --- a/python/LQR.ipynb +++ b/python/LQR.ipynb @@ -494,10 +494,10 @@ "actuator_moment = np.zeros((model.nu, model.nv))\n", "mujoco.mju_sparse2dense(\n", " actuator_moment,\n", - " data.actuator_moment,\n", + " data.actuator_moment.reshape(-1),\n", " data.moment_rownnz,\n", " data.moment_rowadr,\n", - " data.moment_colind,\n", + " data.moment_colind.reshape(-1),\n", ")\n", "ctrl0 = np.atleast_2d(qfrc0) @ np.linalg.pinv(actuator_moment)\n", "ctrl0 = ctrl0.flatten() # Save the ctrl setpoint.\n", From f91588a171341e105f831a2f149239d427248023 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 15 Nov 2024 06:51:59 -0800 Subject: [PATCH 075/426] Fallback to polytope3 if tetrahedron from GJK has the origin on a face (most likely due to a numerical issue). PiperOrigin-RevId: 696872633 Change-Id: I58edb0fdc30ad472c8f1338df728e8d42c25fc39 --- src/engine/engine_collision_gjk.c | 119 +++++++++++++++-------- test/engine/engine_collision_gjk_test.cc | 40 ++++++++ 2 files changed, 117 insertions(+), 42 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 3db7aabb..da2e782d 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -30,23 +30,22 @@ // implementation adapted from Montanari et al, ToG 2017 static void subdistance(mjtNum lambda[4], const mjtNum simplex[12], int n); -// these internal functions compute the barycentric coordinates of the closest point -// to the origin in the n-simplex, where n = 3, 2, 1 respectively +// compute the barycentric coordinates of the closest point to the origin in the n-simplex, +// where n = 3, 2, 1 respectively static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3], const mjtNum s4[3]); static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]); static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]); -// helper function to compute the support point for EPA -static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, - const mjtNum d[3], mjtNum dnorm); - -// support function tweaked for GJK by taking kth iteration point as input and setting both -// support points to recover witness points +// compute the support point for GJK static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3]); -// linear combination of n 3D vectors +// compute the support point for EPA +static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum d[3], mjtNum dnorm); + +// compute the linear combination of n 3D vectors static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); // one face in a polytope @@ -71,17 +70,17 @@ typedef struct { int nmap; // number of faces in map } Polytope; -// copies a vertex into the polytope and returns its index +// make copy of vertex in polytope and return its index static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]); -// attaches a face to the polytope with the given vertex indices; returns non-zero on error -static void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); +// attach a face to the polytope with the given vertex indices; return distance to origin +static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); -// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +// return 1 if objects are in contact; 0 if not; -1 if inconclusive // status must have initial tetrahedrons static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); -// returns the penetration depth of two convex objects; witness points are in status->{x1, x2} +// return the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); // -------------------------------- inlined 3D vector utils -------------------------------------- @@ -120,7 +119,7 @@ static inline void cross3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) res[2] = v1[0]*v2[1] - v1[1]*v2[0]; } -// returns determinant of the 3x3 matrix with columns v1, v2, v3 +// return determinant of the 3x3 matrix with columns v1, v2, v3 static inline mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { // v1 * (v2 x v3) return v1[0]*(v2[1]*v3[2] - v2[2]*v3[1]) @@ -131,7 +130,7 @@ static inline mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v // ---------------------------------------- GJK --------------------------------------------------- -// returns true if both geoms are discrete shapes (i.e. meshes or boxes with no margin) +// return true if both geoms are discrete shapes (i.e. meshes or boxes with no margin) static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // non-zero margin makes geoms smooth if (obj1->margin != 0 || obj2->margin != 0) return 0; @@ -260,7 +259,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { -// computes the support point in obj1 and obj2 for Minkowski difference +// compute the support point in obj1 and obj2 for Minkowski difference static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3], const mjtNum dir_neg[3]) { // obj1 @@ -284,7 +283,7 @@ static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* -// computes the support points in obj1 and obj2 for the kth approximation point +// compute the support points in obj1 and obj2 for the kth approximation point static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3]) { mjtNum dir[3], dir_neg[3]; @@ -298,7 +297,7 @@ static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj -// helper function to compute the support point in the Minkowski difference +// compute the support point in the Minkowski difference for EPA static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3], mjtNum dnorm) { mjtNum dir[3], dir_neg[3]; @@ -323,7 +322,7 @@ static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj -// helper function to compute the support point in the Minkowski difference (without normalization) +// compute the support point in the Minkowski difference for gjkIntersect (without normalization) static void gjkIntersectSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3]) { mjtNum dir_neg[3] = {-dir[0], -dir[1], -dir[2]}; @@ -353,7 +352,7 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const -// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +// return 1 if objects are in contact; 0 if not; -1 if inconclusive static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex1[12], simplex2[12], simplex[12]; memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12); @@ -496,7 +495,7 @@ static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mj -// returns true only when a and b are both strictly positive or both strictly negative +// return true only when a and b are both strictly positive or both strictly negative static inline int sameSign(mjtNum a, mjtNum b) { if (a > 0 && b > 0) return 1; if (a < 0 && b < 0) return 1; @@ -784,7 +783,7 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { // ---------------------------------------- EPA --------------------------------------------------- -// returns 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2 +// return 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2 static int sameSide(const mjtNum p0[3], const mjtNum p1[3], const mjtNum p2[3], const mjtNum p3[3]) { mjtNum diff1[3], diff2[3], diff3[3], diff4[3], n[3]; @@ -804,7 +803,7 @@ static int sameSide(const mjtNum p0[3], const mjtNum p1[3], -// returns 1 if the origin is contained in the tetrahedron, 0 otherwise +// return 1 if the origin is contained in the tetrahedron, 0 otherwise static int testTetra(const mjtNum p0[3], const mjtNum p1[3], const mjtNum p2[3], const mjtNum p3[3]) { return sameSide(p0, p1, p2, p3) @@ -834,7 +833,7 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) { -// creates a polytope from a 1-simplex (returns 0 if polytope can be created) +// create a polytope from a 1-simplex (returns 0 on success) static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum v1[3], v2[3]; sub3(v1, status->simplex1 + 0, status->simplex2 + 0); @@ -916,7 +915,7 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj -// computes the affine coordinates of p on the triangle v1v2v3 +// compute the affine coordinates of p on the triangle v1v2v3 static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { // compute minors as in S2D @@ -962,7 +961,7 @@ static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2 -// returns true if point p and triangle v1v2v3 intersect +// return true if point p and triangle v1v2v3 intersect static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { mjtNum lambda[3]; @@ -980,7 +979,7 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu -// creates a polytope from a 2-simplex (returns 0 if polytope can be created) +// create a polytope from a 2-simplex (returns 0 on success) static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // get vertices of simplex from GJK const mjtNum *v1 = status->simplex, @@ -1058,23 +1057,58 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj -// creates a polytope from a 3-simplex (returns 0 if polytope can be created) -static int polytope4(Polytope* pt, const mjCCDStatus* status) { +// replace a 3-simplex with one of its faces +static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) { + status->nsimplex = 3; + copy3(status->simplex1 + 0, pt->verts1 + v1); + copy3(status->simplex1 + 3, pt->verts1 + v2); + copy3(status->simplex1 + 6, pt->verts1 + v3); + + copy3(status->simplex2 + 0, pt->verts2 + v1); + copy3(status->simplex2 + 3, pt->verts2 + v2); + copy3(status->simplex2 + 6, pt->verts2 + v3); + + copy3(status->simplex + 0, pt->verts + v1); + copy3(status->simplex + 3, pt->verts + v2); + copy3(status->simplex + 6, pt->verts + v3); + + pt->nfaces = 0; + pt->nmap = 0; + pt->nverts = 0; +} + + + +// create a polytope from a 3-simplex (returns 0 on success) +static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); int v2 = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); int v3 = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6); int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9); - attachFace(pt, v1, v2, v3, 1, 3, 2); - attachFace(pt, v1, v4, v2, 2, 3, 0); - attachFace(pt, v1, v3, v4, 0, 3, 1); - attachFace(pt, v4, v3, v2, 2, 0, 1); + // if the origin is on a face, replace the 3-simplex with a 2-simplex + if (attachFace(pt, v1, v2, v3, 1, 3, 2) == 0.0) { + replaceSimplex3(pt, status, v1, v2, v3); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v1, v4, v2, 2, 3, 0) == 0.0) { + replaceSimplex3(pt, status, v1, v4, v2); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v1, v3, v4, 0, 3, 1) == 0.0) { + replaceSimplex3(pt, status, v1, v3, v4); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v4, v3, v2, 2, 0, 1) == 0.0) { + replaceSimplex3(pt, status, v4, v3, v2); + return polytope3(pt, status, obj1, obj2); + } return 0; } -// copies a vertex into the polytope and returns its index +// make a copy of vertex in polytope and return its index static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) { int n = 3*pt->nverts++; copy3(pt->verts1 + n, v1); @@ -1100,15 +1134,15 @@ static int deleteFace(Polytope* pt, Face* face) { -// returns max number of faces that can be stored in polytope +// return max number of faces that can be stored in polytope static inline int maxFaces(Polytope* pt) { return pt->maxfaces - pt->nfaces; } -// attaches a face to the polytope with the given vertex indices; returns non-zero on error -static inline void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) { +// attach a face to the polytope with the given vertex indices; return distance to origin +static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) { Face* face = &pt->faces[pt->nfaces++]; face->verts[0] = v1; face->verts[1] = v2; @@ -1127,6 +1161,7 @@ static inline void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, in int i = pt->nmap++; face->index = i; pt->map[i] = face; + return face->dist; } @@ -1142,7 +1177,7 @@ typedef struct { -// adds an edge to the horizon +// add an edge to the horizon static inline void addEdge(Horizon* h, int index, int edge) { h->edges[h->nedges] = edge; h->indices[h->nedges++] = index; @@ -1185,7 +1220,7 @@ static int horizonRec(Horizon* h, Face* face, int e) { -// creates horizon given the face as starting point +// create horizon given the face as starting point static void horizon(Horizon* h, Face* face) { if (deleteFace(h->pt, face)) return; @@ -1241,7 +1276,7 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu -// returns the penetration depth of two convex objects; witness points are in status->{x1, x2} +// return the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum dist, tolerance = status->tolerance; int k, kmax = status->max_iterations; @@ -1389,7 +1424,7 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m } else if (status->nsimplex == 3) { ret = polytope3(&pt, status, obj1, obj2); } else { - ret = polytope4(&pt, status); + ret = polytope4(&pt, status, obj1, obj2); } // simplex not on boundary (objects are penetrating) diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 73137be1..3ea33dbd 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -408,6 +408,46 @@ TEST_F(MjGjkTest, BoxBox) { mj_deleteModel(model); } +TEST_F(MjGjkTest, LongBox) { + static constexpr char xml[] = R"( + + + + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dir[3], pos[3]; + mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + + EXPECT_NEAR(dist, -0.01, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], 1, kTolerance); + + EXPECT_NEAR(pos[0], 0, kTolerance); + EXPECT_NEAR(pos[1], 0, kTolerance); + EXPECT_NEAR(pos[2], -0.005, kTolerance); + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { static constexpr char xml[] = R"( From 0cb35a0fe1db1f9bf3a99a86203718ffa43ce497 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 15 Nov 2024 12:23:03 -0800 Subject: [PATCH 076/426] Populate missing fields in mjCCDStatus in GJK code. PiperOrigin-RevId: 696965759 Change-Id: Idf0799a44876e95fc8d688dd39663a34f1808b27 --- src/engine/engine_collision_gjk.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index da2e782d..493a6ed5 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -189,7 +189,8 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { status->gjk_iterations = k; status->nsimplex = 0; status->nx = 0; - return mjMAXVAL; + status->dist = mjMAXVAL; + return status->dist; } } else if (status->dist_cutoff < mjMAXVAL) { mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k); @@ -197,7 +198,8 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { status->gjk_iterations = k; status->nsimplex = 0; status->nx = 0; - return mjMAXVAL; + status->dist = mjMAXVAL; + return status->dist; } } @@ -208,7 +210,8 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int ret = gjkIntersect(status, obj1, obj2); if (ret != -1) { status->nx = 0; - return ret > 0 ? 0 : mjMAXVAL; + status->dist = ret > 0 ? 0 : mjMAXVAL; + return status->dist; } k = status->gjk_iterations; backup_gjk = 0; @@ -398,6 +401,7 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // found origin outside the Minkowski difference (return no collision) if (dot3(&normals[3*index], simplex + s[index]) < 0) { + status->nsimplex = 0; status->gjk_iterations = k; return 0; } From 1feaf8fd672a2a07e69f61ca8d84d92f37c59041 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 15 Nov 2024 16:54:44 -0800 Subject: [PATCH 077/426] Add *rgba fields as jax.Arrays for visual domain rando. PiperOrigin-RevId: 697038459 Change-Id: Ic6eae248e916329daa8fbbeb0770cf689f06b87b --- mjx/mujoco/mjx/_src/ray.py | 5 +++-- mjx/mujoco/mjx/_src/types.py | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/mjx/mujoco/mjx/_src/ray.py b/mjx/mujoco/mjx/_src/ray.py index 9335f4e9..6d000373 100644 --- a/mjx/mujoco/mjx/_src/ray.py +++ b/mjx/mujoco/mjx/_src/ray.py @@ -257,8 +257,6 @@ def ray( dists, ids = [], [] geom_filter = m.geom_bodyid != bodyexclude - geom_filter &= (m.geom_matid != -1) | (m.geom_rgba[:, 3] != 0) - geom_filter &= (m.geom_matid == -1) | (m.mat_rgba[m.geom_matid, 3] != 0) geom_filter &= flg_static | (m.body_weldid[m.geom_bodyid] != 0) if geomgroup: geomgroup = np.array(geomgroup, dtype=bool) @@ -268,6 +266,8 @@ def ray( geom_pnts = jax.vmap(lambda x, y: x.T @ (pnt - y))(d.geom_xmat, d.geom_xpos) geom_vecs = jax.vmap(lambda x: x.T @ vec)(d.geom_xmat) + geom_filter_dyn = (m.geom_matid != -1) | (m.geom_rgba[:, 3] != 0) + geom_filter_dyn &= (m.geom_matid == -1) | (m.mat_rgba[m.geom_matid, 3] != 0) for geom_type, fn in _RAY_FUNC.items(): id_, = np.nonzero(geom_filter & (m.geom_type == geom_type)) @@ -281,6 +281,7 @@ def ray( else: dist = jax.vmap(fn)(*args) + dist = jp.where(geom_filter_dyn[id_], dist, jp.inf) dists, ids = dists + [dist], ids + [id_] if not ids: diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index a940d44e..dcfbdb6f 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -946,7 +946,7 @@ class Model(PyTreeNode): geom_margin: jax.Array geom_gap: jax.Array geom_fluid: np.ndarray - geom_rgba: np.ndarray + geom_rgba: jax.Array site_type: np.ndarray site_bodyid: np.ndarray site_sameframe: np.ndarray @@ -1040,7 +1040,7 @@ class Model(PyTreeNode): tex_nchannel: np.ndarray tex_adr: np.ndarray tex_data: jax.Array - mat_rgba: np.ndarray + mat_rgba: jax.Array mat_texid: np.ndarray pair_dim: np.ndarray pair_geom1: np.ndarray From 072039ac60183fb7cbc155c399278f7e350455bc Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 19 Nov 2024 01:26:45 -0800 Subject: [PATCH 078/426] Update Google Benchmark library to v1.9.0. The old version doesn't build under LLVM 19 with -Werror. PiperOrigin-RevId: 697919386 Change-Id: I6cfa64936af9e2ea3b2c03e724b33f5bde7c799a --- cmake/MujocoDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 8d09b631..bd7bdfe8 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -54,7 +54,7 @@ set(MUJOCO_DEP_VERSION_gtest ) set(MUJOCO_DEP_VERSION_benchmark - 7c8ed6b082aa3c7a3402f18e50da4480421d08fd # v1.8.4 + 24e0bd827a8bec8121b128b0634cb34402fb3259 # Incorporate fix for #1859 CACHE STRING "Version of `benchmark` to be fetched." ) From 5c23ae11efc58653ec836caf8755239d2d37c04b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 19 Nov 2024 06:29:49 -0800 Subject: [PATCH 079/426] Fix bug in material `texrepeat` attribute, fixes #2223. Was mistakenly cast from float to int, bug introduced in 3.2.0 PiperOrigin-RevId: 697993181 Change-Id: I643a07e2b0866a4af56d7cbd3344e764a7caf56f --- doc/changelog.rst | 2 ++ doc/includes/references.h | 18 +++++++++--------- include/mujoco/mjrender.h | 18 +++++++++--------- introspect/structs.py | 4 ++-- src/render/render_context.c | 6 +++--- unity/Runtime/Bindings/MjBindings.cs | 2 +- 6 files changed, 26 insertions(+), 24 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 87225bea..dafe7daf 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -13,6 +13,8 @@ MJX Bug fixes ^^^^^^^^^ - Fixed :github:issue:`2212`, type error in ``mjx.get_data``. +- Fixed bug introduced in 3.2.0 in handling of :ref:`texrepeat` attribute, was mistakenly cast + from ``float`` to ``int``, (fixed :github:issue:`2223`). Version 3.2.5 (Nov 4, 2024) --------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 4d4c6b98..ac6c4b35 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1582,14 +1582,14 @@ struct mjrContext_ { // custom OpenGL context unsigned int auxColor_r[mjNAUX]; // auxiliary color buffer for resolving // materials with textures - int mat_texid[mjMAXMATERIAL*mjNTEXROLE]; // material texture ids (-1: no texture) - int mat_texuniform[mjMAXMATERIAL]; // texture repetition for 2d mapping - int mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping + int mat_texid[mjMAXMATERIAL*mjNTEXROLE]; // material texture ids (-1: no texture) + int mat_texuniform[mjMAXMATERIAL]; // uniform cube mapping + float mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping // texture objects and info - int ntexture; // number of allocated textures - int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) - unsigned int texture[mjMAXTEXTURE]; // texture names + int ntexture; // number of allocated textures + int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) + unsigned int texture[mjMAXTEXTURE]; // texture names // displaylist starting positions unsigned int basePlane; // all planes from model @@ -1628,13 +1628,13 @@ struct mjrContext_ { // custom OpenGL context int windowDoublebuffer; // is default/window framebuffer double buffered // framebuffer - int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN // pixel output format - int readPixelFormat; // default color pixel format for mjr_readPixels + int readPixelFormat; // default color pixel format for mjr_readPixels // depth output format - int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR }; typedef struct mjrContext_ mjrContext; typedef enum mjtGeomInertia_ { // type of inertia inference diff --git a/include/mujoco/mjrender.h b/include/mujoco/mjrender.h index ff9dbe34..9a8ed1b7 100644 --- a/include/mujoco/mjrender.h +++ b/include/mujoco/mjrender.h @@ -115,14 +115,14 @@ struct mjrContext_ { // custom OpenGL context unsigned int auxColor_r[mjNAUX]; // auxiliary color buffer for resolving // materials with textures - int mat_texid[mjMAXMATERIAL*mjNTEXROLE]; // material texture ids (-1: no texture) - int mat_texuniform[mjMAXMATERIAL]; // texture repetition for 2d mapping - int mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping + int mat_texid[mjMAXMATERIAL*mjNTEXROLE]; // material texture ids (-1: no texture) + int mat_texuniform[mjMAXMATERIAL]; // uniform cube mapping + float mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping // texture objects and info - int ntexture; // number of allocated textures - int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) - unsigned int texture[mjMAXTEXTURE]; // texture names + int ntexture; // number of allocated textures + int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) + unsigned int texture[mjMAXTEXTURE]; // texture names // displaylist starting positions unsigned int basePlane; // all planes from model @@ -161,13 +161,13 @@ struct mjrContext_ { // custom OpenGL context int windowDoublebuffer; // is default/window framebuffer double buffered // framebuffer - int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN // pixel output format - int readPixelFormat; // default color pixel format for mjr_readPixels + int readPixelFormat; // default color pixel format for mjr_readPixels // depth output format - int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR }; typedef struct mjrContext_ mjrContext; diff --git a/introspect/structs.py b/introspect/structs.py index bed95997..c50bea6e 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -8596,12 +8596,12 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), extents=(1000,), ), - doc='texture repetition for 2d mapping', + doc='uniform cube mapping', ), StructFieldDecl( name='mat_texrepeat', type=ArrayType( - inner_type=ValueType(name='int'), + inner_type=ValueType(name='float'), extents=(2000,), ), doc='texture repetition for 2d mapping', diff --git a/src/render/render_context.c b/src/render/render_context.c index 4c6c3879..b0b2a361 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -1304,7 +1304,7 @@ static void makeMaterial(const mjModel* m, mjrContext* con) { } if (m->nmat >= mjMAXMATERIAL-1) { - mju_error("Maximum number of materials is 100, got %d", m->nmat); + mju_error("Maximum number of materials is %d, got %d", mjMAXMATERIAL, m->nmat); } for (int i=0; i < m->nmat; i++) { if (m->mat_texid[i*mjNTEXROLE + mjTEXROLE_RGB] >= 0) { @@ -1320,8 +1320,8 @@ static void makeMaterial(const mjModel* m, mjrContext* con) { for (int i=0; i < m->ntex; i++) { if (m->tex_type[i] == mjTEXTURE_SKYBOX) { if (m->nmat >= mjMAXMATERIAL-2) { - mju_error("With skybox, maximum number of materials is 99, got %d", - m->nmat); + mju_error("With skybox, maximum number of materials is %d, got %d", + mjMAXMATERIAL-1, m->nmat); } for (int j=0; j < mjNTEXROLE; j++) { con->mat_texid[mjNTEXROLE * (mjMAXMATERIAL-1) + j] = -1; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 358787c1..a8d063be 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5681,7 +5681,7 @@ public unsafe struct mjrContext_ { public fixed uint auxColor_r[10]; public fixed int mat_texid[10000]; public fixed int mat_texuniform[1000]; - public fixed int mat_texrepeat[2000]; + public fixed float mat_texrepeat[2000]; public int ntexture; public fixed int textureType[1000]; public fixed uint texture[1000]; From e18567545bcc76ae29d14bc0f1411dece3248a2e Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 19 Nov 2024 07:30:01 -0800 Subject: [PATCH 080/426] Update various requirements.txt files. PiperOrigin-RevId: 698009096 Change-Id: Iaa2cdadaf31c16c580ce5c2060c8bf2b8377956e --- mjx/cuda_requirements.txt | 28 +-- mjx/requirements.txt | 295 ++++++++++++++++------------- python/build_requirements.txt | 153 ++++++++------- python/make_sdist_requirements.txt | 37 ++-- 4 files changed, 282 insertions(+), 231 deletions(-) diff --git a/mjx/cuda_requirements.txt b/mjx/cuda_requirements.txt index cc812792..3791dff1 100644 --- a/mjx/cuda_requirements.txt +++ b/mjx/cuda_requirements.txt @@ -1,13 +1,15 @@ --f https://storage.googleapis.com/jax-releases/jax_cuda_releases.html - -jax[cuda12_local]==0.4.18; python_version >= '3.9' \ - --hash=sha256:c3ab72ea2f1c5d8ccf2561e79f6562fb2964629f3e55b3ac1c11c48b64c20336 -jaxlib==0.4.18+cuda12.cudnn89; python_version >= '3.9' \ - --hash=sha256:14f74ff081882ea091c121e355051b35932e39cb7ff7242b88a87f3690f3ca90 \ - --hash=sha256:7c87dc2d68257b02e83c04be88a3c447373ee7077d65f43545bcbda5bfe2231d \ - --hash=sha256:4d16e9c7592e1aaca0b3d28d2c8beba415a2721bb7001f2947728247951a250d \ - --hash=sha256:759c08c69f4a5b1e6b39c3e4eff908a04ce3b2b483bb594ed624407c7d12d110 \ - --hash=sha256:a7a04dbe1851cd50d07691282116aee49a2f0be7838e55b76d7ada86db06be62 \ - --hash=sha256:2bf842db3d58c8c6c52fbc8ed3fabefd7b91a21746cd59d3eaf3522eea229b53 \ - --hash=sha256:35d265ef9bb3835a14580cbaa9402060f117e46056f80e0996405fff3964667a \ - --hash=sha256:0e4352f24d629e912965e6435e140c1b06086243a098651f2d01b75f3738b51c +jax[cuda12_local]==0.4.34; python_version >= '3.10' \ + --hash=sha256:b957ca1fc91f7343f91a186af9f19c7f342c946f95a8c11c7f1e5cdfe2e58d9e +jax[cuda12_local]==0.4.30; python_version == '3.9' \ + --hash=sha256:289b30ae03b52f7f4baf6ef082a9f4e3e29c1080e22d13512c5ecf02d5f1a55b +jax-cuda12-plugin==0.4.34; python_version >= '3.10' \ + --hash=sha256:d035ea72bd9b8a65a6ea621bca1affdd33127fa3a52e7bded7692670d360adab \ + --hash=sha256:db988b7ba5063483a936ddbf162f04d1b4412e0d64340f11788c7bbc877e8a43 \ + --hash=sha256:e23721d1654b311b47cd6b35768520284bd036f8c7e6b11600143258b4a0409a \ + --hash=sha256:b2099a4407225122ff76f6dcdc8dbdae47e6f29343bdfd21460ad337dc34a209 +jax-cuda12-plugin==0.4.30; python_version == '3.9' \ + --hash=sha256:d8d196241b9253ecb1144a4409b5deacbb9771624f097b2bbf025da3c7d8f4f8 +jax-cuda12-pjrt==0.4.34; python_version >= '3.10' \ + --hash=sha256:0c7cc98f962cc7fc8e0a5ea6331b42a0cee516f202f1c3019f6aa5cd9530cca0 +jax-cuda12-pjrt==0.4.30; python_version == '3.9' \ + --hash=sha256:895d0198ad99638fcaf976c47592e2a543eef79ea15fabd24a402d055390c328 diff --git a/mjx/requirements.txt b/mjx/requirements.txt index e6130509..3747653a 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -1,141 +1,178 @@ -absl-py==2.0.0 \ - --hash=sha256:9a28abb62774ae4e8edbe2dd4c49ffcd45a6a848952a5eccc6a49f3f0fc1e2f3 -etils[epath]==1.5.1; python_version >= '3.9' \ - --hash=sha256:2c1bfa2817eb4881cb509097f1e65ac6160126ba74ec47b3bb47ee678628d8c8 -jax==0.4.18; python_version >= '3.9' \ - --hash=sha256:2ded3f558b74593c3533036a90c20d41ea35f35c74b25ca0fc86f4aafc388746 -jaxlib==0.4.18; python_version >= '3.9' \ - --hash=sha256:f0d5414bc79bdd667b81ee3c5836641bbd52d6d9c0054043dec8e025857e1260 \ - --hash=sha256:ed1ba86c1a2adea8235269f3e1f5561696068ca60c68b8e5a6f0eb3b978a305a \ - --hash=sha256:dfe84a294ab3de2557c49a48c0d83c555018a5190aefa1134d5cb3219865edcd \ - --hash=sha256:c429a15165b6b5ded5b0c46c5861d0b978a82aaa2200b2e517366d220f3f01ee \ - --hash=sha256:85572a9fa84a17cffd05b771d528012297be4c0e227a07e7dd082c15094749b8 \ - --hash=sha256:055950e663fdc101b544597c1361596ff82816575640685effd4779949c8cf06 \ - --hash=sha256:4ce7b001fb070e2b7926553bedb9490b7671b3dcc176fd7b52df3932c3593cb0 \ - --hash=sha256:f3a8ce7096b3eadd531773c5ef7f7c3bb7552cdf163d682ccd0b0c7f7240d109 \ - --hash=sha256:e73b17ac3a6a3e034bca5e5752b1bf035a2eb50ced721a4651b287f5e2c672f7 \ - --hash=sha256:02895bc15ec578d3bdbdf2c3a2195852d45870f611b26e2e7261cc6b0353a928 \ - --hash=sha256:a72ee7baf663ed5b9c6a426c1919f3755d4d71d4db6abf1979e6ce1f2451fb7f \ - --hash=sha256:3293689a8bef495c7837a82ca3038b92c4f21204cadbad6f497306c58aa554e1 \ - --hash=sha256:43287e8ece61f69b1d2a13d7f5e4d540c6edf4ab60bc1606b2b5f9321a9e8471 \ - --hash=sha256:89fff93b90d054715db0bc3d3b572b799071e63f1fb44edfb1630c5f53631cfd \ - --hash=sha256:e0d78703fd1219d9875f20c6c692bba0973b744d66791e0b3e3cdb230c65a2a5 \ - --hash=sha256:4771e8439c48d1c3cf65e01016da02c6592a310bf973c9609fc3be7df9b49b22 \ - --hash=sha256:f7787a5531d226d6cc9ec2baa7141260bb713435e1cfc053cb9f5cefa9756ac3 \ - --hash=sha256:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \ - --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \ - --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 -pip==23.3.1 \ - --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b -pytest==7.4.2 \ - --hash=sha256:1d881c6124e08ff0a1bb75ba3ec0bfd8b5354a01c194ddd5a0a870a48d99b002 -pytest-xdist==3.3.1 \ - --hash=sha256:ff9daa7793569e6a68544850fd3927cd257cc03a7ef76c95e86915355e82b5f2 -scipy==1.11.3; python_version >= '3.9' \ - --hash=sha256:90271dbde4be191522b3903fc97334e3956d7cfb9cce3f0718d0ab4fd7d8bfd6 \ - --hash=sha256:d2f6dee6cbb0e263b8142ed587bc93e3ed5e777f1f75448d24fb923d9fd4dce6 \ - --hash=sha256:bae66a2d7d5768eaa33008fa5a974389f167183c87bf39160d3fefe6664f8ddc \ - --hash=sha256:0d3a136ae1ff0883fffbb1b05b0b2fea251cb1046a5077d0b435a1839b3e52b7 \ - --hash=sha256:dfcc1552add7cb7c13fb70efcb2389d0624d571aaf2c80b04117e2755a0c5d15 \ - --hash=sha256:e1f97cd89c0fe1a0685f8f89d85fa305deb3067d0668151571ba50913e445820 \ - --hash=sha256:5f290cf561a4b4edfe8d1001ee4be6da60c1c4ea712985b58bf6bc62badee221 \ - --hash=sha256:00f325434b6424952fbb636506f0567898dca7b0f7654d48f1c382ea338ce9a3 \ - --hash=sha256:5664e364f90be8219283eeb844323ff8cd79d7acbd64e15eb9c46b9bc7f6a42a \ - --hash=sha256:925c6f09d0053b1c0f90b2d92d03b261e889b20d1c9b08a3a51f61afc5f58165 \ - --hash=sha256:033c3fd95d55012dd1148b201b72ae854d5086d25e7c316ec9850de4fe776929 \ - --hash=sha256:3e1a8a4657673bfae1e05e1e1d6e94b0cabe5ed0c7c144c8aa7b7dbb774ce5c1 \ - --hash=sha256:e04aa19acc324a1a076abb4035dabe9b64badb19f76ad9c798bde39d41025cdc \ - --hash=sha256:9885e3e4f13b2bd44aaf2a1a6390a11add9f48d5295f7a592393ceb8991577a3 \ - --hash=sha256:370f569c57e1d888304052c18e58f4a927338eafdaef78613c685ca2ea0d1fa0 \ - --hash=sha256:4b4bb134c7aa457e26cc6ea482b016fef45db71417d55cc6d8f43d799cdf9ef2 \ - --hash=sha256:c77da50c9a91e23beb63c2a711ef9e9ca9a2060442757dffee34ea41847d8156 \ - --hash=sha256:9ea7f579182d83d00fed0e5c11a4aa5ffe01460444219dedc448a36adf0c3917 \ - --hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \ - --hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \ - --hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \ - --hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103 \ - --hash=sha256:cf0dbc4d3fe3107358868a60f263c9d8c2e9ba5de8a934cac4164124f727e6ca -setuptools==70.3.0 \ - --hash=sha256:fe384da74336c398e0d956d1cae0669bc02eed936cdb1d49b57de1990dc11ffc -trimesh==4.0.0 \ - --hash=sha256:c1600e2a1121cff069e89d1403c85c88e49c9dc5b636e96c977a924de432ee50 -wheel==0.41.2 \ - --hash=sha256:75909db2664838d015e3d9139004ee16711748a52c8f336b52882266540215d8 +absl-py==2.1.0 \ + --hash=sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308 +etils[epath]==1.10.0; python_version >= '3.10' \ + --hash=sha256:0777fe60a234b4c65ca53470fc64f2dd2d0c6bca7fcc623fdaa8d7fa5a317098 +etils[epath]==1.5.2; python_version == '3.9' \ + --hash=sha256:6dc882d355e1e98a5d1a148d6323679dc47c9a5792939b9de72615aa4737eb0b +jax==0.4.34; python_version >= '3.10' \ + --hash=sha256:b957ca1fc91f7343f91a186af9f19c7f342c946f95a8c11c7f1e5cdfe2e58d9e +jax==0.4.30; python_version == '3.9' \ + --hash=sha256:289b30ae03b52f7f4baf6ef082a9f4e3e29c1080e22d13512c5ecf02d5f1a55b +jaxlib==0.4.34; python_version >= '3.10' \ + --hash=sha256:6b43a974c5d91a19912d138f2658dd8dbb7d30dcdff5c961d896c673e872b611 \ + --hash=sha256:87f25a477cd279840e53718403f97092eba0e8a945fcab47bcf435b6f9119dda \ + --hash=sha256:7be673a876ebd1aef440fb7e3ebaf99a91abeb550c9728c644b7d7c7b5d7c108 \ + --hash=sha256:c303f5acaf6c56ce5ff133a923c9b6247bdebedde15bd2c893c24be4d8f71306 \ + --hash=sha256:72e22e99a5dc890a64443c3fc12f13f20091f578c405a76de077ba42b4c62cd7 \ + --hash=sha256:901cb4040ed24eae40071d8114ea8d10dff436277fa74a1a5b9e7206f641151c \ + --hash=sha256:48272e9034ff868d4328cf0055a07882fd2be93f59dfb6283af7de491f9d1290 \ + --hash=sha256:1a30771d85fa77f9ab8f18e63240f455ab3a3f87660ed7b8d5eea6ceecbe5c1e \ + --hash=sha256:096f0ca309d41fa692a9d1f2f9baab1c5c8ca0749876ebb3f748e738a27c7ff4 \ + --hash=sha256:c7b3e724a30426a856070aba0192b5d199e95b4411070e7ad96ad8b196877b10 \ + --hash=sha256:133070d4fec5525ffea4dc72956398c1cf647a04dcb37f8a935ee82af78d9965 \ + --hash=sha256:3bcfa639ca3cfaf86c8ceebd5fc0d47300fd98a078014a1d0cc03133e1523d5f \ + --hash=sha256:571ef03259835458111596a71a2f4a6fabf4ec34595df4cea555035362ac5bf0 \ + --hash=sha256:c9d3adcae43a33aad4332be9c2aedc5ef751d1e755f917a5afb30c7872eacaa8 \ + --hash=sha256:8ee3f93836e53c86556ccd9449a4ea43516ee05184d031a71dd692e81259f7d9 \ + --hash=sha256:b0001c8f0e2b1c7bc99e4f314b524a340d25653505c1a1484d4041a9d3617f6f \ + --hash=sha256:d840e64b85f8865404d6d225b9bb340e158df1457152a361b05680e24792b232 \ + --hash=sha256:3e60bc826933082e99b19b87c21818a8d26fcdb01f418d47cedff554746fd6cc \ + --hash=sha256:45d719a2ce0ebf21255a277b71d756f3609b7b5be70cddc5d88fd58c35219de0 \ + --hash=sha256:b7a212a3cb5c6acc201c32ae4f4b5f5a9ac09457fbb77ba8db5ce7e7d4adc214 +jaxlib==0.4.30; python_version == '3.9' \ + --hash=sha256:54987e97a22db70f3829b437b9329e4799d653634bacc8b398554d3b90c76b2a \ + --hash=sha256:f74a6b0e09df4b5e2ee399ebb9f0e01190e26e84ccb0a758fadb516415c07f18 \ + --hash=sha256:11602d5556e8baa2f16314c36518e9be4dfae0c2c256a361403fb29dc9dc79a4 \ + --hash=sha256:3d31e01191ce8052bd611aaf16ff967d8d0ec0b63f1ea4b199020cecb248d667 \ + --hash=sha256:ea3a00005faafbe3c18b178d3b534208b3b4027b2be6230227e7b87ce399fc29 +pip==24.3.1 \ + --hash=sha256:3790624780082365f47549d032f3770eeb2b1e8bd1f7b2e02dace1afa361b4ed +pytest==8.3.3 \ + --hash=sha256:a6853c7375b2663155079443d2e45de913a911a11d669df02a50814944db57b2 +pytest-xdist==3.6.1 \ + --hash=sha256:9ed4adfb68a016610848639bb7e02c9352d5d9f03d04809919e2dafc3be4cca7 +scipy==1.14.1; python_version >= '3.10' \ + --hash=sha256:baff393942b550823bfce952bb62270ee17504d02a1801d7fd0719534dfb9c84 \ + --hash=sha256:5149e3fd2d686e42144a093b206aef01932a0059c2a33ddfa67f5f035bdfe13e \ + --hash=sha256:b99722ea48b7ea25e8e015e8341ae74624f72e5f21fc2abd45f3a93266de4c5d \ + --hash=sha256:0c2f95de3b04e26f5f3ad5bb05e74ba7f68b837133a4492414b3afd79dfe540e \ + --hash=sha256:e0cf28db0f24a38b2a0ca33a85a54852586e43cf6fd876365c86e0657cfe7d73 \ + --hash=sha256:4079b90df244709e675cdc8b93bfd8a395d59af40b72e339c2287c91860deb8e \ + --hash=sha256:1729560c906963fc8389f6aac023739ff3983e727b1a4d87696b7bf108316a79 \ + --hash=sha256:2ff38e22128e6c03ff73b6bb0f85f897d2362f8c052e3b8ad00532198fbdae3f \ + --hash=sha256:8f9ea80f2e65bdaa0b7627fb00cbeb2daf163caa015e59b7516395fe3bd1e066 \ + --hash=sha256:30ac8812c1d2aab7131a79ba62933a2a76f582d5dbbc695192453dae67ad6310 \ + --hash=sha256:eb58ca0abd96911932f688528977858681a59d61a7ce908ffd355957f7025cfc \ + --hash=sha256:2843f2d527d9eebec9a43e6b406fb7266f3af25a751aa91d62ff416f54170bc5 \ + --hash=sha256:af29a935803cc707ab2ed7791c44288a682f9c8107bc00f0eccc4f92c08d6e07 \ + --hash=sha256:631f07b3734d34aced009aaf6fedfd0eb3498a97e581c3b1e5f14a04164a456d \ + --hash=sha256:716e389b694c4bb564b4fc0c51bc84d381735e0d39d3f26ec1af2556ec6aad94 \ + --hash=sha256:fef8c87f8abfb884dac04e97824b61299880c43f4ce675dd2cbeadd3c9b466d2 \ + --hash=sha256:278266012eb69f4a720827bdd2dc54b2271c97d84255b2faaa8f161a158c3b37 \ + --hash=sha256:8475230e55549ab3f207bff11ebfc91c805dc3463ef62eda3ccf593254524ce8 \ + --hash=sha256:3a1b111fac6baec1c1d92f27e76511c9e7218f1695d61b59e05e0fe04dc59617 \ + --hash=sha256:c0ee987efa6737242745f347835da2cc5bb9f1b42996a4d97d5c7ff7928cb6f2 \ + --hash=sha256:2da0469a4ef0ecd3693761acbdc20f2fdeafb69e6819cc081308cc978153c675 \ + --hash=sha256:a49f6ed96f83966f576b33a44257d869756df6cf1ef4934f59dd58b25e0327e5 \ + --hash=sha256:8e32dced201274bf96899e6491d9ba3e9a5f6b336708656466ad0522d8528f69 \ + --hash=sha256:2ff0a7e01e422c15739ecd64432743cf7aae2b03f3084288f399affcefe5222d \ + --hash=sha256:97c5dddd5932bd2a1a31c927ba5e1463a53b87ca96b5c9bdf5dfd6096e27efc3 \ + --hash=sha256:8bddf15838ba768bb5f5083c1ea012d64c9a444e16192762bd858f1e126196d0 \ + --hash=sha256:d0d2821003174de06b69e58cef2316a6622b60ee613121199cb2852a873f8cf3 \ + --hash=sha256:b28d2ca4add7ac16ae8bb6632a3c86e4b9e4d52d3e34267f6e1b0c1f8d87e389 +scipy==1.13.1; python_version == '3.9' \ + --hash=sha256:392e4ec766654852c25ebad4f64e4e584cf19820b980bc04960bca0b0cd6eaa2 \ + --hash=sha256:637e98dcf185ba7f8e663e122ebf908c4702420477ae52a04f9908707456ba4d \ + --hash=sha256:d533654b7d221a6a97304ab63c41c96473ff04459e404b83275b60aa8f4b7004 \ + --hash=sha256:8335549ebbca860c52bf3d02f80784e91a004b71b059e3eea9678ba994796a24 \ + --hash=sha256:436bbb42a94a8aeef855d755ce5a465479c721e9d684de76bf61a62e7c2b81d5 +setuptools==75.5.0 \ + --hash=sha256:87cb777c3b96d638ca02031192d40390e0ad97737e27b6b4fa831bea86f2f829 +trimesh==4.5.2 \ + --hash=sha256:2e50f3a7fd135c3045da887a1b9f91230528f3ce11d2ec1ba44750d82d6b4f73 +wheel==0.45.0 \ + --hash=sha256:52f0baa5e6522155090a09c6bd95718cc46956d1b51d537ea5454249edb671c7 # Transitive dependencies of etils[epath] -fsspec==2023.9.2 \ - --hash=sha256:603dbc52c75b84da501b9b2ec8c11e1f61c25984c4a0dda1f129ef391fbfc9b4 -importlib-resources==6.1.0 \ - --hash=sha256:aa50258bbfa56d4e33fbd8aa3ef48ded10d1735f11532b8df95388cc6bdb7e83 -typing_extensions==4.8.0 \ - --hash=sha256:8f92fc8806f9a6b641eaa5318da32b44d401efaac0f6678c9bc448ba3605faa0 -zipp==3.19.1 \ - --hash=sha256:2828e64edb5386ea6a52e7ba7cdb17bb30a73a858f5eb6eb93d8d36f5ea26091 \ - --hash=sha256:35427f6d5594f4acf82d25541438348c26736fa9b3afa2754bcd63cdb99d8e8f +fsspec==2024.10.0 \ + --hash=sha256:03b9a6785766a4de40368b88906366755e2819e758b83705c88cd7cb5fe81871 +importlib-resources==6.4.5 \ + --hash=sha256:ac29d5f956f01d5e4bb63102a5a19957f1b9175e45649977264a1416783bb717 +typing_extensions==4.12.2 \ + --hash=sha256:04e5ca0351e0f3f85c6853954072df659d0d13fac324d0072316b67d7794700d +zipp==3.21.0 \ + --hash=sha256:ac1bbe05fd2991f160ebce24ffbac5f6d11d83dc90891255885223d42b3cd931 # Transitive dependencies of jax and jaxlib -importlib-metadata==6.8.0; python_version < '3.10' \ - --hash=sha256:3ebb78df84a805d7698245025b975d9d67053cd94c79245ba4b3eb694abe68bb -ml-dtypes==0.3.1; python_version >= '3.9' \ - --hash=sha256:3d8ca0acbd377082792d8b97081ba580abdad67c6afb7f827012c675b052f058 \ - --hash=sha256:4828b62fa3bf1ae35faa40f3db9a38ec72fbce02f328a1d14c3a9da4606af364 \ - --hash=sha256:d1a8dc3bac1da2a17d0e2e4cba36ee89721d0bd33ea4765af2eefb5f41409e0f \ - --hash=sha256:a777928dcba8865ab4a8157eeb25d23aed7bc82e5fd74e1d5eca821d3f148b39 \ - --hash=sha256:5e0b0b6bb07fa5ad11bb61d174667176bee5e05857225067aabfc5adc1b51d23 \ - --hash=sha256:5727effa7650f7ab10906542d137cfb3244fdc3b2b519beff42f82def8ba59be \ - --hash=sha256:42a8980afd8b7c8e270e8b5c260237286b5b26acd276fcb758d13cd7cb567e99 \ - --hash=sha256:cb0c404e0dd3e25b56362c1c1e5de0ef717f727dde59fa721af4ce6ab2acca44 \ - --hash=sha256:510d249a91face47211762eb294d6fe64f325356b965fb6388c1bf51bd339267 \ - --hash=sha256:f83ff080df8910c0f987f615b03e4f8198638e0c00c6e679ea8892dda909763b \ - --hash=sha256:fcae2c69715410d96906e1dfe8f017d9f78a0d10e0df91aae52e91f51fdfe45e \ - --hash=sha256:da274599e4950a9b488d21571061f49a185537cc77f2d3f8121151d58a9e9f16 \ - --hash=sha256:438437e2e614a3c91d75581653b6c40ec890e8b5994d7190a90c931740151c95 \ - --hash=sha256:70984b473db6489ec1d8c79b082a1322105155193049d08a3b0c515094e9777b \ - --hash=sha256:4d94b2d1bed77284694f7fd0479640fa7aa5d96433dca3cbcec407a5ef752e77 \ - --hash=sha256:979d7d196d9a17e0135ae22878f74241fbd3522cef58d7b292f1fd5b32282201 -numpy==1.26.0; python_version >= '3.9' \ - --hash=sha256:166b36197e9debc4e384e9c652ba60c0bacc216d0fc89e78f973a9760b503388 \ - --hash=sha256:f042f66d0b4ae6d48e70e28d487376204d3cbf43b84c03bac57e28dac6151581 \ - --hash=sha256:e5e18e5b14a7560d8acf1c596688f4dfd19b4f2945b245a71e5af4ddb7422feb \ - --hash=sha256:7f6bad22a791226d0a5c7c27a80a20e11cfe09ad5ef9084d4d3fc4a299cca505 \ - --hash=sha256:ee84ca3c58fe48b8ddafdeb1db87388dce2c3c3f701bf447b05e4cfcc3679112 \ - --hash=sha256:637c58b468a69869258b8ae26f4a4c6ff8abffd4a8334c830ffb63e0feefe99a \ - --hash=sha256:306545e234503a24fe9ae95ebf84d25cba1fdc27db971aa2d9f1ab6bba19a9dd \ - --hash=sha256:8c6adc33561bd1d46f81131d5352348350fc23df4d742bb246cdfca606ea1208 \ - --hash=sha256:e062aa24638bb5018b7841977c360d2f5917268d125c833a686b7cbabbec496c \ - --hash=sha256:eae430ecf5794cb7ae7fa3808740b015aa80747e5266153128ef055975a72b99 \ - --hash=sha256:f8db2f125746e44dce707dd44d4f4efeea8d7e2b43aace3f8d1f235cfa2733dd \ - --hash=sha256:0621f7daf973d34d18b4e4bafb210bbaf1ef5e0100b5fa750bd9cde84c7ac292 \ - --hash=sha256:51be5f8c349fdd1a5568e72713a21f518e7d6707bcf8503b528b88d33b57dc68 \ - --hash=sha256:767254ad364991ccfc4d81b8152912e53e103ec192d1bb4ea6b1f5a7117040be \ - --hash=sha256:09aaee96c2cbdea95de76ecb8a586cb687d281c881f5f17bfc0fb7f5890f6b91 \ - --hash=sha256:4a873a8180479bc829313e8d9798d5234dfacfc2e8a7ac188418189bb8eafbd2 \ - --hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \ - --hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \ - --hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \ - --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \ - --hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc -opt-einsum==3.3.0 \ - --hash=sha256:2455e59e3947d3c275477df7f5205b30635e266fe6dc300e3d9f9646bfcea147 +importlib-metadata==8.5.0; python_version == '3.9' \ + --hash=sha256:45e54197d28b7a7f1559e60b95e7c567032b602131fbd588f1497f47880aa68b +ml-dtypes==0.5.0 \ + --hash=sha256:cb5cc7b25acabd384f75bbd78892d0c724943f3e2e1986254665a1aa10982e07 \ + --hash=sha256:54415257f00eb44fbcc807454efac3356f75644f1cbfc2d4e5522a72ae1dacab \ + --hash=sha256:e04fde367b2fe901b1d47234426fe8819909bd1dd862a5adb630f27789c20599 \ + --hash=sha256:d3b3db9990c3840986a0e70524e122cfa32b91139c3653df76121ba7776e015f \ + --hash=sha256:afa08343069874a30812871d639f9c02b4158ace065601406a493a8511180c02 \ + --hash=sha256:a38df8df61194aeaae1ab7579075779b4ad32cd1cffd012c28be227fa7f2a70a \ + --hash=sha256:a988bac6572630e1e9c2edd9b1277b4eefd1c86209e52b0d061b775ac33902ff \ + --hash=sha256:d4b1a70a3e5219790d6b55b9507606fc4e02911d1497d16c18dd721eb7efe7d0 \ + --hash=sha256:dc74fd9995513d33eac63d64e436240f5494ec74d522a9f0920194942fc3d2d7 \ + --hash=sha256:2e7534392682c3098bc7341648c650864207169c654aed83143d7a19c67ae06f \ + --hash=sha256:76942f6aeb5c40766d5ea62386daa4148e6a54322aaf5b53eae9e7553240222f \ + --hash=sha256:60275f2b51b56834e840c4809fca840565f9bf8e9a73f6d8c94f5b5935701215 \ + --hash=sha256:968fede07d1f9b926a63df97d25ac656cac1a57ebd33701734eaf704bc55d8d8 \ + --hash=sha256:c7a9152f5876fef565516aa5dd1dccd6fc298a5891b2467973905103eb5c7856 \ + --hash=sha256:ab046f2ff789b1f11b2491909682c5d089934835f9a760fafc180e47dcb676b8 \ + --hash=sha256:8c32138975797e681eb175996d64356bcfa124bdbb6a70460b9768c2b35a6fa4 \ + --hash=sha256:7ee9c320bb0f9ffdf9f6fa6a696ef2e005d1f66438d6f1c1457338e00a02e8cf \ + --hash=sha256:a03fc861b86cc586728e3d093ba37f0cc05e65330c3ebd7688e7bae8290f8859 \ + --hash=sha256:099e09edd54e676903b4538f3815b5ab96f5b119690514602d96bfdb67172cbe \ + --hash=sha256:5f2b59233a0dbb6a560b3137ed6125433289ccba2f8d9c3695a52423a369ed15 +numpy==2.1.3; python_version >= '3.10' \ + --hash=sha256:747641635d3d44bcb380d950679462fae44f54b131be347d5ec2bce47d3df9ed \ + --hash=sha256:5641516794ca9e5f8a4d17bb45446998c6554704d888f86df9b200e66bdcce56 \ + --hash=sha256:c181ba05ce8299c7aa3125c27b9c2167bca4a4445b7ce73d5febc411ca692e43 \ + --hash=sha256:016d0f6f5e77b0f0d45d77387ffa4bb89816b57c835580c3ce8e099ef830befe \ + --hash=sha256:dc258a761a16daa791081d026f0ed4399b582712e6fc887a95af09df10c5ca57 \ + --hash=sha256:f653490b33e9c3a4c1c01d41bc2aef08f9475af51146e4a7710c450cf9761598 \ + --hash=sha256:96fe52fcdb9345b7cd82ecd34547fca4321f7656d500eca497eb7ea5a926692f \ + --hash=sha256:0d30c543f02e84e92c4b1f415b7c6b5326cbe45ee7882b6b77db7195fb971e3a \ + --hash=sha256:2312b2aa89e1f43ecea6da6ea9a810d06aae08321609d8dc0d0eda6d946a541b \ + --hash=sha256:8637dcd2caa676e475503d1f8fdb327bc495554e10838019651b76d17b98e512 \ + --hash=sha256:0fa14563cc46422e99daef53d725d0c326e99e468a9320a240affffe87852564 \ + --hash=sha256:a6b46587b14b888e95e4a24d7b13ae91fa22386c199ee7b418f449032b2fa3b8 \ + --hash=sha256:13138eadd4f4da03074851a698ffa7e405f41a0845a6b1ad135b81596e4e9958 \ + --hash=sha256:f55ba01150f52b1027829b50d70ef1dafd9821ea82905b63936668403c3b471e \ + --hash=sha256:d89dd2b6da69c4fff5e39c28a382199ddedc3a5be5390115608345dec660b9e2 \ + --hash=sha256:bc6f24b3d1ecc1eebfbf5d6051faa49af40b03be1aaa781ebdadcbc090b4539b \ + --hash=sha256:762479be47a4863e261a840e8e01608d124ee1361e48b96916f38b119cfda04a \ + --hash=sha256:973faafebaae4c0aaa1a1ca1ce02434554d67e628b8d805e61f874b84e136b09 \ + --hash=sha256:576a1c1d25e9e02ed7fa5477f30a127fe56debd53b8d2c89d5578f9857d03ca9 \ + --hash=sha256:c80e4a09b3d95b4e1cac08643f1152fa71a0a821a2d4277334c88d54b2219a41 \ + --hash=sha256:4d1167c53b93f1f5d8a139a742b3c6f4d429b54e74e6b57d0eff40045187b15d \ + --hash=sha256:ecc76a9ba2911d8d37ac01de72834d8849e55473457558e12995f4cd53e778e0 \ + --hash=sha256:78574ac2d1a4a02421f25da9559850d59457bac82f2b8d7a44fe83a64f770098 \ + --hash=sha256:e711e02f49e176a01d0349d82cb5f05ba4db7d5e7e0defd026328e5cfb3226d3 \ + --hash=sha256:6a4825252fcc430a182ac4dee5a505053d262c807f8a924603d411f6718b88fd \ + --hash=sha256:825656d0743699c529c5943554d223c021ff0494ff1442152ce887ef4f7561a1 \ + --hash=sha256:b47fbb433d3260adcd51eb54f92a2ffbc90a4595f8970ee00e064c644ac788f5 \ + --hash=sha256:c894b4305373b9c5576d7a12b473702afdf48ce5369c074ba304cc5ad8730dff \ +numpy==2.0.2; python_version == '3.9' \ + --hash=sha256:a3f4ab0caa7f053f6797fcd4e1e25caee367db3112ef2b6ef82d749530768c73 \ + --hash=sha256:f26b258c385842546006213344c50655ff1555a9338e2e5e02a0756dc3e803dd \ + --hash=sha256:1e795a8be3ddbac43274f18588329c72939870a16cae810c2b73461c40718ab1 \ + --hash=sha256:97032a27bd9d8988b9a97a8c4d2c9f2c15a81f61e2f21404d7e8ef00cb5be729 \ + --hash=sha256:2b2955fa6f11907cf7a70dab0d0755159bca87755e831e47932367fc8f2f2d0b \ + --hash=sha256:423e89b23490805d2a5a96fe40ec507407b8ee786d66f7328be214f9679df6dd \ + --hash=sha256:9059e10581ce4093f735ed23f3b9d283b9d517ff46009ddd485f1747eb22653c +opt-einsum==3.4.0 \ + --hash=sha256:69bb92469f86a1565195ece4ac0323943e83477171b91d24c35afe028a90d7cd # Transitive dependencies of pytest -attrs==23.1.0; platform_system == 'Windows' \ - --hash=sha256:1f28b4522cdc2fb4256ac1a020c78acf9cba2c6b461ccd2c126f3aa8e8335d04 +attrs==24.2.0; platform_system == 'Windows' \ + --hash=sha256:81921eb96de3191c8258c199618104dd27ac608d9366f5e35d011eae1867ede2 colorama==0.4.6; platform_system == 'Windows' \ --hash=sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6 -exceptiongroup==1.1.3; python_version < '3.11' \ - --hash=sha256:343280667a4585d195ca1cf9cef84a4e178c4b6cf2274caef9859782b567d5e3 +exceptiongroup==1.2.2; python_version < '3.11' \ + --hash=sha256:3111b9d131c238bec2f8f516e123e14ba243563fb135d3fe885990585aa7795b iniconfig==2.0.0 \ --hash=sha256:b6a85871a79d2e3b22d2d1b94ac2824226a63c6b741c88f7ae975f18b6778374 -packaging==23.2 \ - --hash=sha256:8c491190033a9af7e1d931d0b5dacc2ef47509b34dd0de67ed209b5203fc88c7 -pluggy==1.3.0 \ - --hash=sha256:d89c696a773f8bd377d18e5ecda92b7a3793cbe66c87060a6fb58c7b6e1061f7 -pyparsing==3.1.1 \ - --hash=sha256:32c7c0b711493c72ff18a981d24f28aaf9c1fb7ed5e9667c9e84e3db623bdbfb -tomli==2.0.1; python_version < '3.11' \ - --hash=sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc +packaging==24.2 \ + --hash=sha256:09abb1bccd265c01f4a3aa3f7a7db064b36514d2cba19a2f694fe6150451a759 +pluggy==1.5.0 \ + --hash=sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669 +tomli==2.1.0; python_version < '3.11' \ + --hash=sha256:a5c57c3d1c56f5ccdf89f6523458f60ef716e210fc47c4cfb188c5ba473e0391 # Transitive dependencies of pytest-xdist -execnet==2.0.2 \ - --hash=sha256:88256416ae766bc9e8895c76a87928c0012183da3cc4fc18016e6f050e025f41 +execnet==2.1.1 \ + --hash=sha256:26dee51f1b80cebd6d0ca8e74dd8745419761d3bef34163928cbebbdc4749fdc diff --git a/python/build_requirements.txt b/python/build_requirements.txt index 62adb390..53475f10 100644 --- a/python/build_requirements.txt +++ b/python/build_requirements.txt @@ -1,86 +1,99 @@ -absl-py==2.0.0 \ - --hash=sha256:9a28abb62774ae4e8edbe2dd4c49ffcd45a6a848952a5eccc6a49f3f0fc1e2f3 +absl-py==2.1.0 \ + --hash=sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308 auditwheel==5.4.0; platform_system == 'Linux' \ --hash=sha256:d8410a17523427ba3f7b60c9701d23de28b8f94fa5dab732aa6c30d160df8127 -build==1.0.3 \ - --hash=sha256:589bf99a67df7c9cf07ec0ac0e5e2ea5d4b37ac63301c4986d1acb126aa83f8f -etils[epath]==1.5.1; python_version >= '3.9' \ - --hash=sha256:2c1bfa2817eb4881cb509097f1e65ac6160126ba74ec47b3bb47ee678628d8c8 -glfw==2.6.2 \ - --hash=sha256:c2dcf2395d99ff2506428213bee305bb9ba024043d1f574216e61e6f5df808e9 \ - --hash=sha256:c385c9976133aed57ff4f0ff5210276844cefb4d8a7bf61bbcc1caf10385744b \ - --hash=sha256:fb56cd24f9e173cdddc8f6ebaac4f52304a1dfcae1dadd583038355b73c1ae06 \ - --hash=sha256:faa5596aad5490cdd8657931a66636508c1015a8b7b47018318bb72fcd2b9014 \ - --hash=sha256:d8e4f087eba45f7f4815e3e912867ed5ca16d1047b0958c52047f5b53be67059 -numpy==1.26.0; python_version >= '3.9' \ - --hash=sha256:166b36197e9debc4e384e9c652ba60c0bacc216d0fc89e78f973a9760b503388 \ - --hash=sha256:f042f66d0b4ae6d48e70e28d487376204d3cbf43b84c03bac57e28dac6151581 \ - --hash=sha256:e5e18e5b14a7560d8acf1c596688f4dfd19b4f2945b245a71e5af4ddb7422feb \ - --hash=sha256:7f6bad22a791226d0a5c7c27a80a20e11cfe09ad5ef9084d4d3fc4a299cca505 \ - --hash=sha256:ee84ca3c58fe48b8ddafdeb1db87388dce2c3c3f701bf447b05e4cfcc3679112 \ - --hash=sha256:637c58b468a69869258b8ae26f4a4c6ff8abffd4a8334c830ffb63e0feefe99a \ - --hash=sha256:306545e234503a24fe9ae95ebf84d25cba1fdc27db971aa2d9f1ab6bba19a9dd \ - --hash=sha256:8c6adc33561bd1d46f81131d5352348350fc23df4d742bb246cdfca606ea1208 \ - --hash=sha256:e062aa24638bb5018b7841977c360d2f5917268d125c833a686b7cbabbec496c \ - --hash=sha256:eae430ecf5794cb7ae7fa3808740b015aa80747e5266153128ef055975a72b99 \ - --hash=sha256:f8db2f125746e44dce707dd44d4f4efeea8d7e2b43aace3f8d1f235cfa2733dd \ - --hash=sha256:0621f7daf973d34d18b4e4bafb210bbaf1ef5e0100b5fa750bd9cde84c7ac292 \ - --hash=sha256:51be5f8c349fdd1a5568e72713a21f518e7d6707bcf8503b528b88d33b57dc68 \ - --hash=sha256:767254ad364991ccfc4d81b8152912e53e103ec192d1bb4ea6b1f5a7117040be \ - --hash=sha256:09aaee96c2cbdea95de76ecb8a586cb687d281c881f5f17bfc0fb7f5890f6b91 \ - --hash=sha256:4a873a8180479bc829313e8d9798d5234dfacfc2e8a7ac188418189bb8eafbd2 \ - --hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \ - --hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \ - --hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \ - --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \ - --hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc \ - --hash=sha256:79d9c3c363cbd919d879dd38aeb19d96be8699eda2af5f3f3c97bf774e1e6438 -pip==23.3.1 \ - --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b +build==1.2.2.post1 \ + --hash=sha256:1d61c0887fa860c01971625baae8bdd338e517b836a2f70dd1f7aa3a6b2fc5b5 +etils[epath]==1.10.0; python_version >= '3.10' \ + --hash=sha256:0777fe60a234b4c65ca53470fc64f2dd2d0c6bca7fcc623fdaa8d7fa5a317098 +etils[epath]==1.5.2; python_version == '3.9' \ + --hash=sha256:6dc882d355e1e98a5d1a148d6323679dc47c9a5792939b9de72615aa4737eb0b +glfw==2.7.0 \ + --hash=sha256:20d4b31a5a6a61fb787b25f8408204e0e248313cc500953071d13d30a2e5cc9d \ + --hash=sha256:d8630dd9673860c427abde5b79bbc348e02eccde8a3f2a802c5a2a4fb5d79fb8 \ + --hash=sha256:e33568b0aba2045a3d7555f22fcf83fafcacc7c2fc4cb995741894ea51e43ab6 \ + --hash=sha256:56ea163c964bb0bc336def2d6a6a1bd42f9db4b870ef834ac77d7b7ee68b8dfc \ + --hash=sha256:bd82849edcceda4e262bd1227afaa74b94f9f0731c1197863cd25c15bfc613fc +numpy==2.1.3; python_version >= '3.10' \ + --hash=sha256:747641635d3d44bcb380d950679462fae44f54b131be347d5ec2bce47d3df9ed \ + --hash=sha256:5641516794ca9e5f8a4d17bb45446998c6554704d888f86df9b200e66bdcce56 \ + --hash=sha256:c181ba05ce8299c7aa3125c27b9c2167bca4a4445b7ce73d5febc411ca692e43 \ + --hash=sha256:016d0f6f5e77b0f0d45d77387ffa4bb89816b57c835580c3ce8e099ef830befe \ + --hash=sha256:dc258a761a16daa791081d026f0ed4399b582712e6fc887a95af09df10c5ca57 \ + --hash=sha256:f653490b33e9c3a4c1c01d41bc2aef08f9475af51146e4a7710c450cf9761598 \ + --hash=sha256:96fe52fcdb9345b7cd82ecd34547fca4321f7656d500eca497eb7ea5a926692f \ + --hash=sha256:0d30c543f02e84e92c4b1f415b7c6b5326cbe45ee7882b6b77db7195fb971e3a \ + --hash=sha256:2312b2aa89e1f43ecea6da6ea9a810d06aae08321609d8dc0d0eda6d946a541b \ + --hash=sha256:8637dcd2caa676e475503d1f8fdb327bc495554e10838019651b76d17b98e512 \ + --hash=sha256:0fa14563cc46422e99daef53d725d0c326e99e468a9320a240affffe87852564 \ + --hash=sha256:a6b46587b14b888e95e4a24d7b13ae91fa22386c199ee7b418f449032b2fa3b8 \ + --hash=sha256:13138eadd4f4da03074851a698ffa7e405f41a0845a6b1ad135b81596e4e9958 \ + --hash=sha256:f55ba01150f52b1027829b50d70ef1dafd9821ea82905b63936668403c3b471e \ + --hash=sha256:d89dd2b6da69c4fff5e39c28a382199ddedc3a5be5390115608345dec660b9e2 \ + --hash=sha256:bc6f24b3d1ecc1eebfbf5d6051faa49af40b03be1aaa781ebdadcbc090b4539b \ + --hash=sha256:762479be47a4863e261a840e8e01608d124ee1361e48b96916f38b119cfda04a \ + --hash=sha256:973faafebaae4c0aaa1a1ca1ce02434554d67e628b8d805e61f874b84e136b09 \ + --hash=sha256:576a1c1d25e9e02ed7fa5477f30a127fe56debd53b8d2c89d5578f9857d03ca9 \ + --hash=sha256:c80e4a09b3d95b4e1cac08643f1152fa71a0a821a2d4277334c88d54b2219a41 \ + --hash=sha256:4d1167c53b93f1f5d8a139a742b3c6f4d429b54e74e6b57d0eff40045187b15d \ + --hash=sha256:ecc76a9ba2911d8d37ac01de72834d8849e55473457558e12995f4cd53e778e0 \ + --hash=sha256:78574ac2d1a4a02421f25da9559850d59457bac82f2b8d7a44fe83a64f770098 \ + --hash=sha256:e711e02f49e176a01d0349d82cb5f05ba4db7d5e7e0defd026328e5cfb3226d3 \ + --hash=sha256:6a4825252fcc430a182ac4dee5a505053d262c807f8a924603d411f6718b88fd \ + --hash=sha256:825656d0743699c529c5943554d223c021ff0494ff1442152ce887ef4f7561a1 \ + --hash=sha256:b47fbb433d3260adcd51eb54f92a2ffbc90a4595f8970ee00e064c644ac788f5 \ + --hash=sha256:c894b4305373b9c5576d7a12b473702afdf48ce5369c074ba304cc5ad8730dff +numpy==2.0.2; python_version == '3.9' \ + --hash=sha256:a3f4ab0caa7f053f6797fcd4e1e25caee367db3112ef2b6ef82d749530768c73 \ + --hash=sha256:f26b258c385842546006213344c50655ff1555a9338e2e5e02a0756dc3e803dd \ + --hash=sha256:1e795a8be3ddbac43274f18588329c72939870a16cae810c2b73461c40718ab1 \ + --hash=sha256:97032a27bd9d8988b9a97a8c4d2c9f2c15a81f61e2f21404d7e8ef00cb5be729 \ + --hash=sha256:2b2955fa6f11907cf7a70dab0d0755159bca87755e831e47932367fc8f2f2d0b \ + --hash=sha256:423e89b23490805d2a5a96fe40ec507407b8ee786d66f7328be214f9679df6dd \ + --hash=sha256:9059e10581ce4093f735ed23f3b9d283b9d517ff46009ddd485f1747eb22653c +pip==24.3.1 \ + --hash=sha256:3790624780082365f47549d032f3770eeb2b1e8bd1f7b2e02dace1afa361b4ed PyOpenGL==3.1.7 \ --hash=sha256:a6ab19cf290df6101aaf7470843a9c46207789855746399d0af92521a0a92b7a -pytest==7.4.2 \ - --hash=sha256:1d881c6124e08ff0a1bb75ba3ec0bfd8b5354a01c194ddd5a0a870a48d99b002 -setuptools==70.3.0 \ - --hash=sha256:fe384da74336c398e0d956d1cae0669bc02eed936cdb1d49b57de1990dc11ffc -wheel==0.41.2 \ - --hash=sha256:75909db2664838d015e3d9139004ee16711748a52c8f336b52882266540215d8 +pytest==8.3.3 \ + --hash=sha256:a6853c7375b2663155079443d2e45de913a911a11d669df02a50814944db57b2 +setuptools==75.5.0 \ + --hash=sha256:87cb777c3b96d638ca02031192d40390e0ad97737e27b6b4fa831bea86f2f829 +wheel==0.45.0 \ + --hash=sha256:52f0baa5e6522155090a09c6bd95718cc46956d1b51d537ea5454249edb671c7 # Transitive dependencies of auditwheel -pyelftools==0.30; platform_system == 'Linux' \ - --hash=sha256:544c3440eddb9a0dce70b6611de0b28163d71def759d2ed57a0d00118fc5da86 +pyelftools==0.31; platform_system == 'Linux' \ + --hash=sha256:f52de7b3c7e8c64c8abc04a79a1cf37ac5fb0b8a49809827130b858944840607 # Transitive dependencies of build colorama==0.4.6; platform_system == 'Windows' \ --hash=sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6 -importlib-metadata==6.8.0; python_version < '3.10' \ - --hash=sha256:3ebb78df84a805d7698245025b975d9d67053cd94c79245ba4b3eb694abe68bb -packaging==23.2 \ - --hash=sha256:8c491190033a9af7e1d931d0b5dacc2ef47509b34dd0de67ed209b5203fc88c7 -pyproject_hooks==1.0.0 \ - --hash=sha256:283c11acd6b928d2f6a7c73fa0d01cb2bdc5f07c57a2eeb6e83d5e56b97976f8 -tomli==2.0.1; python_version < '3.11' \ - --hash=sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc +importlib-metadata==8.5.0; python_version == '3.9' \ + --hash=sha256:45e54197d28b7a7f1559e60b95e7c567032b602131fbd588f1497f47880aa68b +packaging==24.2 \ + --hash=sha256:09abb1bccd265c01f4a3aa3f7a7db064b36514d2cba19a2f694fe6150451a759 +pyproject_hooks==1.2.0 \ + --hash=sha256:9e5c6bfa8dcc30091c74b0cf803c81fdd29d94f01992a7707bc97babb1141913 +tomli==2.1.0; python_version < '3.11' \ + --hash=sha256:a5c57c3d1c56f5ccdf89f6523458f60ef716e210fc47c4cfb188c5ba473e0391 # Transitive dependencies of etils[epath] -fsspec==2023.9.2 \ - --hash=sha256:603dbc52c75b84da501b9b2ec8c11e1f61c25984c4a0dda1f129ef391fbfc9b4 -importlib-resources==6.1.0 \ - --hash=sha256:aa50258bbfa56d4e33fbd8aa3ef48ded10d1735f11532b8df95388cc6bdb7e83 -typing_extensions==4.8.0 \ - --hash=sha256:8f92fc8806f9a6b641eaa5318da32b44d401efaac0f6678c9bc448ba3605faa0 -zipp==3.19.1 \ - --hash=sha256:2828e64edb5386ea6a52e7ba7cdb17bb30a73a858f5eb6eb93d8d36f5ea26091 \ - --hash=sha256:35427f6d5594f4acf82d25541438348c26736fa9b3afa2754bcd63cdb99d8e8f +fsspec==2024.10.0 \ + --hash=sha256:03b9a6785766a4de40368b88906366755e2819e758b83705c88cd7cb5fe81871 +importlib-resources==6.4.5 \ + --hash=sha256:ac29d5f956f01d5e4bb63102a5a19957f1b9175e45649977264a1416783bb717 +typing_extensions==4.12.2 \ + --hash=sha256:04e5ca0351e0f3f85c6853954072df659d0d13fac324d0072316b67d7794700d +zipp==3.21.0 \ + --hash=sha256:ac1bbe05fd2991f160ebce24ffbac5f6d11d83dc90891255885223d42b3cd931 # Transitive dependencies of pytest -attrs==23.1.0; platform_system == 'Windows' \ - --hash=sha256:1f28b4522cdc2fb4256ac1a020c78acf9cba2c6b461ccd2c126f3aa8e8335d04 -exceptiongroup==1.1.3; python_version < '3.11' \ - --hash=sha256:343280667a4585d195ca1cf9cef84a4e178c4b6cf2274caef9859782b567d5e3 +attrs==24.2.0; platform_system == 'Windows' \ + --hash=sha256:81921eb96de3191c8258c199618104dd27ac608d9366f5e35d011eae1867ede2 +exceptiongroup==1.2.2; python_version < '3.11' \ + --hash=sha256:3111b9d131c238bec2f8f516e123e14ba243563fb135d3fe885990585aa7795b iniconfig==2.0.0 \ --hash=sha256:b6a85871a79d2e3b22d2d1b94ac2824226a63c6b741c88f7ae975f18b6778374 -pluggy==1.3.0 \ - --hash=sha256:d89c696a773f8bd377d18e5ecda92b7a3793cbe66c87060a6fb58c7b6e1061f7 -pyparsing==3.1.1 \ - --hash=sha256:32c7c0b711493c72ff18a981d24f28aaf9c1fb7ed5e9667c9e84e3db623bdbfb +pluggy==1.5.0 \ + --hash=sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669 diff --git a/python/make_sdist_requirements.txt b/python/make_sdist_requirements.txt index b5839397..21c39fdf 100644 --- a/python/make_sdist_requirements.txt +++ b/python/make_sdist_requirements.txt @@ -1,23 +1,22 @@ -absl-py==2.0.0 \ - --hash=sha256:9a28abb62774ae4e8edbe2dd4c49ffcd45a6a848952a5eccc6a49f3f0fc1e2f3 -build==1.0.3 \ - --hash=sha256:589bf99a67df7c9cf07ec0ac0e5e2ea5d4b37ac63301c4986d1acb126aa83f8f -pip==23.3.1 \ - --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b -setuptools==70.3.0 \ - --hash=sha256:fe384da74336c398e0d956d1cae0669bc02eed936cdb1d49b57de1990dc11ffc +absl-py==2.1.0 \ + --hash=sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308 +build==1.2.2.post1 \ + --hash=sha256:1d61c0887fa860c01971625baae8bdd338e517b836a2f70dd1f7aa3a6b2fc5b5 +pip==24.3.1 \ + --hash=sha256:3790624780082365f47549d032f3770eeb2b1e8bd1f7b2e02dace1afa361b4ed +setuptools==75.5.0 \ + --hash=sha256:87cb777c3b96d638ca02031192d40390e0ad97737e27b6b4fa831bea86f2f829 # Transitive dependencies of build colorama==0.4.6; platform_system == 'Windows' \ --hash=sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6 -importlib-metadata==6.8.0; python_version < '3.10' \ - --hash=sha256:3ebb78df84a805d7698245025b975d9d67053cd94c79245ba4b3eb694abe68bb -packaging==23.2 \ - --hash=sha256:8c491190033a9af7e1d931d0b5dacc2ef47509b34dd0de67ed209b5203fc88c7 -pyproject_hooks==1.0.0 \ - --hash=sha256:283c11acd6b928d2f6a7c73fa0d01cb2bdc5f07c57a2eeb6e83d5e56b97976f8 -tomli==2.0.1; python_version < '3.11' \ - --hash=sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc -zipp==3.19.1 \ - --hash=sha256:2828e64edb5386ea6a52e7ba7cdb17bb30a73a858f5eb6eb93d8d36f5ea26091 \ - --hash=sha256:35427f6d5594f4acf82d25541438348c26736fa9b3afa2754bcd63cdb99d8e8f +importlib-metadata==8.5.0; python_version == '3.9' \ + --hash=sha256:45e54197d28b7a7f1559e60b95e7c567032b602131fbd588f1497f47880aa68b +packaging==24.2 \ + --hash=sha256:09abb1bccd265c01f4a3aa3f7a7db064b36514d2cba19a2f694fe6150451a759 +pyproject_hooks==1.2.0 \ + --hash=sha256:9e5c6bfa8dcc30091c74b0cf803c81fdd29d94f01992a7707bc97babb1141913 +tomli==2.1.0; python_version < '3.11' \ + --hash=sha256:a5c57c3d1c56f5ccdf89f6523458f60ef716e210fc47c4cfb188c5ba473e0391 +zipp==3.21.0 \ + --hash=sha256:ac1bbe05fd2991f160ebce24ffbac5f6d11d83dc90891255885223d42b3cd931 From 821f1d3f9808ba38bcb774d9470b029ac90ed746 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 19 Nov 2024 10:41:15 -0800 Subject: [PATCH 081/426] Do not namespace empty actuator strings during attach. Fixes #2233. PiperOrigin-RevId: 698070772 Change-Id: I19613acfd407ba6a8ede40dad85324a70db480a8 --- src/user/user_objects.cc | 24 ++++++++++++++++++------ test/user/user_api_test.cc | 30 ++++++++++++++++++++---------- 2 files changed, 38 insertions(+), 16 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 35659e3e..8796910d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -5031,8 +5031,12 @@ void mjCEquality::PointToLocal() { void mjCEquality::NameSpace(const mjCModel* m) { mjCBase::NameSpace(m); - spec_name1_ = m->prefix + spec_name1_ + m->suffix; - spec_name2_ = m->prefix + spec_name2_ + m->suffix; + if (!spec_name1_.empty()) { + spec_name1_ = m->prefix + spec_name1_ + m->suffix; + } + if (!spec_name2_.empty()) { + spec_name2_ = m->prefix + spec_name2_ + m->suffix; + } } @@ -5526,7 +5530,9 @@ void mjCWrap::PointToLocal() { void mjCWrap::NameSpace(const mjCModel* m) { name = m->prefix + name + m->suffix; - sidesite = m->prefix + sidesite + m->suffix; + if (!sidesite.empty()) { + sidesite = m->prefix + sidesite + m->suffix; + } } @@ -5724,9 +5730,15 @@ void mjCActuator::NameSpace(const mjCModel* m) { if (!plugin_instance_name.empty()) { plugin_instance_name = m->prefix + plugin_instance_name + m->suffix; } - spec_target_ = m->prefix + spec_target_ + m->suffix; - spec_refsite_ = m->prefix + spec_refsite_ + m->suffix; - spec_slidersite_ = m->prefix + spec_slidersite_ + m->suffix; + if (!spec_target_.empty()) { + spec_target_ = m->prefix + spec_target_ + m->suffix; + } + if (!spec_refsite_.empty()) { + spec_refsite_ = m->prefix + spec_refsite_ + m->suffix; + } + if (!spec_slidersite_.empty()) { + spec_slidersite_ = m->prefix + spec_slidersite_ + m->suffix; + } } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e376d046..9646923d 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -731,6 +731,7 @@ static constexpr char xml_child[] = R"( + @@ -757,6 +758,7 @@ static constexpr char xml_child[] = R"( + @@ -764,8 +766,8 @@ static constexpr char xml_child[] = R"( - - + + )"; @@ -796,6 +798,7 @@ TEST_F(MujocoTest, AttachSame) { + @@ -808,6 +811,7 @@ TEST_F(MujocoTest, AttachSame) { + @@ -832,8 +836,10 @@ TEST_F(MujocoTest, AttachSame) { + + @@ -842,10 +848,10 @@ TEST_F(MujocoTest, AttachSame) { - - - - + + + + )"; @@ -946,6 +952,7 @@ TEST_F(MujocoTest, AttachDifferent) { + @@ -966,6 +973,7 @@ TEST_F(MujocoTest, AttachDifferent) { + @@ -974,8 +982,8 @@ TEST_F(MujocoTest, AttachDifferent) { - - + + )"; @@ -1079,6 +1087,7 @@ TEST_F(MujocoTest, AttachFrame) { + @@ -1100,6 +1109,7 @@ TEST_F(MujocoTest, AttachFrame) { + @@ -1108,8 +1118,8 @@ TEST_F(MujocoTest, AttachFrame) { - - + + )"; From 58d68d08d3141366d366394cbaff908d6f0db39a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 19 Nov 2024 10:46:03 -0800 Subject: [PATCH 082/426] Refactor CopyPlugins() out of TryCompile() PiperOrigin-RevId: 698072233 Change-Id: I2138d9d8383838328611eef463448fe2f15bd173 --- src/user/user_model.cc | 166 +++++++++++++++++++++-------------------- src/user/user_model.h | 1 + 2 files changed, 85 insertions(+), 82 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e01fbc5c..a3e43f45 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2493,7 +2493,90 @@ void mjCModel::CopyTree(mjModel* m) { m->nC = nC = 2 * nOD + nv; } +// copy plugin data +void mjCModel::CopyPlugins(mjModel* m) { + // assign plugin slots and copy plugin config attributes + { + int adr = 0; + for (int i = 0; i < nplugin; ++i) { + m->plugin[i] = plugins_[i]->plugin_slot; + const int size = plugins_[i]->flattened_attributes.size(); + std::memcpy(m->plugin_attr + adr, + plugins_[i]->flattened_attributes.data(), size); + m->plugin_attradr[i] = adr; + adr += size; + } + } + // query and set plugin-related information + { + // set actuator_plugin to the plugin instance ID + std::vector> plugin_to_actuators(nplugin); + for (int i = 0; i < nu; ++i) { + if (actuators_[i]->plugin.active) { + int actuator_plugin = static_cast(actuators_[i]->plugin.element)->id; + m->actuator_plugin[i] = actuator_plugin; + plugin_to_actuators[actuator_plugin].push_back(i); + } else { + m->actuator_plugin[i] = -1; + } + } + + for (int i = 0; i < nbody; ++i) { + if (bodies_[i]->plugin.active) { + m->body_plugin[i] = static_cast(bodies_[i]->plugin.element)->id; + } else { + m->body_plugin[i] = -1; + } + } + + for (int i = 0; i < ngeom; ++i) { + if (geoms_[i]->plugin.active) { + m->geom_plugin[i] = static_cast(geoms_[i]->plugin.element)->id; + } else { + m->geom_plugin[i] = -1; + } + } + + std::vector> plugin_to_sensors(nplugin); + for (int i = 0; i < nsensor; ++i) { + if (sensors_[i]->type == mjSENS_PLUGIN) { + int sensor_plugin = static_cast(sensors_[i]->plugin.element)->id; + m->sensor_plugin[i] = sensor_plugin; + plugin_to_sensors[sensor_plugin].push_back(i); + } else { + m->sensor_plugin[i] = -1; + } + } + + // query plugin->nstate, compute and set plugin_state and plugin_stateadr + // for sensor plugins, also query plugin->nsensordata and set nsensordata + int stateadr = 0; + for (int i = 0; i < nplugin; ++i) { + const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]); + if (!plugin->nstate) { + mju_error("`nstate` is null for plugin at slot %d", m->plugin[i]); + } + int nstate = plugin->nstate(m, i); + m->plugin_stateadr[i] = stateadr; + m->plugin_statenum[i] = nstate; + stateadr += nstate; + if (plugin->capabilityflags & mjPLUGIN_SENSOR) { + for (int sensor_id : plugin_to_sensors[i]) { + if (!plugin->nsensordata) { + mju_error("`nsensordata` is null for plugin at slot %d", m->plugin[i]); + } + int nsensordata = plugin->nsensordata(m, i, sensor_id); + sensors_[sensor_id]->dim = nsensordata; + sensors_[sensor_id]->needstage = + static_cast(plugin->needstage); + this->nsensordata += nsensordata; + } + } + } + m->npluginstate = stateadr; + } +} // copy objects outside kinematic tree void mjCModel::CopyObjects(mjModel* m) { @@ -4061,88 +4144,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { CopyNames(m); CopyPaths(m); CopyTree(m); - - // assign plugin slots and copy plugin config attributes - { - int adr = 0; - for (int i = 0; i < nplugin; ++i) { - m->plugin[i] = plugins_[i]->plugin_slot; - const int size = plugins_[i]->flattened_attributes.size(); - std::memcpy(m->plugin_attr + adr, - plugins_[i]->flattened_attributes.data(), size); - m->plugin_attradr[i] = adr; - adr += size; - } - } - - // query and set plugin-related information - { - // set actuator_plugin to the plugin instance ID - std::vector> plugin_to_actuators(nplugin); - for (int i = 0; i < nu; ++i) { - if (actuators_[i]->plugin.active) { - int actuator_plugin = static_cast(actuators_[i]->plugin.element)->id; - m->actuator_plugin[i] = actuator_plugin; - plugin_to_actuators[actuator_plugin].push_back(i); - } else { - m->actuator_plugin[i] = -1; - } - } - - for (int i = 0; i < nbody; ++i) { - if (bodies_[i]->plugin.active) { - m->body_plugin[i] = static_cast(bodies_[i]->plugin.element)->id; - } else { - m->body_plugin[i] = -1; - } - } - - for (int i = 0; i < ngeom; ++i) { - if (geoms_[i]->plugin.active) { - m->geom_plugin[i] = static_cast(geoms_[i]->plugin.element)->id; - } else { - m->geom_plugin[i] = -1; - } - } - - std::vector> plugin_to_sensors(nplugin); - for (int i = 0; i < nsensor; ++i) { - if (sensors_[i]->type == mjSENS_PLUGIN) { - int sensor_plugin = static_cast(sensors_[i]->plugin.element)->id; - m->sensor_plugin[i] = sensor_plugin; - plugin_to_sensors[sensor_plugin].push_back(i); - } else { - m->sensor_plugin[i] = -1; - } - } - - // query plugin->nstate, compute and set plugin_state and plugin_stateadr - // for sensor plugins, also query plugin->nsensordata and set nsensordata - int stateadr = 0; - for (int i = 0; i < nplugin; ++i) { - const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]); - if (!plugin->nstate) { - mju_error("`nstate` is null for plugin at slot %d", m->plugin[i]); - } - int nstate = plugin->nstate(m, i); - m->plugin_stateadr[i] = stateadr; - m->plugin_statenum[i] = nstate; - stateadr += nstate; - if (plugin->capabilityflags & mjPLUGIN_SENSOR) { - for (int sensor_id : plugin_to_sensors[i]) { - if (!plugin->nsensordata) { - mju_error("`nsensordata` is null for plugin at slot %d", m->plugin[i]); - } - int nsensordata = plugin->nsensordata(m, i, sensor_id); - sensors_[sensor_id]->dim = nsensordata; - sensors_[sensor_id]->needstage = - static_cast(plugin->needstage); - this->nsensordata += nsensordata; - } - } - } - m->npluginstate = stateadr; - } + CopyPlugins(m); // keyframe compilation needs access to nq, nv, na, nmocap, qpos0 ResolveKeyframes(m); diff --git a/src/user/user_model.h b/src/user/user_model.h index 33341228..41865ac6 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -322,6 +322,7 @@ class mjCModel : public mjCModel_, private mjSpec { void CopyPaths(mjModel*); // copy paths, compute path addresses void CopyObjects(mjModel*); // copy objects outside kinematic tree void CopyTree(mjModel*); // copy objects inside kinematic tree + void CopyPlugins(mjModel*); // copy plugin data // objects created here std::vector flexes_; // list of flexes From 34c6f9b76b3579a05c339b3fa5c133af8b594e82 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 19 Nov 2024 14:05:59 -0800 Subject: [PATCH 083/426] Update build_requirements_usd.txt. PiperOrigin-RevId: 698138884 Change-Id: I8a3d093b460bd4acad1fa18806a59c29aaffd4c6 --- python/build_requirements_usd.txt | 135 ++++++++++------------------- python/mujoco/usd/exporter_test.py | 7 +- 2 files changed, 52 insertions(+), 90 deletions(-) diff --git a/python/build_requirements_usd.txt b/python/build_requirements_usd.txt index 898071fd..e6d6323c 100644 --- a/python/build_requirements_usd.txt +++ b/python/build_requirements_usd.txt @@ -1,89 +1,46 @@ -# usd-core is not available for Linux aarch64, so leave it at the end incase it -# fails. -pillow==10.3.0 \ - --hash=sha256:048ad577748b9fa4a99a0548c64f2cb8d672d5bf2e643a739ac8faff1164238c \ - --hash=sha256:048eeade4c33fdf7e08da40ef402e748df113fd0b4584e32c4af74fe78baaeb2 \ - --hash=sha256:0ba26351b137ca4e0db0342d5d00d2e355eb29372c05afd544ebf47c0956ffeb \ - --hash=sha256:0ea2a783a2bdf2a561808fe4a7a12e9aa3799b701ba305de596bc48b8bdfce9d \ - --hash=sha256:1530e8f3a4b965eb6a7785cf17a426c779333eb62c9a7d1bbcf3ffd5bf77a4aa \ - --hash=sha256:16563993329b79513f59142a6b02055e10514c1a8e86dca8b48a893e33cf91e3 \ - --hash=sha256:19aeb96d43902f0a783946a0a87dbdad5c84c936025b8419da0a0cd7724356b1 \ - --hash=sha256:1a1d1915db1a4fdb2754b9de292642a39a7fb28f1736699527bb649484fb966a \ - --hash=sha256:1b87bd9d81d179bd8ab871603bd80d8645729939f90b71e62914e816a76fc6bd \ - --hash=sha256:1dfc94946bc60ea375cc39cff0b8da6c7e5f8fcdc1d946beb8da5c216156ddd8 \ - --hash=sha256:2034f6759a722da3a3dbd91a81148cf884e91d1b747992ca288ab88c1de15999 \ - --hash=sha256:261ddb7ca91fcf71757979534fb4c128448b5b4c55cb6152d280312062f69599 \ - --hash=sha256:2ed854e716a89b1afcedea551cd85f2eb2a807613752ab997b9974aaa0d56936 \ - --hash=sha256:3102045a10945173d38336f6e71a8dc71bcaeed55c3123ad4af82c52807b9375 \ - --hash=sha256:339894035d0ede518b16073bdc2feef4c991ee991a29774b33e515f1d308e08d \ - --hash=sha256:412444afb8c4c7a6cc11a47dade32982439925537e483be7c0ae0cf96c4f6a0b \ - --hash=sha256:4203efca580f0dd6f882ca211f923168548f7ba334c189e9eab1178ab840bf60 \ - --hash=sha256:45ebc7b45406febf07fef35d856f0293a92e7417ae7933207e90bf9090b70572 \ - --hash=sha256:4b5ec25d8b17217d635f8935dbc1b9aa5907962fae29dff220f2659487891cd3 \ - --hash=sha256:4c8e73e99da7db1b4cad7f8d682cf6abad7844da39834c288fbfa394a47bbced \ - --hash=sha256:4e6f7d1c414191c1199f8996d3f2282b9ebea0945693fb67392c75a3a320941f \ - --hash=sha256:4eaa22f0d22b1a7e93ff0a596d57fdede2e550aecffb5a1ef1106aaece48e96b \ - --hash=sha256:50b8eae8f7334ec826d6eeffaeeb00e36b5e24aa0b9df322c247539714c6df19 \ - --hash=sha256:50fd3f6b26e3441ae07b7c979309638b72abc1a25da31a81a7fbd9495713ef4f \ - --hash=sha256:51243f1ed5161b9945011a7360e997729776f6e5d7005ba0c6879267d4c5139d \ - --hash=sha256:5d512aafa1d32efa014fa041d38868fda85028e3f930a96f85d49c7d8ddc0383 \ - --hash=sha256:5f77cf66e96ae734717d341c145c5949c63180842a545c47a0ce7ae52ca83795 \ - --hash=sha256:6b02471b72526ab8a18c39cb7967b72d194ec53c1fd0a70b050565a0f366d355 \ - --hash=sha256:6fb1b30043271ec92dc65f6d9f0b7a830c210b8a96423074b15c7bc999975f57 \ - --hash=sha256:7161ec49ef0800947dc5570f86568a7bb36fa97dd09e9827dc02b718c5643f09 \ - --hash=sha256:72d622d262e463dfb7595202d229f5f3ab4b852289a1cd09650362db23b9eb0b \ - --hash=sha256:74d28c17412d9caa1066f7a31df8403ec23d5268ba46cd0ad2c50fb82ae40462 \ - --hash=sha256:78618cdbccaa74d3f88d0ad6cb8ac3007f1a6fa5c6f19af64b55ca170bfa1edf \ - --hash=sha256:793b4e24db2e8742ca6423d3fde8396db336698c55cd34b660663ee9e45ed37f \ - --hash=sha256:798232c92e7665fe82ac085f9d8e8ca98826f8e27859d9a96b41d519ecd2e49a \ - --hash=sha256:81d09caa7b27ef4e61cb7d8fbf1714f5aec1c6b6c5270ee53504981e6e9121ad \ - --hash=sha256:8ab74c06ffdab957d7670c2a5a6e1a70181cd10b727cd788c4dd9005b6a8acd9 \ - --hash=sha256:8eb0908e954d093b02a543dc963984d6e99ad2b5e36503d8a0aaf040505f747d \ - --hash=sha256:90b9e29824800e90c84e4022dd5cc16eb2d9605ee13f05d47641eb183cd73d45 \ - --hash=sha256:9797a6c8fe16f25749b371c02e2ade0efb51155e767a971c61734b1bf6293994 \ - --hash=sha256:9d2455fbf44c914840c793e89aa82d0e1763a14253a000743719ae5946814b2d \ - --hash=sha256:9d3bea1c75f8c53ee4d505c3e67d8c158ad4df0d83170605b50b64025917f338 \ - --hash=sha256:9e2ec1e921fd07c7cda7962bad283acc2f2a9ccc1b971ee4b216b75fad6f0463 \ - --hash=sha256:9e91179a242bbc99be65e139e30690e081fe6cb91a8e77faf4c409653de39451 \ - --hash=sha256:a0eaa93d054751ee9964afa21c06247779b90440ca41d184aeb5d410f20ff591 \ - --hash=sha256:a2c405445c79c3f5a124573a051062300936b0281fee57637e706453e452746c \ - --hash=sha256:aa7e402ce11f0885305bfb6afb3434b3cd8f53b563ac065452d9d5654c7b86fd \ - --hash=sha256:aff76a55a8aa8364d25400a210a65ff59d0168e0b4285ba6bf2bd83cf675ba32 \ - --hash=sha256:b09b86b27a064c9624d0a6c54da01c1beaf5b6cadfa609cf63789b1d08a797b9 \ - --hash=sha256:b14f16f94cbc61215115b9b1236f9c18403c15dd3c52cf629072afa9d54c1cbf \ - --hash=sha256:b50811d664d392f02f7761621303eba9d1b056fb1868c8cdf4231279645c25f5 \ - --hash=sha256:b7bc2176354defba3edc2b9a777744462da2f8e921fbaf61e52acb95bafa9828 \ - --hash=sha256:c78e1b00a87ce43bb37642c0812315b411e856a905d58d597750eb79802aaaa3 \ - --hash=sha256:c83341b89884e2b2e55886e8fbbf37c3fa5efd6c8907124aeb72f285ae5696e5 \ - --hash=sha256:ca2870d5d10d8726a27396d3ca4cf7976cec0f3cb706debe88e3a5bd4610f7d2 \ - --hash=sha256:ccce24b7ad89adb5a1e34a6ba96ac2530046763912806ad4c247356a8f33a67b \ - --hash=sha256:cd5e14fbf22a87321b24c88669aad3a51ec052eb145315b3da3b7e3cc105b9a2 \ - --hash=sha256:ce49c67f4ea0609933d01c0731b34b8695a7a748d6c8d186f95e7d085d2fe475 \ - --hash=sha256:d33891be6df59d93df4d846640f0e46f1a807339f09e79a8040bc887bdcd7ed3 \ - --hash=sha256:d3b2348a78bc939b4fed6552abfd2e7988e0f81443ef3911a4b8498ca084f6eb \ - --hash=sha256:d886f5d353333b4771d21267c7ecc75b710f1a73d72d03ca06df49b09015a9ef \ - --hash=sha256:d93480005693d247f8346bc8ee28c72a2191bdf1f6b5db469c096c0c867ac015 \ - --hash=sha256:dc1a390a82755a8c26c9964d457d4c9cbec5405896cba94cf51f36ea0d855002 \ - --hash=sha256:dd78700f5788ae180b5ee8902c6aea5a5726bac7c364b202b4b3e3ba2d293170 \ - --hash=sha256:e46f38133e5a060d46bd630faa4d9fa0202377495df1f068a8299fd78c84de84 \ - --hash=sha256:e4b878386c4bf293578b48fc570b84ecfe477d3b77ba39a6e87150af77f40c57 \ - --hash=sha256:f0d0591a0aeaefdaf9a5e545e7485f89910c977087e7de2b6c388aec32011e9f \ - --hash=sha256:fdcbb4068117dfd9ce0138d068ac512843c52295ed996ae6dd1faf537b6dbc27 \ - --hash=sha256:ff61bfd9253c3915e6d41c651d5f962da23eda633cf02262990094a18a55371a - -usd-core==24.8 \ - --hash=sha256:39fe8e266875e1105886cab870df4bcbe2d40a84696177bc574c22b96d843bf6 \ - --hash=sha256:8b38b347dce9336d00dacd4c0e5813bcb43c61e165c653d754e3ee3dc4b2b715 \ - --hash=sha256:d1dfe295ccbc57cac39e6dee2d7ce831d7a6a504b8a17dc974e631e3521f83a6 \ - --hash=sha256:d3e06fd8c953c4de3d24591cdb9e8e65ca28a876519378d1e4a5d9bca419d7c1 \ - --hash=sha256:7c18be89012d03f57d5445101695461c678f370d565259893c1cbcb71a6124e6 \ - --hash=sha256:0a503029fa895c9d2307bde2b15748478a0bcb4e8c4ebfa90c7c613fb8137f9d \ - --hash=sha256:a3e1d9b34d3936716dbce4ee22c135acffab0f58ba9da09d47dc74992cfc5259 \ - --hash=sha256:c8efa7784fd11c3e97dec752d93913a234e976d1ebdf3ba8a23af606c86affb0 \ - --hash=sha256:db4951f221323dea83795015f27105a375f28ddabe15ee237acc0728b8dd0212 \ - --hash=sha256:0de3647478e7165d82d73e4b5eb30a7f83df167cc9b6cec3d19fd75fe6d01302 \ - --hash=sha256:ffdba837680171a468d7ec29edc243fcd47d8d1e044fa810c4037b920d31ef60 \ - --hash=sha256:20c45a3099662d32c1a059f3a08adc7829ef7ce40977c33e28835c6bb34aa321 \ - --hash=sha256:6ecef8b1592a37c03c3cc3255f302cb2f5e67d4b9f691084b4674ed92032243c \ - --hash=sha256:ba067c7476e509b49fa1cec3f19821ab8b10c9b43eeb6cae82676bb498297710 \ - --hash=sha256:f3cafd0ddf647a1a8b2335ba109019628252d775c0b3d0f18407211b7801fe53 +pillow==10.4.0 \ + --hash=sha256:030abdbe43ee02e0de642aee345efa443740aa4d828bfe8e2eb11922ea6a21ea \ + --hash=sha256:e4db64794ccdf6cb83a59d73405f63adbe2a1887012e308828596100a0b2f6cc \ + --hash=sha256:297e388da6e248c98bc4a02e018966af0c5f92dfacf5a5ca22fa01cb3179bca0 \ + --hash=sha256:1ef61f5dd14c300786318482456481463b9d6b91ebe5ef12f405afbba77ed0be \ + --hash=sha256:bee197b30783295d2eb680b311af15a20a8b24024a19c3a26431ff83eb8d1f70 \ + --hash=sha256:6209bb41dc692ddfee4942517c19ee81b86c864b626dbfca272ec0f7cff5d9fb \ + --hash=sha256:8bc1a764ed8c957a2e9cacf97c8b2b053b70307cf2996aafd70e91a082e70df3 \ + --hash=sha256:1d846aea995ad352d4bdcc847535bd56e0fd88d36829d2c90be880ef1ee4668a \ + --hash=sha256:86dcb5a1eb778d8b25659d5e4341269e8590ad6b4e8b44d9f4b07f8d136c414a \ + --hash=sha256:f5b92f4d70791b4a67157321c4e8225d60b119c5cc9aee8ecf153aace4aad4ef \ + --hash=sha256:bf2342ac639c4cf38799a44950bbc2dfcb685f052b9e262f446482afaf4bffca \ + --hash=sha256:29dbdc4207642ea6aad70fbde1a9338753d33fb23ed6956e706936706f52dd80 \ + --hash=sha256:866b6942a92f56300012f5fbac71f2d610312ee65e22f1aa2609e491284e5597 \ + --hash=sha256:673655af3eadf4df6b5457033f086e90299fdd7a47983a13827acf7459c15d94 \ + --hash=sha256:cbed61494057c0f83b83eb3a310f0bf774b09513307c434d4366ed64f4128a91 \ + --hash=sha256:76a911dfe51a36041f2e756b00f96ed84677cdeb75d25c767f296c1c1eda1319 \ + --hash=sha256:bbc527b519bd3aa9d7f429d152fea69f9ad37c95f0b02aebddff592688998abe \ + --hash=sha256:5e84b6cc6a4a3d76c153a6b19270b3526a5a8ed6b09501d3af891daa2a9de7d6 \ + --hash=sha256:5dc6761a6efc781e6a1544206f22c80c3af4c8cf461206d46a1e6006e4429ff3 \ + --hash=sha256:dfe91cb65544a1321e631e696759491ae04a2ea11d36715eca01ce07284738be \ + --hash=sha256:0a9ec697746f268507404647e531e92889890a087e03681a3606d9b920fbee3c \ + --hash=sha256:ecd85a8d3e79cd7158dec1c9e5808e821feea088e2f69a974db5edf84dc53141 \ + --hash=sha256:a985e028fc183bf12a77a8bbf36318db4238a3ded7fa9df1b9a133f1cb79f8fc \ + --hash=sha256:6c762a5b0997f5659a5ef2266abc1d8851ad7749ad9a6a5506eb23d314e4f46b \ + --hash=sha256:e4d49b85c4348ea0b31ea63bc75a9f3857869174e2bf17e7aba02945cd218e6f \ + --hash=sha256:7928ecbf1ece13956b95d9cbcfc77137652b02763ba384d9ab508099a2eca856 \ + --hash=sha256:543f3dc61c18dafb755773efc89aae60d06b6596a63914107f75459cf984164d \ + --hash=sha256:4d9667937cfa347525b319ae34375c37b9ee6b525440f3ef48542fcf66f2731e \ + --hash=sha256:961a7293b2457b405967af9c77dcaa43cc1a8cd50d23c532e62d48ab6cdd56f5 \ + --hash=sha256:b2724fdb354a868ddf9a880cb84d102da914e99119211ef7ecbdc613b8c96b3c \ + --hash=sha256:c76e5786951e72ed3686e122d14c5d7012f16c8303a674d18cdcd6d89557fc5b \ + --hash=sha256:930044bb7679ab003b14023138b50181899da3f25de50e9dbee23b61b4de2126 \ + --hash=sha256:134ace6dc392116566980ee7436477d844520a26a4b1bd4053f6f47d096997fd \ + --hash=sha256:298478fe4f77a4408895605f3482b6cc6222c018b2ce565c2b6b9c354ac3229b \ + --hash=sha256:0ae24a547e8b711ccaaf99c9ae3cd975470e1a30caa80a6aaee9a2f19c05701d +usd-core==24.11; python_version<='3.11' and (platform_machine=='x86_64' or platform_system=='Darwin') \ + --hash=sha256:b25bde521bb65497b8bb882e4dd0de03d111dab4937c941ff4ceea6238933d5b \ + --hash=sha256:a0416e3f5bc120977028d82dda38bd652478042c228d9a7d053f736bb79cde96 \ + --hash=sha256:ecd478c98d4f64b5e4a77e32f3ae5a608535703e9e215d07cf070ad1762bd7aa \ + --hash=sha256:9f2ca0f638068b9ca513fa62371656a0222711630f581fa289744eade8dbb3e0 \ + --hash=sha256:d0e0723854522925599bd0e84ec3f9f254881326192360aad8dc32deab0b06d5 \ + --hash=sha256:6c0d0ab1dc1f9e1b500bc462a5f064d47564b4bc6821f74b4fe359b6c0aa7f30 \ + --hash=sha256:c7086619480b515a9b089676d640421f17718db016807cc2a5496ebeff40573f \ + --hash=sha256:b023f338ccc95a005aae3e1bc68bd35b8f14fd6a75d151ef05347471096d98ed \ + --hash=sha256:ca9dd3afab15e3394268dc93c6e27282191a7652808fca9bba2b47cbafcf176a diff --git a/python/mujoco/usd/exporter_test.py b/python/mujoco/usd/exporter_test.py index c80ae11d..f1f895e3 100644 --- a/python/mujoco/usd/exporter_test.py +++ b/python/mujoco/usd/exporter_test.py @@ -20,11 +20,16 @@ import tempfile from absl.testing import absltest from etils import epath import mujoco -from mujoco.usd import exporter as exporter_module # pylint: disable=g-import-not-at-top + +try: + from mujoco.usd import exporter as exporter_module # pylint: disable=g-import-not-at-top +except ModuleNotFoundError: + exporter_module = None class ExporterTest(absltest.TestCase): + @absltest.skipIf(exporter_module is None, "USD library is not available.") def test_usd_export(self): output_dir_root = os.getenv( From 867e8b8adbf3c78dd6d0cec024e460351bddae18 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 20 Nov 2024 02:29:11 -0800 Subject: [PATCH 084/426] Fix ARM issues with tests with nativeccd. Note: frameless_contact_hfield.xml doesn't work with libccd nor nativeccd on ARM as expected. PiperOrigin-RevId: 698317464 Change-Id: I4c57e04639b9870b416c67d79a16a313c5276cf6 --- test/engine/engine_collision_gjk_test.cc | 5 +++++ test/xml/xml_native_writer_test.cc | 1 + 2 files changed, 6 insertions(+) diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 3ea33dbd..aea0e260 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -335,6 +335,11 @@ TEST_F(MjGjkTest, SmallBoxMesh) { EXPECT_NEAR(dir[1], 0, kTolerance); EXPECT_NEAR(dir[2], 1, kTolerance); + // position + EXPECT_NEAR(mju_abs(pos[0]), 0.08333333, kTolerance); // -pos[0] on ARM + EXPECT_NEAR(pos[1], 0, kTolerance); + EXPECT_NEAR(pos[2], 0, kTolerance); + mj_deleteData(data); mj_deleteModel(model); } diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index cad33aa8..d6a784e2 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1319,6 +1319,7 @@ TEST_F(XMLWriterTest, WriteReadCompare) { absl::StrContains(p.path().string(), "gmsh_") || absl::StrContains(p.path().string(), "shark_") || absl::StrContains(p.path().string(), "cow") || + absl::StrContains(p.path().string(), "frameless_contact_hfield") || absl::StrContains(p.path().string(), "spheremesh")) { continue; } From cd84011d7b1ef67e9be79468951d3be389babe0c Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 20 Nov 2024 04:13:22 -0800 Subject: [PATCH 085/426] Default to nativeccd for geomDistance test. PiperOrigin-RevId: 698340192 Change-Id: I814286903e1726462b24390eb2c3dc5b3b76899e --- test/engine/engine_support_test.cc | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 1f95c660..36dd6e06 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -805,6 +805,9 @@ TEST_F(SupportTest, MulMIsland) { static constexpr char GeomDistanceTestingModel[] = R"( + @@ -853,25 +856,20 @@ TEST_F(SupportTest, GeomDistance) { EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), std::vector{.7, 0, 1, .2, 0, 1})); - // TODO: b/339596989 - Improve the bounds below (mjc_Convex). - // mesh-sphere (close distmax) distmax = 0.701; - eps = 1e-5; + eps = model->opt.ccd_tolerance; EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), DoubleNear(0.7, eps)); - eps = 1e-3; EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), - std::vector{0, 0, .1, 0, 0, .8})); + std::vector{0, 0, .8, 0, 0, .1})); // mesh-sphere (far distmax) distmax = 1.0; - eps = 1e-3; EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), DoubleNear(0.7, eps)); - eps = 2e-2; EXPECT_THAT(fromto, Pointwise(DoubleNear(eps), - std::vector{0, 0, .1, 0, 0, .8})); + std::vector{0, 0, .8, 0, 0, .1})); mj_deleteData(data); mj_deleteModel(model); From aae5fd69064597c3a493c5c905965dd7a890250f Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 20 Nov 2024 04:29:55 -0800 Subject: [PATCH 086/426] Add `nJmom`, the number of non-zeros in sparse `actuator_moment` matrix, to `mjModel`. PiperOrigin-RevId: 698343986 Change-Id: I3d4a4dea1ec095d4e7b231c2fffc0e6241cee39f --- doc/includes/references.h | 5 +-- include/mujoco/mjdata.h | 4 +-- include/mujoco/mjmodel.h | 1 + include/mujoco/mjxmacro.h | 5 +-- introspect/structs.py | 9 ++++-- mjx/mujoco/mjx/_src/io.py | 31 ++++++++++--------- mjx/mujoco/mjx/_src/smooth_test.py | 8 ++--- mjx/mujoco/mjx/_src/types.py | 6 ++-- .../mjx/integration_test/smooth_test.py | 4 +-- src/user/user_model.cc | 12 +++++-- src/user/user_model.h | 1 + test/user/user_model_test.cc | 24 ++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 1 + 13 files changed, 78 insertions(+), 33 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index ac6c4b35..199599f4 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -263,8 +263,8 @@ struct mjData_ { mjtNum* actuator_length; // actuator lengths (nu x 1) int* moment_rownnz; // number of non-zeros in actuator_moment row (nu x 1) int* moment_rowadr; // row start address in colind array (nu x 1) - int* moment_colind; // column indices in sparse Jacobian (nu x nv) - mjtNum* actuator_moment; // actuator moments (nu x nv) + int* moment_colind; // column indices in sparse Jacobian (nJmom x 1) + mjtNum* actuator_moment; // actuator moments (nJmom x 1) // computed by mj_fwdPosition/mj_crb mjtNum* crb; // com-based composite inertia and mass (nbody x 10) @@ -956,6 +956,7 @@ struct mjModel_ { int nB; // number of non-zeros in sparse body-dof matrix int nC; // number of non-zeros in sparse reduced dof-dof matrix int nD; // number of non-zeros in sparse dof-dof matrix + int nJmom; // number of non-zeros in sparse actuator_moment matrix int ntree; // number of kinematic trees under world body int ngravcomp; // number of bodies with nonzero gravcomp int nemax; // number of potential equality-constraint rows diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 96b37acc..7ef96b12 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -291,8 +291,8 @@ struct mjData_ { mjtNum* actuator_length; // actuator lengths (nu x 1) int* moment_rownnz; // number of non-zeros in actuator_moment row (nu x 1) int* moment_rowadr; // row start address in colind array (nu x 1) - int* moment_colind; // column indices in sparse Jacobian (nu x nv) - mjtNum* actuator_moment; // actuator moments (nu x nv) + int* moment_colind; // column indices in sparse Jacobian (nJmom x 1) + mjtNum* actuator_moment; // actuator moments (nJmom x 1) // computed by mj_fwdPosition/mj_crb mjtNum* crb; // com-based composite inertia and mass (nbody x 10) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 3cc59311..ebd752aa 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -663,6 +663,7 @@ struct mjModel_ { int nB; // number of non-zeros in sparse body-dof matrix int nC; // number of non-zeros in sparse reduced dof-dof matrix int nD; // number of non-zeros in sparse dof-dof matrix + int nJmom; // number of non-zeros in sparse actuator_moment matrix int ntree; // number of kinematic trees under world body int ngravcomp; // number of bodies with nonzero gravcomp int nemax; // number of potential equality-constraint rows diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 33b83f7b..05d51606 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -137,6 +137,7 @@ X ( nB ) \ X ( nC ) \ X ( nD ) \ + X ( nJmom ) \ XMJV( ntree ) \ X ( ngravcomp ) \ X ( nemax ) \ @@ -625,8 +626,8 @@ X ( mjtNum, actuator_length, nu, 1 ) \ X ( int, moment_rownnz, nu, 1 ) \ X ( int, moment_rowadr, nu, 1 ) \ - X ( int, moment_colind, nu, MJ_M(nv) ) \ - X ( mjtNum, actuator_moment, nu, MJ_M(nv) ) \ + X ( int, moment_colind, nJmom, 1 ) \ + X ( mjtNum, actuator_moment, nJmom, 1 ) \ X ( mjtNum, crb, nbody, 10 ) \ X ( mjtNum, qM, nM, 1 ) \ X ( mjtNum, qLD, nM, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index c50bea6e..9b8c4c47 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1178,6 +1178,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='number of non-zeros in sparse dof-dof matrix', ), + StructFieldDecl( + name='nJmom', + type=ValueType(name='int'), + doc='number of non-zeros in sparse actuator_moment matrix', + ), StructFieldDecl( name='ntree', type=ValueType(name='int'), @@ -5166,7 +5171,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='column indices in sparse Jacobian', - array_extent=('nu', 'nv'), + array_extent=('nJmom',), ), StructFieldDecl( name='actuator_moment', @@ -5174,7 +5179,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='actuator moments', - array_extent=('nu', 'nv'), + array_extent=('nJmom',), ), StructFieldDecl( name='crb', diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 200d742e..fedb014e 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -276,7 +276,7 @@ def make_data( 'actuator_length': (m.nu, float), 'moment_rownnz': (m.nu, jp.int32), 'moment_rowadr': (m.nu, jp.int32), - 'moment_colind': (m.nu, m.nv, jp.int32), + 'moment_colind': (m.nJmom, jp.int32), 'actuator_moment': (m.nu, m.nv, float), 'crb': (m.nbody, 10, float), 'qM': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), @@ -431,22 +431,23 @@ def get_data_into( continue # MuJoCo actuator_moment is sparse, MJX uses a dense representation. - if field.name == 'actuator_moment' and m.nu: + if field.name == 'actuator_moment': moment_rownnz = np.zeros(m.nu, dtype=np.int32) moment_rowadr = np.zeros(m.nu, dtype=np.int32) - moment_colind = np.zeros(m.nu * m.nv, dtype=np.int32) - actuator_moment = np.zeros(m.nu * m.nv) - mujoco.mju_dense2sparse( - actuator_moment, - d.actuator_moment, - moment_rownnz, - moment_rowadr, - moment_colind, - ) + moment_colind = np.zeros(m.nJmom, dtype=np.int32) + actuator_moment = np.zeros(m.nJmom) + if m.nu: + mujoco.mju_dense2sparse( + actuator_moment, + d.actuator_moment, + moment_rownnz, + moment_rowadr, + moment_colind, + ) result_i.moment_rownnz[:] = moment_rownnz result_i.moment_rowadr[:] = moment_rowadr - result_i.moment_colind[:] = moment_colind.reshape((m.nu, m.nv)) - result_i.actuator_moment[:] = actuator_moment.reshape((m.nu, m.nv)) + result_i.moment_colind[:] = moment_colind + result_i.actuator_moment[:] = actuator_moment continue value = getattr(d_i, field.name) @@ -558,10 +559,10 @@ def put_data( moment = np.zeros((m.nu, m.nv)) mujoco.mju_sparse2dense( moment, - d.actuator_moment.reshape(-1), + d.actuator_moment, d.moment_rownnz, d.moment_rowadr, - d.moment_colind.reshape(-1), + d.moment_colind, ) fields['actuator_moment'] = moment diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 39339297..11cf85fd 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -122,10 +122,10 @@ class SmoothTest(absltest.TestCase): moment = np.zeros((m.nu, m.nv)) mujoco.mju_sparse2dense( moment, - d.actuator_moment.reshape(-1), + d.actuator_moment, d.moment_rownnz, d.moment_rowadr, - d.moment_colind.reshape(-1), + d.moment_colind, ) _assert_eq(moment, dx.actuator_moment, 'actuator_moment') @@ -193,10 +193,10 @@ class SmoothTest(absltest.TestCase): moment = np.zeros((m.nu, m.nv)) mujoco.mju_sparse2dense( moment, - d.actuator_moment.reshape(-1), + d.actuator_moment, d.moment_rownnz, d.moment_rowadr, - d.moment_colind.reshape(-1), + d.moment_colind, ) _assert_eq(moment, dx.actuator_moment, 'actuator_moment') diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index dcfbdb6f..89dab88e 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -536,6 +536,7 @@ class Model(PyTreeNode): nM: number of non-zeros in sparse inertia matrix nD: number of non-zeros in sparse dof-dof matrix nB: number of non-zeros in sparse body-dof matrix + nJmom: number of non-zeros in sparse actuator_moment matrix ntree: number of kinematic trees under world body ngravcomp: number of bodies with nonzero gravcomp nuserdata: size of userdata array @@ -855,6 +856,7 @@ class Model(PyTreeNode): nM: int # pylint:disable=invalid-name nD: int # pylint:disable=invalid-name nB: int # pylint:disable=invalid-name + nJmom: int ntree: int = _restricted_to('mujoco') ngravcomp: int nuserdata: int @@ -1236,8 +1238,8 @@ class Data(PyTreeNode): actuator_length: actuator lengths (nu,) moment_rownnz: number of non-zeros in actuator_moment row (nu,) moment_rowadr: row start address in colind array (nu,) - moment_colind: column indices in sparse Jacobian (nu, nv) - actuator_moment: actuator moments (nu, nv) + moment_colind: column indices in sparse Jacobian (nJmom,) + actuator_moment: actuator moments (nJmom,) crb: com-based composite inertia and mass (nbody, 10) qM: total inertia if sparse: (nM,) if dense: (nv, nv) diff --git a/mjx/mujoco/mjx/integration_test/smooth_test.py b/mjx/mujoco/mjx/integration_test/smooth_test.py index 8d6b2137..5ec8426a 100644 --- a/mjx/mujoco/mjx/integration_test/smooth_test.py +++ b/mjx/mujoco/mjx/integration_test/smooth_test.py @@ -76,10 +76,10 @@ class TransmissionIntegrationTest(parameterized.TestCase): moment = np.zeros((m.nu, m.nv)) mujoco.mju_sparse2dense( moment, - d.actuator_moment.reshape(-1), + d.actuator_moment, d.moment_rownnz, d.moment_rowadr, - d.moment_colind.reshape(-1), + d.moment_colind, ) _assert_eq( moment, diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a3e43f45..927a8b0b 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -777,6 +777,7 @@ void mjCModel::Clear() { nM = 0; nD = 0; nB = 0; + nJmom = 0; njmax = -1; nconmax = -1; nmocap = 0; @@ -2490,7 +2491,7 @@ void mjCModel::CopyTree(mjModel* m) { } } } - m->nC = nC = 2 * nOD + nv; + m->nC = nC = 2 * nOD + nv; } // copy plugin data @@ -4156,6 +4157,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // copy objects outsite kinematic tree (including keyframes) CopyObjects(m); + // compute nJmom + for (int i = 0; i < nu; i++) { + // dense rows + nJmom += nv; + } + m->nJmom = nJmom; + // scale mass if (compiler.settotalmass>0) { mj_setTotalmass(m, compiler.settotalmass); @@ -4292,7 +4300,7 @@ bool mjCModel::CopyBack(const mjModel* m) { neq!=m->neq || ntendon!=m->ntendon || nwrap!=m->nwrap || nsensor!=m->nsensor || nnumeric!=m->nnumeric || nnumericdata!=m->nnumericdata || ntext!=m->ntext || ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD || nC!=m->nC || - nB!=m->nB || nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax || + nB!=m->nB || nJmom!=m->nJmom ||nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax || npaths!=m->npaths) { errInfo = mjCError(0, "incompatible models in CopyBack"); return false; diff --git a/src/user/user_model.h b/src/user/user_model.h index 41865ac6..d4d7dffa 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -118,6 +118,7 @@ class mjCModel_ : public mjsElement { int nB; // number of non-zeros in sparse body-dof matrix int nC; // number of non-zeros in reduced sparse dof-dof matrix int nD; // number of non-zeros in sparse dof-dof matrix + int nJmom; // number of non-zeros in sparse actuator_moment matrix // statistics, as computed by mj_setConst double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 0874a14c..53c6277b 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -116,6 +116,30 @@ TEST_F(UserCModelTest, SameFrame) { mj_deleteModel(model); } +TEST_F(UserCModelTest, ActuatorSparsity) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_EQ(m->nJmom, 4); + mj_deleteModel(m); +} + // ------------- test automatic inference of nuser_xxx ------------------------- diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a8d063be..8858f99c 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5244,6 +5244,7 @@ public unsafe struct mjModel_ { public int nB; public int nC; public int nD; + public int nJmom; public int ntree; public int ngravcomp; public int nemax; From a1b18e707a10b9d2be1bda6b86eec684835d4490 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 20 Nov 2024 10:52:20 -0800 Subject: [PATCH 087/426] Compress `actuator_moment` memory using `nJmom` <= `nu` x `nv`. PiperOrigin-RevId: 698446692 Change-Id: I49c9633e12129a1e690724db82d1f11204e41d9c --- src/engine/engine_core_smooth.c | 2 +- src/user/user_model.cc | 67 ++++++++++++++++++++++++++++++--- src/user/user_model.h | 1 + test/user/user_model_test.cc | 2 +- 4 files changed, 64 insertions(+), 8 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index e9bf33cc..6e9e7b2e 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1087,7 +1087,7 @@ void mj_transmission(const mjModel* m, mjData* d) { int refid = m->actuator_trnid[2*i+1]; if (!jacref) jacref = mj_stackAllocNum(d, 3*nv); - // intialize last dof address for each body + // initialize last dof address for each body int b0 = m->body_weldid[m->site_bodyid[id]]; int b1 = m->body_weldid[m->site_bodyid[refid]]; int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 927a8b0b..fb21b1b1 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2579,6 +2579,65 @@ void mjCModel::CopyPlugins(mjModel* m) { } } + + +// compute non-zeros in actuator_moment matrix +int mjCModel::CountNJmom(const mjModel* m) { + int nu = m->nu; + int nv = m->nv; + + int count = 0; + for (int i = 0; i < nu; i++) { + // extract info + int id = m->actuator_trnid[2 * i]; + + // process according to transmission type + switch ((mjtTrn)m->actuator_trntype[i]) { + case mjTRN_JOINT: + case mjTRN_JOINTINPARENT: + switch ((mjtJoint)m->jnt_type[id]) { + case mjJNT_SLIDE: + case mjJNT_HINGE: + count += 1; + break; + + case mjJNT_BALL: + count += 3; + break; + + case mjJNT_FREE: + count += 6; + break; + } + break; + // TODO(taylorhowell): improve upper bounds + case mjTRN_SLIDERCRANK: + count += nv; + break; + + case mjTRN_TENDON: + count += nv; + break; + + case mjTRN_SITE: + count += nv; + break; + + case mjTRN_BODY: + count += nv; + break; + + default: + // SHOULD NOT OCCUR + throw mjCError(0, "unknown transmission type"); + break; + } + } + return count; +} + + + // copy objects outside kinematic tree void mjCModel::CopyObjects(mjModel* m) { int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr; @@ -4157,12 +4216,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // copy objects outsite kinematic tree (including keyframes) CopyObjects(m); - // compute nJmom - for (int i = 0; i < nu; i++) { - // dense rows - nJmom += nv; - } - m->nJmom = nJmom; + // compute non-zeros in actuator_moment + m->nJmom = nJmom = CountNJmom(m); // scale mass if (compiler.settotalmass>0) { diff --git a/src/user/user_model.h b/src/user/user_model.h index d4d7dffa..1b248ea2 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -324,6 +324,7 @@ class mjCModel : public mjCModel_, private mjSpec { void CopyObjects(mjModel*); // copy objects outside kinematic tree void CopyTree(mjModel*); // copy objects inside kinematic tree void CopyPlugins(mjModel*); // copy plugin data + int CountNJmom(const mjModel* m); // compute number of non-zeros in actuator_moment matrix // objects created here std::vector flexes_; // list of flexes diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 53c6277b..811004a7 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -136,7 +136,7 @@ TEST_F(UserCModelTest, ActuatorSparsity) { )"; mjModel* m = LoadModelFromString(xml); - ASSERT_EQ(m->nJmom, 4); + ASSERT_EQ(m->nJmom, 2); mj_deleteModel(m); } From 1767c11d4618a804a81c50cf7b4ea0eb17790299 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 20 Nov 2024 11:23:41 -0800 Subject: [PATCH 088/426] Direct assignment of `mjData.moment_rownnz` This allows for better sanitizer coverage, errors will be caught upon read rather than upon use. PiperOrigin-RevId: 698458735 Change-Id: I3003f3a122b83bbf42a6510260abbb8353c56a40 --- src/engine/engine_core_smooth.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 6e9e7b2e..d14015aa 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -878,7 +878,6 @@ void mj_transmission(const mjModel* m, mjData* d) { // compute lengths and moments for (int i=0; i < nu; i++) { - rownnz[i] = 0; rowadr[i] = i == 0 ? 0 : rowadr[i-1] + rownnz[i-1]; int adr = rowadr[i]; @@ -893,7 +892,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // slide and hinge joint: scalar gear if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) { // sparsity - rownnz[i]++; + rownnz[i] = 1; colind[adr] = m->jnt_dofadr[id]; length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0]; @@ -927,7 +926,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < 3; j++) { colind[adr+j] = jnt_dofadr + j; } - rownnz[i] += 3; + rownnz[i] = 3; // moment: gearAxis mju_copy3(moment+adr, gearAxis); @@ -957,7 +956,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < 6; j++) { colind[adr+j] = jnt_dofadr + j; } - rownnz[i] += 6; + rownnz[i] = 6; // moment: gear(tran), gearAxis mju_copy3(moment+adr, gear); @@ -1013,7 +1012,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < nv; j++) { colind[adr+j] = j; } - rownnz[i] += nv; + rownnz[i] = nv; // clear moment mju_zero(moment + adr, nv); @@ -1041,7 +1040,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // sparsity int ten_J_rownnz = d->ten_J_rownnz[id]; int ten_J_rowadr = d->ten_J_rowadr[id]; - rownnz[i] += ten_J_rownnz; + rownnz[i] = ten_J_rownnz; mju_copyInt(colind + adr, d->ten_J_colind + ten_J_rowadr, ten_J_rownnz); mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz); @@ -1050,7 +1049,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < nv; j++) { colind[adr+j] = j; } - rownnz[i] += nv; + rownnz[i] = nv; mju_scl(moment+adr, d->ten_J + id*nv, gear[0], nv); } @@ -1061,7 +1060,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < nv; j++) { colind[adr+j] = j; } - rownnz[i] += nv; + rownnz[i] = nv; // get site translation (jac) and rotation (jacS) Jacobians in global frame mj_jacSite(m, d, jac, jacS, id); @@ -1200,7 +1199,7 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < nv; j++) { colind[adr+j] = j; } - rownnz[i] += nv; + rownnz[i] = nv; // cannot compute meaningful length, set to 0 length[i] = 0; From cedd602bc130327bf96d39e717125b2a51f3c3d7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 20 Nov 2024 14:08:58 -0800 Subject: [PATCH 089/426] Compress dense rows in `mjData.actuator_moment` PiperOrigin-RevId: 698516133 Change-Id: Idd861c2b63d77748a6282d72f8241eaeeb88006c --- src/engine/engine_core_smooth.c | 70 +++++++++++++++++++++------------ src/engine/engine_print.c | 2 +- 2 files changed, 46 insertions(+), 26 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d14015aa..6cc5a21f 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -879,7 +879,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // compute lengths and moments for (int i=0; i < nu; i++) { rowadr[i] = i == 0 ? 0 : rowadr[i-1] + rownnz[i-1]; - int adr = rowadr[i]; + int nnz, adr = rowadr[i]; // extract info int id = m->actuator_trnid[2*i]; @@ -1008,12 +1008,6 @@ void mj_transmission(const mjModel* m, mjData* d) { mj_jacSite(m, d, jac, 0, id); mju_subFrom(jac, jacS, 3*nv); - // sparsity - for (int j = 0; j < nv; j++) { - colind[adr+j] = j; - } - rownnz[i] = nv; - // clear moment mju_zero(moment + adr, nv); @@ -1029,6 +1023,17 @@ void mj_transmission(const mjModel* m, mjData* d) { for (int j = 0; j < nv; j++) { moment[adr+j] *= gear[0]; } + + // sparsity (compress) + nnz = 0; + for (int j = 0; j < nv; j++) { + if (moment[adr+j]) { + moment[adr+nnz] = moment[adr+j]; + colind[adr+nnz] = j; + nnz++; + } + } + rownnz[i] = nnz; } break; @@ -1045,23 +1050,22 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_scl(moment + adr, d->ten_J + ten_J_rowadr, gear[0], ten_J_rownnz); } else { - // sparsity - for (int j = 0; j < nv; j++) { - colind[adr+j] = j; - } - rownnz[i] = nv; - mju_scl(moment+adr, d->ten_J + id*nv, gear[0], nv); + + // sparsity (compress) + nnz = 0; + for (int j = 0; j < nv; j++) { + if (moment[adr+j]) { + moment[adr+nnz] = moment[adr+j]; + colind[adr+nnz] = j; + nnz++; + } + } + rownnz[i] = nnz; } break; case mjTRN_SITE: // site - // sparsity - for (int j = 0; j < nv; j++) { - colind[adr+j] = j; - } - rownnz[i] = nv; - // get site translation (jac) and rotation (jacS) Jacobians in global frame mj_jacSite(m, d, jac, jacS, id); @@ -1192,15 +1196,20 @@ void mj_transmission(const mjModel* m, mjData* d) { } } + // sparsity (compress) + nnz = 0; + for (int j = 0; j < nv; j++) { + if (moment[adr+j]) { + moment[adr+nnz] = moment[adr+j]; + colind[adr+nnz] = j; + nnz++; + } + } + rownnz[i] = nnz; + break; case mjTRN_BODY: // body (adhesive contacts) - // sparsity - for (int j = 0; j < nv; j++) { - colind[adr+j] = j; - } - rownnz[i] = nv; - // cannot compute meaningful length, set to 0 length[i] = 0; @@ -1299,6 +1308,17 @@ void mj_transmission(const mjModel* m, mjData* d) { } } + // sparsity (compress) + nnz = 0; + for (int j = 0; j < nv; j++) { + if (moment[adr+j]) { + moment[adr+nnz] = moment[adr+j]; + colind[adr+nnz] = j; + nnz++; + } + } + rownnz[i] = nnz; + break; default: diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index f7132bc4..68e48571 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1084,7 +1084,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); - printSparsity("actuator_moments", m->nu, m->nv, + printSparsity("actuator_moment", m->nu, m->nv, d->moment_rowadr, d->moment_rownnz, d->moment_colind, fp); printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, d->moment_rowadr, d->moment_colind, fp, float_format); From 38ee55a957adaaefb935907b639f6af3156c85eb Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 21 Nov 2024 05:07:28 -0800 Subject: [PATCH 090/426] Remove unused userfacenormal field from mjSpec. PiperOrigin-RevId: 698739284 Change-Id: I6407356ac12ad49454d15ecf9056f2c0bef713d5 --- doc/includes/references.h | 1 - include/mujoco/mjspec.h | 1 - introspect/structs.py | 7 ------- 3 files changed, 9 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 199599f4..fe917ab1 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2003,7 +2003,6 @@ typedef struct mjsMesh_ { // mesh specification mjFloatVec* usernormal; // user normal data mjFloatVec* usertexcoord; // user texcoord data mjIntVec* userface; // user vertex indices - mjIntVec* userfacenormal; // user normal indices mjIntVec* userfacetexcoord; // user texcoord indices mjsPlugin plugin; // sdf plugin mjString* info; // message appended to compiler errors diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 2dc3177b..eddcf222 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -464,7 +464,6 @@ typedef struct mjsMesh_ { // mesh specification mjFloatVec* usernormal; // user normal data mjFloatVec* usertexcoord; // user texcoord data mjIntVec* userface; // user vertex indices - mjIntVec* userfacenormal; // user normal indices mjIntVec* userfacetexcoord; // user texcoord indices mjsPlugin plugin; // sdf plugin mjString* info; // message appended to compiler errors diff --git a/introspect/structs.py b/introspect/structs.py index 9b8c4c47..c1e00a86 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -10341,13 +10341,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='user vertex indices', ), - StructFieldDecl( - name='userfacenormal', - type=PointerType( - inner_type=ValueType(name='mjIntVec'), - ), - doc='user normal indices', - ), StructFieldDecl( name='userfacetexcoord', type=PointerType( From 74dcd51d8319fd646ed09c2703a475a3ee5f302b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 22 Nov 2024 04:33:31 -0800 Subject: [PATCH 091/426] Update changelog with Python 3.13 support (upcoming release) PiperOrigin-RevId: 699127095 Change-Id: I098d1e91e59642f27a764d055c6ae9b77a8b1000 --- doc/changelog.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index dafe7daf..917e2e63 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,10 @@ MJX ^^^ - Added muscle actuators. +Python bindings +^^^^^^^^^^^^^^^ +- Provide prebuilt wheels for Python 3.13. + Bug fixes ^^^^^^^^^ - Fixed :github:issue:`2212`, type error in ``mjx.get_data``. From 1d64362adc003fc45c514a557368baf59efc6cb5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 22 Nov 2024 09:37:18 -0800 Subject: [PATCH 092/426] Add `mjv_copyModel` to copy `mjModel`, skipping large arrays not required for abstract visualization. This functionality is meant for fast copying of `mjModel` when synchronizing the visualization state, as in the Python passive viewer. PiperOrigin-RevId: 699199743 Change-Id: I13a5160063eb09139ec1aee9c4969bc1c6f547f1 --- doc/APIreference/functions.rst | 9 ++++ doc/includes/references.h | 1 + include/mujoco/mjxmacro.h | 23 ++++---- include/mujoco/mujoco.h | 3 ++ introspect/functions.py | 20 +++++++ src/engine/engine_io.c | 80 ++++++++++++++++++++-------- src/engine/engine_io.h | 3 ++ test/engine/engine_io_test.cc | 37 +++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 ++ 9 files changed, 147 insertions(+), 32 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 4406e5c5..4bbf42e2 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -2163,6 +2163,15 @@ Update entire scene given model state. Update entire scene from a scene state, return the number of new mjWARN_VGEOMFULL warnings. +.. _mjv_copyModel: + +`mjv_copyModel <#mjv_copyModel>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjv_copyModel + +Copy mjModel, skip large arrays not required for abstract visualization. + .. _mjv_defaultSceneState: `mjv_defaultSceneState <#mjv_defaultSceneState>`__ diff --git a/doc/includes/references.h b/doc/includes/references.h index fe917ab1..adac0d47 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3347,6 +3347,7 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOption* opt, const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn); +void mjv_copyModel(mjModel* dest, const mjModel* src); void mjv_defaultSceneState(mjvSceneState* scnstate); void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnstate, int maxgeom); diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 05d51606..95a06b2c 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -378,13 +378,13 @@ X ( mjtNum, mesh_scale, nmesh, 3 ) \ X ( mjtNum, mesh_pos, nmesh, 3 ) \ X ( mjtNum, mesh_quat, nmesh, 4 ) \ - X ( float, mesh_vert, nmeshvert, 3 ) \ - X ( float, mesh_normal, nmeshnormal, 3 ) \ - X ( float, mesh_texcoord, nmeshtexcoord, 2 ) \ - X ( int, mesh_face, nmeshface, 3 ) \ - X ( int, mesh_facenormal, nmeshface, 3 ) \ - X ( int, mesh_facetexcoord, nmeshface, 3 ) \ - X ( int, mesh_graph, nmeshgraph, 1 ) \ + XNV ( float, mesh_vert, nmeshvert, 3 ) \ + XNV ( float, mesh_normal, nmeshnormal, 3 ) \ + XNV ( float, mesh_texcoord, nmeshtexcoord, 2 ) \ + XNV ( int, mesh_face, nmeshface, 3 ) \ + XNV ( int, mesh_facenormal, nmeshface, 3 ) \ + XNV ( int, mesh_facetexcoord, nmeshface, 3 ) \ + XNV ( int, mesh_graph, nmeshgraph, 1 ) \ XMJV( int, mesh_pathadr, nmesh, 1 ) \ XMJV( int, skin_matid, nskin, 1 ) \ XMJV( int, skin_group, nskin, 1 ) \ @@ -412,14 +412,14 @@ X ( int, hfield_nrow, nhfield, 1 ) \ X ( int, hfield_ncol, nhfield, 1 ) \ X ( int, hfield_adr, nhfield, 1 ) \ - X ( float, hfield_data, nhfielddata, 1 ) \ + XNV ( float, hfield_data, nhfielddata, 1 ) \ XMJV( int, hfield_pathadr, nhfield, 1 ) \ X ( int, tex_type, ntex, 1 ) \ X ( int, tex_height, ntex, 1 ) \ X ( int, tex_width, ntex, 1 ) \ X ( int, tex_nchannel, ntex, 1 ) \ X ( int, tex_adr, ntex, 1 ) \ - X ( mjtByte, tex_data, ntexdata, 1 ) \ + XNV ( mjtByte, tex_data, ntexdata, 1 ) \ XMJV( int, tex_pathadr, ntex, 1 ) \ XMJV( int, mat_texid, nmat, mjNTEXROLE ) \ XMJV( mjtByte, mat_texuniform, nmat, 1 ) \ @@ -779,4 +779,9 @@ // redefine X to expand to nothing, and XMJV to do what's required #define XMJV X +// alias XNV to be the same as X +// to obtain only X macros for fields that are relevant for mjvScene creation, +// redefine XNV to expand to nothing +#define XNV X + #endif // MUJOCO_MJXMACRO_H_ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 7a38575d..10b2f548 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -692,6 +692,9 @@ MJAPI int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOptio const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn); +// Copy mjModel, skip large arrays not required for abstract visualization. +MJAPI void mjv_copyModel(mjModel* dest, const mjModel* src); + // Set default scene state. MJAPI void mjv_defaultSceneState(mjvSceneState* scnstate); diff --git a/introspect/functions.py b/introspect/functions.py index 89b28e4a..ed936aef 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -4550,6 +4550,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Update entire scene from a scene state, return the number of new mjWARN_VGEOMFULL warnings.', # pylint: disable=line-too-long )), + ('mjv_copyModel', + FunctionDecl( + name='mjv_copyModel', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='dest', + type=PointerType( + inner_type=ValueType(name='mjModel'), + ), + ), + FunctionParameterDecl( + name='src', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + ), + doc='Copy mjModel, skip large arrays not required for abstract visualization.', # pylint: disable=line-too-long + )), ('mjv_defaultSceneState', FunctionDecl( name='mjv_defaultSceneState', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 57684692..ec24d4cf 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -625,10 +625,10 @@ void mj_makeModel(mjModel** dest, } } + + // copy mjModel, if dest==NULL create new model mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { - void* save_bufptr; - // allocate new model if needed if (!dest) { mj_makeModel(&dest, @@ -658,13 +658,13 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { mjERROR("dest and src models have different buffer size"); } - // save buffer ptr, copy everything, restore buffer and other pointers - save_bufptr = dest->buffer; + // save buffer ptr, copy struct, restore buffer and other pointers + void* save_bufptr = dest->buffer; *dest = *src; dest->buffer = save_bufptr; mj_setPtrModel(dest); - // copy buffer + // copy buffer contents { MJMODEL_POINTERS_PREAMBLE(src) #define X(type, name, nr, nc) \ @@ -678,6 +678,38 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { +// copy mjModel, skip large arrays not required for abstract visualization +void mjv_copyModel(mjModel* dest, const mjModel* src) { + // check sizes + if (dest->nbuffer != src->nbuffer) { + mjERROR("dest and src models have different buffer size"); + } + + // save buffer ptr, copy struct, restore buffer and other pointers + void* save_bufptr = dest->buffer; + *dest = *src; + dest->buffer = save_bufptr; + mj_setPtrModel(dest); + + // redefine XNV to do nothing + #undef XNV + #define XNV(type, name, nr, nc) + + // copy buffer contents, skipping arrays marked XNV + { + MJMODEL_POINTERS_PREAMBLE(src) + #define X(type, name, nr, nc) \ + memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(src->nr)*nc); + MJMODEL_POINTERS + #undef X + } + // redefine XNV to be the same as X + #undef XNV + #define XNV X +} + + + // save model to binary file, or memory buffer of szbuf>0 void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz) { FILE* fp = 0; @@ -1401,26 +1433,28 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { #undef X } - // copy arena memory -#undef MJ_D -#define MJ_D(n) (src->n) -#undef MJ_M -#define MJ_M(n) (m->n) -#define X(type, name, nr, nc) \ - if (src->name) { \ - dest->name = (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)); \ - ASAN_UNPOISON_MEMORY_REGION(dest->name, sizeof(type)*nr*nc); \ - memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*nr*nc); \ - } else { \ - dest->name = NULL; \ - } + // copy arena memory + #undef MJ_D + #define MJ_D(n) (src->n) + #undef MJ_M + #define MJ_M(n) (m->n) + + #define X(type, name, nr, nc) \ + if (src->name) { \ + dest->name = (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)); \ + ASAN_UNPOISON_MEMORY_REGION(dest->name, sizeof(type) * nr * nc); \ + memcpy((char*)dest->name, (const char*)src->name, sizeof(type) * nr * nc); \ + } else { \ + dest->name = NULL; \ + } MJDATA_ARENA_POINTERS -#undef X -#undef MJ_M -#define MJ_M(n) n -#undef MJ_D -#define MJ_D(n) n + #undef X + + #undef MJ_M + #define MJ_M(n) n + #undef MJ_D + #define MJ_D(n) n // restore contact pointer dest->contact = dest->arena; diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 4b34784f..a6fac94e 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -69,6 +69,9 @@ void mj_makeModel(mjModel** dest, // copy mjModel; allocate new if dest is NULL MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src); +// copy mjModel, skip large arrays not required for abstract visualization +MJAPI void mjv_copyModel(mjModel* dest, const mjModel* src); + // save model to binary file MJAPI void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz); diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index 7b65de4a..3e6eb9af 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -193,6 +193,43 @@ TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) { mj_deleteModel(model); } +TEST_F(EngineIoTest, MjvCopyModel) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + char error[1024]; + mjModel* model1 = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model1, NotNull()) << error; + + mjModel* model2 = mj_copyModel(nullptr, model1); + ASSERT_THAT(model2, NotNull()) << error; + + model1->mesh_vert[0] = 0.1; + model1->geom_rgba[0] = 0.2; + mj_copyModel(model2, model1); + + EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); + EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2); + + model1->mesh_vert[0] = 0.3; + model1->geom_rgba[0] = 0.4; + mjv_copyModel(model2, model1); + + EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); // unchanged + EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4); + + // mj_deleteData(data); + mj_deleteModel(model2); + mj_deleteModel(model1); +} + using ValidateReferencesTest = MujocoTest; TEST_F(ValidateReferencesTest, BodyReferences) { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 8858f99c..2a553678 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6940,6 +6940,9 @@ public static unsafe extern void mjv_updateScene(mjModel_* m, mjData_* d, mjvOpt [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mjv_updateSceneFromState(mjvSceneState_* scnstate, mjvOption_* opt, mjvPerturb_* pert, mjvCamera_* cam, int catmask, mjvScene_* scn); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mjv_copyModel(mjModel_* dest, mjModel_* src); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mjv_defaultSceneState(mjvSceneState_* scnstate); From 3a12db9ad2c395eeb3a736a23a957234788c0815 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 22 Nov 2024 11:04:20 -0800 Subject: [PATCH 093/426] Remove id attribute from mjSpec objects in Python bindings. Using ids is error prone in scenarios of repeated attachment and detachment. Python users are encouraged to use names for unique identification of model elements. PiperOrigin-RevId: 699227286 Change-Id: Ifd83e6d85d36ff72ea43caf8b82eab9e4d552440 --- doc/changelog.rst | 5 ++++ python/mjspec.ipynb | 20 +++++++------ python/mujoco/specs.cc | 57 ------------------------------------- python/mujoco/specs_test.py | 11 ++----- 4 files changed, 18 insertions(+), 75 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 917e2e63..e448e8bf 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,6 +6,11 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +- Removed id attribute from :ref:`mjSpec` objects in Python bindings. Using ids is error prone in scenarios of repeated + attachment and detachment. Python users are encouraged to use names for unique identification of model elements. + MJX ^^^ - Added muscle actuators. diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index 772faf30..886032f4 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -300,8 +300,8 @@ " \n", "\n", " \n", - " \n", - " \n", + " \n", + " \n", " \n", "\n", "\"\"\"\n", @@ -450,14 +450,16 @@ "source": [ "#@title Video of the movement{vertical-output: true}\n", "\n", + "data = mj.MjData(model)\n", "duration = 10 # (Seconds)\n", "framerate = 30 # (Hz)\n", "video = []\n", "pos_x = []\n", "pos_y = []\n", "geoms = arena.worldbody.find_all(mj.mjtObj.mjOBJ_GEOM)\n", - "torsos = [geom.id for geom in geoms if 'torso' in geom.name]\n", - "actuators = [actuator.id for actuator in arena.actuators]\n", + "torsos_data = [data.geom(geom.name) for geom in geoms if 'torso' in geom.name]\n", + "torsos_model = [model.geom(geom.name) for geom in geoms if 'torso' in geom.name]\n", + "actuators = [data.actuator(actuator.name) for actuator in arena.actuators]\n", "\n", "# Control signal frequency, phase, amplitude.\n", "freq = 5\n", @@ -465,17 +467,17 @@ "amp = 0.9\n", "\n", "# Simulate, saving video frames and torso locations.\n", - "data = mj.MjData(model)\n", "mj.mj_resetData(model, data)\n", "with mj.Renderer(model) as renderer:\n", " while data.time < duration:\n", " # Inject controls and step the physics.\n", - " data.ctrl[actuators] = amp * np.sin(freq * data.time + phase)\n", + " for i, actuator in enumerate(actuators):\n", + " actuator.ctrl = amp * np.sin(freq * data.time + phase[i])\n", " mj.mj_step(model, data)\n", "\n", " # Save torso horizontal positions using name indexing.\n", - " pos_x.append(data.geom_xpos[torsos, 0].copy())\n", - " pos_y.append(data.geom_xpos[torsos, 1].copy())\n", + " pos_x.append([torso.xpos[0] for torso in torsos_data])\n", + " pos_y.append([torso.xpos[1] for torso in torsos_data])\n", "\n", " # Save video frames.\n", " if len(video) < data.time * framerate:\n", @@ -496,7 +498,7 @@ "source": [ "#@title Movement trajectories{vertical-output: true}\n", "\n", - "creature_colors = model.geom_rgba[torsos][:, :3]\n", + "creature_colors = [torso.rgba[:3] for torso in torsos_model]\n", "fig, ax = plt.subplots(figsize=(4, 4))\n", "ax.set_prop_cycle(color=creature_colors)\n", "_ = ax.plot(pos_x, pos_y, linewidth=4)" diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index d88a0544..725894c6 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -407,8 +407,6 @@ PYBIND11_MODULE(_specs, m) { }); // ============================= MJSBODY ===================================== - mjsBody.def_property_readonly( - "id", [](raw::MjsBody& self) -> int { return mjs_getId(self.element); }); mjsBody.def( "add_freejoint", [](raw::MjsBody& self, py::kwargs kwargs) -> raw::MjsJoint* { @@ -650,8 +648,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSFRAME ==================================== - mjsFrame.def_property_readonly( - "id", [](raw::MjsFrame& self) -> int { return mjs_getId(self.element); }); mjsFrame.def("delete", [](raw::MjsFrame& self) { mjs_delete(self.element); }); mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); @@ -671,8 +667,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== - mjsGeom.def_property_readonly( - "id", [](raw::MjsGeom& self) -> int { return mjs_getId(self.element); }); mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); mjsGeom.def("set_frame", [](raw::MjsGeom& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); @@ -688,8 +682,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSJOINT ==================================== - mjsJoint.def_property_readonly( - "id", [](raw::MjsJoint& self) -> int { return mjs_getId(self.element); }); mjsJoint.def("delete", [](raw::MjsJoint& self) { mjs_delete(self.element); }); mjsJoint.def("set_frame", [](raw::MjsJoint& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); @@ -705,8 +697,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSSITE ===================================== - mjsSite.def_property_readonly( - "id", [](raw::MjsSite& self) -> int { return mjs_getId(self.element); }); mjsSite.def("delete", [](raw::MjsSite& self) { mjs_delete(self.element); }); mjsSite.def("set_frame", [](raw::MjsSite& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); @@ -735,9 +725,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== - mjsCamera.def_property_readonly("id", [](raw::MjsCamera& self) -> int { - return mjs_getId(self.element); - }); mjsCamera.def("delete", [](raw::MjsCamera& self) { mjs_delete(self.element); }); mjsCamera.def("set_frame", [](raw::MjsCamera& self, raw::MjsFrame& frame) { @@ -754,8 +741,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSLIGHT ==================================== - mjsLight.def_property_readonly( - "id", [](raw::MjsLight& self) -> int { return mjs_getId(self.element); }); mjsLight.def("delete", [](raw::MjsLight& self) { mjs_delete(self.element); }); mjsLight.def("set_frame", [](raw::MjsLight& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); @@ -771,9 +756,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSMATERIAL ================================= - mjsMaterial.def_property_readonly("id", [](raw::MjsMaterial& self) -> int { - return mjs_getId(self.element); - }); mjsMaterial.def("delete", [](raw::MjsMaterial& self) { mjs_delete(self.element); }); mjsMaterial.def("set_default", @@ -788,8 +770,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSMESH ===================================== - mjsMesh.def_property_readonly( - "id", [](raw::MjsMesh& self) -> int { return mjs_getId(self.element); }); mjsMesh.def("delete", [](raw::MjsMesh& self) { mjs_delete(self.element); }); mjsMesh.def("set_default", [](raw::MjsMesh& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); @@ -802,8 +782,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSPAIR ===================================== - mjsPair.def_property_readonly( - "id", [](raw::MjsPair& self) -> int { return mjs_getId(self.element); }); mjsPair.def("delete", [](raw::MjsPair& self) { mjs_delete(self.element); }); mjsPair.def("set_default", [](raw::MjsPair& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); @@ -816,9 +794,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSEQUAL ==================================== - mjsEquality.def_property_readonly("id", [](raw::MjsEquality& self) -> int { - return mjs_getId(self.element); - }); mjsEquality.def("delete", [](raw::MjsEquality& self) { mjs_delete(self.element); }); mjsEquality.def("set_default", @@ -833,9 +808,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSACTUATOR ================================= - mjsActuator.def_property_readonly("id", [](raw::MjsActuator& self) -> int { - return mjs_getId(self.element); - }); mjsActuator.def("delete", [](raw::MjsActuator& self) { mjs_delete(self.element); }); mjsActuator.def("set_default", @@ -850,9 +822,6 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSTENDON =================================== - mjsTendon.def_property_readonly("id", [](raw::MjsTendon& self) -> int { - return mjs_getId(self.element); - }); mjsTendon.def("delete", [](raw::MjsTendon& self) { mjs_delete(self.element); }); mjsTendon.def("set_default", [](raw::MjsTendon& self, raw::MjsDefault& def) { @@ -890,64 +859,38 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSSENSOR =================================== - mjsSensor.def_property_readonly("id", [](raw::MjsSensor& self) -> int { - return mjs_getId(self.element); - }); mjsSensor.def("delete", [](raw::MjsSensor& self) { mjs_delete(self.element); }); // ============================= MJSFLEX ===================================== - mjsFlex.def_property_readonly( - "id", [](raw::MjsFlex& self) -> int { return mjs_getId(self.element); }); mjsFlex.def("delete", [](raw::MjsFlex& self) { mjs_delete(self.element); }); // ============================= MJSHFIELD =================================== - mjsHField.def_property_readonly("id", [](raw::MjsHField& self) -> int { - return mjs_getId(self.element); - }); mjsHField.def("delete", [](raw::MjsHField& self) { mjs_delete(self.element); }); // ============================= MJSSKIN ===================================== - mjsSkin.def_property_readonly( - "id", [](raw::MjsSkin& self) -> int { return mjs_getId(self.element); }, - py::return_value_policy::reference_internal); mjsSkin.def("delete", [](raw::MjsSkin& self) { mjs_delete(self.element); }); // ============================= MJSTEXTURE ================================== - mjsTexture.def_property_readonly("id", [](raw::MjsTexture& self) -> int { - return mjs_getId(self.element); - }); mjsTexture.def("delete", [](raw::MjsTexture& self) { mjs_delete(self.element); }); // ============================= MJSKEY ====================================== - mjsKey.def_property_readonly( - "id", [](raw::MjsKey& self) -> int { return mjs_getId(self.element); }); mjsKey.def("delete", [](raw::MjsKey& self) { mjs_delete(self.element); }); // ============================= MJSTEXT ===================================== - mjsText.def_property_readonly( - "id", [](raw::MjsText& self) -> int { return mjs_getId(self.element); }); mjsText.def("delete", [](raw::MjsText& self) { mjs_delete(self.element); }); // ============================= MJSNUMERIC ================================== - mjsNumeric.def_property_readonly("id", [](raw::MjsNumeric& self) -> int { - return mjs_getId(self.element); - }); mjsNumeric.def("delete", [](raw::MjsNumeric& self) { mjs_delete(self.element); }); // ============================= MJSEXCLUDE ================================== - mjsExclude.def_property_readonly("id", [](raw::MjsExclude& self) -> int { - return mjs_getId(self.element); - }); mjsExclude.def("delete", [](raw::MjsExclude& self) { mjs_delete(self.element); }); // ============================= MJSTUPLE ==================================== - mjsTuple.def_property_readonly( - "id", [](raw::MjsTuple& self) -> int { return mjs_getId(self.element); }); mjsTuple.def("delete", [](raw::MjsTuple& self) { mjs_delete(self.element); }); // ============================= MJSPLUGIN =================================== diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 7bca48f0..74a34648 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -92,18 +92,11 @@ class SpecsTest(absltest.TestCase): self.assertEqual(site.type, mujoco.mjtGeom.mjGEOM_BOX) np.testing.assert_array_equal(site.userdata, [1, 2, 3, 4, 5, 6]) - # Check that the site and body have no id before compilation. - self.assertEqual(body.id, -1) - self.assertEqual(site.id, -1) - # Compile the spec and check for expected values in the model. model = spec.compile() - self.assertEqual(spec.worldbody.id, 0) - self.assertEqual(body.id, 1) - self.assertEqual(site.id, 0) self.assertEqual(model.nbody, 2) # 2 bodies, including the world body - np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3]) - np.testing.assert_array_equal(model.body_quat[1], [0, 1, 0, 0]) + np.testing.assert_array_equal(model.body('baz').pos, [1, 2, 3]) + np.testing.assert_array_equal(model.body('baz').quat, [0, 1, 0, 0]) self.assertEqual(model.nsite, 1) self.assertEqual(model.nuser_site, 6) np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6]) From f909d63cdf9033d7ce13744ab631c099aa19b753 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 24 Nov 2024 07:34:16 -0800 Subject: [PATCH 094/426] Enable to attach a spec to a frame. PiperOrigin-RevId: 699703988 Change-Id: Idbddcbce1e68286248458eb0bd24d5080451a58a --- doc/modeling.rst | 2 +- doc/python.rst | 36 ++++++++++++++++++++++++++++++++++++ python/mujoco/specs.cc | 18 ++++++++++++++++++ python/mujoco/specs_test.py | 13 +++++++++++++ 4 files changed, 68 insertions(+), 1 deletion(-) diff --git a/doc/modeling.rst b/doc/modeling.rst index b68d5e99..fdf6017f 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1284,7 +1284,7 @@ elastic structures. The box type, as well as the cylinder and ellipsoid types, are now deprecated in favor of 3D flex :ref:`deformable -objects ``. element. +objects `. element. .. _CDeformable: diff --git a/doc/python.rst b/doc/python.rst index 9c824b86..d5e4a165 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -503,6 +503,42 @@ The ``MjSpec`` object wraps the :ref:`mjSpec` struct and can be constructed in t Note the ``from_string()`` and ``from_file()`` methods can only be called at construction time. +Attachments +----------- + +It is possible to combine multiple specs by using attachments. The following options are possible: + +- Attach a body from the child spec to a frame in the parent spec: ``body.attach_body(body, prefix, suffix)``, returns + the newly createdbody in the parent spec. +- Attach a frame from the child spec to a body in the parent spec: ``body.attach_frame(frame, prefix, suffix)``, + returns the newly created frame in the parent spec. +- Attach a body from the child spec to a site in the parent spec: ``site.attach(body, prefix, suffix)``, returns the + newly created body in the parent spec. +- Attach the worldbody from the child spec to a frame in the parent spec and transform it to a frame: + ``body.attach(spec, prefix, suffix)``, returns the newly created frame that the child worldbody was transformed + into. + +.. code-block:: python + + import mujoco + + # Create the parent spec. + parent = mujoco.MjSpec() + body = parent.worldbody.add_body() + frame = parent.worldbody.add_frame() + site = parent.worldbody.add_site() + + # Create the child spec. + child = mujoco.MjSpec() + child_body = child.worldbody.add_body() + child_frame = child.worldbody.add_frame() + + # Attach the child to the parent in different ways. + body_in_frame = frame.attach_body(child_body, 'child-', '') + frame_in_body = body.attach_frame(child_frame, 'child-', '') + body_in_site = site.attach(child_body, 'child-', '') + worldframe_in_frame = frame.attach(child, 'child-', '') + Convenience methods ------------------- diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 725894c6..f97ed6fd 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -665,6 +665,24 @@ PYBIND11_MODULE(_specs, m) { return new_body; }, py::return_value_policy::reference_internal); + mjsFrame.def( + "attach", + [](raw::MjsFrame& self, MjSpec& spec, std::string& prefix, + std::string& suffix) -> raw::MjsFrame* { + auto world = mjs_findBody(spec.ptr, "world"); + if (!world) { + throw pybind11::value_error( + mjs_getError(mjs_getSpec(self.element))); + } + auto attached_world = + mjs_attachBody(&self, world, prefix.c_str(), suffix.c_str()); + if (!attached_world) { + throw pybind11::value_error( + mjs_getError(mjs_getSpec(self.element))); + } + return mjs_bodyToFrame(&attached_world); + }, + py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 74a34648..42686f1b 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -884,6 +884,19 @@ class SpecsTest(absltest.TestCase): frame = body.to_frame() np.testing.assert_array_equal(frame.pos, [1, 2, 3]) + def test_attach_spec_to_frame(self): + child = mujoco.MjSpec() + child.worldbody.add_camera(name='camera') + parent = mujoco.MjSpec() + frame = parent.worldbody.add_frame(name='frame') + frame.attach(child, 'child-', '') + self.assertLen(child.cameras, 1) + self.assertLen(parent.bodies, 1) + self.assertLen(parent.frames, 2) + self.assertEqual(parent.cameras[0].name, 'child-camera') + self.assertEqual(parent.frames[0].name, 'frame') + self.assertEqual(parent.frames[1].name, '') + if __name__ == '__main__': absltest.main() From 13b6055098dc79671f73b396d6b47b9cbcfed671 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 24 Nov 2024 08:38:41 -0800 Subject: [PATCH 095/426] Add material namespacing to sites. Fixes #2243. PiperOrigin-RevId: 699713625 Change-Id: I1b45e43f2a6ccfa64c46489c9426fcc2acdc94c3 --- src/user/user_objects.cc | 9 +++++++++ src/user/user_objects.h | 1 + test/user/user_api_test.cc | 10 +++++----- 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 8796910d..fd3064f3 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3102,6 +3102,15 @@ void mjCSite::CopyFromSpec() { +void mjCSite::NameSpace(const mjCModel* m) { + mjCBase::NameSpace(m); + if (!spec_material_.empty() && model != m) { + spec_material_ = m->prefix + spec_material_ + m->suffix; + } +} + + + // compiler void mjCSite::Compile(void) { CopyFromSpec(); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5b031d6b..3238f1ff 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -615,6 +615,7 @@ class mjCSite : public mjCSite_, private mjsSite { void Compile(void); // compiler void CopyFromSpec(); // copy spec into attributes void PointToLocal(void); + void NameSpace(const mjCModel* m); }; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 9646923d..b50b703d 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -731,7 +731,7 @@ static constexpr char xml_child[] = R"( - + @@ -798,7 +798,7 @@ TEST_F(MujocoTest, AttachSame) { - + @@ -811,7 +811,7 @@ TEST_F(MujocoTest, AttachSame) { - + @@ -952,7 +952,7 @@ TEST_F(MujocoTest, AttachDifferent) { - + @@ -1087,7 +1087,7 @@ TEST_F(MujocoTest, AttachFrame) { - + From c6ba8f27fd26a786d3ba2d07ee6d8cc17292e5c9 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 25 Nov 2024 05:53:03 -0800 Subject: [PATCH 096/426] Add bind() method to MjModel and MjData. This method allows users to bind a spec to a model or data object, making it less verbose to access its arrays. PiperOrigin-RevId: 699952211 Change-Id: I6b71a88d5a6968bc0f902bd49db7c568b46a4dfb --- doc/changelog.rst | 7 ++----- doc/python.rst | 15 ++++++++------- python/mjspec.ipynb | 6 +++--- python/mujoco/indexer_xmacro.h | 33 +++++++++++++++++++++++++++++++++ python/mujoco/specs_test.py | 7 +++++-- python/mujoco/structs.cc | 24 ++++++++++++++++++++++++ 6 files changed, 75 insertions(+), 17 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index e448e8bf..25baeac6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,11 +6,6 @@ Changelog Upcoming version (not yet released) ----------------------------------- -General -^^^^^^^ -- Removed id attribute from :ref:`mjSpec` objects in Python bindings. Using ids is error prone in scenarios of repeated - attachment and detachment. Python users are encouraged to use names for unique identification of model elements. - MJX ^^^ - Added muscle actuators. @@ -18,6 +13,8 @@ MJX Python bindings ^^^^^^^^^^^^^^^ - Provide prebuilt wheels for Python 3.13. +- Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and + detachment. Python users are encouraged to use names for unique identification of model elements. Bug fixes ^^^^^^^^^ diff --git a/doc/python.rst b/doc/python.rst index d5e4a165..3af46b6d 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -585,16 +585,17 @@ Model Editing includes a reimplementation of the ``PyMJCF`` example in the ``dm_control`` `tutorial notebook `__. -``PyMJCF`` provides a notion of "binding", giving access to :ref:`mjModel` and :ref:`mjData` values via the constructing -elements. In the native API, this is done with object ids. For example, say we have multiple geoms containing the string -"torso" in their name. We want to get their Cartesian positions in the XY plane from ``mjData``. This can be done as -follows: +``PyMJCF`` provides a notion of "binding", giving access to :ref:`mjModel` and :ref:`mjData` values via a helper class. +In the native API, the helper class is not needed, so it is possible to directly bind an ``mjs`` object to +:ref:`mjModel` and :ref:`mjData`. This requires the objects to have a non-empty name. For example, say we have multiple +geoms containing the string "torso" in their name. We want to get their Cartesian positions in the XY plane from +``mjData``. This can be done as follows: .. code-block:: python - torsos = [geom.id for geom in spec.geoms if 'torso' in geom.name] - pos_x = data.geom_xpos[torsos, 0] - pos_y = data.geom_xpos[torsos, 1] + torsos = [data.bind(geom) for geom in spec.geoms if 'torso' in geom.name] + pos_x = [torso.xpos[0] for torso in torsos] + pos_y = [torso.xpos[1] for torso in torsos] Notes ----- diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index 886032f4..f6fa9fe9 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -457,9 +457,9 @@ "pos_x = []\n", "pos_y = []\n", "geoms = arena.worldbody.find_all(mj.mjtObj.mjOBJ_GEOM)\n", - "torsos_data = [data.geom(geom.name) for geom in geoms if 'torso' in geom.name]\n", - "torsos_model = [model.geom(geom.name) for geom in geoms if 'torso' in geom.name]\n", - "actuators = [data.actuator(actuator.name) for actuator in arena.actuators]\n", + "torsos_data = [data.bind(geom) for geom in geoms if 'torso' in geom.name]\n", + "torsos_model = [model.bind(geom) for geom in geoms if 'torso' in geom.name]\n", + "actuators = [data.bind(actuator) for actuator in arena.actuators]\n", "\n", "# Control signal frequency, phase, amplitude.\n", "freq = 5\n", diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 6f9d2f9d..2b2b8a43 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -307,6 +307,28 @@ XGROUP( MjModelTupleViews, tuple, ntuple, MJMODEL_TUPLE ) \ XGROUP( MjModelKeyframeViews, key, nkey, MJMODEL_KEYFRAME ) +#define MJMODEL_BIND_GROUPS \ + XGROUP( mjsActuator, actuator) \ + XGROUP( mjsBody, body ) \ + XGROUP( mjsCamera, cam ) \ + XGROUP( mjsEquality, eq ) \ + XGROUP( mjsExclude, exclude ) \ + XGROUP( mjsGeom, geom ) \ + XGROUP( mjsHField, hfield ) \ + XGROUP( mjsJoint, jnt ) \ + XGROUP( mjsLight, light ) \ + XGROUP( mjsMaterial, mat ) \ + XGROUP( mjsMesh, mesh ) \ + XGROUP( mjsNumeric, numeric ) \ + XGROUP( mjsPair, pair ) \ + XGROUP( mjsSensor, sensor ) \ + XGROUP( mjsSite, site ) \ + XGROUP( mjsSkin, skin ) \ + XGROUP( mjsTendon, tendon ) \ + XGROUP( mjsTexture, tex ) \ + XGROUP( mjsTuple, tuple ) \ + XGROUP( mjsKey, key ) + #define MJMODEL_VIEW_GROUPS_ALTNAMES \ XGROUP( cam, camera, MJMODEL_CAMERA ) \ XGROUP( eq, equality, MJMODEL_EQUALITY ) \ @@ -399,6 +421,17 @@ XGROUP( MjDataSiteViews, site, nsite, MJDATA_SITE ) \ XGROUP( MjDataTendonViews, tendon, ntendon, MJDATA_TENDON ) +#define MJDATA_BIND_GROUPS \ + XGROUP( mjsActuator, actuator) \ + XGROUP( mjsBody, body ) \ + XGROUP( mjsCamera, cam ) \ + XGROUP( mjsGeom, geom ) \ + XGROUP( mjsJoint, jnt ) \ + XGROUP( mjsLight, light ) \ + XGROUP( mjsSensor, sensor ) \ + XGROUP( mjsSite, site ) \ + XGROUP( mjsTendon, tendon ) + #define MJDATA_VIEW_GROUPS_ALTNAMES \ XGROUP( cam, camera, MJDATA_CAMERA ) \ XGROUP( jnt, joint, MJDATA_JOINT ) \ diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 42686f1b..7d42d3d0 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -94,9 +94,12 @@ class SpecsTest(absltest.TestCase): # Compile the spec and check for expected values in the model. model = spec.compile() + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) self.assertEqual(model.nbody, 2) # 2 bodies, including the world body - np.testing.assert_array_equal(model.body('baz').pos, [1, 2, 3]) - np.testing.assert_array_equal(model.body('baz').quat, [0, 1, 0, 0]) + np.testing.assert_array_equal(model.bind(body).pos, [1, 2, 3]) + np.testing.assert_array_equal(model.bind(body).quat, [0, 1, 0, 0]) + np.testing.assert_array_equal(data.bind(body).xpos, [1, 2, 3]) self.assertEqual(model.nsite, 1) self.assertEqual(model.nuser_site, 6) np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6]) diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 56a6c8ee..53d1a72b 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -1716,6 +1716,18 @@ This is useful for example when the MJB is not available as a file on disk.)")); MJMODEL_VIEW_GROUPS #undef XGROUP +#define XGROUP(spectype, field) \ + mjModel.def( \ + "bind", \ + [](MjModelWrapper& m, spectype& spec) -> auto& { \ + return m.indexer().field##_by_name(mjs_getString(spec.name)); \ + }, \ + py::return_value_policy::reference_internal, \ + py::arg_v("spec", py::none())); + + MJMODEL_BIND_GROUPS +#undef XGROUP + #define XGROUP(field, altname, FIELD_XMACROS) \ mjModel.def( \ #altname, \ @@ -2050,6 +2062,18 @@ This is useful for example when the MJB is not available as a file on disk.)")); MJDATA_VIEW_GROUPS #undef XGROUP +#define XGROUP(spectype, field) \ + mjData.def( \ + "bind", \ + [](MjDataWrapper& d, spectype& spec) -> auto& { \ + return d.indexer().field##_by_name(mjs_getString(spec.name)); \ + }, \ + py::return_value_policy::reference_internal, \ + py::arg_v("spec", py::none())); + + MJDATA_BIND_GROUPS +#undef XGROUP + #define XGROUP(field, altname, FIELD_XMACROS) \ mjData.def( \ #altname, \ From e9a2f055f044fa2817b41bbc6ddd23a20f58c830 Mon Sep 17 00:00:00 2001 From: Balint-H Date: Mon, 25 Nov 2024 06:37:52 -0800 Subject: [PATCH 097/426] Copybara import of the project: -- 9d63ebca419496e36f94fbfec6f97f0227846cf5 by Balint-H : Throw exception if OBJ file is referenced in MJCF when loading in Unity -- a50486c19a3483cd914e7957d7d4a5efaab9d977 by Balint-H : Adjust line length COPYBARA_INTEGRATE_REVIEW=https://github.com/google-deepmind/mujoco/pull/2248 from Balint-H:fix/unity-obj-warning a50486c19a3483cd914e7957d7d4a5efaab9d977 PiperOrigin-RevId: 699962619 Change-Id: I2edff691fd5cedf79e2564f2ac5f2115393a4bf6 --- unity/Editor/Importer/MjImporterWithAssets.cs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/unity/Editor/Importer/MjImporterWithAssets.cs b/unity/Editor/Importer/MjImporterWithAssets.cs index 4713a9f8..a9a93184 100644 --- a/unity/Editor/Importer/MjImporterWithAssets.cs +++ b/unity/Editor/Importer/MjImporterWithAssets.cs @@ -152,6 +152,13 @@ public class MjImporterWithAssets : MjcfImporter { parentNode.GetStringAttribute("name", defaultValue: string.Empty); var assetReferenceName = MjEngineTool.Sanitize(unsanitizedAssetReferenceName); var sourceFilePath = Path.Combine(_sourceMeshesDir, fileName); + + if (Path.GetExtension(sourceFilePath) == ".obj") { + throw new NotImplementedException("OBJ mesh file loading is not yet implemented. " + + "Please convert to binary STL. " + + $"Attempted to load: {sourceFilePath}"); + } + var targetFilePath = Path.Combine(_targetMeshesDir, assetReferenceName + ".stl"); if (File.Exists(targetFilePath)) { File.Delete(targetFilePath); From f54cc58dc5ce797262e9ad789add00a4926cbe95 Mon Sep 17 00:00:00 2001 From: Balint-H Date: Mon, 25 Nov 2024 14:39:17 +0000 Subject: [PATCH 098/426] Reference the tutorial in the Unity plugin docs, and update information of the existing sections. --- doc/unity.rst | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/doc/unity.rst b/doc/unity.rst index cb2c2517..61993bf5 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -10,6 +10,9 @@ runtime to use the MuJoCo physics engine. Users can import MJCF files and edit relies on Unity for most aspects -- assets, game logic, simulation time -- but uses MuJoCo to determine how objects move, giving the designer access to MuJoCo's full API. +An example project using MuJoCo's Unity plugin in a set of introductory tutorials is available at +https://github.com/Balint-H/mj-unity-tutorial. + .. _UInstallation: Installation instructions @@ -145,10 +148,6 @@ effects: material assets for geom RGBA specification. - It allows the importer to handle :ref:`\ ` elements without replicating MuJoCo’s file-system workflow. -- The current version of MuJoCo generates MJCF files with explicit :ref:`\ ` elements, even when - the original model uses geoms for implicit definition of the body inertia. If you plan to change geom properties of - an imported model, remove these auto-generated ``MjInertial`` components manually. We plan to address this in a - future release of MuJoCo. In Unity, there is no equivalent to MJCF’s “cascading” :ref:`\ ` clauses. Therefore, components in Unity reflect the corresponding elements’ state after applying all the relevant default classes, and the class structure @@ -177,9 +176,11 @@ Scene recreation maintains continuity of physics and state in the following way: persisted. 4. The MuJoCo state (for the joints that persisted) is set from the cache, and Unity transforms are synchronized. -Because the MuJoCo library doesn’t (yet) expose an API for scene editing, adding and removing MuJoCo components causes -complete scene recreation. This can be expensive for large models or if it happens frequently. We expect this -performance limitation to be lifted in future versions of MuJoCo. +MuJoCo library has functionality for dynamic scene editing (through `mjSpec +`_), however, this is not yet +supported in the Unity plugin. Therefore, adding and removing MuJoCo components causes complete scene recreation. This +can be expensive for large models or if it happens frequently. We intend to lift this performance limitation to be in a +future versions of the plugin. Global Settings _______________ @@ -323,6 +324,20 @@ The plug-in allows using arbitrary Unity meshes for MuJoCo collision. At model `__ to create a convex hull of the mesh, and uses that for collisions. Currently the computed convex hull is not visible in Unity, but we intend to expose it in future versions. +Height fields +_____________ + +MuJoCo hfields are represented in Unity through terrain gameobjects. This allows the use of the terrain editing tools +available in Unity to generate shapes for collisions with MuJoCo. When selecting hfield type in the Unity geom +component, the right click context menu provides utility to add the corresponding Unity terrain to the scene. The data +from the terrain is dynamically kept in sync with the simulation. + +MuJoCo plugins +______________ + +The current version of the Unity package does not support loading MJCF scenes that use MuJoCo plugins such as +``elasticity``. Adding basic functionality to do this will be part of an upcoming release. + Interaction with External Processes ___________________________________ From 50bfbc91485123e02792a4c7bec9876ae017eeb3 Mon Sep 17 00:00:00 2001 From: Balint-H Date: Mon, 25 Nov 2024 17:15:42 +0000 Subject: [PATCH 099/426] Use RST link instead of explicit html link. --- doc/unity.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/doc/unity.rst b/doc/unity.rst index 61993bf5..ffa72b4b 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -176,8 +176,7 @@ Scene recreation maintains continuity of physics and state in the following way: persisted. 4. The MuJoCo state (for the joints that persisted) is set from the cache, and Unity transforms are synchronized. -MuJoCo library has functionality for dynamic scene editing (through `mjSpec -`_), however, this is not yet +MuJoCo has functionality for dynamic scene editing (through :ref:`mjSpec`), however, this is not yet supported in the Unity plugin. Therefore, adding and removing MuJoCo components causes complete scene recreation. This can be expensive for large models or if it happens frequently. We intend to lift this performance limitation to be in a future versions of the plugin. From 7e46e21ef303524add2815950feaed8ed5bc272c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 25 Nov 2024 09:17:26 -0800 Subject: [PATCH 100/426] Add mju_mat2Rot. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This function extracts the 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion. It is based on the paper "A robust method to extract the rotational part of deformations" by Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. PiperOrigin-RevId: 700006006 Change-Id: I77550993233dea9cdf68601762a3ae7ded749bdf --- doc/APIreference/functions.rst | 10 ++++ doc/includes/references.h | 1 + include/mujoco/mujoco.h | 4 ++ introspect/functions.py | 22 +++++++++ python/mujoco/functions.cc | 1 + src/engine/engine_util_spatial.c | 41 ++++++++++++++++ src/engine/engine_util_spatial.h | 4 ++ test/engine/engine_util_spatial_test.cc | 65 +++++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 ++ 9 files changed, 151 insertions(+) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 4bbf42e2..7a2b96cf 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3555,6 +3555,16 @@ Integrate quaternion given 3D angular velocity. Construct quaternion performing rotation from z-axis to given vector. +.. _mju_mat2Rot: + +`mju_mat2Rot <#mju_mat2Rot>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_mat2Rot + +extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion +returns the number of iterations required to converge + .. _mju_euler2Quat: `mju_euler2Quat <#mju_euler2Quat>`__ diff --git a/doc/includes/references.h b/doc/includes/references.h index adac0d47..6b9610a9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3481,6 +3481,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]); void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]); void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); +int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]); void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos1[3], const mjtNum quat1[4], diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 10b2f548..0c09f566 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1120,6 +1120,10 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); // Construct quaternion performing rotation from z-axis to given vector. MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); +// extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion +// returns the number of iterations required to converge +MJAPI int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]); + // Convert sequence of Euler angles (radians) to quaternion. // seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations. MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); diff --git a/introspect/functions.py b/introspect/functions.py index ed936aef..6a936263 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -7385,6 +7385,28 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Construct quaternion performing rotation from z-axis to given vector.', # pylint: disable=line-too-long )), + ('mju_mat2Rot', + FunctionDecl( + name='mju_mat2Rot', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='quat', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(4,), + ), + ), + FunctionParameterDecl( + name='mat', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(9,), + ), + ), + ), + doc='extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion returns the number of iterations required to converge', # pylint: disable=line-too-long + )), ('mju_euler2Quat', FunctionDecl( name='mju_euler2Quat', diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 5bb00ba9..e47f5a42 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1069,6 +1069,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); // Poses diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 57720e28..c170d923 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -286,6 +286,47 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { +// extract 3D rotation from an arbitrary 3x3 matrix +static const mjtNum rotEPS = 1e-9; +int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { + // Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. "A + // robust method to extract the rotational part of deformations." In + // Proceedings of the 9th International Conference on Motion in Games, pp. + // 55-60. 2016. + + int iter; + mjtNum col1_mat[3] = {mat[0], mat[3], mat[6]}; + mjtNum col2_mat[3] = {mat[1], mat[4], mat[7]}; + mjtNum col3_mat[3] = {mat[2], mat[5], mat[8]}; + for (iter = 0; iter < 500; iter++) { + mjtNum rot[9]; + mju_quat2Mat(rot, quat); + mjtNum col1_rot[3] = {rot[0], rot[3], rot[6]}; + mjtNum col2_rot[3] = {rot[1], rot[4], rot[7]}; + mjtNum col3_rot[3] = {rot[2], rot[5], rot[8]}; + mjtNum omega[3], vec1[3], vec2[3], vec3[3]; + mju_cross(vec1, col1_rot, col1_mat); + mju_cross(vec2, col2_rot, col2_mat); + mju_cross(vec3, col3_rot, col3_mat); + mju_add3(omega, vec1, vec2); + mju_addTo3(omega, vec3); + mju_scl3(omega, omega, 1.0 / (mju_abs(mju_dot3(col1_rot, col1_mat) + + mju_dot3(col2_rot, col2_mat) + + mju_dot3(col3_rot, col3_mat)) + mjMINVAL)); + mjtNum w = mju_normalize3(omega); + if (w < rotEPS) { + break; + } + mjtNum qrot[4]; + mju_axisAngle2Quat(qrot, omega, w); + mju_mulQuat(quat, qrot, quat); + mju_normalize4(quat); + } + return iter; +} + + + //------------------------------ pose operations (quat, pos) --------------------------------------- // multiply two poses diff --git a/src/engine/engine_util_spatial.h b/src/engine/engine_util_spatial.h index 6d173c5b..c46c9c03 100644 --- a/src/engine/engine_util_spatial.h +++ b/src/engine/engine_util_spatial.h @@ -59,6 +59,10 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); // compute quaternion performing rotation from z-axis to given vector MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); +// extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion +// returns the number of iterations required to converge +MJAPI int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]); + //------------------------------ pose operations (pos, quat) --------------------------------------- diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 7356a91c..e37beb41 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_util_spatial.c #include +#include #include #include @@ -196,5 +197,69 @@ TEST_F(Euler2QuatTest, Euler2Quat) { EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6)); } +using Mat2RotTest = MujocoTest; + +TEST_F(Mat2RotTest, RotationFromArbitraryMatrix) { + // create arbitrary target rotation matrix + mjtNum target[4], rot[9]; + mjtNum axis[3] = {1, 1, 1}; + mju_axisAngle2Quat(target, axis, mjPI/6); + mju_normalize4(target); + mju_quat2Mat(rot, target); + + // combine rotation with arbitrary stretch + mjtNum mat[9]; + mjtNum deformation_gradient[9] = {0.5, 0.25, 0.125, + 0.3, 0.66, 0.999, + 0.4, 0.22, 0.111}; + mjtNum stretch[9]; + mju_mulMatTMat3(stretch, deformation_gradient, deformation_gradient); + mju_mulMatMat3(mat, rot, stretch); + + // calculate rotational part of the matrix + mjtNum quat[4] = {1, 0, 0, 0}; + int niter = mju_mat2Rot(quat, mat); + EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), target)); + EXPECT_LE(niter, 150); +} + +TEST_F(Mat2RotTest, IdentityFromRandomRotation) { + // This test is based on the following paper: + // Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. "A + // robust method to extract the rotational part of deformations." In + // Proceedings of the 9th International Conference on Motion in Games, pp. + // 55-60. 2016. + mjtNum mat[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + srand(123); + + for (int i = 0; i < 100; ++i) { + // random quaternion + mjtNum quat[4]; + for (int j = 0; j < 4; ++j) { + quat[j] = rand() / (float)RAND_MAX; // NOLINT + } + + // calculate rotational part of the matrix + mjtNum res[9]; + mju_normalize4(quat); + EXPECT_LE(mju_mat2Rot(quat, mat), 40); + mju_quat2Mat(res, quat); + EXPECT_THAT(res, Pointwise(DoubleNear(1e-6), mat)); + } +} + +TEST_F(Mat2RotTest, SpecialCases) { + mjtNum eye[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + mjtNum quat[4] = {1, 0, 0, 0}; + EXPECT_EQ(mju_mat2Rot(quat, eye), 0); + EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0})); + mjtNum zero[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + EXPECT_EQ(mju_mat2Rot(quat, zero), 0); + EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0})); + mjtNum ones[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + EXPECT_EQ(mju_mat2Rot(quat, ones), 0); + EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0})); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2a553678..4c1f055c 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7276,6 +7276,9 @@ public static unsafe extern void mju_quatIntegrate(double* quat, double* vel, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_quatZ2Vec(double* quat, double* vec); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mju_mat2Rot(double* quat, double* mat); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_euler2Quat(double* quat, double* euler, [MarshalAs(UnmanagedType.LPStr)]string seq); From 96a8f002bcdb232fb24b6aa0d7bddf96788929bb Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Sun, 24 Nov 2024 15:34:35 -0500 Subject: [PATCH 101/426] Remove nroll argument from rollout nroll can always be implied from the other arguments of rollout. Add tests for nroll inference. --- doc/changelog.rst | 1 + python/mujoco/rollout.cc | 7 +-- python/mujoco/rollout.py | 10 +--- python/mujoco/rollout_test.py | 106 +++++++++++++++++++++++++++++++++- 4 files changed, 112 insertions(+), 12 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 25baeac6..17bdf077 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,6 +15,7 @@ Python bindings - Provide prebuilt wheels for Python 3.13. - Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and detachment. Python users are encouraged to use names for unique identification of model elements. +- Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. Bug fixes ^^^^^^^^^ diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index 6cfbb470..c250751f 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -39,7 +39,6 @@ Roll out open-loop trajectories from initial states, get resulting states and se input arguments (required): model instance of MjModel data associated instance of MjData - nroll integer, number of initial states from which to roll out trajectories nstep integer, number of steps to be taken for each trajectory control_spec specification of controls, ncontrol = mj_stateSize(m, control_spec) state0 (nroll x nstate) nroll initial state vectors, @@ -190,7 +189,7 @@ PYBIND11_MODULE(_rollout, pymodule) { pymodule.def( "rollout", [](const MjModelWrapper& m, MjDataWrapper& d, - int nroll, int nstep, unsigned int control_spec, + int nstep, unsigned int control_spec, const PyCArray state0, std::optional warmstart0, std::optional control, @@ -201,13 +200,14 @@ PYBIND11_MODULE(_rollout, pymodule) { raw::MjData* data = d.get(); // check that some steps need to be taken, return if not - if (nroll < 1 || nstep < 1) { + if (nstep < 1) { return; } // get sizes int nstate = mj_stateSize(model, mjSTATE_FULLPHYSICS); int ncontrol = mj_stateSize(model, control_spec); + int nroll = state0.shape(0); // get raw pointers mjtNum* state0_ptr = get_array_ptr(state0, "state0", nroll, 1, nstate); @@ -232,7 +232,6 @@ PYBIND11_MODULE(_rollout, pymodule) { }, py::arg("model"), py::arg("data"), - py::arg("nroll"), py::arg("nstep"), py::arg("control_spec"), py::arg("state0"), diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 8904e30c..9321f62f 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -29,7 +29,6 @@ def rollout(model: mujoco.MjModel, *, # require subsequent arguments to be named control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value, skip_checks: bool = False, - nroll: Optional[int] = None, nstep: Optional[int] = None, initial_warmstart: Optional[npt.ArrayLike] = None, state: Optional[npt.ArrayLike] = None, @@ -50,7 +49,6 @@ def rollout(model: mujoco.MjModel, ([nroll or 1] x [nstep or 1] x ncontrol) control_spec: mjtState specification of control vectors. skip_checks: Whether to skip internal shape and type checks. - nroll: Number of rollouts (inferred if unspecified). nstep: Number of steps in rollouts (inferred if unspecified). initial_warmstart: Initial qfrc_warmstart array (optional). ([nroll or 1] x nv) @@ -74,7 +72,7 @@ def rollout(model: mujoco.MjModel, # don't allocate output arrays # just call rollout and return if skip_checks: - _rollout.rollout(model, data, nroll, nstep, control_spec, initial_state, + _rollout.rollout(model, data, nstep, control_spec, initial_state, initial_warmstart, control, state, sensordata) return state, sensordata @@ -83,8 +81,6 @@ def rollout(model: mujoco.MjModel, raise ValueError('control_spec can only contain bits in mjSTATE_USER') # check types - if nroll and not isinstance(nroll, int): - raise ValueError('nroll must be an integer') if nstep and not isinstance(nstep, int): raise ValueError('nstep must be an integer') _check_must_be_numeric( @@ -121,7 +117,7 @@ def rollout(model: mujoco.MjModel, _check_trailing_dimension(model.nsensordata, sensordata=sensordata) # infer nroll, check for incompatibilities - nroll = _infer_dimension(0, nroll or 1, + nroll = _infer_dimension(0, 1, initial_state=initial_state, initial_warmstart=initial_warmstart, control=control, @@ -146,7 +142,7 @@ def rollout(model: mujoco.MjModel, sensordata = np.empty((nroll, nstep, model.nsensordata)) # call rollout - _rollout.rollout(model, data, nroll, nstep, control_spec, initial_state, + _rollout.rollout(model, data, nstep, control_spec, initial_state, initial_warmstart, control, state, sensordata) # return outputs diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index e1915592..4c2563e9 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -192,6 +192,110 @@ class MuJoCoRolloutTest(parameterized.TestCase): np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) + @parameterized.parameters(ALL_MODELS.keys()) + def test_infer_nroll_initial_state(self, model_name): + model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + nroll = 5 # number of rollouts + nstep = 1 # number of steps + + initial_state = np.random.randn(nroll, nstate) + control = np.random.randn(nstep, model.nu) + state, sensordata = rollout.rollout(model, data, initial_state, control) + + mujoco.mj_resetData(model, data) + control = np.tile(control, (nroll, 1, 1)) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + @parameterized.parameters(ALL_MODELS.keys()) + def test_infer_nroll_control(self, model_name): + model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + nroll = 5 # number of rollouts + nstep = 1 # number of steps + + initial_state = np.random.randn(nstate) + control = np.random.randn(nroll, nstep, model.nu) + state, sensordata = rollout.rollout(model, data, initial_state, control) + + mujoco.mj_resetData(model, data) + initial_state = np.tile(initial_state, (nroll, 1)) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + @parameterized.parameters(ALL_MODELS.keys()) + def test_infer_nroll_warmstart(self, model_name): + model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + nroll = 5 # number of rollouts + nstep = 1 # number of steps + + initial_state = np.random.randn(nstate) + control = np.random.randn(nstep, model.nu) + initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nroll, 1)) + state, sensordata = rollout.rollout(model, data, initial_state, control, + initial_warmstart=initial_warmstart) + + mujoco.mj_resetData(model, data) + initial_state = np.tile(initial_state, (nroll, 1)) + control = np.tile(control, (nroll, 1, 1)) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + @parameterized.parameters(ALL_MODELS.keys()) + def test_infer_nroll_state(self, model_name): + model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + nroll = 5 # number of rollouts + nstep = 1 # number of steps + + initial_state = np.random.randn(nstate) + control = np.random.randn(nstep, model.nu) + state = np.empty((nroll, nstep, nstate)) + state, sensordata = rollout.rollout(model, data, initial_state, control, + state=state) + + mujoco.mj_resetData(model, data) + initial_state = np.tile(initial_state, (nroll, 1)) + control = np.tile(control, (nroll, 1, 1)) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + @parameterized.parameters(ALL_MODELS.keys()) + def test_infer_nroll_sensordata(self, model_name): + model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + nroll = 5 # number of rollouts + nstep = 1 # number of steps + + initial_state = np.random.randn(nstate) + control = np.random.randn(nstep, model.nu) + sensordata = np.empty((nroll, nstep, model.nsensordata)) + state, sensordata = rollout.rollout(model, data, initial_state, control, + sensordata=sensordata) + + mujoco.mj_resetData(model, data) + initial_state = np.tile(initial_state, (nroll, 1)) + control = np.tile(control, (nroll, 1, 1)) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + @parameterized.parameters(ALL_MODELS.keys()) def test_one_rollout_fixed_ctrl(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) @@ -328,7 +432,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): def call_rollout(initial_state, control, state, sensordata): rollout.rollout(model, thread_local.data, initial_state, control, - skip_checks=True, nroll=initial_state.shape[0], + skip_checks=True, nstep=nstep, state=state, sensordata=sensordata) n = nroll // num_workers # integer division From 943eb6bc7e8b405029f0e6e7bd94e776f38b6bb3 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Sun, 24 Nov 2024 16:50:28 -0500 Subject: [PATCH 102/426] rollout accepts a list of models of length nroll --- doc/changelog.rst | 1 + doc/python.rst | 1 + python/mujoco/rollout.cc | 55 ++++++++++++++++++----------------- python/mujoco/rollout.py | 47 +++++++++++++++++++++--------- python/mujoco/rollout_test.py | 41 +++++++++++++++++++++++--- 5 files changed, 102 insertions(+), 43 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 17bdf077..36865344 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,7 @@ Python bindings - Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and detachment. Python users are encouraged to use names for unique identification of model elements. - Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. +- :ref:`rollout` can now accept lists of MjModel of length ``nroll``. Bug fixes ^^^^^^^^^ diff --git a/doc/python.rst b/doc/python.rst index 3af46b6d..5b82292e 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -700,6 +700,7 @@ states and sensor values. The basic usage form is state, sensordata = rollout.rollout(model, data, initial_state, control) +``model`` is either a single instance of MjModel or a list of compatible MjModel of length ``nroll``. ``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where ``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the :ref:`full physics state`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index c250751f..a738c87a 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -37,7 +37,7 @@ const auto rollout_doc = R"( Roll out open-loop trajectories from initial states, get resulting states and sensor values. input arguments (required): - model instance of MjModel + model list of MjModel instances of length nroll data associated instance of MjData nstep integer, number of steps to be taken for each trajectory control_spec specification of controls, ncontrol = mj_stateSize(m, control_spec) @@ -54,18 +54,18 @@ Roll out open-loop trajectories from initial states, get resulting states and se // C-style rollout function, assumes all arguments are valid // all input fields of d are initialised, contents at call time do not matter // after returning, d will contain the last step of the last rollout -void _unsafe_rollout(const mjModel* m, mjData* d, int nroll, int nstep, unsigned int control_spec, +void _unsafe_rollout(const mjModel** m, mjData* d, int nroll, int nstep, unsigned int control_spec, const mjtNum* state0, const mjtNum* warmstart0, const mjtNum* control, mjtNum* state, mjtNum* sensordata) { // sizes - int nstate = mj_stateSize(m, mjSTATE_FULLPHYSICS); - int ncontrol = mj_stateSize(m, control_spec); - int nv = m->nv, nbody = m->nbody, neq = m->neq; - int nsensordata = m->nsensordata; + int nstate = mj_stateSize(m[0], mjSTATE_FULLPHYSICS); + int ncontrol = mj_stateSize(m[0], control_spec); + int nv = m[0]->nv, nbody = m[0]->nbody, neq = m[0]->neq; + int nsensordata = m[0]->nsensordata; // clear user inputs if unspecified if (!(control_spec & mjSTATE_CTRL)) { - mju_zero(d->ctrl, m->nu); + mju_zero(d->ctrl, m[0]->nu); } if (!(control_spec & mjSTATE_QFRC_APPLIED)) { mju_zero(d->qfrc_applied, nv); @@ -75,26 +75,26 @@ void _unsafe_rollout(const mjModel* m, mjData* d, int nroll, int nstep, unsigned } if (!(control_spec & mjSTATE_MOCAP_POS)) { for (int i = 0; i < nbody; i++) { - int id = m->body_mocapid[i]; - if (id >= 0) mju_copy3(d->mocap_pos+3*id, m->body_pos+3*i); + int id = m[0]->body_mocapid[i]; + if (id >= 0) mju_copy3(d->mocap_pos+3*id, m[0]->body_pos+3*i); } } if (!(control_spec & mjSTATE_MOCAP_QUAT)) { for (int i = 0; i < nbody; i++) { - int id = m->body_mocapid[i]; - if (id >= 0) mju_copy4(d->mocap_quat+4*id, m->body_quat+4*i); + int id = m[0]->body_mocapid[i]; + if (id >= 0) mju_copy4(d->mocap_quat+4*id, m[0]->body_quat+4*i); } } if (!(control_spec & mjSTATE_EQ_ACTIVE)) { for (int i = 0; i < neq; i++) { - d->eq_active[i] = m->eq_active0[i]; + d->eq_active[i] = m[0]->eq_active0[i]; } } // loop over rollouts for (int r = 0; r < nroll; r++) { // set initial state - mj_setState(m, d, state0 + r*nstate, mjSTATE_FULLPHYSICS); + mj_setState(m[r], d, state0 + r*nstate, mjSTATE_FULLPHYSICS); // set warmstart accelerations if (warmstart0) { @@ -124,7 +124,7 @@ void _unsafe_rollout(const mjModel* m, mjData* d, int nroll, int nstep, unsigned for (; t < nstep; t++) { int step = r*nstep + t; if (state) { - mj_getState(m, d, state + step*nstate, mjSTATE_FULLPHYSICS); + mj_getState(m[r], d, state + step*nstate, mjSTATE_FULLPHYSICS); } if (sensordata) { mju_copy(sensordata + step*nsensordata, d->sensordata, nsensordata); @@ -137,15 +137,15 @@ void _unsafe_rollout(const mjModel* m, mjData* d, int nroll, int nstep, unsigned // controls if (control) { - mj_setState(m, d, control + step*ncontrol, control_spec); + mj_setState(m[r], d, control + step*ncontrol, control_spec); } // step - mj_step(m, d); + mj_step(m[r], d); // copy out new state if (state) { - mj_getState(m, d, state + step*nstate, mjSTATE_FULLPHYSICS); + mj_getState(m[r], d, state + step*nstate, mjSTATE_FULLPHYSICS); } // copy out sensor values @@ -188,7 +188,7 @@ PYBIND11_MODULE(_rollout, pymodule) { // get subsequent states and corresponding sensor values pymodule.def( "rollout", - [](const MjModelWrapper& m, MjDataWrapper& d, + [](py::list m, MjDataWrapper& d, int nstep, unsigned int control_spec, const PyCArray state0, std::optional warmstart0, @@ -196,7 +196,12 @@ PYBIND11_MODULE(_rollout, pymodule) { std::optional state, std::optional sensordata ) { - const raw::MjModel* model = m.get(); + // get raw pointers + int nroll = state0.shape(0); + const raw::MjModel* model_ptrs[nroll]; + for (int r = 0; r < nroll; r++) { + model_ptrs[r] = m[r].cast()->get(); + } raw::MjData* data = d.get(); // check that some steps need to be taken, return if not @@ -205,19 +210,17 @@ PYBIND11_MODULE(_rollout, pymodule) { } // get sizes - int nstate = mj_stateSize(model, mjSTATE_FULLPHYSICS); - int ncontrol = mj_stateSize(model, control_spec); - int nroll = state0.shape(0); + int nstate = mj_stateSize(model_ptrs[0], mjSTATE_FULLPHYSICS); + int ncontrol = mj_stateSize(model_ptrs[0], control_spec); - // get raw pointers mjtNum* state0_ptr = get_array_ptr(state0, "state0", nroll, 1, nstate); mjtNum* warmstart0_ptr = get_array_ptr(warmstart0, "warmstart0", nroll, - 1, model->nv); + 1, model_ptrs[0]->nv); mjtNum* control_ptr = get_array_ptr(control, "control", nroll, nstep, ncontrol); mjtNum* state_ptr = get_array_ptr(state, "state", nroll, nstep, nstate); mjtNum* sensordata_ptr = get_array_ptr(sensordata, "sensordata", nroll, - nstep, model->nsensordata); + nstep, model_ptrs[0]->nsensordata); // perform rollouts { @@ -226,7 +229,7 @@ PYBIND11_MODULE(_rollout, pymodule) { // call unsafe rollout function InterceptMjErrors(_unsafe_rollout)( - model, data, nroll, nstep, control_spec, state0_ptr, + model_ptrs, data, nroll, nstep, control_spec, state0_ptr, warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr); } }, diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 9321f62f..25852652 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -14,7 +14,7 @@ # ============================================================================== """Roll out open-loop trajectories from initial states, get subsequent states and sensor values.""" -from typing import Optional +from typing import Optional, Union import mujoco from mujoco import _rollout @@ -22,7 +22,7 @@ import numpy as np from numpy import typing as npt -def rollout(model: mujoco.MjModel, +def rollout(model: Union[mujoco.MjModel, list[mujoco.MjModel]], data: mujoco.MjData, initial_state: npt.ArrayLike, control: Optional[npt.ArrayLike] = None, @@ -41,7 +41,7 @@ def rollout(model: mujoco.MjModel, Allocates outputs if none are given. Args: - model: An mjModel instance. + model: An mjModel or a list of MjModel with the same size signature. data: An associated mjData instance. initial_state: Array of initial states from which to roll out trajectories. ([nroll or 1] x nstate) @@ -90,6 +90,7 @@ def rollout(model: mujoco.MjModel, state=state, sensordata=sensordata) + # check number of dimensions _check_number_of_dimensions(2, initial_state=initial_state, @@ -108,14 +109,6 @@ def rollout(model: mujoco.MjModel, state = _ensure_3d(state) sensordata = _ensure_3d(sensordata) - # check trailing dimensions - nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) - _check_trailing_dimension(nstate, initial_state=initial_state, state=state) - ncontrol = mujoco.mj_stateSize(model, control_spec) - _check_trailing_dimension(ncontrol, control=control) - _check_trailing_dimension(model.nv, initial_warmstart=initial_warmstart) - _check_trailing_dimension(model.nsensordata, sensordata=sensordata) - # infer nroll, check for incompatibilities nroll = _infer_dimension(0, 1, initial_state=initial_state, @@ -123,6 +116,14 @@ def rollout(model: mujoco.MjModel, control=control, state=state, sensordata=sensordata) + if isinstance(model, list) and nroll == 1: + nroll = len(model) + + if isinstance(model, list) and len(model) != nroll: + raise ValueError(f'nroll inferred as {nroll} ' + f'but model is length {len(model)}') + elif not isinstance(model, list): + model = [model] # Use a length 1 list to simplify code below # infer nstep, check for incompatibilities nstep = _infer_dimension(1, nstep or 1, @@ -130,7 +131,27 @@ def rollout(model: mujoco.MjModel, state=state, sensordata=sensordata) - # tile input arrays if required (singleton expansion) + # get nstate/ncontrol/nv/nsensordata + # check that they are equal across models + nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value) + ncontrol = mujoco.mj_stateSize(model[0], control_spec) + nv = model[0].nv + nsensordata = model[0].nsensordata + for m in model[1:]: + if (nstate != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) + or ncontrol != mujoco.mj_stateSize(m, control_spec) + or nv != m.nv + or nsensordata != m.nsensordata): + raise ValueError('models are not compatible') + + # check trailing dimensions + _check_trailing_dimension(nstate, initial_state=initial_state, state=state) + _check_trailing_dimension(ncontrol, control=control) + _check_trailing_dimension(nv, initial_warmstart=initial_warmstart) + _check_trailing_dimension(nsensordata, sensordata=sensordata) + + # tile input arrays/lists if required (singleton expansion) + model = model*nroll if len(model) == 1 else model initial_state = _tile_if_required(initial_state, nroll) initial_warmstart = _tile_if_required(initial_warmstart, nroll) control = _tile_if_required(control, nroll, nstep) @@ -139,7 +160,7 @@ def rollout(model: mujoco.MjModel, if state is None: state = np.empty((nroll, nstep, nstate)) if sensordata is None: - sensordata = np.empty((nroll, nstep, model.nsensordata)) + sensordata = np.empty((nroll, nstep, nsensordata)) # call rollout _rollout.rollout(model, data, nstep, control_spec, initial_state, diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 4c2563e9..8f118d6b 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -334,6 +334,34 @@ class MuJoCoRolloutTest(parameterized.TestCase): np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) + @parameterized.parameters(ALL_MODELS.keys()) + def test_multi_model(self, model_name): + nroll = 3 # number of initial states and models + nstep = 3 # number of timesteps + + spec = mujoco.MjSpec.from_string(ALL_MODELS[model_name]) + + if len(spec.bodies) > 1: + model = [] + for i in range(nroll): + body = spec.bodies[1] + assert body.name != 'world' + body.pos = body.pos + i + model.append(spec.compile()) + else: + model = [spec.compile() for i in range(nroll)] + + nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model[0]) + + initial_state = np.random.randn(nroll, nstate) + control = np.random.randn(nroll, nstep, model[0].nu) + state, sensordata = rollout.rollout(model, data, initial_state, control) + + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + @parameterized.parameters(ALL_MODELS.keys()) def test_multi_rollout_fixed_ctrl_infer_from_output(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) @@ -430,8 +458,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): def thread_initializer(): thread_local.data = mujoco.MjData(model) + model_list = [model]*nroll def call_rollout(initial_state, control, state, sensordata): - rollout.rollout(model, thread_local.data, initial_state, control, + rollout.rollout(model_list, thread_local.data, initial_state, control, skip_checks=True, nstep=nstep, state=state, sensordata=sensordata) @@ -677,13 +706,17 @@ def py_rollout(model, data, initial_state, control, control = ensure_3d(control) nroll = initial_state.shape[0] nstep = control.shape[1] - nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + + if isinstance(model, mujoco.MjModel): + model = [model]*nroll + + nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) + sensordata = np.empty((nroll, nstep, model[0].nsensordata)) for r in range(nroll): state_r, sensordata_r = one_rollout( - model, data, initial_state[r], control[r], control_spec + model[r], data, initial_state[r], control[r], control_spec ) state[r] = state_r sensordata[r] = sensordata_r From e310c23267dead8a1099916440e18d33233ea246 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 26 Nov 2024 02:31:53 -0800 Subject: [PATCH 103/426] Fix return value of mj_saveXML and mj_saveXMLString, fixes #2247 PiperOrigin-RevId: 700269761 Change-Id: Ifb7e9c66c70eac7d681cfcf106712d47291cf9e2 --- doc/APIreference/functions.rst | 5 +++-- doc/APIreference/functions_override.rst | 5 +++-- include/mujoco/mujoco.h | 3 ++- introspect/functions.py | 2 +- src/xml/xml_api.cc | 11 ++++++----- src/xml/xml_api.h | 2 +- test/xml/xml_api_test.cc | 3 +++ 7 files changed, 19 insertions(+), 12 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 7a2b96cf..c80c9628 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -94,7 +94,8 @@ Free last XML model if loaded. Called internally at each load. .. mujoco-include:: mj_saveXMLString -Save spec to XML string, return 1 on success, 0 otherwise. XML saving requires that the spec first be compiled. +Save spec to XML string, return 0 on success, -1 on failure. If the length of the output buffer is too small, returns +the required size. XML saving requires that the spec first be compiled. .. _mj_saveXML: @@ -103,7 +104,7 @@ Save spec to XML string, return 1 on success, 0 otherwise. XML saving requires t .. mujoco-include:: mj_saveXML -Save spec to XML file, return 1 on success, 0 otherwise. XML saving requires that the spec first be compiled. +Save spec to XML file, return 0 on success, -1 otherwise. XML saving requires that the spec first be compiled. .. _Mainsimulation: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 447a0a05..d34fa3f9 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -49,11 +49,12 @@ instances will be deleted; as in :ref:`mj_compile`, the compilation error can be .. _mj_saveXMLString: -Save spec to XML string, return 1 on success, 0 otherwise. XML saving requires that the spec first be compiled. +Save spec to XML string, return 0 on success, -1 on failure. If the length of the output buffer is too small, returns +the required size. XML saving requires that the spec first be compiled. .. _mj_saveXML: -Save spec to XML file, return 1 on success, 0 otherwise. XML saving requires that the spec first be compiled. +Save spec to XML file, return 0 on success, -1 otherwise. XML saving requires that the spec first be compiled. .. _Mainsimulation: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 0c09f566..2e2cab59 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -117,7 +117,8 @@ MJAPI int mj_saveLastXML(const char* filename, const mjModel* m, char* error, in // Free last XML model if loaded. Called internally at each load. MJAPI void mj_freeLastXML(void); -// Save spec to XML string, return 1 on success, 0 otherwise. +// Save spec to XML string, return 0 on success, -1 on failure. +// If length of the output buffer is too small, returns the required size. MJAPI int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int error_sz); // Save spec to XML file, return 1 on success, 0 otherwise. diff --git a/introspect/functions.py b/introspect/functions.py index 6a936263..4e1baaad 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -349,7 +349,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ type=ValueType(name='int'), ), ), - doc='Save spec to XML string, return 1 on success, 0 otherwise.', + doc='Save spec to XML string, return 0 on success, -1 on failure. If length of the output buffer is too small, returns the required size.', # pylint: disable=line-too-long )), ('mj_saveXML', FunctionDecl( diff --git a/src/xml/xml_api.cc b/src/xml/xml_api.cc index 8fadc4cb..627e2699 100644 --- a/src/xml/xml_api.cc +++ b/src/xml/xml_api.cc @@ -226,23 +226,24 @@ mjSpec* mj_parseXMLString(const char* xml, const mjVFS* vfs, char* error, int er -// save spec to XML file, return 1 on success, 0 otherwise +// save spec to XML file, return 0 on success, -1 otherwise int mj_saveXML(const mjSpec* s, const char* filename, char* error, int error_sz) { std::string result = WriteXML(NULL, s, error, error_sz); if (result.empty()) { - return 0; + return -1; } std::ofstream file; file.open(filename); file << result; file.close(); - return 1; + return 0; } -// save spec to string, return 1 on success, 0 otherwise +// save spec to XML string, return 0 on success, -1 on failure +// if length of the output buffer is too small, returns the required size int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int error_sz) { std::string result = WriteXML(NULL, s, error, error_sz); if (result.size() >= xml_sz) { @@ -252,7 +253,7 @@ int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int er return result.size(); } if (result.empty()) { - return 0; + return -1; } result.copy(xml, xml_sz); diff --git a/src/xml/xml_api.h b/src/xml/xml_api.h index fe4d0f18..8bb467c4 100644 --- a/src/xml/xml_api.h +++ b/src/xml/xml_api.h @@ -48,7 +48,7 @@ MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs); MJAPI mjSpec* mj_parseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); MJAPI mjSpec* mj_parseXMLString(const char* xml, const mjVFS* vfs, char* error, int error_sz); -// Save spec to XML file and/or string, return 1 on success, 0 otherwise. +// Save spec to XML file and/or string, return 0 on success, -1 otherwise. MJAPI int mj_saveXML(const mjSpec* s, const char* filename, char* error, int error_sz); MJAPI int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int error_sz); diff --git a/test/xml/xml_api_test.cc b/test/xml/xml_api_test.cc index 9b5b241c..dea098c6 100644 --- a/test/xml/xml_api_test.cc +++ b/test/xml/xml_api_test.cc @@ -140,6 +140,9 @@ TEST_F(MujocoTest, SaveXml) { EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data(); std::array out; + EXPECT_THAT(mj_saveXMLString(NULL, out.data(), out.size(), error.data(), + error.size()), -1); + EXPECT_STREQ(error.data(), "Cannot write empty model"); EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(), error.size()), 0) << error.data(); From bd361447bb1cdb41bb5615ee5dd8a55d5efe770d Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 26 Nov 2024 08:22:28 -0500 Subject: [PATCH 104/426] merge items in changelog --- doc/changelog.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 36865344..3449b398 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,7 +16,8 @@ Python bindings - Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and detachment. Python users are encouraged to use names for unique identification of model elements. - Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. -- :ref:`rollout` can now accept lists of MjModel of length ``nroll``. +- :ref:`rollout` can now accept lists of MjModel of length ``nroll``. ``nroll`` argument deprecated because + its value can always be inferred. Bug fixes ^^^^^^^^^ From b71683739553fd96deff1420d675a7e2588f993e Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 26 Nov 2024 05:16:33 -0800 Subject: [PATCH 105/426] Modify MJX solver to support elliptic friction cones with condim=3 and condim=4. PiperOrigin-RevId: 700306231 Change-Id: I50fd20cd367ecab11b23c03c0e0080266cc404e6 --- mjx/mujoco/mjx/_src/io.py | 5 +++++ mjx/mujoco/mjx/_src/io_test.py | 19 +++++++++++++++++++ mjx/mujoco/mjx/_src/solver.py | 14 ++++++++++++-- mjx/mujoco/mjx/_src/solver_test.py | 19 +++++++++++++++++++ 4 files changed, 55 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index fedb014e..860c8a44 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -224,6 +224,11 @@ def make_data( efc_address=efc_address, ) + if m.opt.cone == types.ConeType.ELLIPTIC and np.any(contact.dim == 1): + raise NotImplementedError( + 'condim=1 with ConeType.ELLIPTIC not implemented.' + ) + zero_fields = { 'solver_niter': (int,), 'time': (float,), diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index cbb10ebe..289c3890 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -20,6 +20,7 @@ import jax from jax import numpy as jp import mujoco from mujoco import mjx +from mujoco.mjx._src.types import ConeType import numpy as np @@ -495,5 +496,23 @@ class DataIOTest(parameterized.TestCase): # calling make_data. they should be interchangeable for jax functions: step_fn_jit(mjx.make_data(m)) + def test_contact_elliptic_condim1(self): + """Test that condim=1 with ConeType.ELLIPTIC is not implemented.""" + m = mujoco.MjModel.from_xml_string(""" + + + + + + + + + + """) + m.opt.cone = ConeType.ELLIPTIC + with self.assertRaises(NotImplementedError): + mjx.make_data(m) + + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/solver.py b/mjx/mujoco/mjx/_src/solver.py index 4c1bcbf9..52db56f7 100644 --- a/mjx/mujoco/mjx/_src/solver.py +++ b/mjx/mujoco/mjx/_src/solver.py @@ -279,7 +279,12 @@ def _update_constraint(m: Model, d: Data, ctx: _Context) -> _Context: friction = d.contact.friction[d.contact.dim > 1] efc_address = d.contact.efc_address[d.contact.dim > 1] dim = d.contact.dim[d.contact.dim > 1] - slice_fn = jax.vmap(lambda x: jax.lax.dynamic_slice(ctx.Jaref, (x,), (6,))) + # to prevent out of range append zeros to ctx.Jaref + slice_fn = jax.vmap( + lambda x: jax.lax.dynamic_slice( + jp.concatenate((ctx.Jaref, jp.zeros((3)))), (x,), (6,) + ) + ) u = slice_fn(efc_address) * ctx.fri mu, n, t = ctx.fri[:, 0], u[:, 0], jax.vmap(math.norm)(u[:, 1:]) @@ -433,7 +438,12 @@ def _linesearch(m: Model, d: Data, ctx: _Context) -> _Context: quad = quad.at[jp.array(efc_con)].add(quad[jp.array(efc_fri)]) # rescale to make primal cone circular - jv_fn = jax.vmap(lambda x: jax.lax.dynamic_slice(jv, (x,), (6,))) + # to prevent out of range append zeros to jv + jv_fn = jax.vmap( + lambda x: jax.lax.dynamic_slice( + jp.concatenate((jv, jp.zeros(3))), (x,), (6,) + ) + ) efc_elliptic = d.contact.efc_address[mask] v = jv_fn(efc_elliptic) * ctx.fri uu = jp.sum(ctx.u[:, 1:] * ctx.u[:, 1:], axis=1) diff --git a/mjx/mujoco/mjx/_src/solver_test.py b/mjx/mujoco/mjx/_src/solver_test.py index 989e4015..57d4a116 100644 --- a/mjx/mujoco/mjx/_src/solver_test.py +++ b/mjx/mujoco/mjx/_src/solver_test.py @@ -21,6 +21,7 @@ import mujoco from mujoco import mjx from mujoco.mjx._src import solver from mujoco.mjx._src import test_util +from mujoco.mjx._src.types import ConeType import numpy as np @@ -146,6 +147,24 @@ class SolverTest(parameterized.TestCase): nnz = dx.efc_J.any(axis=1) _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') + # TODO(taylorhowell): condim=1 with ConeType.ELLIPTIC + @parameterized.product(condim=(3, 4, 6), cone=tuple(ConeType)) + def test_condim(self, condim, cone): + """Test contact dimension.""" + m = mujoco.MjModel.from_xml_string(f""" + + + + + + + + + + """) + m.opt.cone = cone + solver.solve(mjx.put_model(m), mjx.put_data(m, mujoco.MjData(m))) + if __name__ == '__main__': absltest.main() From 5ba4d5849716389d7ac7e0a2095435c521e06007 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 26 Nov 2024 13:16:05 -0500 Subject: [PATCH 106/426] replace array on stack with std::vector --- python/mujoco/rollout.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index a738c87a..838ecc5b 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -54,7 +54,7 @@ Roll out open-loop trajectories from initial states, get resulting states and se // C-style rollout function, assumes all arguments are valid // all input fields of d are initialised, contents at call time do not matter // after returning, d will contain the last step of the last rollout -void _unsafe_rollout(const mjModel** m, mjData* d, int nroll, int nstep, unsigned int control_spec, +void _unsafe_rollout(std::vector& m, mjData* d, int nroll, int nstep, unsigned int control_spec, const mjtNum* state0, const mjtNum* warmstart0, const mjtNum* control, mjtNum* state, mjtNum* sensordata) { // sizes @@ -198,7 +198,8 @@ PYBIND11_MODULE(_rollout, pymodule) { ) { // get raw pointers int nroll = state0.shape(0); - const raw::MjModel* model_ptrs[nroll]; + std::vector model_ptrs; + model_ptrs.reserve(nroll); for (int r = 0; r < nroll; r++) { model_ptrs[r] = m[r].cast()->get(); } From 203602b01f6c81903ac4183ae2c4305c26311f6e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 26 Nov 2024 10:16:16 -0800 Subject: [PATCH 107/426] Move computation of flex_vert0 to user_mesh.cc. This removes the dependency of flex_vert0 from mj_flex and mjData. PiperOrigin-RevId: 700382297 Change-Id: I0b6d023c2e1ba14fa92417b65bfc288f37c223ae --- src/engine/engine_setconst.c | 12 ------------ src/user/user_mesh.cc | 10 ++++++++++ src/user/user_model.cc | 3 +++ src/user/user_objects.h | 2 ++ 4 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index c1926673..04fcec99 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -120,18 +120,6 @@ static void set0(mjModel* m, mjData* d) { m->light_mode[i] = lightmode[i]; } - // compute bounding box coordinates - for (int i=0; i < m->nflex; i++) { - int bvhadr = m->flex_bvhadr[i]; - const mjtNum* bvh = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic); - for (int j=0; j < m->nflexvert; j++) { - for (int k=0; k < 3; k++) { - mjtNum size = 2*(bvh[3+k] - m->flex_radius[i]); - m->flex_vert0[3*j+k] = (d->flexvert_xpos[3*j+k] - bvh[k]) / size + 0.5; - } - } - } - // copy fields mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge); mju_copy(m->tendon_length0, d->ten_length, m->ntendon); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 766e4976..f92f8c66 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -2944,6 +2944,16 @@ void mjCFlex::Compile(const mjVFS* vfs) { // create bounding volume hierarchy CreateBVH(); + + // compute bounding box coordinates + vert0_.assign(3*nvert, 0); + const mjtNum* bvh = tree.Bvh().data(); + for (int j=0; j < nvert; j++) { + for (int k=0; k < 3; k++) { + double size = 2*(bvh[k+3] - radius); + vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / size + 0.5; + } + } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index fb21b1b1..f3223a0f 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2825,6 +2825,9 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert); } + // copy vert0 + mjuu_copyvec(m->flex_vert0 + 3*vert_adr, pfl->vert0_.data(), 3*pfl->nvert); + // copy or set vertbodyid if (pfl->rigid) { for (int k=0; knvert; k++) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 3238f1ff..d5dd09b9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -774,6 +774,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { void Compile(const mjVFS* vfs); // compiler void CreateBVH(void); // create flex BVH void CreateShellPair(void); // create shells and evpairs + + std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box }; From 41b325785a630d45e80101667262f6be8d3a1f3d Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 26 Nov 2024 16:42:08 -0500 Subject: [PATCH 108/426] fix creation of std::vector --- python/mujoco/rollout.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index 838ecc5b..05576ba8 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -198,8 +198,7 @@ PYBIND11_MODULE(_rollout, pymodule) { ) { // get raw pointers int nroll = state0.shape(0); - std::vector model_ptrs; - model_ptrs.reserve(nroll); + std::vector model_ptrs(nroll); for (int r = 0; r < nroll; r++) { model_ptrs[r] = m[r].cast()->get(); } From e6eab04dd092554af7d37ac2f03faa40116ec862 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Wed, 27 Nov 2024 07:02:16 -0500 Subject: [PATCH 109/426] rollout fixups and correctly initialize unspecified controls --- doc/changelog.rst | 2 +- doc/python.rst | 2 +- python/mujoco/rollout.cc | 36 ++++++++++++++++++----------------- python/mujoco/rollout.py | 10 +++++++--- python/mujoco/rollout_test.py | 2 +- 5 files changed, 29 insertions(+), 23 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 3449b398..f148d26b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,7 +16,7 @@ Python bindings - Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and detachment. Python users are encouraged to use names for unique identification of model elements. - Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. -- :ref:`rollout` can now accept lists of MjModel of length ``nroll``. ``nroll`` argument deprecated because +- :ref:`rollout` can now accept sequences of MjModel of length ``nroll``. ``nroll`` argument deprecated because its value can always be inferred. Bug fixes diff --git a/doc/python.rst b/doc/python.rst index 5b82292e..5a97f377 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -700,7 +700,7 @@ states and sensor values. The basic usage form is state, sensordata = rollout.rollout(model, data, initial_state, control) -``model`` is either a single instance of MjModel or a list of compatible MjModel of length ``nroll``. +``model`` is either a single instance of MjModel or a sequence of compatible MjModel of length ``nroll``. ``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where ``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the :ref:`full physics state`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index 05576ba8..ffedda3f 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -73,26 +73,28 @@ void _unsafe_rollout(std::vector& m, mjData* d, int nroll, int n if (!(control_spec & mjSTATE_XFRC_APPLIED)) { mju_zero(d->xfrc_applied, 6*nbody); } - if (!(control_spec & mjSTATE_MOCAP_POS)) { - for (int i = 0; i < nbody; i++) { - int id = m[0]->body_mocapid[i]; - if (id >= 0) mju_copy3(d->mocap_pos+3*id, m[0]->body_pos+3*i); - } - } - if (!(control_spec & mjSTATE_MOCAP_QUAT)) { - for (int i = 0; i < nbody; i++) { - int id = m[0]->body_mocapid[i]; - if (id >= 0) mju_copy4(d->mocap_quat+4*id, m[0]->body_quat+4*i); - } - } - if (!(control_spec & mjSTATE_EQ_ACTIVE)) { - for (int i = 0; i < neq; i++) { - d->eq_active[i] = m[0]->eq_active0[i]; - } - } // loop over rollouts for (int r = 0; r < nroll; r++) { + // clear user inputs if unspecified + if (!(control_spec & mjSTATE_MOCAP_POS)) { + for (int i = 0; i < nbody; i++) { + int id = m[r]->body_mocapid[i]; + if (id >= 0) mju_copy3(d->mocap_pos+3*id, m[r]->body_pos+3*i); + } + } + if (!(control_spec & mjSTATE_MOCAP_QUAT)) { + for (int i = 0; i < nbody; i++) { + int id = m[r]->body_mocapid[i]; + if (id >= 0) mju_copy4(d->mocap_quat+4*id, m[r]->body_quat+4*i); + } + } + if (!(control_spec & mjSTATE_EQ_ACTIVE)) { + for (int i = 0; i < neq; i++) { + d->eq_active[i] = m[r]->eq_active0[i]; + } + } + // set initial state mj_setState(m[r], d, state0 + r*nstate, mjSTATE_FULLPHYSICS); diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 25852652..f314e795 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -14,6 +14,7 @@ # ============================================================================== """Roll out open-loop trajectories from initial states, get subsequent states and sensor values.""" +from collections.abc import Sequence from typing import Optional, Union import mujoco @@ -22,7 +23,7 @@ import numpy as np from numpy import typing as npt -def rollout(model: Union[mujoco.MjModel, list[mujoco.MjModel]], +def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], data: mujoco.MjData, initial_state: npt.ArrayLike, control: Optional[npt.ArrayLike] = None, @@ -41,7 +42,7 @@ def rollout(model: Union[mujoco.MjModel, list[mujoco.MjModel]], Allocates outputs if none are given. Args: - model: An mjModel or a list of MjModel with the same size signature. + model: An mjModel or a sequence of MjModel with the same size signature. data: An associated mjData instance. initial_state: Array of initial states from which to roll out trajectories. ([nroll or 1] x nstate) @@ -76,6 +77,9 @@ def rollout(model: Union[mujoco.MjModel, list[mujoco.MjModel]], initial_warmstart, control, state, sensordata) return state, sensordata + if not isinstance(model, mujoco.MjModel): + model = list(model) + # check control_spec if control_spec & ~mujoco.mjtState.mjSTATE_USER.value: raise ValueError('control_spec can only contain bits in mjSTATE_USER') @@ -151,7 +155,7 @@ def rollout(model: Union[mujoco.MjModel, list[mujoco.MjModel]], _check_trailing_dimension(nsensordata, sensordata=sensordata) # tile input arrays/lists if required (singleton expansion) - model = model*nroll if len(model) == 1 else model + model = model * nroll if len(model) == 1 else model initial_state = _tile_if_required(initial_state, nroll) initial_warmstart = _tile_if_required(initial_warmstart, nroll) control = _tile_if_required(control, nroll, nstep) diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 8f118d6b..32c670e6 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -458,7 +458,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): def thread_initializer(): thread_local.data = mujoco.MjData(model) - model_list = [model]*nroll + model_list = [model] * nroll def call_rollout(initial_state, control, state, sensordata): rollout.rollout(model_list, thread_local.data, initial_state, control, skip_checks=True, From f2e9e38edfc05f25fe93735ca8eb6aa16494fbdf Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Wed, 27 Nov 2024 08:12:35 -0800 Subject: [PATCH 110/426] Set geom_matid as a jax.Array for visual domain randomization. PiperOrigin-RevId: 700706160 Change-Id: I13ef4da8cd3a671446e4bdd186dcacb4d0b3fb00 --- mjx/mujoco/mjx/_src/types.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 89dab88e..6446cd33 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -933,7 +933,7 @@ class Model(PyTreeNode): geom_sameframe: np.ndarray geom_dataid: np.ndarray geom_group: np.ndarray - geom_matid: np.ndarray + geom_matid: jax.Array geom_priority: np.ndarray geom_solmix: jax.Array geom_solref: jax.Array From 300450f8b8441e5f00abc1105089a23f50d7147c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 27 Nov 2024 10:13:28 -0800 Subject: [PATCH 111/426] Add circle to flexcomp and remove rope and loop from composite. Add pulley example to show a circle use case. Switch on flex edge rendering by default (no effect when flex skin is rendered). PiperOrigin-RevId: 700738379 Change-Id: I8469bbf194de01c8dab66c9a51c49879013df141 --- doc/XMLreference.rst | 23 ++--- doc/changelog.rst | 5 + model/flex/pulley.xml | 51 ++++++++++ src/engine/engine_vis_init.c | 2 +- src/user/user_composite.cc | 179 ++--------------------------------- src/user/user_composite.h | 2 - src/user/user_flexcomp.cc | 32 +++++-- src/user/user_flexcomp.h | 1 + src/xml/xml_native_reader.cc | 1 + 9 files changed, 98 insertions(+), 198 deletions(-) create mode 100644 model/flex/pulley.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 16ce21a0..0ce73814 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2981,20 +2981,6 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui define the tendons). The "main" tendons are parallel to the axes of the grid. In addition one can create diagonal "shear" tendons, using the :el:`tendon` sub-element. This type is suitable for simulating strings as well as cloth. - The **rope** type creates a 1D grid of bodies, each having a geom with user-defined type (sphere, capsule or - ellipsoid) and 2 hinge joints with axes orthogonal to the grid, creating a universal joint with the previous body. - This corresponds to a kinematic chain which can bend but cannot stretch or twist. In addition, one can specify - stretch and twist joints (slide and hinge respectively) with the :el:`joint` sub-element. When specified, these extra - joints are equality-constrained, but the constraint is soft by default so that some stretch and twist are possible. - The rope can extend in one or both directions from the parent body. To specify the origin of the rope, the parent - body *must* be named so that it fits the automatic naming convention. For example, to make the parent be the first - body in the chain, and assuming we have prefix="C", the parent body should be named "CB0". When the parent is not at - the end, the rope consists of two kinematic chains starting at the parent and extending in opposite directions. - - The **loop** type is the same as the rope type except the elements are arranged in a circle, and the first and last - elements are equality-constrained to remain connected (using the "connect" constraint type). The softness of this - equality constraint is adjusted with the attributes solrefsmooth and solimpsmooth. - The **cable** type creates a 1D chain of bodies connected with ball joints, each having a geom with user-defined type (cylinder, capsule or box). The geometry can either be defined with an array of 3D vertex coordinates :at:`vertex` or with prescribed functions with the option :at:`curve`. Currently, only linear and trigonometric functions are @@ -3489,7 +3475,7 @@ saving the XML: .. _body-flexcomp-type: -:at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, mesh, gmsh, direct], "grid"` +:at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, disc, circle, mesh, gmsh, direct], "grid"` This attribute determines the type of :el:`flexcomp` object. The remaining attributes and sub-elements are then interpreted according to the type. Default settings are also adjusted depending on the type. Different types correspond to different methods for specifying the flexcomp points and the stretchable elements that connect them. @@ -3513,6 +3499,13 @@ saving the XML: **ellipsoid** is the same as **box**, except the points are projected on the surface of an ellipsoid. + **disc** is the same as **box**, except the points are projected on the surface of a disc. It is only compatible + with :at:`dim=2`. + + **circle** is the same as **grid**, except the points are sampled along a circle so that the first and last points + are the same. The radius of the circle is computed such that each segment has the requested spacing. It is only + compatible with :at:`dim=1`. + **mesh** loads the flexcomp points and elements (i.e. triangles) from a mesh file, in the same file formats as mesh assets. A mesh asset is not actually added to the model. Instead the vertex and face data from the mesh file are used to populate the point and element data of the flexcomp. :at:`dim` is automatically set to 2. Recall that a mesh asset diff --git a/doc/changelog.rst b/doc/changelog.rst index f148d26b..28819ba9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,6 +6,11 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +- Removed rope and loop from :ref:`composite`. The user is encouraged to instead use the :at:`cable` + plugin or :ref:`flexcomp`, respectively. + MJX ^^^ - Added muscle actuators. diff --git a/model/flex/pulley.xml b/model/flex/pulley.xml new file mode 100644 index 00000000..5e07430d --- /dev/null +++ b/model/flex/pulley.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index 3e0d46a2..04d2079e 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -93,7 +93,7 @@ const char* mjVISSTRING[mjNVISFLAG][3] = { {"Static Body", "1", "D"}, {"Skin", "1", ";"}, {"Flex Vert", "0", ""}, - {"Flex Edge", "0", ""}, + {"Flex Edge", "1", ""}, {"Flex Face", "0", ""}, {"Flex Skin", "1", ""}, {"Body Tree", "0", "`"}, diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 73c1227d..ab34b7b8 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -309,15 +309,15 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) case mjCOMPTYPE_ROPE: return comperr(error, - "The \"rope\" composite type is deprecated. Please use " - "\"cable\" instead.", - error_sz); + "The \"rope\" composite type is deprecated. Please use " + "\"cable\" instead.", + error_sz); case mjCOMPTYPE_LOOP: - mju_warning( - "The \"loop\" composite type is deprecated. Please use \"cable\" " - "instead."); - return MakeRope(model, body, error, error_sz); + return comperr(error, + "The \"loop\" composite type is deprecated. Please use " + "\"flexcomp\" instead.", + error_sz); case mjCOMPTYPE_CABLE: return MakeCable(model, body, error, error_sz); @@ -729,171 +729,6 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, } -// make rope -bool mjCComposite::MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz) { - // check dim - if (dim!=1) { - return comperr(error, "Rope must be one-dimensional", error_sz); - } - - // check root body name prefix - char txt[200]; - mju::sprintf_arr(txt, "%sB", prefix.c_str()); - std::string body_name = mjs_getString(body->name); - if (std::strncmp(txt, body_name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) { - mju::strcat_arr(txt, " must be the beginning of root body name"); - return comperr(error, txt, error_sz); - } - - // read origin coordinate from root body - mju::strcpy_arr(txt, body_name.substr(strlen(txt)).c_str()); - int ox = -1; - if (sscanf(txt, "%d", &ox)!=1) { - return comperr(error, "Root body name must contain X coordinate", error_sz); - } - if (ox<0 || ox>=count[0]) { - return comperr(error, "Root body coordinate out of range", error_sz); - } - - // add origin - AddRopeBody(model, body, ox, ox); - - // add elements: right - mjsBody* pbody = body; - for (int ix=ox; ix0; ix--) { - pbody = AddRopeBody(model, pbody, ix, ix-1); - } - - // close loop - if (type==mjCOMPTYPE_LOOP) { - char txt2[200]; - - // add equality constraint - mjsEquality* eq = mjs_addEquality(&model->spec, 0); - eq->type = mjEQ_CONNECT; - mju::sprintf_arr(txt, "%sB0", prefix.c_str()); - mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1); - mjs_setString(eq->name1, txt); - mjs_setString(eq->name2, txt2); - mjuu_setvec(eq->data, -0.5*spacing, 0, 0); - mju_copy(eq->solref, solrefsmooth, mjNREF); - mju_copy(eq->solimp, solimpsmooth, mjNIMP); - - // remove contact between connected bodies - mjsExclude* pair = mjs_addExclude(&model->spec); - mjs_setString(pair->bodyname1, std::string(txt).c_str()); - mjs_setString(pair->bodyname2, std::string(txt2).c_str()); - } - - return true; -} - - - -// add child body for cloth -mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1) { - char txt[100]; - bool isroot = (ix==ix1); - double dx = spacing*(ix1-ix); - - // add child if not root - if (!isroot) { - body = mjs_addBody(body, 0); - mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), ix1); - mjs_setString(body->name, txt); - - // loop - if (type==mjCOMPTYPE_LOOP) { - double alpha = 2*mjPI/count[0]; - double R = 0.5*spacing*sin(mjPI-alpha)/sin(0.5*alpha); - - if (ix1>ix) { - mjuu_setvec(body->pos, R*cos(0.5*alpha), R*sin(0.5*alpha), 0); - mjuu_setvec(body->quat, cos(0.5*alpha), 0, 0, sin(0.5*alpha)); - } else { - mjuu_setvec(body->pos, -R*cos(0.5*alpha), R*sin(0.5*alpha), 0); - mjuu_setvec(body->quat, cos(-0.5*alpha), 0, 0, sin(-0.5*alpha)); - } - } - - // no loop - else { - mjuu_setvec(body->pos, dx, 0, 0); - } - } - - // add geom - mjsGeom* geom = mjs_addGeom(body, &def[0].spec); - mjs_setDefault(geom->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1); - mjs_setString(geom->name, txt); - mjuu_setvec(geom->pos, 0, 0, 0); - mjuu_setvec(geom->quat, sqrt(0.5), 0, sqrt(0.5), 0); - - // root: no joints - if (isroot) { - return body; - } - - // add main joint - for (int i=0; i<2; i++) { - // add joint - mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec); - mjs_setDefault(jnt->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1); - mjs_setString(jnt->name, txt); - jnt->type = mjJNT_HINGE; - mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); - mjuu_setvec(jnt->axis, 0, 0, 0); - jnt->axis[i+1] = 1; - } - - // add twist joint - if (add[mjCOMPKIND_TWIST]) { - // add joint - mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0].spec); - mjs_setDefault(jnt->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1); - mjs_setString(jnt->name, txt); - jnt->type = mjJNT_HINGE; - mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); - mjuu_setvec(jnt->axis, 1, 0, 0); - - // add constraint - mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec); - mjs_setDefault(eq->element, &model->Default()->spec); - eq->type = mjEQ_JOINT; - mjs_setString(eq->name1, mjs_getString(jnt->name)); - } - - // add stretch joint - if (add[mjCOMPKIND_STRETCH]) { - // add joint - mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0].spec); - mjs_setDefault(jnt->element, mjs_getDefault(body->element)); - mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1); - mjs_setString(jnt->name, txt); - jnt->type = mjJNT_SLIDE; - mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); - mjuu_setvec(jnt->axis, 1, 0, 0); - - // add constraint - mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec); - mjs_setDefault(eq->element, &model->Default()->spec); - eq->type = mjEQ_JOINT; - mjs_setString(eq->name1, mjs_getString(jnt->name)); - } - - return body; -} - - // add shear tendons to 2D void mjCComposite::MakeShear(mjCModel* model) { diff --git a/src/user/user_composite.h b/src/user/user_composite.h index add71636..9e42dba3 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -70,7 +70,6 @@ class mjCComposite { bool MakeParticle(mjCModel* model, mjsBody* body, char* error, int error_sz); bool MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz); - bool MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz); bool MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz); void MakeShear(mjCModel* model); @@ -124,7 +123,6 @@ class mjCComposite { int dim; // dimensionality private: - mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1); mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]); // temporary skin vectors diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 67443e22..58a50bcb 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -157,6 +157,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { bool res; switch (type) { case mjFCOMPTYPE_GRID: + case mjFCOMPTYPE_CIRCLE: res = MakeGrid(error, error_sz); break; @@ -548,15 +549,30 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // 1D if (dim == 1) { for (int ix=0; ix < count[0]; ix++) { - // add point - point.push_back(spacing[0]*(ix - 0.5*(count[0]-1))); - point.push_back(0); - point.push_back(0); + if (type == mjFCOMPTYPE_CIRCLE) { + // add point + double theta = 2*mjPI/(count[0]-1); + double radius = spacing[0]/std::sin(theta/2)/2; + point.push_back(radius*std::cos(theta*ix)); + point.push_back(radius*std::sin(theta*ix)); + point.push_back(0); - // add element - if (ix < count[0]-1) { - element.push_back(ix); - element.push_back(ix+1); + // add element + if (ix < count[0]-1) { + element.push_back(ix); + element.push_back(ix == count[0]-2 ? 0 : ix+1); + } + } else { + // add point + point.push_back(spacing[0]*(ix - 0.5*(count[0]-1))); + point.push_back(0); + point.push_back(0); + + // add element + if (ix < count[0]-1) { + element.push_back(ix); + element.push_back(ix+1); + } } } } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 92ce884c..6621c650 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -31,6 +31,7 @@ typedef enum _mjtFcompType { mjFCOMPTYPE_ELLIPSOID, mjFCOMPTYPE_SQUARE, mjFCOMPTYPE_DISC, + mjFCOMPTYPE_CIRCLE, mjFCOMPTYPE_MESH, mjFCOMPTYPE_GMSH, mjFCOMPTYPE_DIRECT, diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 9a8ad20d..e25626d2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -794,6 +794,7 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = { {"ellipsoid", mjFCOMPTYPE_ELLIPSOID}, {"square", mjFCOMPTYPE_SQUARE}, {"disc", mjFCOMPTYPE_DISC}, + {"circle", mjFCOMPTYPE_CIRCLE}, {"mesh", mjFCOMPTYPE_MESH}, {"gmsh", mjFCOMPTYPE_GMSH}, {"direct", mjFCOMPTYPE_DIRECT} From 82b6dbeb40fcf249e1c40cf87c245da8baf7a983 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 29 Nov 2024 07:40:44 -0800 Subject: [PATCH 112/426] Copy attached spec before attaching anything else. Spec were copied after bodies are attached, so the reference to the source spec got lost during mj_copySpec(). PiperOrigin-RevId: 701287267 Change-Id: I55eff53c7ae9d42b03957be48ea506b80d274e03 --- src/user/user_model.cc | 8 +++++--- test/user/user_api_test.cc | 18 ++++++++++++++---- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f3223a0f..556b623b 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -182,6 +182,11 @@ mjCModel& mjCModel::operator=(const mjCModel& other) { *static_cast(this) = static_cast(other); *static_cast(this) = static_cast(other); + // copy attached specs first so that we can resolve references to them + for (const auto* s : other.specs_) { + specs_.push_back(mj_copySpec(s)); + } + // the world copy constructor takes care of copying the tree mjCBody* world = new mjCBody(*other.bodies_[0], this); bodies_.push_back(world); @@ -398,9 +403,6 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) { } CopyList(numerics_, other.numerics_); CopyList(texts_, other.texts_); - for (const auto* s : other.specs_) { - specs_.push_back(mj_copySpec(s)); - } } CopyList(flexes_, other.flexes_); CopyList(pairs_, other.pairs_); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index b50b703d..5da06dc1 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1998,16 +1998,22 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { TEST_F(MujocoTest, DifferentUnitsAllowed) { mjSpec* child = mj_makeSpec(); - child->compiler.degree = 1; + child->compiler.degree = 0; mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0); body->alt.type = mjORIENTATION_EULER; - body->alt.euler[0] = 90; + body->alt.euler[0] = -mjPI / 2; + mjsGeom* geom = mjs_addGeom(body, 0); + geom->size[0] = 1; + mjsJoint* joint = mjs_addJoint(body, 0); + joint->type = mjJNT_HINGE; + joint->range[0] = -mjPI / 4; + joint->range[1] = mjPI / 4; mjSpec* parent = mj_makeSpec(); - parent->compiler.degree = 0; + parent->compiler.degree = 1; mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0); frame->alt.type = mjORIENTATION_EULER; - frame->alt.euler[0] = -mjPI / 2; + frame->alt.euler[0] = 90; EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), NotNull()); mjModel* model = mj_compile(parent, 0); @@ -2016,6 +2022,8 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { EXPECT_NEAR(model->body_quat[5], 0, 1e-12); EXPECT_NEAR(model->body_quat[6], 0, 1e-12); EXPECT_NEAR(model->body_quat[7], 0, 1e-12); + EXPECT_NEAR(model->jnt_range[0], -mjPI / 4, 1e-7); + EXPECT_NEAR(model->jnt_range[1], mjPI / 4, 1e-7); mjSpec* copy = mj_copySpec(parent); EXPECT_THAT(copy, NotNull()); @@ -2030,6 +2038,8 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { EXPECT_NEAR(copy_model->body_quat[1], 0, 1e-12); EXPECT_NEAR(copy_model->body_quat[2], 0, 1e-12); EXPECT_NEAR(copy_model->body_quat[3], 0, 1e-12); + EXPECT_NEAR(copy_model->jnt_range[0], -mjPI / 4, 1e-7); + EXPECT_NEAR(copy_model->jnt_range[1], mjPI / 4, 1e-7); mj_deleteModel(copy_model); mj_deleteSpec(copy); From bbedc7f3393ff350bda1de9e8bbcab507b185aec Mon Sep 17 00:00:00 2001 From: Gabe Oppenheimer Date: Mon, 2 Dec 2024 10:55:31 -0800 Subject: [PATCH 113/426] Update the chancelog for the v3.2.6 release PiperOrigin-RevId: 702021093 Change-Id: I1c748581ad63ae934b043f56b929d7a5a362bb23 --- doc/changelog.rst | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 28819ba9..0c50e7e9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -3,32 +3,32 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.2.6 (Dec 2, 2024) +--------------------------- General ^^^^^^^ -- Removed rope and loop from :ref:`composite`. The user is encouraged to instead use the :at:`cable` - plugin or :ref:`flexcomp`, respectively. +1. Removed rope and loop from :ref:`composite`. The user is encouraged to instead use the :at:`cable` + plugin or :ref:`flexcomp`, respectively. MJX ^^^ -- Added muscle actuators. +2. Added muscle actuators. Python bindings ^^^^^^^^^^^^^^^ -- Provide prebuilt wheels for Python 3.13. -- Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and - detachment. Python users are encouraged to use names for unique identification of model elements. -- Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. -- :ref:`rollout` can now accept sequences of MjModel of length ``nroll``. ``nroll`` argument deprecated because - its value can always be inferred. +3. Provide prebuilt wheels for Python 3.13. +4. Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and + detachment. Python users are encouraged to use names for unique identification of model elements. +5. Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. +6. :ref:`rollout` can now accept sequences of MjModel of length ``nroll``. ``nroll`` argument deprecated because + its value can always be inferred. Bug fixes ^^^^^^^^^ -- Fixed :github:issue:`2212`, type error in ``mjx.get_data``. -- Fixed bug introduced in 3.2.0 in handling of :ref:`texrepeat` attribute, was mistakenly cast - from ``float`` to ``int``, (fixed :github:issue:`2223`). +7. Fixed :github:issue:`2212`, type error in ``mjx.get_data``. +8. Fixed bug introduced in 3.2.0 in handling of :ref:`texrepeat` attribute, was mistakenly cast + from ``float`` to ``int``, (fixed :github:issue:`2223`). Version 3.2.5 (Nov 4, 2024) --------------------------- From 0f64959e279e63b62a7610c23e0396c000f28c06 Mon Sep 17 00:00:00 2001 From: Gabe Oppenheimer Date: Mon, 2 Dec 2024 11:11:44 -0800 Subject: [PATCH 114/426] Update the version of eigen3 ahead of the 3.2.6 release PiperOrigin-RevId: 702026493 Change-Id: Ibb0447b7106e9b873386c153283c81685583350f --- cmake/MujocoDependencies.cmake | 2 +- python/mujoco/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index bd7bdfe8..77a52dec 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -39,7 +39,7 @@ set(MUJOCO_DEP_VERSION_qhull CACHE STRING "Version of `qhull` to be fetched." ) set(MUJOCO_DEP_VERSION_Eigen3 - b396a6fbb2e173f52edb3360485dedf3389ef830 + d34b100c137ac931379ae5e1b888f16a9c8d6c72 CACHE STRING "Version of `Eigen3` to be fetched." ) diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 546cdf63..fe09488f 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -173,7 +173,7 @@ findorfetch( GIT_REPO https://gitlab.com/libeigen/eigen GIT_TAG - b396a6fbb2e173f52edb3360485dedf3389ef830 + d34b100c137ac931379ae5e1b888f16a9c8d6c72 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL From c90a63a926e5247895f1ba356599277250d6b3ee Mon Sep 17 00:00:00 2001 From: Gabe Oppenheimer Date: Mon, 2 Dec 2024 15:28:33 -0800 Subject: [PATCH 115/426] Update the version number to 3.2.7 following the 3.2.6 release. PiperOrigin-RevId: 702103729 Change-Id: Id5581cc823a17360e7bb98ac0aa4342020c1aefa --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4040915e..aaac4e38 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.2.6 + VERSION 3.2.7 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index dc746b79..bc490273 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,2,6,0 -PRODUCTVERSION 3,2,6,0 +FILEVERSION 3,2,7,0 +PRODUCTVERSION 3,2,7,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.6" + VALUE "ProductVersion", "3.2.7" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.6" + VALUE "FileVersion", "3.2.7" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 3da13b43..a0eb1272 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,2,6,0 -PRODUCTVERSION 3,2,6,0 +FILEVERSION 3,2,7,0 +PRODUCTVERSION 3,2,7,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.6" + VALUE "ProductVersion", "3.2.7" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.6" + VALUE "FileVersion", "3.2.7" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 408aee24..5e44d1ea 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -517,7 +517,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - Maximum number of UI rectangles. Defined in `mjui.h `_. * - ``mjVERSION_HEADER`` - - 326 + - 327 - The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a number with the same meaning but for the compiled library. diff --git a/doc/unity.rst b/doc/unity.rst index cb2c2517..41512767 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -30,14 +30,14 @@ _____ The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as a trusted binary. Then, copy the dynamic library file from -``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.6.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.7.dylib`` (it can be found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``. Linux _____ Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from -``~/.mujoco/mujoco-3.2.6/lib/libmujoco.so.3.2.6`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.2.7/lib/libmujoco.so.3.2.7`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 2e2cab59..a6df23ad 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -16,7 +16,7 @@ #define MUJOCO_MUJOCO_H_ // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 326 +#define mjVERSION_HEADER 327 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index df714f31..a5370380 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.2.6" +version = "3.2.7" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.2.6.dev0", + "mujoco>=3.2.7.dev0", "scipy", "trimesh", ] @@ -41,6 +41,6 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.2.6" +Documentation = "https://mujoco.readthedocs.io/en/3.2.7" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.2.6/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html" diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index fe09488f..aa97b9e6 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -84,7 +84,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.6.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.7.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -92,7 +92,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.2.6 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.2.7 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 98e2686e..204eede8 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.2.6 + 3.2.7 CFBundleGetInfoString - 3.2.6 + 3.2.7 CFBundleLongVersionString - 3.2.6 + 3.2.7 CFBundleShortVersionString - 3.2.6 + 3.2.7 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 49827899..94067175 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.2.6" +version = "3.2.7" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.2.6" +Documentation = "https://mujoco.readthedocs.io/en/3.2.7" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.2.6/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 70120f7f..3960206a 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.2.6 + VERSION 3.2.7 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 99af5676..ab89442b 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.2.6 + VERSION 3.2.7 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 630431fd..ff8c1d4c 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -42,8 +42,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 326 -#define mjVERSIONSTRING "3.2.6" + #define mjVERSION 327 +#define mjVERSIONSTRING "3.2.7" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index aba4932d..b6ed835c 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.2.6.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.2.7.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.2.6/lib/libmujoco.so.3.2.6", + "/.mujoco/mujoco-3.2.7/lib/libmujoco.so.3.2.7", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4c1f055c..de038bef 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -109,7 +109,7 @@ public const int mjMAXLINEPNT = 1000; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 326; +public const int mjVERSION_HEADER = 327; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index ee54ac8c..71d4e34c 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.2.6", + "version": "3.2.7", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From a793a3417aaf46b505d0c6feee83fdcfa6b18529 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 2 Dec 2024 16:27:29 -0800 Subject: [PATCH 116/426] Raise error in `mjx.rne_postconstraint` for currently unsupported connect and weld constraints and raise error in `mjx.put_model` for unsupported sensor and equality constraint combinations. PiperOrigin-RevId: 702119449 Change-Id: I034363db41629f19ef98bea79ec89140052a7a3c --- doc/mjx.rst | 9 +++++++-- mjx/mujoco/mjx/_src/io.py | 15 +++++++++++++++ mjx/mujoco/mjx/_src/io_test.py | 32 ++++++++++++++++++++++++++++++++ mjx/mujoco/mjx/_src/smooth.py | 5 +++++ 4 files changed, 59 insertions(+), 2 deletions(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index 87dfd9e3..d13bae44 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -244,8 +244,10 @@ The following features are **fully supported** in MJX: - ``MAGNETOMETER``, ``CAMPROJECTION``, ``RANGEFINDER``, ``JOINTPOS``, ``TENDONPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``FRAMEQUAT``, ``SUBTREECOM``, ``CLOCK``, ``VELOCIMETER``, ``GYRO``, ``JOINTVEL``, ``TENDONVEL``, ``ACTUATORVEL``, ``BALLANGVEL``, ``FRAMELINVEL``, - ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACCELEROMETER``, ``FORCE``, ``TORQUE``, - ``ACTUATORFRC``, ``JOINTACTFRC``, ``FRAMELINACC``, ``FRAMEANGACC``. + ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACTUATORFRC``, ``JOINTACTFRC``, + ``FRAMELINACC``, ``FRAMEANGACC`` + - ``ACCELEROMETER``, ``FORCE``, and ``TORQUE`` are supported if the model does not include connect or weld equality + constraints. The following features are **in development** and coming soon: @@ -274,6 +276,9 @@ The following features are **in development** and coming soon: - All except ``PLUGIN``, ``USER`` * - Lights - Positions and directions of lights + * - :ref:`Sensors ` + - ``ACCELEROMETER``, ``FORCE``, and ``TORQUE`` for models that include connect or weld equality + constraints. The following features are **unsupported**: diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 860c8a44..aa76714f 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -142,6 +142,21 @@ def put_model( ' implemented for spatial tendons.' ) + # check for unsupported sensor and equality constraint combinations + sensor_rne_postconstraint = ( + np.any(m.sensor_type == types.SensorType.ACCELEROMETER) + | np.any(m.sensor_type == types.SensorType.FORCE) + | np.any(m.sensor_type == types.SensorType.TORQUE) + ) + eq_connect_weld = np.any(m.eq_type == types.EqType.CONNECT) | np.any( + m.eq_type == types.EqType.WELD + ) + if sensor_rne_postconstraint and eq_connect_weld: + raise NotImplementedError( + 'rne_postconstraint not implemented with equality constraints:' + ' connect, weld.' + ) + for enum_field, enum_type, mj_type in ( (m.actuator_biastype, types.BiasType, mujoco.mjtBias), (m.actuator_dyntype, types.DynType, mujoco.mjtDyn), diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 289c3890..467b9020 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -513,6 +513,38 @@ class DataIOTest(parameterized.TestCase): with self.assertRaises(NotImplementedError): mjx.make_data(m) + @parameterized.product( + sensor=['accelerometer', 'force', 'torque'], equality=['connect', 'weld'] + ) + def test_sensor_constraint_compatibility(self, sensor, equality): + """Test unsupported sensor and equality constraint combinations.""" + equality_constraint = f'{equality} body1="body1" body2="body2"' + if equality == 'connect': + equality_constraint += ' anchor="0 0 0"' + m = mujoco.MjModel.from_xml_string(f""" + + + + + + + + + + + + + + <{equality_constraint}/> + + + <{sensor} site="site1"/> + + + """) + with self.assertRaises(NotImplementedError): + mjx.put_model(m) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index f2e3b721..dd04ee0d 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -24,6 +24,7 @@ from mujoco.mjx._src import support from mujoco.mjx._src.types import CamLightType from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import DisableBit +from mujoco.mjx._src.types import EqType from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import TrnType @@ -635,6 +636,10 @@ def rne_postconstraint(m: Model, d: Data) -> Data: ) # TODO(taylorhowell): connect and weld constraints + if np.any(m.eq_type == EqType.CONNECT): + raise NotImplementedError('Connect constraints are not implemented.') + if np.any(m.eq_type == EqType.WELD): + raise NotImplementedError('Weld constraints are not implemented.') # forward pass over bodies: compute cacc, cfrc_int def _forward(carry, cfrc_ext, cinert, cvel, body_dofadr, body_dofnum): From 13d3bdedad088b58c71d8f6db6b687e15168b96b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 3 Dec 2024 05:19:02 -0800 Subject: [PATCH 117/426] Add debugging method for printing matrices to stderr to fixture.h PiperOrigin-RevId: 702301299 Change-Id: I01738b6634a91cde6b146475d06731471dcf3764 --- test/engine/engine_derivative_test.cc | 15 --------------- test/fixture.h | 14 ++++++++++++++ 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 51dc26b7..1db7731a 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -14,8 +14,6 @@ // Tests for engine/engine_derivative.c. -#include -#include #include #include #include @@ -74,19 +72,6 @@ static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, return max_error; } -// utility function for matrix printing (debug) -// NOLINTNEXTLINE(clang-diagnostic-unused-function) -static void PrintMatrix(mjtNum* mat, int nrow, int ncol) { - std::cerr.precision(5); - std::cerr << "\n"; - for (int r=0; r < nrow; r++) { - for (int c=0; c < ncol; c++) { - std::cerr << std::fixed << std::setw(9) << mat[c + r*ncol] << " "; - } - std::cerr << "\n"; - } -} - static const char* const kEnergyConservingPendulumPath = "engine/testdata/derivative/energy_conserving_pendulum.xml"; static const char* const kTumblingThinObjectPath = diff --git a/test/fixture.h b/test/fixture.h index 8da8ef5c..4d65623d 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include #include @@ -110,6 +112,18 @@ inline std::vector AsVector(const mjtNum* array, int n) { return std::vector(array, array + n); } +// Prints a matrix to stderr, useful for debugging. +inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) { + std::cerr.precision(p); + std::cerr << "\n"; + for (int r = 0; r < nrow; r++) { + for (int c = 0; c < ncol; c++) { + std::cerr << std::fixed << std::setw(4 + p) << mat[c + r*ncol] << " "; + } + std::cerr << "\n"; + } +} + // Installs a mock filesystem via a resource provider. To obtain thread safety, // each filesystem is scoped for individual unit tests with destructive // operations not permitted. From ec9986dcb750dbe98f0cc8808ee5931eb9288171 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 3 Dec 2024 09:38:18 -0800 Subject: [PATCH 118/426] Clean up changelog. PiperOrigin-RevId: 702369712 Change-Id: I44293f18de54ddc65af31f64c40dfbe608278b6d --- doc/changelog.rst | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 0c50e7e9..91ee4660 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -18,17 +18,17 @@ MJX Python bindings ^^^^^^^^^^^^^^^ 3. Provide prebuilt wheels for Python 3.13. -4. Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of repeated attachment and - detachment. Python users are encouraged to use names for unique identification of model elements. -5. Removed ``nroll`` argument from :ref:`rollout` because its value can always be inferred. -6. :ref:`rollout` can now accept sequences of MjModel of length ``nroll``. ``nroll`` argument deprecated because - its value can always be inferred. +4. Added ``bind`` method and removed id attribute from :ref:`mjSpec` objects. Using ids is error prone in scenarios of + repeated attachment and detachment. Python users are encouraged to use names for unique identification of model + elements. +5. :ref:`rollout` can now accept sequences of MjModel of length ``nroll``. Also removed the ``nroll`` + argument because its value can always be inferred. Bug fixes ^^^^^^^^^ -7. Fixed :github:issue:`2212`, type error in ``mjx.get_data``. -8. Fixed bug introduced in 3.2.0 in handling of :ref:`texrepeat` attribute, was mistakenly cast - from ``float`` to ``int``, (fixed :github:issue:`2223`). +6. Fixed :github:issue:`2212`, type error in ``mjx.get_data``. +7. Fixed bug introduced in 3.2.0 in handling of :ref:`texrepeat` attribute, was mistakenly + cast from ``float`` to ``int``, (fixed :github:issue:`2223`). Version 3.2.5 (Nov 4, 2024) --------------------------- From 0e7d2ef6df8eb5cab260f0dbef4e989743b7f5be Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 4 Dec 2024 01:18:26 -0800 Subject: [PATCH 119/426] Fix bug in box-sphere collider. Fixes #2206 PiperOrigin-RevId: 702627603 Change-Id: Ie6015b7fa6f8507459325d692581b0f5f83aa159 --- doc/changelog.rst | 6 +++++ src/engine/engine_collision_box.c | 14 ++++++------ test/engine/engine_collision_box_test.cc | 29 ++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 7 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 91ee4660..5afaf2f0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,12 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +Bug fixes +^^^^^^^^^ +- Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). Version 3.2.6 (Dec 2, 2024) --------------------------- diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index bb42ce89..1860ad4d 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -38,10 +38,9 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) // raw sphere : box int mjraw_SphereBox(mjContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) -{ + const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { int i, k; - mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3]; + mjtNum tmp[3], center[3], clamped[3], deepest[3]; mjtNum pos[3]; mjtNum dist, closest; @@ -62,19 +61,20 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, if (dist <= mjMINVAL) { closest = (size2[0] + size2[1] + size2[2]) * 2; - for (i = 0; i < 6; i++) - if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) - { + for (i = 0; i < 6; i++) { + if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) { closest = mju_abs((i % 2 ? 1 : -1) * size2[i / 2] - center[i / 2]); k = i; } + } - mju_zero3(nearest); + mjtNum nearest[3] = {0}; nearest[k / 2] = (k % 2 ? -1 : 1); mju_copy3(pos, center); mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); mju_mulMatVec3(con[0].frame, mat2, nearest); + dist = -closest; } else { mju_addToScl3(deepest, tmp, size1[0]); mju_zero3(pos); diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index 5477189d..4f224cf1 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -30,6 +30,7 @@ namespace { using MjCollisionBoxTest = MujocoTest; using ::testing::NotNull; +using ::testing::DoubleNear; static const char* const kBad0FilePath = "engine/testdata/collision_box/boxbox_bad0.xml"; @@ -243,6 +244,34 @@ TEST_F(MjCollisionBoxTest, DeepPenetration) { mj_deleteModel(model); } +TEST_F(MjCollisionBoxTest, BoxSphere) { + constexpr char xml[] = R"( + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) { + data->qpos[2] = z; + mj_forward(model, data); + EXPECT_EQ(data->ncon, 2); + EXPECT_THAT(data->contact[0].dist, DoubleNear(data->contact[1].dist, 1e-8)); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From 0ba717a6530070de9750e2482e4dbb99732d13a4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 4 Dec 2024 04:40:15 -0800 Subject: [PATCH 120/426] Print supernode values in `mj_printData` PiperOrigin-RevId: 702677257 Change-Id: Icecc5dd8604fcadc02bba5fc69c9fd5fd4fbd825 --- src/engine/engine_print.c | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 68e48571..24bd02bb 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -103,7 +103,7 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, fprintf(fp, " "); for (int adr=rowadr[r]; adr < rowadr[r]+rownnz[r]; adr++) { fprintf(fp, " "); - fprintf(fp, "%d: ", colind[adr]); + fprintf(fp, "%2d: ", colind[adr]); fprintf(fp, float_format, mat[adr]); } fprintf(fp, "\n"); @@ -114,8 +114,8 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, // print sparse matrix structure -static void printSparsity(const char* str, int nr, int nc, - const int* rowadr, const int* rownnz, const int* colind, FILE* fp) { +static void printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* rownnz, + const int* rowsuper, const int* colind, FILE* fp) { // if no rows / columns, or too many columns to be visually useful, return if (!nr || !nc || nc > 300) { return; @@ -136,7 +136,9 @@ static void printSparsity(const char* str, int nr, int nc, fprintf(fp, " "); } } - fprintf(fp, " |\n"); + fprintf(fp, " |"); + if (rowsuper && rowsuper[r] > 0) fprintf(fp, " %d", rowsuper[r]); + fprintf(fp, "\n"); if (r < nr-1) fprintf(fp, " "); } for (int c=0; c < nc+2; c++) fprintf(fp, "-"); @@ -1055,7 +1057,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format); } else { printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv, - d->flexedge_J_rowadr, d->flexedge_J_rownnz, d->flexedge_J_colind, fp); + d->flexedge_J_rowadr, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind, fp); printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp); printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp); printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz, @@ -1067,8 +1069,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); } else { - printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, - d->ten_J_rowadr, d->ten_J_rownnz, d->ten_J_colind, fp); + printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, d->ten_J_rownnz, + NULL, d->ten_J_colind, fp); printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, @@ -1085,7 +1087,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); printSparsity("actuator_moment", m->nu, m->nv, - d->moment_rowadr, d->moment_rownnz, d->moment_colind, fp); + d->moment_rowadr, d->moment_rownnz, NULL, d->moment_colind, fp); printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); @@ -1104,7 +1106,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format); // B sparse structure - printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, d->B_rownnz, d->B_colind, fp); + printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, d->B_rownnz, NULL, + d->B_colind, fp); // B_rownnz fprintf(fp, NAME_FORMAT, "B_rownnz"); @@ -1128,7 +1131,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // C sparse structure - printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_rownnz, d->C_colind, fp); + printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_rownnz, NULL, + d->C_colind, fp); fprintf(fp, NAME_FORMAT, "C_rownnz"); for (int i = 0; i < m->nv; i++) { @@ -1158,7 +1162,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // D sparse structure - printSparsity("D: dof-dof matrix", m->nv, m->nv, d->D_rowadr, d->D_rownnz, d->D_colind, fp); + printSparsity("D: dof-dof matrix", m->nv, m->nv, d->D_rowadr, d->D_rownnz, NULL, d->D_colind, fp); // D_rownnz fprintf(fp, NAME_FORMAT, "D_rownnz"); @@ -1255,13 +1259,14 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); printArray("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); } else { - printSparsity("J: constraint Jacobian", d->nefc, m->nv, - d->efc_J_rowadr, d->efc_J_rownnz, d->efc_J_colind, fp); + printSparsity("J: constraint Jacobian", d->nefc, m->nv, d->efc_J_rowadr, d->efc_J_rownnz, + d->efc_J_rowsuper, d->efc_J_colind, fp); printArrayInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp); printArrayInt("EFC_J_ROWADR", d->nefc, 1, d->efc_J_rowadr, fp); printSparse("EFC_J", d->efc_J, d->nefc, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, fp, float_format); - + printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, + d->efc_JT_rownnz, d->efc_JT_rowsuper, d->efc_JT_colind, fp); printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz, From afc86ac1b33cc593b148f0265f0346b81c4a2dc2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 4 Dec 2024 06:29:41 -0800 Subject: [PATCH 121/426] Associate children of deleted body to the newly created frame in mjs_bodyToFrame. Also associate all children that need a parent body to the parent of the deleted body. This is a bug that was causing the former children of the body that gets transformed to a frame to not be children of the new frame. PiperOrigin-RevId: 702702853 Change-Id: I36d48f0446bc87c665b18d34e0b0609d82c70f51 --- src/user/user_objects.cc | 39 +++++++++++++++++++------------------- src/user/user_objects.h | 12 +++++++++++- test/user/user_api_test.cc | 6 ++++-- 3 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index fd3064f3..79e06b97 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -134,6 +134,18 @@ PNGImage PNGImage::Load(const mjCBase* obj, mjResource* resource, return image; } +// associate all child list elements with a frame and copy them to parent list, clear child list +template +void MapFrame(std::vector& parent, std::vector& child, + mjCFrame* frame, mjCBody* parent_body) { + std::for_each(child.begin(), child.end(), [frame, parent_body](T* element) { + element->SetFrame(frame); + element->SetParent(parent_body); + }); + parent.insert(parent.end(), child.begin(), child.end()); + child.clear(); +} + } // namespace @@ -1195,20 +1207,13 @@ mjCFrame* mjCBody::ToFrame() { mjCFrame* newframe = parent->AddFrame(frame); mjuu_copyvec(newframe->spec.pos, spec.pos, 3); mjuu_copyvec(newframe->spec.quat, spec.quat, 4); - parent->bodies.insert(parent->bodies.end(), bodies.begin(), bodies.end()); - parent->geoms.insert(parent->geoms.end(), geoms.begin(), geoms.end()); - parent->joints.insert(parent->joints.end(), joints.begin(), joints.end()); - parent->sites.insert(parent->sites.end(), sites.begin(), sites.end()); - parent->cameras.insert(parent->cameras.end(), cameras.begin(), cameras.end()); - parent->lights.insert(parent->lights.end(), lights.begin(), lights.end()); - parent->frames.insert(parent->frames.end(), frames.begin(), frames.end()); - bodies.clear(); - geoms.clear(); - joints.clear(); - sites.clear(); - cameras.clear(); - lights.clear(); - frames.clear(); + MapFrame(parent->bodies, bodies, newframe, parent); + MapFrame(parent->geoms, geoms, newframe, parent); + MapFrame(parent->joints, joints, newframe, parent); + MapFrame(parent->sites, sites, newframe, parent); + MapFrame(parent->cameras, cameras, newframe, parent); + MapFrame(parent->lights, lights, newframe, parent); + MapFrame(parent->frames, frames, newframe, parent); parent->bodies.erase( std::remove_if(parent->bodies.begin(), parent->bodies.end(), [this](mjCBody* body) { return body == this; }), @@ -1892,12 +1897,6 @@ bool mjCFrame::IsAncestor(const mjCFrame* child) const { -void mjCFrame::SetParent(mjCBody* _body) { - body = _body; -} - - - void mjCFrame::PointToLocal() { spec.element = static_cast(this); spec.name = &name; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d5dd09b9..6c6ed943 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -344,6 +344,9 @@ class mjCBody : public mjCBody_, private mjsBody { mjsFrame* last_attached; // last attached frame to this body + // set parent of this body + void SetParent(const mjCBody* _body) { parentid = _body->id; } + private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor mjCBody& operator=(const mjCBody& other); // copy assignment @@ -406,7 +409,7 @@ class mjCFrame : public mjCFrame_, private mjsFrame { void CopyFromSpec(void); void PointToLocal(void); - void SetParent(mjCBody* _body); + void SetParent(mjCBody* _body) { body = _body; } mjCFrame& operator+=(const mjCBody& other); @@ -457,6 +460,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint { using mjCBase::info; void CopyFromSpec(void); + void SetParent(mjCBody* _body) { body = _body; } // used by mjXWriter and mjCModel const std::vector& get_userdata() const { return userdata_; } @@ -537,6 +541,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom { void SetInertia(void); // compute and set geom inertia bool IsVisual(void) const { return visual_; } void SetNotVisual(void) { visual_ = false; } + void SetParent(mjCBody* _body) { body = _body; } mjtGeom Type() const { return type; } // Compute all coefs modeling the interaction with the surrounding fluid. @@ -601,6 +606,7 @@ class mjCSite : public mjCSite_, private mjsSite { // site's body mjCBody* Body() const { return body; } + void SetParent(mjCBody* _body) { body = _body; } // use strings from mjCBase rather than mjStrings from mjsSite using mjCBase::name; @@ -653,6 +659,8 @@ class mjCCamera : public mjCCamera_, private mjsCamera { const std::string& get_targetbody() const { return targetbody_; } const std::vector& get_userdata() const { return userdata_; } + void SetParent(mjCBody* _body) { body = _body; } + private: void Compile(void); // compiler void CopyFromSpec(void); @@ -691,6 +699,8 @@ class mjCLight : public mjCLight_, private mjsLight { // used by mjXWriter and mjCModel const std::string& get_targetbody() const { return targetbody_; } + void SetParent(mjCBody* _body) { body = _body; } + private: void Compile(void); // compiler void CopyFromSpec(void); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 5da06dc1..1c3ad2c2 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1316,7 +1316,7 @@ TEST_F(MujocoTest, AttachWorld) { static constexpr char xml_parent[] = R"( - + )"; @@ -1327,18 +1327,20 @@ TEST_F(MujocoTest, AttachWorld) { + )"; static constexpr char xml_result[] = R"( - + + From d1b02556ef141f46ad45a0ab11cc8742b80d0e65 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 4 Dec 2024 07:30:33 -0800 Subject: [PATCH 122/426] Allow keyword arguments in attach. PiperOrigin-RevId: 702718597 Change-Id: I1f1a5e9c88c8e102d5cbe5984511e7923ec7be3e --- python/mujoco/specs.cc | 47 ++++++++++++++++++++++++------------- python/mujoco/specs_test.py | 6 ++--- 2 files changed, 34 insertions(+), 19 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index f97ed6fd..9589229d 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -625,15 +625,19 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjsBody.def( "attach_frame", - [](raw::MjsBody& self, raw::MjsFrame& frame, std::string& prefix, - std::string& suffix) -> raw::MjsFrame* { - auto new_frame = - mjs_attachFrame(&self, &frame, prefix.c_str(), suffix.c_str()); + [](raw::MjsBody& self, raw::MjsFrame& frame, + std::optional& prefix, + std::optional& suffix) -> raw::MjsFrame* { + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + auto new_frame = mjs_attachFrame(&self, &frame, p, s); if (!new_frame) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); } return new_frame; }, + py::arg("frame"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); mjsBody.def( "to_frame", @@ -654,34 +658,41 @@ PYBIND11_MODULE(_specs, m) { }); mjsFrame.def( "attach_body", - [](raw::MjsFrame& self, raw::MjsBody& body, std::string& prefix, - std::string& suffix) -> raw::MjsBody* { - auto new_body = - mjs_attachBody(&self, &body, prefix.c_str(), suffix.c_str()); + [](raw::MjsFrame& self, raw::MjsBody& body, + std::optional& prefix, + std::optional& suffix) -> raw::MjsBody* { + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + auto new_body = mjs_attachBody(&self, &body, p, s); if (!new_body) { throw pybind11::value_error( mjs_getError(mjs_getSpec(self.element))); } return new_body; }, + py::arg("body"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); mjsFrame.def( "attach", - [](raw::MjsFrame& self, MjSpec& spec, std::string& prefix, - std::string& suffix) -> raw::MjsFrame* { + [](raw::MjsFrame& self, MjSpec& spec, std::optional& prefix, + std::optional& suffix) -> raw::MjsFrame* { auto world = mjs_findBody(spec.ptr, "world"); if (!world) { throw pybind11::value_error( mjs_getError(mjs_getSpec(self.element))); } - auto attached_world = - mjs_attachBody(&self, world, prefix.c_str(), suffix.c_str()); + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + auto attached_world = mjs_attachBody(&self, world, p, s); if (!attached_world) { throw pybind11::value_error( mjs_getError(mjs_getSpec(self.element))); } return mjs_bodyToFrame(&attached_world); }, + py::arg("spec"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== @@ -730,16 +741,20 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjsSite.def( "attach", - [](raw::MjsSite& self, raw::MjsBody& body, std::string& prefix, - std::string& suffix) -> raw::MjsBody* { - auto new_body = - mjs_attachToSite(&self, &body, prefix.c_str(), suffix.c_str()); + [](raw::MjsSite& self, raw::MjsBody& body, + std::optional& prefix, + std::optional& suffix) -> raw::MjsBody* { + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + auto new_body = mjs_attachToSite(&self, &body, p, s); if (!new_body) { throw pybind11::value_error( mjs_getError(mjs_getSpec(self.element))); } return new_body; }, + py::arg("body"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 7d42d3d0..afbcc576 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -867,7 +867,7 @@ class SpecsTest(absltest.TestCase): parent.compiler.degree = not child.compiler.degree body = child.worldbody.add_body(euler=[90, 0, 0]) frame = parent.worldbody.add_frame(euler=[-mujoco.mjPI / 2, 0, 0]) - frame.attach_body(body, 'child-', '') + frame.attach_body(body, prefix='child-') model = parent.compile() np.testing.assert_almost_equal(model.body_quat[1], [1, 0, 0, 0]) @@ -876,7 +876,7 @@ class SpecsTest(absltest.TestCase): parent = mujoco.MjSpec() site = parent.worldbody.add_site(pos=[1, 2, 3]) body = child.worldbody.add_body() - self.assertIsNotNone(site.attach(body, '_', '')) + self.assertIsNotNone(site.attach(body, prefix='_')) model = parent.compile() np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3]) @@ -892,7 +892,7 @@ class SpecsTest(absltest.TestCase): child.worldbody.add_camera(name='camera') parent = mujoco.MjSpec() frame = parent.worldbody.add_frame(name='frame') - frame.attach(child, 'child-', '') + frame.attach(child, prefix='child-') self.assertLen(child.cameras, 1) self.assertLen(parent.bodies, 1) self.assertLen(parent.frames, 2) From a3b9f986c04c5e353e7546b4e0b0b1ecd48e7395 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 4 Dec 2024 08:20:03 -0800 Subject: [PATCH 123/426] Add spec.find_site(). PiperOrigin-RevId: 702733068 Change-Id: I4cc7fd2b9b59185a4458fdac8537f8287af1f295 --- python/mujoco/specs.cc | 6 ++++++ python/mujoco/specs_test.py | 1 + 2 files changed, 7 insertions(+) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 9589229d..047002f9 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -344,6 +344,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_findFrame(self.ptr, name.c_str()); }, py::return_value_policy::reference_internal); + mjSpec.def( + "find_site", + [](MjSpec& self, std::string& name) -> raw::MjsSite* { + return mjs_asSite(mjs_findElement(self.ptr, mjOBJ_SITE, name.c_str())); + }, + py::return_value_policy::reference_internal); mjSpec.def( "find_default", [](MjSpec& self, std::string& classname) -> const raw::MjsDefault* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index afbcc576..a7133e77 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -723,6 +723,7 @@ class SpecsTest(absltest.TestCase): self.assertIsNotNone(head) site = head.first_site() self.assertIsNotNone(site) + self.assertEqual(site, spec.find_site('head')) site.delete() spec.sensors[-1].delete() From 977fb913cfaf538079934b7ebb999dc9c8acd1e2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 4 Dec 2024 10:36:47 -0800 Subject: [PATCH 124/426] Remove hanging vertex from flexcomp circle type. PiperOrigin-RevId: 702777882 Change-Id: I3ce09c0ed81a9b153c606debc8c22fbb47c93e8b --- src/user/user_flexcomp.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 58a50bcb..8ff486ef 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -550,6 +550,10 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { if (dim == 1) { for (int ix=0; ix < count[0]; ix++) { if (type == mjFCOMPTYPE_CIRCLE) { + if (ix >= count[0]-1) { + continue; + } + // add point double theta = 2*mjPI/(count[0]-1); double radius = spacing[0]/std::sin(theta/2)/2; @@ -558,10 +562,8 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { point.push_back(0); // add element - if (ix < count[0]-1) { - element.push_back(ix); - element.push_back(ix == count[0]-2 ? 0 : ix+1); - } + element.push_back(ix); + element.push_back(ix == count[0]-2 ? 0 : ix+1); } else { // add point point.push_back(spacing[0]*(ix - 0.5*(count[0]-1))); From 10239a673b34536eae625eb8acc8a02267c1e756 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 5 Dec 2024 02:31:54 -0800 Subject: [PATCH 125/426] Change from parent ID to parent pointer in mjCBody. This is required to have parent body info during attach and during body to frame tranformation for uncompiled models. PiperOrigin-RevId: 703038426 Change-Id: Ib482059ba419a020860c649d6ea1b1088045f2e1 --- src/user/user_model.cc | 28 ++++++++++++++------------- src/user/user_objects.cc | 20 ++++++++++--------- src/user/user_objects.h | 5 +++-- test/user/user_api_test.cc | 39 ++++++++++++++++++++++++++++---------- 4 files changed, 58 insertions(+), 34 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 556b623b..3e6134bd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -153,7 +153,7 @@ mjCModel::mjCModel() { world->mass = 0; mjuu_zerovec(world->inertia, 3); world->id = 0; - world->parentid = 0; + world->parent = nullptr; world->weldid = 0; world->name = "world"; world->classname = "main"; @@ -2067,10 +2067,10 @@ void mjCModel::CopyTree(mjModel* m) { for (int i=0; iparentid]; + mjCBody* par = pb->parent; // set body fields - m->body_parentid[i] = pb->parentid; + m->body_parentid[i] = pb->parent ? pb->parent->id : 0; m->body_weldid[i] = pb->weldid; m->body_mocapid[i] = pb->mocapid; m->body_jntnum[i] = (int)pb->joints.size(); @@ -2115,19 +2115,19 @@ void mjCModel::CopyTree(mjModel* m) { if (cntfree>1 || (cntfree==1 && pb->joints.size()>1)) { throw mjCError(pb, "free joint can only appear by itself"); } - if (cntfree && pb->parentid) { + if (cntfree && par && par->name != "world") { throw mjCError(pb, "free joint can only be used on top level"); } // rootid: self if world or child of world, otherwise parent's rootid - if (i==0 || pb->parentid==0) { + if (i==0 || (par && par->name == "world")) { m->body_rootid[i] = i; } else { - m->body_rootid[i] = m->body_rootid[pb->parentid]; + m->body_rootid[i] = m->body_rootid[par->id]; } // init lastdof from parent - pb->lastdof = par->lastdof; + pb->lastdof = par ? par->lastdof : -1; // set sameframe mjtSameFrame sameframe; @@ -2447,7 +2447,9 @@ void mjCModel::CopyTree(mjModel* m) { bodies_[i]->subtreedofs += bodies_[i]->dofnum; // add to parent count - bodies_[bodies_[i]->parentid]->subtreedofs += bodies_[i]->subtreedofs; + if (bodies_[i]->parent) { + bodies_[i]->parent->subtreedofs += bodies_[i]->subtreedofs; + } } // make sure all dofs are in world "subtree", SHOULD NOT OCCUR @@ -2462,10 +2464,10 @@ void mjCModel::CopyTree(mjModel* m) { nB += bodies_[i]->subtreedofs; // add dofs in ancestor bodies - int j = bodies_[i]->parentid; + int j = bodies_[i]->parent ? bodies_[i]->parent->id : 0; while (j > 0) { nB += bodies_[j]->dofnum; - j = bodies_[j]->parentid; + j = bodies_[j]->parent ? bodies_[j]->parent->id : 0; } } m->nB = nB; @@ -3413,7 +3415,7 @@ static void changeframe(double childpos[3], double childquat[4], void mjCModel::FuseReindex(mjCBody* body) { // set parentid and weldid of children for (int i=0; ibodies.size(); i++) { - body->bodies[i]->parentid = body->id; + body->bodies[i]->parent = body; body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ? body->bodies[i]->id : body->weldid); } @@ -3450,7 +3452,7 @@ void mjCModel::FuseStatic(void) { for (int i=1; iparentid]; + mjCBody* par = body->parent; // skip if body has joints or mocap if (!body->joints.empty() || body->mocap) { @@ -3459,7 +3461,7 @@ void mjCModel::FuseStatic(void) { //------------- add mass and inertia (if parent not world) - if (body->parentid>0 && body->mass>=mjMINVAL) { + if (body->parent && body->parent->name != "world" && body->mass>=mjMINVAL) { // body_ipose = body_pose * body_ipose changeframe(body->ipos, body->iquat, body->pos, body->quat); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 79e06b97..3e4c61bf 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -765,7 +765,7 @@ mjCBody::mjCBody(mjCModel* _model) { mjs_defaultBody(&spec); elemtype = mjOBJ_BODY; - parentid = -1; + parent = nullptr; weldid = -1; dofnum = 0; lastdof = -1; @@ -850,6 +850,7 @@ mjCBody& mjCBody::operator+=(const mjCBody& other) { for (int i=0; iparent = this; bodies.back()->frame = other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : nullptr; } @@ -918,6 +919,7 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { continue; } bodies.push_back(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call + bodies.back()->parent = this; bodies.back()->frame = subtree->bodies[i]->frame ? frames[fmap[subtree->bodies[i]->frame]] : nullptr; bodies.back()->NameSpace_(other.model, /*propagate=*/ false); @@ -1098,6 +1100,7 @@ mjCBody* mjCBody::AddBody(mjCDef* _def) { obj->classname = _def ? _def->name : classname; bodies.push_back(obj); + obj->parent = this; return obj; } @@ -1199,11 +1202,6 @@ mjCLight* mjCBody::AddLight(mjCDef* _def) { // create a frame in the parent body and move all contents of this body into it mjCFrame* mjCBody::ToFrame() { - if (parentid < 0) { - // TODO: store the parent pointer instead of using the id - throw mjCError(this, "parent body is not defined, please compile the model first"); - } - mjCBody* parent = model->Bodies()[parentid]; mjCFrame* newframe = parent->AddFrame(frame); mjuu_copyvec(newframe->spec.pos, spec.pos, 3); mjuu_copyvec(newframe->spec.quat, spec.quat, 4); @@ -1218,6 +1216,11 @@ mjCFrame* mjCBody::ToFrame() { std::remove_if(parent->bodies.begin(), parent->bodies.end(), [this](mjCBody* body) { return body == this; }), parent->bodies.end()); + if (model->IsCompiled()) { + mjCBody *world = model->bodies_[0]; + model->ResetTreeLists(); + model->MakeLists(world); + } return newframe; } @@ -1573,7 +1576,6 @@ void mjCBody::Compile(void) { // set parentid and weldid of children for (int i=0; iparentid = id; bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid); } @@ -1721,13 +1723,12 @@ void mjCBody::Compile(void) { } // make sure mocap body is fixed child of world - if (mocap && (dofnum || parentid)) { + if (mocap && (dofnum || (parent && parent->name != "world"))) { throw mjCError(this, "mocap body '%s' is not a fixed child of world", name.c_str()); } // compute body global pose (no joint transformations in qpos0) if (id>0) { - mjCBody* parent = model->Bodies()[parentid]; mjuu_rotVecQuat(xpos0, pos, parent->xquat0); mjuu_addtovec(xpos0, parent->xpos0, 3); mjuu_mulquat(xquat0, parent->xquat0, quat); @@ -1852,6 +1853,7 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { other.ForgetKeyframes(); other.model->prefix = subtree->prefix; other.model->suffix = subtree->suffix; + subtree->SetParent(body); subtree->SetFrame(this); subtree->NameSpace(other.model); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 6c6ed943..bf11a7be 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -236,8 +236,9 @@ class mjCBase : public mjCBase_ { class mjCBody_ : public mjCBase { protected: + mjCBody* parent; + // variables computed by 'Compile' and 'AddXXX' - int parentid; // parent index in global array int weldid; // top index of body we are welded to int dofnum; // number of motion dofs for body int mocapid; // mocap id, -1: not mocap @@ -345,7 +346,7 @@ class mjCBody : public mjCBody_, private mjsBody { mjsFrame* last_attached; // last attached frame to this body // set parent of this body - void SetParent(const mjCBody* _body) { parentid = _body->id; } + void SetParent(mjCBody* _body) { parent = _body; } private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 1c3ad2c2..67b7282d 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1316,7 +1316,9 @@ TEST_F(MujocoTest, AttachWorld) { static constexpr char xml_parent[] = R"( - + + + )"; @@ -1334,15 +1336,21 @@ TEST_F(MujocoTest, AttachWorld) { static constexpr char xml_result[] = R"( - - + + - + + + + + + + - + )"; @@ -1351,27 +1359,38 @@ TEST_F(MujocoTest, AttachWorld) { mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size()); EXPECT_THAT(child, NotNull()) << er.data(); + // attach a body to the frame mjsFrame* frame = mjs_findFrame(parent, "frame"); EXPECT_THAT(frame, NotNull()); + mjsBody* body = mjs_findBody(child, "sphere"); + EXPECT_THAT(body, NotNull()); + mjsBody* attached = mjs_attachBody(frame, body, "attached-", "-1"); + EXPECT_THAT(attached, NotNull()); + mjModel* model1 = mj_compile(parent, 0); + EXPECT_THAT(model1, NotNull()); + + // attach the world to the same frame and convert it to a frame mjsBody* world = mjs_findBody(child, "world"); EXPECT_THAT(world, NotNull()); - mjsBody* child_world = mjs_attachBody(frame, world, "attached-", "-1"); + mjsBody* child_world = mjs_attachBody(frame, world, "attached-", "-2"); EXPECT_THAT(child_world, NotNull()); mjsFrame* frame_world = mjs_bodyToFrame(&child_world); EXPECT_THAT(frame_world, NotNull()); EXPECT_THAT(child_world, IsNull()); - mjModel* model = mj_compile(parent, 0); - EXPECT_THAT(model, NotNull()); + // compile and compare + mjModel* model2 = mj_compile(parent, 0); + EXPECT_THAT(model2, NotNull()); mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) + EXPECT_LE(CompareModel(model2, expected, field), tol) << "Expected and attached models are different!\n" << "Different field: " << field << '\n'; mj_deleteSpec(parent); mj_deleteSpec(child); - mj_deleteModel(model); + mj_deleteModel(model1); + mj_deleteModel(model2); mj_deleteModel(expected); } From 503e5e1815e471ac393466761c1cd5445685e16f Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 5 Dec 2024 03:09:17 -0800 Subject: [PATCH 126/426] Change site.attach to accept a spec instead of a body. Add site.attach_body for attaching a body to a site. PiperOrigin-RevId: 703047770 Change-Id: Ibc1f916f8741872135220461cda7cebc4b418913 --- python/mujoco/specs.cc | 24 ++++++++++++++++- python/mujoco/specs_test.py | 52 +++++++++++++++++++++++++++---------- 2 files changed, 62 insertions(+), 14 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 047002f9..4fcd94b9 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -746,7 +746,7 @@ PYBIND11_MODULE(_specs, m) { }, py::return_value_policy::reference_internal); mjsSite.def( - "attach", + "attach_body", [](raw::MjsSite& self, raw::MjsBody& body, std::optional& prefix, std::optional& suffix) -> raw::MjsBody* { @@ -762,6 +762,28 @@ PYBIND11_MODULE(_specs, m) { py::arg("body"), py::arg("prefix") = py::none(), py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); + mjsSite.def( + "attach", + [](raw::MjsSite& self, MjSpec& spec, + std::optional& prefix, + std::optional& suffix) -> raw::MjsFrame* { + auto world = mjs_findBody(spec.ptr, "world"); + if (!world) { + throw pybind11::value_error( + mjs_getError(mjs_getSpec(self.element))); + } + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + auto attached_world = mjs_attachToSite(&self, world, p, s); + if (!attached_world) { + throw pybind11::value_error( + mjs_getError(mjs_getSpec(self.element))); + } + return mjs_bodyToFrame(&attached_world); + }, + py::arg("body"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), + py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== mjsCamera.def("delete", diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index a7133e77..4090a7cf 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -875,11 +875,26 @@ class SpecsTest(absltest.TestCase): def test_attach_body_to_site(self): child = mujoco.MjSpec() parent = mujoco.MjSpec() - site = parent.worldbody.add_site(pos=[1, 2, 3]) + site = parent.worldbody.add_site(pos=[1, 2, 3], quat=[0, 0, 0, 1]) body = child.worldbody.add_body() - self.assertIsNotNone(site.attach(body, prefix='_')) - model = parent.compile() - np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3]) + + # Attach body to site and compile. + self.assertIsNotNone(site.attach_body(body, prefix='_')) + model1 = parent.compile() + self.assertIsNotNone(model1) + self.assertEqual(model1.nbody, 2) + np.testing.assert_array_equal(model1.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) + + # Attach entire spec to site and compile again. + self.assertIsNotNone(site.attach(child, prefix='child-')) + model2 = parent.compile() + self.assertIsNotNone(model2) + self.assertEqual(model2.nbody, 3) + np.testing.assert_array_equal(model2.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_pos[2], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) + np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) def test_body_to_frame(self): spec = mujoco.MjSpec() @@ -890,16 +905,27 @@ class SpecsTest(absltest.TestCase): def test_attach_spec_to_frame(self): child = mujoco.MjSpec() - child.worldbody.add_camera(name='camera') parent = mujoco.MjSpec() - frame = parent.worldbody.add_frame(name='frame') - frame.attach(child, prefix='child-') - self.assertLen(child.cameras, 1) - self.assertLen(parent.bodies, 1) - self.assertLen(parent.frames, 2) - self.assertEqual(parent.cameras[0].name, 'child-camera') - self.assertEqual(parent.frames[0].name, 'frame') - self.assertEqual(parent.frames[1].name, '') + frame = parent.worldbody.add_frame(pos=[1, 2, 3], quat=[0, 0, 0, 1]) + body = child.worldbody.add_body() + + # Attach body to frame and compile. + self.assertIsNotNone(frame.attach_body(body, prefix='_')) + model1 = parent.compile() + self.assertIsNotNone(model1) + self.assertEqual(model1.nbody, 2) + np.testing.assert_array_equal(model1.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) + + # Attach entire spec to frame and compile again. + self.assertIsNotNone(frame.attach(child, prefix='child-')) + model2 = parent.compile() + self.assertIsNotNone(model2) + self.assertEqual(model2.nbody, 3) + np.testing.assert_array_equal(model2.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_pos[2], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) + np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) if __name__ == '__main__': From f882f4b065199c3618087e3abc7d07af7e0dd6b0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 5 Dec 2024 03:18:51 -0800 Subject: [PATCH 127/426] Remove requirement to specify a name prefix or suffix during attach. PiperOrigin-RevId: 703049862 Change-Id: I5fd6da0581331b1b716110292a8160ae16934157 --- src/user/user_objects.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3e4c61bf..9654182c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -873,10 +873,6 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { other.model->suffix = other.suffix; other.model->StoreKeyframes(model); - if (other.prefix.empty() && other.suffix.empty()) { - throw mjCError(this, "either prefix or suffix must be non-empty"); - } - // attach defaults if (other.model != model) { mjCDef* subdef = new mjCDef(*other.model->Default()); @@ -1845,10 +1841,6 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { other.model->prefix = ""; other.model->suffix = ""; - if (other.prefix.empty() && other.suffix.empty()) { - throw mjCError(this, "either prefix or suffix must be non-empty"); - } - mjCBody* subtree = new mjCBody(other, model); other.ForgetKeyframes(); other.model->prefix = subtree->prefix; From a729acb4fa1f490b5390dbb1a64ea4aee054c477 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 5 Dec 2024 03:41:30 -0800 Subject: [PATCH 128/426] Add spec find_sensor, find_actuator functions. PiperOrigin-RevId: 703054696 Change-Id: Ie384f1a31de3ecd9b22b7f4be43234d452e7bb88 --- python/mujoco/specs.cc | 14 ++++++++++++++ python/mujoco/specs_test.py | 16 ++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 4fcd94b9..1a697124 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -350,6 +350,20 @@ PYBIND11_MODULE(_specs, m) { return mjs_asSite(mjs_findElement(self.ptr, mjOBJ_SITE, name.c_str())); }, py::return_value_policy::reference_internal); + mjSpec.def( + "find_actuator", + [](MjSpec& self, std::string& name) -> raw::MjsActuator* { + return mjs_asActuator( + mjs_findElement(self.ptr, mjOBJ_ACTUATOR, name.c_str())); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_sensor", + [](MjSpec& self, std::string& name) -> raw::MjsSensor* { + return mjs_asSensor( + mjs_findElement(self.ptr, mjOBJ_SENSOR, name.c_str())); + }, + py::return_value_policy::reference_internal); mjSpec.def( "find_default", [](MjSpec& self, std::string& classname) -> const raw::MjsDefault* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 4090a7cf..91e260b0 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -584,13 +584,29 @@ class SpecsTest(absltest.TestCase): sensor1 = spec.add_sensor() sensor2 = spec.add_sensor() sensor3 = spec.add_sensor() + actuator1 = spec.add_actuator() + actuator2 = spec.add_actuator() + actuator3 = spec.add_actuator() sensor1.name = 'sensor1' sensor2.name = 'sensor2' sensor3.name = 'sensor3' + actuator1.name = 'actuator1' + actuator2.name = 'actuator2' + actuator3.name = 'actuator3' self.assertLen(spec.sensors, 3) + self.assertLen(spec.actuators, 3) self.assertEqual(spec.sensors[0].name, 'sensor1') self.assertEqual(spec.sensors[1].name, 'sensor2') self.assertEqual(spec.sensors[2].name, 'sensor3') + self.assertEqual(spec.actuators[0].name, 'actuator1') + self.assertEqual(spec.actuators[1].name, 'actuator2') + self.assertEqual(spec.actuators[2].name, 'actuator3') + self.assertEqual(spec.find_sensor('sensor1'), sensor1) + self.assertEqual(spec.find_sensor('sensor2'), sensor2) + self.assertEqual(spec.find_sensor('sensor3'), sensor3) + self.assertEqual(spec.find_actuator('actuator1'), actuator1) + self.assertEqual(spec.find_actuator('actuator2'), actuator2) + self.assertEqual(spec.find_actuator('actuator3'), actuator3) def test_body_list(self): main_xml = """ From 41968f1527944320f1df4970c93ac10c8ffbfeaa Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 5 Dec 2024 04:53:46 -0800 Subject: [PATCH 129/426] Support body.find_all("joint") in bindings. PiperOrigin-RevId: 703071822 Change-Id: I6457ca0561c794447cc06aab29dcd0aac82c49a0 --- python/mujoco/specs.cc | 4 +++- python/mujoco/specs_test.py | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 1a697124..c376ea35 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -493,6 +493,8 @@ PYBIND11_MODULE(_specs, m) { objtype = mjOBJ_GEOM; } else if (name == "site") { objtype = mjOBJ_SITE; + } else if (name == "joint") { + objtype = mjOBJ_JOINT; } else if (name == "light") { objtype = mjOBJ_LIGHT; } else if (name == "camera") { @@ -500,7 +502,7 @@ PYBIND11_MODULE(_specs, m) { } else { throw pybind11::value_error( "body.find_all supports the types: body, frame, geom, site, " - "light, camera."); + "joint, light, camera."); } return FindAllImpl(self, objtype, true); }, diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 91e260b0..e18db9b7 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -667,7 +667,7 @@ class SpecsTest(absltest.TestCase): self.assertEqual( str(cm.exception), 'body.find_all supports the types: body, frame, geom, site,' - ' light, camera.', + ' joint, light, camera.', ) body4 = spec.worldbody.find_all('body')[3] body4.name = 'body4_new' From fb8e40208c87a95d4b2a7e4b66cf0df99600ccab Mon Sep 17 00:00:00 2001 From: Jake Harmon Date: Thu, 5 Dec 2024 08:35:40 -0800 Subject: [PATCH 130/426] Update references to JAX's GitHub repo JAX has moved from https://github.com/google/jax to https://github.com/jax-ml/jax PiperOrigin-RevId: 703126479 Change-Id: Ie50003b4620db767663f2f95be40557cfe0b8dd3 --- doc/mjx.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index d13bae44..e485bcca 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -7,7 +7,7 @@ MuJoCo XLA (MJX) Starting with version 3.0.0, MuJoCo includes MuJoCo XLA (MJX) under the `mjx `__ directory. MJX allows MuJoCo to run on compute hardware supported by the `XLA `__ compiler via the -`JAX `__ framework. MJX runs on a +`JAX `__ framework. MJX runs on a `all platforms supported by JAX `__: Nvidia and AMD GPUs, Apple Silicon, and `Google Cloud TPUs `__. From 342a72839a40a336c1fbfb62fcdfdc84510bea53 Mon Sep 17 00:00:00 2001 From: Alessandro Croci <57228872+xela-95@users.noreply.github.com> Date: Fri, 6 Dec 2024 09:16:37 +0100 Subject: [PATCH 131/426] Fix typo in mjx.rst --- doc/mjx.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index e485bcca..116161c1 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -416,4 +416,4 @@ The following environment variables should be set: ``XLA_FLAGS=--xla_gpu_triton_gemm_any=true`` This enables the Triton-based GEMM (matmul) emitter for any GEMM that it supports. This can yield a 30% speedup on NVIDIA GPUs. If you have multiple GPUs, you may also benefit from enabling flags related to - `communciation between GPUs `__. + `communication between GPUs `__. From f547d5835123663fb2f7bb0a689ec11769287824 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 6 Dec 2024 04:15:17 -0800 Subject: [PATCH 132/426] Fix typo in docs PiperOrigin-RevId: 703449955 Change-Id: I3a5764278406dea4ff013ff82872cc27f5440f5e --- doc/XMLreference.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 0ce73814..fb8b2a8f 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6392,7 +6392,7 @@ contributed by all actuators to a single scalar joint (hinge or slider). If the :ref:`actuatorgravcomp` attribute is "true", this sensor will also measure contributions by gravity compensation forces (which are added directly to the joint and would *not* register in the :ref:`actuatorfrc`) sensor. This type of sensor is important when multiple actuators act on a single -joint or when a single actuator act on multiple joints. See :ref:`CForceRange` for details. +joint or when a single actuator acts on multiple joints. See :ref:`CForceRange` for details. .. _sensor-jointactuatorfrc-name: From fa8af0eea9760ea0bbafde3a9831cfd5b2f4eeda Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Fri, 6 Dec 2024 06:40:08 -0800 Subject: [PATCH 133/426] Add MuJoCo sparsification fields to MJX data structures. PiperOrigin-RevId: 703482996 Change-Id: I981bfc80825a9d4d6597d54035bc40a4f4ae068a --- mjx/mujoco/mjx/_src/io.py | 12 +++++++++--- mjx/mujoco/mjx/_src/types.py | 35 +++++++++++++++++++++++++---------- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index aa76714f..0988e25c 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -320,12 +320,18 @@ def make_data( 'subtree_angmom': (m.nbody, 3, float), 'qH': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), 'qHDiagInv': (m.nv, float), - 'D_rownnz': (m.nv, jp.int32), - 'D_rowadr': (m.nv, jp.int32), - 'D_colind': (m.nD, jp.int32), 'B_rownnz': (m.nbody, jp.int32), 'B_rowadr': (m.nbody, jp.int32), 'B_colind': (m.nB, jp.int32), + 'C_rownnz': (m.nv, jp.int32), + 'C_rowadr': (m.nv, jp.int32), + 'C_colind': (m.nC, jp.int32), + 'mapM2C': (m.nC, jp.int32), + 'D_rownnz': (m.nv, jp.int32), + 'D_rowadr': (m.nv, jp.int32), + 'D_colind': (m.nD, jp.int32), + 'mapM2D': (m.nD, jp.int32), + 'mapD2M': (m.nM, jp.int32), 'qDeriv': (m.nD, float), 'qLU': (m.nD, float), 'actuator_force': (m.nu, float), diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 6446cd33..35fbdb19 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -536,6 +536,8 @@ class Model(PyTreeNode): nM: number of non-zeros in sparse inertia matrix nD: number of non-zeros in sparse dof-dof matrix nB: number of non-zeros in sparse body-dof matrix + nC: number of non-zeros in sparse reduced dof-dof matrix + nD: number of non-zeros in sparse dof-dof matrix nJmom: number of non-zeros in sparse actuator_moment matrix ntree: number of kinematic trees under world body ngravcomp: number of bodies with nonzero gravcomp @@ -854,8 +856,9 @@ class Model(PyTreeNode): nkey: int nmocap: int nM: int # pylint:disable=invalid-name - nD: int # pylint:disable=invalid-name nB: int # pylint:disable=invalid-name + nC: int # pylint:disable=invalid-name + nD: int # pylint:disable=invalid-name nJmom: int ntree: int = _restricted_to('mujoco') ngravcomp: int @@ -1265,12 +1268,18 @@ class Data(PyTreeNode): subtree_angmom: angular momentum about subtree com (nbody, 3) qH: L'*D*L factorization of modified M (nM,) qHDiagInv: 1/diag(D) of modified M (nv,) - D_rownnz: non-zeros in each row (nv,) - D_rowadr: address of each row in D_colind (nv,) - D_colind: column indices of non-zeros (nD,) - B_rownnz: non-zeros in each row (nbody,) - B_rowadr: address of each row in B_colind (nbody,) - B_colind: column indices of non-zeros (nB,) + B_rownnz: body-dof: non-zeros in each row (nbody,) + B_rowadr: body-dof: address of each row in B_colind (nbody,) + B_colind: body-dof: column indices of non-zeros (nB,) + C_rownnz: reduced dof-dof: non-zeros in each row (nv,) + C_rowadr: reduced dof-dof: address of each row in C_colind (nv,) + C_colind: reduced dof-dof: column indices of non-zeros (nC,) + mapM2C: index mapping from M to C (nC,) + D_rownnz: dof-dof: non-zeros in each row (nv,) + D_rowadr: dof-dof: address of each row in D_colind (nv,) + D_colind: dof-dof: column indices of non-zeros (nD,) + mapM2D: index mapping from M to D (nD,) + mapD2M: index mapping from D to M (nM,) qDeriv: d (passive + actuator - bias) / d qvel (nD,) qLU: sparse LU of (qM - dt*qDeriv) (nD,) actuator_force: actuator force in actuation space (nu,) @@ -1388,12 +1397,18 @@ class Data(PyTreeNode): subtree_angmom: jax.Array qH: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name qHDiagInv: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name - D_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name - D_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name - D_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name B_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name B_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name B_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + C_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + C_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + C_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + mapM2C: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + mapM2D: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + mapD2M: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name qDeriv: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name qLU: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name # position, velocity, control & acceleration dependent: From c1d97d4a22512cde88deb81e2e8c7b194b64bfc1 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 6 Dec 2024 10:45:37 -0800 Subject: [PATCH 134/426] Fix #2237. PiperOrigin-RevId: 703547316 Change-Id: I279876a299e56a16f8d5227fc1d0f64265477853 --- mjx/mujoco/mjx/_src/io.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 0988e25c..dad0b573 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -233,9 +233,11 @@ def make_data( solreffriction=jp.zeros((ncon, mujoco.mjNREF), dtype=float), solimp=jp.zeros((ncon, mujoco.mjNIMP), dtype=float), dim=dim, - geom1=jp.full((ncon,), -1, dtype=jp.int32), - geom2=jp.full((ncon,), -1, dtype=jp.int32), - geom=jp.full((ncon, 2), -1, dtype=jp.int32), + # let jax pick contact.geom int precision, for interop with + # jax_enable_x64 + geom1=jp.full((ncon,), -1, dtype=int), + geom2=jp.full((ncon,), -1, dtype=int), + geom=jp.full((ncon, 2), -1, dtype=int), efc_address=efc_address, ) From 756a8d716ab374e37a504aeac85913b36702771d Mon Sep 17 00:00:00 2001 From: Silvio Date: Sat, 7 Dec 2024 16:01:45 +0100 Subject: [PATCH 135/426] Reduce cmake test boilerplate All instances of mujoco_test in the code base were followed by a call to target_link_libraries( fixture gmock). As anyhow mujoco_test already was calling target_link_libraries to some predefined list of targets (mujoco and gtest_main), this PR adds to the list of default linked targets also fixture and gmock, to reduce the boilerplate. Furthermore, to completly remove the need for calling target_link_libraries after a call to mujoco_test, this PR also add to the mujoco_test macro the ADDITIONAL_LINK_LIBRARIES argument, that can be used if a given test needs to link some additional targets beside the default ones. To permit to use these new features also for mujoco benchmarks, this PR adds a MAIN_TARGET parameter to mujoco_test, to select if gtest_main or another target is used to provide the main entry point to the test executable. --- test/CMakeLists.txt | 30 ++++++++-- test/benchmark/CMakeLists.txt | 83 +++++++-------------------- test/engine/CMakeLists.txt | 43 ++------------ test/plugin/actuator/CMakeLists.txt | 5 +- test/plugin/elasticity/CMakeLists.txt | 1 - test/plugin/sensor/CMakeLists.txt | 1 - test/thread/CMakeLists.txt | 2 - test/user/CMakeLists.txt | 17 +----- test/xml/CMakeLists.txt | 10 +--- 9 files changed, 58 insertions(+), 134 deletions(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 122760a9..0f92803d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -17,10 +17,22 @@ set(MUJOCO_TEST_WORKING_DIR ${CMAKE_CURRENT_SOURCE_DIR}) include(GoogleTest) +# This macro is used to add a C++ test to MuJoCo CMake build system +# +# The CMake targets linked by default with target_link_libraries are: +# mujoco fixture gmock +# +# The macro supports the following parameters: +# * PROPERTIES: multiple value parameter, its value is set as test properties +# via the set_tests_properties( PROPERTIES ) function +# * ADDITIONAL_LINK_LIBRARIES: multiple value parameter, additional libraries linked +# to the test via target_link_libraries +# * MAIN_TARGET: single value parameter, used to specify the target linked to the tests +# that define the main entry poiny. If not indicated, gest_main is used macro(mujoco_test name) set(options) - set(oneValueArgs) - set(multiValueArgs PROPERTIES) + set(oneValueArgs MAIN_TARGET) + set(multiValueArgs PROPERTIES ADDITIONAL_LINK_LIBRARIES) cmake_parse_arguments( _ARGS "${options}" @@ -30,7 +42,15 @@ macro(mujoco_test name) ) add_executable(${name} ${name}.cc) - target_link_libraries(${name} gtest_main mujoco) + target_link_libraries(${name} mujoco fixture gmock) + if(_ARGS_MAIN_TARGET) + target_link_libraries(${name} ${_ARGS_MAIN_TARGET}) + else() + target_link_libraries(${name} gtest_main) + endif() + if(_ARGS_ADDITIONAL_LINK_LIBRARIES) + target_link_libraries(${name} ${_ARGS_ADDITIONAL_LINK_LIBRARIES}) + endif() target_include_directories(${name} PRIVATE ${MUJOCO_TEST_INCLUDE}) set_target_properties(${name} PROPERTIES BUILD_RPATH ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) # gtest_discover_tests is recommended over gtest_add_tests, but has some issues in Windows. @@ -48,6 +68,7 @@ macro(mujoco_test name) if(_ARGS_PROPERTIES) set_tests_properties(${testList} PROPERTIES ${_ARGS_PROPERTIES}) endif() + endmacro() add_library(fixture STATIC fixture.h fixture.cc) @@ -67,13 +88,10 @@ target_link_libraries( target_include_directories(fixture PRIVATE ${mujoco_SOURCE_DIR}/include gmock) mujoco_test(fixture_test) -target_link_libraries(fixture_test fixture gmock) mujoco_test(header_test) -target_link_libraries(header_test fixture gmock) mujoco_test(pipeline_test) -target_link_libraries(pipeline_test fixture gmock) add_subdirectory(benchmark) add_subdirectory(engine) diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index dea826ff..658e0639 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -12,86 +12,45 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Macro for benchmarks that don't use GL (same as mujoco_api_test, but uses -# benchmark::benchmark_main). -macro(mujoco_benchmark_test name) - add_executable(${name} ${name}.cc) - target_link_libraries( - ${name} - benchmark::benchmark_main - mujoco - absl::core_headers - ) - target_include_directories(${name} PRIVATE ${MUJOCO_TEST_INCLUDE}) - # TODO(fraromano) Check RPATH settings - set_target_properties(${name} PROPERTIES BUILD_RPATH ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) - # gtest_discover_tests is recommended over gtest_add_tests, but has some issues in Windows. - gtest_add_tests( - TARGET ${name} - SOURCES ${name}.cc - WORKING_DIRECTORY ${MUJOCO_TEST_WORKING_DIR} - TEST_LIST testList - ) - if(WIN32) - set_tests_properties( - ${testList} PROPERTIES ENVIRONMENT "PATH=$;$ENV{PATH}" - ) - endif() -endmacro() - -mujoco_benchmark_test(ccd_benchmark_test) -target_link_libraries( +mujoco_test( ccd_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(step_benchmark_test) -target_link_libraries( +mujoco_test( step_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(thread_performance_test) -target_link_libraries( +mujoco_test( thread_performance_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(parse_benchmark_test) -target_link_libraries( +mujoco_test( parse_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(engine_util_spatial_benchmark_test) -target_link_libraries( +mujoco_test( engine_util_spatial_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(engine_core_smooth_benchmark_test) -target_link_libraries( +mujoco_test( engine_core_smooth_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) -mujoco_benchmark_test(engine_util_sparse_benchmark_test) -target_link_libraries( +mujoco_test( engine_util_sparse_benchmark_test - fixture - gmock - benchmark::benchmark + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index b562e585..625a0cbf 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -13,91 +13,59 @@ # limitations under the License. mujoco_test(engine_collision_box_test) -target_link_libraries(engine_collision_box_test fixture gmock) mujoco_test(engine_collision_convex_test) -target_link_libraries(engine_collision_convex_test fixture gmock) mujoco_test(engine_collision_driver_test) -target_link_libraries(engine_collision_driver_test fixture gmock) -mujoco_test(engine_collision_gjk_test) -target_link_libraries(engine_collision_gjk_test fixture gmock ccd) +mujoco_test(engine_collision_gjk_test ADDITIONAL_LINK_LIBRARIES ccd) mujoco_test(engine_core_constraint_test) -target_link_libraries(engine_core_constraint_test fixture gmock) -mujoco_test(engine_core_smooth_test) -target_link_libraries(engine_core_smooth_test fixture gmock absl::span) +mujoco_test(engine_core_smooth_test ADDITIONAL_LINK_LIBRARIES absl::span) mujoco_test(engine_derivative_test) -target_link_libraries(engine_derivative_test fixture gmock) mujoco_test(engine_forward_test) -target_link_libraries(engine_forward_test fixture gmock) mujoco_test(engine_inverse_test) -target_link_libraries(engine_inverse_test fixture gmock) mujoco_test(engine_island_test) -target_link_libraries(engine_island_test fixture gmock) -mujoco_test(engine_io_test) -target_link_libraries( - engine_io_test - fixture - gmock - absl::str_format -) +mujoco_test(engine_io_test ADDITIONAL_LINK_LIBRARIES absl::str_format) mujoco_test( engine_plugin_test + ADDITIONAL_LINK_LIBRARIES + absl::str_format PROPERTIES ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries( - engine_plugin_test - fixture - gmock - absl::str_format -) mujoco_test(engine_passive_test) -target_link_libraries(engine_passive_test fixture gmock) mujoco_test(engine_print_test) -target_link_libraries(engine_print_test fixture gmock) mujoco_test(engine_ray_test) -target_link_libraries(engine_ray_test fixture gmock) mujoco_test(engine_sensor_test) -target_link_libraries(engine_sensor_test fixture gmock) mujoco_test(engine_solver_test) -target_link_libraries(engine_solver_test fixture gmock) mujoco_test(engine_sort_test) -target_link_libraries(engine_sort_test fixture gmock) mujoco_test(engine_support_test) -target_link_libraries(engine_support_test fixture gmock) mujoco_test(engine_thread_test) -target_link_libraries(engine_thread_test fixture gmock) mujoco_test(engine_util_blas_test) -target_link_libraries(engine_util_blas_test fixture gmock) mujoco_test(engine_util_errmem_test) -target_link_libraries(engine_util_errmem_test fixture gmock) mujoco_test(engine_util_solve_test) -target_link_libraries(engine_util_solve_test fixture gmock) mujoco_test(engine_util_spatial_test) -target_link_libraries(engine_util_spatial_test fixture gmock) mujoco_test( engine_vis_state_test @@ -105,4 +73,3 @@ mujoco_test( ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries(engine_vis_state_test fixture gmock) diff --git a/test/plugin/actuator/CMakeLists.txt b/test/plugin/actuator/CMakeLists.txt index ac795625..b8e0f10b 100644 --- a/test/plugin/actuator/CMakeLists.txt +++ b/test/plugin/actuator/CMakeLists.txt @@ -14,8 +14,11 @@ mujoco_test( pid_test + ADDITIONAL_LINK_LIBRARIES + absl::cleanup + absl::strings PROPERTIES ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries(pid_test fixture gmock absl::cleanup absl::strings) +target_link_libraries(pid_test fixture gmock ) diff --git a/test/plugin/elasticity/CMakeLists.txt b/test/plugin/elasticity/CMakeLists.txt index 008ae8be..93883937 100644 --- a/test/plugin/elasticity/CMakeLists.txt +++ b/test/plugin/elasticity/CMakeLists.txt @@ -18,4 +18,3 @@ mujoco_test( ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries(elasticity_test fixture gmock) diff --git a/test/plugin/sensor/CMakeLists.txt b/test/plugin/sensor/CMakeLists.txt index 4adcba29..e634d16c 100644 --- a/test/plugin/sensor/CMakeLists.txt +++ b/test/plugin/sensor/CMakeLists.txt @@ -18,4 +18,3 @@ mujoco_test( ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries(sensor_test fixture gmock) diff --git a/test/thread/CMakeLists.txt b/test/thread/CMakeLists.txt index 920bea0d..ada7b7ef 100644 --- a/test/thread/CMakeLists.txt +++ b/test/thread/CMakeLists.txt @@ -13,7 +13,5 @@ # limitations under the License. mujoco_test(thread_pool_test) -target_link_libraries(thread_pool_test fixture gmock) mujoco_test(thread_queue_test) -target_link_libraries(thread_queue_test fixture gmock) diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index ac4dc7b3..c176c3bd 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -12,11 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -mujoco_test(user_model_test) -target_link_libraries(user_model_test fixture gmock absl::str_format) +mujoco_test(user_model_test ADDITIONAL_LINK_LIBRARIES absl::str_format) mujoco_test(user_objects_test) -target_link_libraries(user_objects_test fixture gmock) mujoco_test( user_api_test @@ -24,24 +22,13 @@ mujoco_test( ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries(user_api_test fixture gmock) mujoco_test(user_flex_test) -target_link_libraries(user_flex_test fixture gmock) -mujoco_test(user_mesh_test) -target_link_libraries( - user_mesh_test - fixture - gmock - absl::str_format -) +mujoco_test(user_mesh_test ADDITIONAL_LINK_LIBRARIES absl::str_format) mujoco_test(user_composite_test) -target_link_libraries(user_composite_test fixture gmock) mujoco_test(user_resource_test) -target_link_libraries(user_resource_test fixture gmock) mujoco_test(user_vfs_test) -target_link_libraries(user_vfs_test fixture gmock) diff --git a/test/xml/CMakeLists.txt b/test/xml/CMakeLists.txt index 537a991f..91753174 100644 --- a/test/xml/CMakeLists.txt +++ b/test/xml/CMakeLists.txt @@ -13,20 +13,14 @@ # limitations under the License. mujoco_test(xml_api_test) -target_link_libraries(xml_api_test fixture gmock) mujoco_test(xml_native_reader_test) -target_link_libraries(xml_native_reader_test fixture gmock) mujoco_test( xml_native_writer_test + ADDITIONAL_LINK_LIBRARIES + absl::flat_hash_set PROPERTIES ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) -target_link_libraries( - xml_native_writer_test - fixture - gmock - absl::flat_hash_set -) From 376dc59ea07e005448f7ec159fb0589eaf940683 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sat, 7 Dec 2024 17:25:19 +0100 Subject: [PATCH 136/426] GitHub Actions: Use macos-13 image instead of deprecated macos-12 --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index b22215b6..1a3288b9 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -96,7 +96,7 @@ jobs: -DCMAKE_CXX_COMPILER:STRING=clang++-10 -DMUJOCO_HARDEN:BOOL=ON tmpdir: "/tmp" - - os: macos-12 + - os: macos-13 cmake_args: >- -G Ninja -DMUJOCO_HARDEN:BOOL=ON From f7f1c84a1e405d32eaef8ac0677cbbe2e2f42214 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 8 Dec 2024 06:00:23 -0800 Subject: [PATCH 137/426] Print solver and solver iterations in `testspeed` PiperOrigin-RevId: 703995960 Change-Id: I9cebcf9e8d29803033ae56991acad4678d8138af --- sample/testspeed.cc | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 6d8f55bc..3791bf5b 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -30,10 +30,10 @@ const int maxthread = 512; mjModel* m = NULL; mjData* d[maxthread]; - // per-thread statistics int contacts[maxthread]; int constraints[maxthread]; +mjtNum iterations[maxthread]; mjtNum simtime[maxthread]; // timer @@ -84,6 +84,7 @@ void simulate(int id, int nstep, mjtNum* ctrl) { // clear statistics contacts[id] = 0; constraints[id] = 0; + iterations[id] = 0; // run and time mjtNum start = gettm(); @@ -97,6 +98,16 @@ void simulate(int id, int nstep, mjtNum* ctrl) { // accumulate statistics contacts[id] += d[id]->ncon; constraints[id] += d[id]->nefc; + int nisland = d[id]->solver_nisland; + if (nisland == 1) { + iterations[id] += d[id]->solver_niter[0]; + } else { + mjtNum niter = 0; + for (int j=0; j < nisland; j++) { + niter += d[id]->solver_niter[j]; + } + iterations[id] += niter / nisland; + } } simtime[id] = 1e-6 * (gettm() - start); } @@ -230,15 +241,21 @@ int main(int argc, char** argv) { std::printf("Details for thread 0\n\n"); } + // solver names indexed by mjtSolver + const char* solver[] = {"PGS", "CG", "Newton"}; + const char* solto6[] = {" ", " ", ""}; // complete to 6 characters + // details for thread 0 std::printf(" Simulation time : %.2f s\n", simtime[0]); std::printf(" Steps per second : %.0f\n", nstep/simtime[0]); std::printf(" Realtime factor : %.2f x\n", nstep*m->opt.timestep/simtime[0]); std::printf(" Time per step : %.1f %ss\n\n", 1e6*simtime[0]/nstep, mu_str); - std::printf(" Contacts per step : %.2f\n", static_cast(contacts[0])/nstep); - std::printf(" Constraints per step : %.2f\n", static_cast(constraints[0])/nstep); + std::printf(" %s iters / step %s: %.2f\n", + solver[m->opt.solver], solto6[m->opt.solver], iterations[0]/nstep); + std::printf(" Contacts / step : %.2f\n", static_cast(contacts[0])/nstep); + std::printf(" Constraints / step : %.2f\n", static_cast(constraints[0])/nstep); std::printf(" Degrees of freedom : %d\n", m->nv); - std::printf(" Memory usage : %.1f%% of %s\n\n", + std::printf(" Dynamic memory usage : %.1f%% of %s\n\n", 100 * d[0]->maxuse_arena / (double)(d[0]->narena), mju_writeNumBytes(d[0]->narena)); From b8344c191f909c8059c678163d90de4e148fb0d7 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 9 Dec 2024 07:07:34 -0800 Subject: [PATCH 138/426] Internal change. PiperOrigin-RevId: 704273388 Change-Id: I6de2aa5bf80353dae4f2faf408c4b4c1c2e14f15 --- mjx/mujoco/mjx/_src/io.py | 50 ++++++++++++++++++++++----------------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index dad0b573..d09ae016 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -37,28 +37,32 @@ def _strip_weak_type(tree): return jax.tree_util.tree_map(f, tree) -def _make_option(o: mujoco.MjOption) -> types.Option: +def _make_option( + o: mujoco.MjOption, _full_compat: bool = False +) -> types.Option: """Returns mjx.Option given mujoco.MjOption.""" - if o.integrator not in set(types.IntegratorType): - raise NotImplementedError(f'{mujoco.mjtIntegrator(o.integrator)}') + if not _full_compat: + if o.integrator not in set(types.IntegratorType): + raise NotImplementedError(f'{mujoco.mjtIntegrator(o.integrator)}') - if o.cone not in set(types.ConeType): - raise NotImplementedError(f'{mujoco.mjtCone(o.cone)}') + if o.cone not in set(types.ConeType): + raise NotImplementedError(f'{mujoco.mjtCone(o.cone)}') - if o.jacobian not in set(types.JacobianType): - raise NotImplementedError(f'{mujoco.mjtJacobian(o.jacobian)}') + if o.jacobian not in set(types.JacobianType): + raise NotImplementedError(f'{mujoco.mjtJacobian(o.jacobian)}') - if o.solver not in set(types.SolverType): - raise NotImplementedError(f'{mujoco.mjtSolver(o.solver)}') + if o.solver not in set(types.SolverType): + raise NotImplementedError(f'{mujoco.mjtSolver(o.solver)}') - for i in range(mujoco.mjtEnableBit.mjNENABLE): - if o.enableflags & 2**i: - raise NotImplementedError(f'{mujoco.mjtEnableBit(2 ** i)}') + for i in range(mujoco.mjtEnableBit.mjNENABLE): + if o.enableflags & 2**i: + raise NotImplementedError(f'{mujoco.mjtEnableBit(2 ** i)}') has_fluid_params = o.density > 0 or o.viscosity > 0 or o.wind.any() implicitfast = o.integrator == mujoco.mjtIntegrator.mjINT_IMPLICITFAST - if implicitfast and has_fluid_params: - raise NotImplementedError('implicitfast not implemented for fluid drag.') + if not _full_compat: + if implicitfast and has_fluid_params: + raise NotImplementedError('implicitfast not implemented for fluid drag.') fields = {f.name: getattr(o, f.name, None) for f in types.Option.fields()} fields['integrator'] = types.IntegratorType(o.integrator) @@ -182,16 +186,17 @@ def put_model( fields['tendon_hasfrictionloss'] = fields['tendon_frictionloss'] > 0 fields['geom_rbound_hfield'] = fields['geom_rbound'] fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3)) - fields['opt'] = _make_option(m.opt) + fields['opt'] = _make_option(m.opt, _full_compat=_full_compat) fields['stat'] = _make_statistic(m.stat) # Pre-compile meshes for MJX collisions. fields['mesh_convex'] = [None] * m.nmesh - for i in mesh_geomid: - dataid = m.geom_dataid[i] - if fields['mesh_convex'][dataid] is None: - fields['mesh_convex'][dataid] = mesh.convex(m, dataid) # pytype: disable=unsupported-operands - fields['mesh_convex'] = tuple(fields['mesh_convex']) + if not _full_compat: + for i in mesh_geomid: + dataid = m.geom_dataid[i] + if fields['mesh_convex'][dataid] is None: + fields['mesh_convex'][dataid] = mesh.convex(m, dataid) # pytype: disable=unsupported-operands + fields['mesh_convex'] = tuple(fields['mesh_convex']) model = types.Model(**{k: copy.copy(v) for k, v in fields.items()}) @@ -496,9 +501,10 @@ def get_data_into( value = np.ones(m.nv) if isinstance(value, np.ndarray) and value.shape: - if restricted_to in ('mujoco', 'mjx') and value.shape == (0,): + if restricted_to in ('mujoco', 'mjx'): continue # don't copy fields that are mujoco-only or MJX-only - getattr(result_i, field.name)[:] = value + else: + getattr(result_i, field.name)[:] = value else: setattr(result_i, field.name, value) From 3a14ada308938aee8313457eedff34c9a67092d2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 9 Dec 2024 08:00:29 -0800 Subject: [PATCH 139/426] Store index of diagonal element for C and D in mjData. PiperOrigin-RevId: 704287869 Change-Id: I15d1faa0b5c3ff54721f692ec6329726456ec318 --- doc/includes/references.h | 2 ++ include/mujoco/mjdata.h | 2 ++ include/mujoco/mjxmacro.h | 2 ++ introspect/structs.py | 16 ++++++++++++++ mjx/mujoco/mjx/_src/io.py | 2 ++ mjx/mujoco/mjx/_src/types.py | 4 ++++ src/engine/engine_io.c | 20 ++++++++++++++--- src/engine/engine_print.c | 33 ++++++++++++++++------------ test/fixture.h | 2 +- unity/Runtime/Bindings/MjBindings.cs | 2 ++ 10 files changed, 67 insertions(+), 18 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 6b9610a9..948d88f9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -314,10 +314,12 @@ struct mjData_ { int* B_colind; // body-dof: column indices of non-zeros (nB x 1) int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1) int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1) + int* C_diag; // reduced dof-dof: index of diagonal element (nv x 1) int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1) int* mapM2C; // index mapping from M to C (nC x 1) int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1) int* D_rowadr; // dof-dof: address of each row in D_colind (nv x 1) + int* D_diag; // dof-dof: index of diagonal element (nv x 1) int* D_colind; // dof-dof: column indices of non-zeros (nD x 1) int* mapM2D; // index mapping from M to D (nD x 1) int* mapD2M; // index mapping from D to M (nM x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 7ef96b12..d7185142 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -342,10 +342,12 @@ struct mjData_ { int* B_colind; // body-dof: column indices of non-zeros (nB x 1) int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1) int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1) + int* C_diag; // reduced dof-dof: index of diagonal element (nv x 1) int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1) int* mapM2C; // index mapping from M to C (nC x 1) int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1) int* D_rowadr; // dof-dof: address of each row in D_colind (nv x 1) + int* D_diag; // dof-dof: index of diagonal element (nv x 1) int* D_colind; // dof-dof: column indices of non-zeros (nD x 1) int* mapM2D; // index mapping from M to D (nD x 1) int* mapD2M; // index mapping from D to M (nM x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 95a06b2c..a54c4ff5 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -655,10 +655,12 @@ X ( int, B_colind, nB, 1 ) \ X ( int, C_rownnz, nv, 1 ) \ X ( int, C_rowadr, nv, 1 ) \ + X ( int, C_diag, nv, 1 ) \ X ( int, C_colind, nC, 1 ) \ X ( int, mapM2C, nC, 1 ) \ X ( int, D_rownnz, nv, 1 ) \ X ( int, D_rowadr, nv, 1 ) \ + X ( int, D_diag, nv, 1 ) \ X ( int, D_colind, nD, 1 ) \ X ( int, mapM2D, nD, 1 ) \ X ( int, mapD2M, nM, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index c1e00a86..194e410d 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -5396,6 +5396,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='reduced dof-dof: address of each row in C_colind (nv x 1)', # pylint: disable=line-too-long ), + StructFieldDecl( + name='C_diag', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='reduced dof-dof: index of diagonal element', + array_extent=('nv',), + ), StructFieldDecl( name='C_colind', type=PointerType( @@ -5428,6 +5436,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='dof-dof: address of each row in D_colind', array_extent=('nv',), ), + StructFieldDecl( + name='D_diag', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='dof-dof: index of diagonal element', + array_extent=('nv',), + ), StructFieldDecl( name='D_colind', type=PointerType( diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index d09ae016..10182e76 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -332,10 +332,12 @@ def make_data( 'B_colind': (m.nB, jp.int32), 'C_rownnz': (m.nv, jp.int32), 'C_rowadr': (m.nv, jp.int32), + 'C_diag': (m.nv, jp.int32), 'C_colind': (m.nC, jp.int32), 'mapM2C': (m.nC, jp.int32), 'D_rownnz': (m.nv, jp.int32), 'D_rowadr': (m.nv, jp.int32), + 'D_diag': (m.nv, jp.int32), 'D_colind': (m.nD, jp.int32), 'mapM2D': (m.nD, jp.int32), 'mapD2M': (m.nM, jp.int32), diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 35fbdb19..83fe9f4a 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -1273,10 +1273,12 @@ class Data(PyTreeNode): B_colind: body-dof: column indices of non-zeros (nB,) C_rownnz: reduced dof-dof: non-zeros in each row (nv,) C_rowadr: reduced dof-dof: address of each row in C_colind (nv,) + C_diag: reduced dof-dof: index of diagonal element (nv,) C_colind: reduced dof-dof: column indices of non-zeros (nC,) mapM2C: index mapping from M to C (nC,) D_rownnz: dof-dof: non-zeros in each row (nv,) D_rowadr: dof-dof: address of each row in D_colind (nv,) + D_diag: dof-dof: index of diagonal element (nv,) D_colind: dof-dof: column indices of non-zeros (nD,) mapM2D: index mapping from M to D (nD,) mapD2M: index mapping from D to M (nM,) @@ -1402,10 +1404,12 @@ class Data(PyTreeNode): B_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + C_diag: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name mapM2C: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name D_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name D_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_diag: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name D_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name mapM2D: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name mapD2M: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index ec24d4cf..59704140 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -921,7 +921,8 @@ int mj_sizeModel(const mjModel* m) { // construct sparse representation of dof-dof matrix static void makeDofDofSparse(const mjModel* m, mjData* d, - int* rownnz, int* rowadr, int* colind, int reduced) { + int* rownnz, int* rowadr, int* diag, int* colind, + int reduced) { int nv = m->nv; // no dofs, nothing to do @@ -986,6 +987,19 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, mjERROR("sum of rownnz different from expected"); } + // find diagonal indices + for (int i = 0; i < nv; i++) { + int adr = rowadr[i]; + int j = 0; + while (colind[adr + j] < i && j < rownnz[i]) { + j++; + } + if (colind[adr + j] != i) { + mjERROR("diagonal index not found"); + } + diag[i] = j; + } + mj_freeStack(d); } @@ -1915,14 +1929,14 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // construct sparse matrix representations if (m->body_dofadr) { // make D - makeDofDofSparse(m, d, d->D_rownnz, d->D_rowadr, d->D_colind, /*reduced=*/0); + makeDofDofSparse(m, d, d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind, /*reduced=*/0); // make B, check D and B makeBSparse(m, d); checkDBSparse(m, d); // make C - makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_colind, /*reduced=*/1); + makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind, /*reduced=*/1); makeDmap(m, d); } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 24bd02bb..10ad5668 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -114,8 +114,8 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, // print sparse matrix structure -static void printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* rownnz, - const int* rowsuper, const int* colind, FILE* fp) { +static void printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, + const int* rownnz, const int* rowsuper, const int* colind, FILE* fp) { // if no rows / columns, or too many columns to be visually useful, return if (!nr || !nc || nc > 300) { return; @@ -130,7 +130,11 @@ static void printSparsity(const char* str, int nr, int nc, const int* rowadr, co int nnz = 0; for (int c=0; c < nc; c++) { if (nnz < rownnz[r] && colind[adr + nnz] == c) { - fprintf(fp, "x"); + if (diag && diag[r] == nnz) { + fprintf(fp, "D"); + } else { + fprintf(fp, "x"); + } nnz++; } else { fprintf(fp, " "); @@ -1057,7 +1061,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format); } else { printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv, - d->flexedge_J_rowadr, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind, fp); + d->flexedge_J_rowadr, NULL, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind, fp); printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp); printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp); printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz, @@ -1069,8 +1073,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); } else { - printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, d->ten_J_rownnz, - NULL, d->ten_J_colind, fp); + printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, + d->ten_J_rownnz, NULL, d->ten_J_colind, fp); printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, @@ -1087,7 +1091,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); printSparsity("actuator_moment", m->nu, m->nv, - d->moment_rowadr, d->moment_rownnz, NULL, d->moment_colind, fp); + d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp); printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); @@ -1106,7 +1110,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format); // B sparse structure - printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, d->B_rownnz, NULL, + printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL, d->B_colind, fp); // B_rownnz @@ -1131,8 +1135,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // C sparse structure - printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_rownnz, NULL, - d->C_colind, fp); + printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_diag, d->C_rownnz, + NULL, d->C_colind, fp); fprintf(fp, NAME_FORMAT, "C_rownnz"); for (int i = 0; i < m->nv; i++) { @@ -1162,7 +1166,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // D sparse structure - printSparsity("D: dof-dof matrix", m->nv, m->nv, d->D_rowadr, d->D_rownnz, NULL, d->D_colind, fp); + printSparsity("D: dof-dof matrix", m->nv, m->nv, + d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp); // D_rownnz fprintf(fp, NAME_FORMAT, "D_rownnz"); @@ -1259,13 +1264,13 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); printArray("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); } else { - printSparsity("J: constraint Jacobian", d->nefc, m->nv, d->efc_J_rowadr, d->efc_J_rownnz, - d->efc_J_rowsuper, d->efc_J_colind, fp); + printSparsity("J: constraint Jacobian", d->nefc, m->nv, + d->efc_J_rowadr, NULL, d->efc_J_rownnz, d->efc_J_rowsuper, d->efc_J_colind, fp); printArrayInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp); printArrayInt("EFC_J_ROWADR", d->nefc, 1, d->efc_J_rowadr, fp); printSparse("EFC_J", d->efc_J, d->nefc, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, fp, float_format); - printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, + printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, NULL, d->efc_JT_rownnz, d->efc_JT_rowsuper, d->efc_JT_colind, fp); printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); diff --git a/test/fixture.h b/test/fixture.h index 4d65623d..69224c15 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -118,7 +118,7 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) { std::cerr << "\n"; for (int r = 0; r < nrow; r++) { for (int c = 0; c < ncol; c++) { - std::cerr << std::fixed << std::setw(4 + p) << mat[c + r*ncol] << " "; + std::cerr << std::fixed << std::setw(3 + p) << mat[c + r*ncol] << " "; } std::cerr << "\n"; } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index de038bef..8dec655e 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4940,10 +4940,12 @@ public unsafe struct mjData_ { public int* B_colind; public int* C_rownnz; public int* C_rowadr; + public int* C_diag; public int* C_colind; public int* mapM2C; public int* D_rownnz; public int* D_rowadr; + public int* D_diag; public int* D_colind; public int* mapM2D; public int* mapD2M; From bd1d94c2825b727d9e5fe16d44aeb69922c33723 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 9 Dec 2024 08:46:45 -0800 Subject: [PATCH 140/426] Fix mjx.rst. PiperOrigin-RevId: 704301576 Change-Id: Ia663f5b7fe224ca3e7865829744379eac38fac3e --- doc/mjx.rst | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index 116161c1..cc50fe2e 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -233,7 +233,7 @@ The following features are **fully supported** in MJX: * - :ref:`Cone ` - ``PYRAMIDAL``, ``ELLIPTIC`` * - :ref:`Condim ` - - 1, 3, 4, 6 + - 1, 3, 4, 6 (1 is not supported with ``ELLIPTIC``) * - :ref:`Solver ` - ``CG``, ``NEWTON`` * - Fluid Model @@ -244,10 +244,9 @@ The following features are **fully supported** in MJX: - ``MAGNETOMETER``, ``CAMPROJECTION``, ``RANGEFINDER``, ``JOINTPOS``, ``TENDONPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``FRAMEQUAT``, ``SUBTREECOM``, ``CLOCK``, ``VELOCIMETER``, ``GYRO``, ``JOINTVEL``, ``TENDONVEL``, ``ACTUATORVEL``, ``BALLANGVEL``, ``FRAMELINVEL``, - ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACTUATORFRC``, ``JOINTACTFRC``, - ``FRAMELINACC``, ``FRAMEANGACC`` - - ``ACCELEROMETER``, ``FORCE``, and ``TORQUE`` are supported if the model does not include connect or weld equality - constraints. + ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACCELEROMETER``, ``FORCE``, ``TORQUE``, + ``ACTUATORFRC``, ``JOINTACTFRC``, ``FRAMELINACC``, ``FRAMEANGACC`` + (``ACCELEROMETER``, ``FORCE``, ``TORQUE`` not supported with connect or weld equality constraints) The following features are **in development** and coming soon: @@ -267,7 +266,7 @@ The following features are **in development** and coming soon: * - Dynamics - :ref:`Inverse ` * - :ref:`Tendon Wrapping ` - - ``SPHERE``, ``CYLINDER`` + - ``SPHERE``, ``CYLINDER`` (external wrapping is supported) * - Fluid Model - :ref:`flEllipsoid` * - :ref:`Tendons ` @@ -276,9 +275,6 @@ The following features are **in development** and coming soon: - All except ``PLUGIN``, ``USER`` * - Lights - Positions and directions of lights - * - :ref:`Sensors ` - - ``ACCELEROMETER``, ``FORCE``, and ``TORQUE`` for models that include connect or weld equality - constraints. The following features are **unsupported**: From 537fc2ff45df0303b74381488007e52430216046 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 9 Dec 2024 08:52:31 -0800 Subject: [PATCH 141/426] Use diagonal index to speed up mju_solveLUSparse. PiperOrigin-RevId: 704303282 Change-Id: I72c3fbcd31a5564fc792b73efe9d12a151b4d60f --- src/engine/engine_forward.c | 2 +- src/engine/engine_util_solve.c | 34 ++++++++++++---------------------- src/engine/engine_util_solve.h | 2 +- 3 files changed, 14 insertions(+), 24 deletions(-) diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index e77639e5..181db4d3 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -967,7 +967,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc - mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_colind); + mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind); } // IMPLICITFAST diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 10b50157..4a455359 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -651,41 +651,31 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, // solve mat*res=vec given LU factorization of mat void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, - const int* rownnz, const int* rowadr, const int* colind) { - //------------------ solve (U+I)*res = vec + const int* rownnz, const int* rowadr, const int* diag, const int* colind) { + // solve (U+I)*res = vec for (int i=n-1; i >= 0; i--) { // init: diagonal of (U+I) is 1 res[i] = vec[i]; - // res[i] -= sum_k>i res[k]*LU(i,k) - int j = rownnz[i] - 1; - while (colind[rowadr[i]+j] > i) { - res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j]; - j--; - } - - // make sure j points to diagonal - if (colind[rowadr[i]+j] != i) { - mjERROR("diagonal of U not reached"); + int d1 = diag[i]+1; + int nnz = rownnz[i] - d1; + if (nnz > 0) { + int adr = rowadr[i] + d1; + res[i] -= mju_dotSparse(LU+adr, res, nnz, colind+adr, /*flg_unc1=*/0); } } //------------------ solve L*res(new) = res for (int i=0; i < n; i++) { // res[i] -= sum_k 0) { + res[i] -= mju_dotSparse(LU+adr, res, d, colind+adr, /*flg_unc1=*/0); } // divide by diagonal element of L - res[i] /= LU[rowadr[i]+j]; - - // make sure j points to diagonal - if (colind[rowadr[i]+j] != i) { - mjERROR("diagonal of L not reached"); - } + res[i] /= LU[adr + d]; } } diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 308e2d77..91ea13cc 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -82,7 +82,7 @@ void mju_factorLUSparse(mjtNum *LU, int n, int* scratch, // solve mat*res=vec given LU factorization of mat void mju_solveLUSparse(mjtNum *res, const mjtNum *LU, const mjtNum* vec, int n, - const int *rownnz, const int *rowadr, const int *colind); + const int *rownnz, const int *rowadr, const int* diag, const int *colind); // eigenvalue decomposition of symmetric 3x3 matrix MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]); From 1c4c7b012c25a5a172de97d3d55a73e52c33ae3f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 9 Dec 2024 09:33:20 -0800 Subject: [PATCH 142/426] CSR implementation of mj_solveLD. PiperOrigin-RevId: 704316200 Change-Id: Ibaff0284e40b3ebbe43bb489b6211ce739270e27 --- src/engine/engine_core_smooth.c | 32 +++++ src/engine/engine_core_smooth.h | 7 +- .../engine_core_smooth_benchmark_test.cc | 122 ++++++------------ test/engine/engine_core_smooth_test.cc | 78 ++++++++++- 4 files changed, 152 insertions(+), 87 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 6cc5a21f..a11895b5 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1575,6 +1575,38 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, } + +// in-place sparse backsubstitution: x = inv(L'*D*L)*x +// like mj_solveLD, but using the CSR representation of L +void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, + const int* rownnz, const int* rowadr, const int* diag, const int* colind) { + // x <- L^-T x + for (int i=nv-2; i >= 0; i--) { + int d1 = diag[i] + 1; + int nnz = rownnz[i] - d1; + if (nnz > 0) { + int adr = rowadr[i] + d1; + x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0); + } + } + + // x(i) /= D(i,i) + for (int i=0; i < nv; i++) { + x[i] *= qLDiagInv[i]; + } + + // x <- L^-1 x + for (int i=1; i < nv; i++) { + int d = diag[i]; + if (d > 0) { + int adr = rowadr[i]; + x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); + } + } +} + + + // sparse backsubstitution: x = inv(L'*D*L)*y // use factorization in d void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index 35a78b43..a1fab753 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -55,10 +55,15 @@ MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd MJAPI void mj_factorM(const mjModel* m, mjData* d); -// sparse backsubstitution: x = inv(L'*D*L)*y +// sparse backsubstitution: x = inv(L'*D*L)*x MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, const mjtNum* qLD, const mjtNum* qLDiagInv); +// in-place sparse backsubstitution: x = inv(L'*D*L)*x +// like mj_solveLD, but using the CSR representation of L +MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, + const int* rownnz, const int* rowadr, const int* diag, const int* colind); + // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index fc6c6a89..9a80ae8e 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -19,85 +19,28 @@ #include #include #include +#include "src/engine/engine_core_smooth.h" #include "test/fixture.h" namespace mujoco { namespace { -// number of steps to roll out before benhmarking +// number of steps to roll out before benchmarking static const int kNumWarmupSteps = 200; // number of steps to benchmark static const int kNumBenchmarkSteps = 50; -// ----------------------------- old functions -------------------------------- - -void ABSL_ATTRIBUTE_NOINLINE solveLD_baseline(const mjModel* m, mjtNum* x, - const mjtNum* y, - const mjtNum* qLD, - const mjtNum* qLDiagInv) { - mjtNum tmp; - - // local copies of key variables - int* dof_Madr = m->dof_Madr; - int* dof_parentid = m->dof_parentid; - int nv = m->nv; - - // x = y - if (x != y) { - mju_copy(x, y, nv); - } - - // x <- inv(L') * x; skip simple, exploit sparsity of input vector - for (int i=nv-1; i >= 0; i--) { - if (!m->dof_simplenum[i] && (tmp = x[i])) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - while (j >= 0) { - x[j] -= qLD[Madr_ij++]*tmp; // x(j) -= L(i,j) * x(i) - - // advance to parent - j = dof_parentid[j]; - } - } - } - - // x <- inv(D) * x - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; // x(i) /= L(i,i) - } - - // x <- inv(L) * x; skip simple - for (int i=0; i < nv; i++) { - if (!m->dof_simplenum[i]) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - tmp = x[i]; - while (j>= 0) { - tmp -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j) - - // advance to parent - j = dof_parentid[j]; - } - x[i] = tmp; - } - } -} - -void solveM_baseline(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y) { - solveLD_baseline(m, x, y, d->qLD, d->qLDiagInv); -} - // ----------------------------- benchmark ------------------------------------ -static void BM_solveLD(benchmark::State& state, bool new_function) { - static mjModel* m = LoadModelFromPath("plugin/elasticity/coil.xml"); +static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { + static mjModel* m; + if (coil) { + m = LoadModelFromPath("plugin/elasticity/coil.xml"); + } else { + m = LoadModelFromPath("humanoid/humanoid100.xml"); + } + mjData* d = mj_makeData(m); // warm-up rollout to get a typical state @@ -117,24 +60,21 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; } - // save state - std::vector qpos = AsVector(d->qpos, m->nq); - std::vector qvel = AsVector(d->qvel, m->nv); - std::vector act = AsVector(d->act, m->na); - std::vector warmstart = AsVector(d->qacc_warmstart, m->nv); + // CSR matrix + mjtNum* LDs = mj_stackAllocNum(d, m->nC); + for (int i=0; i < m->nC; i++) { + LDs[i] = d->qLD[d->mapM2C[i]]; + } // reset state, benchmark subsequent kNumBenchmarkSteps steps while (state.KeepRunningBatch(kNumBenchmarkSteps)) { - mju_copy(d->qpos, qpos.data(), m->nq); - mju_copy(d->qvel, qvel.data(), m->nv); - mju_copy(d->act, act.data(), m->na); - mju_copy(d->qacc_warmstart, warmstart.data(), m->nv); - for (int i=0; i < kNumBenchmarkSteps; i++) { - if (new_function) { + if (featherstone) { mj_solveM(m, d, res, grad, 1); } else { - solveM_baseline(m, d, res, grad); + mju_copy(res, grad, m->nv); + mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind); } } } @@ -145,17 +85,29 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { state.SetItemsProcessed(state.iterations()); } -void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_new(benchmark::State& state) { +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_FS(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_solveLD(state, true); + BM_solveLD(state, /*featherstone=*/true, /*coil=*/true); } -BENCHMARK(BM_solveLD_new); +BENCHMARK(BM_solveLD_COIL_FS); -void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_old(benchmark::State& state) { +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_solveLD(state, false); + BM_solveLD(state, /*featherstone=*/false, /*coil=*/true); } -BENCHMARK(BM_solveLD_old); +BENCHMARK(BM_solveLD_COIL_CSR); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_FS(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_solveLD(state, /*featherstone=*/true, /*coil=*/false); +} +BENCHMARK(BM_solveLD_H100_FS); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_CSR(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_solveLD(state, /*featherstone=*/false, /*coil=*/false); +} +BENCHMARK(BM_solveLD_H100_CSR); } // namespace } // namespace mujoco diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 8b2b4f2a..f27d1670 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_core_smooth.c. #include "src/engine/engine_core_smooth.h" +#include "src/engine/engine_util_sparse.h" #include #include @@ -31,6 +32,7 @@ namespace mujoco { namespace { +using ::std::vector; using ::testing::Each; using ::testing::ElementsAre; using ::testing::Eq; @@ -261,7 +263,7 @@ TEST_F(CoreSmoothTest, EqualityBodySite) { while (data->time < 0.1) { mj_step(model, data); } - std::vector sdata = AsVector(data->sensordata, model->nsensordata); + vector sdata = AsVector(data->sensordata, model->nsensordata); // reset mj_resetData(model, data); @@ -405,5 +407,79 @@ TEST_F(CoreSmoothTest, SolveMIsland) { mj_deleteModel(model); } +TEST_F(CoreSmoothTest, SolveLD2) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + int nv = m->nv; + int nC = m->nC; + + // copy LD into LDs: CSR format + vector LDs(nC); + for (int i=0; i < nC; i++) { + LDs[i] = d->qLD[d->mapM2C[i]]; + } + + // compare LD and LDs densified matrices + vector LDdense(nv*nv); + mju_sparse2dense(LDdense.data(), LDs.data(), nv, nv, + d->C_rownnz, d->C_rowadr, d->C_colind); + vector LDdense2(nv*nv); + mj_fullM(m, LDdense2.data(), d->qLD); + + // expect dense matrices to match exactly + for (int i=0; i < nv*nv; i++) EXPECT_EQ(LDdense[i], LDdense2[i]); + + // compare LD and LDs vector solve + vector vec(nv); + vector vec2(nv); + for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i; + for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; + + mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); + mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, + d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind); + + // expect vectors to match up to floating point precision + for (int i=0; i < nv; i++) { + EXPECT_FLOAT_EQ(vec[i], vec2[i]); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco From 6f6244b7395e3e3ae5ca49948a610f0cf88dfea4 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 9 Dec 2024 11:53:53 -0800 Subject: [PATCH 143/426] Fix MJX `get_data_into`. PiperOrigin-RevId: 704367029 Change-Id: I720e624f996a01ab2210a907c610b68ff1ad109c --- mjx/mujoco/mjx/_src/io.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 10182e76..9e09a3f6 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -474,7 +474,7 @@ def get_data_into( if m.nu: mujoco.mju_dense2sparse( actuator_moment, - d.actuator_moment, + d_i.actuator_moment, moment_rownnz, moment_rowadr, moment_colind, From f3b3024291cbe7efd91c4871b598a1226114167e Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 9 Dec 2024 21:10:00 -0800 Subject: [PATCH 144/426] Add pyink and isort config. Reformat. PiperOrigin-RevId: 704533915 Change-Id: I37e9fd51261bd166b725c7460fc65d02fed2b391 --- STYLEGUIDE.md | 6 +- mjx/mujoco/mjx/_src/collision_convex.py | 14 +- mjx/mujoco/mjx/_src/collision_driver.py | 6 +- mjx/mujoco/mjx/_src/collision_driver_test.py | 28 +- mjx/mujoco/mjx/_src/collision_primitive.py | 17 +- mjx/mujoco/mjx/_src/collision_sdf.py | 29 +- mjx/mujoco/mjx/_src/collision_types.py | 1 + mjx/mujoco/mjx/_src/constraint.py | 1 + mjx/mujoco/mjx/_src/constraint_test.py | 4 +- mjx/mujoco/mjx/_src/dataclasses.py | 3 +- mjx/mujoco/mjx/_src/io.py | 15 +- mjx/mujoco/mjx/_src/io_test.py | 5 +- mjx/mujoco/mjx/_src/math.py | 1 + mjx/mujoco/mjx/_src/mesh.py | 4 +- mjx/mujoco/mjx/_src/mesh_test.py | 5 +- mjx/mujoco/mjx/_src/passive.py | 1 + mjx/mujoco/mjx/_src/ray.py | 2 +- mjx/mujoco/mjx/_src/scan.py | 26 +- mjx/mujoco/mjx/_src/scan_test.py | 3 + mjx/mujoco/mjx/_src/sensor_test.py | 2 - mjx/mujoco/mjx/_src/smooth.py | 4 +- mjx/mujoco/mjx/_src/solver.py | 4 +- mjx/mujoco/mjx/_src/test_util.py | 2 +- mjx/mujoco/mjx/_src/types.py | 26 +- mjx/mujoco/mjx/testspeed.py | 4 +- mjx/mujoco/mjx/viewer.py | 5 +- mjx/pyproject.toml | 21 + python/mujoco/bindings_test.py | 410 ++++++++++++------- python/mujoco/memory_leak_test.py | 3 +- python/mujoco/minimize.py | 30 +- python/mujoco/minimize_test.py | 101 +++-- python/mujoco/msh2obj_test.py | 4 +- python/mujoco/render_test.py | 30 +- python/mujoco/renderer.py | 24 +- python/mujoco/renderer_test.py | 6 +- python/mujoco/rollout.py | 113 +++-- python/mujoco/rollout_test.py | 199 +++++---- python/mujoco/specs_test.py | 35 +- python/mujoco/viewer.py | 57 ++- python/pyproject.toml | 21 + python/setup.py | 103 +++-- 41 files changed, 895 insertions(+), 480 deletions(-) diff --git a/STYLEGUIDE.md b/STYLEGUIDE.md index c0f12f52..dfecb4ab 100644 --- a/STYLEGUIDE.md +++ b/STYLEGUIDE.md @@ -6,7 +6,7 @@ possible in your code contributions. ### Scope of this guide -MuJoCo has three main code categories: +Most of this guide involves C/C++ code. For Python, jump to the section [below](#python-code). For MuJoCo C/C++, code has three main categories: 1. **C code:** MuJoCo's core codebase. It consists of public headers under `include/` and C source files and internal headers under `src/`. This style @@ -158,3 +158,7 @@ example above. New code should use the C99 convention. When editing an existing function, please move existing variable declarations into local scope. Pull requests helping us to complete the migration are very welcome. + +### [Python code](#python-code) + +For Python code, run `pyink foo.py` to adhere to Google's [Python style guide](https://google.github.io/styleguide/pyguide.html). For sorting and cleaning imports, run `isort foo.py`. Both `pyink` and `isort` can be pip installed via `pip install pyink isort`. diff --git a/mjx/mujoco/mjx/_src/collision_convex.py b/mjx/mujoco/mjx/_src/collision_convex.py index b6876c9d..465ec2bb 100644 --- a/mjx/mujoco/mjx/_src/collision_convex.py +++ b/mjx/mujoco/mjx/_src/collision_convex.py @@ -906,7 +906,8 @@ def _sat_gaussmap( return edge_axis * sign, degenerate_edge_axis edge_axes, degenerate_edge_axes = jax.vmap(get_normals)( - edge_a_dir, edge_a_pt, edge_b_dir) + edge_a_dir, edge_a_pt, edge_b_dir + ) edge_dist = jax.vmap(jp.dot)(edge_axes, edge_b_pt - edge_a_pt) # handle degenerate axis edge_dist = jp.where(degenerate_edge_axes, -jp.inf, edge_dist) @@ -928,11 +929,14 @@ def _sat_gaussmap( dist, ) a_closest, b_closest = math.closest_segment_to_segment_points( - edge_a_pt[best_edge_idx], edge_a_pt_2[best_edge_idx], - edge_b_pt[best_edge_idx], edge_b_pt_2[best_edge_idx]) + edge_a_pt[best_edge_idx], + edge_a_pt_2[best_edge_idx], + edge_b_pt[best_edge_idx], + edge_b_pt_2[best_edge_idx], + ) pos = jp.where( - is_edge_contact, - jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos) + is_edge_contact, jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos + ) return dist, pos, normal diff --git a/mjx/mujoco/mjx/_src/collision_driver.py b/mjx/mujoco/mjx/_src/collision_driver.py index 129bcf2a..7b947ce1 100644 --- a/mjx/mujoco/mjx/_src/collision_driver.py +++ b/mjx/mujoco/mjx/_src/collision_driver.py @@ -146,13 +146,13 @@ def geom_pairs( b_end = b_start + m.body_geomnum for b1 in range(m.nbody): - if not geom_con[b_start[b1]:b_end[b1]].any(): + if not geom_con[b_start[b1] : b_end[b1]].any(): continue w1 = m.body_weldid[b1] w1_p = m.body_weldid[m.body_parentid[w1]] for b2 in range(b1, m.nbody): - if not geom_con[b_start[b2]:b_end[b2]].any(): + if not geom_con[b_start[b2] : b_end[b2]].any(): continue signature = (b1 << 16) + (b2) if signature in exclude_signature: @@ -272,7 +272,7 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]: jp.clip(m.pair_friction[ip], a_min=eps), m.pair_solref[ip], m.pair_solreffriction[ip], - m.pair_solimp[ip] + m.pair_solimp[ip], )) if geom1.size > 0 and geom2.size > 0: # other contacts get their params from geom fields diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index d5c893d0..068856d7 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -218,7 +218,8 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5) + dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5 + ) _ELLIPSOID_ELLIPSOID = """ @@ -240,7 +241,8 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5) + dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5 + ) _ELLIPSOID_SPHERE = """ @@ -263,7 +265,8 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3) + dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3 + ) _ELLIPSOID_CAPSULE = """ @@ -285,7 +288,8 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3) + dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3 + ) _ELLIPSOID_CYLINDER = """ @@ -308,7 +312,8 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4) + dx.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4 + ) class CapsuleCollisionTest(parameterized.TestCase): @@ -550,7 +555,8 @@ class CylinderTest(absltest.TestCase): # cylinder is vertical xml = self._CYLINDER_PLANE.replace( - ' 0).all()) diff --git a/mjx/mujoco/mjx/_src/collision_primitive.py b/mjx/mujoco/mjx/_src/collision_primitive.py index f5757853..eebd3eb3 100644 --- a/mjx/mujoco/mjx/_src/collision_primitive.py +++ b/mjx/mujoco/mjx/_src/collision_primitive.py @@ -29,6 +29,7 @@ from mujoco.mjx._src.types import Model def collider(ncon: int): """Wraps collision functions for use by collision_driver.""" + def wrapper(func): def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision: g1, g2 = geom.T @@ -119,7 +120,7 @@ def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Collision: # 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] + vec / len_ * cylinder.size[0], ) # project vector on normal @@ -138,11 +139,15 @@ def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Collision: 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, - ]) + 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 diff --git a/mjx/mujoco/mjx/_src/collision_sdf.py b/mjx/mujoco/mjx/_src/collision_sdf.py index 43bcff21..1874211d 100644 --- a/mjx/mujoco/mjx/_src/collision_sdf.py +++ b/mjx/mujoco/mjx/_src/collision_sdf.py @@ -41,6 +41,7 @@ SDFFn = Callable[[jax.Array], jax.Array] def collider(ncon: int): """Wraps collision functions for use by collision_driver.""" + def wrapper(func): def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision: g1, g2 = geom.T @@ -81,7 +82,7 @@ def _capsule(pos: jax.Array, size: jax.Array): def _ellipsoid(pos: jax.Array, size: jax.Array) -> jax.Array: k0 = math.norm(pos / size) - k1 = math.norm(pos / (size*size)) + k1 = math.norm(pos / (size * size)) return k0 * (k0 - 1.0) / (k1 + (k1 == 0.0) * 1e-12) @@ -96,12 +97,12 @@ def _cylinder(pos: jax.Array, size: jax.Array) -> jax.Array: def _cylinder_grad(x: jax.Array, size: jax.Array) -> jax.Array: """Gradient of the cylinder SDF wrt query point and singularities removed.""" - c = jp.sqrt(x[0]*x[0]+x[1]*x[1]) + c = jp.sqrt(x[0] * x[0] + x[1] * x[1]) e = jp.abs(x[2]) a = jp.array([c - size[0], e - size[1]]) b = jp.array([jp.maximum(a[0], 0), jp.maximum(a[1], 0)]) j = jp.argmax(a) - bnorm = jp.sqrt(b[0]*b[0] + b[1]*b[1]) + bnorm = jp.sqrt(b[0] * b[0] + b[1] * b[1]) bnorm += jp.allclose(bnorm, 0) * 1e-12 grada = jp.array([ x[0] / (c + jp.allclose(c, 0) * 1e-12), @@ -151,7 +152,7 @@ def _gradient_step(objective: SDFFn, state: GradientState) -> GradientState: """Performs a step of gradient descent.""" # TODO: find better parameters amin = 1e-4 # minimum value for line search factor scaling the gradient - amax = 2. # maximum value for line search factor scaling the gradient + amax = 2.0 # maximum value for line search factor scaling the gradient nlinesearch = 10 # line search points grad = jax.grad(objective)(state.x) alpha = jp.geomspace(amin, amax, nlinesearch).reshape(nlinesearch, -1) @@ -179,7 +180,11 @@ def _gradient_descent( def _optim( - d1, d2, info1: GeomInfo, info2: GeomInfo, x0: jax.Array, + d1, + d2, + info1: GeomInfo, + info2: GeomInfo, + x0: jax.Array, ) -> Collision: """Optimizes the clearance function.""" d1 = functools.partial(d1, size=info1.size) @@ -198,14 +203,14 @@ def _optim( @collider(ncon=1) def sphere_ellipsoid(s: GeomInfo, e: GeomInfo) -> Collision: - """"Calculates contact between a sphere and an ellipsoid.""" + """Calculates contact between a sphere and an ellipsoid.""" x0 = 0.5 * (s.pos + e.pos) return _optim(_sphere, _ellipsoid, s, e, x0) @collider(ncon=1) def sphere_cylinder(s: GeomInfo, c: GeomInfo) -> Collision: - """"Calculates contact between a sphere and a cylinder.""" + """Calculates contact between a sphere and a cylinder.""" # TODO: implement analytical version. x0 = 0.5 * (s.pos + c.pos) return _optim(_sphere, _cylinder, s, c, x0) @@ -213,14 +218,14 @@ def sphere_cylinder(s: GeomInfo, c: GeomInfo) -> Collision: @collider(ncon=1) def capsule_ellipsoid(c: GeomInfo, e: GeomInfo) -> Collision: - """"Calculates contact between a capsule and an ellipsoid.""" + """ "Calculates contact between a capsule and an ellipsoid.""" x0 = 0.5 * (c.pos + e.pos) return _optim(_capsule, _ellipsoid, c, e, x0) @collider(ncon=2) def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision: - """"Calculates contact between a capsule and a cylinder.""" + """Calculates contact between a capsule and a cylinder.""" # TODO: improve robustness # Near sharp corners, the SDF might give the penetration depth with respect # to a surface that is not in collision. Possible solutions is to find the @@ -235,21 +240,21 @@ def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision: @collider(ncon=1) def ellipsoid_ellipsoid(e1: GeomInfo, e2: GeomInfo) -> Collision: - """"Calculates contact between two ellipsoids.""" + """Calculates contact between two ellipsoids.""" x0 = 0.5 * (e1.pos + e2.pos) return _optim(_ellipsoid, _ellipsoid, e1, e2, x0) @collider(ncon=1) def ellipsoid_cylinder(e: GeomInfo, c: GeomInfo) -> Collision: - """"Calculates contact between and ellipsoid and a cylinder.""" + """Calculates contact between and ellipsoid and a cylinder.""" x0 = 0.5 * (e.pos + c.pos) return _optim(_ellipsoid, _cylinder, e, c, x0) @collider(ncon=4) def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision: - """"Calculates contact between a cylinder and a cylinder.""" + """Calculates contact between a cylinder and a cylinder.""" # TODO: improve robustness # Near sharp corners, the SDF might give the penetration depth with respect # to a surface that is not in collision. Possible solutions is to find the diff --git a/mjx/mujoco/mjx/_src/collision_types.py b/mjx/mujoco/mjx/_src/collision_types.py index e7af8f23..44957711 100644 --- a/mjx/mujoco/mjx/_src/collision_types.py +++ b/mjx/mujoco/mjx/_src/collision_types.py @@ -73,6 +73,7 @@ class FunctionKey: resulting constraint jacobian is determined at compile time. subgrid_size: the size determines the hfield subgrid to collide with """ + types: Tuple[int, int] data_ids: Tuple[int, int] condim: int diff --git a/mjx/mujoco/mjx/_src/constraint.py b/mjx/mujoco/mjx/_src/constraint.py index 158d9157..e778bcd8 100644 --- a/mjx/mujoco/mjx/_src/constraint.py +++ b/mjx/mujoco/mjx/_src/constraint.py @@ -39,6 +39,7 @@ import numpy as np class _Efc(PyTreeNode): """Support data for creating constraint matrices.""" + J: jax.Array pos_aref: jax.Array pos_imp: jax.Array diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index 76d9bc43..4e53fb13 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -30,7 +30,7 @@ _TOLERANCE = 5e-5 def _assert_eq(a, b, name): - tol = _TOLERANCE * 10 # avoid test noise + tol = _TOLERANCE * 10 # avoid test noise err_msg = f'mismatch: {name}' np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) @@ -75,7 +75,7 @@ class ConstraintTest(parameterized.TestCase): _assert_eq(0, dx.efc_aref[order][d.nefc :], 'efc_aref') _assert_eq(d.efc_D, dx.efc_D[order][: d.nefc], 'efc_D') _assert_eq(d.efc_pos, dx.efc_pos[order][: d.nefc], 'efc_pos') - _assert_eq(dx.efc_pos[order][d.nefc:], 0, 'efc_pos') + _assert_eq(dx.efc_pos[order][d.nefc :], 0, 'efc_pos') _assert_eq( d.efc_frictionloss, dx.efc_frictionloss[order][: d.nefc], diff --git a/mjx/mujoco/mjx/_src/dataclasses.py b/mjx/mujoco/mjx/_src/dataclasses.py index a96bb966..0db4a141 100644 --- a/mjx/mujoco/mjx/_src/dataclasses.py +++ b/mjx/mujoco/mjx/_src/dataclasses.py @@ -16,7 +16,6 @@ import copy import dataclasses - import typing from typing import Dict, Optional, Sequence, Tuple, TypeVar, Union import jax @@ -57,7 +56,7 @@ def dataclass(clz: _T) -> _T: meta_fields.append(field) def replace(self, **updates): - """"Returns a new object replacing the specified fields with new values.""" + """Returns a new object replacing the specified fields with new values.""" return dataclasses.replace(self, **updates) data_clz.replace = replace diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 9e09a3f6..7ace198b 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -34,6 +34,7 @@ def _strip_weak_type(tree): if isinstance(leaf, jax.Array): return leaf.astype(jax.dtypes.canonicalize_dtype(leaf.dtype)) return leaf + return jax.tree_util.tree_map(f, tree) @@ -95,7 +96,7 @@ def put_model( m: the model to put onto device device: which device to use - if unspecified picks the default device _full_compat: put all MjModel fields onto device irrespective of MJX support - This is an experimental feature. Avoid using it for now. + This is an experimental feature. Avoid using it for now. Returns: an mjx.Model placed on device @@ -215,8 +216,8 @@ def make_data( m: the model to use device: which device to use - if unspecified picks the default device _full_compat: create all MjData fields on device irrespective of MJX support - This is an experimental feature. Avoid using it for now. - If using this flag, also use _full_compat for put_model. + This is an experimental feature. Avoid using it for now. If using this + flag, also use _full_compat for put_model. Returns: an initialized mjx.Data placed on device @@ -383,7 +384,7 @@ def make_data( contact=contact, efc_type=efc_type, eq_active=m.eq_active0, - **zero_fields + **zero_fields, ) return d @@ -556,8 +557,8 @@ def put_data( d: the data to put on device device: which device to use - if unspecified picks the default device _full_compat: put all MjModel fields onto device irrespective of MJX support - This is an experimental feature. Avoid using it for now. - If using this flag, also use _full_compat for put_model. + This is an experimental feature. Avoid using it for now. If using this + flag, also use _full_compat for put_model. Returns: an mjx.Data placed on device @@ -646,7 +647,7 @@ def put_data( if num_rows > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: num_rows = (num_rows - 1) * 2 efc_i, efc_o = d.contact.efc_address[id_from], efc_address[id_to] - value[efc_o:efc_o + num_rows] = fields[fname][efc_i:efc_i + num_rows] + value[efc_o : efc_o + num_rows] = fields[fname][efc_i : efc_i + num_rows] fields[fname] = value diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 467b9020..8950c677 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -92,12 +92,11 @@ _MULTIPLE_CONSTRAINTS = """ class ModelIOTest(parameterized.TestCase): """IO tests for mjx.Model.""" - @parameterized.parameters( - _MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS - ) + @parameterized.parameters(_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS) def test_put_model(self, xml): m = mujoco.MjModel.from_xml_string(xml) mx = mjx.put_model(m) + def assert_not_weak_type(x): if isinstance(x, jax.Array): assert not x.weak_type diff --git a/mjx/mujoco/mjx/_src/math.py b/mjx/mujoco/mjx/_src/math.py index 00c9f7fd..c7fffbcf 100644 --- a/mjx/mujoco/mjx/_src/math.py +++ b/mjx/mujoco/mjx/_src/math.py @@ -28,6 +28,7 @@ def matmul_unroll(a: jax.Array, b: jax.Array) -> jax.Array: Args: a: left hand of matmul operand b: right hand of matmul operand + Returns: the matrix product of the inputs. """ diff --git a/mjx/mujoco/mjx/_src/mesh.py b/mjx/mujoco/mjx/_src/mesh.py index 31f45571..4c60ee1e 100644 --- a/mjx/mujoco/mjx/_src/mesh.py +++ b/mjx/mujoco/mjx/_src/mesh.py @@ -172,8 +172,8 @@ def _merge_coplanar( # resize faces that exceed max polygon vertices if face.shape[0] > _MAX_HULL_FACE_VERTICES: - name = m.names[m.name_meshadr[meshid]:] - name = name[:name.find(b'\x00')].decode('utf-8') + name = m.names[m.name_meshadr[meshid] :] + name = name[: name.find(b'\x00')].decode('utf-8') warnings.warn( f'Mesh "{name}" has a coplanar face with more than ' f'{_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance ' diff --git a/mjx/mujoco/mjx/_src/mesh_test.py b/mjx/mujoco/mjx/_src/mesh_test.py index 235348cd..f50bffc6 100644 --- a/mjx/mujoco/mjx/_src/mesh_test.py +++ b/mjx/mujoco/mjx/_src/mesh_test.py @@ -53,8 +53,9 @@ class MeshTest(absltest.TestCase): map_ = {v: k for k, v in enumerate(vidx)} h_face = np.vectorize(map_.get)(convex_face) face_verts = sorted([tuple(sorted(set(s))) for s in h_face.tolist()]) - expected_face_verts = sorted([ - (0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)]) + expected_face_verts = sorted( + [(0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)] + ) self.assertSequenceEqual( face_verts, expected_face_verts, diff --git a/mjx/mujoco/mjx/_src/passive.py b/mjx/mujoco/mjx/_src/passive.py index 0975b129..c289690f 100644 --- a/mjx/mujoco/mjx/_src/passive.py +++ b/mjx/mujoco/mjx/_src/passive.py @@ -31,6 +31,7 @@ from mujoco.mjx._src.types import Model def _spring_damper(m: Model, d: Data) -> jax.Array: """Applies joint level spring and damping forces.""" + def fn(jnt_typs, stiffness, qpos_spring, qpos): qpos_i = 0 qfrcs = [] diff --git a/mjx/mujoco/mjx/_src/ray.py b/mjx/mujoco/mjx/_src/ray.py index 6d000373..88db31be 100644 --- a/mjx/mujoco/mjx/_src/ray.py +++ b/mjx/mujoco/mjx/_src/ray.py @@ -269,7 +269,7 @@ def ray( geom_filter_dyn = (m.geom_matid != -1) | (m.geom_rgba[:, 3] != 0) geom_filter_dyn &= (m.geom_matid == -1) | (m.mat_rgba[m.geom_matid, 3] != 0) for geom_type, fn in _RAY_FUNC.items(): - id_, = np.nonzero(geom_filter & (m.geom_type == geom_type)) + (id_,) = np.nonzero(geom_filter & (m.geom_type == geom_type)) if id_.size == 0: continue diff --git a/mjx/mujoco/mjx/_src/scan.py b/mjx/mujoco/mjx/_src/scan.py index cbc9bd0e..496ba4cd 100644 --- a/mjx/mujoco/mjx/_src/scan.py +++ b/mjx/mujoco/mjx/_src/scan.py @@ -144,13 +144,11 @@ def _check_input(m: Model, args: Any, in_types: str) -> None: } for idx, (arg, typ) in enumerate(zip(args, in_types)): if len(arg) != size[typ]: - raise IndexError( - ( - f'f argument "{idx}" with type "{typ}" has length "{len(arg)}"' - f' which does not match the in_types[{idx}] expected length of ' - f'"{size[typ]}".' - ) - ) + raise IndexError(( + f'f argument "{idx}" with type "{typ}" has length "{len(arg)}"' + f' which does not match the in_types[{idx}] expected length of ' + f'"{size[typ]}".' + )) def _check_output( @@ -158,13 +156,11 @@ def _check_output( ) -> None: """Checks that scan output has the right shape.""" if y.shape[0] != take_ids.shape[0]: - raise IndexError( - ( - f'f output "{idx}" with type "{typ}" has shape "{y.shape[0]}" ' - f'which does not match the out_types[{idx}] expected size of' - f' "{take_ids.shape[0]}".' - ) - ) + raise IndexError(( + f'f output "{idx}" with type "{typ}" has shape "{y.shape[0]}" ' + f'which does not match the out_types[{idx}] expected size of' + f' "{take_ids.shape[0]}".' + )) def flat( @@ -400,7 +396,7 @@ def body_tree( if t == 'b': continue elif t == 'j': - key += (tuple(m.jnt_type[np.nonzero(m.jnt_bodyid == id_)[0]])) + key += tuple(m.jnt_type[np.nonzero(m.jnt_bodyid == id_)[0]]) elif t == 'v': key += (len(np.nonzero(m.dof_bodyid == id_)[0]),) elif t == 'q': diff --git a/mjx/mujoco/mjx/_src/scan_test.py b/mjx/mujoco/mjx/_src/scan_test.py index edb57339..74bf3224 100644 --- a/mjx/mujoco/mjx/_src/scan_test.py +++ b/mjx/mujoco/mjx/_src/scan_test.py @@ -90,6 +90,7 @@ class ScanTest(absltest.TestCase): if tuple(jnt_types) == (JointType.FREE,): return None return val + sum(jnt_types) + b_expect = jp.array([[0, 0], [3, 3], [8, 8]]) b_out = scan.flat(m, no_free, 'jb', 'b', m.jnt_type, b_in) np.testing.assert_equal(np.array(b_out), np.array(b_expect)) @@ -99,6 +100,7 @@ class ScanTest(absltest.TestCase): if jnt_types.size == 0: self.fail('world has no dofs, should not be called') return val + sum(jnt_types) + v_in = jp.ones((m.nv, 1)) scan.flat(m, no_world, 'jv', 'v', m.jnt_type, v_in) @@ -141,6 +143,7 @@ class ScanTest(absltest.TestCase): return None carry = jp.zeros_like(val) if carry is None else carry return carry + val + sum(jnt_types) + b_expect = jp.array([[0, 0], [3, 3], [8, 8]]) b_out = scan.body_tree(m, no_free, 'jb', 'b', m.jnt_type, b_in) np.testing.assert_equal(np.array(b_out), np.array(b_expect)) diff --git a/mjx/mujoco/mjx/_src/sensor_test.py b/mjx/mujoco/mjx/_src/sensor_test.py index 37b1e13e..fce6225c 100644 --- a/mjx/mujoco/mjx/_src/sensor_test.py +++ b/mjx/mujoco/mjx/_src/sensor_test.py @@ -17,13 +17,11 @@ from absl.testing import absltest from absl.testing import parameterized import jax - from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import test_util from mujoco.mjx._src.types import ConeType - import numpy as np # tolerance for difference between MuJoCo and MJX smooth calculations - mostly diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index dd04ee0d..9ad010c1 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -334,7 +334,7 @@ def factor_m(m: Model, d: Data) -> Data: pivots = [] out = [] - for (b, e, madr_d, madr_ij) in updates: + for b, e, madr_d, madr_ij in updates: width = e - b rows.append(np.arange(madr_ij, madr_ij + width)) madr_ijs.append(np.full((width,), madr_ij)) @@ -511,7 +511,6 @@ def subtree_vel(m: Model, d: Data) -> Data: angmom_child, mom_parent_child = carry return angmom + mom + angmom_child + mom_parent_child, mom_parent - subtree_angmom, _ = scan.body_tree( m, _subtree_angmom, @@ -535,6 +534,7 @@ def subtree_vel(m: Model, d: Data) -> Data: def rne(m: Model, d: Data) -> Data: """Computes inverse dynamics using the recursive Newton-Euler algorithm.""" + # forward scan over tree: accumulate link center of mass acceleration def cacc_fn(cacc, cdof_dot, qvel): if cacc is None: diff --git a/mjx/mujoco/mjx/_src/solver.py b/mjx/mujoco/mjx/_src/solver.py index 52db56f7..6d3d4f21 100644 --- a/mjx/mujoco/mjx/_src/solver.py +++ b/mjx/mujoco/mjx/_src/solver.py @@ -52,6 +52,7 @@ class _Context(PyTreeNode): u: friction cone (normal and tangents) (num(con.dim > 1), 6) h: cone hessian (num(con.dim > 1), 6, 6) """ + qacc: jax.Array qfrc_constraint: jax.Array Jaref: jax.Array # pylint: disable=invalid-name @@ -225,6 +226,7 @@ class _LSContext(PyTreeNode): def _while_loop_scan(cond_fun, body_fun, init_val, max_iter): """Scan-based implementation (jit ok, reverse-mode autodiff ok).""" + def _iter(val): next_val = body_fun(val) next_cond = cond_fun(next_val) @@ -382,7 +384,7 @@ def _update_gradient(m: Model, d: Data, ctx: _Context) -> _Context: # set efc of cone H along diagonal for i, (condim, addr) in enumerate(zip(dim, efc_address)): h_cone = ctx.h[i, :condim, :condim] - cm = cm.at[addr:addr+condim, addr:addr+condim].add(h_cone) + cm = cm.at[addr : addr + condim, addr : addr + condim].add(h_cone) h = d.efc_J.T @ cm @ d.efc_J else: h = (d.efc_J.T * d.efc_D * ctx.active) @ d.efc_J diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index a5a3af01..f91ee9af 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -116,7 +116,7 @@ def efc_order(m: mujoco.MjModel, d: mujoco.MjData, dx: Data) -> np.ndarray: if dx.contact.dim[i] > 1 and m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL: num_rows = (dx.contact.dim[i] - 1) * 2 if dx.contact.dist[i] > 0: # move empty contacts to end - order = np.append(order, np.repeat(2 ** 16, num_rows)) + order = np.append(order, np.repeat(2**16, num_rows)) continue contact_match = (d.contact.geom == dx.contact.geom[i]).all(axis=-1) contact_match &= (d.contact.pos == dx.contact.pos[i]).all(axis=-1) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 83fe9f4a..4ec13c72 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -47,6 +47,7 @@ class DisableBit(enum.IntFlag): REFSAFE: integrator safety: make ref[0]>=2*timestep SENSOR: sensors """ + CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT EQUALITY = mujoco.mjtDisableBit.mjDSBL_EQUALITY FRICTIONLOSS = mujoco.mjtDisableBit.mjDSBL_FRICTIONLOSS @@ -73,6 +74,7 @@ class JointType(enum.IntEnum): SLIDE: sliding distance along body-fixed axis (1,) HINGE: rotation angle (rad) around body-fixed axis (1,) """ + FREE = mujoco.mjtJoint.mjJNT_FREE BALL = mujoco.mjtJoint.mjJNT_BALL SLIDE = mujoco.mjtJoint.mjJNT_SLIDE @@ -93,6 +95,7 @@ class IntegratorType(enum.IntEnum): RK4: 4th-order Runge Kutta IMPLICITFAST: implicit in velocity, no rne derivative """ + EULER = mujoco.mjtIntegrator.mjINT_EULER RK4 = mujoco.mjtIntegrator.mjINT_RK4 IMPLICITFAST = mujoco.mjtIntegrator.mjINT_IMPLICITFAST @@ -113,6 +116,7 @@ class GeomType(enum.IntEnum): MESH: mesh SDF: signed distance field """ + PLANE = mujoco.mjtGeom.mjGEOM_PLANE HFIELD = mujoco.mjtGeom.mjGEOM_HFIELD SPHERE = mujoco.mjtGeom.mjGEOM_SPHERE @@ -134,6 +138,7 @@ class ConvexMesh(PyTreeNode): edge: edge indexes for all edges in the convex mesh edge_face_normal: indexes for face normals adjacent to edges in `edge` """ + vert: jax.Array face: jax.Array face_normal: jax.Array @@ -148,6 +153,7 @@ class ConeType(enum.IntEnum): PYRAMIDAL: pyramidal ELLIPTIC: elliptic """ + PYRAMIDAL = mujoco.mjtCone.mjCONE_PYRAMIDAL ELLIPTIC = mujoco.mjtCone.mjCONE_ELLIPTIC @@ -160,6 +166,7 @@ class JacobianType(enum.IntEnum): SPARSE: sparse AUTO: sparse if nv>60 and device is TPU, dense otherwise """ + DENSE = mujoco.mjtJacobian.mjJAC_DENSE SPARSE = mujoco.mjtJacobian.mjJAC_SPARSE AUTO = mujoco.mjtJacobian.mjJAC_AUTO @@ -172,6 +179,7 @@ class SolverType(enum.IntEnum): CG: Conjugate gradient (primal) NEWTON: Newton (primal) """ + # unsupported: PGS CG = mujoco.mjtSolver.mjSOL_CG NEWTON = mujoco.mjtSolver.mjSOL_NEWTON @@ -186,6 +194,7 @@ class EqType(enum.IntEnum): JOINT: couple the values of two scalar joints with cubic TENDON: couple the lengths of two tendons with cubic """ + CONNECT = mujoco.mjtEq.mjEQ_CONNECT WELD = mujoco.mjtEq.mjEQ_WELD JOINT = mujoco.mjtEq.mjEQ_JOINT @@ -203,6 +212,7 @@ class WrapType(enum.IntEnum): SPHERE: wrap around sphere CYLINDER: wrap around (infinite) cylinder """ + JOINT = mujoco.mjtWrap.mjWRAP_JOINT PULLEY = mujoco.mjtWrap.mjWRAP_PULLEY SITE = mujoco.mjtWrap.mjWRAP_SITE @@ -219,6 +229,7 @@ class TrnType(enum.IntEnum): TENDON: force on tendon SITE: force on site """ + JOINT = mujoco.mjtTrn.mjTRN_JOINT JOINTINPARENT = mujoco.mjtTrn.mjTRN_JOINTINPARENT SITE = mujoco.mjtTrn.mjTRN_SITE @@ -236,6 +247,7 @@ class DynType(enum.IntEnum): FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration MUSCLE: piece-wise linear filter with two time constants """ + NONE = mujoco.mjtDyn.mjDYN_NONE INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR FILTER = mujoco.mjtDyn.mjDYN_FILTER @@ -252,6 +264,7 @@ class GainType(enum.IntEnum): AFFINE: const + kp*length + kv*velocity MUSCLE: muscle FLV curve computed by muscle_gain """ + FIXED = mujoco.mjtGain.mjGAIN_FIXED AFFINE = mujoco.mjtGain.mjGAIN_AFFINE MUSCLE = mujoco.mjtGain.mjGAIN_MUSCLE @@ -266,6 +279,7 @@ class BiasType(enum.IntEnum): AFFINE: const + kp*length + kv*velocity MUSCLE: muscle passive force computed by muscle_bias """ + NONE = mujoco.mjtBias.mjBIAS_NONE AFFINE = mujoco.mjtBias.mjBIAS_AFFINE MUSCLE = mujoco.mjtBias.mjBIAS_MUSCLE @@ -282,6 +296,7 @@ class ConstraintType(enum.IntEnum): CONTACT_FRICTIONLESS: frictionless contact CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone """ + EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY FRICTION_DOF = mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF FRICTION_TENDON = mujoco.mjtConstraint.mjCNSTR_FRICTION_TENDON @@ -302,6 +317,7 @@ class CamLightType(enum.IntEnum): TARGETBODY: pos fixed in body, rot tracks target body TARGETBODYCOM: pos fixed in body, rot tracks target subtree com """ + FIXED = mujoco.mjtCamLight.mjCAMLIGHT_FIXED TRACK = mujoco.mjtCamLight.mjCAMLIGHT_TRACK TRACKCOM = mujoco.mjtCamLight.mjCAMLIGHT_TRACKCOM @@ -346,6 +362,7 @@ class SensorType(enum.IntEnum): FRAMELINACC: 3D linear acceleration FRAMEANGACC: 3D angular acceleration """ + MAGNETOMETER = mujoco.mjtSensor.mjSENS_MAGNETOMETER CAMPROJECTION = mujoco.mjtSensor.mjSENS_CAMPROJECTION RANGEFINDER = mujoco.mjtSensor.mjSENS_RANGEFINDER @@ -391,6 +408,7 @@ class ObjType(PyTreeNode): SITE: site CAMERA: camera """ + UNKNOWN = mujoco.mjtObj.mjOBJ_UNKNOWN BODY = mujoco.mjtObj.mjOBJ_BODY XBODY = mujoco.mjtObj.mjOBJ_XBODY @@ -437,7 +455,7 @@ class Option(PyTreeNode): disableactuator: bit flags for disabling actuators by group id (not used) sdf_initpoints: number of starting points for gradient descent (not used) sdf_iterations: max number of iterations for gradient descent (not used) - """ + """ # fmt: skip timestep: jax.Array apirate: jax.Array = _restricted_to('mujoco') impratio: jax.Array @@ -480,6 +498,7 @@ class Statistic(PyTreeNode): extent: spatial extent (not used) center: center of model (not used) """ + meaninertia: jax.Array meanmass: jax.Array meansize: jax.Array @@ -813,6 +832,7 @@ class Model(PyTreeNode): name_keyadr: keyframe name pointers (nkey,) names: names of all objects, 0-terminated (nnames,) """ + nq: int nv: int nu: int @@ -1161,7 +1181,7 @@ class Contact(PyTreeNode): geom2: id of geom 2; deprecated, use geom[1] geom: geom ids (2,) efc_address: address in efc; -1: not included - """ + """ # fmt: skip dist: jax.Array pos: jax.Array frame: jax.Array @@ -1306,7 +1326,7 @@ class Data(PyTreeNode): _qM_sparse: qM in sparse representation (nM,) _qLD_sparse: qLD in sparse representation (nM,) _qLDiagInv_sparse: qLDiagInv in sparse representation (nv,) - """ + """ # fmt: skip # constant sizes: ne: int nf: int diff --git a/mjx/mujoco/mjx/testspeed.py b/mjx/mujoco/mjx/testspeed.py index 9ccba845..4d59ad33 100644 --- a/mjx/mujoco/mjx/testspeed.py +++ b/mjx/mujoco/mjx/testspeed.py @@ -22,7 +22,9 @@ from etils import epath import mujoco from mujoco import mjx -_MJCF = flags.DEFINE_string('mjcf', None, 'path to model `.xml` or `.mjb`', required=True) +_MJCF = flags.DEFINE_string( + 'mjcf', None, 'path to model `.xml` or `.mjb`', required=True +) _BASE_PATH = flags.DEFINE_string( 'base_path', None, 'base path, defaults to mujoco.mjx resource path' ) diff --git a/mjx/mujoco/mjx/viewer.py b/mjx/mujoco/mjx/viewer.py index 7ad564c9..5cdce300 100644 --- a/mjx/mujoco/mjx/viewer.py +++ b/mjx/mujoco/mjx/viewer.py @@ -28,8 +28,9 @@ import mujoco.viewer _JIT = flags.DEFINE_bool('jit', True, 'To jit or not to jit.') -_MODEL_PATH = flags.DEFINE_string('mjcf', None, 'Path to a MuJoCo MJCF file.', - required=True) +_MODEL_PATH = flags.DEFINE_string( + 'mjcf', None, 'Path to a MuJoCo MJCF file.', required=True +) _VIEWER_GLOBAL_STATE = { diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index a5370380..4d276d40 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -44,3 +44,24 @@ Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" Documentation = "https://mujoco.readthedocs.io/en/3.2.7" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html" + +[tool.isort] +force_single_line = true +force_sort_within_sections = true +lexicographical = true +single_line_exclusions = ["typing"] +order_by_type = false +group_by_package = true +line_length = 120 +use_parentheses = true +multi_line_output = 3 +skip_glob = ["**/*.ipynb"] + +[tool.pyink] +line-length = 80 +unstable = true +pyink-indentation = 2 +pyink-use-majority-quotes = true +extend-exclude = '''( + .ipynb$ +)''' diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index ffdfb75b..daed5d2c 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -120,13 +120,17 @@ class MuJoCoBindingsTest(parameterized.TestCase): xml_2 = rb"""""" xml_3 = rb"""""" model = mujoco.MjModel.from_xml_string( - xml_1, {'model_.xml': xml_2, 'model__.xml': xml_3}) + xml_1, {'model_.xml': xml_2, 'model__.xml': xml_3} + ) self.assertEqual( - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'plane'), 0) + mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'plane'), 0 + ) self.assertEqual( - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'box'), 1) + mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'box'), 1 + ) self.assertEqual( - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'ball'), 2) + mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, 'ball'), 2 + ) def test_load_xml_repeated_asset_name(self): # Assets aren't allowed to have the same filename (even if they have @@ -139,23 +143,25 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_can_read_array(self): np.testing.assert_array_equal( self.model.body_pos, - [[0, 0, 0], [0, 0, 0.1], [0, 0, 0], [0, 0, 0], [42.0, 0, 42.0]]) + [[0, 0, 0], [0, 0, 0.1], [0, 0, 0], [0, 0, 0], [42.0, 0, 42.0]], + ) def test_can_set_array(self): self.data.qpos = 0.12345 np.testing.assert_array_equal( - self.data.qpos, [0.12345]*len(self.data.qpos)) + self.data.qpos, [0.12345] * len(self.data.qpos) + ) def test_array_is_a_view(self): qpos_ref = self.data.qpos self.data.qpos = 0.789 - np.testing.assert_array_equal( - qpos_ref, [0.789]*len(self.data.qpos)) + np.testing.assert_array_equal(qpos_ref, [0.789] * len(self.data.qpos)) # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo - @absltest.skipIf(sys.implementation.name == 'pypy', - reason='requires sys.getrefcount') + @absltest.skipIf( + sys.implementation.name == 'pypy', reason='requires sys.getrefcount' + ) def test_array_keeps_struct_alive(self): model = mujoco.MjModel.from_xml_string(TEST_XML) qpos0 = model.qpos0 @@ -185,11 +191,15 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_named_indexing_actuator_ctrl(self): actuator_id = mujoco.mj_name2id( - self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'myactuator') - self.assertIs(self.data.actuator('myactuator'), - self.data.actuator(actuator_id)) - self.assertIs(self.data.actuator('myactuator').ctrl, - self.data.actuator(actuator_id).ctrl) + self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, 'myactuator' + ) + self.assertIs( + self.data.actuator('myactuator'), self.data.actuator(actuator_id) + ) + self.assertIs( + self.data.actuator('myactuator').ctrl, + self.data.actuator(actuator_id).ctrl, + ) self.assertEqual(self.data.actuator('myactuator').ctrl.shape, (1,)) # Test that the indexer is returning a view into the underlying struct. @@ -202,41 +212,49 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_named_indexing_invalid_names_in_model(self): with self.assertRaisesRegex( KeyError, - r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]"): + r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]", + ): self.model.geom('badgeom') def test_named_indexing_no_name_argument_in_model(self): with self.assertRaisesRegex( KeyError, - r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]"): + r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]", + ): self.model.joint() def test_named_indexing_invalid_names_in_data(self): with self.assertRaisesRegex( KeyError, - r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]"): + r"Invalid name 'badgeom'\. Valid names: \['mybox', 'myplane'\]", + ): self.data.geom('badgeom') def test_named_indexing_no_name_argument_in_data(self): with self.assertRaisesRegex( KeyError, - r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]"): + r"Invalid name ''\. Valid names: \['myball', 'myfree', 'myhinge'\]", + ): self.data.jnt() def test_named_indexing_invalid_index_in_model(self): with self.assertRaisesRegex( - IndexError, r'Invalid index 3\. Valid indices from 0 to 2'): + IndexError, r'Invalid index 3\. Valid indices from 0 to 2' + ): self.model.geom(3) with self.assertRaisesRegex( - IndexError, r'Invalid index -1\. Valid indices from 0 to 2'): + IndexError, r'Invalid index -1\. Valid indices from 0 to 2' + ): self.model.geom(-1) def test_named_indexing_invalid_index_in_data(self): with self.assertRaisesRegex( - IndexError, r'Invalid index 3\. Valid indices from 0 to 2'): + IndexError, r'Invalid index 3\. Valid indices from 0 to 2' + ): self.data.geom(3) with self.assertRaisesRegex( - IndexError, r'Invalid index -1\. Valid indices from 0 to 2'): + IndexError, r'Invalid index -1\. Valid indices from 0 to 2' + ): self.data.geom(-1) def test_named_indexing_geom_size(self): @@ -267,45 +285,53 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_named_indexing_ragged_qpos(self): balljoint_id = mujoco.mj_name2id( - self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myball') + self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myball' + ) self.assertIs(self.data.joint('myball'), self.data.joint(balljoint_id)) - self.assertIs(self.data.joint('myball').qpos, - self.data.joint(balljoint_id).qpos) + self.assertIs( + self.data.joint('myball').qpos, self.data.joint(balljoint_id).qpos + ) self.assertEqual(self.data.joint('myball').qpos.shape, (4,)) # Test that the indexer is returning a view into the underlying struct. qpos_from_indexer = self.data.joint('myball').qpos qpos_idx = self.model.jnt_qposadr[balljoint_id] - self.data.qpos[qpos_idx:qpos_idx+4] = [4, 5, 6, 7] + self.data.qpos[qpos_idx : qpos_idx + 4] = [4, 5, 6, 7] np.testing.assert_array_equal(qpos_from_indexer, [4, 5, 6, 7]) self.data.joint('myball').qpos = [9, 8, 7, 6] - np.testing.assert_array_equal(self.data.qpos[qpos_idx:qpos_idx+4], - [9, 8, 7, 6]) + np.testing.assert_array_equal( + self.data.qpos[qpos_idx : qpos_idx + 4], [9, 8, 7, 6] + ) def test_named_indexing_ragged2d_cdof(self): freejoint_id = mujoco.mj_name2id( - self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myfree') + self.model, mujoco.mjtObj.mjOBJ_JOINT, 'myfree' + ) self.assertIs(self.data.joint('myfree'), self.data.joint(freejoint_id)) - self.assertIs(self.data.joint('myfree').cdof, - self.data.joint(freejoint_id).cdof) + self.assertIs( + self.data.joint('myfree').cdof, self.data.joint(freejoint_id).cdof + ) self.assertEqual(self.data.joint('myfree').cdof.shape, (6, 6)) # Test that the indexer is returning a view into the underlying struct. cdof_from_indexer = self.data.joint('myfree').cdof dof_idx = self.model.jnt_dofadr[freejoint_id] - self.data.cdof[dof_idx:dof_idx+6, :] = np.reshape(range(36), (6, 6)) - np.testing.assert_array_equal(cdof_from_indexer, - np.reshape(range(36), (6, 6))) + self.data.cdof[dof_idx : dof_idx + 6, :] = np.reshape(range(36), (6, 6)) + np.testing.assert_array_equal( + cdof_from_indexer, np.reshape(range(36), (6, 6)) + ) self.data.joint('myfree').cdof = 42 - np.testing.assert_array_equal(self.data.cdof[dof_idx:dof_idx+6], [[42]*6]*6) + np.testing.assert_array_equal( + self.data.cdof[dof_idx : dof_idx + 6], [[42] * 6] * 6 + ) def test_named_indexing_repr_in_data(self): - expected_repr = '''<_MjDataGeomViews + expected_repr = """<_MjDataGeomViews id: 1 name: 'mybox' xmat: array([0., 0., 0., 0., 0., 0., 0., 0., 0.]) xpos: array([0., 0., 0.]) ->''' +>""" self.assertEqual(expected_repr, repr(self.data.geom('mybox'))) def test_named_indexing_body_repr_in_data(self): @@ -328,8 +354,15 @@ class MuJoCoBindingsTest(parameterized.TestCase): self.assertGreater(self.data._address, 0) self.assertGreater(model2._address, 0) self.assertGreater(data2._address, 0) - self.assertLen({self.model._address, self.data._address, - model2._address, data2._address}, 4) + self.assertLen( + { + self.model._address, + self.data._address, + model2._address, + data2._address, + }, + 4, + ) def test_mjmodel_can_read_and_write_opt(self): self.assertEqual(self.model.opt.timestep, 0.002) @@ -361,7 +394,9 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_mjmodel_can_access_names_directly(self): # mjModel offers direct access to names array, to allow usecases other than # id2name - model_name = str(self.model.names[0:self.model.names.find(b'\0')], 'utf-8') + model_name = str( + self.model.names[0 : self.model.names.find(b'\0')], 'utf-8' + ) self.assertEqual(model_name, 'test') start_index = self.model.name_geomadr[0] @@ -402,15 +437,15 @@ class MuJoCoBindingsTest(parameterized.TestCase): model_copy = copy.copy(self.model) self.assertEqual( - mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_JOINT, 0), - 'myfree') + mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_JOINT, 0), 'myfree' + ) self.assertEqual( - mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 0), - 'myplane') + mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 0), 'myplane' + ) self.assertEqual( - mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 1), - 'mybox') + mujoco.mj_id2name(model_copy, mujoco.mjtObj.mjOBJ_GEOM, 1), 'mybox' + ) # Make sure it's a copy. self.model.geom_size[1] = 0.5 @@ -420,7 +455,7 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_mjdata_can_copy(self): self.data.qpos = [0, 0, 0.1*np.sqrt(2) - 0.001, np.cos(np.pi/8), np.sin(np.pi/8), 0, 0, 0, - 1, 0, 0, 0] + 1, 0, 0, 0] # fmt: skip mujoco.mj_forward(self.model, self.data) data_copy = copy.copy(self.data) @@ -455,7 +490,8 @@ class MuJoCoBindingsTest(parameterized.TestCase): contact_copy.append(copy.copy(self.data.contact[i])) # Sort contacts in anticlockwise order contact_copy = sorted( - contact_copy, key=lambda x: np.arctan2(x.pos[1], x.pos[0])) + contact_copy, key=lambda x: np.arctan2(x.pos[1], x.pos[0]) + ) np.testing.assert_allclose(contact_copy[0].pos[:2], [-0.1, -0.1]) np.testing.assert_allclose(contact_copy[1].pos[:2], [0.1, -0.1]) np.testing.assert_allclose(contact_copy[2].pos[:2], [0.1, 0.1]) @@ -502,7 +538,8 @@ class MuJoCoBindingsTest(parameterized.TestCase): # Sort contacts in anticlockwise order sorted_contact = sorted( - contact, key=lambda x: np.arctan2(x.pos[1], x.pos[0])) + contact, key=lambda x: np.arctan2(x.pos[1], x.pos[0]) + ) np.testing.assert_allclose(sorted_contact[0].pos[:2], [-0.1, -0.1]) np.testing.assert_allclose(sorted_contact[1].pos[:2], [0.1, -0.1]) np.testing.assert_allclose(sorted_contact[2].pos[:2], [0.1, 0.1]) @@ -589,7 +626,7 @@ class MuJoCoBindingsTest(parameterized.TestCase): self.assertEqual(data2.ncon, 4) self.assertEqual(data2.contact, self.data.contact) - self.data.qpos[3:7] = [np.cos(np.pi/8), np.sin(np.pi/8), 0, 0] + self.data.qpos[3:7] = [np.cos(np.pi / 8), np.sin(np.pi / 8), 0, 0] self.data.qpos[2] *= (np.sqrt(2) - 1) * 0.1 - 1e-6 mujoco.mj_forward(self.model, self.data) self.assertEqual(self.data.ncon, 2) @@ -674,7 +711,7 @@ class MuJoCoBindingsTest(parameterized.TestCase): def test_mju_rotVecQuat(self): # pylint: disable=invalid-name vec = [1, 0, 0] - quat = [np.cos(np.pi/8), 0, 0, np.sin(np.pi/8)] + quat = [np.cos(np.pi / 8), 0, 0, np.sin(np.pi / 8)] expected = np.array([1, 1, 0]) / np.sqrt(2) # Check that the output argument works, and that the binding returns None. @@ -722,7 +759,7 @@ class MuJoCoBindingsTest(parameterized.TestCase): size = mujoco.mj_stateSize(self.model, spec) state_bad_size = np.empty(size + 1, np.float64) - expected_message = ('state size should equal mj_stateSize(m, spec)') + expected_message = 'state size should equal mj_stateSize(m, spec)' with self.assertRaisesWithLiteralMatch(TypeError, expected_message): mujoco.mj_getState(self.model, self.data, state_bad_size, spec) @@ -781,8 +818,9 @@ class MuJoCoBindingsTest(parameterized.TestCase): mat = np.empty((3, 10), np.float64) mujoco.mj_angmomMat(self.model, self.data, mat, 0) - np.testing.assert_almost_equal(mat @ self.data.qvel, - self.data.subtree_angmom[0, :]) + np.testing.assert_almost_equal( + mat @ self.data.qvel, self.data.subtree_angmom[0, :] + ) def test_mj_jacSite(self): # pylint: disable=invalid-name mujoco.mj_forward(self.model, self.data) @@ -792,20 +830,22 @@ class MuJoCoBindingsTest(parameterized.TestCase): jacp = np.empty((3, 10), np.float64) mujoco.mj_jacSite(self.model, self.data, jacp, None, site_id) - expected_jacp = np.array( - [[0, 0, 0, 0, 0, 0, -1, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) + expected_jacp = np.array([ + [0, 0, 0, 0, 0, 0, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + ]) np.testing.assert_array_equal(jacp, expected_jacp) # Call mj_jacSite with only jacr. jacr = np.empty((3, 10), np.float64) mujoco.mj_jacSite(self.model, self.data, None, jacr, site_id) - expected_jacr = np.array( - [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) + expected_jacr = np.array([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + ]) np.testing.assert_array_equal(jacr, expected_jacr) # Call mj_jacSite with both jacp and jacr. @@ -818,12 +858,14 @@ class MuJoCoBindingsTest(parameterized.TestCase): # Check that the jacp argument must have the right size. with self.assertRaises(TypeError): mujoco.mj_jacSite( - self.model, self.data, np.empty((3, 6), jacp.dtype), None, site_id) + self.model, self.data, np.empty((3, 6), jacp.dtype), None, site_id + ) # Check that the jacr argument must have the right size. with self.assertRaises(TypeError): mujoco.mj_jacSite( - self.model, self.data, None, np.empty((4, 7), jacr.dtype), site_id) + self.model, self.data, None, np.empty((4, 7), jacr.dtype), site_id + ) # The following two checks need to be done with fully initialized arrays, # since pybind11 prints out the array's contents when generating TypeErrors. @@ -832,12 +874,14 @@ class MuJoCoBindingsTest(parameterized.TestCase): # Check that the jacp argument must have the right dtype. with self.assertRaises(TypeError): mujoco.mj_jacSite( - self.model, self.data, np.zeros(jacp.shape, int), None, site_id) + self.model, self.data, np.zeros(jacp.shape, int), None, site_id + ) # Check that the jacr argument must have the right dtype. with self.assertRaises(TypeError): mujoco.mj_jacSite( - self.model, self.data, None, np.zeros(jacr.shape, int), site_id) + self.model, self.data, None, np.zeros(jacr.shape, int), site_id + ) def test_docstrings(self): # pylint: disable=invalid-name self.assertEqual( @@ -845,13 +889,15 @@ class MuJoCoBindingsTest(parameterized.TestCase): """mj_versionString() -> str Return the current version of MuJoCo as a null-terminated string. -""") +""", + ) self.assertEqual( mujoco.mj_Euler.__doc__, """mj_Euler(m: mujoco._structs.MjModel, d: mujoco._structs.MjData) -> None Euler integrator, semi-implicit in velocity. -""") +""", + ) def test_float_constant(self): self.assertEqual(mujoco.mjMAXVAL, 1e10) @@ -866,17 +912,19 @@ Euler integrator, semi-implicit in velocity. self.assertLen(mujoco.mjVISSTRING, mujoco.mjtVisFlag.mjNVISFLAG) self.assertLen(mujoco.mjRNDSTRING, mujoco.mjtRndFlag.mjNRNDFLAG) self.assertEqual(mujoco.mjDISABLESTRING[11], 'Refsafe') - self.assertEqual(mujoco.mjVISSTRING[mujoco.mjtVisFlag.mjVIS_INERTIA], - ('Inertia', '0', 'I')) + self.assertEqual( + mujoco.mjVISSTRING[mujoco.mjtVisFlag.mjVIS_INERTIA], + ('Inertia', '0', 'I'), + ) def test_enum_values(self): self.assertEqual(mujoco.mjtJoint.mjJNT_FREE, 0) self.assertEqual(mujoco.mjtJoint.mjJNT_BALL, 1) self.assertEqual(mujoco.mjtJoint.mjJNT_SLIDE, 2) self.assertEqual(mujoco.mjtJoint.mjJNT_HINGE, 3) - self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1<<0) - self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1<<1) - self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1<<2) + self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) + self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) + self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) @@ -899,8 +947,9 @@ Euler integrator, semi-implicit in velocity. x = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k'] self.assertEqual(x[mujoco.mjtFrame.mjFRAME_WORLD], 'h') self.assertEqual( - x[mujoco.mjtFrame.mjFRAME_GEOM:mujoco.mjtFrame.mjFRAME_CAMERA], - ['c', 'd']) + x[mujoco.mjtFrame.mjFRAME_GEOM : mujoco.mjtFrame.mjFRAME_CAMERA], + ['c', 'd'], + ) def test_enum_ops(self): # Note: when modifying this test, make sure the enum value is an odd number @@ -909,10 +958,12 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtFrame.mjFRAME_WORLD, 7.0) self.assertEqual(7, mujoco.mjtFrame.mjFRAME_WORLD) self.assertEqual(7.0, mujoco.mjtFrame.mjFRAME_WORLD) - self.assertEqual(mujoco.mjtFrame.mjFRAME_WORLD, - mujoco.mjtFrame.mjFRAME_WORLD) - self.assertNotEqual(mujoco.mjtFrame.mjFRAME_WORLD, - mujoco.mjtFrame.mjFRAME_NONE) + self.assertEqual( + mujoco.mjtFrame.mjFRAME_WORLD, mujoco.mjtFrame.mjFRAME_WORLD + ) + self.assertNotEqual( + mujoco.mjtFrame.mjFRAME_WORLD, mujoco.mjtFrame.mjFRAME_NONE + ) self.assertEqual(-mujoco.mjtFrame.mjFRAME_WORLD, -7) self.assertIsInstance(-mujoco.mjtFrame.mjFRAME_WORLD, int) @@ -989,22 +1040,28 @@ Euler integrator, semi-implicit in velocity. self.assertEqual( mujoco.mjtDisableBit.mjDSBL_GRAVITY | mujoco.mjtDisableBit.mjDSBL_LIMIT, - 72) + 72, + ) self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE | 33, 33) self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE & 33, 32) self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE ^ 33, 1) self.assertEqual(33 | mujoco.mjtDisableBit.mjDSBL_PASSIVE, 33) self.assertEqual(33 & mujoco.mjtDisableBit.mjDSBL_PASSIVE, 32) self.assertEqual(33 ^ mujoco.mjtDisableBit.mjDSBL_PASSIVE, 1) - self.assertEqual(mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL << 1, - mujoco.mjtDisableBit.mjDSBL_WARMSTART) - self.assertEqual(mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 3, - mujoco.mjtDisableBit.mjDSBL_CONTACT) + self.assertEqual( + mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL << 1, + mujoco.mjtDisableBit.mjDSBL_WARMSTART, + ) + self.assertEqual( + mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 3, + mujoco.mjtDisableBit.mjDSBL_CONTACT, + ) def test_can_raise_error(self): self.data.pstack = self.data.narena - with self.assertRaisesRegex(mujoco.FatalError, - r'\Amj_stackAlloc: insufficient memory:'): + with self.assertRaisesRegex( + mujoco.FatalError, r'\Amj_stackAlloc: insufficient memory:' + ): mujoco.mj_forward(self.model, self.data) def test_mjcb_time(self): @@ -1042,7 +1099,8 @@ Euler integrator, semi-implicit in velocity. with self.assertRaises(TestError) as e: mujoco.mj_forward(self.model, self.data) self.assertEqual( - e.exception.args, ('string', (1, 2, 3), {'a': 1, 'b': 2})) + e.exception.args, ('string', (1, 2, 3), {'a': 1, 'b': 2}) + ) # Should not raise now that we've cleared the callback. mujoco.mj_forward(self.model, self.data) @@ -1050,12 +1108,14 @@ Euler integrator, semi-implicit in velocity. def test_mjcb_time_wrong_return_type(self): with temporary_callback(mujoco.set_mjcb_time, lambda: 'string'): with self.assertRaisesWithLiteralMatch( - TypeError, 'mjcb_time callback did not return a number'): + TypeError, 'mjcb_time callback did not return a number' + ): mujoco.mj_forward(self.model, self.data) def test_mjcb_time_not_callable(self): with self.assertRaisesWithLiteralMatch( - TypeError, 'callback is not an Optional[Callable]'): + TypeError, 'callback is not an Optional[Callable]' + ): mujoco.set_mjcb_time(1) def test_mjcb_sensor(self): @@ -1088,8 +1148,9 @@ Euler integrator, semi-implicit in velocity. # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo - @absltest.skipIf(sys.implementation.name == 'pypy', - reason='requires sys.getrefcount') + @absltest.skipIf( + sys.implementation.name == 'pypy', reason='requires sys.getrefcount' + ) def test_mjcb_control_not_leak_memory(self): model_instances = [] data_instances = [] @@ -1110,8 +1171,9 @@ Euler integrator, semi-implicit in velocity. # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo - @absltest.skipIf(sys.implementation.name == 'pypy', - reason='requires sys.getrefcount') + @absltest.skipIf( + sys.implementation.name == 'pypy', reason='requires sys.getrefcount' + ) def test_mjdata_holds_ref_to_model(self): data = mujoco.MjData(mujoco.MjModel.from_xml_string('')) model = data.model @@ -1150,9 +1212,15 @@ Euler integrator, semi-implicit in velocity. # When the scene is updated, geoms are added to the scene # (ngeom is incremented) mujoco.mj_forward(self.model, self.data) - mujoco.mjv_updateScene(self.model, self.data, mujoco.MjvOption(), - None, mujoco.MjvCamera(), - mujoco.mjtCatBit.mjCAT_ALL, scene) + mujoco.mjv_updateScene( + self.model, + self.data, + mujoco.MjvOption(), + None, + mujoco.MjvCamera(), + mujoco.mjtCatBit.mjCAT_ALL, + scene, + ) self.assertGreater(scene.ngeom, 0) def test_mjv_scene_without_model(self): @@ -1164,10 +1232,19 @@ Euler integrator, semi-implicit in velocity. # mj_ray has tricky argument types geomid = np.zeros(1, np.int32) mujoco.mj_forward(self.model, self.data) - mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, - geomid) - mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1], - [0, 0, 0, 0, 0, 0], 0, 0, geomid) + mujoco.mj_ray( + self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, geomid + ) + mujoco.mj_ray( + self.model, + self.data, + [0, 0, 0], + [0, 0, 1], + [0, 0, 0, 0, 0, 0], + 0, + 0, + geomid, + ) # Check that named arguments work mujoco.mj_ray( m=self.model, @@ -1177,7 +1254,8 @@ Euler integrator, semi-implicit in velocity. geomgroup=None, flg_static=0, bodyexclude=0, - geomid=geomid) + geomid=geomid, + ) def test_mj_multi_ray(self): nray = 3 @@ -1201,14 +1279,13 @@ Euler integrator, semi-implicit in velocity. geomid=geomid, dist=dist, nray=nray, - cutoff=mujoco.mjMAXVAL) + cutoff=mujoco.mjMAXVAL, + ) for i in range(0, 3): self.assertEqual( dist[i], - mujoco.mj_ray( - self.model, self.data, pnt, vec[i], None, 1, -1, geom1 - ), + mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1), ) self.assertEqual(geomid[i], geom1) self.assertEqual(geomid[i], geom_ex[i]) @@ -1217,16 +1294,28 @@ Euler integrator, semi-implicit in velocity. def test_inverse_fd_none(self): eps = 1e-6 flg_centered = 0 - mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered, - None, None, None, None, None, None, None) + mujoco.mjd_inverseFD( + self.model, + self.data, + eps, + flg_centered, + None, + None, + None, + None, + None, + None, + None, + ) def test_geom_distance(self): mujoco.mj_forward(self.model, self.data) fromto = np.empty(6, np.float64) dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto) self.assertEqual(dist, 41.9) - np.testing.assert_array_equal(fromto, - np.array((42., 0., 0., 42., 0., 41.9))) + np.testing.assert_array_equal( + fromto, np.array((42.0, 0.0, 0.0, 42.0, 0.0, 41.9)) + ) def test_inverse_fd(self): eps = 1e-6 @@ -1238,8 +1327,19 @@ Euler integrator, semi-implicit in velocity. ds_dv = np.zeros((self.model.nv, self.model.nsensordata)) ds_da = np.zeros((self.model.nv, self.model.nsensordata)) dm_dq = np.zeros((self.model.nv, self.model.nM)) - mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered, - df_dq, df_dv, df_da, ds_dq, ds_dv, ds_da, dm_dq) + mujoco.mjd_inverseFD( + self.model, + self.data, + eps, + flg_centered, + df_dq, + df_dv, + df_da, + ds_dq, + ds_dv, + ds_da, + dm_dq, + ) self.assertGreater(np.linalg.norm(df_dq), eps) self.assertGreater(np.linalg.norm(df_dv), eps) self.assertGreater(np.linalg.norm(df_da), eps) @@ -1272,15 +1372,17 @@ Euler integrator, semi-implicit in velocity. n_total = 4 n_band = 1 n_dense = 1 - dense = np.array([[1.0, 0, 0, 0.1], - [0, 2.0, 0, 0.2], - [0, 0, 3.0, 0.3], - [0.1, 0.2, 0.3, 4.0]]) - band = np.zeros(n_band*(n_total-n_dense) + n_dense*n_total) + dense = np.array([ + [1.0, 0, 0, 0.1], + [0, 2.0, 0, 0.2], + [0, 0, 3.0, 0.3], + [0.1, 0.2, 0.3, 4.0], + ]) + band = np.zeros(n_band * (n_total - n_dense) + n_dense * n_total) mujoco.mju_dense2Band(band, dense, n_total, n_band, n_dense) for i in range(4): index = mujoco.mju_bandDiag(i, n_total, n_band, n_dense) - self.assertEqual(band[index], i+1) + self.assertEqual(band[index], i + 1) dense2 = np.zeros((n_total, n_total)) flg_sym = 1 mujoco.mju_band2Dense(dense2, band, n_total, n_band, n_dense, flg_sym) @@ -1288,20 +1390,22 @@ Euler integrator, semi-implicit in velocity. vec = np.array([[2.0], [2.0], [3.0], [4.0]]) res = np.zeros_like(vec) n_vec = 1 - mujoco.mju_bandMulMatVec(res, band, vec, - n_total, n_band, n_dense, n_vec, flg_sym) + mujoco.mju_bandMulMatVec( + res, band, vec, n_total, n_band, n_dense, n_vec, flg_sym + ) np.testing.assert_array_equal(res, dense @ vec) diag_add = 0 diag_mul = 0 - mujoco.mju_cholFactorBand(band, n_total, n_band, n_dense, - diag_add, diag_mul) + mujoco.mju_cholFactorBand( + band, n_total, n_band, n_dense, diag_add, diag_mul + ) mujoco.mju_cholSolveBand(res, band, vec, n_total, n_band, n_dense) np.testing.assert_almost_equal(res, np.linalg.solve(dense, vec)) def test_mju_box_qp(self): n = 5 res = np.zeros(n) - r = np.zeros((n, n+7)) + r = np.zeros((n, n + 7)) index = np.zeros(n, np.int32) h = np.eye(n) g = np.ones((n,)) @@ -1324,7 +1428,7 @@ Euler integrator, semi-implicit in velocity. mat = np.linspace(0, 1, 16).reshape(4, 4) res = np.empty((4, 4), np.float64) mujoco.mju_symmetrize(res, mat) - np.testing.assert_array_equal(res, 0.5*(mat + mat.T)) + np.testing.assert_array_equal(res, 0.5 * (mat + mat.T)) def test_mju_clip(self): self.assertEqual(mujoco.mju_clip(1.5, 1.0, 2.0), 1.5) @@ -1332,14 +1436,14 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mju_clip(1.5, 0.0, 1.0), 1.0) def test_mju_mul_vec_mat_vec(self): - vec1 = np.array([1., 2., 3.]) - vec2 = np.array([3., 2., 1.]) - mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]]) - self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.) + vec1 = np.array([1.0, 2.0, 3.0]) + vec2 = np.array([3.0, 2.0, 1.0]) + mat = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]) + self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.0) def test_mju_dense_to_sparse(self): - mat = np.array([[0., 1., 0.], [2., 0., 3.]]) - expected_vals = np.array([1., 2., 3.]) + mat = np.array([[0.0, 1.0, 0.0], [2.0, 0.0, 3.0]]) + expected_vals = np.array([1.0, 2.0, 3.0]) expected_rownnz = np.array([1, 2]) expected_rowadr = np.array([0, 1]) expected_colind = np.array([1, 0, 2]) @@ -1355,8 +1459,8 @@ Euler integrator, semi-implicit in velocity. np.testing.assert_array_equal(col_ind, expected_colind) def test_mju_sparse_to_dense(self): - expected = np.array([[0., 1., 0.], [2., 0., 3.]]) - mat = np.array((1., 2., 3.)) + expected = np.array([[0.0, 1.0, 0.0], [2.0, 0.0, 3.0]]) + mat = np.array((1.0, 2.0, 3.0)) rownnz = np.array([1, 2]) rowadr = np.array([0, 1]) colind = np.array([1, 0, 2]) @@ -1366,10 +1470,10 @@ Euler integrator, semi-implicit in velocity. def test_mju_euler_to_quat(self): quat = np.zeros(4) - euler = np.array([0, np.pi/2, 0]) + euler = np.array([0, np.pi / 2, 0]) seq = 'xyz' mujoco.mju_euler2Quat(quat, euler, seq) - expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.]) + expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.0]) np.testing.assert_almost_equal(quat, expected_quat) error = 'mju_euler2Quat: seq must contain exactly 3 characters' @@ -1377,7 +1481,7 @@ Euler integrator, semi-implicit in velocity. mujoco.mju_euler2Quat(quat, euler, 'xy') with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error): mujoco.mju_euler2Quat(quat, euler, 'xyzy') - error = 'mju_euler2Quat: seq[2] is \'p\', should be one of x, y, z, X, Y, Z' + error = "mju_euler2Quat: seq[2] is 'p', should be one of x, y, z, X, Y, Z" with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error): mujoco.mju_euler2Quat(quat, euler, 'xYp') @@ -1396,8 +1500,16 @@ Euler integrator, semi-implicit in velocity. mujoco.mj_step(self.model, self.data) data2 = pickle.loads(pickle.dumps(self.data)) attr_to_compare = ( - 'time', 'qpos', 'qvel', 'qacc', 'xpos', 'mocap_pos', - 'warning', 'energy', 'contact', 'efc_J' + 'time', + 'qpos', + 'qvel', + 'qacc', + 'xpos', + 'mocap_pos', + 'warning', + 'energy', + 'contact', + 'efc_J', ) self._assert_attributes_equal(data2, self.data, attr_to_compare) for _ in range(10): @@ -1410,8 +1522,16 @@ Euler integrator, semi-implicit in velocity. mujoco.mj_step(self.model, self.data) data2 = pickle.loads(pickle.dumps(self.data)) attr_to_compare = ( - 'time', 'qpos', 'qvel', 'qacc', 'xpos', 'mocap_pos', - 'warning', 'energy', 'contact', 'efc_J' + 'time', + 'qpos', + 'qvel', + 'qacc', + 'xpos', + 'mocap_pos', + 'warning', + 'energy', + 'contact', + 'efc_J', ) self._assert_attributes_equal(data2, self.data, attr_to_compare) for _ in range(10): @@ -1422,7 +1542,10 @@ Euler integrator, semi-implicit in velocity. def test_pickle_mjmodel(self): model2 = pickle.loads(pickle.dumps(self.model)) attr_to_compare = ( - 'nq', 'nmat', 'body_pos', 'names', + 'nq', + 'nmat', + 'body_pos', + 'names', ) self._assert_attributes_equal(model2, self.model, attr_to_compare) @@ -1506,8 +1629,11 @@ Euler integrator, semi-implicit in velocity. else: self.assertEqual(actual_value, expected_value) except AssertionError as e: - self.fail("Attribute '{}' differs from expected value: {}".format( - name, str(e))) + self.fail( + "Attribute '{}' differs from expected value: {}".format( + name, str(e) + ) + ) if __name__ == '__main__': diff --git a/python/mujoco/memory_leak_test.py b/python/mujoco/memory_leak_test.py index 55667a7b..4a713085 100644 --- a/python/mujoco/memory_leak_test.py +++ b/python/mujoco/memory_leak_test.py @@ -57,6 +57,7 @@ class MemoryLeakTest(absltest.TestCase): soft = -1 try: import resource # pylint: disable=g-import-not-at-top + soft, hard = resource.getrlimit(resource.RLIMIT_AS) resource.setrlimit(resource.RLIMIT_AS, (limit_in_bytes, hard)) except (ImportError, ValueError): @@ -65,5 +66,5 @@ class MemoryLeakTest(absltest.TestCase): return soft -if __name__ == '__main__': +if __name__ == "__main__": absltest.main() diff --git a/python/mujoco/minimize.py b/python/mujoco/minimize.py index 60ffae98..449d4bb8 100644 --- a/python/mujoco/minimize.py +++ b/python/mujoco/minimize.py @@ -209,7 +209,7 @@ def least_squares( # Decrease mu agressively: sequential decreases grow exponentially. def decrease_mu(mu, n_reduc): - dmu = (1/mu_factor) ** (2**n_reduc) + dmu = (1 / mu_factor) ** (2**n_reduc) mu = 0.0 if mu * dmu < mu_min else mu * dmu n_reduc += 1 return mu, n_reduc @@ -427,7 +427,6 @@ def jacobian_fd( Returns: jac: Jacobian of the residual at x. n_res: updated number of residual evaluations (add x.size). - """ n = x.size if bounds is None: @@ -438,7 +437,7 @@ def jacobian_fd( xh = x + np.diag(eps_vec) rh = residual(xh) jac = (rh - r) / eps_vec - return jac, n_res+n + return jac, n_res + n def check_jacobian( @@ -467,14 +466,15 @@ def check_jacobian( Returns: n_res: updated number of residual evaluations. - """ jac_fd, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds) denom = np.abs(jac).sum() + np.abs(jac_fd).sum() + 1e-8 rel_diff = np.abs(jac - jac_fd) / denom if np.any(rel_diff > 1e-5): - raise ValueError(f'User-provided {name} does not match finite-differences ' - 'to a relative tolerance of 1e-5.') + raise ValueError( + f'User-provided {name} does not match finite-differences ' + 'to a relative tolerance of 1e-5.' + ) print(f'User-provided {name} matches finite-differences.', file=output) return n_res @@ -489,8 +489,8 @@ def check_norm( Args: r: residual vector. - norm: Norm function returning either the norm scalar or its gradient - and Gauss-Newton Hessian. + norm: Norm function returning either the norm scalar or its gradient and + Gauss-Newton Hessian. eps: finite-difference step size. output: Optional file or StringIO to which to print messages. """ @@ -506,12 +506,16 @@ def check_norm( # Check that Hessian is positive-definite. if np.any(np.linalg.eigvals(n_h) < 0): h_min = np.min(np.linalg.eigvals(n_h)) - raise ValueError('User-provided norm Hessian is not positive definite. ' - f'Minimum eigenvalue is {h_min:<.4g}') + raise ValueError( + 'User-provided norm Hessian is not positive definite. ' + f'Minimum eigenvalue is {h_min:<.4g}' + ) # Local function returning norm values (vectorized). def norm_vec(v): - norms = [np.atleast_2d(norm.value(v[:, i:i+1])) for i in range(v.shape[1])] + norms = [ + np.atleast_2d(norm.value(v[:, i : i + 1])) for i in range(v.shape[1]) + ] return np.hstack(norms) # Check the norm gradient. @@ -519,7 +523,9 @@ def check_norm( # Local function returning norm gradients (vectorized). def grad_vec(v): - gradients = [norm.grad_hess(v[:, i:i+1], eye)[0] for i in range(v.shape[1])] + gradients = [ + norm.grad_hess(v[:, i : i + 1], eye)[0] for i in range(v.shape[1]) + ] return np.hstack(gradients) # Check the norm Hessian. diff --git a/python/mujoco/minimize_test.py b/python/mujoco/minimize_test.py index 6f4cd3cc..5cbf1cfd 100644 --- a/python/mujoco/minimize_test.py +++ b/python/mujoco/minimize_test.py @@ -56,8 +56,9 @@ class MinimizeTest(absltest.TestCase): x0 = np.array((0.0, 0.0)) out = io.StringIO() - x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out, - check_derivatives=True) + x, _ = minimize.least_squares( + x0, residual, jacobian=jacobian, output=out, check_derivatives=True + ) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) self.assertIn('norm(dx) < tol', out.getvalue()) @@ -67,9 +68,15 @@ class MinimizeTest(absltest.TestCase): def bad_jacobian(x, r): del r # Unused. return np.array([[-1, 0], [-20 * x[0, 0], 15]]) + with self.assertRaisesRegex(ValueError, r'\bJacobian does not match\b'): - minimize.least_squares(x0, residual, jacobian=bad_jacobian, output=out, - check_derivatives=True) + minimize.least_squares( + x0, + residual, + jacobian=bad_jacobian, + output=out, + check_derivatives=True, + ) def test_max_iter(self) -> None: dim = 20 # High-D Rosenbrock @@ -98,13 +105,16 @@ class MinimizeTest(absltest.TestCase): x0 = np.array((0.0, 0.0)) expected_x = np.array((1.0, 1.0)) - bounds_types = {'inbounds': [np.array((-2.0, -2.0)), np.array((2.0, 2.0))], - 'onlower': [np.array((-2.0, 2.0)), np.array((0.5, 3.0))], - 'onupper': [np.array((-2.0, -2.0)), np.array((0.5, 2.0))]} + bounds_types = { + 'inbounds': [np.array((-2.0, -2.0)), np.array((2.0, 2.0))], + 'onlower': [np.array((-2.0, 2.0)), np.array((0.5, 3.0))], + 'onupper': [np.array((-2.0, -2.0)), np.array((0.5, 2.0))], + } # In bounds finds true minimum. - x, _ = minimize.least_squares(x0, residual, bounds=bounds_types['inbounds'], - output=out) + x, _ = minimize.least_squares( + x0, residual, bounds=bounds_types['inbounds'], output=out + ) np.testing.assert_array_almost_equal(x, expected_x) self.assertIn('norm(dx) < tol', out.getvalue()) @@ -157,8 +167,9 @@ class MinimizeTest(absltest.TestCase): print(f'Hello iteration {len(trace)}!', file=out) x0 = np.array((0.0, 0.0)) - x, _ = minimize.least_squares(x0, residual, output=out, - iter_callback=iter_callback) + x, _ = minimize.least_squares( + x0, residual, output=out, iter_callback=iter_callback + ) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) self.assertIn('Hello iteration 3!', out.getvalue()) @@ -170,11 +181,12 @@ class MinimizeTest(absltest.TestCase): p = 0.01 # Smoothing radius for smooth-L2 norm. class SmoothL2(minimize.Norm): + def value(self, r): - return np.sqrt((r.T @ r).item() + p*p) - p + return np.sqrt((r.T @ r).item() + p * p) - p def grad_hess(self, r, proj): - s = np.sqrt((r.T @ r).item() + p*p) + s = np.sqrt((r.T @ r).item() + p * p) y_r = r / s grad = proj.T @ y_r y_rr = (np.eye(r.size) - y_r @ y_r.T) / s @@ -183,8 +195,9 @@ class MinimizeTest(absltest.TestCase): out = io.StringIO() x0 = np.array((0.0, 0.0)) - x, _ = minimize.least_squares(x0, residual, norm=SmoothL2(), output=out, - check_derivatives=True) + x, _ = minimize.least_squares( + x0, residual, norm=SmoothL2(), output=out, check_derivatives=True + ) expected_x = np.array((1.0, 1.0)) np.testing.assert_array_almost_equal(x, expected_x) self.assertIn('norm(dx) < tol', out.getvalue()) @@ -192,11 +205,12 @@ class MinimizeTest(absltest.TestCase): self.assertIn('User-provided norm Hessian matches', out.getvalue()) class SmoothL2BadGrad(minimize.Norm): + def value(self, r): - return np.sqrt((r.T @ r).item() + p*p) - p + return np.sqrt((r.T @ r).item() + p * p) - p def grad_hess(self, r, proj): - s = np.sqrt((r.T @ r).item() + p*p) + s = np.sqrt((r.T @ r).item() + p * p) y_r = r / s grad = proj.T @ (y_r + 0.001) # 0.001 is erronous. y_rr = (np.eye(r.size) - y_r @ y_r.T) / s @@ -204,15 +218,21 @@ class MinimizeTest(absltest.TestCase): return grad, hess with self.assertRaisesRegex(ValueError, r'\bgradient does not match\b'): - minimize.least_squares(x0, residual, norm=SmoothL2BadGrad(), output=out, - check_derivatives=True) + minimize.least_squares( + x0, + residual, + norm=SmoothL2BadGrad(), + output=out, + check_derivatives=True, + ) class SmoothL2BadHess(minimize.Norm): + def value(self, r): - return np.sqrt((r.T @ r).item() + p*p) - p + return np.sqrt((r.T @ r).item() + p * p) - p def grad_hess(self, r, proj): - s = np.sqrt((r.T @ r).item() + p*p) + s = np.sqrt((r.T @ r).item() + p * p) y_r = r / s grad = proj.T @ y_r y_rr = (1.001 * np.eye(r.size) - y_r @ y_r.T) / s # 1.001 is erronous. @@ -220,15 +240,21 @@ class MinimizeTest(absltest.TestCase): return grad, hess with self.assertRaisesRegex(ValueError, r'\bHessian does not match\b'): - minimize.least_squares(x0, residual, norm=SmoothL2BadHess(), output=out, - check_derivatives=True) + minimize.least_squares( + x0, + residual, + norm=SmoothL2BadHess(), + output=out, + check_derivatives=True, + ) class SmoothL2AsymHess(minimize.Norm): + def value(self, r): - return np.sqrt((r.T @ r).item() + p*p) - p + return np.sqrt((r.T @ r).item() + p * p) - p def grad_hess(self, r, proj): - s = np.sqrt((r.T @ r).item() + p*p) + s = np.sqrt((r.T @ r).item() + p * p) y_r = r / s grad = proj.T @ y_r y_rr = (np.eye(r.size) - (y_r + 0.0001) @ y_r.T) / s @@ -236,15 +262,21 @@ class MinimizeTest(absltest.TestCase): return grad, hess with self.assertRaisesRegex(ValueError, r'\bnot symmetric\b'): - minimize.least_squares(x0, residual, norm=SmoothL2AsymHess(), output=out, - check_derivatives=True) + minimize.least_squares( + x0, + residual, + norm=SmoothL2AsymHess(), + output=out, + check_derivatives=True, + ) class SmoothL2NegHess(minimize.Norm): + def value(self, r): - return np.sqrt((r.T @ r).item() + p*p) - p + return np.sqrt((r.T @ r).item() + p * p) - p def grad_hess(self, r, proj): - s = np.sqrt((r.T @ r).item() + p*p) + s = np.sqrt((r.T @ r).item() + p * p) y_r = r / s grad = proj.T @ y_r y_rr = -(np.eye(r.size) - y_r @ y_r.T) / s # Negative-definite. @@ -252,7 +284,14 @@ class MinimizeTest(absltest.TestCase): return grad, hess with self.assertRaisesRegex(ValueError, r'\bnot positive definite\b'): - minimize.least_squares(x0, residual, norm=SmoothL2NegHess(), output=out, - check_derivatives=True) + minimize.least_squares( + x0, + residual, + norm=SmoothL2NegHess(), + output=out, + check_derivatives=True, + ) + + if __name__ == '__main__': absltest.main() diff --git a/python/mujoco/msh2obj_test.py b/python/mujoco/msh2obj_test.py index cbba8d84..97166db4 100644 --- a/python/mujoco/msh2obj_test.py +++ b/python/mujoco/msh2obj_test.py @@ -63,7 +63,8 @@ class MshTest(absltest.TestCase): obj = msh2obj.msh_to_obj(msh_path) obj_model = mujoco.MjModel.from_xml_string( - _XML, {"abdomen_1_body.obj": obj.encode()}) + _XML, {"abdomen_1_body.obj": obj.encode()} + ) for field in _MESH_FIELDS: np.testing.assert_allclose( @@ -73,5 +74,6 @@ class MshTest(absltest.TestCase): err_msg=f"Field {field} does not match between msh and obj models.", ) + if __name__ == "__main__": absltest.main() diff --git a/python/mujoco/render_test.py b/python/mujoco/render_test.py index af5705ae..2df0a813 100644 --- a/python/mujoco/render_test.py +++ b/python/mujoco/render_test.py @@ -19,8 +19,9 @@ import mujoco import numpy as np -@absltest.skipUnless(hasattr(mujoco, 'GLContext'), - 'MuJoCo rendering is disabled') +@absltest.skipUnless( + hasattr(mujoco, 'GLContext'), 'MuJoCo rendering is disabled' +) class MuJoCoRenderTest(absltest.TestCase): def setUp(self): @@ -48,8 +49,14 @@ class MuJoCoRenderTest(absltest.TestCase): scene = mujoco.MjvScene(self.model, maxgeom=0) mujoco.mjv_updateScene( - self.model, self.data, mujoco.MjvOption(), mujoco.MjvPerturb(), - mujoco.MjvCamera(), mujoco.mjtCatBit.mjCAT_ALL, scene) + self.model, + self.data, + mujoco.MjvOption(), + mujoco.MjvPerturb(), + mujoco.MjvCamera(), + mujoco.mjtCatBit.mjCAT_ALL, + scene, + ) context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150) mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context) @@ -62,7 +69,7 @@ class MuJoCoRenderTest(absltest.TestCase): mujoco.mjr_rectangle(blue_rect, 0, 0, 1, 1) expected_upside_down_image = np.zeros((480, 640, 3), dtype=np.uint8) - expected_upside_down_image[67:67+123, 56:56+234, 2] = 255 + expected_upside_down_image[67 : 67 + 123, 56 : 56 + 234, 2] = 255 upside_down_image = np.empty((480, 640, 3), dtype=np.uint8) mujoco.mjr_readPixels(upside_down_image, None, full_rect, context) @@ -71,7 +78,8 @@ class MuJoCoRenderTest(absltest.TestCase): # Check that mjr_readPixels can accept a flattened array. upside_down_image[:] = 0 mujoco.mjr_readPixels( - np.reshape(upside_down_image, -1), None, full_rect, context) + np.reshape(upside_down_image, -1), None, full_rect, context + ) np.testing.assert_array_equal(upside_down_image, expected_upside_down_image) context.free() @@ -81,8 +89,14 @@ class MuJoCoRenderTest(absltest.TestCase): scene = mujoco.MjvScene(self.model, maxgeom=0) mujoco.mjv_updateScene( - self.model, self.data, mujoco.MjvOption(), None, - mujoco.MjvCamera(), mujoco.mjtCatBit.mjCAT_ALL, scene) + self.model, + self.data, + mujoco.MjvOption(), + None, + mujoco.MjvCamera(), + mujoco.mjtCatBit.mjCAT_ALL, + scene, + ) context = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150) mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context) diff --git a/python/mujoco/renderer.py b/python/mujoco/renderer.py index afe5fc77..68175755 100644 --- a/python/mujoco/renderer.py +++ b/python/mujoco/renderer.py @@ -32,7 +32,7 @@ class Renderer: model: _structs.MjModel, height: int = 240, width: int = 320, - max_geom: int = 10000 + max_geom: int = 10000, ) -> None: """Initializes a new `Renderer`. @@ -43,6 +43,7 @@ class Renderer: max_geom: Optional integer specifying the maximum number of geoms that can be rendered in the same scene. If None this will be chosen automatically based on the estimated maximum number of renderable geoms in the model. + Raises: ValueError: If `camera_id` is outside the valid range, or if `width` or `height` exceed the dimensions of MuJoCo's offscreen framebuffer. @@ -220,9 +221,7 @@ the clause: # Convert 3-channel uint8 to 1-channel uint32. image3 = out.astype(np.uint32) segimage = ( - image3[:, :, 0] - + image3[:, :, 1] * (2**8) - + image3[:, :, 2] * (2**16) + image3[:, :, 0] + image3[:, :, 1] * (2**8) + image3[:, :, 2] * (2**16) ) # Remap segid to 2-channel (object ID, object type) pair. # Seg ID 0 is background -- will be remapped to (-1, -1). @@ -251,15 +250,15 @@ the clause: self, data: _structs.MjData, camera: Union[int, str, _structs.MjvCamera] = -1, - scene_option: Optional[_structs.MjvOption] = None - ): + scene_option: Optional[_structs.MjvOption] = None, + ): """Updates geometry used for rendering. Args: data: An instance of `MjData`. camera: An instance of `MjvCamera`, a string or an integer - scene_option: A custom `MjvOption` instance to use to render - the scene instead of the default. + scene_option: A custom `MjvOption` instance to use to render the scene + instead of the default. Raises: ValueError: If `camera_id` is outside the valid range, or if camera does @@ -274,8 +273,10 @@ the clause: if camera_id == -1: raise ValueError(f'The camera "{camera}" does not exist.') if camera_id < -1 or camera_id >= self._model.ncam: - raise ValueError(f'The camera id {camera_id} is out of' - f' range [-1, {self._model.ncam}).') + raise ValueError( + f'The camera id {camera_id} is out of' + f' range [-1, {self._model.ncam}).' + ) # Render camera. camera = _structs.MjvCamera() @@ -295,7 +296,8 @@ the clause: data, scene_option, None, - camera, _enums.mjtCatBit.mjCAT_ALL.value, + camera, + _enums.mjtCatBit.mjCAT_ALL.value, self._scene, ) diff --git a/python/mujoco/renderer_test.py b/python/mujoco/renderer_test.py index e8022006..bb5f62f1 100644 --- a/python/mujoco/renderer_test.py +++ b/python/mujoco/renderer_test.py @@ -20,9 +20,11 @@ import mujoco import numpy as np -@absltest.skipUnless(hasattr(mujoco, 'GLContext'), - 'MuJoCo rendering is disabled') +@absltest.skipUnless( + hasattr(mujoco, 'GLContext'), 'MuJoCo rendering is disabled' +) class MuJoCoRendererTest(parameterized.TestCase): + def test_renderer_unknown_camera_name(self): xml = """ diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index f314e795..98eaa3f2 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -23,17 +23,19 @@ import numpy as np from numpy import typing as npt -def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], - data: mujoco.MjData, - initial_state: npt.ArrayLike, - control: Optional[npt.ArrayLike] = None, - *, # require subsequent arguments to be named - control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value, - skip_checks: bool = False, - nstep: Optional[int] = None, - initial_warmstart: Optional[npt.ArrayLike] = None, - state: Optional[npt.ArrayLike] = None, - sensordata: Optional[npt.ArrayLike] = None): +def rollout( + model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], + data: mujoco.MjData, + initial_state: npt.ArrayLike, + control: Optional[npt.ArrayLike] = None, + *, # require subsequent arguments to be named + control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value, + skip_checks: bool = False, + nstep: Optional[int] = None, + initial_warmstart: Optional[npt.ArrayLike] = None, + state: Optional[npt.ArrayLike] = None, + sensordata: Optional[npt.ArrayLike] = None, +): """Rolls out open-loop trajectories from initial states, get subsequent states and sensor values. Python wrapper for rollout.cc, see documentation therein. @@ -66,15 +68,24 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], Raises: ValueError: bad shapes or sizes. - """ + """ # fmt: skip # skip_checks shortcut: # don't infer nroll/nstep # don't support singleton expansion # don't allocate output arrays # just call rollout and return if skip_checks: - _rollout.rollout(model, data, nstep, control_spec, initial_state, - initial_warmstart, control, state, sensordata) + _rollout.rollout( + model, + data, + nstep, + control_spec, + initial_state, + initial_warmstart, + control, + state, + sensordata, + ) return state, sensordata if not isinstance(model, mujoco.MjModel): @@ -92,17 +103,16 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], initial_warmstart=initial_warmstart, control=control, state=state, - sensordata=sensordata) - + sensordata=sensordata, + ) # check number of dimensions - _check_number_of_dimensions(2, - initial_state=initial_state, - initial_warmstart=initial_warmstart) - _check_number_of_dimensions(3, - control=control, - state=state, - sensordata=sensordata) + _check_number_of_dimensions( + 2, initial_state=initial_state, initial_warmstart=initial_warmstart + ) + _check_number_of_dimensions( + 3, control=control, state=state, sensordata=sensordata + ) # ensure 2D, make contiguous, row-major (C ordering) initial_state = _ensure_2d(initial_state) @@ -114,38 +124,46 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], sensordata = _ensure_3d(sensordata) # infer nroll, check for incompatibilities - nroll = _infer_dimension(0, 1, - initial_state=initial_state, - initial_warmstart=initial_warmstart, - control=control, - state=state, - sensordata=sensordata) + nroll = _infer_dimension( + 0, + 1, + initial_state=initial_state, + initial_warmstart=initial_warmstart, + control=control, + state=state, + sensordata=sensordata, + ) if isinstance(model, list) and nroll == 1: nroll = len(model) if isinstance(model, list) and len(model) != nroll: - raise ValueError(f'nroll inferred as {nroll} ' - f'but model is length {len(model)}') + raise ValueError( + f'nroll inferred as {nroll} but model is length {len(model)}' + ) elif not isinstance(model, list): - model = [model] # Use a length 1 list to simplify code below + model = [model] # Use a length 1 list to simplify code below # infer nstep, check for incompatibilities - nstep = _infer_dimension(1, nstep or 1, - control=control, - state=state, - sensordata=sensordata) + nstep = _infer_dimension( + 1, nstep or 1, control=control, state=state, sensordata=sensordata + ) # get nstate/ncontrol/nv/nsensordata # check that they are equal across models - nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value) + nstate = mujoco.mj_stateSize( + model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value + ) ncontrol = mujoco.mj_stateSize(model[0], control_spec) nv = model[0].nv nsensordata = model[0].nsensordata for m in model[1:]: - if (nstate != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) + if ( + nstate + != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) or ncontrol != mujoco.mj_stateSize(m, control_spec) or nv != m.nv - or nsensordata != m.nsensordata): + or nsensordata != m.nsensordata + ): raise ValueError('models are not compatible') # check trailing dimensions @@ -167,8 +185,17 @@ def rollout(model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], sensordata = np.empty((nroll, nstep, nsensordata)) # call rollout - _rollout.rollout(model, data, nstep, control_spec, initial_state, - initial_warmstart, control, state, sensordata) + _rollout.rollout( + model, + data, + nstep, + control_spec, + initial_state, + initial_warmstart, + control, + state, + sensordata, + ) # return outputs return state, sensordata @@ -227,8 +254,8 @@ def _infer_dimension(dim, value, **kwargs): Args: dim: Dimension to be inferred. value: Initial guess of inferred value (1: unknown). - **kwargs: List of arrays which should all have the same size (or 1) - along dimension dim. + **kwargs: List of arrays which should all have the same size (or 1) along + dimension dim. Returns: Inferred dimension. diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 32c670e6..3cc0d062 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -127,10 +127,12 @@ TEST_XML_DIVERGE = r""" """ -ALL_MODELS = {'TEST_XML': TEST_XML, - 'TEST_XML_NO_SENSORS': TEST_XML_NO_SENSORS, - 'TEST_XML_NO_ACTUATORS': TEST_XML_NO_ACTUATORS, - 'TEST_XML_EMPTY': TEST_XML_EMPTY} +ALL_MODELS = { + 'TEST_XML': TEST_XML, + 'TEST_XML_NO_SENSORS': TEST_XML_NO_SENSORS, + 'TEST_XML_NO_ACTUATORS': TEST_XML_NO_ACTUATORS, + 'TEST_XML_EMPTY': TEST_XML_EMPTY, +} # ------------------------------ tests ----------------------------------------- @@ -242,8 +244,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nroll, 1)) - state, sensordata = rollout.rollout(model, data, initial_state, control, - initial_warmstart=initial_warmstart) + state, sensordata = rollout.rollout( + model, data, initial_state, control, initial_warmstart=initial_warmstart + ) mujoco.mj_resetData(model, data) initial_state = np.tile(initial_state, (nroll, 1)) @@ -264,8 +267,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) state = np.empty((nroll, nstep, nstate)) - state, sensordata = rollout.rollout(model, data, initial_state, control, - state=state) + state, sensordata = rollout.rollout( + model, data, initial_state, control, state=state + ) mujoco.mj_resetData(model, data) initial_state = np.tile(initial_state, (nroll, 1)) @@ -286,8 +290,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) sensordata = np.empty((nroll, nstep, model.nsensordata)) - state, sensordata = rollout.rollout(model, data, initial_state, control, - sensordata=sensordata) + state, sensordata = rollout.rollout( + model, data, initial_state, control, sensordata=sensordata + ) mujoco.mj_resetData(model, data) initial_state = np.tile(initial_state, (nroll, 1)) @@ -309,8 +314,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): control = np.random.randn(model.nu) state = np.empty((nroll, nstep, nstate)) sensordata = np.empty((nroll, nstep, model.nsensordata)) - rollout.rollout(model, data, initial_state, control, - state=state, sensordata=sensordata) + rollout.rollout( + model, data, initial_state, control, state=state, sensordata=sensordata + ) control = np.tile(control, (nstep, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) @@ -374,8 +380,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nroll, nstate) control = np.random.randn(nroll, 1, model.nu) state = np.empty((nroll, nstep, nstate)) - state, sensordata = rollout.rollout(model, data, initial_state, control, - state=state) + state, sensordata = rollout.rollout( + model, data, initial_state, control, state=state + ) control = np.repeat(control, nstep, axis=1) py_state, py_sensordata = py_rollout(model, data, initial_state, control) @@ -393,17 +400,21 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nroll, nstate) - control_spec = (mujoco.mjtState.mjSTATE_CTRL | - mujoco.mjtState.mjSTATE_QFRC_APPLIED | - mujoco.mjtState.mjSTATE_XFRC_APPLIED) + control_spec = ( + mujoco.mjtState.mjSTATE_CTRL + | mujoco.mjtState.mjSTATE_QFRC_APPLIED + | mujoco.mjtState.mjSTATE_XFRC_APPLIED + ) ncontrol = mujoco.mj_stateSize(model, control_spec) control = np.random.randn(nroll, nstep, ncontrol) - state, sensordata = rollout.rollout(model, data, initial_state, control, - control_spec=control_spec) + state, sensordata = rollout.rollout( + model, data, initial_state, control, control_spec=control_spec + ) - py_state, py_sensordata = py_rollout(model, data, initial_state, control, - control_spec=control_spec) + py_state, py_sensordata = py_rollout( + model, data, initial_state, control, control_spec=control_spec + ) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @@ -416,15 +427,19 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.empty((nroll, nstate)) # get diverging (0, 2) and non-diverging (1, 3) states - mujoco.mj_getState(model, data, initial_state[0], - mujoco.mjtState.mjSTATE_FULLPHYSICS) - mujoco.mj_getState(model, data, initial_state[2], - mujoco.mjtState.mjSTATE_FULLPHYSICS) + mujoco.mj_getState( + model, data, initial_state[0], mujoco.mjtState.mjSTATE_FULLPHYSICS + ) + mujoco.mj_getState( + model, data, initial_state[2], mujoco.mjtState.mjSTATE_FULLPHYSICS + ) mujoco.mj_resetDataKeyframe(model, data, 0) # keyframe 0 does not diverge - mujoco.mj_getState(model, data, initial_state[1], - mujoco.mjtState.mjSTATE_FULLPHYSICS) - mujoco.mj_getState(model, data, initial_state[3], - mujoco.mjtState.mjSTATE_FULLPHYSICS) + mujoco.mj_getState( + model, data, initial_state[1], mujoco.mjtState.mjSTATE_FULLPHYSICS + ) + mujoco.mj_getState( + model, data, initial_state[3], mujoco.mjtState.mjSTATE_FULLPHYSICS + ) nstep = 10000 # divergence after ~15s, timestep = 2e-3 @@ -459,27 +474,40 @@ class MuJoCoRolloutTest(parameterized.TestCase): thread_local.data = mujoco.MjData(model) model_list = [model] * nroll + def call_rollout(initial_state, control, state, sensordata): - rollout.rollout(model_list, thread_local.data, initial_state, control, - skip_checks=True, - nstep=nstep, state=state, sensordata=sensordata) + rollout.rollout( + model_list, + thread_local.data, + initial_state, + control, + skip_checks=True, + nstep=nstep, + state=state, + sensordata=sensordata, + ) n = nroll // num_workers # integer division chunks = [] # a list of tuples, one per worker - for i in range(num_workers-1): - chunks.append((initial_state[i*n:(i+1)*n], - control[i*n:(i+1)*n], - state[i*n:(i+1)*n], - sensordata[i*n:(i+1)*n])) + for i in range(num_workers - 1): + chunks.append(( + initial_state[i * n : (i + 1) * n], + control[i * n : (i + 1) * n], + state[i * n : (i + 1) * n], + sensordata[i * n : (i + 1) * n], + )) # last chunk, absorbing the remainder: - chunks.append((initial_state[(num_workers-1)*n:], - control[(num_workers-1)*n:], - state[(num_workers-1)*n:], - sensordata[(num_workers-1)*n:])) + chunks.append(( + initial_state[(num_workers - 1) * n :], + control[(num_workers - 1) * n :], + state[(num_workers - 1) * n :], + sensordata[(num_workers - 1) * n :], + )) with concurrent.futures.ThreadPoolExecutor( - max_workers=num_workers, initializer=thread_initializer) as executor: + max_workers=num_workers, initializer=thread_initializer + ) as executor: futures = [] for chunk in chunks: futures.append(executor.submit(call_rollout, *chunk)) @@ -513,12 +541,14 @@ class MuJoCoRolloutTest(parameterized.TestCase): state, _ = rollout.rollout(model, data, state1[0], control) # assert that stepping without warmstarts is not exact - np.testing.assert_raises(AssertionError, - np.testing.assert_array_equal, state, state2) + np.testing.assert_raises( + AssertionError, np.testing.assert_array_equal, state, state2 + ) # take step using rollout, take warmstart into account - state, _ = rollout.rollout(model, data, state1, control, - initial_warmstart=initial_warmstart) + state, _ = rollout.rollout( + model, data, state1, control, initial_warmstart=initial_warmstart + ) # assert exact equality np.testing.assert_array_equal(state, np.expand_dims(state2, axis=0)) @@ -530,19 +560,21 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.zeros(nstate) - control_spec = (mujoco.mjtState.mjSTATE_MOCAP_POS | - mujoco.mjtState.mjSTATE_MOCAP_QUAT) + control_spec = ( + mujoco.mjtState.mjSTATE_MOCAP_POS | mujoco.mjtState.mjSTATE_MOCAP_QUAT + ) - pos1 = np.array((1., 2., 3.)) - quat1 = np.array((1., 2., 3., 4.)) + pos1 = np.array((1.0, 2.0, 3.0)) + quat1 = np.array((1.0, 2.0, 3.0, 4.0)) quat1 /= np.linalg.norm(quat1) - pos2 = np.array((2., 3., 4.)) - quat2 = np.array((2., 3., 4., 5.)) + pos2 = np.array((2.0, 3.0, 4.0)) + quat2 = np.array((2.0, 3.0, 4.0, 5.0)) quat2 /= np.linalg.norm(quat2) control = np.hstack((pos1, pos2, quat1, quat2)) - _, sensordata = rollout.rollout(model, data, initial_state, control, - control_spec=control_spec) + _, sensordata = rollout.rollout( + model, data, initial_state, control, control_spec=control_spec + ) np.testing.assert_array_almost_equal(sensordata[0][0][:3], pos1) np.testing.assert_array_almost_equal(sensordata[0][0][3:], quat1) @@ -562,7 +594,8 @@ class MuJoCoRolloutTest(parameterized.TestCase): model.opt.solver = 10 # invalid solver type with self.assertRaisesWithLiteralMatch( - mujoco.FatalError, 'mj_fwdConstraint: unknown solver type 10'): + mujoco.FatalError, 'mj_fwdConstraint: unknown solver type 10' + ): rollout.rollout(model, data, initial_state, ctrl) def test_invalid(self): @@ -576,12 +609,14 @@ class MuJoCoRolloutTest(parameterized.TestCase): control = 'string' with self.assertRaisesWithLiteralMatch( - ValueError, 'control must be a numpy array or float'): + ValueError, 'control must be a numpy array or float' + ): rollout.rollout(model, data, initial_state, control) control = np.zeros((2, 3, 4, 5)) with self.assertRaisesWithLiteralMatch( - ValueError, 'control can have at most 3 dimensions'): + ValueError, 'control can have at most 3 dimensions' + ): rollout.rollout(model, data, initial_state, control) def test_bad_sizes(self): @@ -594,28 +629,33 @@ class MuJoCoRolloutTest(parameterized.TestCase): initial_state = np.random.randn(nroll, nstate + 1) with self.assertRaisesWithLiteralMatch( - ValueError, 'trailing dimension of initial_state must be 6, got 7'): + ValueError, 'trailing dimension of initial_state must be 6, got 7' + ): rollout.rollout(model, data, initial_state) initial_state = np.random.randn(nroll, nstate) control = np.random.randn(1, nstep, model.nu + 1) with self.assertRaisesWithLiteralMatch( - ValueError, 'trailing dimension of control must be 2, got 3'): + ValueError, 'trailing dimension of control must be 2, got 3' + ): rollout.rollout(model, data, initial_state, control) control = np.random.randn(nroll, nstep, model.nu) - state = np.random.randn(nroll, nstep+1, nstate) # incompatible nstep + state = np.random.randn(nroll, nstep + 1, nstate) # incompatible nstep with self.assertRaisesWithLiteralMatch( - ValueError, 'dimension 1 inferred as 3 but state has 4'): + ValueError, 'dimension 1 inferred as 3 but state has 4' + ): rollout.rollout(model, data, initial_state, control, state=state) initial_state = np.random.randn(nroll, nstate) control = np.random.randn(nroll, nstep, model.nu) bad_spec = mujoco.mjtState.mjSTATE_ACT with self.assertRaisesWithLiteralMatch( - ValueError, 'control_spec can only contain bits in mjSTATE_USER'): - rollout.rollout(model, data, initial_state, control, - control_spec=bad_spec) + ValueError, 'control_spec can only contain bits in mjSTATE_USER' + ): + rollout.rollout( + model, data, initial_state, control, control_spec=bad_spec + ) def test_stateless(self): model = mujoco.MjModel.from_xml_string(TEST_XML) @@ -655,8 +695,9 @@ def get_state(model, data): return state.reshape((1, nstate)) -def step(model, data, state, control, - control_spec=mujoco.mjtState.mjSTATE_CTRL): +def step( + model, data, state, control, control_spec=mujoco.mjtState.mjSTATE_CTRL +): if state is not None: mujoco.mj_setState(model, data, state, mujoco.mjtState.mjSTATE_FULLPHYSICS) mujoco.mj_setState(model, data, control, control_spec) @@ -664,8 +705,13 @@ def step(model, data, state, control, return (get_state(model, data), data.sensordata) -def one_rollout(model, data, initial_state, control, - control_spec=mujoco.mjtState.mjSTATE_CTRL): +def one_rollout( + model, + data, + initial_state, + control, + control_spec=mujoco.mjtState.mjSTATE_CTRL, +): nstep = control.shape[0] nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) state = np.empty((nstep, nstate)) @@ -673,9 +719,9 @@ def one_rollout(model, data, initial_state, control, mujoco.mj_resetData(model, data) for t in range(nstep): - state[t], sensordata[t] = step(model, data, - initial_state if t == 0 else None, - control[t], control_spec) + state[t], sensordata[t] = step( + model, data, initial_state if t == 0 else None, control[t], control_spec + ) return state, sensordata @@ -700,15 +746,20 @@ def ensure_3d(arg): return np.ascontiguousarray(arg, dtype=np.float64) -def py_rollout(model, data, initial_state, control, - control_spec=mujoco.mjtState.mjSTATE_CTRL): +def py_rollout( + model, + data, + initial_state, + control, + control_spec=mujoco.mjtState.mjSTATE_CTRL, +): initial_state = ensure_2d(initial_state) control = ensure_3d(control) nroll = initial_state.shape[0] nstep = control.shape[1] if isinstance(model, mujoco.MjModel): - model = [model]*nroll + model = [model] * nroll nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index e18db9b7..bd9997e1 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -104,7 +104,9 @@ class SpecsTest(absltest.TestCase): self.assertEqual(model.nuser_site, 6) np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6]) - self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + self.assertEqual( + spec.to_xml(), + textwrap.dedent("""\ @@ -116,7 +118,8 @@ class SpecsTest(absltest.TestCase): - """),) + """), + ) def test_kwarg(self): # Create a spec. @@ -467,7 +470,7 @@ class SpecsTest(absltest.TestCase): # Try to compile, get error. expected_error = ( 'Error: size 0 must be positive in geom\n' - + f'Element name \'MyGeom\', id 0, geom added on line {added_on_line}' + + f"Element name 'MyGeom', id 0, geom added on line {added_on_line}" ) with self.assertRaisesRegex(ValueError, expected_error): spec.compile() @@ -531,7 +534,9 @@ class SpecsTest(absltest.TestCase): spec.worldbody.add_geom(main) spec.compile() - self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + self.assertEqual( + spec.to_xml(), + textwrap.dedent("""\ @@ -547,7 +552,8 @@ class SpecsTest(absltest.TestCase): - """)) + """), + ) spec = mujoco.MjSpec() spec.modelname = 'test' @@ -561,7 +567,9 @@ class SpecsTest(absltest.TestCase): spec.worldbody.add_geom(main) spec.compile() - self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + self.assertEqual( + spec.to_xml(), + textwrap.dedent("""\ @@ -577,7 +585,8 @@ class SpecsTest(absltest.TestCase): - """)) + """), + ) def test_element_list(self): spec = mujoco.MjSpec() @@ -718,13 +727,17 @@ class SpecsTest(absltest.TestCase): """ - spec = mujoco.MjSpec.from_string(textwrap.dedent(""" + spec = mujoco.MjSpec.from_string( + textwrap.dedent(""" - """), {'included.xml': included_xml.encode('utf-8')}) - self.assertEqual(spec.worldbody.first_body().first_geom().type, - mujoco.mjtGeom.mjGEOM_BOX) + """), + {'included.xml': included_xml.encode('utf-8')}, + ) + self.assertEqual( + spec.worldbody.first_body().first_geom().type, mujoco.mjtGeom.mjGEOM_BOX + ) def test_delete(self): file_path = epath.resource_path("mujoco") / "testdata" / "model.xml" diff --git a/python/mujoco/viewer.py b/python/mujoco/viewer.py index 28c55e44..31ed22a8 100644 --- a/python/mujoco/viewer.py +++ b/python/mujoco/viewer.py @@ -42,7 +42,7 @@ PERCENT_REALTIME = ( 10, 8, 6.6, 5, 4, 3.3, 2.5, 2, 1.6, 1.3, 1, 0.8, 0.66, 0.5, 0.4, 0.33, 0.25, 0.2, 0.16, 0.13, 0.1 -) +) # fmt: skip # Maximum time mis-alignment before re-sync. MAX_SYNC_MISALIGN = 0.1 @@ -194,12 +194,13 @@ def _file_loader(path: str) -> _LoaderWithPathType: def _reload( - simulate: _Simulate, loader: _InternalLoaderType, - notify_loaded: Optional[Callable[[], None]] = None + simulate: _Simulate, + loader: _InternalLoaderType, + notify_loaded: Optional[Callable[[], None]] = None, ) -> Optional[Tuple[mujoco.MjModel, mujoco.MjData]]: """Internal function for reloading a model in the viewer.""" try: - simulate.load_message('') # path is unknown at this point + simulate.load_message('') # path is unknown at this point load_tuple = loader() except Exception as e: # pylint: disable=broad-except simulate.load_error = str(e) @@ -275,14 +276,16 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]): # Inject noise. if simulate.ctrl_noise_std != 0.0: # Convert rate and scale to discrete time (Ornstein–Uhlenbeck). - rate = math.exp(-m.opt.timestep / - max(simulate.ctrl_noise_rate, mujoco.mjMINVAL)) + rate = math.exp( + -m.opt.timestep / max(simulate.ctrl_noise_rate, mujoco.mjMINVAL) + ) scale = simulate.ctrl_noise_std * math.sqrt(1 - rate * rate) for i in range(m.nu): # Update noise. - ctrl_noise[i] = (rate * ctrl_noise[i] + - scale * mujoco.mju_standardNormal(None)) + ctrl_noise[i] = rate * ctrl_noise[ + i + ] + scale * mujoco.mju_standardNormal(None) # Apply noise. d.ctrl[i] = ctrl_noise[i] @@ -291,12 +294,18 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]): slowdown = 100 / PERCENT_REALTIME[simulate.real_time_index] # Misalignment: distance from target sim time > MAX_SYNC_MISALIGN. - misaligned = abs(elapsedcpu / slowdown - - elapsedsim) > MAX_SYNC_MISALIGN + misaligned = ( + abs(elapsedcpu / slowdown - elapsedsim) > MAX_SYNC_MISALIGN + ) # Out-of-sync (for any reason): reset sync times, step. - if (elapsedsim < 0 or elapsedcpu < 0 or synccpu == 0 or misaligned or - simulate.speed_changed): + if ( + elapsedsim < 0 + or elapsedcpu < 0 + or synccpu == 0 + or misaligned + or simulate.speed_changed + ): # Re-sync. synccpu = startcpu syncsim = d.time @@ -312,9 +321,9 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]): prevsim = d.time refreshtime = SIM_REFRESH_FRACTION / simulate.refresh_rate # Step while sim lags behind CPU and within refreshtime. - while (((d.time - syncsim) * slowdown < - (time.time() - synccpu)) and - ((time.time() - startcpu) < refreshtime)): + while ( + (d.time - syncsim) * slowdown < (time.time() - synccpu) + ) and ((time.time() - startcpu) < refreshtime): # Measure slowdown before first step. if not measured and elapsedsim: simulate.measured_slowdown = elapsedcpu / elapsedsim @@ -329,7 +338,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]): break # save current state to history buffer - if (stepped): + if stepped: simulate.add_to_history() else: # simulate.run is False: GUI is paused. @@ -355,7 +364,8 @@ def _launch_internal( raise ValueError('mjData is specified but mjModel is not') elif callable(model) and data is not None: raise ValueError( - 'mjData should not be specified when an mjModel loader is used') + 'mjData should not be specified when an mjModel loader is used' + ) elif loader is not None and model is not None: raise ValueError('model and loader are both specified') elif run_physics_thread and handle_return is not None: @@ -398,14 +408,17 @@ def _launch_internal( if run_physics_thread: side_thread = threading.Thread( - target=_physics_loop, args=(simulate, loader)) + target=_physics_loop, args=(simulate, loader) + ) else: side_thread = threading.Thread( - target=_reload, args=(simulate, loader, notify_loaded)) + target=_reload, args=(simulate, loader, notify_loaded) + ) def make_exit(simulate): def exit_simulate(): simulate.exit() + return exit_simulate exit_simulate = make_exit(simulate) @@ -456,8 +469,7 @@ def launch_passive( if not isinstance(data, mujoco.MjData): raise ValueError(f'`data` is not a mujoco.MjData: got {data!r}') if key_callback is not None and not callable(key_callback): - raise ValueError( - f'`key_callback` is not callable: got {key_callback!r}') + raise ValueError(f'`key_callback` is not callable: got {key_callback!r}') mujoco.mj_forward(model, data) handle_return = queue.Queue(1) @@ -480,7 +492,8 @@ def launch_passive( if not isinstance(_MJPYTHON, _MjPythonBase): raise RuntimeError( '`launch_passive` requires that the Python script be run under ' - '`mjpython` on macOS') + '`mjpython` on macOS' + ) _MJPYTHON.launch_on_ui_thread( model, data, diff --git a/python/pyproject.toml b/python/pyproject.toml index 94067175..600229f4 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -65,3 +65,24 @@ usd = [ "usd-core", "pillow" ] + +[tool.isort] +force_single_line = true +force_sort_within_sections = true +lexicographical = true +single_line_exclusions = ["typing"] +order_by_type = false +group_by_package = true +line_length = 120 +use_parentheses = true +multi_line_output = 3 +skip_glob = ["**/*.ipynb"] + +[tool.pyink] +line-length = 80 +unstable = true +pyink-indentation = 2 +pyink-use-majority-quotes = true +extend-exclude = '''( + .ipynb$ +)''' diff --git a/python/setup.py b/python/setup.py index 8bac24ff..cebdec84 100644 --- a/python/setup.py +++ b/python/setup.py @@ -101,15 +101,15 @@ def tokenize_quoted_substr(input_string, quote_char, placeholders=None): placeholders = placeholders if placeholders is not None else dict() prev_end = -1 for start, end in start_and_end(quote_positions): - output_string += input_string[prev_end+1:start] + output_string += input_string[prev_end + 1 : start] while True: placeholder = ''.join(random.choices(string.ascii_lowercase, k=5)) if placeholder not in input_string and placeholder not in output_string: break output_string += placeholder - placeholders[placeholder] = input_string[start+1:end] + placeholders[placeholder] = input_string[start + 1 : end] prev_end = end - output_string += input_string[prev_end+1:] + output_string += input_string[prev_end + 1 :] return output_string, placeholders @@ -145,15 +145,17 @@ class BuildCMakeExtension(build_ext.build_ext): """Uses CMake to build extensions.""" def run(self): - self._is_apple = (platform.system() == 'Darwin') - (self._mujoco_library_path, - self._mujoco_include_path, - self._mujoco_plugins_path, - self._mujoco_framework_path) = self._find_mujoco() + self._is_apple = platform.system() == 'Darwin' + ( + self._mujoco_library_path, + self._mujoco_include_path, + self._mujoco_plugins_path, + self._mujoco_framework_path, + ) = self._find_mujoco() self._configure_cmake() for ext in self.extensions: assert ext.name.startswith(EXT_PREFIX) - assert '.' not in ext.name[len(EXT_PREFIX):] + assert '.' not in ext.name[len(EXT_PREFIX) :] self.build_extension(ext) self._copy_external_libraries() self._copy_mujoco_headers() @@ -163,20 +165,22 @@ class BuildCMakeExtension(build_ext.build_ext): def _find_mujoco(self): if MUJOCO_PATH not in os.environ: - raise RuntimeError( - f'{MUJOCO_PATH} environment variable is not set') + raise RuntimeError(f'{MUJOCO_PATH} environment variable is not set') if MUJOCO_PLUGIN_PATH not in os.environ: raise RuntimeError( - f'{MUJOCO_PLUGIN_PATH} environment variable is not set') + f'{MUJOCO_PLUGIN_PATH} environment variable is not set' + ) library_path = None include_path = None plugin_path = os.environ[MUJOCO_PLUGIN_PATH] for directory, subdirs, filenames in os.walk(os.environ[MUJOCO_PATH]): if self._is_apple and 'mujoco.framework' in subdirs: - return (os.path.join(directory, 'mujoco.framework/Versions/A'), - os.path.join(directory, 'mujoco.framework/Headers'), - plugin_path, - directory) + return ( + os.path.join(directory, 'mujoco.framework/Versions/A'), + os.path.join(directory, 'mujoco.framework/Headers'), + plugin_path, + directory, + ) if fnmatch.filter(filenames, get_mujoco_lib_pattern()): library_path = directory if os.path.exists(os.path.join(directory, 'mujoco/mujoco.h')): @@ -190,63 +194,78 @@ class BuildCMakeExtension(build_ext.build_ext): for directory, _, filenames in os.walk(os.environ[MUJOCO_PATH]): for pattern in get_external_lib_patterns(): for filename in fnmatch.filter(filenames, pattern): - shutil.copyfile(os.path.join(directory, filename), - os.path.join(dst, filename)) + shutil.copyfile( + os.path.join(directory, filename), os.path.join(dst, filename) + ) def _copy_plugin_libraries(self): dst = os.path.join( os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)), - 'plugin') + 'plugin', + ) os.makedirs(dst) for directory, _, filenames in os.walk(self._mujoco_plugins_path): for pattern in get_plugin_lib_patterns(): for filename in fnmatch.filter(filenames, pattern): - shutil.copyfile(os.path.join(directory, filename), - os.path.join(dst, filename)) + shutil.copyfile( + os.path.join(directory, filename), os.path.join(dst, filename) + ) def _copy_mujoco_headers(self): dst = os.path.join( os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)), - 'include/mujoco') + 'include/mujoco', + ) os.makedirs(dst) for directory, _, filenames in os.walk(self._mujoco_include_path): for filename in fnmatch.filter(filenames, '*.h'): - shutil.copyfile(os.path.join(directory, filename), - os.path.join(dst, filename)) + shutil.copyfile( + os.path.join(directory, filename), os.path.join(dst, filename) + ) def _copy_mjpython(self): src_dir = os.path.join(os.path.dirname(__file__), 'mujoco/mjpython') dst_contents_dir = os.path.join( os.path.dirname(self.get_ext_fullpath(self.extensions[0].name)), - 'MuJoCo_(mjpython).app/Contents') + 'MuJoCo_(mjpython).app/Contents', + ) os.makedirs(dst_contents_dir) - shutil.copyfile(os.path.join(src_dir, 'Info.plist'), - os.path.join(dst_contents_dir, 'Info.plist')) + shutil.copyfile( + os.path.join(src_dir, 'Info.plist'), + os.path.join(dst_contents_dir, 'Info.plist'), + ) dst_bin_dir = os.path.join(dst_contents_dir, 'MacOS') os.makedirs(dst_bin_dir) - shutil.copyfile(os.path.join(self.build_temp, 'mjpython'), - os.path.join(dst_bin_dir, 'mjpython')) + shutil.copyfile( + os.path.join(self.build_temp, 'mjpython'), + os.path.join(dst_bin_dir, 'mjpython'), + ) os.chmod(os.path.join(dst_bin_dir, 'mjpython'), 0o755) dst_resources_dir = os.path.join(dst_contents_dir, 'Resources') os.makedirs(dst_resources_dir) - shutil.copyfile(os.path.join(src_dir, 'mjpython.icns'), - os.path.join(dst_resources_dir, 'mjpython.icns')) + shutil.copyfile( + os.path.join(src_dir, 'mjpython.icns'), + os.path.join(dst_resources_dir, 'mjpython.icns'), + ) def _configure_cmake(self): """Check for CMake.""" cmake = os.environ.get(MUJOCO_CMAKE, 'cmake') build_cfg = 'Debug' if self.debug else 'Release' cmake_module_path = os.path.join( - os.path.dirname(__file__), 'mujoco', 'cmake') + os.path.dirname(__file__), 'mujoco', 'cmake' + ) cmake_args = [ f'-DPython3_ROOT_DIR:PATH={sys.prefix}', f'-DPython3_EXECUTABLE:STRING={sys.executable}', f'-DCMAKE_MODULE_PATH:PATH={cmake_module_path}', f'-DCMAKE_BUILD_TYPE:STRING={build_cfg}', f'-DCMAKE_LIBRARY_OUTPUT_DIRECTORY:PATH={self.build_temp}', - f'-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL={"OFF" if self.debug else "ON"}', + ( + f'-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL={"OFF" if self.debug else "ON"}' + ), '-DCMAKE_Fortran_COMPILER:STRING=', '-DBUILD_TESTING:BOOL=OFF', ] @@ -284,14 +303,17 @@ class BuildCMakeExtension(build_ext.build_ext): for arg in cmake_args: print(f' {arg}') subprocess.check_call( - [cmake] + cmake_args + - [os.path.join(os.path.dirname(__file__), 'mujoco')], - cwd=self.build_temp) + [cmake] + + cmake_args + + [os.path.join(os.path.dirname(__file__), 'mujoco')], + cwd=self.build_temp, + ) print('Building all extensions with CMake') subprocess.check_call( [cmake, '--build', '.', f'-j{os.cpu_count()}', '--config', build_cfg], - cwd=self.build_temp) + cwd=self.build_temp, + ) def build_extension(self, ext): dest_path = self.get_ext_fullpath(ext.name) @@ -331,6 +353,7 @@ class InstallScripts(install_scripts.install_scripts): else: self.outfiles.append(oldfile) + setuptools.setup( long_description=get_long_description(), long_description_content_type='text/markdown', @@ -350,7 +373,7 @@ setuptools.setup( CMakeExtension('mujoco._specs'), CMakeExtension('mujoco._structs'), ], - scripts=[ - 'mujoco/mjpython/mjpython.py' - ] if platform.system() == 'Darwin' else [], + scripts=['mujoco/mjpython/mjpython.py'] + if platform.system() == 'Darwin' + else [], ) From a7fed25fdd1284bff7eed17c8bd71db38f92c57f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 10 Dec 2024 08:03:58 -0800 Subject: [PATCH 145/426] Document how to save `MjSpec` to XML. Fixes #2075 PiperOrigin-RevId: 704712337 Change-Id: I51c1a14c5c48ffc8e37d9e31162a3e70a3b10313 --- doc/python.rst | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/doc/python.rst b/doc/python.rst index 5a97f377..ddaeb0a1 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -479,6 +479,7 @@ Below is a minimal usage example, more examples can be found in the Model Editin import mujoco spec = mujoco.MjSpec() + spec.modelname = "my model" body = spec.worldbody.add_body( pos=[1, 2, 3], quat=[0, 1, 0, 0], @@ -503,9 +504,30 @@ The ``MjSpec`` object wraps the :ref:`mjSpec` struct and can be constructed in t Note the ``from_string()`` and ``from_file()`` methods can only be called at construction time. -Attachments +Save to XML ----------- +Compiled ``MjSpec`` objects can be saved to XML string with the ``to_xml()`` method: + +.. code-block:: python + + print(spec.to_xml()) + +.. code-block:: XML + + + + + + + + + + + +Attachment +---------- + It is possible to combine multiple specs by using attachments. The following options are possible: - Attach a body from the child spec to a frame in the parent spec: ``body.attach_body(body, prefix, suffix)``, returns From a364308d5ab74bdfc5b32f8351c68692abe3e718 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 10 Dec 2024 08:52:26 -0800 Subject: [PATCH 146/426] Use GJK to recover contact points for sphere and capsule geoms in nativeccd. PiperOrigin-RevId: 704727907 Change-Id: Iff641b6a32637c48e9892dbaa60826617c54ed06 --- mjx/mujoco/mjx/_src/collision_driver_test.py | 7 +- src/engine/engine_collision_convex.c | 37 ++++++++- src/engine/engine_collision_convex.h | 6 ++ src/engine/engine_collision_gjk.c | 85 +++++++++++++++++++- test/engine/engine_collision_gjk_test.cc | 4 +- 5 files changed, 131 insertions(+), 8 deletions(-) diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index 068856d7..4b083223 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -56,6 +56,7 @@ def _collide( d = mujoco.MjData(m) dx = mjx.put_data(m, d) + m.opt.enableflags |= mujoco.mjtEnableBit.mjENBL_NATIVECCD mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) kinematics_jit_fn = jax.jit(mjx.kinematics) @@ -241,7 +242,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5 + dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-2 ) _ELLIPSOID_SPHERE = """ @@ -265,7 +266,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3 + dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-4 ) _ELLIPSOID_CAPSULE = """ @@ -288,7 +289,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3 + dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-5 ) _ELLIPSOID_CYLINDER = """ diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 35211837..712841a8 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -142,6 +142,16 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu +// point support function +void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom; + res[0] = pos[0]; + res[1] = pos[1]; + res[2] = pos[2]; +} + + + // sphere support function static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -158,6 +168,31 @@ static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) +// line support function (capsule) +void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjModel* m = obj->model; + const mjData* d = obj->data; + + // capsule data + int i = 3*obj->geom; + const mjtNum* mat = d->geom_xmat + 3*i; + const mjtNum* pos = d->geom_xpos + i; + mjtNum length = m->geom_size[i+1]; + + // rotate dir to geom local frame + mjtNum local_dir[3], tmp[3]; + mulMatTVec3(local_dir, mat, dir); + + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = (local_dir[2] >= 0 ? length : -length); + + // transform result to global frame + localToGlobal(res, mat, tmp, pos); +} + + + // capsule support function static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -180,7 +215,7 @@ static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3] tmp[2] = local_dir[2] * radius; // add cylinder contribution - tmp[2] += mju_sign(local_dir[2]) * length; + tmp[2] += (local_dir[2] >= 0 ? length : -length); // transform result to global frame localToGlobal(res, mat, tmp, pos); diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index c402c9c6..407a856c 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -73,6 +73,12 @@ MJAPI void mjccd_center(const void *obj, ccd_vec3_t *center); // libccd support function MJAPI void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); +// support function for point +void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); + +// support function for line (capsule) +void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); + // pairwise geom collision functions using ccd int mjc_PlaneConvex (const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 493a6ed5..1be326de 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -156,7 +156,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum* x1_k = status->x1; // the kth approximation point for obj1 mjtNum* x2_k = status->x2; // the kth approximation point for obj2 mjtNum x_k[3]; // the kth approximation point in Minkowski difference - mjtNum lambda[4]; // barycentric coordinates for x_k + mjtNum lambda[4] = {1, 0, 0, 0}; // barycentric coordinates for x_k mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff; // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 @@ -179,6 +179,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum diff[3]; sub3(diff, x_k, s_k); if (2*dot3(x_k, diff) < epsilon) { + if (!k) n = 1; break; } @@ -1374,9 +1375,30 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o +// inflate a contact by margin +static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) { + mjtNum n[3]; + sub3(n, status->x2, status->x1); + mju_normalize3(n); + if (margin1) { + status->x1[0] += margin1 * n[0]; + status->x1[1] += margin1 * n[1]; + status->x1[2] += margin1 * n[2]; + } + if (margin2) { + status->x2[0] -= margin2 * n[0]; + status->x2[1] -= margin2 * n[1]; + status->x2[2] -= margin2 * n[2]; + } + status->dist -= (margin1 + margin2); +} + + + // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // set up + mjtNum dist; obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); status->gjk_iterations = 0; @@ -1386,7 +1408,66 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m status->max_contacts = config->max_contacts; status->dist_cutoff = config->dist_cutoff; - mjtNum dist = gjk(status, obj1, obj2); + // special handling for sphere and capsule (shrink to point and line respectively) + if (obj1->geom_type == mjGEOM_SPHERE || obj2->geom_type == mjGEOM_SPHERE || + obj1->geom_type == mjGEOM_CAPSULE || obj2->geom_type == mjGEOM_CAPSULE) { + void (*support1)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj1->support; + void (*support2)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj2->support; + mjtNum margin1 = 0, margin2 = 0; + + if (obj1->geom_type == mjGEOM_SPHERE) { + const mjModel* m = obj1->model; + margin1 = m->geom_size[3*obj1->geom]; + support1 = obj1->support; + obj1->support = mjc_pointSupport; + } else if (obj1->geom_type == mjGEOM_CAPSULE) { + const mjModel* m = obj1->model; + margin1 = m->geom_size[3*obj1->geom]; + support1 = obj1->support; + obj1->support = mjc_lineSupport; + } + + if (obj2->geom_type == mjGEOM_SPHERE) { + const mjModel* m = obj2->model; + margin2 = m->geom_size[3*obj2->geom]; + support2 = obj2->support; + obj2->support = mjc_pointSupport; + } else if (obj2->geom_type == mjGEOM_CAPSULE) { + const mjModel* m = obj2->model; + margin2 = m->geom_size[3*obj2->geom]; + support2 = obj2->support; + obj2->support = mjc_lineSupport; + } + + status->dist_cutoff += margin1 + margin2; + dist = gjk(status, obj1, obj2); + status->dist_cutoff = config->dist_cutoff; + + // shallow penetration, inflate contact + if (dist > 0) { + inflate(status, margin1, margin2); + if (status->dist > status->dist_cutoff) { + status->dist = mjMAXVAL; + } + return status->dist; + } + + // contact not needed + if (!config->max_contacts) { + status->nx = 0; + status->dist = 0; + return 0; + } + + // deep penetration, reset everything and run GJK again + status->gjk_iterations = 0; + obj1->support = support1; + obj2->support = support2; + obj1->center(status->x1, obj1); + obj2->center(status->x2, obj2); + } + + dist = gjk(status, obj1, obj2); // penetration recovery for contacts not needed if (!config->max_contacts) { diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index aea0e260..3093d82a 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -245,8 +245,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { // direction EXPECT_NEAR(dir[0], 1, kTolerance); - EXPECT_NEAR(dir[1], 0, 0.001); - EXPECT_NEAR(dir[2], 0, 0.001); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], 0, kTolerance); // position EXPECT_NEAR(pos[0], 1, kTolerance); From f4593d5d79d68f1b070157127b894bf667a37508 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 10 Dec 2024 10:05:53 -0800 Subject: [PATCH 147/426] Highlight XML strings using pygments in mjSpec notebook. PiperOrigin-RevId: 704751058 Change-Id: Iaf7fae7f13ef12d59bac762170a6dfca01494164 --- python/mjspec.ipynb | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index f6fa9fe9..c1e1fbf9 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -109,7 +109,7 @@ "# More legible printing from numpy.\n", "np.set_printoptions(precision=3, suppress=True, linewidth=100)\n", "\n", - "from IPython.display import clear_output\n", + "from IPython.display import clear_output, HTML, display\n", "clear_output()\n", "\n", "# Get MuJoCo's humanoid model and a Franka arm from the MuJoCo Menagerie.\n", @@ -121,6 +121,12 @@ "!git clone https://github.com/google-deepmind/mujoco_menagerie\n", "franka_file = 'mujoco_menagerie/franka_fr3/fr3.xml'\n", "\n", + "def print_xml(xml_string):\n", + " formatter = pygments.formatters.HtmlFormatter(style='lovelace')\n", + " lexer = pygments.lexers.XmlLexer()\n", + " highlighted = pygments.highlight(xml_string, lexer, formatter)\n", + " display(HTML(f\"{highlighted}\"))\n", + "\n", "def render(model, data=None, height=250):\n", " if data is None:\n", " data = mj.MjData(model)\n", @@ -172,6 +178,7 @@ "render(model)\n", "\n", "# Change the mjSpec, re-compile and re-render\n", + "spec.modelname = \"edited model\"\n", "geoms = spec.worldbody.find_all(mj.mjtObj.mjOBJ_GEOM)\n", "geoms[0].name = 'blue_box'\n", "geoms[0].rgba = [0, 0, 1, 1]\n", @@ -193,7 +200,7 @@ "id": "Tw_yUwqxKwCI" }, "source": [ - "`mjSpec` can save XML to string, saving all modifications." + "`mjSpec` can save XML to string, with all modifications:" ] }, { @@ -204,9 +211,7 @@ }, "outputs": [], "source": [ - "#@title Print an XML from an `mjSpec` {vertical-output: true}\n", - "\n", - "print(spec.to_xml())" + "print_xml(spec.to_xml())" ] }, { From 049a26010260c95c7802797206b928b14dff6fe8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 10 Dec 2024 11:17:51 -0800 Subject: [PATCH 148/426] Remove superfluous newlines in Schema exception messages. PiperOrigin-RevId: 704781176 Change-Id: Ie23ac2deab4a5173e1e7a60ffe9e751fe054a2dc --- src/xml/xml_native_reader.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index e25626d2..70d3342b 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -837,15 +837,13 @@ void mjXReader::PrintSchema(std::stringstream& str, bool html, bool pad) { void mjXReader::Parse(XMLElement* root, const mjVFS* vfs) { // check schema if (!schema.GetError().empty()) { - throw mjXError(0, "XML Schema Construction Error: %s\n", - schema.GetError().c_str()); + throw mjXError(0, "XML Schema Construction Error: %s", schema.GetError().c_str()); } // validate XMLElement* bad = 0; if ((bad = schema.Check(root, 0))) { - throw mjXError(bad, "Schema violation: %s\n", - schema.GetError().c_str()); + throw mjXError(bad, "Schema violation: %s", schema.GetError().c_str()); } // get model name From 3ca97248a32a460d90b08a323a4447ad14591c58 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 11 Dec 2024 04:10:06 -0800 Subject: [PATCH 149/426] Fix comparison in engine_core_constraint_test. (was comparing number to itself) PiperOrigin-RevId: 705053943 Change-Id: I3d179ab4bb2ed1bc1b74b4edec7ce1114ad16b37 --- test/engine/engine_core_constraint_test.cc | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 8baeeadc..a0a8f149 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -477,6 +477,7 @@ TEST_F(CoreConstraintTest, MulJacTVecIsland) { mj_deleteModel(model); } +// compare mj_constraintUpdate and mj_constraintUpdate_island TEST_F(CoreConstraintTest, ConstraintUpdateIsland) { const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); @@ -557,12 +558,15 @@ TEST_F(CoreConstraintTest, ConstraintUpdateIsland) { } // compare cone Hessians - for (int c=0; c < data2->ncon; c++) { - int efcadr = data2->contact[c].efc_address; - if (data2->efc_island[efcadr] == island) { - for (int j=0; j < 36; j++) { - EXPECT_THAT(data2->contact[c].H[j], - DoubleNear(data2->contact[c].H[j], 1e-12)); + if (cone == mjCONE_ELLIPTIC) { + for (int c=0; c < data2->ncon; c++) { + int efcadr = data2->contact[c].efc_address; + if (data2->efc_island[efcadr] == island && + data2->efc_state[efcadr] == mjCNSTRSTATE_CONE) { + for (int j=0; j < 36; j++) { + EXPECT_THAT(data2->contact[c].H[j], + DoubleNear(data1->contact[c].H[j], 1e-12)); + } } } } From 26918875006dec8d924dcb779fb2d8cab64a3b4d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 11 Dec 2024 09:03:39 -0800 Subject: [PATCH 150/426] Add engine-internal convenience macro for allocating typed arrays, improve error message. PiperOrigin-RevId: 705126655 Change-Id: I2bd8fada6d33a919d2fb82297f93ac57958355a4 --- python/mujoco/bindings_test.py | 2 +- src/engine/engine_collision_driver.c | 36 ++++++-------- src/engine/engine_collision_gjk.c | 16 +++---- src/engine/engine_collision_sdf.c | 4 +- src/engine/engine_core_constraint.c | 60 ++++++++++++------------ src/engine/engine_core_smooth.c | 54 ++++++++++----------- src/engine/engine_derivative.c | 40 ++++++++-------- src/engine/engine_derivative_fd.c | 54 ++++++++++----------- src/engine/engine_forward.c | 34 +++++++------- src/engine/engine_inverse.c | 18 +++---- src/engine/engine_io.c | 56 ++++++++++++++-------- src/engine/engine_io.h | 10 +++- src/engine/engine_island.c | 12 ++--- src/engine/engine_passive.c | 2 +- src/engine/engine_print.c | 2 +- src/engine/engine_ray.c | 4 +- src/engine/engine_sensor.c | 4 +- src/engine/engine_setconst.c | 14 +++--- src/engine/engine_solver.c | 70 ++++++++++++++-------------- src/engine/engine_support.c | 36 +++++++------- src/engine/engine_util_container.c | 3 +- src/engine/engine_util_solve.c | 8 ++-- src/engine/engine_util_sparse.c | 10 ++-- src/engine/engine_vis_interact.c | 4 +- src/engine/engine_vis_visualize.c | 4 +- 25 files changed, 283 insertions(+), 274 deletions(-) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index daed5d2c..6bfebb90 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1060,7 +1060,7 @@ Euler integrator, semi-implicit in velocity. def test_can_raise_error(self): self.data.pstack = self.data.narena with self.assertRaisesRegex( - mujoco.FatalError, r'\Amj_stackAlloc: insufficient memory:' + mujoco.FatalError, r'\Amj_stackAlloc: out of memory, stack overflow' ): mujoco.mj_forward(self.model, self.data) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index ef5710a6..f46d8157 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -289,7 +289,7 @@ void mj_collision(const mjModel* m, mjData* d) { // broadphase collision detector TM_START; int nmaxpairs = (nbodyflex*(nbodyflex - 1))/2; - int* broadphasepair = mj_stackAllocInt(d, nmaxpairs); + int* broadphasepair = mjSTACKALLOC(d, nmaxpairs, int); int nbfpair = mj_broadphase(m, d, broadphasepair, nmaxpairs); unsigned int last_signature = -1; TM_END(mjTIMER_COL_BROAD); @@ -368,8 +368,7 @@ void mj_collision(const mjModel* m, mjData* d) { int n = ncon_after - ncon_before; if (n > 1) { mj_markStack(d); - mjContact* buf = (mjContact*)mj_stackAllocByte(d, n * sizeof(mjContact), - _Alignof(mjContact)); + mjContact* buf = mjSTACKALLOC(d, n, mjContact); contactSort(d->contact + ncon_before, buf, n, (void*)m); mj_freeStack(d); } @@ -496,15 +495,6 @@ struct mjCollisionTree_ { typedef struct mjCollisionTree_ mjCollisionTree; - -// collision tree allocation -static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) { - return (mjCollisionTree*) mj_stackAllocByte( - d, max_stack * sizeof(mjCollisionTree), _Alignof(mjCollisionTree)); -} - - - // checks if the proposed collision pair is already present in pair_geom and calls narrow phase void mj_collideGeomPair(const mjModel* m, mjData* d, int g1, int g2, int merged, int startadr, int pairadr) { @@ -667,7 +657,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, // TODO(b/273737633): Store bvh max depths to make this bound tighter. const int max_stack = (isbody1 ? m->body_bvhnum[bf1] : m->flex_bvhnum[f1]) + (isbody2 ? m->body_bvhnum[bf2] : m->flex_bvhnum[f2]); - mjCollisionTree* stack = mj_stackAllocTree(d, max_stack); + mjCollisionTree* stack = mjSTACKALLOC(d, max_stack, mjCollisionTree); int nstack = 1; stack[0].node1 = stack[0].node2 = 0; @@ -1037,8 +1027,8 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int } // allocate sort buffer - mjtSAP* sortbuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); - mjtSAP* activebuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); + mjtSAP* sortbuf = mjSTACKALLOC(d, 2*n, mjtSAP); + mjtSAP* activebuf = mjSTACKALLOC(d, 2*n, mjtSAP); // init sortbuf with specified axis for (int i=0; i < n; i++) { @@ -1049,7 +1039,7 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int } // sort along specified axis - mjtSAP* buf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); + mjtSAP* buf = mjSTACKALLOC(d, 2*n, mjtSAP); SAPsort(sortbuf, buf, 2*n, NULL); // define the other two axes @@ -1235,7 +1225,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { // allocate collidable bodyflex ids, construct list mj_markStack(d); - int* bfid = mj_stackAllocInt(d, nbodyflex); + int* bfid = mjSTACKALLOC(d, nbodyflex, int); int ncollide = 0; for (int i=1; i < nbodyflex; i++) { if (canCollide(m, i)) { @@ -1245,14 +1235,14 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { if (ncollide > 1) { // allocate and construct AAMMs for collidable only - mjtNum* aamm = mj_stackAllocNum(d, 6*ncollide); + mjtNum* aamm = mjSTACKALLOC(d, 6*ncollide, mjtNum); for (int i=0; i < ncollide; i++) { makeAAMM(m, d, aamm+6*i, bfid[i], frame); } // call SAP int maxsappair = ncollide*(ncollide-1)/2; - int* sappair = mj_stackAllocInt(d, maxsappair); + int* sappair = mjSTACKALLOC(d, maxsappair, int); int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair); if (nsappair < 0) { mjERROR("SAP failed"); @@ -1283,7 +1273,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { // sort bodyflex pairs by signature if (npair > 1) { - int* buf = mj_stackAllocInt(d, npair); + int* buf = mjSTACKALLOC(d, npair, int); bfsort(bfpair, buf, npair, NULL); } @@ -1800,7 +1790,7 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { mj_markStack(d); // allocate and construct active element ids - int* elid = mj_stackAllocInt(d, m->flex_elemnum[f]); + int* elid = mjSTACKALLOC(d, m->flex_elemnum[f], int); int nactive = 0; int flex_elemnum = m->flex_elemnum[f]; for (int i=0; i < flex_elemnum; i++) { @@ -1816,7 +1806,7 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { } // allocate and construct AAMMs for active elements - mjtNum* aamm = mj_stackAllocNum(d, 6*nactive); + mjtNum* aamm = mjSTACKALLOC(d, 6*nactive, mjtNum); const mjtNum* elemaabb = d->flexelem_aabb + 6*m->flex_elemadr[f]; for (int i=0; i < nactive; i++) { mju_sub3(aamm+6*i+0, elemaabb+6*elid[i], elemaabb+6*elid[i]+3); @@ -1829,7 +1819,7 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { // call SAP; hard limit on number of pairs to avoid out-of-memory int maxsappair = mjMIN(nactive*(nactive-1)/2, 1000000); - int* sappair = mj_stackAllocInt(d, maxsappair); + int* sappair = mjSTACKALLOC(d, maxsappair, int); int nsappair = mj_SAP(d, aamm, nactive, axis, sappair, maxsappair); if (nsappair < 0) { mjERROR("SAP failed"); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 1be326de..e8c4653f 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1291,8 +1291,8 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o // initialize horizon Horizon h; mj_markStack(d); - h.indices = mj_stackAllocInt(d, 6 + status->max_iterations); - h.edges = mj_stackAllocInt(d, 6 + status->max_iterations); + h.indices = mjSTACKALLOC(d, 6 + status->max_iterations, int); + h.edges = mjSTACKALLOC(d, 6 + status->max_iterations, int); h.nedges = 0; h.pt = pt; @@ -1483,9 +1483,9 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m pt.nfaces = pt.nmap = pt.nverts = 0; // allocate memory for vertices - pt.verts = mj_stackAllocNum(d, 3*(5 + N)); - pt.verts1 = mj_stackAllocNum(d, 3*(5 + N)); - pt.verts2 = mj_stackAllocNum(d, 3*(5 + N)); + pt.verts = mjSTACKALLOC(d, 3*(5 + N), mjtNum); + pt.verts1 = mjSTACKALLOC(d, 3*(5 + N), mjtNum); + pt.verts2 = mjSTACKALLOC(d, 3*(5 + N), mjtNum); // allocate memory for faces pt.maxfaces = (6*N > 1000) ? 6*N : 1000; // use 1000 faces as lower bound @@ -1497,11 +1497,9 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m size_t max_size = mj_stackBytesAvailable(d) - 12*(N * sizeof(int)); if (size1 + size2 > max_size) { pt.maxfaces = max_size / (sizeof(Face) + sizeof(Face*)); - size1 = sizeof(Face) * pt.maxfaces; - size2 = sizeof(Face*) * pt.maxfaces; } - pt.faces = mj_stackAllocByte(d, size1, _Alignof(Face)); - pt.map = mj_stackAllocByte(d, size2, _Alignof(Face*)); + pt.faces = mjSTACKALLOC(d, pt.maxfaces, Face); + pt.map = mjSTACKALLOC(d, pt.maxfaces, Face*); int ret; if (status->nsimplex == 2) { diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index e01de86f..c6fe8eba 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -516,9 +516,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, int node; }; typedef struct CollideTreeArgs_ CollideTreeArgs; - CollideTreeArgs* stack = (CollideTreeArgs*) mj_stackAllocByte( - d, max_stack * sizeof(CollideTreeArgs), _Alignof(CollideTreeArgs)); - + CollideTreeArgs* stack = mjSTACKALLOC(d, max_stack, CollideTreeArgs); int nstack = 0; stack[nstack].node = 0; nstack++; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 2c62248a..8d380e9e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -485,14 +485,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mj_markStack(d); // allocate space - jac[0] = mj_stackAllocNum(d, 6*nv); - jac[1] = mj_stackAllocNum(d, 6*nv); - jacdif = mj_stackAllocNum(d, 6*nv); + jac[0] = mjSTACKALLOC(d, 6*nv, mjtNum); + jac[1] = mjSTACKALLOC(d, 6*nv, mjtNum); + jacdif = mjSTACKALLOC(d, 6*nv, mjtNum); if (issparse) { - chain = mj_stackAllocInt(d, nv); - chain2 = mj_stackAllocInt(d, nv); - buf_ind = mj_stackAllocInt(d, nv); - sparse_buf = mj_stackAllocNum(d, nv); + chain = mjSTACKALLOC(d, nv, int); + chain2 = mjSTACKALLOC(d, nv, int); + buf_ind = mjSTACKALLOC(d, nv, int); + sparse_buf = mjSTACKALLOC(d, nv, mjtNum); } // find active equality constraints @@ -756,7 +756,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { mj_markStack(d); // allocate Jacobian - jac = mj_stackAllocNum(d, nv); + jac = mjSTACKALLOC(d, nv, mjtNum); // find frictional dofs for (int i=0; i < nv; i++) { @@ -813,7 +813,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { mj_markStack(d); // allocate Jacobian - jac = mj_stackAllocNum(d, nv); + jac = mjSTACKALLOC(d, nv, mjtNum); // find joint limits for (int i=0; i < m->njnt; i++) { @@ -953,16 +953,16 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { mj_markStack(d); // allocate Jacobian - jac = mj_stackAllocNum(d, 6*nv); - jacdif = mj_stackAllocNum(d, 6*nv); + jac = mjSTACKALLOC(d, 6*nv, mjtNum); + jacdif = mjSTACKALLOC(d, 6*nv, mjtNum); jacdifp = jacdif; jacdifr = jacdif + 3*nv; - jac1p = mj_stackAllocNum(d, 3*nv); - jac2p = mj_stackAllocNum(d, 3*nv); - jac1r = mj_stackAllocNum(d, 3*nv); - jac2r = mj_stackAllocNum(d, 3*nv); + jac1p = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2p = mjSTACKALLOC(d, 3*nv, mjtNum); + jac1r = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2r = mjSTACKALLOC(d, 3*nv, mjtNum); if (issparse) { - chain = mj_stackAllocInt(d, nv); + chain = mjSTACKALLOC(d, nv, int); } // find contacts to be included @@ -1589,8 +1589,8 @@ static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain, int nv = m->nv, NV; mj_markStack(d); - int* bodychain = mj_stackAllocInt(d, nv); - int* tempchain = mj_stackAllocInt(d, nv); + int* bodychain = mjSTACKALLOC(d, nv, int); + int* tempchain = mjSTACKALLOC(d, nv, int); // set first NV = mj_bodyChain(m, body[0], chain); @@ -1643,8 +1643,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { mj_markStack(d); if (nnz) { - chain = mj_stackAllocInt(d, nv); - chain2 = mj_stackAllocInt(d, nv); + chain = mjSTACKALLOC(d, nv, int); + chain2 = mjSTACKALLOC(d, nv, int); } // find active equality constraints @@ -1870,7 +1870,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { } mj_markStack(d); - int *chain = mj_stackAllocInt(d, m->nv); + int *chain = mjSTACKALLOC(d, m->nv, int); for (int i=0; i < ncon; i++) { mjContact* con = d->contact + i; @@ -2068,19 +2068,19 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { mj_markStack(d); // space for backsubM2(J')' and its traspose - mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv); - mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc); + mjtNum* JM2 = mjSTACKALLOC(d, nefc*nv, mjtNum); + mjtNum* JM2T = mjSTACKALLOC(d, nv*nefc, mjtNum); // sparse if (mj_isSparse(m)) { // space for JM2 and JM2T indices - int* rownnz = mj_stackAllocInt(d, nefc); - int* rowadr = mj_stackAllocInt(d, nefc); - int* colind = mj_stackAllocInt(d, nefc*nv); - int* rowsuper = mj_stackAllocInt(d, nefc); - int* rownnzT = mj_stackAllocInt(d, nv); - int* rowadrT = mj_stackAllocInt(d, nv); - int* colindT = mj_stackAllocInt(d, nv*nefc); + int* rownnz = mjSTACKALLOC(d, nefc, int); + int* rowadr = mjSTACKALLOC(d, nefc, int); + int* colind = mjSTACKALLOC(d, nefc*nv, int); + int* rowsuper = mjSTACKALLOC(d, nefc, int); + int* rownnzT = mjSTACKALLOC(d, nv, int); + int* rowadrT = mjSTACKALLOC(d, nv, int); + int* colindT = mjSTACKALLOC(d, nv*nefc, int); // construct JM2 = backsubM2(J')' by rows for (int r=0; r < nefc; r++) { diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index a11895b5..ca16cb30 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -184,7 +184,7 @@ void mj_comPos(const mjModel* m, mjData* d) { int nbody = m->nbody, njnt = m->njnt; mjtNum offset[3], axis[3]; mj_markStack(d); - mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody); + mjtNum* mass_subtree = mjSTACKALLOC(d, m->nbody, mjtNum); // clear subtree mju_zero(mass_subtree, m->nbody); @@ -393,7 +393,7 @@ void mj_camlight(const mjModel* m, mjData* d) { // update dynamic BVH; leaf aabbs must be updated before call void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { mj_markStack(d); - int* modified = mj_stackAllocInt(d, bvhnum); + int* modified = mjSTACKALLOC(d, bvhnum, int); mju_zeroInt(modified, bvhnum); // mark leafs as modified @@ -526,10 +526,10 @@ void mj_flex(const mjModel* m, mjData* d) { // allocate space mj_markStack(d); - mjtNum* jac1 = mj_stackAllocNum(d, 3*nv); - mjtNum* jac2 = mj_stackAllocNum(d, 3*nv); - mjtNum* jacdif = mj_stackAllocNum(d, 3*nv); - int* chain = issparse ? mj_stackAllocInt(d, nv) : NULL; + mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); + int* chain = issparse ? mjSTACKALLOC(d, nv, int) : NULL; // clear Jacobian: sparse or dense if (issparse) { @@ -631,14 +631,14 @@ void mj_tendon(const mjModel* m, mjData* d) { // allocate space mj_markStack(d); - jac1 = mj_stackAllocNum(d, 3*nv); - jac2 = mj_stackAllocNum(d, 3*nv); - jacdif = mj_stackAllocNum(d, 3*nv); - tmp = mj_stackAllocNum(d, nv); + jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); + jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); + tmp = mjSTACKALLOC(d, nv, mjtNum); if (issparse) { - chain = mj_stackAllocInt(d, nv); - buf_ind = mj_stackAllocInt(d, nv); - sparse_buf = mj_stackAllocNum(d, nv); + chain = mjSTACKALLOC(d, nv, int); + buf_ind = mjSTACKALLOC(d, nv, int); + sparse_buf = mjSTACKALLOC(d, nv, mjtNum); } // clear results @@ -863,9 +863,9 @@ void mj_transmission(const mjModel* m, mjData* d) { // allocate Jacbians mj_markStack(d); - mjtNum* jac = mj_stackAllocNum(d, 3*nv); - mjtNum* jacA = mj_stackAllocNum(d, 3*nv); - mjtNum* jacS = mj_stackAllocNum(d, 3*nv); + mjtNum* jac = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacA = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacS = mjSTACKALLOC(d, 3*nv, mjtNum); // define stack variables required for body transmission, don't allocate int issparse = mj_isSparse(m); @@ -1088,7 +1088,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // reference site defined else { int refid = m->actuator_trnid[2*i+1]; - if (!jacref) jacref = mj_stackAllocNum(d, 3*nv); + if (!jacref) jacref = mjSTACKALLOC(d, 3*nv, mjtNum); // initialize last dof address for each body int b0 = m->body_weldid[m->site_bodyid[id]]; @@ -1190,7 +1190,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_mulMatVec3(wrench, d->site_xmat+9*refid, gear+3); // moment_tmp: global Jacobian projected on wrench, add to moment - if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv); + if (!moment_tmp) moment_tmp = mjSTACKALLOC(d, nv, mjtNum); mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv); mju_addTo(moment+adr, moment_tmp, nv); } @@ -1220,12 +1220,12 @@ void mj_transmission(const mjModel* m, mjData* d) { { // allocate stack variables for the first mjTRN_BODY if (!efc_force) { - efc_force = mj_stackAllocNum(d, d->nefc); - moment_exclude = mj_stackAllocNum(d, nv); - jacdifp = mj_stackAllocNum(d, 3*nv); - jac1p = mj_stackAllocNum(d, 3*nv); - jac2p = mj_stackAllocNum(d, 3*nv); - chain = issparse ? mj_stackAllocInt(d, nv) : NULL; + efc_force = mjSTACKALLOC(d, d->nefc, mjtNum); + moment_exclude = mjSTACKALLOC(d, nv, mjtNum); + jacdifp = mjSTACKALLOC(d, 3*nv, mjtNum); + jac1p = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2p = mjSTACKALLOC(d, 3*nv, mjtNum); + chain = issparse ? mjSTACKALLOC(d, nv, int) : NULL; } // clear efc_force and moment_exclude @@ -1804,7 +1804,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { int nbody = m->nbody; mjtNum dx[3], dv[3], dp[3], dL[3]; mj_markStack(d); - mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody); + mjtNum* body_vel = mjSTACKALLOC(d, 6*m->nbody, mjtNum); // bodywise quantities for (int i=0; i < nbody; i++) { @@ -1871,8 +1871,8 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { int nbody = m->nbody, nv = m->nv; mjtNum tmp[6], tmp1[6]; mj_markStack(d); - mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6); - mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6); + mjtNum* loc_cacc = mjSTACKALLOC(d, m->nbody*6, mjtNum); + mjtNum* loc_cfrc_body = mjSTACKALLOC(d, m->nbody*6, mjtNum); // set world acceleration to -gravity mju_zero(loc_cacc, 6); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 15d48149..d4c764cb 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -395,11 +395,11 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; mj_markStack(d); - mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv); - mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv); - mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv); - mjtNum* Dcfrcbody = mj_stackAllocNum(d, nbody*6*nv); - mjtNum* row = mj_stackAllocNum(d, nv); + mjtNum* Dcvel = mjSTACKALLOC(d, nbody*6*nv, mjtNum); + mjtNum* Dcdofdot = mjSTACKALLOC(d, nv*6*nv, mjtNum); + mjtNum* Dcacc = mjSTACKALLOC(d, nbody*6*nv, mjtNum); + mjtNum* Dcfrcbody = mjSTACKALLOC(d, nbody*6*nv, mjtNum); + mjtNum* row = mjSTACKALLOC(d, nv, mjtNum); // compute Dcvel and Dcdofdot mjd_comVel_vel_dense(m, d, Dcvel, Dcdofdot); @@ -610,11 +610,11 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; mj_markStack(d); - mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD); - mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB); - mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB); - mjtNum* Dcfrcbody = mj_stackAllocNum(d, 6*m->nB); - mjtNum* row = mj_stackAllocNum(d, nv); + mjtNum* Dcdofdot = mjSTACKALLOC(d, 6*m->nD, mjtNum); + mjtNum* Dcvel = mjSTACKALLOC(d, 6*m->nB, mjtNum); + mjtNum* Dcacc = mjSTACKALLOC(d, 6*m->nB, mjtNum); + mjtNum* Dcfrcbody = mjSTACKALLOC(d, 6*m->nB, mjtNum); + mjtNum* row = mjSTACKALLOC(d, nv, mjtNum); // clear mju_zero(Dcdofdot, 6*m->nD); @@ -695,7 +695,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* // allocate dense row mj_markStack(d); - mjtNum* row = mj_stackAllocNum(d, nv); + mjtNum* row = mjSTACKALLOC(d, nv, mjtNum); // process non-zero elements of B for (int i=0; i < n; i++) { @@ -734,7 +734,7 @@ static void addJTBJSparse( // allocate row mj_markStack(d); - mjtNum* row = mj_stackAllocNum(d, nv); + mjtNum* row = mjSTACKALLOC(d, nv, mjtNum); // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) ) for (int i = 0; i < n; i++) { @@ -829,7 +829,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { // allocate dense actuator_moment row mj_markStack(d); - mjtNum* moment = mj_stackAllocNum(d, nv); + mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum); // process actuators for (int i=0; i < nu; i++) { @@ -1181,10 +1181,10 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { int nv = m->nv; int nnz = nv; int rownnz[6], rowadr[6]; - mjtNum* J = mj_stackAllocNum(d, 6*nv); - mjtNum* tmp = mj_stackAllocNum(d, 3*nv); - int* colind = mj_stackAllocInt(d, 6*nv); - int* colind_compressed = mj_stackAllocInt(d, 6*nv); + mjtNum* J = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* tmp = mjSTACKALLOC(d, 3*nv, mjtNum); + int* colind = mjSTACKALLOC(d, 6*nv, int); + int* colind_compressed = mjSTACKALLOC(d, 6*nv, int); mjtNum lvel[6], wind[6], lwind[6]; mjtNum geom_interaction_coef, magnus_lift_coef, kutta_lift_coef; @@ -1287,9 +1287,9 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { int nv = m->nv; int rownnz[6], rowadr[6]; - mjtNum* J = mj_stackAllocNum(d, 6*nv); - mjtNum* tmp = mj_stackAllocNum(d, 3*nv); - int* colind = mj_stackAllocInt(d, 6*nv); + mjtNum* J = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* tmp = mjSTACKALLOC(d, 3*nv, mjtNum); + int* colind = mjSTACKALLOC(d, 6*nv, int); mjtNum lvel[6], wind[6], lwind[6], box[3], B; mjtNum* inertia = m->body_inertia + 3*i; diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 239779af..b5741232 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -176,9 +176,9 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; mj_markStack(d); - mjtNum* qfrc_passive = mj_stackAllocNum(d, nv); - mjtNum* fd = mj_stackAllocNum(d, nv); - int* cnt = mj_stackAllocInt(d, nv); + mjtNum* qfrc_passive = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* fd = mjSTACKALLOC(d, nv, mjtNum); + int* cnt = mjSTACKALLOC(d, nv, int); // clear row counters mju_zeroInt(cnt, nv); @@ -227,10 +227,10 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; mj_markStack(d); - mjtNum* plus = mj_stackAllocNum(d, nv); - mjtNum* minus = mj_stackAllocNum(d, nv); - mjtNum* fd = mj_stackAllocNum(d, nv); - int* cnt = mj_stackAllocInt(d, nv); + mjtNum* plus = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* minus = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* fd = mjSTACKALLOC(d, nv, mjtNum); + int* cnt = mjSTACKALLOC(d, nv, int); // clear row counters mju_zeroInt(cnt, nv); @@ -314,20 +314,20 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, unsigned int restore_spec = mjSTATE_FULLPHYSICS | mjSTATE_CTRL; restore_spec |= mjDISABLED(mjDSBL_WARMSTART) ? 0 : mjSTATE_WARMSTART; - mjtNum *fullstate = mj_stackAllocNum(d, mj_stateSize(m, restore_spec)); - mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state - mjtNum *next = mj_stackAllocNum(d, nq+nv+na); // next state - mjtNum *next_plus = mj_stackAllocNum(d, nq+nv+na); // forward-nudged next state - mjtNum *next_minus = mj_stackAllocNum(d, nq+nv+na); // backward-nudged next state + mjtNum *fullstate = mjSTACKALLOC(d, mj_stateSize(m, restore_spec), mjtNum); + mjtNum *state = mjSTACKALLOC(d, nq+nv+na, mjtNum); // current state + mjtNum *next = mjSTACKALLOC(d, nq+nv+na, mjtNum); // next state + mjtNum *next_plus = mjSTACKALLOC(d, nq+nv+na, mjtNum); // forward-nudged next state + mjtNum *next_minus = mjSTACKALLOC(d, nq+nv+na, mjtNum); // backward-nudged next state // sensors int skipsensor = !DsDq && !DsDv && !DsDa && !DsDu; - mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values - mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // forward-nudged sensors - mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // backward-nudged sensors + mjtNum *sensor = skipsensor ? NULL : mjSTACKALLOC(d, ns, mjtNum); // sensor values + mjtNum *sensor_plus = skipsensor ? NULL : mjSTACKALLOC(d, ns, mjtNum); // forward-nudged + mjtNum *sensor_minus = skipsensor ? NULL : mjSTACKALLOC(d, ns, mjtNum); // backward-nudged // controls - mjtNum *ctrl = mj_stackAllocNum(d, nu); + mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum); // save current inputs mj_getState(m, d, fullstate, restore_spec); @@ -485,7 +485,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference positions: skip=mjSTAGE_NONE if (DyDq || DsDq) { - mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation + mjtNum *dpos = mjSTACKALLOC(d, nv, mjtNum); // allocate position perturbation for (int i=0; i < nv; i++) { // nudge forward mju_zero(dpos, nv); @@ -563,10 +563,10 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente mj_markStack(d); // allocate transposed matrices - mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed) - mjtNum *BT = B ? mj_stackAllocNum(d, nu*ndx) : NULL; // control-transition matrix (transposed) - mjtNum *CT = C ? mj_stackAllocNum(d, ndx*ns) : NULL; // state-observation matrix (transposed) - mjtNum *DT = D ? mj_stackAllocNum(d, nu*ns) : NULL; // control-observation matrix (transposed) + mjtNum *AT = A ? mjSTACKALLOC(d, ndx*ndx, mjtNum) : NULL; // state-transition (transposed) + mjtNum *BT = B ? mjSTACKALLOC(d, nu*ndx, mjtNum) : NULL; // control-transition (transposed) + mjtNum *CT = C ? mjSTACKALLOC(d, ndx*ns, mjtNum) : NULL; // state-observation (transposed) + mjtNum *DT = D ? mjSTACKALLOC(d, nu*ns, mjtNum) : NULL; // control-observation (transposed) // set offset pointers if (A) { @@ -629,11 +629,11 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio // local vectors mj_markStack(d); - mjtNum *pos = mj_stackAllocNum(d, nq); // position - mjtNum *force = mj_stackAllocNum(d, nv); // force - mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force - mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values - mjtNum *mass = DmDq ? mj_stackAllocNum(d, nM) : NULL; // mass matrix + mjtNum *pos = mjSTACKALLOC(d, nq, mjtNum); // position + mjtNum *force = mjSTACKALLOC(d, nv, mjtNum); // force + mjtNum *force_plus = mjSTACKALLOC(d, nv, mjtNum); // nudged force + mjtNum *sensor = skipsensor ? NULL : mjSTACKALLOC(d, ns, mjtNum); // sensor values + mjtNum *mass = DmDq ? mjSTACKALLOC(d, nM, mjtNum) : NULL; // mass matrix // save current positions mju_copy(pos, d->qpos, nq); @@ -687,7 +687,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio // position: skip = mjSTAGE_NONE if (DfDq || DsDq || DmDq) { - mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation + mjtNum *dpos = mjSTACKALLOC(d, nv, mjtNum); // allocate position perturbation for (int i=0; i < nv; i++) { // nudge mju_zero(dpos, nv); diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 181db4d3..b90ba5d8 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -286,7 +286,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // local, clamped copy of ctrl mj_markStack(d); - mjtNum *ctrl = mj_stackAllocNum(d, nu); + mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum); mju_copy(ctrl, d->ctrl, nu); if (!mjDISABLED(mjDSBL_CLAMPCTRL)) { clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL); @@ -531,7 +531,7 @@ static void warmstart(const mjModel* m, mjData* d) { // warmstart with best of (qacc_warmstart, qacc_smooth) if (!mjDISABLED(mjDSBL_WARMSTART)) { mj_markStack(d); - mjtNum* jar = mj_stackAllocNum(d, nefc); + mjtNum* jar = mjSTACKALLOC(d, nefc, mjtNum); // start with qacc = qacc_warmstart mju_copy(d->qacc, d->qacc_warmstart, nv); @@ -548,7 +548,7 @@ static void warmstart(const mjModel* m, mjData* d) { if (m->opt.solver == mjSOL_PGS) { // cost(force_warmstart) mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc); - mjtNum* ARf = mj_stackAllocNum(d, nefc); + mjtNum* ARf = mjSTACKALLOC(d, nefc, mjtNum); if (mj_isSparse(m)) mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, @@ -568,7 +568,7 @@ static void warmstart(const mjModel* m, mjData* d) { // non-PGS else { // add Gauss to cost(qacc_warmstart) - mjtNum* Ma = mj_stackAllocNum(d, nv); + mjtNum* Ma = mjSTACKALLOC(d, nv, mjtNum); mj_mulM(m, d, Ma, d->qacc_warmstart); for (int i=0; i < nv; i++) { cost_warmstart += 0.5*(Ma[i]-d->qfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]); @@ -618,9 +618,9 @@ void* mj_solCG_island_wrapper(void* args) { void mj_solCG_island_multithreaded(const mjModel* m, mjData* d) { mj_markStack(d); // allocate array of arguments to be passed to threads - mjSolIslandArgs* sol_cg_island_args = - mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs)); - mjTask* tasks = mj_stackAllocByte(d, sizeof(mjTask) * d->nisland, _Alignof(mjTask)); + mjSolIslandArgs* sol_cg_island_args = mjSTACKALLOC(d, d->nisland, mjSolIslandArgs); + mjTask* tasks = mjSTACKALLOC(d, d->nisland, mjTask); + for (int island = 0; island < d->nisland; ++island) { sol_cg_island_args[island].m = m; @@ -772,8 +772,8 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; int nv = m->nv, nM = m->nM; mj_markStack(d); - mjtNum* qfrc = mj_stackAllocNum(d, nv); - mjtNum* qacc = mj_stackAllocNum(d, nv); + mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum); // check for dof damping if disable flag is not set int dof_damping = 0; @@ -794,7 +794,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // damping: integrate implicitly else { if (!skipfactor) { - mjtNum* MhB = mj_stackAllocNum(d, nM); + mjtNum* MhB = mjSTACKALLOC(d, nM, mjtNum); // MhB = M + h*diag(B) mju_copy(MhB, d->qM, nM); @@ -857,10 +857,10 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // allocate space for intermediate solutions mj_markStack(d); - dX = mj_stackAllocNum(d, 2*nv+na); + dX = mjSTACKALLOC(d, 2*nv+na, mjtNum); for (int i=0; i < N; i++) { - X[i] = mj_stackAllocNum(d, nq+nv+na); - F[i] = mj_stackAllocNum(d, nv+na); + X[i] = mjSTACKALLOC(d, nq+nv+na, mjtNum); + F[i] = mjSTACKALLOC(d, nv+na, mjtNum); } // precompute C and T; C,T,A have size (N-1) @@ -941,8 +941,8 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv, nM = m->nM, nD = m->nD; mj_markStack(d); - mjtNum* qfrc = mj_stackAllocNum(d, nv); - mjtNum* qacc = mj_stackAllocNum(d, nv); + mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* qacc = mjSTACKALLOC(d, nv, mjtNum); // set qfrc = qfrc_smooth + qfrc_constraint mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); @@ -962,7 +962,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD); // factorize qLU - int* scratch = mj_stackAllocInt(d, nv); + int* scratch = mjSTACKALLOC(d, nv, int); mju_factorLUSparse(d->qLU, nv, scratch, d->D_rownnz, d->D_rowadr, d->D_colind); } @@ -977,7 +977,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mjd_smooth_vel(m, d, /* flg_bias = */ 0); // modified mass matrix MhB = qDeriv[Lower] - mjtNum* MhB = mj_stackAllocNum(d, nM); + mjtNum* MhB = mjSTACKALLOC(d, nM, mjtNum); for (int i=0; i < nM; i++) { MhB[i] = d->qDeriv[d->mapD2M[i]]; } diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index f582728e..f372947a 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -76,7 +76,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mjtNum *qacc = d->qacc; mj_markStack(d); - mjtNum* qfrc = mj_stackAllocNum(d, nv); + mjtNum* qfrc = mjSTACKALLOC(d, nv, mjtNum); // use selected integrator switch ((mjtIntegrator) m->opt.integrator) { @@ -132,11 +132,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mjd_smooth_vel(m, d, /* flg_bias = */ 0); // save mass matrix - mjtNum* qMsave = mj_stackAllocNum(d, m->nM); + mjtNum* qMsave = mjSTACKALLOC(d, m->nM, mjtNum); mju_copy(qMsave, d->qM, m->nM); // set M = M - dt*qDeriv (reduced to M nonzeros) - mjtNum* qDerivReduced = mj_stackAllocNum(d, m->nM); + mjtNum* qDerivReduced = mjSTACKALLOC(d, m->nM, mjtNum); for (int i=0; i < nM; i++) { qDerivReduced[i] = d->qDeriv[d->mapD2M[i]]; } @@ -171,7 +171,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { } mj_markStack(d); - mjtNum* jar = mj_stackAllocNum(d, nefc); + mjtNum* jar = mjSTACKALLOC(d, nefc, mjtNum); // compute jar = Jac*qacc - aref mj_mulJacVec(m, d, jar, d->qacc); @@ -218,7 +218,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d, if (mjENABLED(mjENBL_INVDISCRETE)) { // save current qacc - qacc = mj_stackAllocNum(d, nv); + qacc = mjSTACKALLOC(d, nv, mjtNum); mju_copy(qacc, d->qacc, nv); // modify qacc in-place @@ -271,10 +271,10 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { // allocate mj_markStack(d); - qforce = mj_stackAllocNum(d, nv); - dif = mj_stackAllocNum(d, nv); - save_qfrc_constraint = mj_stackAllocNum(d, nv); - save_efc_force = mj_stackAllocNum(d, nefc); + qforce = mjSTACKALLOC(d, nv, mjtNum); + dif = mjSTACKALLOC(d, nv, mjtNum); + save_qfrc_constraint = mjSTACKALLOC(d, nv, mjtNum); + save_efc_force = mjSTACKALLOC(d, nefc, mjtNum); // qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator // should equal result of inverse dynamics diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 59704140..34cc8dbb 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -931,7 +931,7 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, } mj_markStack(d); - int* remaining = mj_stackAllocInt(d, nv); + int* remaining = mjSTACKALLOC(d, nv, int); // compute rownnz mju_zeroInt(rownnz, nv); @@ -1044,7 +1044,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { // allocate and clear incremental row counts mj_markStack(d); - int* cnt = mj_stackAllocInt(d, nbody); + int* cnt = mjSTACKALLOC(d, nbody, int); mju_zeroInt(cnt, nbody); // add subtree dofs to colind @@ -1134,7 +1134,7 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src, mj_markStack(d); // init remaining - int* remaining = mj_stackAllocInt(d, nv); + int* remaining = mjSTACKALLOC(d, nv, int); mju_copyInt(remaining, rownnz, nv); // copy data @@ -1202,7 +1202,7 @@ static void makeDmap(const mjModel* m, mjData* d) { mj_markStack(d); // make mapM2D - int* M = mj_stackAllocInt(d, nM); + int* M = mjSTACKALLOC(d, nM, int); for (int i=0; i < nM; i++) M[i] = i; for (int i=0; i < nD; i++) d->mapM2D[i] = -1; copyM2Sparse(m, d, d->mapM2D, M, /*reduced=*/0); @@ -1215,7 +1215,7 @@ static void makeDmap(const mjModel* m, mjData* d) { } // make mapD2M - int* D = mj_stackAllocInt(d, nD); + int* D = mjSTACKALLOC(d, nD, int); for (int i=0; i < nD; i++) D[i] = i; for (int i=0; i < nM; i++) d->mapD2M[i] = -1; copyD2MSparse(m, d, d->mapD2M, D); @@ -1591,7 +1591,8 @@ void* mj_arenaAllocByte(mjData* d, size_t bytes, size_t alignment) { // internal: allocate size bytes on the provided stack shard // declared inline so that modular arithmetic with specific alignments can be optimized out -static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_t size, size_t alignment) { +static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_t size, + size_t alignment, const char* caller, int line) { // return NULL if empty if (mjUNLIKELY(!size)) { return NULL; @@ -1614,10 +1615,19 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_ size_t stack_available_bytes = stack_info->top - stack_info->limit; size_t stack_required_bytes = stack_info->top - new_top_ptr; if (mjUNLIKELY(stack_required_bytes > stack_available_bytes)) { - mju_error("mj_stackAlloc: insufficient memory: max = %zu, available = %zu, requested = %zu " - "(ne = %d, nf = %d, nefc = %d, ncon = %d)", + char info[1024]; + if (caller) { + snprintf(info, sizeof(info), " at %s, line %d", caller, line); + } else { + info[0] = '\0'; + } + mju_error("mj_stackAlloc: out of memory, stack overflow%s\n" + " max = %zu, available = %zu, requested = %zu\n" + " nefc = %d, ncon = %d", + info, stack_info->bottom - stack_info->limit, stack_available_bytes, stack_required_bytes, - d->ne, d->nf, d->nefc, d->ncon); + d->nefc, d->ncon); + } #ifdef ADDRESS_SANITIZER @@ -1652,20 +1662,20 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_ // internal: allocate size bytes in mjData // declared inline so that modular arithmetic with specific alignments can be optimized out -static inline void* stackalloc(mjData* d, size_t size, size_t alignment) { +static inline void* stackalloc(mjData* d, size_t size, size_t alignment, + const char* caller, int line) { + // single threaded allocation if (!d->threadpool) { mjStackInfo stack_info = get_stack_info_from_data(d); - - void* result = stackallocinternal(d, &stack_info, size, alignment); - + void* result = stackallocinternal(d, &stack_info, size, alignment, caller, line); d->pstack = stack_info.bottom - stack_info.top; - return result; } + // multi threaded allocation size_t thread_id = mju_threadPoolCurrentWorkerId((mjThreadPool*)d->threadpool); mjStackInfo* stack_info = mju_getStackInfoForThread(d, thread_id); - return stackallocinternal(d, stack_info, size, alignment); + return stackallocinternal(d, stack_info, size, alignment, caller, line); } @@ -1677,7 +1687,7 @@ __attribute__((always_inline)) static inline void markstackinternal(mjData* d, mjStackInfo* stack_info) { size_t top_old = stack_info->top; mjStackFrame* s = - (mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame)); + (mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame), NULL, 0); s->pbase = stack_info->stack_base; s->pstack = top_old; #ifdef ADDRESS_SANITIZER @@ -1779,7 +1789,15 @@ size_t mj_stackBytesAvailable(mjData* d) { // allocate bytes on the stack void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment) { - return stackalloc(d, bytes, alignment); + return stackalloc(d, bytes, alignment, NULL, 0); +} + + + +// allocate bytes on the stack, with caller information +void* mj_stackAllocInfo(mjData* d, size_t bytes, size_t alignment, + const char* caller, int line) { + return stackalloc(d, bytes, alignment, caller, line); } @@ -1789,7 +1807,7 @@ mjtNum* mj_stackAllocNum(mjData* d, size_t size) { if (mjUNLIKELY(size >= SIZE_MAX / sizeof(mjtNum))) { mjERROR("requested size is too large (more than 2^64 bytes)."); } - return (mjtNum*) stackalloc(d, size * sizeof(mjtNum), _Alignof(mjtNum)); + return (mjtNum*) stackalloc(d, size * sizeof(mjtNum), _Alignof(mjtNum), NULL, 0); } @@ -1799,7 +1817,7 @@ int* mj_stackAllocInt(mjData* d, size_t size) { if (mjUNLIKELY(size >= SIZE_MAX / sizeof(int))) { mjERROR("requested size is too large (more than 2^64 bytes)."); } - return (int*) stackalloc(d, size * sizeof(int), _Alignof(int)); + return (int*) stackalloc(d, size * sizeof(int), _Alignof(int), NULL, 0); } diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index a6fac94e..298452fa 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -134,9 +134,17 @@ void mj__freeStack(mjData* d) __attribute__((noinline)); // returns the number of bytes available on the stack MJAPI size_t mj_stackBytesAvailable(mjData* d); -// mjData stack allocate +// allocate bytes on the stack MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment); +// allocate bytes on the stack, with added caller information +MJAPI void* mj_stackAllocInfo(mjData* d, size_t bytes, size_t alignment, + const char* caller, int line); + +// macro to allocate a stack array of given type, adds caller information +#define mjSTACKALLOC(d, num, type) \ +(type*) mj_stackAllocInfo(d, (num) * sizeof(type), _Alignof(type), __func__, __LINE__) + // mjData stack allocate for array of mjtNums MJAPI mjtNum* mj_stackAllocNum(mjData* d, size_t size); diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 26d67213..9a8be763 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -426,14 +426,14 @@ void mj_island(const mjModel* m, mjData* d) { // allocate edge array int nedge_max = countMaxEdge(m, d); - int* edge = mj_stackAllocInt(d, 2*nedge_max); + int* edge = mjSTACKALLOC(d, 2*nedge_max, int); // get tree-tree edges and rownnz counts from efc arrays - int* rownnz = mj_stackAllocInt(d, ntree); // number of edges per tree + int* rownnz = mjSTACKALLOC(d, ntree, int); // number of edges per tree int nedge = findEdges(m, d, rownnz, edge, nedge_max); // compute starting address of tree's column indices while resetting rownnz - int* rowadr = mj_stackAllocInt(d, ntree); + int* rowadr = mjSTACKALLOC(d, ntree, int); rowadr[0] = 0; for (int r=1; r < ntree; r++) { rowadr[r] = rowadr[r-1] + rownnz[r-1]; @@ -442,7 +442,7 @@ void mj_island(const mjModel* m, mjData* d) { rownnz[ntree-1] = 0; // copy column indices: list each tree's neighbors - int* colind = mj_stackAllocInt(d, nedge); + int* colind = mjSTACKALLOC(d, nedge, int); for (int e=0; e < nedge; e++) { int row = edge[2*e]; int col = edge[2*e + 1]; @@ -450,8 +450,8 @@ void mj_island(const mjModel* m, mjData* d) { } // discover islands - int* tree_island = mj_stackAllocInt(d, ntree); // id of island assigned to tree - int* stack = mj_stackAllocInt(d, nedge); + int* tree_island = mjSTACKALLOC(d, ntree, int); // id of island assigned to tree + int* stack = mjSTACKALLOC(d, nedge, int); d->nisland = mj_floodFill(tree_island, ntree, rownnz, rowadr, colind, stack); // allocate island arrays on arena diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 8dcf46b8..3365ac73 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -134,7 +134,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mjtNum kD = m->flex_damping[f] / m->opt.timestep; mj_markStack(d); - mjtNum* qfrc = mj_stackAllocNum(d, 3*m->flex_vertnum[f]); + mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum); mju_zero(qfrc, 3*m->flex_vertnum[f]); // compute force element-by-element diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 10ad5668..4de41fca 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -892,7 +892,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // allocate full inertia if it's small if (m->nv <= 200) { - M = mj_stackAllocNum(d, m->nv*m->nv); + M = mjSTACKALLOC(d, m->nv*m->nv, mjtNum); } #ifdef MEMORY_SANITIZER diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index d8824bf4..db58e69c 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -1351,8 +1351,8 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* mj_markStack(d); // allocate source - mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom); - int* geom_eliminate = mj_stackAllocInt(d, m->ngeom); + mjtNum* geom_ba = mjSTACKALLOC(d, 4*m->ngeom, mjtNum); + int* geom_eliminate = mjSTACKALLOC(d, m->ngeom, int); // initialize source mju_multiRayPrepare(m, d, pnt, NULL, geomgroup, flg_static, bodyexclude, diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index a82fb303..56966941 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -996,15 +996,13 @@ void mj_energyPos(const mjModel* m, mjData* d) { // velocity-dependent energy (kinetic) void mj_energyVel(const mjModel* m, mjData* d) { - mjtNum *vec; - // return if disabled (already cleared in potential) if (!mjENABLED(mjENBL_ENERGY)) { return; } mj_markStack(d); - vec = mj_stackAllocNum(d, m->nv); + mjtNum *vec = mjSTACKALLOC(d, m->nv, mjtNum); // kinetic energy: 0.5 * qvel' * M * qvel mj_mulM(m, d, vec, d->qvel); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 04fcec99..046ba9d3 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -65,22 +65,22 @@ static void set0(mjModel* m, mjData* d) { int nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; mj_markStack(d); - mjtNum* jac = mj_stackAllocNum(d, 6*nv); - mjtNum* tmp = mj_stackAllocNum(d, 6*nv); - mjtNum* moment = mj_stackAllocNum(d, nv); + mjtNum* jac = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* tmp = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum); int* cammode = 0; int* lightmode = 0; // save camera and light mode, set to fixed if (m->ncam) { - cammode = mj_stackAllocInt(d, m->ncam); + cammode = mjSTACKALLOC(d, m->ncam, int); for (int i=0; i < m->ncam; i++) { cammode[i] = m->cam_mode[i]; m->cam_mode[i] = mjCAMLIGHT_FIXED; } } if (m->nlight) { - lightmode = mj_stackAllocInt(d, m->nlight); + lightmode = mjSTACKALLOC(d, m->nlight, int); for (int i=0; i < m->nlight; i++) { lightmode[i] = m->light_mode[i]; m->light_mode[i] = mjCAMLIGHT_FIXED; @@ -427,7 +427,7 @@ static void setStat(mjModel* m, mjData* d) { mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10}; mjtNum rbound; mj_markStack(d); - mjtNum* body = mj_stackAllocNum(d, m->nbody); + mjtNum* body = mjSTACKALLOC(d, m->nbody, mjtNum); // compute bounding box of bodies, joint centers, geoms and sites for (int i=1; i < m->nbody; i++) { @@ -595,7 +595,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, // dense actuator_moment row mj_markStack(d); - mjtNum* moment = mj_stackAllocNum(d, nv); + mjtNum* moment = mjSTACKALLOC(d, nv, mjtNum); mju_sparse2dense(moment, d->actuator_moment, 1, nv, d->moment_rownnz + index, d->moment_rowadr + index, d->moment_colind); diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index cb4553ea..ac506fa4 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -330,8 +330,8 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { const mjtNum *floss = d->efc_frictionloss; mjtNum *force = d->efc_force; mj_markStack(d); - mjtNum* ARinv = mj_stackAllocNum(d, nefc); - int* oldstate = mj_stackAllocInt(d, nefc); + mjtNum* ARinv = mjSTACKALLOC(d, nefc, mjtNum); + int* oldstate = mjSTACKALLOC(d, nefc, int); // TODO: b/295296178 - Use island index (currently hardcoded to 0) int island = 0; @@ -555,8 +555,8 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; mjContact* con; mj_markStack(d); - mjtNum* ARinv = mj_stackAllocNum(d, nefc); - int* oldstate = mj_stackAllocInt(d, nefc); + mjtNum* ARinv = mjSTACKALLOC(d, nefc, mjtNum); + int* oldstate = mjSTACKALLOC(d, nefc, int); // TODO: b/295296178 - Use island index (currently hardcoded to 0) int island = 0; @@ -837,28 +837,28 @@ static void CGallocate(const mjModel* m, mjData* d, mjCGContext* ctx, ctx->efcind = island < 0 ? NULL : d->island_efcind + d->island_efcadr[island]; // common arrays - ctx->Jaref = mj_stackAllocNum(d, nefc); - ctx->Jv = mj_stackAllocNum(d, nefc); - ctx->Ma = mj_stackAllocNum(d, nv); - ctx->Mv = mj_stackAllocNum(d, nv); - ctx->grad = mj_stackAllocNum(d, nv); - ctx->Mgrad = mj_stackAllocNum(d, nv); - ctx->search = mj_stackAllocNum(d, nv); - ctx->quad = mj_stackAllocNum(d, nefc*3); + ctx->Jaref = mjSTACKALLOC(d, nefc, mjtNum); + ctx->Jv = mjSTACKALLOC(d, nefc, mjtNum); + ctx->Ma = mjSTACKALLOC(d, nv, mjtNum); + ctx->Mv = mjSTACKALLOC(d, nv, mjtNum); + ctx->grad = mjSTACKALLOC(d, nv, mjtNum); + ctx->Mgrad = mjSTACKALLOC(d, nv, mjtNum); + ctx->search = mjSTACKALLOC(d, nv, mjtNum); + ctx->quad = mjSTACKALLOC(d, nefc*3, mjtNum); // Newton only, known-size arrays ctx->flg_Newton = flg_Newton; if (flg_Newton) { - ctx->D = mj_stackAllocNum(d, nefc); + ctx->D = mjSTACKALLOC(d, nefc, mjtNum); // sparse Newton only if (mj_isSparse(m)) { - ctx->C = mj_stackAllocNum(d, m->nC); - ctx->H_rowadr = mj_stackAllocInt(d, nv); - ctx->H_rownnz = mj_stackAllocInt(d, nv); - ctx->H_lowernnz = mj_stackAllocInt(d, nv); - ctx->L_rownnz = mj_stackAllocInt(d, nv); - ctx->L_rowadr = mj_stackAllocInt(d, nv); + ctx->C = mjSTACKALLOC(d, m->nC, mjtNum); + ctx->H_rowadr = mjSTACKALLOC(d, nv, int); + ctx->H_rownnz = mjSTACKALLOC(d, nv, int); + ctx->H_lowernnz = mjSTACKALLOC(d, nv, int); + ctx->L_rownnz = mjSTACKALLOC(d, nv, int); + ctx->L_rowadr = mjSTACKALLOC(d, nv, int); } } } @@ -1416,8 +1416,8 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { } // allocate H_colind and H - ctx->H_colind = mj_stackAllocInt(d, ctx->nH); - ctx->H = mj_stackAllocNum(d, ctx->nH); + ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int); + ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); // compute H = J'*D*J mju_sqrMatTDSparse(ctx->H, d->efc_J, d->efc_JT, ctx->D, nefc, nv, @@ -1440,10 +1440,10 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { } // allocate L_colind, L, Lcone - ctx->L_colind = mj_stackAllocInt(d, ctx->nL); - ctx->L = mj_stackAllocNum(d, ctx->nL); + ctx->L_colind = mjSTACKALLOC(d, ctx->nL, int); + ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); if (m->opt.cone == mjCONE_ELLIPTIC) { - ctx->Lcone = mj_stackAllocNum(d, ctx->nL); + ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); } // count nonzeros in rows of H lower triangle @@ -1471,9 +1471,9 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { else { // allocate L, Lcone ctx->nL = nv*nv; - ctx->L = mj_stackAllocNum(d, ctx->nL); + ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); if (m->opt.cone == mjCONE_ELLIPTIC) { - ctx->Lcone = mj_stackAllocNum(d, ctx->nL); + ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); } // compute H = M + J'*D*J @@ -1572,9 +1572,9 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { mj_markStack(d); // storage for L'*J - mjtNum* LTJ = mj_stackAllocNum(d, 6*nv); - mjtNum* LTJ_row = mj_stackAllocNum(d, nv); - int* LTJ_ind = mj_stackAllocInt(d, nv); + mjtNum* LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* LTJ_row = mjSTACKALLOC(d, nv, mjtNum); + int* LTJ_ind = mjSTACKALLOC(d, nv, int); // add contributions for (int i=0; i < nefc; i++) { @@ -1646,8 +1646,8 @@ static void HessianIncremental(const mjModel* m, mjData* d, mjCGContext* ctx, co mj_markStack(d); // local space - mjtNum* vec = mj_stackAllocNum(d, nv); - int* vec_ind = mj_stackAllocInt(d, nv); + mjtNum* vec = mjSTACKALLOC(d, nv, mjtNum); + int* vec_ind = mjSTACKALLOC(d, nv, int); // clear update counter ctx->nupdate = 0; @@ -1727,11 +1727,11 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int island, int maxiter, // allocate local storage if (!flg_Newton) { - gradold = mj_stackAllocNum(d, nv); - Mgradold = mj_stackAllocNum(d, nv); - Mgraddif = mj_stackAllocNum(d, nv); + gradold = mjSTACKALLOC(d, nv, mjtNum); + Mgradold = mjSTACKALLOC(d, nv, mjtNum); + Mgraddif = mjSTACKALLOC(d, nv, mjtNum); } - int* oldstate = mj_stackAllocInt(d, nefc); + int* oldstate = mjSTACKALLOC(d, nefc, int); // initialize matrix-vector products int flg_vecunc = 1; // d->qacc is uncompressed diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index ff8c1d4c..e77520e2 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -459,7 +459,7 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { int nv = m->nv; mj_markStack(d); - mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); + mjtNum* jacp_b = mjSTACKALLOC(d, 3*nv, mjtNum); // clear output mju_zero(jacp, 3*nv); @@ -505,8 +505,8 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA // get full Jacobian of point mj_markStack(d); - mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv)); - mjtNum* jacr = mj_stackAllocNum(d, 3*nv); + mjtNum* jacp = (jacPoint ? jacPoint : mjSTACKALLOC(d, 3*nv, mjtNum)); + mjtNum* jacr = mjSTACKALLOC(d, 3*nv, mjtNum); mj_jac(m, d, jacp, jacr, point, body); // jacAxis_col = cross(jacr_col, axis) @@ -741,15 +741,15 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, mjtNum* jacr = flg_rot ? jac + 3*nv : NULL; mj_markStack(d); - mjtNum* jtmp = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv); + mjtNum* jtmp = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum); mjtNum* jp = jtmp; mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL; // sparse if (mj_isSparse(m)) { - mjtNum* buf = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv); - int* buf_ind = mj_stackAllocInt(d, nv); - int* bodychain = mj_stackAllocInt(d, nv); + mjtNum* buf = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum); + int* buf_ind = mjSTACKALLOC(d, nv, int); + int* bodychain = mjSTACKALLOC(d, nv, int); // set first NV = mj_bodyChain(m, body[0], chain); @@ -878,10 +878,10 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { mj_markStack(d); // stack allocations - mjtNum* jacp = mj_stackAllocNum(d, 3*nv); - mjtNum* jacr = mj_stackAllocNum(d, 3*nv); - mjtNum* term1 = mj_stackAllocNum(d, 3*nv); - mjtNum* term2 = mj_stackAllocNum(d, 3*nv); + mjtNum* jacp = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacr = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* term1 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* term2 = mjSTACKALLOC(d, 3*nv, mjtNum); // clear output mju_zero(mat, 3*nv); @@ -1153,7 +1153,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, mj_markStack(d); // create reduced sparse inertia matrix C - mjtNum* C = mj_stackAllocNum(d, nC); + mjtNum* C = mjSTACKALLOC(d, nC, mjtNum); for (int i=0; i < nC; i++) { C[i] = d->qM[d->mapM2C[i]]; } @@ -1178,8 +1178,8 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, int nv = m->nv; mj_markStack(d); - int* buf_ind = mj_stackAllocInt(d, nv); - mjtNum* sparse_buf = mj_stackAllocNum(d, nv); + int* buf_ind = mjSTACKALLOC(d, nv, int); + mjtNum* sparse_buf = mjSTACKALLOC(d, nv, mjtNum); // add to destination for (int i=0; i < nv; i++) { @@ -1230,9 +1230,9 @@ void mj_applyFT(const mjModel* m, mjData* d, // allocate local variables mj_markStack(d); - mjtNum* jacp = force ? mj_stackAllocNum(d, 3*nv) : NULL; - mjtNum* jacr = torque ? mj_stackAllocNum(d, 3*nv) : NULL; - mjtNum* qforce = mj_stackAllocNum(d, nv); + mjtNum* jacp = force ? mjSTACKALLOC(d, 3*nv, mjtNum) : NULL; + mjtNum* jacr = torque ? mjSTACKALLOC(d, 3*nv, mjtNum) : NULL; + mjtNum* qforce = mjSTACKALLOC(d, nv, mjtNum); // make sure body is in range if (body < 0 || body >= m->nbody) { @@ -1242,7 +1242,7 @@ void mj_applyFT(const mjModel* m, mjData* d, // sparse case if (mj_isSparse(m)) { // construct chain and sparse Jacobians - int* chain = mj_stackAllocInt(d, nv); + int* chain = mjSTACKALLOC(d, nv, int); int NV = mj_bodyChain(m, body, chain); mj_jacSparse(m, d, jacp, jacr, point, body, NV, chain); diff --git a/src/engine/engine_util_container.c b/src/engine/engine_util_container.c index 21d190c1..98fa11ca 100644 --- a/src/engine/engine_util_container.c +++ b/src/engine/engine_util_container.c @@ -24,8 +24,7 @@ // stack allocate and initialize new mjArrayList mjArrayList* mju_arrayListCreate(mjData* d, size_t element_size, size_t initial_capacity) { - mjArrayList* array_list = (mjArrayList*) mj_stackAllocByte( - d, sizeof(mjArrayList), _Alignof(mjArrayList)); + mjArrayList* array_list = mjSTACKALLOC(d, 1, mjArrayList); initial_capacity = mjMAX(1, initial_capacity); array_list->d = d; array_list->element_size = element_size; diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 4a455359..30fdc651 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -148,8 +148,8 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, int rank = n; mj_markStack(d); - mjtNum* buf = mj_stackAllocNum(d, n); - int* buf_ind = mj_stackAllocInt(d, n); + mjtNum* buf = mjSTACKALLOC(d, n, mjtNum); + int* buf_ind = mjSTACKALLOC(d, n, int); // backpass over rows for (int r=n-1; r >= 0; r--) { @@ -241,8 +241,8 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz, const int* rowadr, int* colind, int x_nnz, int* x_ind, mjData* d) { mj_markStack(d); - int* buf_ind = mj_stackAllocInt(d, n); - mjtNum* sparse_buf = mj_stackAllocNum(d, n); + int* buf_ind = mjSTACKALLOC(d, n, int); + mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum); // backpass over rows corresponding to non-zero x(r) int rank = n, i = x_nnz - 1; diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 434a0ce4..87580444 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -678,7 +678,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { mj_markStack(d); - int* chain = mj_stackAllocInt(d, 2*nr); + int* chain = mjSTACKALLOC(d, 2*nr, int); int nchain = 0; int* res_colind = NULL; @@ -784,11 +784,11 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, mj_markStack(d); // a dense row buffer that stores the current row in the resulting matrix - mjtNum* buffer = mj_stackAllocNum(d, nc); + mjtNum* buffer = mjSTACKALLOC(d, nc, mjtNum); // these mark the currently set columns in the dense row buffer, // used for when creating the resulting sparse row - int* markers = mj_stackAllocInt(d, nc); + int* markers = mjSTACKALLOC(d, nc, int); for (int i=0; i < nc; i++) { int* cols = res_colind+res_rowadr[i]; @@ -899,8 +899,8 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, int n, mjData* d) { mj_markStack(d); - int* parent = mj_stackAllocInt(d, n); - int* flag = mj_stackAllocInt(d, n); + int* parent = mjSTACKALLOC(d, n, int); + int* flag = mjSTACKALLOC(d, n, int); // loop over rows in reverse order for (int r = n - 1; r >= 0; r--) { diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index cea65167..eb3530b1 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -539,8 +539,8 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur int sel = pert->select; mjtNum headpos[3], forward[3], dif[3]; - mjtNum* jac = mj_stackAllocNum(d, 3*nv); - mjtNum* jacM2 = mj_stackAllocNum(d, 3*nv); + mjtNum* jac = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacM2 = mjSTACKALLOC(d, 3*nv, mjtNum); // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 5f619fdd..8f5be4e0 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1817,7 +1817,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // allocate catenary mj_markStack(d); - mjtNum* catenary = mj_stackAllocNum(d, 3*ncatenary); + mjtNum* catenary = mjSTACKALLOC(d, 3*ncatenary, mjtNum); // points along catenary path int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary, ncatenary); @@ -2520,7 +2520,7 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO else { // allocate and clear vertex normals for smoothing mj_markStack(d); - mjtNum* vertnorm = mj_stackAllocNum(d, 3*m->flex_vertnum[f]); + mjtNum* vertnorm = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum); mju_zero(vertnorm, 3*m->flex_vertnum[f]); // add vertex normals: top element sides in 2D, shell fragments in 3D From ec230fa4b622a387cd7e0d2bee491c974b2a3674 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 11 Dec 2024 09:43:27 -0800 Subject: [PATCH 151/426] Prune face linear map in EPA. PiperOrigin-RevId: 705138358 Change-Id: I835ce7094d066b19ee39af48e0b83b51f7a0954e --- src/engine/engine_collision_gjk.c | 196 ++++++++++++++--------- test/engine/engine_collision_gjk_test.cc | 49 ++++++ 2 files changed, 168 insertions(+), 77 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index e8c4653f..b97d67a4 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -14,6 +14,7 @@ #include "engine/engine_collision_gjk.h" +#include #include #include #include @@ -51,10 +52,10 @@ static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); // one face in a polytope typedef struct { int verts[3]; // indices of the three vertices of the face in the polytope - int adj[3]; // adjacent faces (one for each edge: [v1,v2], [v2,v3], [v3,v1]) - mjtNum v[3]; // the projection of the origin on the face (can be used as face normal) + int adj[3]; // adjacent faces, one for each edge: [v1,v2], [v2,v3], [v3,v1] + mjtNum v[3]; // projection of the origin on face, can be used as face normal mjtNum dist; // norm of v; negative if deleted - int index; // index in map + int index; // index in map; -1: not in map, -2: deleted from polytope } Face; // polytope used in the Expanding Polytope Algorithm (EPA) @@ -290,10 +291,15 @@ static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* // compute the support points in obj1 and obj2 for the kth approximation point static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3]) { - mjtNum dir[3], dir_neg[3]; - copy3(dir_neg, x_k); - mju_normalize3(dir_neg); // mjc_support assumes a normalized direction - scl3(dir, dir_neg, -1); + mjtNum dir[3] = {-1, 0, 0}, dir_neg[3] = {1, 0, 0}; + + // mjc_support requires a normalized direction + mjtNum norm = dot3(x_k, x_k); + if (norm > mjMINVAL*mjMINVAL) { + norm = 1/mju_sqrt(norm); + scl3(dir_neg, x_k, norm); + scl3(dir, dir_neg, -1); + } // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) support(s1, s2, obj1, obj2, dir, dir_neg); @@ -304,20 +310,14 @@ static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj // compute the support point in the Minkowski difference for EPA static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3], mjtNum dnorm) { - mjtNum dir[3], dir_neg[3]; + mjtNum dir[3] = {1, 0, 0}, dir_neg[3] = {-1, 0, 0}; // mjc_support assumes a normalized direction - if (dnorm < mjMINVAL) { - dir[0] = 1, dir_neg[0] = -1; - dir[1] = 0, dir_neg[1] = 0; - dir[2] = 0, dir_neg[2] = 0; - } else { + if (dnorm > mjMINVAL) { dir[0] = d[0] / dnorm; dir[1] = d[1] / dnorm; dir[2] = d[2] / dnorm; - dir_neg[0] = -dir[0]; - dir_neg[1] = -dir[1]; - dir_neg[2] = -dir[2]; + scl3(dir_neg, dir, -1); } // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) @@ -343,12 +343,10 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const sub3(diff1, v3, v1); sub3(diff2, v2, v1); cross3(normal, diff1, diff2); - mjtNum norm = mju_norm3(normal); - if (norm > mjMINVAL && norm < mjMAXVAL) { - mjtNum invnorm = 1/norm; - normal[0] *= invnorm; - normal[1] *= invnorm; - normal[2] *= invnorm; + mjtNum norm = dot3(normal, normal); + if (norm > mjMINVAL*mjMINVAL && norm < mjMAXVAL*mjMAXVAL) { + norm = 1/mju_sqrt(norm); + scl3(normal, normal, norm); return dot3(normal, v1); } return mjMAXVAL; // cannot recover normal (ignore face) @@ -453,8 +451,8 @@ static inline void lincomb3(mjtNum res[3], const mjtNum coef[3], const mjtNum v1 // res = origin projected onto plane defined by v1, v2, v3 -static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], - const mjtNum v3[3]) { +static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], + const mjtNum v3[3]) { mjtNum diff21[3], diff31[3], diff32[3], n[3], nv, nn; sub3(diff21, v2, v1); sub3(diff31, v3, v1); @@ -464,18 +462,20 @@ static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const m cross3(n, diff32, diff21); nv = dot3(n, v2); nn = dot3(n, n); + if (nn == 0) return 1; if (nv != 0 && nn > mjMINVAL) { scl3(res, n, nv / nn); - return; + return 0; } // n = (v2 - v1) x (v3 - v1) cross3(n, diff21, diff31); nv = dot3(n, v1); nn = dot3(n, n); + if (nn == 0) return 1; if (nv != 0 && nn > mjMINVAL) { scl3(res, n, nv / nn); - return; + return 0; } // n = (v1 - v3) x (v2 - v3) @@ -483,6 +483,7 @@ static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const m nv = dot3(n, v3); nn = dot3(n, n); scl3(res, n, nv / nn); + return 0; } @@ -619,7 +620,6 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const lambda[1] = lambda_2d[1]; lambda[2] = lambda_2d[2]; lambda[3] = 0; - dmin = d; } } } @@ -629,7 +629,11 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]) { // project origin onto affine hull of the simplex mjtNum p_o[3]; - projectOriginPlane(p_o, s1, s2, s3); + if (projectOriginPlane(p_o, s1, s2, s3)) { + S1D(lambda, s1, s2); + lambda[2] = 0; + return; + } // Below are the minors M_i4 of the matrix M given by // [[ s1_x, s2_x, s3_x, s4_x ], @@ -750,7 +754,6 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const lambda[0] = lambda_1d[0]; lambda[1] = lambda_1d[1]; lambda[2] = 0; - dmin = d; } } } @@ -897,7 +900,6 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj int v4i = newVertex(pt, v4a, v4b); int v5i = newVertex(pt, v5a, v5b); - // build hexahedron attachFace(pt, v1i, v3i, v4i, 1, 3, 2); attachFace(pt, v1i, v5i, v3i, 2, 4, 0); @@ -909,10 +911,13 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // if the origin is on the affine hull of any of the faces then the origin is not in the // hexahedron or the hexahedron is degenerate for (int i = 0; i < 6; i++) { + pt->map[i] = pt->faces + i; + pt->faces[i].index = i; if (pt->faces[i].dist < mjMINVAL) { return 3; } } + pt->nmap = 6; // valid hexahedron for EPA return 0; @@ -1053,10 +1058,13 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // if the origin is on the affine hull of any of the faces then the origin is not in the // hexahedron or the hexahedron is degenerate for (int i = 0; i < 6; i++) { + pt->map[i] = pt->faces + i; + pt->faces[i].index = i; if (pt->faces[i].dist < mjMINVAL) { return 8; } } + pt->nmap = 6; return 0; } @@ -1078,7 +1086,6 @@ static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, in copy3(status->simplex + 6, pt->verts + v3); pt->nfaces = 0; - pt->nmap = 0; pt->nverts = 0; } @@ -1108,6 +1115,12 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj replaceSimplex3(pt, status, v4, v3, v2); return polytope3(pt, status, obj1, obj2); } + + for (int i = 0; i < 4; i++) { + pt->map[i] = pt->faces + i; + pt->faces[i].index = i; + } + pt->nmap = 4; return 0; } @@ -1125,16 +1138,12 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) { // delete face from map (return non-zero on error) -static int deleteFace(Polytope* pt, Face* face) { - // SHOULD NOT OCCUR - if (pt->nmap < 2) { - pt->nmap = 0; - return 1; +static void deleteFace(Polytope* pt, Face* face) { + if (face->index >= 0) { + pt->map[face->index] = pt->map[--pt->nmap]; + pt->map[face->index]->index = face->index; } - face->dist = -1; - pt->map[face->index] = pt->map[--pt->nmap]; - pt->map[face->index]->index = face->index; - return 0; + face->index = -2; // mark face as deleted from map and polytope } @@ -1147,7 +1156,8 @@ static inline int maxFaces(Polytope* pt) { // attach a face to the polytope with the given vertex indices; return distance to origin -static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) { +static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, + int adj1, int adj2, int adj3) { Face* face = &pt->faces[pt->nfaces++]; face->verts[0] = v1; face->verts[1] = v2; @@ -1159,13 +1169,11 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, face->adj[2] = adj3; // compute witness point v - projectOriginPlane(face->v, pt->verts + v1, pt->verts + v2, pt->verts + v3); - face->dist = mju_norm3(face->v); + int ret = projectOriginPlane(face->v, pt->verts + v3, pt->verts + v2, pt->verts + v1); + if (ret) return 0; + face->dist = mju_sqrt(dot3(face->v, face->v)); + face->index = -1; - // store face in map - int i = pt->nmap++; - face->index = i; - pt->map[i] = face; return face->dist; } @@ -1205,13 +1213,13 @@ static int horizonRec(Horizon* h, Face* face, int e) { // v is visible from w so it is deleted and adjacent faces are checked if (dot3(face->v, h->w) >= dist2) { - if (deleteFace(h->pt, face)) return 1; // escape recursion on error + deleteFace(h->pt, face); // recursively search the adjacent faces on the next two edges for (int k = 1; k < 3; k++) { int i = (e + k) % 3; Face* adjFace = &h->pt->faces[face->adj[i]]; - if (adjFace->dist > 0) { + if (adjFace->index > -2) { int adjEdge = getEdge(adjFace, face->verts[(i + 1) % 3]); if (!horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[i], adjEdge); @@ -1227,7 +1235,7 @@ static int horizonRec(Horizon* h, Face* face, int e) { // create horizon given the face as starting point static void horizon(Horizon* h, Face* face) { - if (deleteFace(h->pt, face)) return; + deleteFace(h->pt, face); // first edge Face* adjFace = &h->pt->faces[face->adj[0]]; @@ -1239,14 +1247,14 @@ static void horizon(Horizon* h, Face* face) { // second edge adjFace = &h->pt->faces[face->adj[1]]; adjEdge = getEdge(adjFace, face->verts[2]); - if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) { + if (adjFace->index > -2 && !horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[1], adjEdge); } // third edge adjFace = &h->pt->faces[face->adj[2]]; adjEdge = getEdge(adjFace, face->verts[0]); - if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) { + if (adjFace->index > -2 && !horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[2], adjEdge); } } @@ -1283,10 +1291,10 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu // return the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { - mjtNum dist, tolerance = status->tolerance; + mjtNum tolerance = status->tolerance, lower, upper = FLT_MAX; int k, kmax = status->max_iterations; mjData* d = (mjData*) obj1->data; - Face* face; // face closest to origin + Face* face, *pface; // face closest to origin // initialize horizon Horizon h; @@ -1297,42 +1305,41 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o h.pt = pt; for (k = 0; k < kmax; k++) { - // find the face closest to the origin - if (!pt->nmap) { - mju_warning("EPA: empty polytope"); - mj_freeStack(d); - return 0; // assume 0 depth - } + pface = face; - dist = mjMAXVAL; + // find the face closest to the origin (lower bound for penetration depth) + lower = FLT_MAX; for (int i = 0; i < pt->nmap; i++) { - if (pt->map[i]->dist < dist) { + if (pt->map[i]->dist < lower) { face = pt->map[i]; - dist = face->dist; + lower = face->dist; } } - // check if dist is 0 - if (dist <= 0) { + // face not valid, return previous face + if (lower > upper) { + face = pface; + break; + } + + // check if lower bound is 0 + if (lower <= 0) { mju_warning("EPA: origin lies on affine hull of face"); + break; } // compute support point w from the closest face's normal mjtNum w1[3], w2[3], w[3]; - epaSupport(w1, w2, obj1, obj2, face->v, dist); + epaSupport(w1, w2, obj1, obj2, face->v, lower); sub3(w, w1, w2); - mjtNum next_dist = dot3(face->v, w) / dist; - if (next_dist - dist < tolerance) { + mjtNum upper_k = dot3(face->v, w) / lower; // upper bound for kth iteration + if (upper_k < upper) upper = upper_k; + if (upper - lower < tolerance) { break; } h.w = w; horizon(&h, face); - if (!pt->nmap) { - h.nedges = 0; - // next iteration will clean up and error out - continue; - } // insert w as new vertex and attach faces along the horizon int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges; @@ -1349,11 +1356,26 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o int v1 = horFace->verts[horEdge], v2 = horFace->verts[(horEdge + 1) % 3]; horFace->adj[horEdge] = nfaces; - attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1); + mjtNum dist = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1); + + // unrecoverable numerical issue + if (dist == 0) { + mj_freeStack(d); + status->epa_iterations = k; + status->nx = 0; + return 0; + } + + // store face in map + if (dist >= lower && dist <= upper) { + int i = pt->nmap++; + pt->map[i] = &pt->faces[pt->nfaces - 1]; + pt->map[i]->index = i; + } // attach remaining faces for (int i = 1; i < nedges; i++) { - int cur = nfaces + i; // index of attached face + int cur = nfaces + i; // index of attached face int next = nfaces + (i + 1) % nedges; // index of next face horIndex = h.indices[i], horEdge = h.edges[i]; @@ -1361,16 +1383,36 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o v1 = horFace->verts[horEdge]; v2 = horFace->verts[(horEdge + 1) % 3]; horFace->adj[horEdge] = cur; - attachFace(pt, wi, v2, v1, cur - 1, horIndex, next); + dist = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next); + + // unrecoverable numerical issue + if (dist == 0) { + mj_freeStack(d); + status->epa_iterations = k; + status->nx = 0; + return 0; + } + + // store face in map + if (dist >= lower && dist <= upper) { + int idx = pt->nmap++; + pt->map[idx] = &pt->faces[pt->nfaces - 1]; + pt->map[idx]->index = idx; + } } h.nedges = 0; // clear horizon + + // no face candidates left + if (!pt->nmap) { + break; + } } mj_freeStack(d); epaWitness(pt, face, status->x1, status->x2); status->epa_iterations = k; status->nx = 1; - return dist; + return face->dist; } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 3093d82a..241575c6 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -286,6 +286,55 @@ TEST_F(MjGjkTest, BoxBoxDepth) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxDepth2) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xpos[0] = -0.000171208577507291721461757383; + xpos[1] = -0.000171208577507290908310128019; + xpos[2] = 1.067119586248553853025100579544; + + xmat[0] = 0.999999966039443077825410455262; + xmat[1] = -0.000000033960556969622165789148; + xmat[2] = -0.000260616790777324182967061850; + xmat[3] = -0.000000033960556972087627235699; + xmat[4] = 0.999999966039443077825410455262; + xmat[5] = -0.000260616790777321797722282382; + xmat[6] = 0.000260616790777324182967061850; + xmat[7] = 0.000260616790777321797722282382; + xmat[8] = 0.999999932078886044628518448008; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dir[3], pos[3]; + mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + + if (dist < 0) { + EXPECT_NEAR(dist, -0.033401579411886845, kTolerance); + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], 1, kTolerance); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"( From b26d6f0466f003849dfe0981874b4f743aeaf0e8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 14 Dec 2024 01:53:13 -0800 Subject: [PATCH 152/426] Disable tendon catenary visualization when tendon is actuated. Add note about catenary visualization. PiperOrigin-RevId: 706161179 Change-Id: Ifaf571e039306309869078d1a0bcf9a69f1fe9de --- doc/XMLreference.rst | 12 ++++++++++++ src/engine/engine_vis_visualize.c | 15 +++++++++++++-- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index fb8b2a8f..49818cfa 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4661,6 +4661,18 @@ A second form of wrapping is where the tendon is constrained to pass *through* a wrap around it. This is enabled automatically when a sidesite is specified and its position is inside the volume of the obstacle geom. +.. youtube:: I2q7D0Vda-A + :width: 300px + :align: right + +**Visualization:** Tendon paths are visualized as in the image above, respecting the :ref:`width`, +:ref:`material` and :ref:`rgba` attributes below. A special kind of +visualization is used for unactuated 2-point tendons with :ref:`range` or +:ref:`springlength` of the form :at-val:`[0 X]`, with positive X. Such tendons act like a +cable, applying force only when stretched. Therefore when not stretched, they are drawn as a catenary of +length X, as in the clip on the right of `this example model +`__. + .. _tendon-spatial-name: :at:`name`: :at-val:`string, optional` diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 8f5be4e0..e48f1834 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1729,6 +1729,16 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_TENDON; category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { + // mark actuated tendons + int* tendon_actuated = mjSTACKALLOC(d, m->ntendon, int); + mju_zeroInt(tendon_actuated, m->ntendon); + for (int i=0; i < m->nu; i++) { + if (m->actuator_trntype[i] == mjTRN_TENDON) { + tendon_actuated[m->actuator_trnid[2*i]] = 1; + } + } + + // draw tendons for (int i=0; i < m->ntendon; i++) { if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) { // tendon has a deadband spring @@ -1752,9 +1762,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero m->tendon_num[i] == 2 && // only two sites on the tendon - (limitedspring || limitedconstraint) && // either spring or constraint length limits + (limitedspring != limitedconstraint) && // either spring or constraint length limits m->tendon_damping[i] == 0 && // no damping - m->tendon_frictionloss[i] == 0; // no frictionloss + m->tendon_frictionloss[i] == 0 && // no frictionloss + tendon_actuated[i] == 0; // no actuator // conditions not met: draw straight lines if (!draw_catenary) { From a7eb6efd4e3181d1f15428cdcb3f42458613e115 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Mon, 16 Dec 2024 09:56:12 -0800 Subject: [PATCH 153/426] Copybara import of the project: -- 3a95b62f59e81bfef0f076afb173ecc14b27943d by Levi Burner : rollout prototype native threadpool for comparing to python threads -- efd8be1124ac839b902de45973a3ca8b9f2215e6 by Levi Burner : copy mjpcs threadpool into python bindings -- 75603eea3e8362e354a9675e8a6cd14e56ec3d28 by Levi Burner : rollout use threadpool as translation unit -- 06b90febd021663f6cc81fd7895e4d6e2008ed97 by Levi Burner : rollout add chunk_divisor parameter -- 298ab2f3c0d6e12530832c3cdbf784dd92d54806 by Levi Burner : rollout add native threading test -- 169cf9978e7abad6edd1392b8e6aab995e4f8f10 by Levi Burner : rollout exchange chunk_divisor arg for chunk_size -- 265af851d74432d261277d3dbda11cdef1841bc8 by Levi Burner : rollout fix cosmetics -- 1e8bffa88bf36190501b334bef31147e23db39f7 by Levi Burner : make native rollout a class instead of a function -- ba788214b047577f58c41ce0ab6c62c277cd8b0d by Levi Burner : rollout update docs and changelog -- e4cb7732319e04cba2ab2c2ad848c659f6309808 by Levi Burner : rollout don't register atexit handler for Rollout objects -- 5a08d2efdbbbb01d4b1231ff9a36a1dc44f4d9ee by Levi Burner : rollout nthread kwarg, rename shutdown_pool to close, fixups -- f622378543596a208339af0208fa3a70bf2a8007 by Levi Burner : rollout add missing .close() calls -- 50f3ebca43c53eac03f03943c34bb1e46967bd4f by Levi Burner : rollout return immediately COPYBARA_INTEGRATE_REVIEW=https://github.com/google-deepmind/mujoco/pull/2282 from aftersomemath:rollout-threaded 50f3ebca43c53eac03f03943c34bb1e46967bd4f PiperOrigin-RevId: 706744277 Change-Id: I1ab2263b7d6ce30cf1908aec8fd5f2eb976a19e6 --- doc/changelog.rst | 8 + doc/python.rst | 44 +++- python/mujoco/CMakeLists.txt | 2 +- python/mujoco/rollout.cc | 226 +++++++++++++------ python/mujoco/rollout.py | 401 +++++++++++++++++++++++----------- python/mujoco/rollout_test.py | 116 +++++++++- python/mujoco/threadpool.cc | 87 ++++++++ python/mujoco/threadpool.h | 80 +++++++ 8 files changed, 761 insertions(+), 203 deletions(-) create mode 100644 python/mujoco/threadpool.cc create mode 100644 python/mujoco/threadpool.h diff --git a/doc/changelog.rst b/doc/changelog.rst index 5afaf2f0..33456f82 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,6 +5,14 @@ Changelog Upcoming version (not yet released) ----------------------------------- +Python bindings +^^^^^^^^^^^^^^^ +- :ref:`rollout` now features native multi-threading. If a sequence of ``MjData`` instances + of length ``nthread`` is passed in, ``rollout`` will automatically create a thread pool and parallelize + the computation. The thread pool can be resused across calls, but then the function cannot be called simultaneously + from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which + encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. + Bug fixes ^^^^^^^^^ - Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). diff --git a/doc/python.rst b/doc/python.rst index ddaeb0a1..35c27d73 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -711,18 +711,20 @@ The ``mujoco`` package contains two sub-modules: ``mujoco.rollout`` and ``mujoco rollout ------- -``mujoco.rollout`` shows how to add additional C/C++ functionality, exposed as a Python module via pybind11. It is -implemented in `rollout.cc `__ +``mujoco.rollout`` and ``mujoco.rollout.Rollout`` shows how to add additional C/C++ functionality, exposed as a Python module +via pybind11. It is implemented in `rollout.cc `__ and wrapped in `rollout.py `__. The module performs a common functionality where tight loops implemented outside of Python are beneficial: rolling out a trajectory (i.e., calling :ref:`mj_step` in a loop), given an intial state and sequence of controls, and returning subsequent -states and sensor values. The basic usage form is +states and sensor values. The rollouts are run in parallel with an internally managed thread pool if multiple MjData instances +(one per thread) are passed as an argument. The basic usage form is .. code-block:: python state, sensordata = rollout.rollout(model, data, initial_state, control) ``model`` is either a single instance of MjModel or a sequence of compatible MjModel of length ``nroll``. +``data`` is either a single instance of MjData or a sequence of compatible MjData of length ``nthread``. ``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where ``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the :ref:`full physics state`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are @@ -732,13 +734,41 @@ specified by passing an optional ``control_spec`` bitflag. If a rollout diverges, the current state and sensor values are used to fill the remainder of the trajectory. Therefore, non-increasing time values can be used to detect diverged rollouts. -The ``rollout`` function is designed to be completely stateless, so all inputs of the stepping pipeline are set and any +The ``rollout`` function is designed to be computationally stateless, so all inputs of the stepping pipeline are set and any values already present in the given ``MjData`` instance will have no effect on the output. -Since the Global Interpreter Lock can be released, this function can be efficiently threaded using Python threads. See -the ``test_threading`` function in +By default ``rollout.rollout`` creates a new thread pool every call if ``len(data) > 1``. To reuse the thread pool +over multiple calls use the ``persistent_pool`` argument. ``rollout.rollout`` is not thread safe when using +a persistent pool. The basic usage form is + +.. code-block:: python + + state, sensordata = rollout.rollout(model, data, initial_state, persistent_pool=True) + +The pool is shutdown on interpreter shutdown or by a call to ``rollout.shutdown_persistent_pool``. + +To use multiple thread pools from multiple threads, use ``Rollout`` objects. The basic usage form is + +.. code-block:: python + + # Pool shutdown upon exiting block. + with rollout.Rollout(nthread=nthread) as rollout_: + rollout_.rollout(model, data, initial_state) + +or + +.. code-block:: python + + # Pool shutdown on object deletion or call to rollout_.close(). + # To ensure clean shutdown of threads, call close() before interpreter exit. + rollout_ = rollout.Rollout(nthread=nthread) + rollout_.rollout(model, data, initial_state) + rollout_.close() + +Since the Global Interpreter Lock is released, this function can also be threaded using Python threads. However, this +is less efficient than using native threads. See the ``test_threading`` function in `rollout_test.py `__ for an example -of threaded operation (and more generally for usage examples). +of threaded operation (and for more general usage examples). .. _PyMinimize: diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index aa97b9e6..b6d6c078 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -383,7 +383,7 @@ target_link_libraries( structs_header ) -mujoco_pybind11_module(_rollout rollout.cc) +mujoco_pybind11_module(_rollout rollout.cc threadpool.cc) target_link_libraries(_rollout PRIVATE functions_header mujoco raw) mujoco_pybind11_module( diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index ffedda3f..3f27f1d6 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -20,6 +21,7 @@ #include "errors.h" #include "raw.h" #include "structs.h" +#include "threadpool.h" #include #include #include @@ -31,14 +33,24 @@ namespace { namespace py = ::pybind11; +using PyCArray = py::array_t; + // NOLINTBEGIN(whitespace/line_length) +const auto rollout_init_doc = R"( +Construct a rollout object containing a thread pool for parallel rollouts. + + input arguments (optional): + nthread integer, number of threads in pool + if zero, this pool is not started and rollouts run on the calling thread +)"; + const auto rollout_doc = R"( Roll out open-loop trajectories from initial states, get resulting states and sensor values. input arguments (required): model list of MjModel instances of length nroll - data associated instance of MjData + data list of associated MjData instances of length nthread nstep integer, number of steps to be taken for each trajectory control_spec specification of controls, ncontrol = mj_stateSize(m, control_spec) state0 (nroll x nstate) nroll initial state vectors, @@ -49,12 +61,14 @@ Roll out open-loop trajectories from initial states, get resulting states and se output arguments (optional): state (nroll x nstep x nstate) nroll nstep states sensordata (nroll x nstep x nsendordata) nroll trajectories of nstep sensordata vectors + chunk_size integer, determines threadpool chunk size. If unspecified + chunk_size = max(1, nroll / (nthread * 10)) )"; // C-style rollout function, assumes all arguments are valid // all input fields of d are initialised, contents at call time do not matter // after returning, d will contain the last step of the last rollout -void _unsafe_rollout(std::vector& m, mjData* d, int nroll, int nstep, unsigned int control_spec, +void _unsafe_rollout(std::vector& m, mjData* d, int start_roll, int end_roll, int nstep, unsigned int control_spec, const mjtNum* state0, const mjtNum* warmstart0, const mjtNum* control, mjtNum* state, mjtNum* sensordata) { // sizes @@ -75,7 +89,7 @@ void _unsafe_rollout(std::vector& m, mjData* d, int nroll, int n } // loop over rollouts - for (int r = 0; r < nroll; r++) { + for (int r = start_roll; r < end_roll; r++) { // clear user inputs if unspecified if (!(control_spec & mjSTATE_MOCAP_POS)) { for (int i = 0; i < nbody; i++) { @@ -158,6 +172,43 @@ void _unsafe_rollout(std::vector& m, mjData* d, int nroll, int n } } +// C-style threaded version of _unsafe_rollout +void _unsafe_rollout_threaded(std::vector& m, std::vector& d, + int nroll, int nstep, unsigned int control_spec, + const mjtNum* state0, const mjtNum* warmstart0, + const mjtNum* control, mjtNum* state, mjtNum* sensordata, + ThreadPool* pool, int chunk_size) { + int nfulljobs = nroll / chunk_size; + int chunk_remainder = nroll % chunk_size; + int njobs = (chunk_remainder > 0) ? nfulljobs + 1 : nfulljobs; + + // Reset the pool counter + pool->ResetCount(); + + // schedule all jobs of full (chunk) size + for (int j = 0; j < nfulljobs; j++) { + auto task = [=, &m, &d](void) { + int id = pool->WorkerId(); + _unsafe_rollout(m, d[id], j*chunk_size, (j+1)*chunk_size, + nstep, control_spec, state0, warmstart0, control, state, sensordata); + }; + pool->Schedule(task); + } + + // schedule any remaining jobs of size < chunk_size + if (chunk_remainder > 0) { + auto task = [=, &m, &d](void) { + _unsafe_rollout(m, d[pool->WorkerId()], nfulljobs*chunk_size, + nfulljobs*chunk_size+chunk_remainder, + nstep, control_spec, state0, warmstart0, control, state, sensordata); + }; + pool->Schedule(task); + } + + // wait for job counter to incremented up to the number of jobs submitted by this thread + pool->WaitCount(njobs); +} + // NOLINTEND(whitespace/line_length) // check size of optional argument to rollout(), return raw pointer @@ -181,71 +232,118 @@ mjtNum* get_array_ptr(std::optional> arg, return static_cast(info.ptr); } +class Rollout { + public: + Rollout(int nthread) : nthread_(nthread) { + if (this->nthread_ > 0) { + this->pool_ = std::make_shared(this->nthread_); + } + } + + void rollout(py::list m, py::list d, int nstep, unsigned int control_spec, + const PyCArray state0, std::optional warmstart0, + std::optional control, + std::optional state, + std::optional sensordata, + std::optional chunk_size) { + // get raw pointers + int nroll = state0.shape(0); + std::vector model_ptrs(nroll); + for (int r = 0; r < nroll; r++) { + model_ptrs[r] = m[r].cast()->get(); + } + + // check length d and nthread are consistent + if (this->nthread_ == 0 && py::len(d) > 1) { + std::ostringstream msg; + msg << "More than one data instance passed but " + << "rollout is configured to run on main thread"; + py::value_error(msg.str()); + } else if (this->nthread_ != py::len(d)) { + std::ostringstream msg; + msg << "Length of data: " << py::len(d) + << " not equal to nthread: " << this->nthread_; + py::value_error(msg.str()); + } + + std::vector data_ptrs(py::len(d)); + for (int t = 0; t < py::len(d); t++) { + data_ptrs[t] = d[t].cast()->get(); + } + + // check that some steps need to be taken, return if not + if (nstep < 1) { + return; + } + + // get sizes + int nstate = mj_stateSize(model_ptrs[0], mjSTATE_FULLPHYSICS); + int ncontrol = mj_stateSize(model_ptrs[0], control_spec); + + mjtNum* state0_ptr = get_array_ptr(state0, "state0", nroll, 1, nstate); + mjtNum* warmstart0_ptr = + get_array_ptr(warmstart0, "warmstart0", nroll, 1, model_ptrs[0]->nv); + mjtNum* control_ptr = + get_array_ptr(control, "control", nroll, nstep, ncontrol); + mjtNum* state_ptr = get_array_ptr(state, "state", nroll, nstep, nstate); + mjtNum* sensordata_ptr = get_array_ptr(sensordata, "sensordata", nroll, + nstep, model_ptrs[0]->nsensordata); + + // perform rollouts + { + // release the GIL + py::gil_scoped_release no_gil; + + // call unsafe rollout function, multi or single threaded + if (this->nthread_ > 0 && nroll > 1) { + int chunk_size_final = 1; + if (!chunk_size.has_value()) { + chunk_size_final = std::max(1, nroll / (10 * this->nthread_)); + } else { + chunk_size_final = *chunk_size; + } + InterceptMjErrors(_unsafe_rollout_threaded)( + model_ptrs, data_ptrs, nroll, nstep, control_spec, state0_ptr, + warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr, + this->pool_.get(), chunk_size_final); + } else { + InterceptMjErrors(_unsafe_rollout)( + model_ptrs, data_ptrs[0], 0, nroll, nstep, control_spec, state0_ptr, + warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr); + } + } + } + + private: + int nthread_; + std::shared_ptr pool_; +}; PYBIND11_MODULE(_rollout, pymodule) { namespace py = ::pybind11; - using PyCArray = py::array_t; - // roll out open loop trajectories from multiple initial states - // get subsequent states and corresponding sensor values - pymodule.def( - "rollout", - [](py::list m, MjDataWrapper& d, - int nstep, unsigned int control_spec, - const PyCArray state0, - std::optional warmstart0, - std::optional control, - std::optional state, - std::optional sensordata - ) { - // get raw pointers - int nroll = state0.shape(0); - std::vector model_ptrs(nroll); - for (int r = 0; r < nroll; r++) { - model_ptrs[r] = m[r].cast()->get(); - } - raw::MjData* data = d.get(); - - // check that some steps need to be taken, return if not - if (nstep < 1) { - return; - } - - // get sizes - int nstate = mj_stateSize(model_ptrs[0], mjSTATE_FULLPHYSICS); - int ncontrol = mj_stateSize(model_ptrs[0], control_spec); - - mjtNum* state0_ptr = get_array_ptr(state0, "state0", nroll, 1, nstate); - mjtNum* warmstart0_ptr = get_array_ptr(warmstart0, "warmstart0", nroll, - 1, model_ptrs[0]->nv); - mjtNum* control_ptr = get_array_ptr(control, "control", nroll, - nstep, ncontrol); - mjtNum* state_ptr = get_array_ptr(state, "state", nroll, nstep, nstate); - mjtNum* sensordata_ptr = get_array_ptr(sensordata, "sensordata", nroll, - nstep, model_ptrs[0]->nsensordata); - - // perform rollouts - { - // release the GIL - py::gil_scoped_release no_gil; - - // call unsafe rollout function - InterceptMjErrors(_unsafe_rollout)( - model_ptrs, data, nroll, nstep, control_spec, state0_ptr, - warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr); - } - }, - py::arg("model"), - py::arg("data"), - py::arg("nstep"), - py::arg("control_spec"), - py::arg("state0"), - py::arg("warmstart0") = py::none(), - py::arg("control") = py::none(), - py::arg("state") = py::none(), - py::arg("sensordata") = py::none(), - py::doc(rollout_doc) - ); + py::class_(pymodule, "Rollout") + .def( + py::init([](int nthread) { + return std::make_unique(nthread); + }), + py::kw_only(), + py::arg("nthread"), + py::doc(rollout_init_doc)) + .def( + "rollout", + &Rollout::rollout, + py::arg("model"), + py::arg("data"), + py::arg("nstep"), + py::arg("control_spec"), + py::arg("state0"), + py::arg("warmstart0") = py::none(), + py::arg("control") = py::none(), + py::arg("state") = py::none(), + py::arg("sensordata") = py::none(), + py::arg("chunk_size") = py::none(), + py::doc(rollout_doc)); } } // namespace diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 98eaa3f2..95b6ad3f 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -14,6 +14,7 @@ # ============================================================================== """Roll out open-loop trajectories from initial states, get subsequent states and sensor values.""" +import atexit from collections.abc import Sequence from typing import Optional, Union @@ -23,9 +24,243 @@ import numpy as np from numpy import typing as npt +class Rollout: + """Rollout object containing a thread pool for parallel rollouts.""" + + def __init__(self, *, nthread: Optional[int] = None): + """Construct a rollout object containing a thread pool for parallel rollouts. + + Args: + nthread: Number of threads in pool. + If zero, this pool is not started and rollouts run on the calling thread. + """ # fmt: skip + self.nthread = 0 if nthread is None else nthread + self.rollout_ = _rollout.Rollout(nthread=self.nthread) + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.close() + + def close(self): + del self.rollout_ + self.rollout_ = None + + def rollout( + self, + model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], + data: Union[mujoco.MjData, Sequence[mujoco.MjData]], + initial_state: npt.ArrayLike, + control: Optional[npt.ArrayLike] = None, + *, # require subsequent arguments to be named + control_spec: int = mujoco.mjtState.mjSTATE_CTRL.value, + skip_checks: bool = False, + nstep: Optional[int] = None, + initial_warmstart: Optional[npt.ArrayLike] = None, + state: Optional[npt.ArrayLike] = None, + sensordata: Optional[npt.ArrayLike] = None, + chunk_size: Optional[int] = None, + ): + """Rolls out open-loop trajectories from initial states, get subsequent state and sensor values. + + Python wrapper for rollout.cc, see documentation therein. + Infers nroll and nstep. + Tiles inputs with singleton dimensions. + Allocates outputs if none are given. + + Args: + model: An instance or length nroll sequence of MjModel with the same size signature. + data: Associated mjData instance or sequence of instances with length nthread. + initial_state: Array of initial states from which to roll out trajectories. + ([nroll or 1] x nstate) + control: Open-loop controls array to apply during the rollouts. + ([nroll or 1] x [nstep or 1] x ncontrol) + control_spec: mjtState specification of control vectors. + skip_checks: Whether to skip internal shape and type checks. + nstep: Number of steps in rollouts (inferred if unspecified). + initial_warmstart: Initial qfrc_warmstart array (optional). + ([nroll or 1] x nv) + state: State output array (optional). + (nroll x nstep x nstate) + sensordata: Sensor data output array (optional). + (nroll x nstep x nsensordata) + chunk_size: Determines threadpool chunk size. If unspecified, + chunk_size = max(1, nroll / (nthread * 10)) + + Returns: + state: + State output array, (nroll x nstep x nstate). + sensordata: + Sensor data output array, (nroll x nstep x nsensordata). + + Raises: + RuntimeError: rollout requested after thread pool shutdown. + ValueError: bad shapes or sizes. + """ # fmt: skip + + if self.rollout_ is None: + raise RuntimeError('rollout requested after thread pool shutdown') + + # skip_checks shortcut: + # don't infer nroll/nstep + # don't support singleton expansion + # don't allocate output arrays + # just call rollout and return + if skip_checks: + self.rollout_.rollout( + model, + data, + nstep, + control_spec, + initial_state, + initial_warmstart, + control, + state, + sensordata, + chunk_size, + ) + return state, sensordata + + if not isinstance(model, mujoco.MjModel): + model = list(model) + + # check control_spec + if control_spec & ~mujoco.mjtState.mjSTATE_USER.value: + raise ValueError('control_spec can only contain bits in mjSTATE_USER') + + # check types + if nstep and not isinstance(nstep, int): + raise ValueError('nstep must be an integer') + if chunk_size and not isinstance(chunk_size, int): + raise ValueError('chunk_size must be an integer') + _check_must_be_numeric( + initial_state=initial_state, + initial_warmstart=initial_warmstart, + control=control, + state=state, + sensordata=sensordata, + ) + + # check number of dimensions + _check_number_of_dimensions( + 2, initial_state=initial_state, initial_warmstart=initial_warmstart + ) + _check_number_of_dimensions( + 3, control=control, state=state, sensordata=sensordata + ) + + # ensure 2D, make contiguous, row-major (C ordering) + initial_state = _ensure_2d(initial_state) + initial_warmstart = _ensure_2d(initial_warmstart) + + # ensure 3D, make contiguous, row-major (C ordering) + control = _ensure_3d(control) + state = _ensure_3d(state) + sensordata = _ensure_3d(sensordata) + + # infer nroll, check for incompatibilities + nroll = _infer_dimension( + 0, + 1, + initial_state=initial_state, + initial_warmstart=initial_warmstart, + control=control, + state=state, + sensordata=sensordata, + ) + if isinstance(model, list) and nroll == 1: + nroll = len(model) + + if isinstance(model, list) and len(model) != nroll: + raise ValueError( + f'nroll inferred as {nroll} but model is length {len(model)}' + ) + elif not isinstance(model, list): + model = [model] # Use a length 1 list to simplify code below + + if not isinstance(data, list): + data = [data] # Use a length 1 list to simplify code below + + # infer nstep, check for incompatibilities + nstep = _infer_dimension( + 1, nstep or 1, control=control, state=state, sensordata=sensordata + ) + + # get nstate/ncontrol/nv/nsensordata + # check that they are equal across models + nstate = mujoco.mj_stateSize( + model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value + ) + ncontrol = mujoco.mj_stateSize(model[0], control_spec) + nv = model[0].nv + nsensordata = model[0].nsensordata + for m in model[1:]: + if ( + nstate + != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) + or ncontrol != mujoco.mj_stateSize(m, control_spec) + or nv != m.nv + or nsensordata != m.nsensordata + ): + raise ValueError('models are not compatible') + + # check trailing dimensions + _check_trailing_dimension(nstate, initial_state=initial_state, state=state) + _check_trailing_dimension(ncontrol, control=control) + _check_trailing_dimension(nv, initial_warmstart=initial_warmstart) + _check_trailing_dimension(nsensordata, sensordata=sensordata) + + # tile input arrays/lists if required (singleton expansion) + model = model * nroll if len(model) == 1 else model + initial_state = _tile_if_required(initial_state, nroll) + initial_warmstart = _tile_if_required(initial_warmstart, nroll) + control = _tile_if_required(control, nroll, nstep) + + # allocate output if not provided + if state is None: + state = np.empty((nroll, nstep, nstate)) + if sensordata is None: + sensordata = np.empty((nroll, nstep, nsensordata)) + + # call rollout + self.rollout_.rollout( + model, + data, + nstep, + control_spec, + initial_state, + initial_warmstart, + control, + state, + sensordata, + chunk_size, + ) + + # return outputs + return state, sensordata + + +persistent_rollout = None + + +def shutdown_persistent_pool(): + """Shutdown the persistent thread pool that is optionally created by rollout. + + This is called automatically interpreter shutdown, but can also be called manually. + """ # fmt: skip + global persistent_rollout + if persistent_rollout is not None: + persistent_rollout.close() + persistent_rollout = None + + +atexit.register(shutdown_persistent_pool) + + def rollout( model: Union[mujoco.MjModel, Sequence[mujoco.MjModel]], - data: mujoco.MjData, + data: Union[mujoco.MjData, Sequence[mujoco.MjData]], initial_state: npt.ArrayLike, control: Optional[npt.ArrayLike] = None, *, # require subsequent arguments to be named @@ -35,6 +270,8 @@ def rollout( initial_warmstart: Optional[npt.ArrayLike] = None, state: Optional[npt.ArrayLike] = None, sensordata: Optional[npt.ArrayLike] = None, + chunk_size: Optional[int] = None, + persistent_pool: bool = False, ): """Rolls out open-loop trajectories from initial states, get subsequent states and sensor values. @@ -44,8 +281,8 @@ def rollout( Allocates outputs if none are given. Args: - model: An mjModel or a sequence of MjModel with the same size signature. - data: An associated mjData instance. + model: An instance or length nroll sequence of MjModel with the same size signature. + data: Associated mjData instance or sequence of instances with length nthread. initial_state: Array of initial states from which to roll out trajectories. ([nroll or 1] x nstate) control: Open-loop controls array to apply during the rollouts. @@ -59,6 +296,9 @@ def rollout( (nroll x nstep x nstate) sensordata: Sensor data output array (optional). (nroll x nstep x nsensordata) + chunk_size: Determines threadpool chunk size. If unspecified, + chunk_size = max(1, nroll / (nthread * 10)) + persistent_pool: Determines if a persistent thread pool is created or reused. Returns: state: @@ -69,136 +309,41 @@ def rollout( Raises: ValueError: bad shapes or sizes. """ # fmt: skip - # skip_checks shortcut: - # don't infer nroll/nstep - # don't support singleton expansion - # don't allocate output arrays - # just call rollout and return - if skip_checks: - _rollout.rollout( + if not isinstance(data, list): + data = [data] # Use a length 1 list to simplify code below + + nthread = len(data) if len(data) > 1 else 0 + + # Use a persistent thread pool if requested + if persistent_pool: + # Create or restart persistent threadpool + global persistent_rollout + if persistent_rollout is None: + persistent_rollout = Rollout(nthread=nthread) + if persistent_rollout.nthread != nthread: + persistent_rollout.close() + persistent_rollout = Rollout(nthread=nthread) + rollout_ = persistent_rollout + else: + rollout_ = Rollout(nthread=nthread) + + try: + return rollout_.rollout( model, data, - nstep, - control_spec, initial_state, - initial_warmstart, control, - state, - sensordata, + control_spec=control_spec, + skip_checks=skip_checks, + nstep=nstep, + initial_warmstart=initial_warmstart, + state=state, + sensordata=sensordata, + chunk_size=chunk_size, ) - return state, sensordata - - if not isinstance(model, mujoco.MjModel): - model = list(model) - - # check control_spec - if control_spec & ~mujoco.mjtState.mjSTATE_USER.value: - raise ValueError('control_spec can only contain bits in mjSTATE_USER') - - # check types - if nstep and not isinstance(nstep, int): - raise ValueError('nstep must be an integer') - _check_must_be_numeric( - initial_state=initial_state, - initial_warmstart=initial_warmstart, - control=control, - state=state, - sensordata=sensordata, - ) - - # check number of dimensions - _check_number_of_dimensions( - 2, initial_state=initial_state, initial_warmstart=initial_warmstart - ) - _check_number_of_dimensions( - 3, control=control, state=state, sensordata=sensordata - ) - - # ensure 2D, make contiguous, row-major (C ordering) - initial_state = _ensure_2d(initial_state) - initial_warmstart = _ensure_2d(initial_warmstart) - - # ensure 3D, make contiguous, row-major (C ordering) - control = _ensure_3d(control) - state = _ensure_3d(state) - sensordata = _ensure_3d(sensordata) - - # infer nroll, check for incompatibilities - nroll = _infer_dimension( - 0, - 1, - initial_state=initial_state, - initial_warmstart=initial_warmstart, - control=control, - state=state, - sensordata=sensordata, - ) - if isinstance(model, list) and nroll == 1: - nroll = len(model) - - if isinstance(model, list) and len(model) != nroll: - raise ValueError( - f'nroll inferred as {nroll} but model is length {len(model)}' - ) - elif not isinstance(model, list): - model = [model] # Use a length 1 list to simplify code below - - # infer nstep, check for incompatibilities - nstep = _infer_dimension( - 1, nstep or 1, control=control, state=state, sensordata=sensordata - ) - - # get nstate/ncontrol/nv/nsensordata - # check that they are equal across models - nstate = mujoco.mj_stateSize( - model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS.value - ) - ncontrol = mujoco.mj_stateSize(model[0], control_spec) - nv = model[0].nv - nsensordata = model[0].nsensordata - for m in model[1:]: - if ( - nstate - != mujoco.mj_stateSize(m, mujoco.mjtState.mjSTATE_FULLPHYSICS.value) - or ncontrol != mujoco.mj_stateSize(m, control_spec) - or nv != m.nv - or nsensordata != m.nsensordata - ): - raise ValueError('models are not compatible') - - # check trailing dimensions - _check_trailing_dimension(nstate, initial_state=initial_state, state=state) - _check_trailing_dimension(ncontrol, control=control) - _check_trailing_dimension(nv, initial_warmstart=initial_warmstart) - _check_trailing_dimension(nsensordata, sensordata=sensordata) - - # tile input arrays/lists if required (singleton expansion) - model = model * nroll if len(model) == 1 else model - initial_state = _tile_if_required(initial_state, nroll) - initial_warmstart = _tile_if_required(initial_warmstart, nroll) - control = _tile_if_required(control, nroll, nstep) - - # allocate output if not provided - if state is None: - state = np.empty((nroll, nstep, nstate)) - if sensordata is None: - sensordata = np.empty((nroll, nstep, nsensordata)) - - # call rollout - _rollout.rollout( - model, - data, - nstep, - control_spec, - initial_state, - initial_warmstart, - control, - state, - sensordata, - ) - - # return outputs - return state, sensordata + finally: + if not persistent_pool: + rollout_.close() def _check_must_be_numeric(**kwargs): diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 3cc0d062..af8a5d3a 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -355,7 +355,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): body.pos = body.pos + i model.append(spec.compile()) else: - model = [spec.compile() for i in range(nroll)] + model = [spec.compile() for _ in range(nroll)] nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model[0]) @@ -461,7 +461,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(TEST_XML) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) num_workers = 32 - nroll = 10000 + nroll = 100 nstep = 5 initial_state = np.random.randn(nroll, nstate) state = np.empty((nroll, nstep, nstate)) @@ -478,7 +478,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): def call_rollout(initial_state, control, state, sensordata): rollout.rollout( model_list, - thread_local.data, + [thread_local.data], initial_state, control, skip_checks=True, @@ -519,6 +519,116 @@ class MuJoCoRolloutTest(parameterized.TestCase): np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) + def test_threading_native(self): + model = mujoco.MjModel.from_xml_string(TEST_XML) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + num_workers = 32 + nroll = 100 + nstep = 5 + initial_state = np.random.randn(nroll, nstate) + state = np.empty((nroll, nstep, nstate)) + sensordata = np.empty((nroll, nstep, model.nsensordata)) + control = np.random.randn(nroll, nstep, model.nu) + + model_list = [model] * nroll + data_list = [mujoco.MjData(model) for _ in range(num_workers)] + + rollout.rollout( + model_list, + data_list, + initial_state, + control, + nstep=nstep, + state=state, + sensordata=sensordata, + ) + + data = mujoco.MjData(model) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + def test_threading_native_persistent_object(self): + model = mujoco.MjModel.from_xml_string(TEST_XML) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + num_workers = 32 + nroll = 100 + nstep = 5 + initial_state = np.random.randn(nroll, nstate) + state = np.empty((nroll, nstep, nstate)) + sensordata = np.empty((nroll, nstep, model.nsensordata)) + control = np.random.randn(nroll, nstep, model.nu) + + model_list = [model] * nroll + data_list = [mujoco.MjData(model) for _ in range(num_workers)] + + with rollout.Rollout(nthread=num_workers) as rollout_: + for _ in range(2): + rollout_.rollout( + model_list, + data_list, + initial_state, + control, + nstep=nstep, + state=state, + sensordata=sensordata, + ) + + data = mujoco.MjData(model) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + + rollout_ = rollout.Rollout(nthread=num_workers) + for _ in range(2): + rollout_.rollout( + model_list, + data_list, + initial_state, + control, + nstep=nstep, + state=state, + sensordata=sensordata, + ) + + data = mujoco.MjData(model) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + rollout_.close() + + def test_threading_native_persistent_function(self): + model = mujoco.MjModel.from_xml_string(TEST_XML) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + num_workers = 32 + nroll = 100 + nstep = 5 + initial_state = np.random.randn(nroll, nstate) + state = np.empty((nroll, nstep, nstate)) + sensordata = np.empty((nroll, nstep, model.nsensordata)) + control = np.random.randn(nroll, nstep, model.nu) + + model_list = [model] * nroll + data_list = [mujoco.MjData(model) for _ in range(num_workers)] + + for _ in range(2): + rollout.rollout( + model_list, + data_list, + initial_state, + control, + nstep=nstep, + state=state, + sensordata=sensordata, + persistent_pool=True, + ) + + data = mujoco.MjData(model) + py_state, py_sensordata = py_rollout(model, data, initial_state, control) + np.testing.assert_array_equal(state, py_state) + np.testing.assert_array_equal(sensordata, py_sensordata) + rollout.shutdown_persistent_pool() + # ---------------------------- test advanced operation def test_warmstart(self): diff --git a/python/mujoco/threadpool.cc b/python/mujoco/threadpool.cc new file mode 100644 index 00000000..cd131b18 --- /dev/null +++ b/python/mujoco/threadpool.cc @@ -0,0 +1,87 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "threadpool.h" + +#include +#include +#include +#include +#include + +#include + +namespace mujoco::python { + +ABSL_CONST_INIT thread_local int ThreadPool::worker_id_ = -1; + +// ThreadPool constructor +ThreadPool::ThreadPool(int num_threads) : ctr_(0) { + for (int i = 0; i < num_threads; i++) { + threads_.push_back(std::thread(&ThreadPool::WorkerThread, this, i)); + } +} + +// ThreadPool destructor +ThreadPool::~ThreadPool() { + { + std::unique_lock lock(m_); + for (int i = 0; i < threads_.size(); i++) { + queue_.push(nullptr); + } + cv_in_.notify_all(); + } + for (auto& thread : threads_) { + thread.join(); + } +} + +// ThreadPool scheduler +void ThreadPool::Schedule(std::function task) { + std::unique_lock lock(m_); + queue_.push(std::move(task)); + cv_in_.notify_one(); +} + +// ThreadPool worker +void ThreadPool::WorkerThread(int i) { + worker_id_ = i; + while (true) { + auto task = [&]() { + std::unique_lock lock(m_); + cv_in_.wait(lock, [&]() { return !queue_.empty(); }); + std::function task = std::move(queue_.front()); + queue_.pop(); + cv_in_.notify_one(); + return task; + }(); + if (task == nullptr) { + { + std::unique_lock lock(m_); + ++ctr_; + cv_ext_.notify_one(); + } + break; + } + task(); + + { + std::unique_lock lock(m_); + ++ctr_; + cv_ext_.notify_one(); + } + } +} + +} // namespace mujoco::python diff --git a/python/mujoco/threadpool.h b/python/mujoco/threadpool.h new file mode 100644 index 00000000..5a142ad0 --- /dev/null +++ b/python/mujoco/threadpool.h @@ -0,0 +1,80 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PYTHON_THREADPOOL_H_ +#define MUJOCO_PYTHON_THREADPOOL_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace mujoco::python { + +// ThreadPool class +class ThreadPool { + public: + // constructor + explicit ThreadPool(int num_threads); + + // destructor + ~ThreadPool(); + + int NumThreads() const { return threads_.size(); } + + // returns an ID between 0 and NumThreads() - 1. must be called within + // worker thread (returns -1 if not). + static int WorkerId() { return worker_id_; } + + // ----- methods ----- // + // set task for threadpool + void Schedule(std::function task); + + // return number of tasks completed + std::uint64_t GetCount() { return ctr_; } + + // reset count to zero + void ResetCount() { ctr_ = 0; } + + // wait for count, then return + void WaitCount(int value) { + std::unique_lock lock(m_); + cv_ext_.wait(lock, [&]() { return this->GetCount() >= value; }); + } + + private: + // ----- methods ----- // + + // execute task with available thread + void WorkerThread(int i); + + ABSL_CONST_INIT static thread_local int worker_id_; + + // ----- members ----- // + std::vector threads_; + std::mutex m_; + std::condition_variable cv_in_; + std::condition_variable cv_ext_; + std::queue> queue_; + std::uint64_t ctr_; +}; + +} // namespace mujoco::python + +#endif // MUJOCO_PYTHON_THREADPOOL_H_ From 644dfc7fb54a53781bcfc5b1d87d18d547b0e033 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 16 Dec 2024 10:04:06 -0800 Subject: [PATCH 154/426] Fix assigning bytes to texture data. PiperOrigin-RevId: 706747323 Change-Id: I6b0ee67d4f35db3a149ba8d9979546d3c712bb8b --- python/mujoco/codegen/generate_spec_bindings.py | 7 ++++--- python/mujoco/specs_test.py | 6 ++++++ 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index 700ff883..2db53870 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -170,11 +170,12 @@ def _ptr_binding_code( return MjTypeVec(self.{fullvarname}->data(), self.{fullvarname}->size()); }}, - []({rawclassname}& self, py::object rhs) {{ + []({rawclassname}& self, py::bytes& rhs) {{ self.{fullvarname}->clear(); self.{fullvarname}->reserve(py::len(rhs)); - for (auto val : rhs) {{ - self.{fullvarname}->push_back(py::cast(val)); + std::string_view rhs_view = py::cast(rhs); + for (auto val : rhs_view) {{ + self.{fullvarname}->push_back(static_cast(val)); }} }}, py::return_value_policy::move);""" elif vartype == 'mjStringVec': diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index bd9997e1..e0d3e836 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -891,6 +891,12 @@ class SpecsTest(absltest.TestCase): with self.assertRaises(IndexError): material.textures[-1] = 'x' + def test_assign_texture(self): + spec = mujoco.MjSpec() + texture = spec.add_texture(name='texture', height=2, width=2) + texture.data = np.zeros((2, 2, 3), dtype=np.uint8).tobytes() + spec.compile() + def test_attach_units(self): child = mujoco.MjSpec() parent = mujoco.MjSpec() From 0cf5500b6ad2c1d6893bb5f4b4ddbdc67bf7ae07 Mon Sep 17 00:00:00 2001 From: AaronYoung5 Date: Sat, 14 Dec 2024 07:47:24 -0500 Subject: [PATCH 155/426] Fixed MjSpec introspection with visual.rgba and visual.headlight. Added access to MjSpec.visual.global_. --- python/mujoco/codegen/generate_spec_bindings.py | 7 ++++++- python/mujoco/specs.cc | 9 +++++++++ python/mujoco/specs_test.py | 9 +++++++++ 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index 2db53870..c6764a3d 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -227,8 +227,13 @@ def _binding_code(field: ast_nodes.StructFieldDecl, key: str) -> str: if isinstance(field.type, ast_nodes.ValueType): return _value_binding_code(field.type, key, field.name) elif isinstance(field.type, ast_nodes.AnonymousStructDecl): + code = "" + if field.name in ['headlight', 'rgba']: + for subfield in field.type.fields: + code += _binding_code(subfield, 'mjVisual'+field.name.title()) field.type = ast_nodes.ValueType(name='mjVisual'+field.name.title()) - return _value_binding_code(field.type, key, field.name) + code += _value_binding_code(field.type, key, field.name) + return code elif isinstance(field.type, ast_nodes.PointerType): return _ptr_binding_code(field.type, key, field.name) elif isinstance(field.type, ast_nodes.ArrayType): diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index c376ea35..60a4fa69 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -236,6 +236,8 @@ PYBIND11_MODULE(_specs, m) { py::class_ mjOption(m, "MjOption"); py::class_ mjStatistic(m, "MjStatistic"); py::class_ mjVisual(m, "MjVisual"); + py::class_ mjVisualHeadlight(m, "MjVisualHeadlight"); + py::class_ mjVisualRgba(m, "MjVisualRgba"); py::class_ mjsCompiler(m, "MjsCompiler"); DefineArray(m, "MjCharVec"); DefineArray(m, "MjStringVec"); @@ -979,6 +981,13 @@ PYBIND11_MODULE(_specs, m) { }); mjsPlugin.def("delete", [](raw::MjsPlugin& self) { mjs_delete(self.element); }); + // ============================= MJVISUAL ==================================== + mjVisual.def_property( + "global_", + [](raw::MjVisual& self) -> raw::MjVisualGlobal& { return self.global; }, + [](raw::MjVisual& self, raw::MjVisualGlobal& value) { + self.global = value; + }); #include "specs.cc.inc" } // PYBIND11_MODULE // NOLINT diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index e0d3e836..329f08c9 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -848,22 +848,31 @@ class SpecsTest(absltest.TestCase): + + """) self.assertEqual(spec.option.timestep, 0.001) self.assertEqual(spec.stat.meansize, 0.05) self.assertEqual(spec.visual.quality.shadowsize, 4096) + self.assertEqual(spec.visual.headlight.active, 0) + self.assertEqual(spec.visual.global_, getattr(spec.visual, 'global')) + np.testing.assert_array_equal(spec.visual.rgba.camera, [0, 0, 0, 0]) spec.option.timestep = 0.002 spec.stat.meansize = 0.06 spec.visual.quality.shadowsize = 8192 + spec.visual.headlight.active = 1 + spec.visual.rgba.camera = [1, 1, 1, 1] model = spec.compile() self.assertEqual(model.opt.timestep, 0.002) self.assertEqual(model.stat.meansize, 0.06) self.assertEqual(model.vis.quality.shadowsize, 8192) + self.assertEqual(model.vis.headlight.active, 1) + np.testing.assert_array_equal(model.vis.rgba.camera, [1, 1, 1, 1]) def test_assign_list_element(self): spec = mujoco.MjSpec() From 287256e63fe181a2f9b1eb82ae34988597e70cfd Mon Sep 17 00:00:00 2001 From: AaronYoung5 Date: Mon, 16 Dec 2024 14:22:59 -0500 Subject: [PATCH 156/426] added struct function for spec bindings --- .../mujoco/codegen/generate_spec_bindings.py | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index c6764a3d..d37fc8cc 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -70,6 +70,19 @@ def _value_binding_code( return f'{classname}.def_property({",".join(def_property_args)});' +def _struct_binding_code( + field: ast_nodes.AnonymousStructDecl, classname: str = '', varname: str = '' +) -> str: + code = '' + name = classname + varname.title() + # explicitly generate for nested fields with arrays + if any(isinstance(f.type, ast_nodes.ArrayType) for f in field.fields): + for subfield in field.fields: + code += _binding_code(subfield, name) + # generate for the struct itself + field = ast_nodes.ValueType(name=name) + code += _value_binding_code(field, classname, varname) + return code def _array_binding_code( field: ast_nodes.ArrayType, classname: str = '', varname: str = '' @@ -227,13 +240,7 @@ def _binding_code(field: ast_nodes.StructFieldDecl, key: str) -> str: if isinstance(field.type, ast_nodes.ValueType): return _value_binding_code(field.type, key, field.name) elif isinstance(field.type, ast_nodes.AnonymousStructDecl): - code = "" - if field.name in ['headlight', 'rgba']: - for subfield in field.type.fields: - code += _binding_code(subfield, 'mjVisual'+field.name.title()) - field.type = ast_nodes.ValueType(name='mjVisual'+field.name.title()) - code += _value_binding_code(field.type, key, field.name) - return code + return _struct_binding_code(field.type, key, field.name) elif isinstance(field.type, ast_nodes.PointerType): return _ptr_binding_code(field.type, key, field.name) elif isinstance(field.type, ast_nodes.ArrayType): From 3ef5ebcf01f7fd4b674be8018fb1f1bf4cc27800 Mon Sep 17 00:00:00 2001 From: AaronYoung5 Date: Tue, 17 Dec 2024 12:57:48 -0500 Subject: [PATCH 157/426] removed trailing spaces --- python/mujoco/specs.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 60a4fa69..2749d5e5 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -983,10 +983,10 @@ PYBIND11_MODULE(_specs, m) { [](raw::MjsPlugin& self) { mjs_delete(self.element); }); // ============================= MJVISUAL ==================================== mjVisual.def_property( - "global_", + "global_", [](raw::MjVisual& self) -> raw::MjVisualGlobal& { return self.global; }, - [](raw::MjVisual& self, raw::MjVisualGlobal& value) { - self.global = value; + [](raw::MjVisual& self, raw::MjVisualGlobal& value) { + self.global = value; }); #include "specs.cc.inc" From edec8c5e3a585e6a53e51204b9d81a7912bcc7c1 Mon Sep 17 00:00:00 2001 From: Kristian Hartikainen Date: Wed, 18 Dec 2024 06:28:24 -0800 Subject: [PATCH 158/426] Copybara import of the project: -- 0e1aae484a1a6d2d52d0b99bf62f116d42ed9a1f by Kristian Hartikainen : Fix typo in `changelog.rst` COPYBARA_INTEGRATE_REVIEW=https://github.com/google-deepmind/mujoco/pull/2298 from hartikainen:patch-1 0e1aae484a1a6d2d52d0b99bf62f116d42ed9a1f PiperOrigin-RevId: 707528220 Change-Id: Ie3bc28cf7a8456e4ba8bfc405ab7190a84d5a69e --- doc/changelog.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 33456f82..a4988dfb 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,7 +9,7 @@ Python bindings ^^^^^^^^^^^^^^^ - :ref:`rollout` now features native multi-threading. If a sequence of ``MjData`` instances of length ``nthread`` is passed in, ``rollout`` will automatically create a thread pool and parallelize - the computation. The thread pool can be resused across calls, but then the function cannot be called simultaneously + the computation. The thread pool can be reused across calls, but then the function cannot be called simultaneously from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. From 4d0865c2e8b8b1413401d666c1d94f13459e9aea Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Thu, 19 Dec 2024 12:41:25 -0800 Subject: [PATCH 159/426] Add light fields to mjx.Model, needed for Madrona. PiperOrigin-RevId: 707996293 Change-Id: I238dac72b8f209955b2a1060571db3b14e355dd5 --- mjx/mujoco/mjx/_src/types.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 4ec13c72..a97d0cb2 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -666,11 +666,13 @@ class Model(PyTreeNode): light_bodyid: id of light's body (nlight,) light_targetbodyid: id of targeted body; -1: none (nlight,) light_directional: directional light (nlight,) + light_castshadow: does light cast shadows (nlight,) light_pos: position rel. to body frame (nlight, 3) light_dir: direction rel. to body frame (nlight, 3) light_poscom0: global position rel. to sub-com in qpos0 (nlight, 3) light_pos0: global position rel. to body in qpos0 (nlight, 3) light_dir0: global direction in qpos0 (nlight, 3) + light_cutoff: OpenGL cutoff (nlight,) flex_contype: flex contact type (nflex,) flex_conaffinity: flex contact affinity (nflex,) flex_condim: contact dimensionality (1, 3, 4, 6) (nflex,) @@ -879,7 +881,7 @@ class Model(PyTreeNode): nB: int # pylint:disable=invalid-name nC: int # pylint:disable=invalid-name nD: int # pylint:disable=invalid-name - nJmom: int + nJmom: int # pylint:disable=invalid-name ntree: int = _restricted_to('mujoco') ngravcomp: int nuserdata: int @@ -993,12 +995,14 @@ class Model(PyTreeNode): light_mode: np.ndarray light_bodyid: np.ndarray = _restricted_to('mujoco') light_targetbodyid: np.ndarray = _restricted_to('mujoco') - light_directional: np.ndarray + light_directional: jax.Array + light_castshadow: jax.Array light_pos: jax.Array light_dir: jax.Array light_poscom0: np.ndarray = _restricted_to('mujoco') light_pos0: np.ndarray light_dir0: np.ndarray + light_cutoff: jax.Array flex_contype: np.ndarray = _restricted_to('mujoco') flex_conaffinity: np.ndarray = _restricted_to('mujoco') flex_condim: np.ndarray = _restricted_to('mujoco') From 3f855f32d9b14179e17812056732d6809f0b4320 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 20 Dec 2024 10:10:25 -0800 Subject: [PATCH 160/426] Fix stack allocation leak in `mjv_addGeoms()` PiperOrigin-RevId: 708356387 Change-Id: If3d227028ea8f809390911b578878d985d9d7b1a --- src/engine/engine_vis_visualize.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index e48f1834..2764bee7 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1730,6 +1730,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, category = mjCAT_DYNAMIC; if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { // mark actuated tendons + mj_markStack(d); int* tendon_actuated = mjSTACKALLOC(d, m->ntendon, int); mju_zeroInt(tendon_actuated, m->ntendon); for (int i=0; i < m->nu; i++) { @@ -1827,7 +1828,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int ncatenary = m->vis.quality.numslices + 1; // allocate catenary - mj_markStack(d); mjtNum* catenary = mjSTACKALLOC(d, 3*ncatenary, mjtNum); // points along catenary path @@ -1852,10 +1852,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, FINISH } - mj_freeStack(d); } } } + mj_freeStack(d); } // slider-crank From 273ced428ae65f2e211be728cdea6c78931c8d13 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 24 Dec 2024 06:43:08 -0800 Subject: [PATCH 161/426] Add simple unit test for mjv_updateScene. PiperOrigin-RevId: 709334033 Change-Id: I69adc1d3ad0565f3684346749a372894d12ce99a --- test/engine/CMakeLists.txt | 2 + test/engine/engine_vis_visualize_test.cc | 81 ++++++++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 test/engine/engine_vis_visualize_test.cc diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 625a0cbf..2acdc4b9 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -73,3 +73,5 @@ mujoco_test( ENVIRONMENT "MUJOCO_PLUGIN_DIR=$" ) + +mujoco_test(engine_vis_visualize_test) diff --git a/test/engine/engine_vis_visualize_test.cc b/test/engine/engine_vis_visualize_test.cc new file mode 100644 index 00000000..4a9a6497 --- /dev/null +++ b/test/engine/engine_vis_visualize_test.cc @@ -0,0 +1,81 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::NotNull; +using MjvSceneTest = MujocoTest; + +constexpr int kMaxGeom = 10000; + +static const char* const kModelPath = "testdata/model.xml"; + +TEST_F(MjvSceneTest, UpdateScene) { + for (const char* path : {kModelPath}) { + const std::string xml_path = GetTestDataFilePath(path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()) << "Failed to load model from " << path; + mjData* data = mj_makeData(model); + + while (data->time < .2) { + mj_step(model, data); + } + + mjvScene scn; + mjv_defaultScene(&scn); + mjv_makeScene(model, &scn, kMaxGeom); + + mjvOption opt; + mjv_defaultOption(&opt); + + mjvPerturb pert; + mjv_defaultPerturb(&pert); + + mjvCamera cam; + mjv_defaultFreeCamera(model, &cam); + + // Enable all flags to exercise all code paths + for (int i = 0; i < mjNVISFLAG; ++i) { + opt.flags[i] = 1; + } + + mjv_updateScene(model, data, &opt, &pert, &cam, mjCAT_ALL, &scn); + EXPECT_GT(scn.ngeom, 0); + if (model->nskin) EXPECT_GT(scn.nskin, 0); + EXPECT_GT(scn.nlight, 0); + + mjv_updateScene(model, data, &opt, &pert, &cam, mjCAT_ALL, &scn); + + // call mj_copyData to expose any memory leaks mjv_updateScene. + mjData* data_copy = mj_copyData(nullptr, model, data); + + mjv_freeScene(&scn); + mj_deleteData(data_copy); + mj_deleteData(data); + mj_deleteModel(model); + } +} + +} // namespace +} // namespace mujoco From ea98c57921ec213aed0a61995f65034f7b8888d9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 25 Dec 2024 07:43:26 -0800 Subject: [PATCH 162/426] Clean up engine_core_smooth_benchmark_test. PiperOrigin-RevId: 709573203 Change-Id: I461f259c33d74b3eaf0b6fe58124fe09418c5303 --- .../engine_core_smooth_benchmark_test.cc | 29 +++++++------------ 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index 9a80ae8e..3b728169 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -14,7 +14,6 @@ // A benchmark for comparing different implementations of mj_solveLD. -#include #include #include #include @@ -25,9 +24,6 @@ namespace mujoco { namespace { -// number of steps to roll out before benchmarking -static const int kNumWarmupSteps = 200; - // number of steps to benchmark static const int kNumBenchmarkSteps = 50; @@ -42,37 +38,31 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { } mjData* d = mj_makeData(m); + mj_forward(m, d); - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate gradient + // allocate input and output vectors mj_markStack(d); - mjtNum *grad = mj_stackAllocNum(d, m->nv); - mjtNum *Ma = mj_stackAllocNum(d, m->nv); + mjtNum *vec = mj_stackAllocNum(d, m->nv); mjtNum *res = mj_stackAllocNum(d, m->nv); - // compute gradient - mj_mulM(m, d, Ma, d->qacc); + // arbitrary input vector for (int i=0; i < m->nv; i++) { - grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; + vec[i] = 0.2 + 0.3*i; } - // CSR matrix + // make CSR matrix mjtNum* LDs = mj_stackAllocNum(d, m->nC); for (int i=0; i < m->nC; i++) { LDs[i] = d->qLD[d->mapM2C[i]]; } - // reset state, benchmark subsequent kNumBenchmarkSteps steps + // benchmark while (state.KeepRunningBatch(kNumBenchmarkSteps)) { for (int i=0; i < kNumBenchmarkSteps; i++) { if (featherstone) { - mj_solveM(m, d, res, grad, 1); + mj_solveM(m, d, res, vec, 1); } else { - mju_copy(res, grad, m->nv); + mju_copy(res, vec, m->nv); mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind); } @@ -82,6 +72,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { // finalize mj_freeStack(d); mj_deleteData(d); + mj_deleteModel(m); state.SetItemsProcessed(state.iterations()); } From a8a960771234c8d84b085516d9f76a53477af28c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 25 Dec 2024 08:08:49 -0800 Subject: [PATCH 163/426] Inline `mju_dotSparse` in engine_util_sparse.h PiperOrigin-RevId: 709577108 Change-Id: I76547122b5780bec76a12384effcb66315f84e13 --- src/engine/engine_util_sparse.c | 49 ----------------------------- src/engine/engine_util_sparse.h | 56 ++++++++++++++++++++++++++++++--- 2 files changed, 52 insertions(+), 53 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 87580444..27b26576 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -27,55 +27,6 @@ //------------------------------ sparse operations ------------------------------------------------- -// dot-product, first vector is sparse -// flg_unc1: is vec1 memory layout uncompressed -mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, - int flg_unc1) { -#ifdef mjUSEAVX - return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1); -#else - int i = 0; - mjtNum res = 0; - int n_4 = nnz1 - 4; - mjtNum res0 = 0; - mjtNum res1 = 0; - mjtNum res2 = 0; - mjtNum res3 = 0; - - - if (flg_unc1) { - for (; i <= n_4; i+=4) { - res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]]; - res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]]; - res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]]; - res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]]; - } - } else { - for (; i <= n_4; i+=4) { - res0 += vec1[i+0] * vec2[ind1[i+0]]; - res1 += vec1[i+1] * vec2[ind1[i+1]]; - res2 += vec1[i+2] * vec2[ind1[i+2]]; - res3 += vec1[i+3] * vec2[ind1[i+3]]; - } - } - res = (res0 + res2) + (res1 + res3); - - // scalar part - if (flg_unc1) { - for (; i < nnz1; i++) { - res += vec1[ind1[i]] * vec2[ind1[i]]; - } - } else { - for (; i < nnz1; i++) { - res += vec1[i] * vec2[ind1[i]]; - } - } - - return res; -#endif // mjUSEAVX -} - - // dot-productX3, first vector is sparse; supernode of size 3 void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2, diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index d1dc2d5d..319af57b 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -25,10 +25,6 @@ extern "C" { //------------------------------ sparse operations ------------------------------------------------- -// dot-product, vec1 is sparse, can be uncompressed -MJAPI mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, - int flg_unc1); - // dot-product, both vectors are sparse, vec2 can be uncompressed MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, int nnz2, const int* ind2, int flg_unc2); @@ -114,6 +110,58 @@ MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); MJAPI int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, int n, mjData* d); +// ------------------------------ inlined functions ------------------------------------------------ + +// dot-product, first vector is sparse +// flg_unc1: is vec1 memory layout uncompressed +static inline +mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, + int flg_unc1) { +#ifdef mjUSEAVX + return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1); +#else + int i = 0; + mjtNum res = 0; + int n_4 = nnz1 - 4; + mjtNum res0 = 0; + mjtNum res1 = 0; + mjtNum res2 = 0; + mjtNum res3 = 0; + + + if (flg_unc1) { + for (; i <= n_4; i+=4) { + res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]]; + res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]]; + res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]]; + res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]]; + } + } else { + for (; i <= n_4; i+=4) { + res0 += vec1[i+0] * vec2[ind1[i+0]]; + res1 += vec1[i+1] * vec2[ind1[i+1]]; + res2 += vec1[i+2] * vec2[ind1[i+2]]; + res3 += vec1[i+3] * vec2[ind1[i+3]]; + } + } + res = (res0 + res2) + (res1 + res3); + + // scalar part + if (flg_unc1) { + for (; i < nnz1; i++) { + res += vec1[ind1[i]] * vec2[ind1[i]]; + } + } else { + for (; i < nnz1; i++) { + res += vec1[i] * vec2[ind1[i]]; + } + } + + return res; +#endif // mjUSEAVX +} + + #ifdef __cplusplus } #endif From 8dd360a2c087558f96b33b6341605783d0e2aa35 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 27 Dec 2024 08:17:58 -0800 Subject: [PATCH 164/426] Add some elements to test model. PiperOrigin-RevId: 710067190 Change-Id: I478085b8177ea8ebfa4190db8eb722c306c9793e --- test/testdata/model.xml | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 98274616..e122f927 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -43,12 +43,13 @@ - + + @@ -81,9 +82,10 @@ + - + @@ -127,6 +129,10 @@ + + + + From e67599f5171108c6a233f4d99ac06a0eea6b3064 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 28 Dec 2024 08:49:36 -0800 Subject: [PATCH 165/426] Add weak deprecation notice include in XMLreference. PiperOrigin-RevId: 710300945 Change-Id: I5a3dd066ef643d606d97ec36ffe1e797bb628659 --- doc/XMLreference.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 49818cfa..a903b526 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -264,6 +264,11 @@ how to use includes and how to modularize large files if desired. The name of the XML file to be included. The file location is relative to the directory of the main MJCF file. If the file is not in the same directory, it should be prefixed with a relative path. +.. admonition:: Prefer attach to include + :class: note + + While some use cases for :ref:`include` remain valid, it is recommended to use the + :ref:`attach` element instead, where applicable. .. _mujoco: From 0336114a8882f3a512a225e9d4faf6c2c46eaa85 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 28 Dec 2024 16:04:20 -0800 Subject: [PATCH 166/426] Add `restrict` to output variable in `mj_solveLDs`. PiperOrigin-RevId: 710358020 Change-Id: Ia63048161d7622f21954e98346b7ff4e37ecb342 --- src/engine/engine_core_smooth.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ca16cb30..869ea13e 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1578,7 +1578,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L -void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, +void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, const int* rownnz, const int* rowadr, const int* diag, const int* colind) { // x <- L^-T x for (int i=nv-2; i >= 0; i--) { From 8cb253b6ca240833885c82b557d6884746fd8ba0 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 30 Dec 2024 04:14:31 -0800 Subject: [PATCH 167/426] Fix typos. PiperOrigin-RevId: 710678815 Change-Id: Iab6f603f8f4d782ac7f34140add39e1f37625167 --- doc/includes/references.h | 2 +- include/mujoco/mjui.h | 2 +- introspect/structs.py | 2 +- mjx/mujoco/mjx/_src/smooth.py | 2 +- src/engine/engine_forward.c | 2 +- src/engine/engine_support.c | 4 ++-- src/engine/engine_vis_interact.c | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 948d88f9..58a6b7d5 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2500,7 +2500,7 @@ struct mjUI_ { // entire UI // UI sizes (framebuffer units) int width; // width - int height; // current heigth + int height; // current height int maxheight; // height when all sections open int scroll; // scroll from top of UI diff --git a/include/mujoco/mjui.h b/include/mujoco/mjui.h index f68141ce..990bf11e 100644 --- a/include/mujoco/mjui.h +++ b/include/mujoco/mjui.h @@ -304,7 +304,7 @@ struct mjUI_ { // entire UI // UI sizes (framebuffer units) int width; // width - int height; // current heigth + int height; // current height int maxheight; // height when all sections open int scroll; // scroll from top of UI diff --git a/introspect/structs.py b/introspect/structs.py index 194e410d..396b4dbe 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -12380,7 +12380,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='height', type=ValueType(name='int'), - doc='current heigth', + doc='current height', ), StructFieldDecl( name='maxheight', diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index 9ad010c1..815c3007 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -988,7 +988,7 @@ def _site_dof_mask(m: Model) -> np.ndarray: mask = np.ones((m.nu, m.nv)) for i in np.nonzero(m.actuator_trnid[:, 1] != -1)[0]: id_, refid = m.actuator_trnid[i] - # intialize last dof address for each body + # initialize last dof address for each body b0 = m.body_weldid[m.site_bodyid[id_]] b1 = m.body_weldid[m.site_bodyid[refid]] dofadr0 = m.body_dofadr[b0] + m.body_dofnum[b0] - 1 diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b90ba5d8..b9e1e6bf 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -147,7 +147,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) { mj_collision(m, d); // timed internally (POS_COLLISION) } - // have threadpool: inertia and collision on seperate threads + // have threadpool: inertia and collision on separate threads else { mjTask tasks[2]; mjFwdPositionArgs forward_args; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index e77520e2..12538817 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -278,7 +278,7 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) { return 0; } - // intialize last dof address for each body + // initialize last dof address for each body da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1; da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1; @@ -361,7 +361,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) { return 0; } - // intialize last dof + // initialize last dof int da = m->body_dofadr[body] + m->body_dofnum[body] - 1; int NV = 0; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index eb3530b1..2cb8c8a0 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -497,7 +497,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, // get current model rotation mju_f2n(rotate, scn->rotate, 4); - // compose rotation, normalize and and set + // compose rotation, normalize and set mju_mulQuat(result, quat, rotate); mju_normalize4(result); mju_n2f(scn->rotate, result, 4); From e42370c982ea7a4fe37be20f31c7fb32be283f38 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 30 Dec 2024 04:18:11 -0800 Subject: [PATCH 168/426] Avoid allocation and copying in implicit solver's `addJTBJSparse` PiperOrigin-RevId: 710679558 Change-Id: Ie75fdcd1127ff619c2668f568ab6a8a11e079dda --- src/engine/engine_derivative.c | 20 +++++--------------- src/engine/engine_util_sparse.c | 10 +++++----- src/engine/engine_util_sparse.h | 8 ++++---- 3 files changed, 14 insertions(+), 24 deletions(-) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index d4c764cb..deb2c0c9 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -730,11 +730,6 @@ static void addJTBJSparse( const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* B, int n, int offset, const int* J_rownnz, const int* J_rowadr, const int* J_colind) { - int nv = m->nv; - - // allocate row - mj_markStack(d); - mjtNum* row = mjSTACKALLOC(d, nv, mjtNum); // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) ) for (int i = 0; i < n; i++) { @@ -749,19 +744,14 @@ static void addJTBJSparse( int ik = J_rowadr[offset_i] + k; int colik = J_colind[ik]; - // row = J(i,k)*B(i,j)*J(j,:) - mju_scl(row, J + J_rowadr[offset_j], J[ik]*B[i*n+j], J_rownnz[offset_j]); - - // qDeriv(k,:) += row - mju_addToSparseInc(d->qDeriv + d->D_rowadr[colik], row, - d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik], - J_rownnz[offset_j], J_colind + J_rowadr[offset_j]); + // qDeriv(k,:) += J(j,:) * J(i,k)*B(i,j) + mju_addToSclSparseInc(d->qDeriv + d->D_rowadr[colik], J + J_rowadr[offset_j], + d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik], + J_rownnz[offset_j], J_colind + J_rowadr[offset_j], + J[ik]*B[i*n+j]); } } } - - // free space - mj_freeStack(d); } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 27b26576..895179e2 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -355,10 +355,10 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu -// dst += src, only at common non-zero indices -void mju_addToSparseInc(mjtNum* dst, const mjtNum* src, - int nnzdst, const int* inddst, - int nnzsrc, const int* indsrc) { +// dst += scl*src, only at common non-zero indices +void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, + int nnzdst, const int* inddst, + int nnzsrc, const int* indsrc, mjtNum scl) { if (!nnzdst || !nnzsrc) { return; } @@ -368,7 +368,7 @@ void mju_addToSparseInc(mjtNum* dst, const mjtNum* src, // common non-zero index if (inds == indd) { // add - dst[adrd] += src[adrs]; + dst[adrd] += scl * src[adrs]; // advance src if (++adrs < nnzsrc) { diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 319af57b..48b41979 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -63,10 +63,10 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind); -// dst += src, only at common non-zero indices -void mju_addToSparseInc(mjtNum* dst, const mjtNum* src, - int nnzdst, const int* inddst, - int nnzsrc, const int* indsrc); +// dst += scl * src, only at common non-zero indices +void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, + int nnzdst, const int* inddst, + int nnzsrc, const int* indsrc, mjtNum scl); // add to sparse matrix: dst = dst + scl*src, return nnz of result int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl, From ec322641b78188fcd15c98dc60ea0f9dc3f6223e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 30 Dec 2024 11:14:00 -0800 Subject: [PATCH 169/426] Fix bug in `mj_mulM2`. PiperOrigin-RevId: 710756899 Change-Id: I30c0725859ba0f1a59750dc89eebb9eba84fa71b --- doc/changelog.rst | 1 + src/engine/engine_support.c | 40 ++++-------- test/engine/engine_core_smooth_test.cc | 86 ++++++++++++++++---------- test/engine/engine_support_test.cc | 79 +++++++++++++++++++++-- test/engine/testdata/inertia.xml | 39 ++++++++++++ 5 files changed, 178 insertions(+), 67 deletions(-) create mode 100644 test/engine/testdata/inertia.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index a4988dfb..c5ec24be 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,7 @@ Python bindings Bug fixes ^^^^^^^^^ - Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). +- Fixed a bug in :ref:`mj_mulM2` and added a test. Version 3.2.6 (Dec 2, 2024) --------------------------- diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 12538817..94a03de4 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1085,51 +1085,28 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum // multiply vector by M^(1/2) void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { - int adr, nv = m->nv; + int nv = m->nv; const mjtNum* qLD = d->qLD; const mjtNum* qLDiagSqrtInv = d->qLDiagSqrtInv; const int* dofMadr = m->dof_Madr; mju_zero(res, nv); + // res = L * vec for (int i=0; i < nv; i++) { -#ifdef mjUSEAVX - // simple: diagonal division, AVX - if (m->dof_simplenum[i] >= 4) { - // init - __m256d result, val1, val2; - - // parallel computation - val1 = _mm256_loadu_pd(vec+i); - val2 = _mm256_set_pd(qLDiagSqrtInv[dofMadr[i+3]], - qLDiagSqrtInv[dofMadr[i+2]], - qLDiagSqrtInv[dofMadr[i+1]], - qLDiagSqrtInv[dofMadr[i+0]]); - result = _mm256_div_pd(val1, val2); - - // store result - _mm256_storeu_pd(res+i, result); - - // skip rest of block - i += 3; - continue; - } -#endif - - // simple: diagonal division + // simple: diagonal if (m->dof_simplenum[i]) { - res[i] = vec[i]/qLDiagSqrtInv[i]; + res[i] = vec[i]; } // regular: full multiplication else { // diagonal - adr = dofMadr[i]; - res[i] += vec[i]/qLDiagSqrtInv[i]; + res[i] += vec[i]; // off-diagonal int j = m->dof_parentid[i]; - adr++; + int adr = dofMadr[i] + 1; while (j >= 0) { res[i] += qLD[adr]*vec[j]; @@ -1139,6 +1116,11 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) } } } + + // res = sqrt(D) * res + for (int i=0; i < nv; i++) { + res[i] /= qLDiagSqrtInv[i]; + } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index f27d1670..a6d2d0b3 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -407,39 +407,61 @@ TEST_F(CoreSmoothTest, SolveMIsland) { mj_deleteModel(model); } -TEST_F(CoreSmoothTest, SolveLD2) { - static constexpr char xml[] = R"( - - - - - +static const char* const kInertiaPath = "engine/testdata/inertia.xml"; + +TEST_F(CoreSmoothTest, FactorI) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error; + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // dense L matrix + int nv = model->nv; + vector Ldense(nv*nv); + mj_fullM(model, Ldense.data(), data->qLD); + // clear upper triangle, set diagonal to 1 + for (int i=0; i < nv; i++) { + for (int j=i; j < nv; j++) { + Ldense[i*nv+j] = i == j ? 1 : 0; + } + } + + // dense D matrix + vector Ddense(nv*nv); + mj_fullM(model, Ddense.data(), data->qLD); + // clear everything but the diagonal + for (int i=0; i < nv; i++) { + for (int j=0; j < nv; j++) { + if (i != j) Ddense[i*nv+j] = 0; + } + } + + // perform multiplication: M = L^T * D * L + vector tmp(nv*nv); + vector M(nv*nv); + mju_mulMatMat(tmp.data(), Ddense.data(), Ldense.data(), nv, nv, nv); + mju_mulMatTMat(M.data(), Ldense.data(), tmp.data(), nv, nv, nv); + + // dense M matrix + vector Mexpected(nv*nv); + mj_fullM(model, Mexpected.data(), data->qM); + + // expect matrices to match to floating point precision + EXPECT_THAT(M, Pointwise(DoubleNear(1e-12), Mexpected)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(CoreSmoothTest, SolveLD2) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; - - - - - - - - - - - - - - - - - - - - - - - - )"; - mjModel* m = LoadModelFromString(xml); mjData* d = mj_makeData(m); mj_forward(m, d); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 36dd6e06..456143cd 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -29,12 +29,13 @@ namespace mujoco { namespace { -using ::testing::DoubleNear; -using ::testing::Eq; +using ::std::vector; using ::testing::ContainsRegex; // NOLINT -using ::testing::MatchesRegex; -using ::testing::Pointwise; +using ::testing::DoubleNear; using ::testing::ElementsAreArray; +using ::testing::Eq; +using ::testing::MatchesRegex; +using ::testing::NotNull; using ::testing::Pointwise; using AngMomMatTest = MujocoTest; @@ -685,9 +686,9 @@ TEST_F(SupportTest, GetSetStateStepEqual) { mj_deleteModel(model); } -using AddMTest = MujocoTest; +using InertiaTest = MujocoTest; -TEST_F(AddMTest, DenseSameAsSparse) { +TEST_F(InertiaTest, DenseSameAsSparse) { mjModel* m = LoadModelFromPath("humanoid/humanoid100.xml"); mjData* d = mj_makeData(m); int nv = m->nv; @@ -732,6 +733,72 @@ TEST_F(AddMTest, DenseSameAsSparse) { mj_deleteModel(m); } +static const char* const kInertiaPath = "engine/testdata/inertia.xml"; + +TEST_F(InertiaTest, mulM) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error; + int nv = model->nv; + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // dense M matrix + vector Mdense(nv*nv); + mj_fullM(model, Mdense.data(), data->qM); + + // arbitrary RHS vector + vector vec(nv); + for (int i=0; i < nv; i++) vec[i] = vec[i] = 20 + 30*i; + + // multiply directly + vector res1(nv, 0); + mju_mulMatVec(res1.data(), Mdense.data(), vec.data(), nv, nv); + + // multiply with mj_mulM + vector res2(nv, 0); + mj_mulM(model, data, res2.data(), vec.data()); + + // expect vectors to match to floating point precision + EXPECT_THAT(res1, Pointwise(DoubleNear(1e-10), res2)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(InertiaTest, mulM2) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error; + int nv = model->nv; + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // arbitrary RHS vector + vector vec(nv); + for (int i=0; i < nv; i++) vec[i] = .2 + .3*i; + + // multiply sqrtMvec = M^1/2 * vec + vector sqrtMvec(nv); + mj_mulM2(model, data, sqrtMvec.data(), vec.data()); + + // multiply Mvec = M * vec + vector Mvec(nv); + mj_mulM(model, data, Mvec.data(), vec.data()); + + // compute vec' * M * vec in two different ways, expect them to match + mjtNum sqrtMvec2 = mju_dot(sqrtMvec.data(), sqrtMvec.data(), nv); + mjtNum vecMvec = mju_dot(vec.data(), Mvec.data(), nv); + EXPECT_FLOAT_EQ(sqrtMvec2, vecMvec); + + mj_deleteData(data); + mj_deleteModel(model); +} + static const char* const kIlslandEfcPath = "engine/testdata/island/island_efc.xml"; diff --git a/test/engine/testdata/inertia.xml b/test/engine/testdata/inertia.xml new file mode 100644 index 00000000..dc6a4a15 --- /dev/null +++ b/test/engine/testdata/inertia.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 69c9ac074a7f54c7f9b7619b8bcc3d0eb69bfbcd Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 30 Dec 2024 15:23:41 -0800 Subject: [PATCH 170/426] Remove `mjData.qLDiagSqrtInv`, add corresponding argument to `mj_solveM2`. - `qLDiagSqrtInv` is only required for the dual solvers. It is now computed as-needed rather than unconditionally. - `mj_solveM2` now requires a new input array `sqrtInvD` which contains the square root of the inverse diagonal D (formerly saved in `qLDiagSqrtInv`). PiperOrigin-RevId: 710805133 Change-Id: I0622d6a8da3882916824e9c10bad9223c122c321 --- doc/changelog.rst | 9 +++++++++ doc/includes/references.h | 4 ++-- include/mujoco/mjdata.h | 1 - include/mujoco/mjxmacro.h | 1 - include/mujoco/mujoco.h | 3 ++- introspect/functions.py | 6 ++++++ introspect/structs.py | 8 -------- mjx/mujoco/mjx/_src/io.py | 1 - mjx/mujoco/mjx/_src/types.py | 2 -- python/mujoco/functions.cc | 8 ++++++-- python/mujoco/indexer_xmacro.h | 1 - src/engine/engine_core_constraint.c | 10 ++++++++-- src/engine/engine_core_smooth.c | 18 +++++++----------- src/engine/engine_core_smooth.h | 6 +++--- src/engine/engine_forward.c | 4 ++-- src/engine/engine_print.c | 1 - src/engine/engine_support.c | 5 ++--- src/engine/engine_vis_interact.c | 6 +++++- unity/Runtime/Bindings/MjBindings.cs | 3 +-- 19 files changed, 53 insertions(+), 44 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index c5ec24be..aae6bdb5 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -13,6 +13,15 @@ Python bindings from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. +General +^^^^^^^ + +.. admonition:: Breaking API changes (minor) + :class: attention + + - The field ``mjData.qLDiagSqrtInv`` has been removed. This field is only required for the dual solvers. It is now + computed as-needed rather than unconditionally. Relatedly, added the corresponding argument to :ref:`mj_solveM2`. + Bug fixes ^^^^^^^^^ - Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). diff --git a/doc/includes/references.h b/doc/includes/references.h index 58a6b7d5..685d8e3d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -273,7 +273,6 @@ struct mjData_ { // computed by mj_fwdPosition/mj_factorM mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) - mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) // computed by mj_collisionTree mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6) @@ -3221,7 +3220,8 @@ void mj_transmission(const mjModel* m, mjData* d); void mj_crb(const mjModel* m, mjData* d); void mj_factorM(const mjModel* m, mjData* d); void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); -void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, + const mjtNum* sqrtInvD, int n); void mj_comVel(const mjModel* m, mjData* d); void mj_passive(const mjModel* m, mjData* d); void mj_subtreeVel(const mjModel* m, mjData* d); diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index d7185142..c29af364 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -301,7 +301,6 @@ struct mjData_ { // computed by mj_fwdPosition/mj_factorM mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) - mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) // computed by mj_collisionTree mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index a54c4ff5..9edd1fed 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -632,7 +632,6 @@ X ( mjtNum, qM, nM, 1 ) \ X ( mjtNum, qLD, nM, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ - X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \ XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ XMJV( mjtByte, bvh_active, nbvh, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index a6df23ad..66f5fa98 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -365,7 +365,8 @@ MJAPI void mj_factorM(const mjModel* m, mjData* d); MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); // Half of linear solve: x = sqrt(inv(D))*inv(L')*y -MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, + const mjtNum* sqrtInvD, int n); // Compute cvel, cdof_dot. MJAPI void mj_comVel(const mjModel* m, mjData* d); diff --git a/introspect/functions.py b/introspect/functions.py index 4e1baaad..b16bee97 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -1868,6 +1868,12 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='mjtNum', is_const=True), ), ), + FunctionParameterDecl( + name='sqrtInvD', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), FunctionParameterDecl( name='n', type=ValueType(name='int'), diff --git a/introspect/structs.py b/introspect/structs.py index 396b4dbe..f02158d9 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -5213,14 +5213,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='1/diag(D)', array_extent=('nv',), ), - StructFieldDecl( - name='qLDiagSqrtInv', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='1/sqrt(diag(D))', - array_extent=('nv',), - ), StructFieldDecl( name='bvh_aabb_dyn', type=PointerType( diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 7ace198b..ec00ee87 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -310,7 +310,6 @@ def make_data( 'qM': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), 'qLD': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), 'qLDiagInv': (m.nv, float) if support.is_sparse(m) else (0, float), - 'qLDiagSqrtInv': (m.nv, float), 'bvh_aabb_dyn': (m.nbvhdynamic, 6, float), 'bvh_active': (m.nbvh, jp.uint8), 'flexedge_velocity': (m.nflexedge, float), diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index a97d0cb2..8d6a62d9 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -1274,7 +1274,6 @@ class Data(PyTreeNode): if dense: (nv, nv) qLDiagInv: 1/diag(D) if sparse: (nv,) if dense: (0,) - qLDiagSqrtInv: 1/sqrt(diag(D)) (nv,) bvh_aabb_dyn: global bounding box (center, size) (nbvhdynamic, 6) bvh_active: volume has been added to collisions (nbvh,) flexedge_velocity: flex edge velocities (nflexedge,) @@ -1404,7 +1403,6 @@ class Data(PyTreeNode): qM: jax.Array # pylint:disable=invalid-name qLD: jax.Array # pylint:disable=invalid-name qLDiagInv: jax.Array # pylint:disable=invalid-name - qLDiagSqrtInv: jax.Array # pylint:disable=invalid-name bvh_aabb_dyn: jax.Array = _restricted_to('mujoco') bvh_active: jax.Array = _restricted_to('mujoco') # position, velocity dependent: diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index e47f5a42..7fed04f7 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -234,7 +234,7 @@ PYBIND11_MODULE(_functions, pymodule) { DEF_WITH_OMITTED_PY_ARGS(traits::mj_solveM2, "n")( pymodule, [](const raw::MjModel* m, raw::MjData* d, Eigen::Ref x, - Eigen::Ref y) { + Eigen::Ref y, Eigen::Ref sqrtInvD) { if (x.rows() != y.rows()) { throw py::type_error( "the first dimension of x and y should be of the same size"); @@ -247,8 +247,12 @@ PYBIND11_MODULE(_functions, pymodule) { throw py::type_error( "the last dimension of y should be of size nv"); } + if (sqrtInvD.size() != m->nv) { + throw py::type_error( + "the size of sqrtInvD should be nv"); + } return InterceptMjErrors(::mj_solveM2)( - m, d, x.data(), y.data(), y.rows()); + m, d, x.data(), y.data(), sqrtInvD.data(), y.rows()); }); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 2b2b8a43..f39d2da2 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -379,7 +379,6 @@ X( mjtNum, , xaxis, njnt, 3 ) \ X( mjtNum, , cdof, nv, 6 ) \ X( mjtNum, , qLDiagInv, nv, 1 ) \ - X( mjtNum, , qLDiagSqrtInv, nv, 1 ) \ X( mjtNum, , cdof_dot, nv, 6 ) \ X( mjtNum, , qfrc_bias, nv, 1 ) \ X( mjtNum, , qfrc_passive, nv, 1 ) \ diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 8d380e9e..de206a59 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2067,6 +2067,12 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { mj_markStack(d); + // inverse square root of D from inertia LDL decomposition + mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum); + for (int i=0; i < nv; i++) { + sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]); + } + // space for backsubM2(J')' and its traspose mjtNum* JM2 = mjSTACKALLOC(d, nefc*nv, mjtNum); mjtNum* JM2T = mjSTACKALLOC(d, nv*nefc, mjtNum); @@ -2140,7 +2146,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // process if not zero if (xi) { // x(i) /= sqrt(L(i,i)) - JM2[adr+i] *= d->qLDiagSqrtInv[colind[adr+i]]; + JM2[adr+i] *= sqrtInvD[colind[adr+i]]; // x(j) -= L(i,j) * x(i) int Madr_ij = m->dof_Madr[colind[adr+i]]+1; @@ -2191,7 +2197,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // dense else { // JM2 = backsubM2(J')' - mj_solveM2(m, d, JM2, d->efc_J, nefc); + mj_solveM2(m, d, JM2, d->efc_J, sqrtInvD, nefc); // construct JM2T mju_transpose(JM2T, JM2, nefc, nv); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 869ea13e..045ec6da 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -774,7 +774,7 @@ void mj_tendon(const mjModel* m, mjData* d) { L[i] += (mju_dist3(wpnt, wpnt+3) + wlen + mju_dist3(wpnt+6, wpnt+9))/divisor; } - // accumulate moments if consequtive points are in different bodies + // accumulate moments if consecutive points are in different bodies for (int k=0; k < (wlen < 0 ? 1 : 3); k++) { if (wbody[k] != wbody[k+1]) { // get 3D position difference, normalize @@ -1387,8 +1387,7 @@ void mj_crb(const mjModel* m, mjData* d) { // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv, - mjtNum* qLDiagSqrtInv) { +void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv) { int cnt; int Madr_kk, Madr_ki; mjtNum tmp; @@ -1445,9 +1444,6 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu for (int i=0; i < nv; i++) { mjtNum qLDi = qLD[dof_Madr[i]]; qLDiagInv[i] = 1.0/qLDi; - if (qLDiagSqrtInv) { - qLDiagSqrtInv[i] = 1.0/mju_sqrt(qLDi); - } } } @@ -1456,7 +1452,7 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd void mj_factorM(const mjModel* m, mjData* d) { TM_START; - mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv, d->qLDiagSqrtInv); + mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv); TM_ADD(mjTIMER_POS_INERTIA); } @@ -1685,10 +1681,10 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int // half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y -void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { +void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, + const mjtNum* sqrtInvD, int n) { // local copies of key variables mjtNum* qLD = d->qLD; - mjtNum* qLDiagSqrtInv = d->qLDiagSqrtInv; int* dof_Madr = m->dof_Madr; int* dof_parentid = m->dof_parentid; int nv = m->nv; @@ -1720,7 +1716,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) // x <- sqrt(inv(D)) * x for (int i=0; i < nv; i++) { - x[i+offset] *= qLDiagSqrtInv[i]; // x(i) /= sqrt(L(i,i)) + x[i+offset] *= sqrtInvD[i]; // x(i) /= sqrt(L(i,i)) } } } @@ -1781,7 +1777,7 @@ void mj_comVel(const mjModel* m, mjData* d) { default: // in principle we should use the new velocity to compute cdofdot, - // but it makes no difference becase crossMotion(cdof, cdof) = 0, + // but it makes no difference because crossMotion(cdof, cdof) = 0, // and using the old velocity may be more accurate numerically mju_crossMotion(cdofdot+6*j, cvel, d->cdof+6*(bda+j)); diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index a1fab753..f33e09ff 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -49,8 +49,7 @@ MJAPI void mj_transmission(const mjModel* m, mjData* d); MJAPI void mj_crb(const mjModel* m, mjData* d); // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv, - mjtNum* qLDiagSqrtInv); +MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv); // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd MJAPI void mj_factorM(const mjModel* m, mjData* d); @@ -71,7 +70,8 @@ MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, in MJAPI void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* x, int island); // half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y -MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, + const mjtNum* sqrtInvD, int n); //-------------------------- velocity -------------------------------------------------------------- diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b9e1e6bf..08f33625 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -803,7 +803,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { } // factor - mj_factorI(m, d, MhB, d->qH, d->qHDiagInv, 0); + mj_factorI(m, d, MhB, d->qH, d->qHDiagInv); } // solve @@ -986,7 +986,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM); // factorize - mj_factorI(m, d, MhB, d->qH, d->qHDiagInv, NULL); + mj_factorI(m, d, MhB, d->qH, d->qHDiagInv); } // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 4de41fca..e6278806 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1107,7 +1107,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format); - printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format); // B sparse structure printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL, diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 94a03de4..a25f874e 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1087,7 +1087,6 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { int nv = m->nv; const mjtNum* qLD = d->qLD; - const mjtNum* qLDiagSqrtInv = d->qLDiagSqrtInv; const int* dofMadr = m->dof_Madr; mju_zero(res, nv); @@ -1117,9 +1116,9 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) } } - // res = sqrt(D) * res + // res *= sqrt(D) for (int i=0; i < nv; i++) { - res[i] /= qLDiagSqrtInv[i]; + res[i] *= mju_sqrt(qLD[dofMadr[i]]); } } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 2cb8c8a0..ec19a119 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -541,6 +541,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mjtNum* jac = mjSTACKALLOC(d, 3*nv, mjtNum); mjtNum* jacM2 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum); // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { @@ -554,8 +555,11 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mju_addTo3(selpos, d->xpos+3*sel); // compute average spatial inertia at selection point + for (int i=0; i < nv; i++) { + sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]); + } mj_jac(m, d, jac, NULL, selpos, sel); - mj_solveM2(m, d, jacM2, jac, 3); + mj_solveM2(m, d, jacM2, jac, sqrtInvD, 3); mjtNum invmass = mju_dot(jacM2+0*nv, jacM2+0*nv, nv) + mju_dot(jacM2+1*nv, jacM2+1*nv, nv) + mju_dot(jacM2+2*nv, jacM2+2*nv, nv); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 8dec655e..92fbd9e9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4917,7 +4917,6 @@ public unsafe struct mjData_ { public double* qM; public double* qLD; public double* qLDiagInv; - public double* qLDiagSqrtInv; public double* bvh_aabb_dyn; public byte* bvh_active; public double* flexedge_velocity; @@ -6679,7 +6678,7 @@ public static unsafe extern void mj_factorM(mjModel_* m, mjData_* d); public static unsafe extern void mj_solveM(mjModel_* m, mjData_* d, double* x, double* y, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mj_solveM2(mjModel_* m, mjData_* d, double* x, double* y, int n); +public static unsafe extern void mj_solveM2(mjModel_* m, mjData_* d, double* x, double* y, double* sqrtInvD, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_comVel(mjModel_* m, mjData_* d); From 4510c6d29031922e07567515b1e29ca3db486b6b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 2 Jan 2025 09:03:54 -0800 Subject: [PATCH 171/426] Further speedup to CSR back-substitution using `dof_simplenum`. PiperOrigin-RevId: 711440268 Change-Id: I81cd9a6a8b8ec78d08cfbc34f60a9216ea753833 --- src/engine/engine_core_smooth.c | 27 +++++--- src/engine/engine_core_smooth.h | 3 +- test/benchmark/CMakeLists.txt | 2 +- ...mark_test.cc => solveLD_benchmark_test.cc} | 3 +- test/engine/engine_core_smooth_test.cc | 65 ++++++++++++++++++- 5 files changed, 85 insertions(+), 15 deletions(-) rename test/benchmark/{engine_core_smooth_benchmark_test.cc => solveLD_benchmark_test.cc} (96%) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 045ec6da..d17d06db 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1575,15 +1575,19 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, - const int* rownnz, const int* rowadr, const int* diag, const int* colind) { + const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, + const int* colind) { // x <- L^-T x for (int i=nv-2; i >= 0; i--) { - int d1 = diag[i] + 1; - int nnz = rownnz[i] - d1; - if (nnz > 0) { - int adr = rowadr[i] + d1; - x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0); + // skip diagonal (simple) rows + if (diagnum[i]) { + continue; } + + int d1 = diagind[i] + 1; + int nnz = rownnz[i] - d1; + int adr = rowadr[i] + d1; + x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0); } // x(i) /= D(i,i) @@ -1593,11 +1597,14 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv // x <- L^-1 x for (int i=1; i < nv; i++) { - int d = diag[i]; - if (d > 0) { - int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); + // skip diagonal (simple) rows + if (diagnum[i]) { + i += diagnum[i] - 1; // when iterating forward we can skip ahead + continue; } + + int adr = rowadr[i]; + x[i] -= mju_dotSparse(qLDs+adr, x, diagind[i], colind+adr, /*flg_unc1=*/0); } } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index f33e09ff..db1af84b 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -61,7 +61,8 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, - const int* rownnz, const int* rowadr, const int* diag, const int* colind); + const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, + const int* colind); // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 658e0639..0407edc0 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -44,7 +44,7 @@ mujoco_test( ) mujoco_test( - engine_core_smooth_benchmark_test + solveLD_benchmark_test MAIN_TARGET benchmark::benchmark_main ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc similarity index 96% rename from test/benchmark/engine_core_smooth_benchmark_test.cc rename to test/benchmark/solveLD_benchmark_test.cc index 3b728169..5564fc78 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -64,7 +64,8 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { } else { mju_copy(res, vec, m->nv); mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, - d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind); + d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, + d->C_colind); } } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index a6d2d0b3..dd406caa 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -456,7 +456,54 @@ TEST_F(CoreSmoothTest, FactorI) { mj_deleteModel(model); } -TEST_F(CoreSmoothTest, SolveLD2) { +// in-place sparse backsubstitution: x = inv(L'*D*L)*x +// like mj_solveLD, but using the CSR representation of L +// variant that only uses the lower triangle of qLDs +static void mj_solveLDsLower(mjtNum* x, const mjtNum* qLDs, + const mjtNum* qLDiagInv, int nv, const int* rownnz, + const int* rowadr, const int* diagind, + const int* diagnum, const int* colind, + int* scratch) { + int* marker = scratch; + for (int i=1; i < nv; i++) { + marker[i] = rowadr[i] + diagind[i] - 1; + } + + // x <- L^-T x + for (int i=nv-2; i >= 0; i--) { + // skip diagonal (simple) rows + if (diagnum[i]) { + continue; + } + + for (int j=i+1; j < nv; j++) { + if (colind[marker[j]] == i) { + x[i] -= qLDs[marker[j]--] * x[j]; + } + } + } + + // x(i) /= D(i,i) + for (int i=0; i < nv; i++) { + x[i] *= qLDiagInv[i]; + } + + // x <- L^-1 x + for (int i=1; i < nv; i++) { + // skip diagonal (simple) rows + if (diagnum[i]) { + i += diagnum[i] - 1; // when iterating forward we can skip ahead + continue; + } + + int d = diagind[i]; + int adr = rowadr[i]; + x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); + } +} + + +TEST_F(CoreSmoothTest, SolveLDs) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); @@ -490,9 +537,23 @@ TEST_F(CoreSmoothTest, SolveLD2) { for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i; for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; + // use upper triangle mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, - d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind); + d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, + d->C_colind); + + // expect vectors to match up to floating point precision + for (int i=0; i < nv; i++) { + EXPECT_FLOAT_EQ(vec[i], vec2[i]); + } + + // don't use use upper triangle + mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); + vector scratch(nv); + mj_solveLDsLower(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz, + d->C_rowadr, d->C_diag, m->dof_simplenum, d->C_colind, + scratch.data()); // expect vectors to match up to floating point precision for (int i=0; i < nv; i++) { From 6c880eb0fa797f8e6b2695902af122e6e864b869 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 2 Jan 2025 12:00:53 -0800 Subject: [PATCH 172/426] Clean up `mju_sqrMatTDSparse` PiperOrigin-RevId: 711486739 Change-Id: I5f9145dcd8522a9cb372cbdf60611d7d05cb524e --- src/engine/engine_util_sparse.c | 54 +++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 20 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 895179e2..2c9fd721 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -742,7 +742,8 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, int* markers = mjSTACKALLOC(d, nc, int); for (int i=0; i < nc; i++) { - int* cols = res_colind+res_rowadr[i]; + int rowadr_i = res_rowadr[i]; + int* cols = res_colind + rowadr_i; res_rownnz[i] = 0; buffer[i] = 0; @@ -755,18 +756,26 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } // iterate through each row of M' - int end = rowadrT[i] + rownnzT[i]; - for (int r = rowadrT[i]; r < end; r++) { + int adrT = rowadrT[i]; + int end_r = adrT + rownnzT[i]; + for (int r = adrT; r < end_r; r++) { int t = colindT[r]; - mjtNum v = diag ? matT[r] * diag[t] : matT[r]; - for (int c=rowadr[t]; c < rowadr[t]+rownnz[t]; c++) { + int adr = rowadr[t]; + int end_c = adr + rownnz[t]; + for (int c=adr; c < end_c; c++) { int cc = colind[c]; + // ignore upper triangle if (cc > i) { break; } - buffer[cc] += v*mat[c]; + // add value to buffer + if (diag) { + buffer[cc] += matT[r] * diag[t] * mat[c]; + } else { + buffer[cc] += matT[r] * mat[c]; + } // only need to insert nnz if not marked if (!markers[cc]) { @@ -810,22 +819,26 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } } - end = res_rownnz[i]; + end_r = res_rownnz[i]; // rowsuperT: reuse sparsity, copy into res if (rowsuperT && rowsuperT[i]) { - for (int r=0; r < end; r++) { - res[res_rowadr[i] + r] = buffer[cols[r]]; - buffer[cols[r]] = 0; + for (int r=0; r < end_r; r++) { + int c = cols[r]; + res[rowadr_i + r] = buffer[c]; + buffer[c] = 0; } - } else { - // clear out buffers since sparsity cannot be reused - for (int r=0; r < end; r++) { - int cc = cols[r]; - res[res_rowadr[i] + r] = buffer[cc]; - res_colind[res_rowadr[i] + r] = cc; - buffer[cc] = 0; - markers[cc] = 0; + } + + // clear out buffers, sparsity cannot be reused + else { + for (int r=0; r < end_r; r++) { + int c = cols[r]; + int adr = rowadr_i + r; + res[adr] = buffer[c]; + res_colind[adr] = c; + buffer[c] = 0; + markers[c] = 0; } } } @@ -833,8 +846,9 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, // fill upper triangle for (int i=0; i < nc; i++) { - int end = res_rowadr[i] + res_rownnz[i] - 1; - for (int j=res_rowadr[i]; j < end; j++) { + int start = res_rowadr[i]; + int end = start + res_rownnz[i] - 1; + for (int j=start; j < end; j++) { int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++; res[adr] = res[j]; res_colind[adr] = i; From 923f75fd2368f74e65c5050ea6f0321dcd7dc364 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 3 Jan 2025 02:32:54 -0800 Subject: [PATCH 173/426] Use only lower triangle in CSR back-substitution. PiperOrigin-RevId: 711686157 Change-Id: I4fc98cdfb927e5608ce3a99ea34890ce556fcfd7 --- src/engine/engine_core_smooth.c | 17 +++++--- test/engine/engine_core_smooth_test.cc | 60 -------------------------- 2 files changed, 10 insertions(+), 67 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d17d06db..641b7bdc 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1578,16 +1578,19 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, const int* colind) { // x <- L^-T x - for (int i=nv-2; i >= 0; i--) { - // skip diagonal (simple) rows - if (diagnum[i]) { + for (int i=nv-1; i > 0; i--) { + // skip diagonal (simple) rows, exploit sparsity of input vector + if (diagnum[i] || x[i] == 0) { continue; } - int d1 = diagind[i] + 1; - int nnz = rownnz[i] - d1; - int adr = rowadr[i] + d1; - x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0); + int d = diagind[i]; + int adr_i = rowadr[i]; + mjtNum x_i = x[i]; + for (int j=0; j < d; j++) { + int adr = adr_i + j; + x[colind[adr]] -= qLDs[adr] * x_i; + } } // x(i) /= D(i,i) diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index dd406caa..1fa16ec2 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -456,53 +456,6 @@ TEST_F(CoreSmoothTest, FactorI) { mj_deleteModel(model); } -// in-place sparse backsubstitution: x = inv(L'*D*L)*x -// like mj_solveLD, but using the CSR representation of L -// variant that only uses the lower triangle of qLDs -static void mj_solveLDsLower(mjtNum* x, const mjtNum* qLDs, - const mjtNum* qLDiagInv, int nv, const int* rownnz, - const int* rowadr, const int* diagind, - const int* diagnum, const int* colind, - int* scratch) { - int* marker = scratch; - for (int i=1; i < nv; i++) { - marker[i] = rowadr[i] + diagind[i] - 1; - } - - // x <- L^-T x - for (int i=nv-2; i >= 0; i--) { - // skip diagonal (simple) rows - if (diagnum[i]) { - continue; - } - - for (int j=i+1; j < nv; j++) { - if (colind[marker[j]] == i) { - x[i] -= qLDs[marker[j]--] * x[j]; - } - } - } - - // x(i) /= D(i,i) - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; - } - - // x <- L^-1 x - for (int i=1; i < nv; i++) { - // skip diagonal (simple) rows - if (diagnum[i]) { - i += diagnum[i] - 1; // when iterating forward we can skip ahead - continue; - } - - int d = diagind[i]; - int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); - } -} - - TEST_F(CoreSmoothTest, SolveLDs) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; @@ -537,7 +490,6 @@ TEST_F(CoreSmoothTest, SolveLDs) { for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i; for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; - // use upper triangle mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, @@ -548,18 +500,6 @@ TEST_F(CoreSmoothTest, SolveLDs) { EXPECT_FLOAT_EQ(vec[i], vec2[i]); } - // don't use use upper triangle - mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); - vector scratch(nv); - mj_solveLDsLower(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz, - d->C_rowadr, d->C_diag, m->dof_simplenum, d->C_colind, - scratch.data()); - - // expect vectors to match up to floating point precision - for (int i=0; i < nv; i++) { - EXPECT_FLOAT_EQ(vec[i], vec2[i]); - } - mj_deleteData(d); mj_deleteModel(m); } From 63e2836f6382007bff109f56ead2110ddb7045a4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 3 Jan 2025 08:50:48 -0800 Subject: [PATCH 174/426] Expose `mj_printSparsity` in `engine_print.h`. This is useful for debugging sparse operations. PiperOrigin-RevId: 711765082 Change-Id: I532a5218da0f921dd4b04730d0b12c5136e7ca65 --- src/engine/engine_print.c | 37 +++++++++++++++++++------------------ src/engine/engine_print.h | 6 ++++++ 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index e6278806..6cba6352 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -114,8 +114,8 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, // print sparse matrix structure -static void printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, - const int* rownnz, const int* rowsuper, const int* colind, FILE* fp) { +void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, + const int* rownnz, const int* rowsuper, const int* colind, FILE* fp) { // if no rows / columns, or too many columns to be visually useful, return if (!nr || !nc || nc > 300) { return; @@ -1060,8 +1060,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format); } else { - printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv, - d->flexedge_J_rowadr, NULL, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind, fp); + mj_printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv, + d->flexedge_J_rowadr, NULL, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind, + fp); printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp); printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp); printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz, @@ -1073,8 +1074,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); } else { - printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, - d->ten_J_rownnz, NULL, d->ten_J_colind, fp); + mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL, + d->ten_J_rownnz, NULL, d->ten_J_colind, fp); printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, @@ -1090,8 +1091,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); - printSparsity("actuator_moment", m->nu, m->nv, - d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp); + mj_printSparsity("actuator_moment", m->nu, m->nv, + d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp); printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz, d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); @@ -1109,8 +1110,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format); // B sparse structure - printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL, - d->B_colind, fp); + mj_printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL, + d->B_colind, fp); // B_rownnz fprintf(fp, NAME_FORMAT, "B_rownnz"); @@ -1134,8 +1135,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // C sparse structure - printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_diag, d->C_rownnz, - NULL, d->C_colind, fp); + mj_printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_diag, d->C_rownnz, + NULL, d->C_colind, fp); fprintf(fp, NAME_FORMAT, "C_rownnz"); for (int i = 0; i < m->nv; i++) { @@ -1165,8 +1166,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // D sparse structure - printSparsity("D: dof-dof matrix", m->nv, m->nv, - d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp); + mj_printSparsity("D: dof-dof matrix", m->nv, m->nv, + d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp); // D_rownnz fprintf(fp, NAME_FORMAT, "D_rownnz"); @@ -1263,14 +1264,14 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); printArray("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); } else { - printSparsity("J: constraint Jacobian", d->nefc, m->nv, - d->efc_J_rowadr, NULL, d->efc_J_rownnz, d->efc_J_rowsuper, d->efc_J_colind, fp); + mj_printSparsity("J: constraint Jacobian", d->nefc, m->nv, d->efc_J_rowadr, NULL, + d->efc_J_rownnz, d->efc_J_rowsuper, d->efc_J_colind, fp); printArrayInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp); printArrayInt("EFC_J_ROWADR", d->nefc, 1, d->efc_J_rowadr, fp); printSparse("EFC_J", d->efc_J, d->nefc, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, fp, float_format); - printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, NULL, - d->efc_JT_rownnz, d->efc_JT_rowsuper, d->efc_JT_colind, fp); + mj_printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, NULL, + d->efc_JT_rownnz, d->efc_JT_rowsuper, d->efc_JT_colind, fp); printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz, diff --git a/src/engine/engine_print.h b/src/engine/engine_print.h index b6f9fb0c..60a0ac17 100644 --- a/src/engine/engine_print.h +++ b/src/engine/engine_print.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_PRINT_H_ #define MUJOCO_SRC_ENGINE_ENGINE_PRINT_H_ +#include + #include #include #include @@ -40,6 +42,10 @@ MJAPI void mj_printFormattedData(const mjModel* m, mjData* d, const char* filena // print data to text file MJAPI void mj_printData(const mjModel* m, mjData* d, const char* filename); +// print sparse matrix structure +MJAPI void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, + const int* rownnz, const int* rowsuper, const int* colind, FILE* fp); + #ifdef __cplusplus } #endif From 340c0d7c971e3862d2f3ff825f953cdb1486c7b1 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 3 Jan 2025 09:21:24 -0800 Subject: [PATCH 175/426] Fix a box-box bad collision in NativeCCD. PiperOrigin-RevId: 711773153 Change-Id: I8a526ef5933be19d863e67f40edeb95b0b1249cd --- src/engine/engine_collision_gjk.c | 24 +++++++++++++-------- src/engine/engine_collision_gjk.h | 27 ++++++++++++++++++------ test/engine/engine_collision_gjk_test.cc | 27 ++++++++++++++++++++++++ 3 files changed, 62 insertions(+), 16 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index b97d67a4..0a4c86ff 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -890,7 +890,12 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj if (mju_abs(det3(v1, v3, v4)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v3, v4)) < mjMINVAL || mju_abs(det3(v2, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v4, v5)) < mjMINVAL) { - return 2; + return mjEPA_P2_INVALID_FACES; + } + + // check that origin is in the hexahedron + if (!testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) { + return mjEPA_P2_MISSING_ORIGIN; } // save vertices and get indices for each one @@ -914,7 +919,7 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj pt->map[i] = pt->faces + i; pt->faces[i].index = i; if (pt->faces[i].dist < mjMINVAL) { - return 3; + return mjEPA_P2_ORIGIN_ON_FACE; } } pt->nmap = 6; @@ -1003,7 +1008,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj cross3(n, diff1, diff2); mjtNum n_norm = mju_norm3(n); if (n_norm < mjMINVAL) { - return 4; + return mjEPA_P3_BAD_NORMAL; } // negative of triangle normal n @@ -1016,7 +1021,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // check that v4 is not contained in the 2-simplex if (triPointIntersect(v1, v2, v3, v4)) { - return 5; + return mjEPA_P3_INVALID_V4; } // get 5th vertex in -n direction @@ -1026,7 +1031,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // check that v5 is not contained in the 2-simplex if (triPointIntersect(v1, v2, v3, v5)) { - return 6; + return mjEPA_P3_INVALID_V5; } // if origin does not lie on simplex then we need to check that the hexahedron contains the @@ -1036,7 +1041,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // it but within tolerance from it. In that case the hexahedron could possibly be constructed // that doesn't contain the origin, but nonetheless there is penetration depth. if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { - return 7; + return mjEPA_P3_MISSING_ORIGIN; } // save vertices and get indices for each one @@ -1061,7 +1066,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj pt->map[i] = pt->faces + i; pt->faces[i].index = i; if (pt->faces[i].dist < mjMINVAL) { - return 8; + return mjEPA_P3_ORIGIN_ON_FACE; } } pt->nmap = 6; @@ -1444,7 +1449,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); status->gjk_iterations = 0; - status->epa_iterations = -1; + status->epa_iterations = 0; + status->epa_status = mjEPA_NOCONTACT; status->tolerance = config->tolerance; status->max_iterations = config->max_iterations; status->max_contacts = config->max_contacts; @@ -1551,12 +1557,12 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m } else { ret = polytope4(&pt, status, obj1, obj2); } + status->epa_status = ret; // simplex not on boundary (objects are penetrating) if (!ret) { dist = -epa(status, &pt, obj1, obj2); } else { - status->epa_iterations = -ret; dist = 0; } mj_freeStack(d); diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index f04ca77a..5c0601c6 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -25,17 +25,30 @@ extern "C" { #endif +// Status of an EPA run +typedef enum { + mjEPA_NOCONTACT = -1, + mjEPA_SUCCESS = 0, + mjEPA_P2_INVALID_FACES, + mjEPA_P2_MISSING_ORIGIN, + mjEPA_P2_ORIGIN_ON_FACE, + mjEPA_P3_BAD_NORMAL, + mjEPA_P3_INVALID_V4, + mjEPA_P3_INVALID_V5, + mjEPA_P3_MISSING_ORIGIN, + mjEPA_P3_ORIGIN_ON_FACE, +} mjEPAStatus; + // configuration for convex collision detection -struct _mjCCDConfig { +typedef struct { int max_iterations; // the maximum number of iterations for GJK and EPA mjtNum tolerance; // tolerance used by GJK and EPA int max_contacts; // set to max number of contact points to recover mjtNum dist_cutoff; // set to max geom distance to recover -}; -typedef struct _mjCCDConfig mjCCDConfig; +} mjCCDConfig; // data produced from running GJK and EPA -struct _mjCCDStatus { +typedef struct { // geom distance information mjtNum dist; // distance between geoms mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1 @@ -50,13 +63,13 @@ struct _mjCCDStatus { // statistics for debugging purposes int gjk_iterations; // number of iterations that GJK ran - int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run) + int epa_iterations; // number of iterations that EPA ran (zero if EPA did not run) + mjEPAStatus epa_status; // status of the EPA run mjtNum simplex1[12]; // the simplex that GJK returned for obj1 mjtNum simplex2[12]; // the simplex that GJK returned for obj2 mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference int nsimplex; // size of simplex 1 & 2 -}; -typedef struct _mjCCDStatus mjCCDStatus; +} mjCCDStatus; // run general convex collision detection, returns positive for distance, negative for penetration MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 241575c6..b2b8f440 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -335,6 +335,33 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxTouching) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dir[3], pos[3]; + mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + + EXPECT_EQ(dist, mjMAXVAL); + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"( From abe8ffdee7dc4c843b79843918804aac601c332f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 3 Jan 2025 14:41:08 -0800 Subject: [PATCH 176/426] Enable AVX for inlined `mju_dotSparse`. PiperOrigin-RevId: 711858423 Change-Id: Id2d06a9e87085e9f810779fba4e8ed9c19a4cba9 --- src/engine/engine_util_sparse.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 48b41979..f1f46f4a 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -18,6 +18,7 @@ #include #include #include +#include "engine/engine_util_sparse_avx.h" // IWYU pragma: keep #ifdef __cplusplus extern "C" { From f8843166136046e49126a376a82c2e9cad730ddc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Sat, 4 Jan 2025 11:18:53 +0000 Subject: [PATCH 177/426] Add links to the engine plugins and warning about repo versions --- doc/unity.rst | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/doc/unity.rst b/doc/unity.rst index ffa72b4b..3a282555 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -21,7 +21,10 @@ Installation instructions The plug-in directory (available at https://github.com/google-deepmind/mujoco/tree/main/unity) includes a ``package.json`` file. Unity's package manager recognizes this file and will import the plug-in's C# codebase to your project. In addition, Unity also needs the native MuJoCo library, which can be found in the specific platform archive at -https://github.com/google-deepmind/mujoco/releases. +https://github.com/google-deepmind/mujoco/releases. If you wish to simply use the plug-in and not develop it, you should +use one of the version-specific stable commits of the repository, identified by git tags. Check out the relevant version +of the cloned repository with git (``git checkout 3.X.Y`` where X and Y specify the engine version). Simply using the +``main`` branch of the repository may not be compatible with the most recent release binary of MuJoCo. On Unity version 2020.2 and later, the Package Manager will look for the native library file and copy it to the package directory when the package is imported. Alternatively, you can manually copy the native library to the package directory @@ -334,8 +337,8 @@ from the terrain is dynamically kept in sync with the simulation. MuJoCo plugins ______________ -The current version of the Unity package does not support loading MJCF scenes that use MuJoCo plugins such as -``elasticity``. Adding basic functionality to do this will be part of an upcoming release. +The current version of the Unity package does not support loading MJCF scenes that use :ref:`MuJoCo plugins` such as +`elasticity/ `__ . Adding basic functionality to do this will be part of an upcoming release. Interaction with External Processes ___________________________________ From 364ab5947d5d3c1e3d5639652ff2b05525e9dbd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Sat, 4 Jan 2025 11:21:54 +0000 Subject: [PATCH 178/426] Fix typo --- doc/unity.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/unity.rst b/doc/unity.rst index 3a282555..1264ea64 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -338,7 +338,7 @@ MuJoCo plugins ______________ The current version of the Unity package does not support loading MJCF scenes that use :ref:`MuJoCo plugins` such as -`elasticity/ `__ . Adding basic functionality to do this will be part of an upcoming release. +`elasticity `__ . Adding basic functionality to do this will be part of an upcoming release. Interaction with External Processes ___________________________________ From 94230a8bee4ec4af78f320ea70e5e8fee9be9495 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 5 Jan 2025 09:11:40 -0800 Subject: [PATCH 179/426] Fix bad indent. PiperOrigin-RevId: 712267512 Change-Id: I3463b0f53b808b726ff0fe5ea46ec8d30164eac4 --- src/engine/engine_vis_visualize.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 2764bee7..d0da232a 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1837,7 +1837,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int j=0; j < npoints-1; j++) { START - sz[0] = m->tendon_width[i]; + sz[0] = m->tendon_width[i]; // construct geom mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3); From daed8f46c8f243c5bca245963c09fa9f91d293be Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Jan 2025 03:50:44 -0800 Subject: [PATCH 180/426] Don't assume compressed input in `mju_transposeSparse`, allow NULL pointer for matrix values, to only transpose the structure. PiperOrigin-RevId: 712472432 Change-Id: I6dd34583ab181f0c9677c55230cecdf0aed46f9e --- src/engine/engine_util_sparse.c | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 2c9fd721..80c3f256 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -552,12 +552,13 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, // clear number of non-zeros for each row of transposed mju_zeroInt(res_rownnz, nc); - // total number of non-zeros of mat - int nnz = rowadr[nr-1] + rownnz[nr-1]; - // count the number of non-zeros for each row of the transposed matrix - for (int i = 0; i < nnz; i++) { - res_rownnz[colind[i]]++; + for (int r = 0; r < nr; r++) { + int start = rowadr[r]; + int end = start + rownnz[r]; + for (int j = start; j < end; j++) { + res_rownnz[colind[j]]++; + } } // compute the row addresses for the transposed matrix @@ -566,18 +567,18 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, res_rowadr[i] = res_rowadr[i-1] + res_rownnz[i-1]; } - // r holds the current row in mat - int r = 0; - // iterate through each non-zero entry of mat - for (int i = 0; i < nnz; i++) { - // iterate to get to the current row (skipping rows with all zeros) - while ((i-rowadr[r]) >= rownnz[r]) r++; - - // swap rows with columns and increment res_rowadr - int c = res_rowadr[colind[i]]++; - res[c] = mat[i]; - res_colind[c] = r; + for (int r = 0; r < nr; r++) { + int start = rowadr[r]; + int end = start + rownnz[r]; + for (int i = start; i < end; i++) { + // swap rows with columns and increment res_rowadr + int c = res_rowadr[colind[i]]++; + res_colind[c] = r; + if (res) { + res[c] = mat[i]; + } + } } // shift back row addresses From 65048d6b8ffaaeb74968622bba60ef89410e2f56 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 6 Jan 2025 04:16:56 -0800 Subject: [PATCH 181/426] Do not use fast lookup for frames. Fast lookup is not possible since mjOBJ_FRAME > mjNOBJECT, which is the size of the objects maps used for the search. Fixes #2328. PiperOrigin-RevId: 712479460 Change-Id: I7bcfa75ff8a1e288183d60ed4a44cab5bc6f4173 --- src/user/user_api.cc | 2 +- test/user/user_api_test.cc | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index af465e75..af7cd9b5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -614,7 +614,7 @@ mjsBody* mjs_findBody(mjSpec* s, const char* name) { // find element in spec by name mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name) { mjCModel* model = static_cast(s->element); - if (model->IsCompiled()) { + if (model->IsCompiled() && type != mjOBJ_FRAME) { return model->FindObject(type, std::string(name)); // fast lookup } switch (type) { diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 67b7282d..c6620c1c 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1014,6 +1014,9 @@ TEST_F(MujocoTest, AttachDifferent) { mjModel* m_attached = mj_compile(parent, 0); EXPECT_THAT(m_attached, NotNull()); + // check frame is present + EXPECT_THAT(mjs_findFrame(parent, "frame"), NotNull()); + // check full name stored in mjModel EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1"); From ee6f4837f3d400a077d67fe048637b0e1d0bda76 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 6 Jan 2025 04:34:49 -0800 Subject: [PATCH 182/426] Auto-generate 3D mesh from convex OBJ if dim=3 in flex. This requires the user to explicitly specify an origin. PiperOrigin-RevId: 712483526 Change-Id: Ie316a5eccb676bb918d4bb746795019145c10f5b --- doc/XMLreference.rst | 7 + doc/XMLschema.rst | 2 + model/flex/asset/cap.obj | 617 +++++++++++++++++++++++++++++++++++ model/flex/gripper.xml | 66 ++++ src/user/user_flexcomp.cc | 37 ++- src/user/user_flexcomp.h | 1 + src/xml/xml_native_reader.cc | 8 +- test/fixture.cc | 31 +- 8 files changed, 747 insertions(+), 22 deletions(-) create mode 100644 model/flex/asset/cap.obj create mode 100644 model/flex/gripper.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a903b526..5d290e6b 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3632,6 +3632,13 @@ saving the XML: These attributes are directly passed through to the automatically-generated :ref:`flex` object and have the same meaning. +.. _body-flexcomp-origin: + +:at:`origin`: :at-val:`real(3), "0 0 0"` + The origin of the flexcomp. Used for generating a volumetric mesh from an OBJ surface mesh. Each surface triangle is + connected to the origin to create a tetrahedron, so the resulting volumetric mesh is guaranteed to be well-formed + only for convex shapes. + .. _flexcomp-contact: :el-prefix:`flexcomp/` |-| **contact** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index caf07b20..2d1bc9bd 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -445,6 +445,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | :ref:`euler` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`origin` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flexcomp |br| |_2| |L| | | .. table:: | | :ref:`edge | ? | :class: mjcf-attributes | diff --git a/model/flex/asset/cap.obj b/model/flex/asset/cap.obj new file mode 100644 index 00000000..05cce780 --- /dev/null +++ b/model/flex/asset/cap.obj @@ -0,0 +1,617 @@ +#### +# +# OBJ File Generated by Meshlab +# +#### +# Object cap.obj +# +# Vertices: 217 +# Faces: 384 +# +#### +v -0.00000000 0.00000000 0.07411810 +v 0.09659259 0.00000000 -0.00000001 +v 0.04829629 0.08365163 0.00000001 +v -0.04829629 0.08365163 0.00000001 +v -0.09659259 0.00000000 -0.00000001 +v -0.04829629 -0.08365163 0.00000001 +v 0.04829629 -0.08365163 0.00000001 +v 0.05341240 -0.00000000 0.05865872 +v 0.08365164 0.04829629 -0.00000001 +v 0.02670619 0.04625649 0.05865872 +v 0.00000000 0.09659259 -0.00000001 +v -0.02670619 0.04625649 0.05865872 +v -0.08365164 0.04829629 -0.00000001 +v -0.05341240 0.00000000 0.05865872 +v -0.08365164 -0.04829629 -0.00000001 +v -0.02670619 -0.04625649 0.05865872 +v 0.00000000 -0.09659259 -0.00000001 +v 0.02670619 -0.04625649 0.05865872 +v 0.08365164 -0.04829629 -0.00000001 +v 0.06724097 0.02381060 0.04420168 +v 0.05424107 0.04632709 0.04420168 +v 0.04028573 0.02325897 0.06263974 +v 0.01299990 0.07013769 0.04420167 +v -0.01299990 0.07013769 0.04420168 +v -0.00000000 0.04651795 0.06263975 +v -0.05424108 0.04632709 0.04420166 +v -0.06724098 0.02381060 0.04420166 +v -0.04028573 0.02325897 0.06263974 +v -0.06724097 -0.02381060 0.04420168 +v -0.05424107 -0.04632709 0.04420168 +v -0.04028573 -0.02325897 0.06263974 +v -0.01299990 -0.07013769 0.04420167 +v 0.01299990 -0.07013769 0.04420168 +v 0.00000000 -0.04651795 0.06263975 +v 0.05424108 -0.04632709 0.04420166 +v 0.06724098 -0.02381060 0.04420166 +v 0.04028573 -0.02325897 0.06263974 +v 0.02678733 -0.00000000 0.07046351 +v 0.01339367 0.02319851 0.07046351 +v 0.07868688 -0.00000000 0.03583015 +v 0.09330128 0.02500000 -0.00000001 +v 0.06830128 0.06830128 -0.00000004 +v 0.03934343 0.06814483 0.03583015 +v -0.01339367 0.02319851 0.07046351 +v 0.02500000 0.09330128 -0.00000001 +v -0.02500000 0.09330128 -0.00000001 +v -0.03934344 0.06814483 0.03583015 +v -0.02678734 0.00000000 0.07046351 +v -0.06830128 0.06830128 -0.00000004 +v -0.09330128 0.02500000 -0.00000001 +v -0.07868689 0.00000000 0.03583013 +v -0.01339367 -0.02319851 0.07046351 +v -0.09330128 -0.02500000 -0.00000001 +v -0.06830128 -0.06830128 -0.00000004 +v -0.03934343 -0.06814483 0.03583015 +v 0.01339367 -0.02319851 0.07046351 +v -0.02500000 -0.09330128 -0.00000001 +v 0.02500000 -0.09330128 -0.00000001 +v 0.03934344 -0.06814483 0.03583015 +v 0.06830128 -0.06830128 -0.00000004 +v 0.09330128 -0.02500000 -0.00000001 +v 0.06102579 0.03523326 0.04507211 +v 0.04719808 0.03485627 0.05509577 +v 0.05378547 0.02344661 0.05509576 +v -0.00000000 0.07046651 0.04507211 +v -0.00658737 0.05830287 0.05509577 +v 0.00658736 0.05830287 0.05509577 +v -0.06102579 0.03523326 0.04507211 +v -0.05378547 0.02344662 0.05509576 +v -0.04719809 0.03485627 0.05509576 +v -0.06102579 -0.03523326 0.04507211 +v -0.04719808 -0.03485627 0.05509577 +v -0.05378547 -0.02344661 0.05509576 +v 0.00000000 -0.07046651 0.04507211 +v 0.00658737 -0.05830287 0.05509577 +v -0.00658736 -0.05830287 0.05509577 +v 0.06102579 -0.03523326 0.04507211 +v 0.05378547 -0.02344662 0.05509576 +v 0.04719809 -0.03485627 0.05509576 +v 0.03349884 0.01162469 0.06762048 +v 0.02681670 0.02319850 0.06762048 +v 0.02009635 0.01160263 0.07138842 +v 0.08512369 0.01233582 0.02512579 +v 0.08049080 0.02435185 0.02823111 +v 0.07300231 0.01205527 0.04138871 +v 0.06133472 0.05753115 0.02823112 +v 0.05324498 0.06755137 0.02512579 +v 0.04694132 0.05719421 0.04138872 +v 0.00668214 0.03482319 0.06762048 +v -0.00668214 0.03482319 0.06762048 +v -0.00000000 0.02320527 0.07138842 +v 0.03187871 0.07988720 0.02512577 +v 0.01915608 0.08188300 0.02823112 +v 0.02606098 0.06924948 0.04138871 +v -0.01915608 0.08188300 0.02823112 +v -0.03187871 0.07988719 0.02512579 +v -0.02606099 0.06924949 0.04138871 +v -0.02681670 0.02319850 0.06762048 +v -0.03349884 0.01162470 0.06762048 +v -0.02009636 0.01160263 0.07138842 +v -0.05324498 0.06755137 0.02512577 +v -0.06133472 0.05753115 0.02823112 +v -0.04694132 0.05719422 0.04138871 +v -0.08049081 0.02435185 0.02823111 +v -0.08512370 0.01233582 0.02512577 +v -0.07300231 0.01205527 0.04138870 +v -0.03349884 -0.01162469 0.06762048 +v -0.02681670 -0.02319850 0.06762048 +v -0.02009635 -0.01160263 0.07138842 +v -0.08512370 -0.01233582 0.02512577 +v -0.08049080 -0.02435185 0.02823111 +v -0.07300231 -0.01205527 0.04138871 +v -0.06133472 -0.05753115 0.02823112 +v -0.05324498 -0.06755137 0.02512579 +v -0.04694132 -0.05719421 0.04138872 +v -0.00668214 -0.03482319 0.06762048 +v 0.00668214 -0.03482319 0.06762048 +v 0.00000000 -0.02320527 0.07138842 +v -0.03187871 -0.07988720 0.02512577 +v -0.01915608 -0.08188300 0.02823112 +v -0.02606098 -0.06924948 0.04138871 +v 0.01915608 -0.08188300 0.02823112 +v 0.03187871 -0.07988719 0.02512579 +v 0.02606099 -0.06924949 0.04138871 +v 0.02681670 -0.02319850 0.06762048 +v 0.03349884 -0.01162470 0.06762048 +v 0.02009636 -0.01160263 0.07138842 +v 0.05324498 -0.06755137 0.02512577 +v 0.06133472 -0.05753115 0.02823112 +v 0.04694132 -0.05719422 0.04138871 +v 0.08049081 -0.02435185 0.02823111 +v 0.08512369 -0.01233582 0.02512579 +v 0.07300231 -0.01205527 0.04138871 +v 0.06011157 0.01183397 0.05315326 +v 0.04687075 0.01169339 0.06167607 +v 0.07504904 0.03594581 0.02957566 +v 0.06865450 0.04702147 0.02957566 +v 0.04030430 0.04614115 0.05315326 +v 0.03356215 0.03474456 0.06167607 +v 0.01980726 0.05797512 0.05315328 +v 0.01330860 0.04643795 0.06167607 +v 0.00639453 0.08296728 0.02957566 +v -0.00639453 0.08296728 0.02957566 +v -0.01980726 0.05797513 0.05315326 +v -0.01330860 0.04643795 0.06167607 +v -0.04030430 0.04614115 0.05315326 +v -0.03356215 0.03474456 0.06167607 +v -0.06865451 0.04702147 0.02957564 +v -0.07504904 0.03594581 0.02957565 +v -0.06011157 0.01183397 0.05315326 +v -0.04687076 0.01169339 0.06167606 +v -0.06011157 -0.01183397 0.05315326 +v -0.04687075 -0.01169339 0.06167607 +v -0.07504904 -0.03594581 0.02957566 +v -0.06865450 -0.04702147 0.02957566 +v -0.04030430 -0.04614115 0.05315326 +v -0.03356215 -0.03474456 0.06167607 +v -0.01980726 -0.05797512 0.05315328 +v -0.01330860 -0.04643795 0.06167607 +v -0.00639453 -0.08296728 0.02957566 +v 0.00639453 -0.08296728 0.02957566 +v 0.01980726 -0.05797513 0.05315326 +v 0.01330860 -0.04643795 0.06167607 +v 0.04030430 -0.04614115 0.05315326 +v 0.03356215 -0.03474456 0.06167607 +v 0.06865451 -0.04702147 0.02957564 +v 0.07504904 -0.03594581 0.02957565 +v 0.06011157 -0.01183397 0.05315326 +v 0.04687075 -0.01169339 0.06167607 +v 0.01339461 -0.00000000 0.07321696 +v 0.00669730 0.01160007 0.07321696 +v 0.04015301 -0.00000000 0.06570269 +v 0.02007650 0.03477353 0.06570269 +v 0.06638318 -0.00000000 0.04890629 +v 0.08950821 -0.00000000 0.01870825 +v 0.09576622 0.01260786 -0.00000001 +v 0.08923991 0.03696438 -0.00000001 +v 0.03319159 0.05748951 0.04890630 +v 0.07663205 0.05880184 -0.00000001 +v 0.05880184 0.07663205 -0.00000001 +v 0.04475410 0.07751638 0.01870825 +v -0.00669731 0.01160007 0.07321696 +v -0.02007651 0.03477353 0.06570269 +v 0.03696438 0.08923991 -0.00000001 +v 0.01260786 0.09576622 -0.00000001 +v -0.03319159 0.05748951 0.04890630 +v -0.01260786 0.09576622 -0.00000001 +v -0.03696438 0.08923991 -0.00000001 +v -0.04475411 0.07751638 0.01870823 +v -0.01339461 0.00000000 0.07321696 +v -0.04015301 0.00000000 0.06570269 +v -0.05880184 0.07663205 -0.00000001 +v -0.07663205 0.05880184 -0.00000001 +v -0.06638319 0.00000000 0.04890629 +v -0.08923991 0.03696438 -0.00000001 +v -0.09576622 0.01260786 -0.00000001 +v -0.08950821 0.00000000 0.01870823 +v -0.00669730 -0.01160007 0.07321696 +v -0.02007650 -0.03477353 0.06570269 +v -0.09576622 -0.01260786 -0.00000001 +v -0.08923991 -0.03696438 -0.00000001 +v -0.03319159 -0.05748951 0.04890630 +v -0.07663205 -0.05880184 -0.00000001 +v -0.05880184 -0.07663205 -0.00000001 +v -0.04475410 -0.07751638 0.01870825 +v 0.00669731 -0.01160007 0.07321696 +v 0.02007651 -0.03477353 0.06570269 +v -0.03696438 -0.08923991 -0.00000001 +v -0.01260786 -0.09576622 -0.00000001 +v 0.03319159 -0.05748951 0.04890630 +v 0.01260786 -0.09576622 -0.00000001 +v 0.03696438 -0.08923991 -0.00000001 +v 0.04475411 -0.07751638 0.01870823 +v 0.05880184 -0.07663205 -0.00000001 +v 0.07663205 -0.05880184 -0.00000001 +v 0.08923991 -0.03696438 -0.00000001 +v 0.09576622 -0.01260786 -0.00000001 +# 217 vertices, 0 vertices normals + +f 62 63 64 +f 65 66 67 +f 68 69 70 +f 71 72 73 +f 74 75 76 +f 77 78 79 +f 80 81 82 +f 83 84 85 +f 86 87 88 +f 89 90 91 +f 92 93 94 +f 95 96 97 +f 98 99 100 +f 101 102 103 +f 104 105 106 +f 107 108 109 +f 110 111 112 +f 113 114 115 +f 116 117 118 +f 119 120 121 +f 122 123 124 +f 125 126 127 +f 128 129 130 +f 131 132 133 +f 134 64 135 +f 136 137 62 +f 63 138 139 +f 140 67 141 +f 142 143 65 +f 66 144 145 +f 146 70 147 +f 148 149 68 +f 69 150 151 +f 152 73 153 +f 154 155 71 +f 72 156 157 +f 158 76 159 +f 160 161 74 +f 75 162 163 +f 164 79 165 +f 166 167 77 +f 78 168 169 +f 170 82 171 +f 172 135 80 +f 81 139 173 +f 174 85 134 +f 175 176 83 +f 84 177 136 +f 138 88 178 +f 137 179 86 +f 87 180 181 +f 171 91 182 +f 173 141 89 +f 90 145 183 +f 178 94 140 +f 181 184 92 +f 93 185 142 +f 144 97 186 +f 143 187 95 +f 96 188 189 +f 182 100 190 +f 183 147 98 +f 99 151 191 +f 186 103 146 +f 189 192 101 +f 102 193 148 +f 150 106 194 +f 149 195 104 +f 105 196 197 +f 190 109 198 +f 191 153 107 +f 108 157 199 +f 194 112 152 +f 197 200 110 +f 111 201 154 +f 156 115 202 +f 155 203 113 +f 114 204 205 +f 198 118 206 +f 199 159 116 +f 117 163 207 +f 202 121 158 +f 205 208 119 +f 120 209 160 +f 162 124 210 +f 161 211 122 +f 123 212 213 +f 206 127 170 +f 207 165 125 +f 126 169 172 +f 210 130 164 +f 213 214 128 +f 129 215 166 +f 168 133 174 +f 167 216 131 +f 132 217 175 +f 20 62 64 +f 62 21 63 +f 64 63 22 +f 23 65 67 +f 65 24 66 +f 67 66 25 +f 26 68 70 +f 68 27 69 +f 70 69 28 +f 29 71 73 +f 71 30 72 +f 73 72 31 +f 32 74 76 +f 74 33 75 +f 76 75 34 +f 35 77 79 +f 77 36 78 +f 79 78 37 +f 38 80 82 +f 80 22 81 +f 82 81 39 +f 40 83 85 +f 83 41 84 +f 85 84 20 +f 21 86 88 +f 86 42 87 +f 88 87 43 +f 39 89 91 +f 89 25 90 +f 91 90 44 +f 43 92 94 +f 92 45 93 +f 94 93 23 +f 24 95 97 +f 95 46 96 +f 97 96 47 +f 44 98 100 +f 98 28 99 +f 100 99 48 +f 47 101 103 +f 101 49 102 +f 103 102 26 +f 27 104 106 +f 104 50 105 +f 106 105 51 +f 48 107 109 +f 107 31 108 +f 109 108 52 +f 51 110 112 +f 110 53 111 +f 112 111 29 +f 30 113 115 +f 113 54 114 +f 115 114 55 +f 52 116 118 +f 116 34 117 +f 118 117 56 +f 55 119 121 +f 119 57 120 +f 121 120 32 +f 33 122 124 +f 122 58 123 +f 124 123 59 +f 56 125 127 +f 125 37 126 +f 127 126 38 +f 59 128 130 +f 128 60 129 +f 130 129 35 +f 36 131 133 +f 131 61 132 +f 133 132 40 +f 8 134 135 +f 134 20 64 +f 135 64 22 +f 20 136 62 +f 136 9 137 +f 62 137 21 +f 22 63 139 +f 63 21 138 +f 139 138 10 +f 10 140 141 +f 140 23 67 +f 141 67 25 +f 23 142 65 +f 142 11 143 +f 65 143 24 +f 25 66 145 +f 66 24 144 +f 145 144 12 +f 12 146 147 +f 146 26 70 +f 147 70 28 +f 26 148 68 +f 148 13 149 +f 68 149 27 +f 28 69 151 +f 69 27 150 +f 151 150 14 +f 14 152 153 +f 152 29 73 +f 153 73 31 +f 29 154 71 +f 154 15 155 +f 71 155 30 +f 31 72 157 +f 72 30 156 +f 157 156 16 +f 16 158 159 +f 158 32 76 +f 159 76 34 +f 32 160 74 +f 160 17 161 +f 74 161 33 +f 34 75 163 +f 75 33 162 +f 163 162 18 +f 18 164 165 +f 164 35 79 +f 165 79 37 +f 35 166 77 +f 166 19 167 +f 77 167 36 +f 37 78 169 +f 78 36 168 +f 169 168 8 +f 1 170 171 +f 170 38 82 +f 171 82 39 +f 38 172 80 +f 172 8 135 +f 80 135 22 +f 39 81 173 +f 81 22 139 +f 173 139 10 +f 8 174 134 +f 174 40 85 +f 134 85 20 +f 40 175 83 +f 175 2 176 +f 83 176 41 +f 20 84 136 +f 84 41 177 +f 136 177 9 +f 10 138 178 +f 138 21 88 +f 178 88 43 +f 21 137 86 +f 137 9 179 +f 86 179 42 +f 43 87 181 +f 87 42 180 +f 181 180 3 +f 1 171 182 +f 171 39 91 +f 182 91 44 +f 39 173 89 +f 173 10 141 +f 89 141 25 +f 44 90 183 +f 90 25 145 +f 183 145 12 +f 10 178 140 +f 178 43 94 +f 140 94 23 +f 43 181 92 +f 181 3 184 +f 92 184 45 +f 23 93 142 +f 93 45 185 +f 142 185 11 +f 12 144 186 +f 144 24 97 +f 186 97 47 +f 24 143 95 +f 143 11 187 +f 95 187 46 +f 47 96 189 +f 96 46 188 +f 189 188 4 +f 1 182 190 +f 182 44 100 +f 190 100 48 +f 44 183 98 +f 183 12 147 +f 98 147 28 +f 48 99 191 +f 99 28 151 +f 191 151 14 +f 12 186 146 +f 186 47 103 +f 146 103 26 +f 47 189 101 +f 189 4 192 +f 101 192 49 +f 26 102 148 +f 102 49 193 +f 148 193 13 +f 14 150 194 +f 150 27 106 +f 194 106 51 +f 27 149 104 +f 149 13 195 +f 104 195 50 +f 51 105 197 +f 105 50 196 +f 197 196 5 +f 1 190 198 +f 190 48 109 +f 198 109 52 +f 48 191 107 +f 191 14 153 +f 107 153 31 +f 52 108 199 +f 108 31 157 +f 199 157 16 +f 14 194 152 +f 194 51 112 +f 152 112 29 +f 51 197 110 +f 197 5 200 +f 110 200 53 +f 29 111 154 +f 111 53 201 +f 154 201 15 +f 16 156 202 +f 156 30 115 +f 202 115 55 +f 30 155 113 +f 155 15 203 +f 113 203 54 +f 55 114 205 +f 114 54 204 +f 205 204 6 +f 1 198 206 +f 198 52 118 +f 206 118 56 +f 52 199 116 +f 199 16 159 +f 116 159 34 +f 56 117 207 +f 117 34 163 +f 207 163 18 +f 16 202 158 +f 202 55 121 +f 158 121 32 +f 55 205 119 +f 205 6 208 +f 119 208 57 +f 32 120 160 +f 120 57 209 +f 160 209 17 +f 18 162 210 +f 162 33 124 +f 210 124 59 +f 33 161 122 +f 161 17 211 +f 122 211 58 +f 59 123 213 +f 123 58 212 +f 213 212 7 +f 1 206 170 +f 206 56 127 +f 170 127 38 +f 56 207 125 +f 207 18 165 +f 125 165 37 +f 38 126 172 +f 126 37 169 +f 172 169 8 +f 18 210 164 +f 210 59 130 +f 164 130 35 +f 59 213 128 +f 213 7 214 +f 128 214 60 +f 35 129 166 +f 129 60 215 +f 166 215 19 +f 8 168 174 +f 168 36 133 +f 174 133 40 +f 36 167 131 +f 167 19 216 +f 131 216 61 +f 40 132 175 +f 132 61 217 +f 175 217 2 +# 384 faces, 0 coords texture + +# End of File diff --git a/model/flex/gripper.xml b/model/flex/gripper.xml new file mode 100644 index 00000000..b364913a --- /dev/null +++ b/model/flex/gripper.xml @@ -0,0 +1,66 @@ + + + + + + diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 8ff486ef..7b1b0116 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -940,8 +940,8 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { } // check dim - if (def.spec.flex->dim != 2) { - return comperr(error, "Flex dim must be 2 in for mesh", error_sz); + if (def.spec.flex->dim < 2) { + return comperr(error, "Flex dim must be at least 2 for mesh", error_sz); } // load resource @@ -990,15 +990,42 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { mesh.RemoveRepeated(); } - // copy faces - element = mesh.Face(); - // copy vertices, convert from float to double point = vector (mesh.nvert()*3); for (int i=0; i < mesh.nvert()*3; i++) { point[i] = (double) mesh.Vert(i); } + // copy faces or create 3D mesh + if (def.spec.flex->dim == 2) { + element = mesh.Face(); + } else { + point.insert(point.begin() + 0, origin[0]); + point.insert(point.begin() + 1, origin[1]); + point.insert(point.begin() + 2, origin[2]); + for (int i=0; i < mesh.Face().size(); i+=3) { + // only add tetrahedra with positive volume + int tet[3] = {mesh.Face()[i+0]+1, + mesh.Face()[i+1]+1, + mesh.Face()[i+2]+1}; + double edge1[3], edge2[3], edge3[3]; + for (int i=0; i < 3; i++) { + edge1[i] = point[3*tet[0]+i] - origin[i]; + edge2[i] = point[3*tet[1]+i] - origin[i]; + edge3[i] = point[3*tet[2]+i] - origin[i]; + } + double normal[3]; + mjuu_crossvec(normal, edge1, edge2); + if (mjuu_dot3(normal, edge3) < mjMINVAL) { + continue; + } + element.push_back(0); + element.push_back(tet[0]); + element.push_back(tet[1]); + element.push_back(tet[2]); + } + } + return true; } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 6621c650..bb951685 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -68,6 +68,7 @@ class mjCFlexcomp { int count[3]; // grid count in each dimension double spacing[3]; // spacing between grid elements double scale[3]; // scaling for mesh and direct + double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh double mass; // total mass of auto-generated bodies double inertiabox; // size of inertia box for each body bool equality; // create edge equality constraint diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 70d3342b..f07fa341 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -311,10 +311,10 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"config", "*", "2", "key", "value"}, {">"}, {">"}, - {"flexcomp", "*", "24", "name", "type", "group", "dim", + {"flexcomp", "*", "25", "name", "type", "group", "dim", "count", "spacing", "radius", "rigid", "mass", "inertiabox", "scale", "file", "point", "element", "texcoord", "material", "rgba", - "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"}, + "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"}, {"<"}, {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"}, {"elasticity", "?", "4", "young", "poisson", "damping", "thickness"}, @@ -2643,6 +2643,10 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { ReadAttrInt(elem, "dim", &dflex.dim); ReadAttr(elem, "radius", 1, &dflex.radius, text); ReadAttrInt(elem, "group", &dflex.group); + if (!ReadAttr(elem, "origin", 3, fcomp.origin, text) && + fcomp.type == mjFCOMPTYPE_MESH && dflex.dim == 3) { + throw mjXError(elem, "origin must be specified for mesh flexcomps if dim=3"); + } // pose ReadAttr(elem, "pos", 3, fcomp.pos, text); diff --git a/test/fixture.cc b/test/fixture.cc index e6cef02f..545691b4 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -231,21 +231,22 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, #undef X if (maxdif > 0) return maxdif; - // compare arrays, apart from bvh-related ones, as those are sensitive to - // numerical differences when meshes are perfectly symmetric. -#define X(type, name, nr, nc) \ - if (strncmp(#name, "bvh_", 4)) { \ - for (int r = 0; r < m1->nr; r++) { \ - for (int c = 0; c < nc; c++) { \ - dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ - if (dif > maxdif) { \ - maxdif = dif; \ - field = #name; \ - field += " row: " + std::to_string(r); \ - field += " col: " + std::to_string(c); \ - } \ - } \ - } \ + // compare arrays, apart from bvh-related ones (which includes flex_vert0), as + // those are sensitive to numerical differences when meshes are perfectly + // symmetric. +#define X(type, name, nr, nc) \ + if (strncmp(#name, "bvh_", 4) && strncmp(#name, "flex_vert0", 4)) { \ + for (int r = 0; r < m1->nr; r++) { \ + for (int c = 0; c < nc; c++) { \ + dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ + if (dif > maxdif) { \ + maxdif = dif; \ + field = #name; \ + field += " row: " + std::to_string(r); \ + field += " col: " + std::to_string(c); \ + } \ + } \ + } \ } // NOLINT MJMODEL_POINTERS #undef X From 7eb8231fdaf55429eb801740d2a3c34e2cd57b85 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Jan 2025 04:58:06 -0800 Subject: [PATCH 183/426] Represent only the lower triangle in Newton solver's reduced dof-dof matrix. PiperOrigin-RevId: 712488529 Change-Id: Iad91c72654376539791d7856765a0d0ac9088251 --- src/engine/engine_core_constraint.c | 4 +- src/engine/engine_io.c | 25 ++++++--- src/engine/engine_solver.c | 18 ++++-- src/engine/engine_util_sparse.c | 42 ++++++++------ src/engine/engine_util_sparse.h | 4 +- src/user/user_model.cc | 2 +- .../engine_util_sparse_benchmark_test.cc | 16 +++--- test/engine/engine_core_smooth_test.cc | 8 ++- test/engine/engine_solver_test.cc | 5 +- test/engine/engine_support_test.cc | 8 ++- test/engine/engine_util_sparse_test.cc | 56 +++++++++---------- 11 files changed, 112 insertions(+), 76 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index de206a59..10120294 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2176,12 +2176,12 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // AR = JM2 * JM2' mju_sqrMatTDSparseInit(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, - rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d); + rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); mju_sqrMatTDSparse(d->efc_AR, JM2T, JM2, NULL, nv, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, rownnzT, rowadrT, colindT, NULL, - rownnz, rowadr, colind, rowsuper, d); + rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); // add R to diagonal of AR for (int i=0; i < nefc; i++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 34cc8dbb..1cbfa804 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -943,8 +943,11 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, // process below diagonal unless reduced and dof is simple if (!reduced || !m->dof_simplenum[i]) { while ((j = m->dof_parentid[j]) >= 0) { + // both reduced and non-reduced have lower triangle rownnz[i]++; - rownnz[j]++; + + // only non-reduced has upper triangle + if (!reduced) rownnz[j]++; } } } @@ -969,8 +972,11 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, remaining[i]--; colind[rowadr[i] + remaining[i]] = j; - remaining[j]--; - colind[rowadr[j] + remaining[j]] = i; + // only non-reduced has upper triangle + if (!reduced) { + remaining[j]--; + colind[rowadr[j] + remaining[j]] = i; + } } } } @@ -1152,8 +1158,11 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src, remaining[i]--; dst[rowadr[i] + remaining[i]] = src[adr]; - remaining[j]--; - dst[rowadr[j] + remaining[j]] = src[adr]; + // only non-reduced has upper triangle + if (!reduced) { + remaining[j]--; + dst[rowadr[j] + remaining[j]] = src[adr]; + } adr++; } @@ -1172,7 +1181,7 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src, -// integer valued dst[M] = src[D lower], handle different sparsity representations +// integer valued dst[M] = src[D lower] static void copyD2MSparse(const mjModel* m, const mjData* d, int* dst, const int* src) { int nv = m->nv; @@ -1197,7 +1206,7 @@ static void copyD2MSparse(const mjModel* m, const mjData* d, int* dst, const int // construct index mappings between M <-> D and M -> C -static void makeDmap(const mjModel* m, mjData* d) { +static void makeDofDofmap(const mjModel* m, mjData* d) { int nM = m->nM, nC = m->nC, nD = m->nD; mj_markStack(d); @@ -1955,7 +1964,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // make C makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind, /*reduced=*/1); - makeDmap(m, d); + makeDofDofmap(m, d); } // restore pluginstate and plugindata diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index ac506fa4..39ca9b30 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1403,7 +1403,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { mju_sqrMatTDSparseInit(ctx->H_rownnz, ctx->H_rowadr, nv, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add nC to Hessian total nonzeros (unavoidable overcounting since H_colind is still unknown) ctx->nH = m->nC + ctx->H_rowadr[nv - 1] + ctx->H_rownnz[nv - 1]; @@ -1424,14 +1424,24 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, ctx->C, d->C_rownnz, d->C_rowadr, d->C_colind); + // transiently compute H'; mju_cholFactorNNZ is memory-contiguous in upper triangle layout + mj_markStack(d); + int* HT_rownnz = mjSTACKALLOC(d, nv, int); + int* HT_rowadr = mjSTACKALLOC(d, nv, int); + int* HT_colind = mjSTACKALLOC(d, ctx->nH, int); + mju_transposeSparse(NULL, NULL, nv, nv, + HT_rownnz, HT_rowadr, HT_colind, + ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); + // count total and row non-zeros of reverse-Cholesky factor L - ctx->nL = mju_cholFactorNNZ(ctx->L_rownnz, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, d); + ctx->nL = mju_cholFactorNNZ(ctx->L_rownnz, HT_rownnz, HT_rowadr, HT_colind, nv, d); + mj_freeStack(d); // compute L row adresses: rowadr = cumsum(rownnz) ctx->L_rowadr[0] = 0; @@ -1508,7 +1518,7 @@ static void FactorizeHessian(const mjModel* m, mjData* d, mjCGContext* ctx, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 80c3f256..444edbf2 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -628,7 +628,7 @@ void mju_superSparse(int nr, int* rowsuper, void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, const int* rownnz, const int* rowadr, const int* colind, const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d) { + const int* rowsuperT, mjData* d, int flg_upper) { mj_markStack(d); int* chain = mjSTACKALLOC(d, 2*nr, int); int nchain = 0; @@ -640,8 +640,10 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, res_rownnz[r] = res_rownnz[r - 1]; // fill in upper triangle - for (int j=0; j < nchain; j++) { - res_rownnz[res_colind[j]]++; + if (flg_upper) { + for (int j=0; j < nchain; j++) { + res_rownnz[res_colind[j]]++; + } } // update chain with diagonal @@ -691,15 +693,17 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, res_colind = chain + inew; // update upper triangle - int nchain_end = nchain; + if (flg_upper) { + int nchain_end = nchain; - // avoid double counting. - if (nchain > 0 && res_colind[nchain-1] == r) { - nchain_end = nchain - 1; - } + // avoid double counting + if (nchain > 0 && res_colind[nchain-1] == r) { + nchain_end = nchain - 1; + } - for (int j=0; j < nchain_end; j++) { - res_rownnz[res_colind[j]]++; + for (int j=0; j < nchain_end; j++) { + res_rownnz[res_colind[j]]++; + } } } } @@ -731,7 +735,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d) { + mjData* d, int flg_upper) { // allocate space for accumulation buffer and matT mj_markStack(d); @@ -846,13 +850,15 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, // fill upper triangle - for (int i=0; i < nc; i++) { - int start = res_rowadr[i]; - int end = start + res_rownnz[i] - 1; - for (int j=start; j < end; j++) { - int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++; - res[adr] = res[j]; - res_colind[adr] = i; + if (flg_upper) { + for (int i=0; i < nc; i++) { + int start = res_rowadr[i]; + int end = start + res_rownnz[i] - 1; + for (int j=start; j < end; j++) { + int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++; + res[adr] = res[j]; + res_colind[adr] = i; + } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index f1f46f4a..62cf6890 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -96,13 +96,13 @@ MJAPI void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d); + mjData* d, int flg_upper); // precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse MJAPI void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, const int* rownnz, const int* rowadr, const int* colind, const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d); + const int* rowsuperT, mjData* d, int flg_upper); // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 3e6134bd..0cf6a4ba 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2495,7 +2495,7 @@ void mjCModel::CopyTree(mjModel* m) { } } } - m->nC = nC = 2 * nOD + nv; + m->nC = nC = nOD + nv; } // copy plugin data diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index d77dc067..838e71a1 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -43,7 +43,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, - const int* colindT, const int* rowsuperT, mjData* d) { + const int* colindT, const int* rowsuperT, mjData* d, int unused) { mj_markStack(d); int* chain = mj_stackAllocInt(d, 2 * nc); mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -453,7 +453,8 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, d->efc_JT_rownnz, d->efc_JT_rowadr, - d->efc_JT_colind, d->efc_JT_rowsuper, d); + d->efc_JT_colind, d->efc_JT_rowsuper, d, + /*flg_upper=*/1); // compute H = M + J'*D*J mj_addM(m, d, H, rownnz, rowadr, colind); @@ -578,7 +579,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { func(H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d); + d->efc_JT_rowsuper, d, /*flg_upper=*/1); } } else { for (auto s : state) { @@ -587,10 +588,11 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDSparse_baseline(H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, - d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d); + mju_sqrMatTDSparse_baseline( + H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, + d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, + d->efc_JT_rowsuper, d, /*unused=*/0); } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 1fa16ec2..0cf12290 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -481,8 +481,12 @@ TEST_F(CoreSmoothTest, SolveLDs) { vector LDdense2(nv*nv); mj_fullM(m, LDdense2.data(), d->qLD); - // expect dense matrices to match exactly - for (int i=0; i < nv*nv; i++) EXPECT_EQ(LDdense[i], LDdense2[i]); + // expect lower triangles to match exactly + for (int i=0; i < nv; i++) { + for (int j=0; j < i; j++) { + EXPECT_EQ(LDdense[i*nv+j], LDdense2[i*nv+j]); + } + } // compare LD and LDs vector solve vector vec(nv); diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index d20c3d38..6343ce24 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -61,8 +61,9 @@ static const char* const kIlslandEfcPath = // compare accelerations produced by CG solver with and without islands TEST_F(SolverTest, IslandsEquivalent) { const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; model->opt.solver = mjSOL_CG; // use CG solver model->opt.tolerance = 0; // set tolerance to 0 model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 456143cd..8e915328 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -725,8 +725,12 @@ TEST_F(InertiaTest, DenseSameAsSparse) { // dense addM mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); - // dense comparison, should be same matrix - EXPECT_THAT(dst_dense, ElementsAreArray(dst_sparse)); + // dense comparison, lower triangle should match + for (int i=0; i < nv; i++) { + for (int j=0; j < i; j++) { + EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]); + } + } // clean up mj_deleteData(d); diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index ade96c0b..37da5155 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -327,7 +327,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -336,7 +336,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -372,7 +372,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -382,7 +382,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -420,7 +420,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4)); @@ -429,7 +429,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 0, 0, 0, 0)); @@ -468,7 +468,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 0, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 2)); @@ -477,7 +477,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 0, 0, 0, 4, 35, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 0, 0, 0, 0, 2, 0)); @@ -514,7 +514,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -523,7 +523,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(5, 6, 4, 6, 9, 0, 4, 16, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -559,7 +559,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 3)); @@ -568,7 +568,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(1, 2, 0, 4, 0, 0, 2, 13, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0)); @@ -606,7 +606,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 2, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2)); @@ -615,7 +615,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { mju_sqrMatTDUncompressedInit(rowadrH, 2); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 2, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 4, 35)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 1)); @@ -652,7 +652,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -661,7 +661,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 2, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(14, 18, 8, 18, 27, 0, 8, 32, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -699,7 +699,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -708,7 +708,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -747,7 +747,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -756,7 +756,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(14, 14, 14, 14, 14, 14, 14, 14, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -795,7 +795,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -804,7 +804,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -843,7 +843,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 4, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4)); EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12)); @@ -852,7 +852,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { mju_sqrMatTDUncompressedInit(rowadrH, 4); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 4, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10)); @@ -895,7 +895,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 5, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0, 0, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4, 4, 4)); @@ -904,7 +904,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { mju_sqrMatTDUncompressedInit(rowadrH, 5); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 5, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)); @@ -945,7 +945,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 7, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7)); EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42)); @@ -954,7 +954,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { mju_sqrMatTDUncompressedInit(rowadrH, 7); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 1, 7, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT( matH, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, From ac11e5faa6700ab54409c0713e2d80cd688c217a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Jan 2025 05:47:13 -0800 Subject: [PATCH 184/426] Add CSR implementation of `mj_factorI` PiperOrigin-RevId: 712498431 Change-Id: I13b52e53482ed97da8788875d4d95e2beb5ca7c1 --- src/engine/engine_core_smooth.c | 46 +++++- src/engine/engine_core_smooth.h | 8 +- test/benchmark/CMakeLists.txt | 12 ++ test/benchmark/factorI_benchmark_test.cc | 102 ++++++++++++ test/benchmark/inertia_benchmark_test.cc | 101 ++++++++++++ test/benchmark/solveLD_benchmark_test.cc | 3 +- test/benchmark/testdata/inertia.xml | 199 +++++++++++++++++++++++ test/engine/engine_core_smooth_test.cc | 49 +++++- test/fixture.h | 8 +- 9 files changed, 514 insertions(+), 14 deletions(-) create mode 100644 test/benchmark/factorI_benchmark_test.cc create mode 100644 test/benchmark/inertia_benchmark_test.cc create mode 100644 test/benchmark/testdata/inertia.xml diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 641b7bdc..81e276f6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1440,10 +1440,9 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu } } - // compute 1/diag(D), 1/sqrt(diag(D)) + // compute 1/diag(D) for (int i=0; i < nv; i++) { - mjtNum qLDi = qLD[dof_Madr[i]]; - qLDiagInv[i] = 1.0/qLDi; + qLDiagInv[i] = 1.0 / qLD[dof_Madr[i]]; } } @@ -1458,6 +1457,40 @@ void mj_factorM(const mjModel* m, mjData* d) { +// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd +// like mj_factorI, but using CSR representation +void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { + // backward loop over rows + for (int k=nv-1; k >= 0; k--) { + // get row k's address, diagonal index, inverse diagonal value + int rowadr_k = rowadr[k]; + int diag_k = rowadr_k + rownnz[k] - 1; + mjtNum invD = 1 / mat[diag_k]; + if (diaginv) diaginv[k] = invD; + + // skip if simple + if (diagnum[k]) { + continue; + } + + // update triangle above row k, inclusive + for (int adr=diag_k - 1; adr >= rowadr_k; adr--) { + // tmp = L(k, i) / L(k, k) + mjtNum tmp = mat[adr] * invD; + + // update row i < k: L(i, 0..i) -= L(i, 0..i) * L(k, i) / L(k, k) + int i = colind[adr]; + mju_addToScl(mat + rowadr[i], mat + rowadr_k, -tmp, rownnz[i]); + + // update ith element of row k: L(k, i) /= L(k, k) + mat[adr] = tmp; + } + } +} + + + // in-place sparse backsubstitution: x = inv(L'*D*L)*x // L is in lower triangle of qLD; D is on diagonal of qLD // handle n vectors at once @@ -1575,8 +1608,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, - const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, - const int* colind) { + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { // x <- L^-T x for (int i=nv-1; i > 0; i--) { // skip diagonal (simple) rows, exploit sparsity of input vector @@ -1584,7 +1616,7 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv continue; } - int d = diagind[i]; + int d = rownnz[i] - 1; int adr_i = rowadr[i]; mjtNum x_i = x[i]; for (int j=0; j < d; j++) { @@ -1607,7 +1639,7 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv } int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, diagind[i], colind+adr, /*flg_unc1=*/0); + x[i] -= mju_dotSparse(qLDs+adr, x, rownnz[i] - 1, colind+adr, /*flg_unc1=*/0); } } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index db1af84b..f4733b08 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -51,6 +51,11 @@ MJAPI void mj_crb(const mjModel* m, mjData* d); // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv); +// sparse L'*D*L factorizaton of inertia-like matrix +// like mj_factorI, but using CSR representation +MJAPI void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); + // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd MJAPI void mj_factorM(const mjModel* m, mjData* d); @@ -61,8 +66,7 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, - const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, - const int* colind); + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 0407edc0..0a23fafe 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -43,6 +43,18 @@ mujoco_test( ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ) +mujoco_test( + factorI_benchmark_test + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers +) + +mujoco_test( + inertia_benchmark_test + MAIN_TARGET benchmark::benchmark_main + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers +) + mujoco_test( solveLD_benchmark_test MAIN_TARGET benchmark::benchmark_main diff --git a/test/benchmark/factorI_benchmark_test.cc b/test/benchmark/factorI_benchmark_test.cc new file mode 100644 index 00000000..55118a99 --- /dev/null +++ b/test/benchmark/factorI_benchmark_test.cc @@ -0,0 +1,102 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// A benchmark for comparing different implementations of mj_factorI. + +#include +#include +#include +#include +#include "src/engine/engine_core_smooth.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +// number of steps to benchmark +static const int kNumBenchmarkSteps = 50; + +// ----------------------------- benchmark ------------------------------------ + +static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { + static mjModel* m; + if (coil) { + m = LoadModelFromPath("plugin/elasticity/coil.xml"); + } else { + m = LoadModelFromPath("humanoid/humanoid100.xml"); + } + + mjData* d = mj_makeData(m); + mj_forward(m, d); + + // allocate inputs and outputs + mj_markStack(d); + + // CSR matrices + mjtNum* Ms = mj_stackAllocNum(d, m->nC); + mjtNum* LDs = mj_stackAllocNum(d, m->nC); + for (int i=0; i < m->nC; i++) { + Ms[i] = d->qM[d->mapM2C[i]]; + } + + // benchmark + while (state.KeepRunningBatch(kNumBenchmarkSteps)) { + for (int i=0; i < kNumBenchmarkSteps; i++) { + if (legacy) { + mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv); + } else { + mju_copy(LDs, Ms, m->nC); + mj_factorIs(LDs, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + } + } + } + + // finalize + mj_freeStack(d); + mj_deleteData(d); + mj_deleteModel(m); + state.SetItemsProcessed(state.iterations()); +} + +void ABSL_ATTRIBUTE_NO_TAIL_CALL +BM_factorI_COIL_LEGACY(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_factorI(state, /*legacy=*/true, /*coil=*/true); +} +BENCHMARK(BM_factorI_COIL_LEGACY); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL +BM_factorI_COIL_CSR(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_factorI(state, /*legacy=*/false, /*coil=*/true); +} +BENCHMARK(BM_factorI_COIL_CSR); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL +BM_factorI_H100_LEGACY(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_factorI(state, /*legacy=*/true, /*coil=*/false); +} +BENCHMARK(BM_factorI_H100_LEGACY); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL +BM_factorI_H100_CSR(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_factorI(state, /*legacy=*/false, /*coil=*/false); +} +BENCHMARK(BM_factorI_H100_CSR); + +} // namespace +} // namespace mujoco diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc new file mode 100644 index 00000000..8ba580b7 --- /dev/null +++ b/test/benchmark/inertia_benchmark_test.cc @@ -0,0 +1,101 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// A benchmark for comparing legacy and two CSR implementations of inertia +// factor and then solve. + +#include +#include +#include +#include +#include "src/engine/engine_core_smooth.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +// number of steps to benchmark +static const int kNumBenchmarkSteps = 50; + +// ----------------------------- benchmark ------------------------------------ + +enum class SolveType { + kLegacy = 0, + kCsr, +}; + +static void BM_solve(benchmark::State& state, SolveType type) { + static mjModel* m; + m = LoadModelFromPath("../test/benchmark/testdata/inertia.xml"); + + mjData* d = mj_makeData(m); + mj_forward(m, d); + + // allocate input and output vectors + mj_markStack(d); + + // make CSR matrix + mjtNum* Ms = mj_stackAllocNum(d, m->nC); + mjtNum* LDs = mj_stackAllocNum(d, m->nC); + for (int i=0; i < m->nC; i++) { + Ms[i] = d->qM[d->mapM2C[i]]; + } + + // arbitrary input vector + mjtNum *res = mj_stackAllocNum(d, m->nv); + mjtNum *vec = mj_stackAllocNum(d, m->nv); + for (int i=0; i < m->nv; i++) { + vec[i] = 0.2 + 0.3*i; + } + + // benchmark + while (state.KeepRunningBatch(kNumBenchmarkSteps)) { + for (int i=0; i < kNumBenchmarkSteps; i++) { + switch (type) { + case SolveType::kLegacy: + mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv); + mj_solveM(m, d, res, vec, 1); + break; + case SolveType::kCsr: + mju_copy(LDs, Ms, m->nC); + mj_factorIs(LDs, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mju_copy(res, vec, m->nv); + mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + } + } + } + + // finalize + mj_freeStack(d); + mj_deleteData(d); + mj_deleteModel(m); + state.SetItemsProcessed(state.iterations()); +} + +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_LEGACY(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_solve(state, SolveType::kLegacy); +} +BENCHMARK(BM_solve_LEGACY); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_CSR(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_solve(state, SolveType::kCsr); +} +BENCHMARK(BM_solve_CSR); + +} // namespace +} // namespace mujoco diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index 5564fc78..f2dc39e1 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -64,8 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { } else { mju_copy(res, vec, m->nv); mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, - d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, - d->C_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/benchmark/testdata/inertia.xml b/test/benchmark/testdata/inertia.xml new file mode 100644 index 00000000..5d08a751 --- /dev/null +++ b/test/benchmark/testdata/inertia.xml @@ -0,0 +1,199 @@ + + diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 0cf12290..717fde98 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -496,8 +496,7 @@ TEST_F(CoreSmoothTest, SolveLDs) { mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, - d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, - d->C_colind); + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv; i++) { @@ -508,5 +507,51 @@ TEST_F(CoreSmoothTest, SolveLDs) { mj_deleteModel(m); } +TEST_F(CoreSmoothTest, FactorIs) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; + + mjData* d = mj_makeData(m); + mj_forward(m, d); + + int nC = m->nC, nv = m->nv; + + // copy qM into LDs, qLD into qLDexpected: CSR format + vector qLDsExpected(nC); + vector qLDs(nC); + for (int i=0; i < nC; i++) { + int index = d->mapM2C[i]; + qLDs[i] = d->qM[index]; // mj_factorIs is in-place + qLDsExpected[i] = d->qLD[index]; + } + + vector qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); + vector qLDiagInv(nv, 0); + + mj_factorIs(qLDs.data(), qLDiagInv.data(), nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + + // expect outputs to match to floating point precision + EXPECT_THAT(qLDs, Pointwise(DoubleNear(1e-12), qLDsExpected)); + EXPECT_THAT(qLDiagInv, Pointwise(DoubleNear(1e-12), qLDiagInvExpected)); + + /* uncomment for debugging + vector LDdense(nv*nv); + + mju_sparse2dense(LDdense.data(), qLDexpected.data(), nv, nv, + d->C_rownnz, d->C_rowadr, d->C_colind); + PrintMatrix(LDdense.data(), nv, nv, 2); + + mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv, + d->C_rownnz, d->C_rowadr, d->C_colind); + PrintMatrix(LDdense.data(), nv, nv, 2); + */ + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/fixture.h b/test/fixture.h index 69224c15..d2860d7c 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -118,7 +118,13 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) { std::cerr << "\n"; for (int r = 0; r < nrow; r++) { for (int c = 0; c < ncol; c++) { - std::cerr << std::fixed << std::setw(3 + p) << mat[c + r*ncol] << " "; + mjtNum val = mat[c + r*ncol]; + if (val) { + std::cerr << std::fixed << std::setw(5 + p) << val << " "; + } else { + // don't print exact zeros + std::cerr << std::string(6 + p, ' '); + } } std::cerr << "\n"; } From b5df2c10bff3c1b62d4f5eee15477793f7f756aa Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Jan 2025 06:57:19 -0800 Subject: [PATCH 185/426] Remove `mjData.C_diag`, no longer required as C is now lower triangular. PiperOrigin-RevId: 712514204 Change-Id: I8293f65ebc09c6a8fed76b0e59d2e2a667e01a43 --- doc/includes/references.h | 1 - include/mujoco/mjdata.h | 1 - include/mujoco/mjxmacro.h | 1 - introspect/structs.py | 8 -------- mjx/mujoco/mjx/_src/io.py | 1 - mjx/mujoco/mjx/_src/types.py | 2 -- src/engine/engine_io.c | 22 ++++++++++++---------- src/engine/engine_print.c | 2 +- unity/Runtime/Bindings/MjBindings.cs | 1 - 9 files changed, 13 insertions(+), 26 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 685d8e3d..8cd73e5a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -313,7 +313,6 @@ struct mjData_ { int* B_colind; // body-dof: column indices of non-zeros (nB x 1) int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1) int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1) - int* C_diag; // reduced dof-dof: index of diagonal element (nv x 1) int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1) int* mapM2C; // index mapping from M to C (nC x 1) int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index c29af364..b51b19ae 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -341,7 +341,6 @@ struct mjData_ { int* B_colind; // body-dof: column indices of non-zeros (nB x 1) int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1) int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1) - int* C_diag; // reduced dof-dof: index of diagonal element (nv x 1) int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1) int* mapM2C; // index mapping from M to C (nC x 1) int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 9edd1fed..2d78fc02 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -654,7 +654,6 @@ X ( int, B_colind, nB, 1 ) \ X ( int, C_rownnz, nv, 1 ) \ X ( int, C_rowadr, nv, 1 ) \ - X ( int, C_diag, nv, 1 ) \ X ( int, C_colind, nC, 1 ) \ X ( int, mapM2C, nC, 1 ) \ X ( int, D_rownnz, nv, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index f02158d9..be60293c 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -5388,14 +5388,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='reduced dof-dof: address of each row in C_colind (nv x 1)', # pylint: disable=line-too-long ), - StructFieldDecl( - name='C_diag', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='reduced dof-dof: index of diagonal element', - array_extent=('nv',), - ), StructFieldDecl( name='C_colind', type=PointerType( diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index ec00ee87..55b6c08c 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -332,7 +332,6 @@ def make_data( 'B_colind': (m.nB, jp.int32), 'C_rownnz': (m.nv, jp.int32), 'C_rowadr': (m.nv, jp.int32), - 'C_diag': (m.nv, jp.int32), 'C_colind': (m.nC, jp.int32), 'mapM2C': (m.nC, jp.int32), 'D_rownnz': (m.nv, jp.int32), diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 8d6a62d9..16fa3a81 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -1296,7 +1296,6 @@ class Data(PyTreeNode): B_colind: body-dof: column indices of non-zeros (nB,) C_rownnz: reduced dof-dof: non-zeros in each row (nv,) C_rowadr: reduced dof-dof: address of each row in C_colind (nv,) - C_diag: reduced dof-dof: index of diagonal element (nv,) C_colind: reduced dof-dof: column indices of non-zeros (nC,) mapM2C: index mapping from M to C (nC,) D_rownnz: dof-dof: non-zeros in each row (nv,) @@ -1426,7 +1425,6 @@ class Data(PyTreeNode): B_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name - C_diag: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name C_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name mapM2C: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name D_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 1cbfa804..e32a9add 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -994,16 +994,18 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, } // find diagonal indices - for (int i = 0; i < nv; i++) { - int adr = rowadr[i]; - int j = 0; - while (colind[adr + j] < i && j < rownnz[i]) { - j++; + if (diag) { + for (int i = 0; i < nv; i++) { + int adr = rowadr[i]; + int j = 0; + while (colind[adr + j] < i && j < rownnz[i]) { + j++; + } + if (colind[adr + j] != i) { + mjERROR("diagonal index not found"); + } + diag[i] = j; } - if (colind[adr + j] != i) { - mjERROR("diagonal index not found"); - } - diag[i] = j; } mj_freeStack(d); @@ -1963,7 +1965,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { checkDBSparse(m, d); // make C - makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind, /*reduced=*/1); + makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, NULL, d->C_colind, /*reduced=*/1); makeDofDofmap(m, d); } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 6cba6352..9f6b6e33 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1135,7 +1135,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, "\n\n"); // C sparse structure - mj_printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_diag, d->C_rownnz, + mj_printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, NULL, d->C_rownnz, NULL, d->C_colind, fp); fprintf(fp, NAME_FORMAT, "C_rownnz"); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 92fbd9e9..7ad3d1b6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4939,7 +4939,6 @@ public unsafe struct mjData_ { public int* B_colind; public int* C_rownnz; public int* C_rowadr; - public int* C_diag; public int* C_colind; public int* mapM2C; public int* D_rownnz; From f607d9554ed758be3d7c3cd822feb8c6d7cfc611 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 6 Jan 2025 07:49:35 -0800 Subject: [PATCH 186/426] Check that there are no pending keyframes while writing a spec. This can happen when a body is detached without compiling or recompiling. Also added error catching when trying to delete a body instead of detaching it. Fixes #2327 PiperOrigin-RevId: 712526730 Change-Id: I4b48df83120fca12d475c85b3a54893841449653 --- doc/APIreference/functions.rst | 2 +- doc/includes/references.h | 2 +- include/mujoco/mujoco.h | 6 +++--- introspect/functions.py | 6 +++--- src/user/user_api.cc | 13 ++++++++++--- src/user/user_api.h | 4 ++-- src/user/user_model.cc | 1 - src/xml/xml_native_writer.cc | 6 +++++- test/user/user_api_test.cc | 12 ++++++++++++ 9 files changed, 37 insertions(+), 15 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c80c9628..590aaf72 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3918,7 +3918,7 @@ Add frame to body. .. mujoco-include:: mjs_delete -Delete object corresponding to the given element. +Delete object corresponding to the given element, return 0 on success. .. _AddNonTreeElements: diff --git a/doc/includes/references.h b/doc/includes/references.h index 8cd73e5a..e1432bd2 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3578,7 +3578,7 @@ mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -void mjs_delete(mjsElement* element); +int mjs_delete(mjsElement* element); mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); mjsSensor* mjs_addSensor(mjSpec* s); mjsFlex* mjs_addFlex(mjSpec* s); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 66f5fa98..9e4d25ff 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -121,7 +121,7 @@ MJAPI void mj_freeLastXML(void); // If length of the output buffer is too small, returns the required size. MJAPI int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int error_sz); -// Save spec to XML file, return 1 on success, 0 otherwise. +// Save spec to XML file, return 0 on success, -1 otherwise. MJAPI int mj_saveXML(const mjSpec* s, const char* filename, char* error, int error_sz); @@ -1453,8 +1453,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -// Delete object corresponding to the given element. -MJAPI void mjs_delete(mjsElement* element); +// Delete object corresponding to the given element, return 0 on success. +MJAPI int mjs_delete(mjsElement* element); //---------------------------------- Non-tree elements --------------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index b16bee97..fa2141a9 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -379,7 +379,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ type=ValueType(name='int'), ), ), - doc='Save spec to XML file, return 1 on success, 0 otherwise.', + doc='Save spec to XML file, return 0 on success, -1 otherwise.', )), ('mj_step', FunctionDecl( @@ -9277,7 +9277,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ('mjs_delete', FunctionDecl( name='mjs_delete', - return_type=ValueType(name='void'), + return_type=ValueType(name='int'), parameters=( FunctionParameterDecl( name='element', @@ -9286,7 +9286,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Delete object corresponding to the given element.', + doc='Delete object corresponding to the given element, return 0 on success.', # pylint: disable=line-too-long )), ('mjs_addActuator', FunctionDecl( diff --git a/src/user/user_api.cc b/src/user/user_api.cc index af7cd9b5..7e819007 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -254,10 +254,17 @@ int mjs_activatePlugin(mjSpec* s, const char* name) { -// delete object, it will call the appropriate destructor since ~mjCBase is virtual -void mjs_delete(mjsElement* element) { +// delete object, return 0 if success +int mjs_delete(mjsElement* element) { mjCBase* object = static_cast(element); - object->model->DeleteElement(element); + try { + // it will call the appropriate destructor since ~mjCBase is virtual + object->model->DeleteElement(element); + return 0; + } catch (mjCError& e) { + object->model->SetError(e); + return -1; + } } diff --git a/src/user/user_api.h b/src/user/user_api.h index 70eb6af2..ac545e74 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -108,8 +108,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -// Delete object corresponding to the given element. -MJAPI void mjs_delete(mjsElement* element); +// Delete object corresponding to the given element, return 0 on success. +MJAPI int mjs_delete(mjsElement* element); //---------------------------------- Add non-tree elements ----------------------------------------- diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 0cf6a4ba..7359c1f0 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -530,7 +530,6 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { ResetTreeLists(); } - PointToLocal(); return *this; } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e9a47920..3ae9fc22 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -858,7 +858,7 @@ mjXWriter::mjXWriter(void) { // cast model void mjXWriter::SetModel(const mjSpec* _spec, const mjModel* m) { if (_spec) { - model = (mjCModel*)_spec->element; + model = static_cast(_spec->element); } if (m) { model->CopyBack(m); @@ -2207,6 +2207,10 @@ void mjXWriter::Keyframe(XMLElement* root) { // create section XMLElement* section = InsertEnd(root, "keyframe"); + if (!model->key_pending_.empty()) { + throw mjXError(0, "Model has pending keyframes. It must be (re)compiled before writing XML."); + } + // write all keyframes for (int i=0; inkey; i++) { XMLElement* elem = InsertEnd(section, "key"); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index c6620c1c..e641db58 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1222,9 +1222,21 @@ void TestDetachBody(bool compile) { mjsBody* body = mjs_findBody(child, "body"); EXPECT_THAT(body, NotNull()); + // get an error if trying to delete the body + EXPECT_EQ(mjs_delete(body->element), -1); + EXPECT_THAT(mjs_getError(child), HasSubstr("use detach instead")); + // detach subtree EXPECT_THAT(mjs_detachBody(child, body), 0); + // try saving to XML before compiling again + std::array e; + std::array s; + EXPECT_EQ(mj_saveXMLString(child, s.data(), 1024, e.data(), 1024), -1); + EXPECT_THAT(e.data(), compile + ? HasSubstr("Model has pending keyframes") + : HasSubstr("Only compiled model can be written")); + // compile new model mjModel* m_detached = mj_compile(child, 0); EXPECT_THAT(m_detached, NotNull()); From 0be7e6a1f6c32c5707d9d78ee2831bed6e9a78f0 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 6 Jan 2025 10:27:16 -0800 Subject: [PATCH 187/426] Fix #2306. PiperOrigin-RevId: 712575320 Change-Id: Ia74ef0b31b4e7098647340760572a1f6368eab64 --- mjx/mujoco/mjx/_src/io.py | 3 ++- mjx/mujoco/mjx/_src/io_test.py | 9 +++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 55b6c08c..7a000a15 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -681,4 +681,5 @@ def put_data( # copy because device_put is async: data = types.Data(**{k: copy.copy(v) for k, v in fields.items()}) - return jax.device_put(data, device=device) + data = jax.device_put(data, device=device) + return _strip_weak_type(data) diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 8950c677..b1730f33 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -310,6 +310,15 @@ class DataIOTest(parameterized.TestCase): np.testing.assert_allclose(dx.cvel, d.cvel) np.testing.assert_allclose(dx.cdof_dot, d.cdof_dot) + # check that there are no weak types + self.assertFalse( + any( + jax.tree_util.tree_flatten( + jax.tree_util.tree_map(lambda x: x.weak_type, dx) + )[0] + ) + ) + # check that qM is transformed properly qm = np.zeros((m.nv, m.nv), dtype=np.float64) mujoco.mj_fullM(m, qm, d.qM) From 674d227050473b5a7b96b76a9d0982d5c85c9af4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 7 Jan 2025 02:38:50 -0800 Subject: [PATCH 188/426] Standardize names of sparse fill-in pre-counting functions PiperOrigin-RevId: 712835830 Change-Id: I8e90dfa52af56ede917e1fd90b8d551898c244e5 --- src/engine/engine_core_constraint.c | 4 +- src/engine/engine_solver.c | 10 ++-- src/engine/engine_util_sparse.c | 46 ++++++++-------- src/engine/engine_util_sparse.h | 14 ++--- test/engine/engine_util_sparse_test.cc | 72 +++++++++++++------------- 5 files changed, 75 insertions(+), 71 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 10120294..c1313cd6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2175,8 +2175,8 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { mju_superSparse(nefc, rowsuper, rownnz, rowadr, colind); // AR = JM2 * JM2' - mju_sqrMatTDSparseInit(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, - rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); + mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, + rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); mju_sqrMatTDSparse(d->efc_AR, JM2T, JM2, NULL, nv, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 39ca9b30..7f0fe182 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1400,10 +1400,10 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { } // initialize Hessian rowadr, rownnz - mju_sqrMatTDSparseInit(ctx->H_rownnz, ctx->H_rowadr, nv, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, - d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d, /*flg_upper=*/0); + mju_sqrMatTDSparseCount(ctx->H_rownnz, ctx->H_rowadr, nv, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, + d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, + d->efc_JT_rowsuper, d, /*flg_upper=*/0); // add nC to Hessian total nonzeros (unavoidable overcounting since H_colind is still unknown) ctx->nH = m->nC + ctx->H_rowadr[nv - 1] + ctx->H_rownnz[nv - 1]; @@ -1440,7 +1440,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); // count total and row non-zeros of reverse-Cholesky factor L - ctx->nL = mju_cholFactorNNZ(ctx->L_rownnz, HT_rownnz, HT_rowadr, HT_colind, nv, d); + ctx->nL = mju_cholFactorCount(ctx->L_rownnz, HT_rownnz, HT_rowadr, HT_colind, nv, d); mj_freeStack(d); // compute L row adresses: rowadr = cumsum(rownnz) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 444edbf2..e4e1c1c9 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -625,10 +625,10 @@ void mju_superSparse(int nr, int* rowsuper, // precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse -void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, - const int* rownnz, const int* rowadr, const int* colind, - const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d, int flg_upper) { +void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, mjData* d, int flg_upper) { mj_markStack(d); int* chain = mjSTACKALLOC(d, 2*nr, int); int nchain = 0; @@ -865,11 +865,11 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, mj_freeStack(d); } -// compute row non-zeros of reverse-Cholesky factor L, return total non-zeros -// based on ldl_symbolic from 'Algorithm 8xx: a concise sparse Cholesky factorization package' -// note: reads pattern from upper triangle -int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, - int n, mjData* d) { +// precount row non-zeros of reverse-Cholesky factor L, return total non-zeros +// based on ldl_symbolic from 'Algorithm 8xx: a concise sparse Cholesky factorization package' +// reads pattern from upper triangle +int mju_cholFactorCount(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, + int n, mjData* d) { mj_markStack(d); int* parent = mjSTACKALLOC(d, n, int); int* flag = mjSTACKALLOC(d, n, int); @@ -880,24 +880,28 @@ int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const flag[r] = r; L_rownnz[r] = 1; // start with 1 for diagonal - // loop over non-zero columns + // loop over non-zero columns of upper triangle int start = rowadr[r]; int end = start + rownnz[r]; for (int c = start; c < end; c++) { int i = colind[c]; - if (i > r) { - // traverse from i to ancestor, stop when row is flagged - while (flag[i] != r) { - // if not yet set, set parent to current row - if (parent[i] == -1) { - parent[i] = r; - } - // increment non-zeros, flag row i, advance to parent - L_rownnz[i]++; - flag[i] = r; - i = parent[i]; + // skip lower triangle + if (i <= r) { + continue; + } + + // traverse from i to ancestor, stop when row is flagged + while (flag[i] != r) { + // if not yet set, set parent to current row + if (parent[i] == -1) { + parent[i] = r; } + + // increment non-zeros, flag row i, advance to parent + L_rownnz[i]++; + flag[i] = r; + i = parent[i]; } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 62cf6890..04c37610 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -99,17 +99,17 @@ MJAPI void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT mjData* d, int flg_upper); // precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse -MJAPI void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, - const int* rownnz, const int* rowadr, const int* colind, - const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d, int flg_upper); +MJAPI void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, mjData* d, int flg_upper); // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); -// compute row non-zeros of reverse-Cholesky factor L, return total -MJAPI int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, - int n, mjData* d); +// precount row non-zeros of reverse-Cholesky factor L, return total +MJAPI int mju_cholFactorCount(int* L_rownnz, const int* rownnz, const int* rowadr, + const int* colind, int n, mjData* d); // ------------------------------ inlined functions ------------------------------------------------ diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 37da5155..d1057b22 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -326,8 +326,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { int rowadrH[] = {0, 0, 0}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -371,8 +371,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { int rowadrH[] = {0, 0, 0}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -419,8 +419,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { mjtNum diag[] = {2, 3, 4}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4)); @@ -467,8 +467,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 0, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 2)); @@ -513,8 +513,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -558,8 +558,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { int rowadrH[] = {0, 0, 0}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 3)); @@ -605,8 +605,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { mjtNum diag[] = {2, 3, 4}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 2, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 2, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2)); @@ -651,8 +651,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { mjtNum diag[] = {2, 3}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -698,8 +698,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { mjtNum diag[] = {2, 3, 4}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -746,8 +746,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { mjtNum diag[] = {1, 1, 1}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -794,8 +794,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { mjtNum diag[] = {1, 1, 1}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -842,8 +842,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { mjtNum diag[] = {1, 1, 1}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 4, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 4, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4)); EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12)); @@ -894,8 +894,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { mjtNum diag[] = {1, 1, 1}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 5, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 5, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0, 0, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4, 4, 4)); @@ -944,8 +944,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { int rowadrH[] = {0, 0, 0, 0, 0, 0, 0}; // test precount - mju_sqrMatTDSparseInit(rownnzH, rowadrH, 7, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + mju_sqrMatTDSparseCount(rownnzH, rowadrH, 7, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7)); EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42)); @@ -985,8 +985,8 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int colindA[4]; int rownnzA_factor[2]; mju_dense2sparse(sparseA, matA, nA, nA, rownnzA, rowadrA, colindA, 4); - int nnzA = mju_cholFactorNNZ(rownnzA_factor, - rownnzA, rowadrA, colindA, nA, d); + int nnzA = mju_cholFactorCount(rownnzA_factor, + rownnzA, rowadrA, colindA, nA, d); EXPECT_EQ(nnzA, 2); EXPECT_THAT(AsVector(rownnzA_factor, 2), ElementsAre(1, 1)); @@ -1001,8 +1001,8 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int colindB[9]; int rownnzB_factor[3]; mju_dense2sparse(sparseB, matB, nB, nB, rownnzB, rowadrB, colindB, 9); - int nnzB = mju_cholFactorNNZ(rownnzB_factor, - rownnzB, rowadrB, colindB, nB, d); + int nnzB = mju_cholFactorCount(rownnzB_factor, + rownnzB, rowadrB, colindB, nB, d); EXPECT_EQ(nnzB, 5); EXPECT_THAT(AsVector(rownnzB_factor, 3), ElementsAre(1, 2, 2)); @@ -1017,8 +1017,8 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int colindC[9]; int rownnzC_factor[3]; mju_dense2sparse(sparseC, matC, nC, nC, rownnzC, rowadrC, colindC, 9); - int nnzC = mju_cholFactorNNZ(rownnzC_factor, - rownnzC, rowadrC, colindC, nC, d); + int nnzC = mju_cholFactorCount(rownnzC_factor, + rownnzC, rowadrC, colindC, nC, d); EXPECT_EQ(nnzC, 4); EXPECT_THAT(AsVector(rownnzC_factor, 3), ElementsAre(1, 2, 1)); @@ -1034,8 +1034,8 @@ TEST_F(EngineUtilSparseTest, MjuCholFactorNNZ) { int colindD[16]; int rownnzD_factor[4]; mju_dense2sparse(sparseD, matD, nD, nD, rownnzD, rowadrD, colindD, 16); - int nnzD = mju_cholFactorNNZ(rownnzD_factor, - rownnzD, rowadrD, colindD, nD, d); + int nnzD = mju_cholFactorCount(rownnzD_factor, + rownnzD, rowadrD, colindD, nD, d); EXPECT_EQ(nnzD, 8); EXPECT_THAT(AsVector(rownnzD_factor, 4), ElementsAre(1, 2, 2, 3)); From 1c69e64e6ea71206364e20a23c636b39df341a9f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 7 Jan 2025 04:36:13 -0800 Subject: [PATCH 189/426] Extend test comparing dense and sparse pipelines to all solvers. PiperOrigin-RevId: 712862658 Change-Id: Ieb8515d32a225a098ab96055598a5d56bad7e476 --- test/pipeline_test.cc | 38 ++++++++++++++++++++++---------------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/test/pipeline_test.cc b/test/pipeline_test.cc index fb7c347a..f9c40713 100644 --- a/test/pipeline_test.cc +++ b/test/pipeline_test.cc @@ -31,32 +31,38 @@ static const char* const kDefaultModel = "testdata/model.xml"; using ::testing::Pointwise; using ::testing::DoubleNear; +using ::testing::NotNull; using PipelineTest = MujocoTest; -// Joint and actuator damping should integrate identically under implicit +// sparse and dense pipelines should produce the same results, for all solvers TEST_F(PipelineTest, SparseDenseEquivalent) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); - ASSERT_NE(model, nullptr) << error; + ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); - // set dense jacobian, call mj_forward, save accelerations - model->opt.jacobian = mjJAC_DENSE; - mj_forward(model, data); - std::vector qacc_dense = AsVector(data->qacc, model->nv); - - // set sparse jacobian, call mj_forward, save accelerations - model->opt.jacobian = mjJAC_SPARSE; - mj_forward(model, data); - std::vector qacc_sparse = AsVector(data->qacc, model->nv); - - // expect accelerations to be insignificantly different mjtNum tol = 1e-11; - EXPECT_THAT(qacc_dense, Pointwise(DoubleNear(tol), qacc_sparse)); - // TODO: is 1e-12 larger than we expect? - // investigate sources of discrepancy, eliminate if possible + + for (mjtSolver solver : {mjSOL_NEWTON, mjSOL_PGS, mjSOL_CG}) { + model->opt.solver = solver; + + // set dense jacobian, call mj_forward, save accelerations + model->opt.jacobian = mjJAC_DENSE; + mj_resetDataKeyframe(model, data, 0); + mj_forward(model, data); + std::vector qacc_dense = AsVector(data->qacc, model->nv); + + // set sparse jacobian, call mj_forward, save accelerations + model->opt.jacobian = mjJAC_SPARSE; + mj_resetDataKeyframe(model, data, 0); + mj_forward(model, data); + std::vector qacc_sparse = AsVector(data->qacc, model->nv); + + // expect accelerations to be insignificantly different + EXPECT_THAT(qacc_dense, Pointwise(DoubleNear(tol), qacc_sparse)); + } mj_deleteData(data); mj_deleteModel(model); From 4d82ab57626f2036e6b7b928628456264f88fea7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 7 Jan 2025 04:41:09 -0800 Subject: [PATCH 190/426] Add function to merge chains of indices. PiperOrigin-RevId: 712863690 Change-Id: I15652bec03dc9ce90e230788bd12b44b1a2b8217 --- src/engine/engine_util_sparse.c | 11 ----- src/engine/engine_util_sparse.h | 65 ++++++++++++++++++++++++++ test/engine/engine_util_sparse_test.cc | 18 +++++++ 3 files changed, 83 insertions(+), 11 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index e4e1c1c9..6fbb0ff4 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -206,17 +206,6 @@ static void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum -// return 1 if vec1==vec2, 0 otherwise -static int mju_compare(const int* vec1, const int* vec2, int n) { -#ifdef mjUSEAVX - return mju_compare_avx(vec1, vec2, n); -#else - return !memcmp(vec1, vec2, n*sizeof(int)); -#endif // mjUSEAVX -} - - - // count the number of non-zeros in the sum of two sparse vectors int mju_combineSparseCount(int a_nnz, int b_nnz, const int* a_ind, const int* b_ind) { int a = 0, b = 0, c_nnz = 0; diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 04c37610..34bc777f 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_UTIL_SPARSE_H_ #define MUJOCO_SRC_ENGINE_ENGINE_UTIL_SPARSE_H_ +#include + #include #include #include @@ -162,6 +164,69 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int #endif // mjUSEAVX } +// return 1 if vec1==vec2, 0 otherwise +static inline +int mju_compare(const int* vec1, const int* vec2, int n) { +#ifdef mjUSEAVX + return mju_compare_avx(vec1, vec2, n); +#else + return !memcmp(vec1, vec2, n*sizeof(int)); +#endif // mjUSEAVX +} + + +// merge unique sorted integers, merge array must be large enough (not checked for) +static inline +int mj_mergeSorted(int* merge, const int* chain1, int n1, const int* chain2, int n2) { + // special case: one or both empty + if (n1 == 0) { + if (n2 == 0) { + return 0; + } + memcpy(merge, chain2, n2 * sizeof(int)); + return n2; + } else if (n2 == 0) { + memcpy(merge, chain1, n1 * sizeof(int)); + return n1; + } + + // special case: identical pattern + if (n1 == n2 && mju_compare(chain1, chain2, n1)) { + memcpy(merge, chain1, n1 * sizeof(int)); + return n1; + } + + // merge while both chains are non-empty + int i = 0, j = 0, k = 0; + while (i < n1 && j < n2) { + int c1 = chain1[i]; + int c2 = chain2[j]; + + if (c1 < c2) { + merge[k++] = c1; + i++; + } else if (c1 > c2) { + merge[k++] = c2; + j++; + } else { // c1 == c2 + merge[k++] = c1; + i++; + j++; + } + } + + // copy remaining + if (i < n1) { + memcpy(merge + k, chain1 + i, (n1 - i)*sizeof(int)); + k += n1 - i; + } else if (j < n2) { + memcpy(merge + k, chain2 + j, (n2 - j)*sizeof(int)); + k += n2 - j; + } + + return k; +} + #ifdef __cplusplus } diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index d1057b22..20a543df 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -1100,5 +1100,23 @@ TEST_F(EngineUtilSparseTest, MjuDenseToSparse) { EXPECT_EQ(status0, 1); } +TEST_F(EngineUtilSparseTest, MergeSorted) { + const int chain1_a[] = {1, 2, 3}; + const int chain2_a[] = {}; + int merged_a[3]; + int n1 = 3; + int n2 = 0; + EXPECT_EQ(mj_mergeSorted(merged_a, chain1_a, n1, chain2_a, n2), 3); + EXPECT_THAT(merged_a, ElementsAre(1, 2, 3)); + + const int chain1_b[] = {1, 3, 5, 7, 8}; + const int chain2_b[] = {2, 4, 5, 6, 8}; + int merged_b[8]; + n1 = 5; + n2 = 5; + EXPECT_EQ(mj_mergeSorted(merged_b, chain1_b, n1, chain2_b, n2), 8); + EXPECT_THAT(merged_b, ElementsAre(1, 2, 3, 4, 5, 6, 7, 8)); +} + } // namespace } // namespace mujoco From df1d15fa38a6b532844cf26b6dcb4f4ccf1d2e59 Mon Sep 17 00:00:00 2001 From: Yuxuan Jiang Date: Wed, 8 Jan 2025 09:53:36 +0800 Subject: [PATCH 191/426] Fix typo in modeling.rst --- doc/modeling.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/modeling.rst b/doc/modeling.rst index fdf6017f..78a5d336 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1607,7 +1607,7 @@ dedicated section :ref:`therein`. :ref:`Numerical Integration` section. The default recommended choice is the ``implicitfast`` integrator. 3. :ref:`Constraint Jacobians`: Try switching the Jacobian setting between "dense" and "sparse". These - two options use seperate code paths using dense or sparse algebra, but are otherwise compationally identical, so the + two options use seperate code paths using dense or sparse algebra, but are otherwise computationally identical, so the faster one is always preferred. The default "auto" heuristic does not always make the right choice. 4. **Constraint solver:** If the profiler reports that a large chunk of time is spent in the solver, consider the following: From 8a5f0920816ece01bba7b23265db8927a1c77845 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 8 Jan 2025 03:30:41 -0800 Subject: [PATCH 192/426] Allow CSR back-substitution to handle multiple vectors. PiperOrigin-RevId: 713229673 Change-Id: I7a5b43fe966cf9e482bd41e2eea6c30dd3ffa1d4 --- src/engine/engine_core_smooth.c | 97 ++++++++++++++++++------ src/engine/engine_core_smooth.h | 4 +- test/benchmark/inertia_benchmark_test.cc | 2 +- test/benchmark/solveLD_benchmark_test.cc | 2 +- test/engine/engine_core_smooth_test.cc | 40 +++++++++- 5 files changed, 116 insertions(+), 29 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 81e276f6..a3e255c7 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1607,39 +1607,88 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x // like mj_solveLD, but using the CSR representation of L -void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, +void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { - // x <- L^-T x - for (int i=nv-1; i > 0; i--) { - // skip diagonal (simple) rows, exploit sparsity of input vector - if (diagnum[i] || x[i] == 0) { - continue; + // single vector + if (n == 1) { + // x <- L^-T x + for (int i=nv-1; i > 0; i--) { + // skip diagonal rows, zero elements in input vector + mjtNum x_i = x[i]; + if (x_i == 0 || diagnum[i]) { + continue; + } + + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int adr=start; adr < end; adr++) { + x[colind[adr]] -= qLDs[adr] * x_i; + } } - int d = rownnz[i] - 1; - int adr_i = rowadr[i]; - mjtNum x_i = x[i]; - for (int j=0; j < d; j++) { - int adr = adr_i + j; - x[colind[adr]] -= qLDs[adr] * x_i; + // x <- D^-1 x + for (int i=0; i < nv; i++) { + x[i] *= qLDiagInv[i]; + } + + // x <- L^-1 x + for (int i=1; i < nv; i++) { + // skip diagonal rows + if (diagnum[i]) { + i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i + continue; + } + + int adr = rowadr[i]; + x[i] -= mju_dotSparse(qLDs+adr, x, rownnz[i] - 1, colind+adr, /*flg_unc1=*/0); } } - // x(i) /= D(i,i) - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; - } + // multiple vectors + else { + // x <- L^-T x + for (int i=nv-1; i > 0; i--) { + // skip diagonal rows + if (diagnum[i]) { + continue; + } - // x <- L^-1 x - for (int i=1; i < nv; i++) { - // skip diagonal (simple) rows - if (diagnum[i]) { - i += diagnum[i] - 1; // when iterating forward we can skip ahead - continue; + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int adr=start; adr < end; adr++) { + int j = colind[adr]; + mjtNum val = qLDs[adr]; + for (int offset=0; offset < n*nv; offset+=nv) { + mjtNum x_i; + if ((x_i = x[i+offset])) { + x[j+offset] -= val * x_i; + } + } + } } - int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, rownnz[i] - 1, colind+adr, /*flg_unc1=*/0); + // x <- D^-1 x + for (int i=0; i < nv; i++) { + mjtNum invD_i = qLDiagInv[i]; + for (int offset=0; offset < n*nv; offset+=nv) { + x[i+offset] *= invD_i; + } + } + + // x <- L^-1 x + for (int i=1; i < nv; i++) { + // skip diagonal rows + if (diagnum[i]) { + i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i + continue; + } + + int adr = rowadr[i]; + int d = rownnz[i] - 1; + for (int offset=0; offset < n*nv; offset+=nv) { + x[i+offset] -= mju_dotSparse(qLDs+adr, x+offset, d, colind+adr, /*flg_unc1=*/0); + } + } } } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index f4733b08..a710416c 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -64,8 +64,8 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, const mjtNum* qLD, const mjtNum* qLDiagInv); // in-place sparse backsubstitution: x = inv(L'*D*L)*x -// like mj_solveLD, but using the CSR representation of L -MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, +// handle n vectors at once +MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc index 8ba580b7..7ddc3468 100644 --- a/test/benchmark/inertia_benchmark_test.cc +++ b/test/benchmark/inertia_benchmark_test.cc @@ -72,7 +72,7 @@ static void BM_solve(benchmark::State& state, SolveType type) { mj_factorIs(LDs, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); mju_copy(res, vec, m->nv); - mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, + mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, 1, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index f2dc39e1..18ebe69b 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -63,7 +63,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { mj_solveM(m, d, res, vec, 1); } else { mju_copy(res, vec, m->nv); - mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, + mj_solveLDs(res, LDs, d->qLDiagInv, m->nv, 1, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 717fde98..e5f66c38 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -495,7 +495,7 @@ TEST_F(CoreSmoothTest, SolveLDs) { for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); - mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, + mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, 1, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision @@ -507,6 +507,44 @@ TEST_F(CoreSmoothTest, SolveLDs) { mj_deleteModel(m); } +TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; + + mjData* d = mj_makeData(m); + mj_forward(m, d); + + int nv = m->nv; + int nC = m->nC; + + // copy LD into LDs: CSR format + vector LDs(nC); + for (int i=0; i < nC; i++) { + LDs[i] = d->qLD[d->mapM2C[i]]; + } + + // compare n LD and LDs vector solve + int n = 3; + vector vec(nv*n); + vector vec2(nv*n); + for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i; + for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0; + + mj_solveLD(m, vec.data(), n, d->qLD, d->qLDiagInv); + mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, n, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + + // expect vectors to match up to floating point precision + for (int i=0; i < nv*n; i++) { + EXPECT_FLOAT_EQ(vec[i], vec2[i]); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + TEST_F(CoreSmoothTest, FactorIs) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; From 661c3e01dc5ec73b63a32e44eee6c6b53ea48f56 Mon Sep 17 00:00:00 2001 From: Alessandro Croci <57228872+xela-95@users.noreply.github.com> Date: Wed, 8 Jan 2025 14:21:24 +0100 Subject: [PATCH 193/426] Update simulation.rst Add missing backticks to `mjData.mocap_quat` in simulation.rst --- doc/programming/simulation.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index d892ea03..da93d588 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -267,7 +267,7 @@ control laws obtained from trajectory optimizers would normally be indexed by `` The reason for the "official" caveat above is because user callbacks may store additional state variables that change over time and affect the callback outputs; indeed the field ``mjData.userdata`` exists mostly for that purpose. Other state-like quantities that are part of mjData and are treated as inputs by forward dynamics are ``mjData.mocap_pos`` and -mjData.mocap_quat. These quantities are unusual in that they are meant to change at each time step (normally driven by a +``mjData.mocap_quat``. These quantities are unusual in that they are meant to change at each time step (normally driven by a motion capture device), however this change is implemented by the user, while the simulator treats them as constants. In that sense they are no different from all the constants in mjModel, or the function callback pointers set by the user: such constants affect the computation, but are not part of the state vector of a dynamical system. From 7a6bf06706dabc219bf417a9ddcf89d8cbaeede2 Mon Sep 17 00:00:00 2001 From: Alessandro Croci <57228872+xela-95@users.noreply.github.com> Date: Wed, 8 Jan 2025 14:27:46 +0100 Subject: [PATCH 194/426] Update simulation.rst Add missing backticks to `mjData.xfrc_applied` --- doc/programming/simulation.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index da93d588..05bf4d67 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -286,7 +286,7 @@ Next we turn to the controls and applied forces. The control vector in MuJoCo is u = (mjData.ctrl, mjData.qfrc_applied, mjData.xfrc_applied) These quantities specify control signals (``mjData.ctrl``) for the actuators defined in the model, or directly apply -forces and torques specified in joint space (``mjData.qfrc_applied``) or in Cartesian space (mjData.xfrc_applied). +forces and torques specified in joint space (``mjData.qfrc_applied``) or in Cartesian space (``mjData.xfrc_applied``). Finally, calling mj_forward which corresponds to the abstract dynamics function ``f(t,x,u)`` computes the time-derivative of the state vector. The corresponding fields of mjData are From 6654d63438f1359975fc3e861047fcd01826e378 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 8 Jan 2025 05:42:59 -0800 Subject: [PATCH 195/426] Add spec lookup in the attached spec array using the compiler option pointer. This enables to find the spec associated with the compiler option stored in the objects, which do not necessarily belong to the model that owns them. PiperOrigin-RevId: 713259419 Change-Id: I579c770147ff07ed2a5edd20e7a795ee7cc27147 --- doc/programming/modeledit.rst | 7 +++ src/user/user_model.cc | 27 +++++++++- src/user/user_model.h | 7 +++ src/user/user_objects.cc | 6 +-- test/user/user_api_test.cc | 87 ++++++++++++++++++------------ test/xml/xml_native_reader_test.cc | 66 +++++++++++++++++++++++ 6 files changed, 161 insertions(+), 39 deletions(-) diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index 9c95e7c5..eb65b6e7 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -117,6 +117,8 @@ to :ref:`attach a body to a site`: mjSpec* parent = mj_makeSpec(); mjSpec* child = mj_makeSpec(); + parent->compiler.degree = 0; + child->compiler.degree = 1; mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), NULL); mjsSite* site = mjs_addSite(mjs_findBody(parent, "world"), NULL); mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), NULL); @@ -133,6 +135,11 @@ or :ref:`attach a frame to a body`: mjsFrame* frame = mjs_addFrame(mjs_findBody(child, "world"), NULL); mjsFrame* attached_frame = mjs_attachFrame(body, frame, "attached-", "-1"); +Note that in the above examples, the parent and child models have different values for ``compiler.degree``, +corresponding to the :ref:`compiler/angle` attribute, specifying the units in which angles are +interperted. Compiler options are carried over during attachment, so the child model will be compiled using X, while the +parent will be compiled using Y. + .. _meDefault: Default classes diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7359c1f0..57b6a7da 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -146,6 +146,9 @@ mjCModel::mjCModel() { defaults_.push_back(new mjCDef); defaults_.back()->name = "main"; + // point to model from spec + PointToLocal(); + // world body mjCBody* world = new mjCBody(this); mjuu_zerovec(world->pos, 3); @@ -163,14 +166,15 @@ mjCModel::mjCModel() { // create mjCBase lists from children lists CreateObjectLists(); - // point to model from spec - PointToLocal(); + // the source spec is the model itself, overwritten in the copy constructor + source_spec_ = &spec; } mjCModel::mjCModel(const mjCModel& other) { CreateObjectLists(); + source_spec_ = (mjSpec*)&other.spec; *this = other; } @@ -1222,6 +1226,25 @@ mjSpec* mjCModel::FindSpec(std::string name) const { +// find spec by mjsCompiler pointer +mjSpec* mjCModel::FindSpec(const mjsCompiler* compiler_) const { + for (auto spec : specs_) { + if (&(static_cast(spec->element)->GetSourceSpec()->compiler) == compiler_) { + return spec; + } + } + return nullptr; +} + + + +// get the spec from which this model was created +mjSpec* mjCModel::GetSourceSpec() const { + return source_spec_; +} + + + //------------------------------- COMPILER PHASES -------------------------------------------------- // make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights diff --git a/src/user/user_model.h b/src/user/user_model.h index 1b248ea2..9109018c 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -236,6 +236,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name mjCBase* FindTree(mjCBody* body, mjtObj type, std::string name); // find tree object given name mjSpec* FindSpec(std::string name) const; // find spec given name + mjSpec* FindSpec(const mjsCompiler* compiler_) const; // find spec given mjsCompiler void ActivatePlugin(const mjpPlugin* plugin, int slot); // activate plugin // accessors @@ -303,10 +304,16 @@ class mjCModel : public mjCModel_, private mjSpec { // map from default class name to default class pointer std::unordered_map def_map; + // get the spec from which this model was created + mjSpec* GetSourceSpec() const; + private: // settings for each defaults class std::vector defaults_; + // spec from which this model was created in copy constructor + mjSpec* source_spec_; + // list of active plugins std::vector> active_plugins_; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 9654182c..c96872f5 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -798,7 +798,7 @@ mjCBody::mjCBody(mjCModel* _model) { mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) { model = _model; - mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); + mjSpec* origin = model->FindSpec(other.compiler); compiler = origin ? &origin->compiler : &model->spec.compiler; *this = other; CopyPlugin(); @@ -881,7 +881,7 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { } // copy input frame - mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); + mjSpec* origin = model->FindSpec(other.compiler); frames.push_back(new mjCFrame(other)); frames.back()->body = this; frames.back()->model = model; @@ -947,7 +947,7 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, if (pframe && !pframe->IsAncestor(src[i]->frame)) { continue; // skip if the element is not inside pframe } - mjSpec* origin = model->FindSpec(mjs_getString(src[i]->model->spec.modelname)); + mjSpec* origin = model->FindSpec(src[i]->compiler); dst.push_back(new T(*src[i])); dst.back()->body = this; dst.back()->model = model; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e641db58..9a81a9b9 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -2033,52 +2033,71 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { } TEST_F(MujocoTest, DifferentUnitsAllowed) { - mjSpec* child = mj_makeSpec(); - child->compiler.degree = 0; - mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0); - body->alt.type = mjORIENTATION_EULER; - body->alt.euler[0] = -mjPI / 2; - mjsGeom* geom = mjs_addGeom(body, 0); - geom->size[0] = 1; - mjsJoint* joint = mjs_addJoint(body, 0); - joint->type = mjJNT_HINGE; - joint->range[0] = -mjPI / 4; - joint->range[1] = mjPI / 4; + static constexpr char child_xml[] = R"( + + - mjSpec* parent = mj_makeSpec(); - parent->compiler.degree = 1; - mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0); - frame->alt.type = mjORIENTATION_EULER; - frame->alt.euler[0] = 90; + + + + + + + + )"; - EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), NotNull()); - mjModel* model = mj_compile(parent, 0); + static constexpr char parent_xml[] = R"( + + + + + + + + + + )"; + + std::array error; + mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size()); + mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size()); + ASSERT_THAT(spec, NotNull()) << error.data(); + mjs_attachBody(mjs_findFrame(spec, "frame"), mjs_findBody(child, "child"), + "child_", ""); + + mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->njnt, 2); + EXPECT_NEAR(model->jnt_range[0], -mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[1], mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[2], -mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[3], mjPI, 1e-6); EXPECT_NEAR(model->body_quat[4], 1, 1e-12); EXPECT_NEAR(model->body_quat[5], 0, 1e-12); EXPECT_NEAR(model->body_quat[6], 0, 1e-12); EXPECT_NEAR(model->body_quat[7], 0, 1e-12); - EXPECT_NEAR(model->jnt_range[0], -mjPI / 4, 1e-7); - EXPECT_NEAR(model->jnt_range[1], mjPI / 4, 1e-7); - mjSpec* copy = mj_copySpec(parent); - EXPECT_THAT(copy, NotNull()); - mj_deleteModel(model); + mjSpec* copied_spec = mj_copySpec(spec); + ASSERT_THAT(copied_spec, NotNull()); mj_deleteSpec(child); - mj_deleteSpec(parent); + mj_deleteSpec(spec); + mj_deleteModel(model); // check that deleting `parent` or `child` does not invalidate the copy - mjModel* copy_model = mj_compile(copy, 0); - EXPECT_THAT(copy_model, NotNull()); - EXPECT_NEAR(copy_model->body_quat[0], 1, 1e-12); - EXPECT_NEAR(copy_model->body_quat[1], 0, 1e-12); - EXPECT_NEAR(copy_model->body_quat[2], 0, 1e-12); - EXPECT_NEAR(copy_model->body_quat[3], 0, 1e-12); - EXPECT_NEAR(copy_model->jnt_range[0], -mjPI / 4, 1e-7); - EXPECT_NEAR(copy_model->jnt_range[1], mjPI / 4, 1e-7); + mjModel* copied_model = mj_compile(copied_spec, 0); + EXPECT_THAT(copied_model, NotNull()); + EXPECT_THAT(copied_model->njnt, 2); + EXPECT_NEAR(copied_model->jnt_range[0], -mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[1], mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[2], -mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[3], mjPI, 1e-6); + EXPECT_NEAR(copied_model->body_quat[4], 1, 1e-12); + EXPECT_NEAR(copied_model->body_quat[5], 0, 1e-12); + EXPECT_NEAR(copied_model->body_quat[6], 0, 1e-12); + EXPECT_NEAR(copied_model->body_quat[7], 0, 1e-12); - mj_deleteModel(copy_model); - mj_deleteSpec(copy); + mj_deleteSpec(copied_spec); + mj_deleteModel(copied_model); } TEST_F(MujocoTest, CopyAttachedSpec) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 219ef5e1..c7509183 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1579,6 +1579,72 @@ TEST_F(XMLReaderTest, InvalidAttach) { mj_deleteVFS(vfs.get()); } +TEST_F(XMLReaderTest, LookupCompilerOptionWithoutSpecCopy) { + static constexpr char child_xml[] = R"( + + + + + + + + + + + )"; + + static constexpr char parent_xml[] = R"( + + + + + + + + + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "child.xml", child_xml, sizeof(child_xml)); + mj_addBufferVFS(vfs.get(), "parent.xml", parent_xml, sizeof(parent_xml)); + + std::array error; + auto* spec = mj_parseXMLString(parent_xml, vfs.get(), error.data(), + error.size()); + ASSERT_THAT(spec, NotNull()) << error.data(); + + mjModel* model = mj_compile(spec, vfs.get()); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->njnt, 2); + EXPECT_NEAR(model->jnt_range[0], -3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[1], 3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[2], -3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[3], 3.14159, 1e-5); + + mjSpec* copied_spec = mj_copySpec(spec); + ASSERT_THAT(copied_spec, NotNull()); + mjModel* copied_model = mj_compile(copied_spec, vfs.get()); + EXPECT_THAT(copied_model, NotNull()); + EXPECT_THAT(copied_model->njnt, 2); + EXPECT_NEAR(copied_model->jnt_range[0], -3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[1], 3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[2], -3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[3], 3.14159, 1e-5); + + mj_deleteSpec(spec); + mj_deleteSpec(copied_spec); + mj_deleteModel(model); + mj_deleteModel(copied_model); + + mj_deleteVFS(vfs.get()); +} + // ----------------------- test camera parsing --------------------------------- TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { From f912e8df16de61977e88cb4170654801dee037e3 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 8 Jan 2025 08:11:54 -0800 Subject: [PATCH 196/426] Print sparsity structure of `mjData.efc_AR` PiperOrigin-RevId: 713296073 Change-Id: Ifede6ea66dd1177f5e85c8498860decebaa65f06 --- src/engine/engine_print.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 9f6b6e33..7dc50712 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1272,10 +1272,14 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, d->efc_J_rowadr, d->efc_J_colind, fp, float_format); mj_printSparsity("JT: constraint Jacobian transposed", m->nv, d->nefc, d->efc_JT_rowadr, NULL, d->efc_JT_rownnz, d->efc_JT_rowsuper, d->efc_JT_colind, fp); - printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); - printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); - printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz, - d->efc_AR_rowadr, d->efc_AR_colind, fp, float_format); + if (mj_isDual(m)) { + printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); + printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); + printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz, + d->efc_AR_rowadr, d->efc_AR_colind, fp, float_format); + mj_printSparsity("efc_AR: inverse constraint inertia", d->nefc, d->nefc, d->efc_AR_rowadr, + NULL, d->efc_AR_rownnz, NULL, d->efc_AR_colind, fp); + } } printArray("EFC_POS", d->nefc, 1, d->efc_pos, fp, float_format); From 357ea024c00d6f918d119b0fce68aa455330c8aa Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Wed, 8 Jan 2025 14:30:56 -0800 Subject: [PATCH 197/426] Remove unconditional `mj_markStack` in `mjv_addGeoms`. Prior to commit 3f855f32d9b14179e17812056732d6809f0b4320, calling `mjv_updateScene` and `mjv_updateSceneFromState` on an "empty" `mjData` (one without a buffer but where all sizes are also zero) was a valid operation. The `mj_markStack` call requires free stack space, so a call to `mjv_addGeoms` on an empty `mjData` results in a stack overflow whenever `mjVIS_TENDON` is enabled. Fixes #2305. PiperOrigin-RevId: 713418586 Change-Id: I99b5de23fe94ba9aa86eeef0700c891976c9aecc --- src/engine/engine_vis_visualize.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index d0da232a..abcf8f34 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1728,7 +1728,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // spatial tendons objtype = mjOBJ_TENDON; category = mjCAT_DYNAMIC; - if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { + if (vopt->flags[mjVIS_TENDON] && (category & catmask) && m->ntendon) { // mark actuated tendons mj_markStack(d); int* tendon_actuated = mjSTACKALLOC(d, m->ntendon, int); From 40ef08c8edbac44264aa80a6aaa11eb2b7c52a8a Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Wed, 8 Jan 2025 15:47:15 -0800 Subject: [PATCH 198/426] Don't pollute the global namespace when using `mjpython`. Fixes #2265 PiperOrigin-RevId: 713442833 Change-Id: If155d9f57d4deef8b848478ebd8077a00417d5c5 --- doc/changelog.rst | 1 + python/mujoco/mjpython/mjpython.mm | 243 ++++++++++++++++------------- 2 files changed, 136 insertions(+), 108 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index aae6bdb5..a32b8004 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -12,6 +12,7 @@ Python bindings the computation. The thread pool can be reused across calls, but then the function cannot be called simultaneously from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. +- Fix global namespace pollution when using ``mjpython`` (:github:issue:`2265`). General ^^^^^^^ diff --git a/python/mujoco/mjpython/mjpython.mm b/python/mujoco/mjpython/mjpython.mm index e2b5d92d..f7e159c1 100644 --- a/python/mujoco/mjpython/mjpython.mm +++ b/python/mujoco/mjpython/mjpython.mm @@ -86,95 +86,113 @@ void* mjpython_pymain(void* vargs) { // Set up the condition variable to pass control back to the macOS main thread. gil = cpython.PyGILState_Ensure(); - cpython.PyRun_SimpleStringFlags("import threading; cond = threading.Condition()", nullptr); + cpython.PyRun_SimpleStringFlags(R"( +def _mjpython_make_cond(): + # Don't pollute the global namespace. + global _mjpython_make_cond + del _mjpython_make_cond + + import threading + + global cond + cond = threading.Condition() + +_mjpython_make_cond() +)", nullptr); py_initialized.store(true); // Wait until GLFW is initialized on macOS main thread, set up the queue and an atexit hook // to enqueue a termination flag upon exit. cpython.PyRun_SimpleStringFlags(R"( -import atexit +def _mjpython_init(): + # Don't pollute the global namespace. + global _mjpython_init + del _mjpython_init -# The mujoco.viewer module should only be imported here after glfw.init() in the macOS main thread. -with cond: - cond.wait() -import mujoco.viewer + import atexit + import threading -# Similar to a queue.Queue(maxsize=1), but where only one active task is allowed at a time. -# With queue.Queue(1), another item is allowed to be enqueued before task_done is called. -class _MjPythonImpl(mujoco.viewer._MjPythonBase): + # The mujoco.viewer module should only be imported after glfw.init() in the macOS main thread. + with cond: + cond.wait() + import mujoco.viewer - # Termination statuses - NOT_TERMINATED = 0 - TERMINATION_REQUESTED = 1 - TERMINATION_ACCEPTED = 2 - TERMINATED = 3 + # Similar to a queue.Queue(maxsize=1), but where only one active task is allowed at a time. + # With queue.Queue(1), another item is allowed to be enqueued before task_done is called. + class _MjPythonImpl(mujoco.viewer._MjPythonBase): - def __init__(self): - self._cond = threading.Condition() - self._task = None - self._termination = self.__class__.NOT_TERMINATED - self._busy = False + # Termination statuses + NOT_TERMINATED = 0 + TERMINATION_REQUESTED = 1 + TERMINATION_ACCEPTED = 2 + TERMINATED = 3 - def launch_on_ui_thread( - self, - model, - data, - handle_return, - key_callback, - show_left_ui, - show_right_ui, - ): - with self._cond: - if self._busy or self._task is not None: - raise RuntimeError('another MuJoCo viewer is already open') - else: - self._task = ( - model, - data, - handle_return, - key_callback, - show_left_ui, - show_right_ui, - ) + def __init__(self): + self._cond = threading.Condition() + self._task = None + self._termination = self.__class__.NOT_TERMINATED + self._busy = False + + def launch_on_ui_thread( + self, + model, + data, + handle_return, + key_callback, + show_left_ui, + show_right_ui, + ): + with self._cond: + if self._busy or self._task is not None: + raise RuntimeError('another MuJoCo viewer is already open') + else: + self._task = ( + model, + data, + handle_return, + key_callback, + show_left_ui, + show_right_ui, + ) + self._cond.notify() + + def terminate(self): + with self._cond: + self._termination = self.__class__.TERMINATION_REQUESTED + self._cond.notify() + self._cond.wait_for( + lambda: self._termination == self.__class__.TERMINATED) + + def get(self): + with self._cond: + self._cond.wait_for( + lambda: self._task is not None or self._termination) + + if self._termination: + if self._termination == self.__class__.TERMINATION_REQUESTED: + self._termination = self.__class__.TERMINATION_ACCEPTED + return None + + task = self._task + self._busy = True + self._task = None + return task + + def done(self): + with self._cond: + self._busy = False + if self._termination == self.__class__.TERMINATION_ACCEPTED: + self._termination = self.__class__.TERMINATED self._cond.notify() - def terminate(self): - with self._cond: - self._termination = self.__class__.TERMINATION_REQUESTED - self._cond.notify() - self._cond.wait_for( - lambda: self._termination == self.__class__.TERMINATED) - def get(self): - with self._cond: - self._cond.wait_for( - lambda: self._task is not None or self._termination) + mujoco.viewer._MJPYTHON = _MjPythonImpl() + atexit.register(mujoco.viewer._MJPYTHON.terminate) - if self._termination: - if self._termination == self.__class__.TERMINATION_REQUESTED: - self._termination = self.__class__.TERMINATION_ACCEPTED - return None + with cond: + cond.notify() - task = self._task - self._busy = True - self._task = None - return task - - def done(self): - with self._cond: - self._busy = False - if self._termination == self.__class__.TERMINATION_ACCEPTED: - self._termination = self.__class__.TERMINATED - self._cond.notify() - - -mujoco.viewer._MJPYTHON = _MjPythonImpl() -atexit.register(mujoco.viewer._MJPYTHON.terminate) -del _MjPythonImpl # Don't pollute globals for user script. - -with cond: - cond.notify() -del cond # Don't pollute globals for user script. +_mjpython_init() )", nullptr); // Run the Python interpreter main loop. @@ -283,47 +301,56 @@ int main(int argc, char** argv) { // to finish setting up _MJPYTHON, then serve incoming viewer launch requests. PyGILState_STATE gil = cpython.PyGILState_Ensure(); cpython.PyRun_SimpleStringFlags(R"( -import ctypes +def _mjpython_main(): + # Don't pollute the global namespace. + global _mjpython_main + del _mjpython_main -# GLFW must be initialized on the OS main thread (i.e. here). -import glfw -import mujoco.viewer + import ctypes -glfw.init() -glfw.poll_events() -ctypes.CDLL(None).mjpython_hide_dock_icon() + # GLFW must be initialized on the OS main thread (i.e. here). + import glfw + import mujoco.viewer -# Wait for Python main thread to finish setting up _MJPYTHON -with cond: - cond.notify() - cond.wait() + glfw.init() + glfw.poll_events() + ctypes.CDLL(None).mjpython_hide_dock_icon() -while True: - try: - # Wait for an incoming payload. - task = mujoco.viewer._MJPYTHON.get() + # Wait for Python main thread to finish setting up _MJPYTHON + global cond + with cond: + cond.notify() + cond.wait() + del cond - # None means that we are exiting. - if task is None: - glfw.terminate() - break + while True: + try: + # Wait for an incoming payload. + task = mujoco.viewer._MJPYTHON.get() - # Otherwise, launch the viewer. - model, data, handle_return, key_callback, show_left_ui, show_right_ui = task - ctypes.CDLL(None).mjpython_show_dock_icon() - mujoco.viewer._launch_internal( - model, - data, - run_physics_thread=False, - handle_return=handle_return, - key_callback=key_callback, - show_left_ui=show_left_ui, - show_right_ui=show_right_ui, - ) - ctypes.CDLL(None).mjpython_hide_dock_icon() + # None means that we are exiting. + if task is None: + glfw.terminate() + break - finally: - mujoco.viewer._MJPYTHON.done() + # Otherwise, launch the viewer. + model, data, handle_return, key_callback, show_left_ui, show_right_ui = task + ctypes.CDLL(None).mjpython_show_dock_icon() + mujoco.viewer._launch_internal( + model, + data, + run_physics_thread=False, + handle_return=handle_return, + key_callback=key_callback, + show_left_ui=show_left_ui, + show_right_ui=show_right_ui, + ) + ctypes.CDLL(None).mjpython_hide_dock_icon() + + finally: + mujoco.viewer._MJPYTHON.done() + +_mjpython_main() )", nullptr); cpython.PyGILState_Release(gil); From f7d38dac621cf098622fefcca762c4eccf2f3cbc Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 9 Jan 2025 04:55:19 -0800 Subject: [PATCH 199/426] Add recursive call to FindSpec for handling nested attachments. PiperOrigin-RevId: 713629578 Change-Id: I23efa5a86bac187fc42eb955d29cc03cbacfd9e0 --- src/user/user_mesh.cc | 3 +++ src/user/user_model.cc | 12 ++++++++---- test/user/user_api_test.cc | 31 ++++++++++++++++++++++++++++--- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index f92f8c66..4de4e2e3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -151,6 +151,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; + if (_model) compiler = &_model->spec.compiler; classname = (_def ? _def->name : (_model ? "main" : "")); // in case this body is not compiled @@ -1981,6 +1982,7 @@ mjCSkin::mjCSkin(mjCModel* _model) { // set model pointer model = _model; + if (model) compiler = &model->spec.compiler; // clear data spec_file_.clear(); @@ -2638,6 +2640,7 @@ mjCFlex::mjCFlex(mjCModel* _model) { // set model model = _model; + if (_model) compiler = &_model->spec.compiler; // clear internal variables nvert = 0; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 57b6a7da..55fcb0d8 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -236,7 +236,7 @@ void mjCModel::CopyList(std::vector& dest, } // copy the element from the other model to this model source[i]->ForgetKeyframes(); - mjSpec* origin = FindSpec(mjs_getString(source[i]->model->spec.modelname)); + mjSpec* origin = FindSpec(source[i]->compiler); dest.push_back(candidate); dest.back()->model = this; dest.back()->compiler = origin ? &origin->compiler : &spec.compiler; @@ -1228,9 +1228,13 @@ mjSpec* mjCModel::FindSpec(std::string name) const { // find spec by mjsCompiler pointer mjSpec* mjCModel::FindSpec(const mjsCompiler* compiler_) const { - for (auto spec : specs_) { - if (&(static_cast(spec->element)->GetSourceSpec()->compiler) == compiler_) { - return spec; + if (&GetSourceSpec()->compiler == compiler_) { + return (mjSpec*)&spec; + } + for (auto s : specs_) { + mjSpec* source = static_cast(s->element)->FindSpec(compiler_); + if (source) { + return source; } } return nullptr; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 9a81a9b9..7394c7f1 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -2033,6 +2033,19 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { } TEST_F(MujocoTest, DifferentUnitsAllowed) { + static constexpr char gchild_xml[] = R"( + + + + + + + + + + + )"; + static constexpr char child_xml[] = R"( @@ -2041,6 +2054,7 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { + @@ -2059,19 +2073,27 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { )"; std::array error; + mjSpec* gchild = + mj_parseXMLString(gchild_xml, 0, error.data(), error.size()); mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size()); mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size()); ASSERT_THAT(spec, NotNull()) << error.data(); - mjs_attachBody(mjs_findFrame(spec, "frame"), mjs_findBody(child, "child"), + mjs_attachBody(mjs_findFrame(child, "frame"), + mjs_findBody(gchild, "gchild"), + "gchild_", ""); + mjs_attachBody(mjs_findFrame(spec, "frame"), + mjs_findBody(child, "child"), "child_", ""); mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); - EXPECT_THAT(model->njnt, 2); + EXPECT_THAT(model->njnt, 3); EXPECT_NEAR(model->jnt_range[0], -mjPI, 1e-6); EXPECT_NEAR(model->jnt_range[1], mjPI, 1e-6); EXPECT_NEAR(model->jnt_range[2], -mjPI, 1e-6); EXPECT_NEAR(model->jnt_range[3], mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[4], -mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[5], mjPI, 1e-6); EXPECT_NEAR(model->body_quat[4], 1, 1e-12); EXPECT_NEAR(model->body_quat[5], 0, 1e-12); EXPECT_NEAR(model->body_quat[6], 0, 1e-12); @@ -2079,6 +2101,7 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { mjSpec* copied_spec = mj_copySpec(spec); ASSERT_THAT(copied_spec, NotNull()); + mj_deleteSpec(gchild); mj_deleteSpec(child); mj_deleteSpec(spec); mj_deleteModel(model); @@ -2086,11 +2109,13 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { // check that deleting `parent` or `child` does not invalidate the copy mjModel* copied_model = mj_compile(copied_spec, 0); EXPECT_THAT(copied_model, NotNull()); - EXPECT_THAT(copied_model->njnt, 2); + EXPECT_THAT(copied_model->njnt, 3); EXPECT_NEAR(copied_model->jnt_range[0], -mjPI, 1e-6); EXPECT_NEAR(copied_model->jnt_range[1], mjPI, 1e-6); EXPECT_NEAR(copied_model->jnt_range[2], -mjPI, 1e-6); EXPECT_NEAR(copied_model->jnt_range[3], mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[4], -mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[5], mjPI, 1e-6); EXPECT_NEAR(copied_model->body_quat[4], 1, 1e-12); EXPECT_NEAR(copied_model->body_quat[5], 0, 1e-12); EXPECT_NEAR(copied_model->body_quat[6], 0, 1e-12); From 419da1739506c1cb022b51f27a437bab71714a3c Mon Sep 17 00:00:00 2001 From: Andrea Gesmundo Date: Thu, 9 Jan 2025 05:22:44 -0800 Subject: [PATCH 200/426] Fix plot label. PiperOrigin-RevId: 713635908 Change-Id: Icbeef26053b10747024ea65b23ca86816f4fdb2e --- python/tutorial.ipynb | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/tutorial.ipynb b/python/tutorial.ipynb index c3d4193d..0c7be7be 100644 --- a/python/tutorial.ipynb +++ b/python/tutorial.ipynb @@ -1603,13 +1603,13 @@ "source": [ "ax = plt.gca()\n", "\n", - "ax.plot(np.asarray(times), np.asarray(sensordata), label='timestep = {:2.2g}ms'.format(1000*dt))\n", + "ax.plot(np.asarray(times), np.asarray(sensordata), label=[f\"axis {v}\" for v in ['x', 'y', 'z']])\n", "\n", "# finalize plot\n", "ax.set_title('Accelerometer values')\n", "ax.set_ylabel('meter/second^2')\n", "ax.set_xlabel('second')\n", - "ax.legend(frameon=True, loc='lower right');\n", + "ax.legend(frameon=True, loc='lower right')\n", "plt.tight_layout()" ] }, From 90049343b9e39433dd6d65bf232b58e3778a5d55 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 9 Jan 2025 07:37:31 -0800 Subject: [PATCH 201/426] Fix texture indexing bug in bindings. Fixes #2341. PiperOrigin-RevId: 713667648 Change-Id: I7fa5da1506306c5374febf762ce54aff9146079b --- python/mujoco/bindings_test.py | 17 +++++++++++++++++ python/mujoco/indexers.cc | 1 + 2 files changed, 18 insertions(+) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 6bfebb90..47182aa5 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -93,6 +93,19 @@ TEST_XML_PLUGIN = r""" """ +TEST_XML_TEXTURE = r""" + + + + + + + + + +""" + @contextlib.contextmanager def temporary_callback(setter, callback): @@ -1619,6 +1632,10 @@ Euler integrator, semi-implicit in velocity. self.assertIsNot(data1.model, data2.model) self.assertNotEqual(data1.model._address, data2.model._address) + def test_texture_size(self): + model = mujoco.MjModel.from_xml_string(TEST_XML_TEXTURE) + self.assertEqual(model.tex('tex').data.shape, (512, 512, 3)) + def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare): for name in attr_to_compare: actual_value = getattr(actual_obj, name) diff --git a/python/mujoco/indexers.cc b/python/mujoco/indexers.cc index 72311a93..158f4cae 100644 --- a/python/mujoco/indexers.cc +++ b/python/mujoco/indexers.cc @@ -103,6 +103,7 @@ py::array_t MakeArray(T* base_ptr, int index, std::vector&& shape, shape.insert(shape.begin(), m.hfield_nrow[index]); } else if (MjSize == &raw::MjModel::ntexdata) { offset = m.tex_adr[index]; + shape.insert(shape.begin(), m.tex_nchannel[index]); shape.insert(shape.begin(), m.tex_width[index]); shape.insert(shape.begin(), m.tex_height[index]); } else if (MjSize == &raw::MjModel::nsensordata) { From daba00ca9d1635e297d5c611ccd6da21991df37f Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 9 Jan 2025 08:34:49 -0800 Subject: [PATCH 202/426] Fix a box-box bad collision in NativeCCD. PiperOrigin-RevId: 713683497 Change-Id: I0c4cfd55d00faa196d8759046c87ee4ef74f01ab --- src/engine/engine_collision_gjk.c | 8 +-- test/engine/engine_collision_gjk_test.cc | 64 ++++++++++++++++++++++++ 2 files changed, 68 insertions(+), 4 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 0a4c86ff..fedaded6 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1104,19 +1104,19 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9); // if the origin is on a face, replace the 3-simplex with a 2-simplex - if (attachFace(pt, v1, v2, v3, 1, 3, 2) == 0.0) { + if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL) { replaceSimplex3(pt, status, v1, v2, v3); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1, v4, v2, 2, 3, 0) == 0.0) { + if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL) { replaceSimplex3(pt, status, v1, v4, v2); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1, v3, v4, 0, 3, 1) == 0.0) { + if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL) { replaceSimplex3(pt, status, v1, v3, v4); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v4, v3, v2, 2, 0, 1) == 0.0) { + if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL) { replaceSimplex3(pt, status, v4, v3, v2); return polytope3(pt, status, obj1, obj2); } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index b2b8f440..bb8daf65 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -335,6 +335,70 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxDepth3) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.965925826289068201191412299522; + xmat[1] = -0.258819045102520739476403832668; + xmat[2] = 0.000000000000000006339100926609; + xmat[3] = 0.258819045102520739476403832668; + xmat[4] = 0.965925826289068201191412299522; + xmat[5] = -0.000000000000000214827792362716; + xmat[6] = 0.000000000000000049478422780336; + xmat[7] = 0.000000000000000209148392896446; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346499999999199323474918799; + xpos[1] = -0.023505500000000002086553152481; + xpos[2] = -4.562296442400120888294168253196; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.866025403784438707610604524234; + xmat[1] = -0.499999999999999944488848768742; + xmat[2] = 0.000000000000000018716705841316; + xmat[3] = 0.499999999999999944488848768742; + xmat[4] = 0.866025403784438707610604524234; + xmat[5] = -0.000000000000000263161736875730; + xmat[6] = 0.000000000000000115371725704125; + xmat[7] = 0.000000000000000237263102359077; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346499999999797803074130798; + xpos[1] = -0.023505499999999998617106200527; + xpos[2] = -4.659230360891631228525966434972; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dir[3], pos[3]; + mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + + EXPECT_NEAR(dist, -0.003066, kTolerance); + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, BoxBoxTouching) { static constexpr char xml[] = R"( From 00010f58486c594adc382aeeabae2aaff7f484c4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 9 Jan 2025 10:16:37 -0800 Subject: [PATCH 203/426] Compute diagonal indices in `mj_sqrMatTDSparse` PiperOrigin-RevId: 713714087 Change-Id: Icc8eae74e6e47ba1d12a6e9aa0d774ab006cfe38 --- src/engine/engine_core_constraint.c | 13 +++--- src/engine/engine_solver.c | 4 +- src/engine/engine_util_sparse.c | 12 ++++-- src/engine/engine_util_sparse.h | 5 +-- .../engine_util_sparse_benchmark_test.cc | 10 +++-- test/engine/engine_util_sparse_test.cc | 43 +++++++++++++------ 6 files changed, 53 insertions(+), 34 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c1313cd6..175b6de9 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2174,23 +2174,20 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // construct supernodes mju_superSparse(nefc, rowsuper, rownnz, rowadr, colind); - // AR = JM2 * JM2' + // pre-count efc_AR_rownnz, efc_AR_rowadr mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); + // AR = JM2 * JM2' + int* diagind = mjSTACKALLOC(d, nefc, int); mju_sqrMatTDSparse(d->efc_AR, JM2T, JM2, NULL, nv, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, rownnzT, rowadrT, colindT, NULL, - rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); + rownnz, rowadr, colind, rowsuper, d, diagind); // add R to diagonal of AR for (int i=0; i < nefc; i++) { - for (int j=0; j < d->efc_AR_rownnz[i]; j++) { - if (i == d->efc_AR_colind[d->efc_AR_rowadr[i]+j]) { - d->efc_AR[d->efc_AR_rowadr[i]+j] += d->efc_R[i]; - break; - } - } + d->efc_AR[diagind[i]] += d->efc_R[i]; } } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 7f0fe182..29f30a2e 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1424,7 +1424,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d, /*flg_upper=*/0); + d, /*diagind=*/NULL); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, @@ -1518,7 +1518,7 @@ static void FactorizeHessian(const mjModel* m, mjData* d, mjCGContext* ctx, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d, /*flg_upper=*/0); + d, /*diagind=*/NULL); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 6fbb0ff4..e32115cf 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -724,7 +724,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d, int flg_upper) { + mjData* d, int* diagind) { // allocate space for accumulation buffer and matT mj_markStack(d); @@ -838,8 +838,14 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } - // fill upper triangle - if (flg_upper) { + // diagonal indices requested: fill upper triangle + if (diagind) { + // save diagonal indices + for (int i=0; i < nc; i++) { + diagind[i] = res_rowadr[i] + res_rownnz[i] - 1; + } + + // fill upper triangle for (int i=0; i < nc; i++) { int start = res_rowadr[i]; int end = start + res_rownnz[i] - 1; diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 34bc777f..1473936b 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -89,8 +89,7 @@ MJAPI void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, MJAPI void mju_superSparse(int nr, int* rowsuper, const int* rownnz, const int* rowadr, const int* colind); -// compute sparse M'*diag*M (diag=NULL: compute M'*M), res has uncompressed layout -// res_rowadr is required to be precomputed +// compute sparse M'*diag*M (diag=NULL: compute M'*M), res_rowadr must be precomputed MJAPI void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag, int nr, int nc, int* res_rownnz, const int* res_rowadr, int* res_colind, @@ -98,7 +97,7 @@ MJAPI void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d, int flg_upper); + mjData* d, int* diagind); // precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse MJAPI void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index 838e71a1..bc41edfa 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -43,7 +43,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, - const int* colindT, const int* rowsuperT, mjData* d, int unused) { + const int* colindT, const int* rowsuperT, mjData* d, int* unused) { mj_markStack(d); int* chain = mj_stackAllocInt(d, 2 * nc); mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -435,6 +435,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); int* colind = mj_stackAllocInt(d, m->nv*m->nv); + int* diagind = mj_stackAllocInt(d, m->nv); // compute D corresponding to quad states mjtNum* D = mj_stackAllocNum(d, d->nefc); @@ -454,7 +455,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { d->efc_J_colind, d->efc_J_rowsuper, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, d, - /*flg_upper=*/1); + diagind); // compute H = M + J'*D*J mj_addM(m, d, H, rownnz, rowadr, colind); @@ -559,6 +560,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); int* colind = mj_stackAllocInt(d, m->nv * m->nv); + int* diagind = mj_stackAllocInt(d, m->nv); // compute D corresponding to quad states mjtNum* D = mj_stackAllocNum(d, d->nefc); @@ -579,7 +581,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { func(H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d, /*flg_upper=*/1); + d->efc_JT_rowsuper, d, diagind); } } else { for (auto s : state) { @@ -592,7 +594,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d, /*unused=*/0); + d->efc_JT_rowsuper, d, /*unused=*/nullptr); } } diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 20a543df..505080fe 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -324,6 +324,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; // test precount mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, @@ -336,7 +337,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -369,6 +370,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; // test precount mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, @@ -382,7 +384,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -415,6 +417,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {2, 3, 4}; @@ -429,7 +432,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 0, 0, 0, 0)); @@ -462,6 +465,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {2, 3, 4}; @@ -477,7 +481,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 0, 0, 0, 4, 35, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 0, 0, 0, 0, 2, 0)); @@ -510,6 +514,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; // test precount @@ -523,7 +528,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(5, 6, 4, 6, 9, 0, 4, 16, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -556,6 +561,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; // test precount mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, @@ -568,12 +574,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(1, 2, 0, 4, 0, 0, 2, 13, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0)); EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(diagindH, ElementsAre(0, 3, 7)); mj_deleteData(data); mj_deleteModel(model); @@ -601,6 +608,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { int colindH[] = {0, 0, 0, 0}; int rownnzH[] = {0, 0}; int rowadrH[] = {0, 0}; + int diagindH[] = {0, 0}; mjtNum diag[] = {2, 3, 4}; @@ -615,7 +623,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { mju_sqrMatTDUncompressedInit(rowadrH, 2); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 2, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(66, 4, 4, 35)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 1)); @@ -647,6 +655,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {2, 3}; @@ -661,7 +670,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 2, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(14, 18, 8, 18, 27, 0, 8, 32, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -694,6 +703,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {2, 3, 4}; @@ -708,7 +718,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, 1); + nullptr, data, diagindH); EXPECT_THAT(matH, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -742,6 +752,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {1, 1, 1}; @@ -756,7 +767,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, 1); + rowsuperT, data, diagindH); EXPECT_THAT(matH, ElementsAre(14, 14, 14, 14, 14, 14, 14, 14, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -790,6 +801,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0}; int rowadrH[] = {0, 0, 0}; + int diagindH[] = {0, 0, 0}; mjtNum diag[] = {1, 1, 1}; @@ -804,7 +816,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, 1); + rowsuperT, data, diagindH); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -838,6 +850,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0, 0}; int rowadrH[] = {0, 0, 0, 0}; + int diagindH[] = {0, 0, 0, 0}; mjtNum diag[] = {1, 1, 1}; @@ -852,7 +865,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { mju_sqrMatTDUncompressedInit(rowadrH, 4); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 4, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, 1); + rowsuperT, data, diagindH); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10)); @@ -890,6 +903,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0, 0, 0}; int rowadrH[] = {0, 0, 0, 0, 0}; + int diagindH[] = {0, 0, 0, 0, 0}; mjtNum diag[] = {1, 1, 1}; @@ -904,7 +918,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { mju_sqrMatTDUncompressedInit(rowadrH, 5); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 5, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, 1); + rowsuperT, data, diagindH); EXPECT_THAT(matH, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)); @@ -942,6 +956,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; int rownnzH[] = {0, 0, 0, 0, 0, 0, 0}; int rowadrH[] = {0, 0, 0, 0, 0, 0, 0}; + int diagindH[] = {0, 0, 0, 0, 0, 0, 0}; // test precount mju_sqrMatTDSparseCount(rownnzH, rowadrH, 7, rownnz, rowadr, colind, @@ -954,7 +969,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { mju_sqrMatTDUncompressedInit(rowadrH, 7); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 1, 7, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, 1); + rowsuperT, data, diagindH); EXPECT_THAT( matH, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, From b0f885ead03218d5d4cbb8e6ad8b341c41f5da63 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Jan 2025 01:10:28 -0800 Subject: [PATCH 204/426] Minor edits to the Programming/Simulation docs page. PiperOrigin-RevId: 713952376 Change-Id: I5b7230a001bd49954d401846f40cd122ee4e748c --- doc/programming/simulation.rst | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index d892ea03..b35977a7 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -556,6 +556,13 @@ or termination of the iterative solver. Model changes ~~~~~~~~~~~~~ +.. admonition:: Model editing framework + :class: tip + + The discussion below regarding mjModel changes at runtime was written before the 3.2.0 introduction of the + :doc:`Model Editing` framework. It is still valid, but the new framework is the safe and recommended way + to modify models. + The MuJoCo model contained in mjModel is supposed to represent constant physical properties of the system, and in theory should not change after compilation. Of course in practice things are not that simple. It is often desirable to change the physics options in ``mjModel.opt``, so as to experiment with different aspects of the physics or to create @@ -590,8 +597,8 @@ asking a "decompiler" to make corresponding changes to the C code -- it is just .. _siLayout: -Data layout and buffer allocation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Data layout +~~~~~~~~~~~ All matrices in MuJoCo are in **row-major** format. For example, the linear memory array (a0, a1, ... a5) represents the 2-by-3 matrix @@ -712,8 +719,8 @@ and :ref:`mj_stackAllocByte` is provided for allocation of arbitrary number of b .. _siError: -Errors, warnings, memory allocation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Errors and warnings +~~~~~~~~~~~~~~~~~~~ When a terminal error occurs, MuJoCo calls the function :ref:`mju_error` internally. Here is what mju_error does: From 3f32cc24791f1f9c32ce6b41d5ffa464ab0925d7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 10 Jan 2025 06:15:42 -0800 Subject: [PATCH 205/426] Add mjSpec binding to MJX. PiperOrigin-RevId: 714026500 Change-Id: I5cb3fe6cc975b3aa43d44185e3bc186b45845cf3 --- mjx/mujoco/mjx/_src/support.py | 173 +++++++++++++++++++++++++++- mjx/mujoco/mjx/_src/support_test.py | 86 ++++++++++++++ 2 files changed, 257 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 1f3c8137..18992535 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -13,7 +13,8 @@ # limitations under the License. # ============================================================================== """Engine support functions.""" -from typing import Optional, Tuple, Union +from collections.abc import Sequence +from typing import Optional, Tuple, Union, Any import jax from jax import numpy as jp @@ -236,7 +237,7 @@ def _getadr( def id2name( m: Union[Model, mujoco.MjModel], typ: mujoco._enums.mjtObj, i: int ) -> Optional[str]: - """Gets the name of an object with the specified mjtObj type and id. + """Gets the name of an object with the specified mjtObj type and ids. See mujoco.id2name for more info. @@ -284,6 +285,174 @@ def name2id( return names_map.get(name, -1) +class BindModel(object): + """Class holding the requested MJX Model and spec id for binding a spec to Model.""" + + def __init__(self, model: Model, specs: Sequence[Any]): + self.model = model + try: + iter(specs) + except TypeError: + specs = [specs] + ids = [] + for spec in specs: + match spec: + case mujoco.MjsBody(): + self.prefix = 'body_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name)) + case mujoco.MjsJoint(): + self.prefix = 'jnt_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name)) + case mujoco.MjsGeom(): + self.prefix = 'geom_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name)) + case mujoco.MjsSite(): + self.prefix = 'site_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name)) + case mujoco.MjsLight(): + self.prefix = 'light_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name)) + case mujoco.MjsCamera(): + self.prefix = 'cam_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name)) + case mujoco.MjsMesh(): + self.prefix = 'mesh_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name)) + case mujoco.MjsHfield(): + self.prefix = 'hfield_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name)) + case mujoco.MjsPair(): + self.prefix = 'pair_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_PAIR, spec.name)) + case mujoco.MjsTendon(): + self.prefix = 'tendon_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name)) + case mujoco.MjsActuator(): + self.prefix = 'actuator_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name)) + case mujoco.MjsSensor(): + self.prefix = 'sensor_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name)) + case mujoco.MjsNumeric(): + self.prefix = 'numeric_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_NUMERIC, spec.name)) + case mujoco.MjsText(): + self.prefix = 'text_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TEXT, spec.name)) + case mujoco.MjsTuple(): + self.prefix = 'tuple_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TUPLE, spec.name)) + case mujoco.MjsKey(): + self.prefix = 'key_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_KEY, spec.name)) + case mujoco.MjsEquality(): + self.prefix = 'eq_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name)) + case mujoco.MjsExclude(): + self.prefix = 'exclude_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EXCLUDE, spec.name)) + case mujoco.MjsSkin(): + self.prefix = 'skin_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SKIN, spec.name)) + case mujoco.MjsMaterial(): + self.prefix = 'material_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_MATERIAL, spec.name)) + case _: + raise ValueError('invalid spec type') + if len(ids) == 1: + self.id = ids[0] + else: + self.id = ids + + def __getattr__(self, name: str): + return getattr(self.model, self.prefix + name)[self.id, :] + + +def _bind_model(self: Model, obj: Sequence[Any]) -> BindModel: + """Bind a Mujoco spec to an MJX Model.""" + return BindModel(self, obj) + + +class BindData(object): + """Class holding the requested MJX Data and spec id for binding a spec to Data.""" + + def __init__(self, data: Data, model: Model, specs: Sequence[Any]): + self.data = data + try: + iter(specs) + except TypeError: + specs = [specs] + ids = [] + for spec in specs: + match spec: + case mujoco.MjsBody(): + self.prefix = '' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name)) + case mujoco.MjsJoint(): + self.prefix = 'jnt_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name)) + case mujoco.MjsGeom(): + self.prefix = 'geom_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name)) + case mujoco.MjsSite(): + self.prefix = 'site_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name)) + case mujoco.MjsLight(): + self.prefix = 'light_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name)) + case mujoco.MjsCamera(): + self.prefix = 'cam_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name)) + case mujoco.MjsTendon(): + self.prefix = 'tendon_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name)) + case mujoco.MjsActuator(): + self.prefix = 'actuator_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name)) + case mujoco.MjsSensor(): + self.prefix = 'sensor_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name)) + case _: + raise ValueError('invalid spec type') + if len(ids) == 1: + self.id = ids[0] + else: + self.id = ids + + def __getname(self, name: str): + try: + getattr(self.data, self.prefix + name) + return self.prefix + name + except AttributeError: + try: + getattr(self.data, name) + return name + except AttributeError as e: + raise ValueError(f'invalid name: {name}') from e + + def __getattr__(self, name: str): + return getattr(self.data, self.__getname(name))[self.id, ...] + + def set(self, name: str, value: jax.Array) -> Data: + """Set the value of an array in an MJX Data.""" + array = getattr(self.data, self.__getname(name)) + if len(value) == 1: + array = array.at[self.id].set(value[0]) + else: + for i, v in enumerate(value): + array = array.at[self.id[i]].set(v) + return self.data.replace(**{self.__getname(name): array}) + + +def _bind_data(self: Data, model: Model, obj: Sequence[Any]) -> BindData: + """Bind a Mujoco spec to an MJX Data.""" + return BindData(self, model, obj) + + +Model.bind = _bind_model +Data.bind = _bind_data + + def _decode_pyramid( pyramid: jax.Array, mu: jax.Array, condim: int ) -> jax.Array: diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index c955ba72..9add48b3 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -157,6 +157,92 @@ class SupportTest(parameterized.TestCase): i = i if n is not None else -1 self.assertEqual(support.name2id(mx, obj, n), i) + def test_bind(self): + xml = """ + + + + + + + + + + + + + + + + + + + + + + + """ + + s = mujoco.MjSpec.from_string(xml) + m = s.compile() + d = mujoco.MjData(m) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + mujoco.mj_step(m, d) + dx = mjx.step(mx, dx) + + # test getting + np.testing.assert_array_equal(mx.bind(s.bodies).pos, m.body_pos) + np.testing.assert_array_equal(dx.bind(mx, s.bodies).xpos, d.xpos) + for i in range(m.nbody): + np.testing.assert_array_equal(m.bind(s.bodies[i]).pos, m.body_pos[i, :]) + np.testing.assert_array_equal(mx.bind(s.bodies[i]).pos, m.body_pos[i, :]) + np.testing.assert_array_equal(d.bind(s.bodies[i]).xpos, d.xpos[i, :]) + np.testing.assert_array_equal( + dx.bind(mx, s.bodies[i]).xpos, d.xpos[i, :] + ) + + np.testing.assert_array_equal(mx.bind(s.geoms).size, m.geom_size) + np.testing.assert_array_equal(dx.bind(mx, s.geoms).xpos, d.geom_xpos) + for i in range(m.ngeom): + np.testing.assert_array_equal(m.bind(s.geoms[i]).size, m.geom_size[i, :]) + np.testing.assert_array_equal(mx.bind(s.geoms[i]).size, m.geom_size[i, :]) + np.testing.assert_array_equal(d.bind(s.geoms[i]).xpos, d.geom_xpos[i, :]) + np.testing.assert_array_equal( + dx.bind(mx, s.geoms[i]).xpos, d.geom_xpos[i, :] + ) + + np.testing.assert_array_equal(mx.bind(s.joints).axis, m.jnt_axis) + for i in range(m.njnt): + np.testing.assert_array_equal(m.bind(s.joints[i]).axis, m.jnt_axis[i, :]) + np.testing.assert_array_equal(mx.bind(s.joints[i]).axis, m.jnt_axis[i, :]) + + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, d.ctrl) + for i in range(m.nu): + np.testing.assert_array_equal(d.bind(s.actuators[i]).ctrl, d.ctrl[i]) + np.testing.assert_array_equal( + dx.bind(mx, s.actuators[i]).ctrl, d.ctrl[i] + ) + + # test setting + np.testing.assert_array_equal(d.ctrl, [0, 0, 0]) + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, d.ctrl) + dx2 = dx.bind(mx, s.actuators).set('ctrl', [1, 2, 3]) + np.testing.assert_array_equal(dx2.bind(mx, s.actuators).ctrl, [1, 2, 3]) + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) + dx3 = dx.bind(mx, s.actuators[1:]).set('ctrl', [4, 5]) + np.testing.assert_array_equal(dx3.bind(mx, s.actuators).ctrl, [0, 4, 5]) + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) + dx4 = dx.bind(mx, s.actuators[1]).set('ctrl', [6]) + np.testing.assert_array_equal(dx4.bind(mx, s.actuators).ctrl, [0, 6, 0]) + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) + + # test invalid name + with self.assertRaises(ValueError): + print(dx.bind(mx, s.actuators).actuator_ctrl) + with self.assertRaises(ValueError): + print(dx.bind(mx, s.actuators).set('actuator_ctrl', [1, 2, 3])) + _CONTACTS = """ From da32b0db30476c43897b6f38b9f64dc05f863114 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 10 Jan 2025 08:07:05 -0800 Subject: [PATCH 206/426] Handle `ctrl` special case explicitly in bind(). PiperOrigin-RevId: 714055763 Change-Id: I020008723af4a590926268a2ea615af54e20aa0b --- mjx/mujoco/mjx/_src/support.py | 19 ++++++++++--------- mjx/mujoco/mjx/_src/support_test.py | 6 ++++-- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 18992535..9032b8d2 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -404,7 +404,7 @@ class BindData(object): self.prefix = 'cam_' ids.append(name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name)) case mujoco.MjsTendon(): - self.prefix = 'tendon_' + self.prefix = 'ten_' ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name)) case mujoco.MjsActuator(): self.prefix = 'actuator_' @@ -412,6 +412,9 @@ class BindData(object): case mujoco.MjsSensor(): self.prefix = 'sensor_' ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name)) + case mujoco.MjsEquality(): + self.prefix = 'eq_' + ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name)) case _: raise ValueError('invalid spec type') if len(ids) == 1: @@ -420,15 +423,13 @@ class BindData(object): self.id = ids def __getname(self, name: str): - try: - getattr(self.data, self.prefix + name) - return self.prefix + name - except AttributeError: - try: - getattr(self.data, name) + if name == 'ctrl': + if self.prefix == 'actuator_': return name - except AttributeError as e: - raise ValueError(f'invalid name: {name}') from e + else: + raise AttributeError('ctrl is not available for this type') + else: + return self.prefix + name def __getattr__(self, name: str): return getattr(self.data, self.__getname(name))[self.id, ...] diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 9add48b3..567d2cc7 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -238,9 +238,11 @@ class SupportTest(parameterized.TestCase): np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) # test invalid name - with self.assertRaises(ValueError): + with self.assertRaises(AttributeError): + print(dx.bind(mx, s.geoms).ctrl) + with self.assertRaises(AttributeError): print(dx.bind(mx, s.actuators).actuator_ctrl) - with self.assertRaises(ValueError): + with self.assertRaises(AttributeError): print(dx.bind(mx, s.actuators).set('actuator_ctrl', [1, 2, 3])) _CONTACTS = """ From ca6162e6d06a1f4f3d709ce2d13f3b5bdc3d1bc9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Jan 2025 08:30:50 -0800 Subject: [PATCH 207/426] Refactor `mj_projectConstraint` to use CSR representation, don't allocate quadratic memory inside the function. PiperOrigin-RevId: 714062168 Change-Id: Ib6ddd0e751fabcded20911e3ced59672be0da8b5 --- src/engine/engine_core_constraint.c | 223 +++++++++++++++++----------- 1 file changed, 138 insertions(+), 85 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 175b6de9..02efb04f 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2073,37 +2073,79 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]); } - // space for backsubM2(J')' and its traspose - mjtNum* JM2 = mjSTACKALLOC(d, nefc*nv, mjtNum); - mjtNum* JM2T = mjSTACKALLOC(d, nv*nefc, mjtNum); - // sparse if (mj_isSparse(m)) { - // space for JM2 and JM2T indices - int* rownnz = mjSTACKALLOC(d, nefc, int); - int* rowadr = mjSTACKALLOC(d, nefc, int); - int* colind = mjSTACKALLOC(d, nefc*nv, int); - int* rowsuper = mjSTACKALLOC(d, nefc, int); - int* rownnzT = mjSTACKALLOC(d, nv, int); - int* rowadrT = mjSTACKALLOC(d, nv, int); - int* colindT = mjSTACKALLOC(d, nv*nefc, int); + // compute B = backsubM2(J')' and its transpose + + + // === pre-count B_rownnz, B_rowadr, nB (total nonzeros) + + // allocate B rownnz and rowadr + int* B_rownnz = mjSTACKALLOC(d, nefc, int); + int* B_rowadr = mjSTACKALLOC(d, nefc, int); + + // markers for merged dofs, initialized to -1 + int* marker = mjSTACKALLOC(d, nv, int); + for (int i=0; i < nv; i++) { + marker[i] = -1; + } + + B_rowadr[0] = 0; + for (int r=0; r < nefc; r++) { + int nnz = 0; // nonzeros in row r of B + + // traverse row r of J in reverse, count unique nonzeros + int start = d->efc_J_rowadr[r]; + int end = start + d->efc_J_rownnz[r]; + for (int i=end-1; i >= start; i--) { + int j = d->efc_J_colind[i]; + + // if dof j is marked, it was already counted by a child dof: skip it + if (marker[j] == r) { + continue; + } + + // traverse row j of C, marking new unique nonzeros + int nnzC = d->C_rownnz[j]; + int adrC = d->C_rowadr[j]; + for (int k=0; k < nnzC; k++) { + int c = d->C_colind[adrC + k]; + if (marker[c] != r) { + marker[c] = r; + nnz++; + } + } + } + + // update rownnz and rowadr + B_rownnz[r] = nnz; + if (r < nefc - 1) { + B_rowadr[r+1] = B_rowadr[r] + nnz; + } + } + + // total non-zeros in B + int nB = B_rowadr[nefc-1] + B_rownnz[nefc-1]; + + + // === fill in B column indices, copy values from J + + // allocate values and column indices + mjtNum* B = mjSTACKALLOC(d, nB, mjtNum); + int* B_colind = mjSTACKALLOC(d, nB, int); - // construct JM2 = backsubM2(J')' by rows for (int r=0; r < nefc; r++) { // init row - int nnz = 0; - int adr = (r > 0 ? rowadr[r-1]+rownnz[r-1] : 0); - int remain = d->efc_J_rownnz[r]; + int end = B_rowadr[r] + B_rownnz[r]; + int adrJ = d->efc_J_rowadr[r]; + int remainJ = d->efc_J_rownnz[r]; + int nnzB = 0; // complete chain in reverse while (1) { - // assign row descriptor - rownnz[r] = nnz; - rowadr[r] = adr; - // get previous dof in src and dst - int prev_src = (remain > 0 ? d->efc_J_colind[d->efc_J_rowadr[r]+remain-1] : -1); - int prev_dst = (nnz > 0 ? m->dof_parentid[colind[adr+nnz-1]] : -1); + int prev_src = (remainJ > 0 ? d->efc_J_colind[adrJ + remainJ - 1] : -1); + int prev_dst = (nnzB > 0 ? m->dof_parentid[B_colind[end - nnzB]] : -1); // both finished: break if (prev_src < 0 && prev_dst < 0) { @@ -2112,80 +2154,87 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // add src else if (prev_src >= prev_dst) { - colind[adr+nnz] = prev_src; - JM2[adr+nnz] = d->efc_J[d->efc_J_rowadr[r]+remain-1]; - remain--; - nnz++; + nnzB++; + remainJ--; + B_colind[end - nnzB] = prev_src; + B[end - nnzB] = d->efc_J[adrJ + remainJ]; } // add dst else { - colind[adr+nnz] = prev_dst; - JM2[adr+nnz] = 0; - nnz++; + nnzB++; + B_colind[end - nnzB] = prev_dst; + B[end - nnzB] = 0; } } - // reverse order of chain: make it increasing - for (int i=0; i < nnz/2; i++) { - int tmp_col = colind[adr+i]; - colind[adr+i] = colind[adr+nnz-i-1]; - colind[adr+nnz-i-1] = tmp_col; - - mjtNum tmp_dat = JM2[adr+i]; - JM2[adr+i] = JM2[adr+nnz-i-1]; - JM2[adr+nnz-i-1] = tmp_dat; - } - - // sparse backsubM2 - for (int i=nnz-1; i >= 0; i--) { - // save x(i) and i-pointer - mjtNum xi = JM2[adr+i]; - int pi = i; - - // process if not zero - if (xi) { - // x(i) /= sqrt(L(i,i)) - JM2[adr+i] *= sqrtInvD[colind[adr+i]]; - - // x(j) -= L(i,j) * x(i) - int Madr_ij = m->dof_Madr[colind[adr+i]]+1; - int j = m->dof_parentid[colind[adr+i]]; - while (j >= 0) { - // match dof id in sparse vector - while (colind[adr+pi] > j) { - pi--; - } - - // scale - JM2[adr+pi] -= d->qLD[Madr_ij++] * xi; - - // advance to parent - j = m->dof_parentid[j]; - } - } + // compare with B_rownnz: SHOULD NOT OCCUR + if (nnzB != B_rownnz[r]) { + mjERROR("pre and post-count of B_rownnz are not equal on row %d", r); } } - // construct JM2T - mju_transposeSparse(JM2T, JM2, nefc, nv, - rownnzT, rowadrT, colindT, rownnz, rowadr, colind); - // construct supernodes - mju_superSparse(nefc, rowsuper, rownnz, rowadr, colind); + // === in-place sparse back-substitution: B <- B * M^-1/2 + + // make qLD + int nC = m->nC; + mjtNum* qLD = mjSTACKALLOC(d, nC, mjtNum); + for (int i=0; i < nC; i++) { + qLD[i] = d->qLD[d->mapM2C[i]]; + } + + // sparse backsubM2 (half of LD back-substitution) + for (int r=0; r < nefc; r++) { + int nnzB = B_rownnz[r]; + int adrB = B_rowadr[r]; + + // B(r,:) <- inv(L') * B(r,:), exploit sparsity of input vector + for (int i=adrB + nnzB-1; i >= adrB; i--) { + mjtNum b = B[i]; + if (b == 0) { + continue; + } + int j = B_colind[i]; + int adrC = d->C_rowadr[j]; + mju_addToSclSparseInc(B + adrB, qLD + adrC, + nnzB, B_colind + adrB, + d->C_rownnz[j]-1, d->C_colind + adrC, -b); + } + + // B(r,:) <- sqrt(inv(D)) * B(r,:) + for (int i=adrB; i < adrB + nnzB; i++) { + int j = B_colind[i]; + B[i] *= sqrtInvD[j]; + } + } + + // construct B supernodes + int* B_rowsuper = mjSTACKALLOC(d, nefc, int); + mju_superSparse(nefc, B_rowsuper, B_rownnz, B_rowadr, B_colind); + + // construct B transposed + int* BT_rownnz = mjSTACKALLOC(d, nv, int); + int* BT_rowadr = mjSTACKALLOC(d, nv, int); + int* BT_colind = mjSTACKALLOC(d, nB, int); + mjtNum* BT = mjSTACKALLOC(d, nB, mjtNum); + mju_transposeSparse(BT, B, nefc, nv, + BT_rownnz, BT_rowadr, BT_colind, + B_rownnz, B_rowadr, B_colind); // pre-count efc_AR_rownnz, efc_AR_rowadr - mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, - rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); + mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, + BT_rownnz, BT_rowadr, BT_colind, + B_rownnz, B_rowadr, B_colind, B_rowsuper, d, /*flg_upper=*/1); - // AR = JM2 * JM2' + // A = B * B' int* diagind = mjSTACKALLOC(d, nefc, int); - mju_sqrMatTDSparse(d->efc_AR, JM2T, JM2, NULL, nv, nefc, + mju_sqrMatTDSparse(d->efc_AR, BT, B, NULL, nv, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, - rownnzT, rowadrT, colindT, NULL, - rownnz, rowadr, colind, rowsuper, d, diagind); + BT_rownnz, BT_rowadr, BT_colind, NULL, + B_rownnz, B_rowadr, B_colind, B_rowsuper, d, diagind); - // add R to diagonal of AR + // AR = A + diag(R) for (int i=0; i < nefc; i++) { d->efc_AR[diagind[i]] += d->efc_R[i]; } @@ -2193,14 +2242,18 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // dense else { - // JM2 = backsubM2(J')' - mj_solveM2(m, d, JM2, d->efc_J, sqrtInvD, nefc); + // space for backsubM2(J')' and its traspose + mjtNum* B = mjSTACKALLOC(d, nefc*nv, mjtNum); + mjtNum* BT = mjSTACKALLOC(d, nv*nefc, mjtNum); - // construct JM2T - mju_transpose(JM2T, JM2, nefc, nv); + // B = backsubM2(J')' + mj_solveM2(m, d, B, d->efc_J, sqrtInvD, nefc); - // AR = JM2 * JM2' - mju_sqrMatTD(d->efc_AR, JM2T, NULL, nv, nefc); + // construct BT + mju_transpose(BT, B, nefc, nv); + + // AR = B * B' + mju_sqrMatTD(d->efc_AR, BT, NULL, nv, nefc); // add R to diagonal of AR for (int r=0; r < nefc; r++) { From 2409a6e5378cf06b3cf01de0cfaf264a365930fb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Jan 2025 09:22:58 -0800 Subject: [PATCH 208/426] Add `mjData.nA` the number of non-zeros in the constraint inverse inertia matrix. PiperOrigin-RevId: 714076988 Change-Id: I4c323a6e81dfb23f7972b662719bbcb688bacf4a --- doc/includes/references.h | 5 +++-- include/mujoco/mjdata.h | 5 +++-- include/mujoco/mjxmacro.h | 5 +++-- introspect/structs.py | 9 +++++++-- python/mujoco/structs.cc | 2 ++ src/engine/engine_core_constraint.c | 3 ++- src/engine/engine_io.c | 1 + test/engine/engine_io_test.cc | 1 + unity/Runtime/Bindings/MjBindings.cs | 1 + 9 files changed, 23 insertions(+), 9 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index e1432bd2..0303015e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -170,6 +170,7 @@ struct mjData_ { int nl; // number of limit constraints int nefc; // number of constraints int nJ; // number of non-zeros in constraint Jacobian + int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands // global properties @@ -391,8 +392,8 @@ struct mjData_ { // computed by mj_projectConstraint (PGS solver) int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) int* efc_AR_rowadr; // row start address in colind array (nefc x 1) - int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) - mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) + int* efc_AR_colind; // column indices in sparse AR (nA x 1) + mjtNum* efc_AR; // J*inv(M)*J' + R (nA x 1) //-------------------- arena-allocated: POSITION, VELOCITY dependent diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index b51b19ae..3c4d889e 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -198,6 +198,7 @@ struct mjData_ { int nl; // number of limit constraints int nefc; // number of constraints int nJ; // number of non-zeros in constraint Jacobian + int nA; // number of non-zeros in constraint inverse inertia matrix int nisland; // number of detected constraint islands // global properties @@ -419,8 +420,8 @@ struct mjData_ { // computed by mj_projectConstraint (PGS solver) int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) int* efc_AR_rowadr; // row start address in colind array (nefc x 1) - int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) - mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) + int* efc_AR_colind; // column indices in sparse AR (nA x 1) + mjtNum* efc_AR; // J*inv(M)*J' + R (nA x 1) //-------------------- arena-allocated: POSITION, VELOCITY dependent diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 2d78fc02..6de6198d 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -715,8 +715,8 @@ #define MJDATA_ARENA_POINTERS_DUAL \ X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \ X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \ - X( int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc) ) \ - X( mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc) ) + X( int, efc_AR_colind, MJ_D(nA), 1 ) \ + X( mjtNum, efc_AR, MJ_D(nA), 1 ) // array fields of mjData that are used for constraint islands #define MJDATA_ARENA_POINTERS_ISLAND \ @@ -757,6 +757,7 @@ X( int, nl ) \ X( int, nefc ) \ X( int, nJ ) \ + X( int, nA ) \ X( int, nisland ) \ X( mjtNum, time ) \ X( uintptr_t, threadpool ) diff --git a/introspect/structs.py b/introspect/structs.py index be60293c..f988d597 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4701,6 +4701,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='number of non-zeros in constraint Jacobian', ), + StructFieldDecl( + name='nA', + type=ValueType(name='int'), + doc='number of non-zeros in constraint inverse inertia matrix', + ), StructFieldDecl( name='nisland', type=ValueType(name='int'), @@ -5797,7 +5802,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), ), doc='column indices in sparse AR', - array_extent=('nefc', 'nefc'), + array_extent=('nA',), ), StructFieldDecl( name='efc_AR', @@ -5805,7 +5810,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc="J*inv(M)*J' + R", - array_extent=('nefc', 'nefc'), + array_extent=('nA',), ), StructFieldDecl( name='efc_vel', diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 53d1a72b..f3fa7a0b 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -748,6 +748,7 @@ void MjDataWrapper::Serialize(std::ostream& output) const { X(ne); X(nf); X(nJ); + X(nA); X(nefc); X(nisland); X(time); @@ -825,6 +826,7 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { X(ne); X(nf); X(nJ); + X(nA); X(nefc); X(nisland); X(time); diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 02efb04f..a65a467e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1943,7 +1943,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { // driver: call all functions above void mj_makeConstraint(const mjModel* m, mjData* d) { // clear sizes - d->ne = d->nf = d->nl = d->nefc = d->nJ = 0; + d->ne = d->nf = d->nl = d->nefc = d->nJ = d->nA = 0; // disabled or Jacobian not allocated: return if (mjDISABLED(mjDSBL_CONSTRAINT)) { @@ -1960,6 +1960,7 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { d->nJ = nefc_allocated * m->nv; } d->nefc = nefc_allocated; + d->nA = d->nefc * d->nefc; // allocate efc arrays on arena if (!arenaAllocEfc(m, d)) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e32a9add..ddbb3557 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1893,6 +1893,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { d->nl = 0; d->nefc = 0; d->nJ = 0; + d->nA = 0; d->nisland = 0; // clear global properties diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index 3e6eb9af..470f6f02 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -157,6 +157,7 @@ TEST_F(EngineIoTest, ResetVariableSizes) { EXPECT_EQ(data->nf, 0); EXPECT_EQ(data->nefc, 0); EXPECT_EQ(data->nJ, 0); + EXPECT_EQ(data->nA, 0); EXPECT_EQ(data->ncon, 0); mj_deleteData(data); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7ad3d1b6..19c46aa0 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4852,6 +4852,7 @@ public unsafe struct mjData_ { public int nl; public int nefc; public int nJ; + public int nA; public int nisland; public double time; public fixed double energy[2]; From d4ca66a47b93df7f351ffecc0048f88ce850c302 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Jan 2025 10:18:14 -0800 Subject: [PATCH 209/426] Allocate dual-solver memory inside `mj_projectConstraint`, no more than required. PiperOrigin-RevId: 714093658 Change-Id: Id7e710c5dffd24871019a72a7bf2aa6eb3f7a36e --- doc/changelog.rst | 3 ++ src/engine/engine_core_constraint.c | 45 +++++++++++++++++++++++++---- 2 files changed, 42 insertions(+), 6 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index a32b8004..38d5b392 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -23,6 +23,9 @@ General - The field ``mjData.qLDiagSqrtInv`` has been removed. This field is only required for the dual solvers. It is now computed as-needed rather than unconditionally. Relatedly, added the corresponding argument to :ref:`mj_solveM2`. +- Reduced the memory footprint of the PGS solver's :ref:`A matrix`. This was the last remaining dense-memory + allocation in MuJoCo, allowing for a significant reduction of the :ref:`dynamic memory allocation heuristic`. + Bug fixes ^^^^^^^^^ - Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index a65a467e..264cfe71 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -72,9 +72,6 @@ static int arenaAllocEfc(const mjModel* m, mjData* d) { } MJDATA_ARENA_POINTERS_SOLVER - if (mj_isDual(m)) { - MJDATA_ARENA_POINTERS_DUAL - } #undef X #undef MJ_M @@ -1960,7 +1957,6 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { d->nJ = nefc_allocated * m->nv; } d->nefc = nefc_allocated; - d->nA = d->nefc * d->nefc; // allocate efc arrays on arena if (!arenaAllocEfc(m, d)) { @@ -2223,11 +2219,36 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { BT_rownnz, BT_rowadr, BT_colind, B_rownnz, B_rowadr, B_colind); - // pre-count efc_AR_rownnz, efc_AR_rowadr + // allocate AR row nonzeros and addresses on arena + d->efc_AR_rownnz = mj_arenaAllocByte(d, sizeof(int) * nefc, _Alignof(int)); + d->efc_AR_rowadr = mj_arenaAllocByte(d, sizeof(int) * nefc, _Alignof(int)); + if (!d->efc_AR_rownnz || !d->efc_AR_rowadr) { + mj_warning(d, mjWARN_CNSTRFULL, d->narena); + mj_clearEfc(d); + d->parena = d->ncon * sizeof(mjContact); + mj_freeStack(d); + return; + } + + // pre-count A nonzeros (compute AR_rownnz, AR_rowadr) mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, BT_rownnz, BT_rowadr, BT_colind, B_rownnz, B_rowadr, B_colind, B_rowsuper, d, /*flg_upper=*/1); + // nA = total number of nonzeros in A + d->nA = d->efc_AR_rownnz[nefc - 1] + d->efc_AR_rowadr[nefc - 1]; + + // allocate A values and column indices on arena + d->efc_AR = mj_arenaAllocByte(d, sizeof(mjtNum) * d->nA, _Alignof(mjtNum)); + d->efc_AR_colind = mj_arenaAllocByte(d, sizeof(int) * d->nA, _Alignof(int)); + if (!d->efc_AR || !d->efc_AR_colind) { + mj_warning(d, mjWARN_CNSTRFULL, d->narena); + mj_clearEfc(d); + d->parena = d->ncon * sizeof(mjContact); + mj_freeStack(d); + return; + } + // A = B * B' int* diagind = mjSTACKALLOC(d, nefc, int); mju_sqrMatTDSparse(d->efc_AR, BT, B, NULL, nv, nefc, @@ -2243,7 +2264,19 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // dense else { - // space for backsubM2(J')' and its traspose + d->nA = nefc * nefc; + + // arena-allocate efc_AR + d->efc_AR = mj_arenaAllocByte(d, sizeof(mjtNum) * d->nA, _Alignof(mjtNum)); + if (!d->efc_AR) { + mj_warning(d, mjWARN_CNSTRFULL, d->narena); + mj_clearEfc(d); + d->parena = d->ncon * sizeof(mjContact); + mj_freeStack(d); + return; + } + + // space for B = backsubM2(J')' and its transpose mjtNum* B = mjSTACKALLOC(d, nefc*nv, mjtNum); mjtNum* BT = mjSTACKALLOC(d, nv*nefc, mjtNum); From 1c32b5d2dd02842e532e80319c42705ae44ce920 Mon Sep 17 00:00:00 2001 From: Andrea Gesmundo Date: Mon, 13 Jan 2025 05:40:56 -0800 Subject: [PATCH 210/426] Fix attach logic. Replace .attach() with .attach_body() where a body is passed as a first argument instead of a spec that is expected by .attach(). +Fix typo. PiperOrigin-RevId: 714933134 Change-Id: Idb242fbef0fe164075826c01d4e1995c7d4ae565 --- python/mjspec.ipynb | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index c1e1fbf9..2f09d3cf 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -372,7 +372,7 @@ " theta = 2 * i * np.pi / num_legs\n", " hip_pos = BODY_RADIUS * np.array([np.cos(theta), np.sin(theta), 0])\n", " hip_site = spec.worldbody.add_site(pos=hip_pos, euler=[0, 0, theta])\n", - " hip_site.attach(leg.spec.find_body('thigh'), '', '-' + str(i))\n", + " hip_site.attach_body(leg.spec.find_body('thigh'), '', '-' + str(i))\n", "\n", " return spec" ] @@ -428,7 +428,7 @@ " spawn_pos = (xpos.flat[i], ypos.flat[i], zpos.flat[i])\n", " spawn_site = arena.worldbody.add_site(pos=spawn_pos, group=3)\n", " # Attach to the arena at the spawn sites, with a free joint.\n", - " spawn_body = spawn_site.attach(spec.worldbody, '', '-' + str(i))\n", + " spawn_body = spawn_site.attach_body(spec.worldbody, '', '-' + str(i))\n", " spawn_body.add_freejoint()\n", "\n", "# Instantiate the physics and render.\n", @@ -654,7 +654,7 @@ "id": "owcmKeuSzQRy" }, "source": [ - "We wish to remove the arms and replace them with the legs. This can be done by first storing the arm positions into frames attaches to the torso. Then we can detach the arms and self-attach the legs into the frames." + "We wish to remove the arms and replace them with the legs. This can be done by first storing the arm positions into frames attached to the torso. Then we can detach the arms and self-attach the legs into the frames." ] }, { @@ -807,15 +807,6 @@ "model = spec.compile()\n", "render(model, height=400)" ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "id": "pyy2q_mSVAX1" - }, - "outputs": [], - "source": [] } ], "metadata": { From 64d0f57c502605afae2569ea366ba6e49a2c519e Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 13 Jan 2025 06:57:12 -0800 Subject: [PATCH 211/426] Fixes for MJX tendons and muscle actuators. Fixes #2317. PiperOrigin-RevId: 714951834 Change-Id: I823b3a01b2b76cb4707cb313afd1a187f9bdb333 --- mjx/mujoco/mjx/_src/smooth.py | 4 +++- mjx/mujoco/mjx/_src/support.py | 42 ++++++++++++++++++++++++---------- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index 815c3007..f52391ed 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -734,7 +734,9 @@ def tendon(m: Model, d: Data) -> Data: for adr, num in zip(m.tendon_adr, m.tendon_num): for id_pulley in wrap_id_pulley: if adr <= id_pulley < adr + num: - divisor[id_pulley : adr + num] = m.wrap_prm[id_pulley] + divisor[id_pulley : adr + num] = np.maximum( + mujoco.mjMINVAL, m.wrap_prm[id_pulley] + ) # process spatial tendon sites (wrap_id_site,) = np.nonzero(m.wrap_type == WrapType.SITE) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 9032b8d2..57c9475b 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -529,7 +529,9 @@ def _length_circle( p0n = math.normalize(p0).reshape(-1) p1n = math.normalize(p1).reshape(-1) - angle = jp.arccos(jp.dot(p0n, p1n)) + # clip input to closed interval for jp.arccos to prevent potential nan + # TODO(taylorhowell): add test for case where clip is necessary + angle = jp.arccos(jp.clip(jp.dot(p0n, p1n), -1, 1)) # flip if necessary cross = p0[1] * p1[0] - p0[0] * p1[1] @@ -554,7 +556,11 @@ def _is_intersect( (p2[0] - p1[0]) * (p1[1] - p3[1]) - (p2[1] - p1[1]) * (p1[0] - p3[0]) ) / det - return (a >= 0) & (a <= 1) & (b >= 0) & (b <= 1) + return jp.where( + jp.abs(det) < mujoco.mjMINVAL, + 0, + (a >= 0) & (a <= 1) & (b >= 0) & (b <= 1), + ) def wrap_circle( @@ -567,7 +573,9 @@ def wrap_circle( sqrad = rad * rad dif = jp.array([d[2] - d[0], d[3] - d[1]]) dd = dif[0] ** 2 + dif[1] ** 2 - a = jp.clip(-(dif[0] * d[0] + dif[1] * d[1]) / dd, 0, 1) + a = jp.clip( + -(dif[0] * d[0] + dif[1] * d[1]) / jp.maximum(mujoco.mjMINVAL, dd), 0, 1 + ) seg = jp.array([a * dif[0] + d[0], a * dif[1] + d[1]]) point_inside0 = sqlen0 < sqrad @@ -581,13 +589,21 @@ def wrap_circle( # construct the two solutions, compute goodness def _sol(sgn): - sqrt0 = jp.sqrt(sqlen0 - sqrad) - sqrt1 = jp.sqrt(sqlen1 - sqrad) + sqrt0 = jp.sqrt(jp.maximum(mujoco.mjMINVAL, sqlen0 - sqrad)) + sqrt1 = jp.sqrt(jp.maximum(mujoco.mjMINVAL, sqlen1 - sqrad)) - d00 = (d[0] * sqrad + sgn * rad * d[1] * sqrt0) / sqlen0 - d01 = (d[1] * sqrad - sgn * rad * d[0] * sqrt0) / sqlen0 - d10 = (d[2] * sqrad - sgn * rad * d[3] * sqrt1) / sqlen1 - d11 = (d[3] * sqrad + sgn * rad * d[2] * sqrt1) / sqlen1 + d00 = (d[0] * sqrad + sgn * rad * d[1] * sqrt0) / jp.maximum( + mujoco.mjMINVAL, sqlen0 + ) + d01 = (d[1] * sqrad - sgn * rad * d[0] * sqrt0) / jp.maximum( + mujoco.mjMINVAL, sqlen0 + ) + d10 = (d[2] * sqrad - sgn * rad * d[3] * sqrt1) / jp.maximum( + mujoco.mjMINVAL, sqlen1 + ) + d11 = (d[3] * sqrad + sgn * rad * d[2] * sqrt1) / jp.maximum( + mujoco.mjMINVAL, sqlen1 + ) sol = jp.array([[d00, d01], [d10, d11]]) @@ -785,9 +801,8 @@ def muscle_gain( # velocity curve y = fvmax - 1 - FV = fvmax # pylint:disable=invalid-name FV = jp.where( # pylint:disable=invalid-name - V <= y, fvmax - jp.square(y - V) / jp.maximum(mujoco.mjMINVAL, y), FV + V <= y, fvmax - jp.square(y - V) / jp.maximum(mujoco.mjMINVAL, y), fvmax ) FV = jp.where(V <= 0, jp.square(V + 1), FV) # pylint:disable=invalid-name FV = jp.where(V <= -1, 0, FV) # pylint:disable=invalid-name @@ -845,7 +860,10 @@ def muscle_dynamics_timescale( # sigmoid function over 0 <= x <= 1 using quintic polynomial # sigmoid: f(x) = 6 * x^5 - 15 * x^4 + 10 * x^3 # solution of f(0) = f'(0) = f''(0) = 0, f(1) = 1, f'(1) = f''(1) = 0 - return jp.clip(x**3 * (3 * x * (2 * x - 5) + 10), 0, 1) + sol = x * x * x * (3 * x * (2 * x - 5) + 10) + sol = jp.where(x <= 0, 0, sol) + sol = jp.where(x >= 1, 1, sol) + return sol # smooth switching # scale by width, center around 0.5 midpoint, rescale to bounds From ba183adb6795f917f8e52252ce9475411805f8d0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 13 Jan 2025 08:40:56 -0800 Subject: [PATCH 212/426] Add assets to MjSpec wrapper. This enables associating assets with a spec object. Also, remove `spec.compile(assets)` and replace it with the `spec.assets` attribute, which must be be set before compile if assets are present. PiperOrigin-RevId: 714981385 Change-Id: Ic3a33c3b75d3a7e14868622aadb57b2a8461a649 --- python/mujoco/specs.cc | 82 ++++++++++++++++++++++++------------- python/mujoco/specs_test.py | 4 +- 2 files changed, 56 insertions(+), 30 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index c376ea35..d5141765 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -71,25 +71,43 @@ using MjDoubleRefVec = Eigen::Ref; struct MjSpec { MjSpec() : ptr(mj_makeSpec()) {} - MjSpec(raw::MjSpec* ptr) : ptr(ptr) {} + MjSpec(raw::MjSpec* ptr, + const std::unordered_map& assets_ = {}) + : ptr(ptr) { + for (const auto& asset : assets_) { + assets[asset.first.c_str()] = asset.second; + } + } // copy constructor and assignment - MjSpec(const MjSpec& other) : ptr(mj_copySpec(other.ptr)) {} + MjSpec(const MjSpec& other) : ptr(mj_copySpec(other.ptr)) { + assets = other.assets; + } MjSpec& operator=(const MjSpec& other) { ptr = mj_copySpec(other.ptr); + assets = other.assets; return *this; } // move constructor and move assignment - MjSpec(MjSpec&& other) : ptr(other.ptr) { other.ptr = nullptr; } + MjSpec(MjSpec&& other) : ptr(other.ptr) { + other.ptr = nullptr; + assets = other.assets; + other.assets.clear(); + } MjSpec& operator=(MjSpec&& other) { ptr = other.ptr; other.ptr = nullptr; + assets = other.assets; + other.assets.clear(); return *this; } - ~MjSpec() { mj_deleteSpec(ptr); } + ~MjSpec() { + mj_deleteSpec(ptr); + } raw::MjSpec* ptr; + py::dict assets; }; template @@ -263,6 +281,9 @@ PYBIND11_MODULE(_specs, m) { throw py::value_error(error); } } + if (assets.has_value()) { + return MjSpec(spec, assets.value()); + } return MjSpec(spec); }, py::arg("filename"), py::arg("assets") = py::none(), R"mydelimiter( @@ -305,6 +326,9 @@ PYBIND11_MODULE(_specs, m) { throw py::value_error(error); } } + if (assets.has_value()) { + return MjSpec(spec, assets.value()); + } return MjSpec(spec); }, py::arg("xml"), py::arg("assets") = py::none(), R"mydelimiter( @@ -324,7 +348,7 @@ PYBIND11_MODULE(_specs, m) { m, d); }); mjSpec.def("copy", [](const MjSpec& self) -> MjSpec { - return MjSpec(mj_copySpec(self.ptr)); + return MjSpec(self); }); mjSpec.def_property_readonly( "worldbody", @@ -370,33 +394,33 @@ PYBIND11_MODULE(_specs, m) { return mjs_findDefault(self.ptr, classname.c_str()); }, py::return_value_policy::reference_internal); - mjSpec.def("compile", [mjmodel_from_spec_ptr](MjSpec& self) { - return mjmodel_from_spec_ptr(reinterpret_cast(self.ptr)); + mjSpec.def("compile", [mjmodel_from_spec_ptr](MjSpec& self) -> py::object { + if (self.assets.empty()) { + return mjmodel_from_spec_ptr(reinterpret_cast(self.ptr)); + } + mjVFS vfs; + mj_defaultVFS(&vfs); + for (auto item : self.assets) { + std::string buffer = py::cast(item.second); + mj_addBufferVFS(&vfs, py::cast(item.first).c_str(), + buffer.c_str(), buffer.size()); + }; + auto model = + mjmodel_from_spec_ptr(reinterpret_cast(self.ptr), + reinterpret_cast(&vfs)); + mj_deleteVFS(&vfs); + return model; }); - mjSpec.def( - "compile", - [mjmodel_from_spec_ptr](MjSpec& self, py::dict& assets) -> py::object { - mjVFS vfs; - mj_defaultVFS(&vfs); + mjSpec.def_property( + "assets", + [](MjSpec& self) -> py::dict { + return self.assets; + }, + [](MjSpec& self, py::dict& assets) { for (auto item : assets) { - std::string buffer = py::cast(item.second); - mj_addBufferVFS(&vfs, py::cast(item.first).c_str(), - buffer.c_str(), buffer.size()); + self.assets[item.first] = item.second; }; - auto model = - mjmodel_from_spec_ptr(reinterpret_cast(self.ptr), - reinterpret_cast(&vfs)); - mj_deleteVFS(&vfs); - return model; - }, R"mydelimiter( - Compiles the spec and returns the compiled model. - - Parameters - ---------- - assets : dict, optional - A dictionary of assets to be used by the spec. The keys are asset names - and the values are asset contents. - )mydelimiter"); + }, py::return_value_policy::reference_internal); mjSpec.def("to_xml", [](MjSpec& self) -> std::string { int size = mj_saveXMLString(self.ptr, nullptr, 0, nullptr, 0); std::unique_ptr buf(new char[size + 1]); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index e0d3e836..d7524372 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -714,8 +714,10 @@ class SpecsTest(absltest.TestCase): geom = spec.worldbody.add_geom() geom.type = mujoco.mjtGeom.mjGEOM_MESH geom.meshname = 'cube' - model = spec.compile({'cube.obj': cube}) + spec.assets = {'cube.obj': cube} + model = spec.compile() self.assertEqual(model.nmeshvert, 8) + self.assertEqual(spec.assets['cube.obj'], cube) def test_include(self): included_xml = """ From 61973a33c967c2dd2dd3a074bca29f153c09b39a Mon Sep 17 00:00:00 2001 From: Gabe Oppenheimer Date: Tue, 14 Jan 2025 11:50:30 -0800 Subject: [PATCH 213/426] Update eigen3 and the changelog ahead of the 3.2.7 release. PiperOrigin-RevId: 715467319 Change-Id: I4a6a168bc981c976890c46b784e94805afc86323 --- cmake/MujocoDependencies.cmake | 2 +- doc/changelog.rst | 28 ++++++++++++++-------------- python/mujoco/CMakeLists.txt | 2 +- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 77a52dec..23e4e71e 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -39,7 +39,7 @@ set(MUJOCO_DEP_VERSION_qhull CACHE STRING "Version of `qhull` to be fetched." ) set(MUJOCO_DEP_VERSION_Eigen3 - d34b100c137ac931379ae5e1b888f16a9c8d6c72 + 7f2377859377da6f22152015c28b12c04752af77 CACHE STRING "Version of `Eigen3` to be fetched." ) diff --git a/doc/changelog.rst b/doc/changelog.rst index 38d5b392..d9a9936d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,17 +2,17 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.2.7 (Jan 14, 2025) +---------------------------- Python bindings ^^^^^^^^^^^^^^^ -- :ref:`rollout` now features native multi-threading. If a sequence of ``MjData`` instances - of length ``nthread`` is passed in, ``rollout`` will automatically create a thread pool and parallelize - the computation. The thread pool can be reused across calls, but then the function cannot be called simultaneously - from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which - encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. -- Fix global namespace pollution when using ``mjpython`` (:github:issue:`2265`). +1. :ref:`rollout` now features native multi-threading. If a sequence of ``MjData`` instances + of length ``nthread`` is passed in, ``rollout`` will automatically create a thread pool and parallelize + the computation. The thread pool can be reused across calls, but then the function cannot be called simultaneously + from multiple threads. To run multiple threaded rollouts simultaneously, use the new class ``Rollout`` which + encapsulates the thread pool. Contribution by :github:user:`aftersomemath`. +2. Fix global namespace pollution when using ``mjpython`` (:github:issue:`2265`). General ^^^^^^^ @@ -20,16 +20,16 @@ General .. admonition:: Breaking API changes (minor) :class: attention - - The field ``mjData.qLDiagSqrtInv`` has been removed. This field is only required for the dual solvers. It is now - computed as-needed rather than unconditionally. Relatedly, added the corresponding argument to :ref:`mj_solveM2`. + 3. The field ``mjData.qLDiagSqrtInv`` has been removed. This field is only required for the dual solvers. It is now + computed as-needed rather than unconditionally. Relatedly, added the corresponding argument to :ref:`mj_solveM2`. -- Reduced the memory footprint of the PGS solver's :ref:`A matrix`. This was the last remaining dense-memory - allocation in MuJoCo, allowing for a significant reduction of the :ref:`dynamic memory allocation heuristic`. +4. Reduced the memory footprint of the PGS solver's :ref:`A matrix`. This was the last remaining dense-memory + allocation in MuJoCo, allowing for a significant reduction of the :ref:`dynamic memory allocation heuristic`. Bug fixes ^^^^^^^^^ -- Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). -- Fixed a bug in :ref:`mj_mulM2` and added a test. +5. Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). +6. Fixed a bug in :ref:`mj_mulM2` and added a test. Version 3.2.6 (Dec 2, 2024) --------------------------- diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index b6d6c078..4301dd0f 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -173,7 +173,7 @@ findorfetch( GIT_REPO https://gitlab.com/libeigen/eigen GIT_TAG - d34b100c137ac931379ae5e1b888f16a9c8d6c72 + 7f2377859377da6f22152015c28b12c04752af77 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL From 66f2758ea529bcd68a30631e4d34aebbfda4f3cf Mon Sep 17 00:00:00 2001 From: Andrea Gesmundo Date: Tue, 14 Jan 2025 23:56:02 -0800 Subject: [PATCH 214/426] Minor updates. Fix a typo and a comment. PiperOrigin-RevId: 715672977 Change-Id: I281002aad2b40b59544c30f41aa113e13ae9d09e --- mjx/tutorial.ipynb | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index 751a150a..905e2ae3 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -321,7 +321,7 @@ "source": [ "Now let's run the same exact simulation on the GPU device using MJX!\n", "\n", - "In the example below, we use `mjx.step` instead of `mujoco.mj_step`, and we also [`jax.jit`](https://jax.readthedocs.io/en/latest/jax-101/02-jitting.html) the `mjx.step` so that it runs efficiently on the GPU. After each step, we convert the `mjx.Data` back to `mjData` so that we can use the MuJoCo renderer.\n" + "In the example below, we use `mjx.step` instead of `mujoco.mj_step`, and we also [`jax.jit`](https://jax.readthedocs.io/en/latest/jax-101/02-jitting.html) the `mjx.step` so that it runs efficiently on the GPU. For each frame, we convert the `mjx.Data` back to `mjData` so that we can use the MuJoCo renderer.\n" ] }, { @@ -563,7 +563,7 @@ "\n", "Let's instantiate the environment and visualize a short rollout.\n", "\n", - "NOTE: Since episodes terminates early if the torso is below the healthy z-range, the only relevant contacts for this task are between the feet and the plane. We turn off other contacts." + "NOTE: Since episodes terminate early if the torso is below the healthy z-range, the only relevant contacts for this task are between the feet and the plane. We turn off other contacts." ] }, { From 10c7207ceaf9669d0cd3c0dfce6974bc796da6c0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 15 Jan 2025 05:08:48 -0800 Subject: [PATCH 215/426] Process asset path before adding VFS buffer. Also, handle VFS errors. PiperOrigin-RevId: 715755127 Change-Id: Iaf455d448f16e37ab89ceaa5797319135acd73c9 --- python/mujoco/specs.cc | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index d5141765..f8167182 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -400,11 +400,22 @@ PYBIND11_MODULE(_specs, m) { } mjVFS vfs; mj_defaultVFS(&vfs); - for (auto item : self.assets) { - std::string buffer = py::cast(item.second); - mj_addBufferVFS(&vfs, py::cast(item.first).c_str(), - buffer.c_str(), buffer.size()); - }; + for (const auto& asset : self.assets) { + std::string buffer_name = + _impl::StripPath(py::cast(asset.first).c_str()); + std::string buffer = py::cast(asset.second); + const int vfs_error = InterceptMjErrors(mj_addBufferVFS)( + &vfs, buffer_name.c_str(), buffer.c_str(), buffer.size()); + if (vfs_error) { + mj_deleteVFS(&vfs); + if (vfs_error == 2) { + throw py::value_error("Repeated file name in assets dict: " + + buffer_name); + } else { + throw py::value_error("Asset failed to load: " + buffer_name); + } + } + } auto model = mjmodel_from_spec_ptr(reinterpret_cast(self.ptr), reinterpret_cast(&vfs)); From 05d4c3d670225b6f1217e106a27fac8bb0b9a0c3 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 15 Jan 2025 07:41:45 -0800 Subject: [PATCH 216/426] Add mjs_getParent to retrieve the parent body of an object. PiperOrigin-RevId: 715794859 Change-Id: Ib9792787cdb966f579938dde3f94ebabca8ef1fc --- doc/APIreference/functions.rst | 9 ++++++++ doc/includes/references.h | 1 + doc/python.rst | 2 ++ include/mujoco/mujoco.h | 3 +++ introspect/functions.py | 16 +++++++++++++ python/mujoco/specs.cc | 42 ++++++++++++++++++++++++++++++++++ python/mujoco/specs_test.py | 9 ++++++++ src/user/user_api.cc | 24 +++++++++++++++++++ src/user/user_api.h | 3 +++ src/user/user_objects.h | 7 ++++++ 10 files changed, 116 insertions(+) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 590aaf72..91fdbd7b 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4175,6 +4175,15 @@ Find element in spec by name. Find child body by name. +.. _mjs_getParent: + +`mjs_getParent <#mjs_getParent>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_getParent + +Get parent body. + .. _mjs_findFrame: `mjs_findFrame <#mjs_findFrame>`__ diff --git a/doc/includes/references.h b/doc/includes/references.h index 0303015e..d9742797 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3607,6 +3607,7 @@ mjSpec* mjs_findSpec(mjSpec* spec, const char* name); mjsBody* mjs_findBody(mjSpec* s, const char* name); mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); mjsBody* mjs_findChild(mjsBody* body, const char* name); +mjsBody* mjs_getParent(mjsElement* element); mjsFrame* mjs_findFrame(mjSpec* s, const char* name); mjsDefault* mjs_getDefault(mjsElement* element); const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); diff --git a/doc/python.rst b/doc/python.rst index 35c27d73..90f861a2 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -591,6 +591,8 @@ Recursive search: ``body.find_all(mujoco.mjtObj.mjOBJ_SITE)`` or ``body.find_all('site')`` will return a list of all sites under the body. +Additionally, the parent body of a given element - including bodies and frames - can be accessed via the ``parent`` +property. For example, the parent of a site can be accessed via ``site.parent``. Relationship to ``PyMJCF`` -------------------------- diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 9e4d25ff..52d73f79 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1546,6 +1546,9 @@ MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); // Find child body by name. MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name); +// Get parent body. +MJAPI mjsBody* mjs_getParent(mjsElement* element); + // Find frame by name. MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); diff --git a/introspect/functions.py b/introspect/functions.py index fa2141a9..4fd2374a 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -9828,6 +9828,22 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Find child body by name.', )), + ('mjs_getParent', + FunctionDecl( + name='mjs_getParent', + return_type=PointerType( + inner_type=ValueType(name='mjsBody'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Get parent body.', + )), ('mjs_findFrame', FunctionDecl( name='mjs_findFrame', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index f8167182..a0dc13e4 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -680,6 +680,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_getSpec(self.element); }, py::return_value_policy::reference_internal); + mjsBody.def_property_readonly( + "parent", + [](raw::MjsBody& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsBody.def( "attach_frame", [](raw::MjsBody& self, raw::MjsFrame& frame, @@ -713,6 +719,12 @@ PYBIND11_MODULE(_specs, m) { mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); }); + mjsFrame.def_property_readonly( + "parent", + [](raw::MjsFrame& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsFrame.def( "attach_body", [](raw::MjsFrame& self, raw::MjsBody& body, @@ -760,6 +772,12 @@ PYBIND11_MODULE(_specs, m) { mjsGeom.def("set_default", [](raw::MjsGeom& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); }); + mjsGeom.def_property_readonly( + "parent", + [](raw::MjsGeom& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsGeom.def( "default", [](raw::MjsGeom& self) -> raw::MjsDefault* { @@ -775,6 +793,12 @@ PYBIND11_MODULE(_specs, m) { mjsJoint.def("set_default", [](raw::MjsJoint& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); }); + mjsJoint.def_property_readonly( + "parent", + [](raw::MjsJoint& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsJoint.def( "default", [](raw::MjsJoint& self) -> raw::MjsDefault* { @@ -790,6 +814,12 @@ PYBIND11_MODULE(_specs, m) { mjsSite.def("set_default", [](raw::MjsSite& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); }); + mjsSite.def_property_readonly( + "parent", + [](raw::MjsSite& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsSite.def( "default", [](raw::MjsSite& self) -> raw::MjsDefault* { @@ -845,6 +875,12 @@ PYBIND11_MODULE(_specs, m) { mjsCamera.def("set_default", [](raw::MjsCamera& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); }); + mjsCamera.def_property_readonly( + "parent", + [](raw::MjsCamera& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsCamera.def( "default", [](raw::MjsCamera& self) -> raw::MjsDefault* { @@ -860,6 +896,12 @@ PYBIND11_MODULE(_specs, m) { mjsLight.def("set_default", [](raw::MjsLight& self, raw::MjsDefault& def) { mjs_setDefault(self.element, &def); }); + mjsLight.def_property_readonly( + "parent", + [](raw::MjsLight& self) -> raw::MjsBody* { + return mjs_getParent(self.element); + }, + py::return_value_policy::reference_internal); mjsLight.def( "default", [](raw::MjsLight& self) -> raw::MjsDefault* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index d7524372..dd8a6df7 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -646,6 +646,10 @@ class SpecsTest(absltest.TestCase): self.assertEqual(spec.bodies[2].name, 'body2') self.assertEqual(spec.bodies[3].name, 'body3') self.assertEqual(spec.bodies[4].name, 'body4') + self.assertEqual(spec.bodies[1].parent, spec.worldbody) + self.assertEqual(spec.bodies[2].parent, spec.worldbody) + self.assertEqual(spec.bodies[3].parent, spec.bodies[1]) + self.assertEqual(spec.bodies[4].parent, spec.bodies[3]) self.assertLen(spec.worldbody.find_all(bodytype), 4) self.assertLen(spec.bodies[1].find_all(bodytype), 2) self.assertLen(spec.bodies[3].find_all(bodytype), 1) @@ -671,6 +675,11 @@ class SpecsTest(absltest.TestCase): self.assertEqual(spec.bodies[3].sites[2].name, 'site3') self.assertEqual(spec.bodies[3].sites[3].name, 'site4') self.assertEqual(spec.bodies[4].sites[0].name, 'site5') + self.assertEqual(spec.bodies[3].sites[0].parent, spec.bodies[3]) + self.assertEqual(spec.bodies[3].sites[1].parent, spec.bodies[3]) + self.assertEqual(spec.bodies[3].sites[2].parent, spec.bodies[3]) + self.assertEqual(spec.bodies[3].sites[3].parent, spec.bodies[3]) + self.assertEqual(spec.bodies[4].sites[0].parent, spec.bodies[4]) with self.assertRaises(ValueError) as cm: spec.worldbody.find_all('actuator') self.assertEqual( diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 7e819007..8eb612a7 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -649,6 +649,30 @@ mjsBody* mjs_findChild(mjsBody* bodyspec, const char* name) { +// get parent body +mjsBody* mjs_getParent(mjsElement* element) { + switch (element->elemtype) { + case mjOBJ_BODY: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_FRAME: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_JOINT: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_GEOM: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_SITE: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_CAMERA: + return &(static_cast(element)->GetParent()->spec); + case mjOBJ_LIGHT: + return &(static_cast(element)->GetParent()->spec); + default: + return nullptr; + } +} + + + // find frame by name mjsFrame* mjs_findFrame(mjSpec* s, const char* name) { mjsElement* frame = mjs_findElement(s, mjOBJ_FRAME, name); diff --git a/src/user/user_api.h b/src/user/user_api.h index ac545e74..b6722710 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -201,6 +201,9 @@ MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); // Find child body by name. MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name); +// Get parent body. +MJAPI mjsBody* mjs_getParent(mjsElement* element); + // Find frame by name. MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index bf11a7be..ca13a741 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -347,6 +347,7 @@ class mjCBody : public mjCBody_, private mjsBody { // set parent of this body void SetParent(mjCBody* _body) { parent = _body; } + mjCBody* GetParent() const { return parent; } private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor @@ -411,6 +412,7 @@ class mjCFrame : public mjCFrame_, private mjsFrame { void CopyFromSpec(void); void PointToLocal(void); void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } mjCFrame& operator+=(const mjCBody& other); @@ -462,6 +464,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint { void CopyFromSpec(void); void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } // used by mjXWriter and mjCModel const std::vector& get_userdata() const { return userdata_; } @@ -543,6 +546,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom { bool IsVisual(void) const { return visual_; } void SetNotVisual(void) { visual_ = false; } void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } mjtGeom Type() const { return type; } // Compute all coefs modeling the interaction with the surrounding fluid. @@ -608,6 +612,7 @@ class mjCSite : public mjCSite_, private mjsSite { // site's body mjCBody* Body() const { return body; } void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } // use strings from mjCBase rather than mjStrings from mjsSite using mjCBase::name; @@ -661,6 +666,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera { const std::vector& get_userdata() const { return userdata_; } void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } private: void Compile(void); // compiler @@ -701,6 +707,7 @@ class mjCLight : public mjCLight_, private mjsLight { const std::string& get_targetbody() const { return targetbody_; } void SetParent(mjCBody* _body) { body = _body; } + mjCBody* GetParent() const { return body; } private: void Compile(void); // compiler From ff1ff44e34e62149d38c0c8b3f0c40f0cc0f4998 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 15 Jan 2025 08:20:34 -0800 Subject: [PATCH 217/426] Fix typo MjsHfield->MjsHField. PiperOrigin-RevId: 715808003 Change-Id: Ic510f7a3dc4d2c93789ce27162d004b20762bd12 --- mjx/mujoco/mjx/_src/support.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 57c9475b..dead920c 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -318,7 +318,7 @@ class BindModel(object): case mujoco.MjsMesh(): self.prefix = 'mesh_' ids.append(name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name)) - case mujoco.MjsHfield(): + case mujoco.MjsHField(): self.prefix = 'hfield_' ids.append(name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name)) case mujoco.MjsPair(): From f1d557c12517b0e0af7eef8d4e45dffeb12591a7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 15 Jan 2025 09:36:44 -0800 Subject: [PATCH 218/426] Support for MJX bind set() for scalars. PiperOrigin-RevId: 715832281 Change-Id: I217021e81fae95e44e9297a1c447891ff76572bd --- mjx/mujoco/mjx/_src/support.py | 4 ++++ mjx/mujoco/mjx/_src/support_test.py | 3 +++ 2 files changed, 7 insertions(+) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index dead920c..c5ae3157 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -437,6 +437,10 @@ class BindData(object): def set(self, name: str, value: jax.Array) -> Data: """Set the value of an array in an MJX Data.""" array = getattr(self.data, self.__getname(name)) + try: + iter(value) + except TypeError: + value = [value] if len(value) == 1: array = array.at[self.id].set(value[0]) else: diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 567d2cc7..53c69cf9 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -236,6 +236,9 @@ class SupportTest(parameterized.TestCase): dx4 = dx.bind(mx, s.actuators[1]).set('ctrl', [6]) np.testing.assert_array_equal(dx4.bind(mx, s.actuators).ctrl, [0, 6, 0]) np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) + dx5 = dx.bind(mx, s.actuators[1]).set('ctrl', 7) + np.testing.assert_array_equal(dx5.bind(mx, s.actuators).ctrl, [0, 7, 0]) + np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, [0, 0, 0]) # test invalid name with self.assertRaises(AttributeError): From 6436055c6cd2ea2c652ef8680b4b631d3be69c27 Mon Sep 17 00:00:00 2001 From: Gabe Oppenheimer Date: Wed, 15 Jan 2025 11:12:28 -0800 Subject: [PATCH 219/426] Update the version number to 3.2.8 following the 3.2.7 release. PiperOrigin-RevId: 715870970 Change-Id: I9fc9a09d959043f3029faa7296a717ea56f75c89 --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aaac4e38..74a133a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.2.7 + VERSION 3.2.8 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index bc490273..6c5e7afc 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,2,7,0 -PRODUCTVERSION 3,2,7,0 +FILEVERSION 3,2,8,0 +PRODUCTVERSION 3,2,8,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.7" + VALUE "ProductVersion", "3.2.8" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.7" + VALUE "FileVersion", "3.2.8" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index a0eb1272..2ed8b9bd 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,2,7,0 -PRODUCTVERSION 3,2,7,0 +FILEVERSION 3,2,8,0 +PRODUCTVERSION 3,2,8,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.2.7" + VALUE "ProductVersion", "3.2.8" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.2.7" + VALUE "FileVersion", "3.2.8" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 5e44d1ea..e2a5725f 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -517,7 +517,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - Maximum number of UI rectangles. Defined in `mjui.h `_. * - ``mjVERSION_HEADER`` - - 327 + - 328 - The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a number with the same meaning but for the compiled library. diff --git a/doc/unity.rst b/doc/unity.rst index 41512767..cb09350e 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -30,14 +30,14 @@ _____ The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as a trusted binary. Then, copy the dynamic library file from -``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.7.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.2.8.dylib`` (it can be found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``. Linux _____ Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from -``~/.mujoco/mujoco-3.2.7/lib/libmujoco.so.3.2.7`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.2.8/lib/libmujoco.so.3.2.8`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 52d73f79..8fb5d3aa 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -16,7 +16,7 @@ #define MUJOCO_MUJOCO_H_ // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 327 +#define mjVERSION_HEADER 328 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index 4d276d40..3a4d2829 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.2.7" +version = "3.2.8" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.2.7.dev0", + "mujoco>=3.2.8.dev0", "scipy", "trimesh", ] @@ -41,9 +41,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.2.7" +Documentation = "https://mujoco.readthedocs.io/en/3.2.8" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.8/changelog.html" [tool.isort] force_single_line = true diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 4301dd0f..f4e28502 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -84,7 +84,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.7.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.2.8.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -92,7 +92,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.2.7 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.2.8 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 204eede8..e02e34d0 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.2.7 + 3.2.8 CFBundleGetInfoString - 3.2.7 + 3.2.8 CFBundleLongVersionString - 3.2.7 + 3.2.8 CFBundleShortVersionString - 3.2.7 + 3.2.8 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 600229f4..0e20b478 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.2.7" +version = "3.2.8" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.2.7" +Documentation = "https://mujoco.readthedocs.io/en/3.2.8" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.2.7/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.2.8/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 3960206a..d3bb7d94 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.2.7 + VERSION 3.2.8 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index ab89442b..2ec43e7a 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.2.7 + VERSION 3.2.8 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a25f874e..e5237a61 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -42,8 +42,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 327 -#define mjVERSIONSTRING "3.2.7" + #define mjVERSION 328 +#define mjVERSIONSTRING "3.2.8" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index b6ed835c..132da3d2 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.2.7.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.2.8.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.2.7/lib/libmujoco.so.3.2.7", + "/.mujoco/mujoco-3.2.8/lib/libmujoco.so.3.2.8", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 19c46aa0..59f65d87 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -109,7 +109,7 @@ public const int mjMAXLINEPNT = 1000; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 327; +public const int mjVERSION_HEADER = 328; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 71d4e34c..378cce99 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.2.7", + "version": "3.2.8", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From c2138c3fb0ec400893cd19fa1cc2c2b4083b7be1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 16 Jan 2025 03:52:31 -0800 Subject: [PATCH 220/426] Do not copy the spec during attach. Use a reference count for managing the memory. PiperOrigin-RevId: 716169486 Change-Id: Id270c4858c17b9250115e9544d5ea143584e2d5f --- doc/APIreference/functions.rst | 9 ++ doc/changelog.rst | 9 +- doc/includes/references.h | 1 + doc/programming/modeledit.rst | 24 ++-- doc/python.rst | 10 +- include/mujoco/mujoco.h | 3 + introspect/functions.py | 18 +++ python/mujoco/specs.cc | 8 ++ python/mujoco/specs_test.py | 40 ++++--- src/user/user_api.cc | 13 ++- src/user/user_api.h | 3 + src/user/user_mesh.cc | 3 - src/user/user_model.cc | 128 +++++++++++++++----- src/user/user_model.h | 9 +- src/user/user_objects.cc | 208 +++++++++++++++++++++------------ src/user/user_objects.h | 26 ++++- src/xml/xml_native_reader.cc | 6 + test/user/user_api_test.cc | 159 +++++++++++++++++++------ test/user/user_model_test.cc | 1 + 19 files changed, 505 insertions(+), 173 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 91fdbd7b..19d792fd 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1510,6 +1510,15 @@ Free memory allocation in mjSpec. Activate plugin. Returns 0 on success. +.. _mjs_setDeepCopy: + +`mjs_setDeepCopy <#mjs_setDeepCopy>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_setDeepCopy + +Turn deep copy on or off attach. Returns 0 on success. + .. _Errorandmemory: Error and memory diff --git a/doc/changelog.rst b/doc/changelog.rst index d9a9936d..c3a0cac9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,8 +2,13 @@ Changelog ========= -Version 3.2.7 (Jan 14, 2025) ----------------------------- +Upcoming version (not yet released) +----------------------------------- + +- Added ``mjs_setDeepCopy`` API function. When the deep copy flag is 0, attaching a model will not copy it to the + parent, so the original references to the child allow to modify the parent as well. The default behavior is to perform + such a shallow copy. The old behavioud of creating a deep copy of the child model while attaching can be restored by + setting the deep copy flag to 1. Python bindings ^^^^^^^^^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index d9742797..26513c8d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3181,6 +3181,7 @@ mjSpec* mj_makeSpec(void); mjSpec* mj_copySpec(const mjSpec* s); void mj_deleteSpec(mjSpec* s); int mjs_activatePlugin(mjSpec* s, const char* name); +int mjs_setDeepCopy(mjSpec* s, int deepcopy); void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format); void mj_printModel(const mjModel* m, const char* filename); void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index eb65b6e7..fa800b67 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -106,12 +106,14 @@ procedurally, default classes are passed in explicitly to element constructors. Attachment ^^^^^^^^^^ -This framework introduces a powerful new feature: attaching and detaching model subtrees. Attachment allows the user -copy a subtree from one model into another, while also copying related referenced assets and referencing elements from -outside the kinematic tree (e.g., actuators and sensors). Similarly, detaching a subtree will remove all associated -elements from the model. This feature is already used to power the :ref:`attach` and -:ref:`replicate` meta-elements in MJCF. It is possible to :ref:`attach a body to a frame` and -to :ref:`attach a body to a site`: + +This framework introduces a powerful new feature: attaching and detaching model subtrees. This feature is already used +to power the :ref:`attach` an :ref:`replicate` meta-elements in MJCF. Attachment allows the user +to move or copy a subtree from one model into another, while also copying or moving related referenced assets and +referencing elements from outside the kinematic tree (e.g., actuators and sensors). Similarly, detaching a subtree will +remove all associated elements from the model. The default behavior is to move during attach. The user can select to +instead copy by passing the corresponding flag to ``mjs_setDeepCopy``. This flag is temporary set to true while parsing +XMLs. It is possible to :ref:`attach a body to a frame`: .. code-block:: C @@ -120,9 +122,17 @@ to :ref:`attach a body to a site`: parent->compiler.degree = 0; child->compiler.degree = 1; mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), NULL); - mjsSite* site = mjs_addSite(mjs_findBody(parent, "world"), NULL); mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), NULL); mjsBody* attached_body_1 = mjs_attachBody(frame, body, "attached-", "-1"); + +or :ref:`attach a body to a site`: + +.. code-block:: C + + mjSpec* parent = mj_makeSpec(); + mjSpec* child = mj_makeSpec(); + mjsSite* site = mjs_addSite(mjs_findBody(parent, "world"), NULL); + mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), NULL); mjsBody* attached_body_2 = mjs_attachToSite(site, body, "attached-", "-2"); or :ref:`attach a frame to a body`: diff --git a/doc/python.rst b/doc/python.rst index 90f861a2..95bd409a 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -531,15 +531,19 @@ Attachment It is possible to combine multiple specs by using attachments. The following options are possible: - Attach a body from the child spec to a frame in the parent spec: ``body.attach_body(body, prefix, suffix)``, returns - the newly createdbody in the parent spec. + the reference to the attached body, which should be identical to the body used as input. - Attach a frame from the child spec to a body in the parent spec: ``body.attach_frame(frame, prefix, suffix)``, - returns the newly created frame in the parent spec. + returns the reference to the attached frame, which should be identical to the frame used as input. - Attach a body from the child spec to a site in the parent spec: ``site.attach(body, prefix, suffix)``, returns the - newly created body in the parent spec. + reference to the attached body, which should be identical to the body used as input. - Attach the worldbody from the child spec to a frame in the parent spec and transform it to a frame: ``body.attach(spec, prefix, suffix)``, returns the newly created frame that the child worldbody was transformed into. +Attaching does not copy, so all the child reference are still valid in the parent and therefore modifying the child will +modify the parent. This is not true for the attach :ref:`attach` an :ref:`replicate` +meta-elements in MJCF, which create deep copies while attaching. + .. code-block:: python import mujoco diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 8fb5d3aa..1fefa52f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -242,6 +242,9 @@ MJAPI void mj_deleteSpec(mjSpec* s); // Activate plugin. Returns 0 on success. MJAPI int mjs_activatePlugin(mjSpec* s, const char* name); +// Turn deep copy on or off attach. Returns 0 on success. +MJAPI int mjs_setDeepCopy(mjSpec* s, int deepcopy); + //---------------------------------- Printing ------------------------------------------------------ diff --git a/introspect/functions.py b/introspect/functions.py index 4fd2374a..cdde54ff 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -1045,6 +1045,24 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Activate plugin. Returns 0 on success.', )), + ('mjs_setDeepCopy', + FunctionDecl( + name='mjs_setDeepCopy', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), + FunctionParameterDecl( + name='deepcopy', + type=ValueType(name='int'), + ), + ), + doc='Turn deep copy on or off attach. Returns 0 on success.', + )), ('mj_printFormattedModel', FunctionDecl( name='mj_printFormattedModel', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index a0dc13e4..2f37f64f 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -350,6 +350,14 @@ PYBIND11_MODULE(_specs, m) { mjSpec.def("copy", [](const MjSpec& self) -> MjSpec { return MjSpec(self); }); + mjSpec.def_property( + "copy_during_attach", + [](MjSpec& self) { + throw pybind11::value_error("copy_during_attach can only be set."); + }, + [](MjSpec& self, bool deepcopy) { + return mjs_setDeepCopy(self.ptr, deepcopy); + }); mjSpec.def_property_readonly( "worldbody", [](MjSpec& self) -> raw::MjsBody* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index dd8a6df7..d7b86481 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -918,27 +918,31 @@ class SpecsTest(absltest.TestCase): model = parent.compile() np.testing.assert_almost_equal(model.body_quat[1], [1, 0, 0, 0]) - def test_attach_body_to_site(self): - child = mujoco.MjSpec() + def test_attach_to_site(self): parent = mujoco.MjSpec() site = parent.worldbody.add_site(pos=[1, 2, 3], quat=[0, 0, 0, 1]) - body = child.worldbody.add_body() # Attach body to site and compile. - self.assertIsNotNone(site.attach_body(body, prefix='_')) + child1 = mujoco.MjSpec() + body1 = child1.worldbody.add_body() + self.assertIs(body1, site.attach_body(body1, prefix='_')) + body1.pos = [1, 1, 1] model1 = parent.compile() self.assertIsNotNone(model1) self.assertEqual(model1.nbody, 2) - np.testing.assert_array_equal(model1.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4]) np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) # Attach entire spec to site and compile again. - self.assertIsNotNone(site.attach(child, prefix='child-')) + child2 = mujoco.MjSpec() + body2 = child2.worldbody.add_body(name='body') + self.assertIsNotNone(site.attach(child2, prefix='child-')) + body2.pos = [-1, -1, -1] model2 = parent.compile() self.assertIsNotNone(model2) self.assertEqual(model2.nbody, 3) - np.testing.assert_array_equal(model2.body_pos[1], [1, 2, 3]) - np.testing.assert_array_equal(model2.body_pos[2], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_pos[1], [0, 1, 4]) + np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2]) np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) @@ -949,27 +953,31 @@ class SpecsTest(absltest.TestCase): frame = body.to_frame() np.testing.assert_array_equal(frame.pos, [1, 2, 3]) - def test_attach_spec_to_frame(self): - child = mujoco.MjSpec() + def test_attach_to_frame(self): parent = mujoco.MjSpec() frame = parent.worldbody.add_frame(pos=[1, 2, 3], quat=[0, 0, 0, 1]) - body = child.worldbody.add_body() # Attach body to frame and compile. - self.assertIsNotNone(frame.attach_body(body, prefix='_')) + child1 = mujoco.MjSpec() + body1 = child1.worldbody.add_body() + self.assertIs(body1, frame.attach_body(body1, prefix='_')) + body1.pos = [1, 1, 1] model1 = parent.compile() self.assertIsNotNone(model1) self.assertEqual(model1.nbody, 2) - np.testing.assert_array_equal(model1.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4]) np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) # Attach entire spec to frame and compile again. - self.assertIsNotNone(frame.attach(child, prefix='child-')) + child2 = mujoco.MjSpec() + body2 = child2.worldbody.add_body(name='body') + self.assertIsNotNone(frame.attach(child2, prefix='child-')) + body2.pos = [-1, -1, -1] model2 = parent.compile() self.assertIsNotNone(model2) self.assertEqual(model2.nbody, 3) - np.testing.assert_array_equal(model2.body_pos[1], [1, 2, 3]) - np.testing.assert_array_equal(model2.body_pos[2], [1, 2, 3]) + np.testing.assert_array_equal(model2.body_pos[1], [0, 1, 4]) + np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2]) np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 8eb612a7..5c9bb28f 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -208,7 +208,7 @@ int mjs_detachBody(mjSpec* s, mjsBody* b) { model->SetError(e); return -1; } - delete body; + model->Detach(body); return 0; } @@ -254,6 +254,15 @@ int mjs_activatePlugin(mjSpec* s, const char* name) { +// set deep copy flag +int mjs_setDeepCopy(mjSpec* s, int deepcopy) { + mjCModel* model = static_cast(s->element); + model->SetDeepCopy(deepcopy); + return 0; +} + + + // delete object, return 0 if success int mjs_delete(mjsElement* element) { mjCBase* object = static_cast(element); @@ -705,7 +714,7 @@ const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* s mjsFrame* mjs_bodyToFrame(mjsBody** body) { mjCBody* bodyC = static_cast((*body)->element); mjCFrame* frameC = bodyC->ToFrame(); - delete bodyC; + bodyC->model->Detach(bodyC); *body = nullptr; return &frameC->spec; } diff --git a/src/user/user_api.h b/src/user/user_api.h index b6722710..89079048 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -63,6 +63,9 @@ MJAPI void mjs_addSpec(mjSpec* s, mjSpec* child); // Activate plugin, return 0 on success. MJAPI int mjs_activatePlugin(mjSpec* s, const char* name); +// Turn deep copy on or off attach. Returns 0 on success. +MJAPI int mjs_setDeepCopy(mjSpec* s, int deepcopy); + //---------------------------------- Attachment ---------------------------------------------------- diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 4de4e2e3..2e349459 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -278,9 +278,6 @@ void mjCMesh::CopyPlugin() { mjCMesh::~mjCMesh() { if (center_) mju_free(center_); if (graph_) mju_free(graph_); - if (spec.plugin.active && spec.plugin.name->empty() && model) { - model->DeleteElement(spec.plugin.element); - } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 55fcb0d8..532eece0 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -139,6 +139,7 @@ mjCModel::mjCModel() { center_auto[0] = center_auto[1] = center_auto[2] = 0; #endif + deepcopy_ = false; nplugin = 0; Clear(); @@ -181,6 +182,7 @@ mjCModel::mjCModel(const mjCModel& other) { mjCModel& mjCModel::operator=(const mjCModel& other) { + deepcopy_ = true; if (this != &other) { this->spec = other.spec; *static_cast(this) = static_cast(other); @@ -210,6 +212,7 @@ mjCModel& mjCModel::operator=(const mjCModel& other) { ids[i] = other.ids[i]; } } + deepcopy_ = other.deepcopy_; return *this; } @@ -222,20 +225,29 @@ void mjCModel::CopyList(std::vector& dest, // loop over the elements from the other model int nsource = (int)source.size(); for (int i = 0; i < nsource; i++) { - T* candidate = new T(*source[i]); + T* candidate = deepcopy_ ? new T(*source[i]) : source[i]; try { // try to find the referenced object in this model - candidate->NameSpace(source[i]->model); + mjCModel* source_model = source[i]->model; + candidate->model = this; + candidate->NameSpace(source_model); candidate->CopyFromSpec(); candidate->ResolveReferences(this); } catch (mjCError err) { // if not present, skip the element // TODO: do not skip elements that contain user errors - delete candidate; + if (deepcopy_) { + candidate->model = nullptr; + delete candidate; + } continue; } // copy the element from the other model to this model - source[i]->ForgetKeyframes(); + if (deepcopy_) { + source[i]->ForgetKeyframes(); + } else { + candidate->AddRef(); + } mjSpec* origin = FindSpec(source[i]->compiler); dest.push_back(candidate); dest.back()->model = this; @@ -324,9 +336,12 @@ void mjCModel::CopyExplicitPlugin(T* obj) { return; } mjCPlugin* origin = static_cast(obj->spec.plugin.element); - mjCPlugin* candidate = new mjCPlugin(*origin); + mjCPlugin* candidate = deepcopy_ ? new mjCPlugin(*origin) : origin; candidate->id = plugins_.size(); candidate->model = this; + if (!deepcopy_) { + candidate->AddRef(); + } plugins_.push_back(candidate); obj->spec.plugin.element = candidate; } @@ -566,7 +581,7 @@ void deletefromlist(std::vector* list, mjsElement* element) { for (int j = 0; j < list->size(); ++j) { list->at(j)->id = -1; if (list->at(j) == element) { - delete list->at(j); + list->at(j)->Release(); list->erase(list->begin() + j); j--; } @@ -577,8 +592,9 @@ void deletefromlist(std::vector* list, mjsElement* element) { // discard all invalid elements from all lists void mjCModel::DeleteElement(mjsElement* el) { - mjCBody *world = bodies_[0]; + mjCBody *world = nullptr; if (compiled) { + world = bodies_[0]; ResetTreeLists(); } @@ -588,8 +604,14 @@ void mjCModel::DeleteElement(mjsElement* el) { break; case mjOBJ_GEOM: - deletefromlist(&(static_cast(el)->body->geoms), el); + { + mjCGeom* geom = static_cast(el); + if (geom->plugin.active && geom->plugin.name->empty() && geom->GetRef() == 1) { + DeleteElement(geom->plugin.element); + } + deletefromlist(&(geom->body->geoms), el); break; + } case mjOBJ_SITE: deletefromlist(&(static_cast(el)->body->sites), el); @@ -607,6 +629,36 @@ void mjCModel::DeleteElement(mjsElement* el) { deletefromlist(&(static_cast(el)->body->cameras), el); break; + case mjOBJ_MESH: + { + mjCMesh* mesh = static_cast(el); + if (mesh->plugin.active && mesh->plugin.name->empty() && mesh->GetRef() == 1) { + DeleteElement(mesh->plugin.element); + } + deletefromlist(object_lists_[mjOBJ_MESH], el); + break; + } + + case mjOBJ_ACTUATOR: + { + mjCActuator* actuator = static_cast(el); + if (actuator->plugin.active && actuator->plugin.name->empty() && actuator->GetRef() == 1) { + DeleteElement(actuator->plugin.element); + } + deletefromlist(object_lists_[mjOBJ_ACTUATOR], el); + break; + } + + case mjOBJ_SENSOR: + { + mjCSensor* sensor = static_cast(el); + if (sensor->plugin.active && sensor->plugin.name->empty() && sensor->GetRef() == 1) { + DeleteElement(sensor->plugin.element); + } + deletefromlist(object_lists_[mjOBJ_SENSOR], el); + break; + } + default: deletefromlist(object_lists_[el->elemtype], el); break; @@ -621,6 +673,29 @@ void mjCModel::DeleteElement(mjsElement* el) { +// recursively delete all plugins in the subtree +void deletesubtreeplugin(mjCBody* subtree, mjCModel* model) { + mjsPlugin* plugin = &(subtree->spec.plugin); + if (plugin->active && plugin->name->empty()) { + model->DeleteElement(plugin->element); + } + for (auto* body : subtree->Bodies()) { + deletesubtreeplugin(body, model); + } +} + + + +// deletes all plugins in the subtree and then the subtree itself +void mjCModel::Detach(mjCBody* subtree) { + if (subtree->GetRef() == 1) { + deletesubtreeplugin(subtree, this); + } + subtree->Release(); +} + + + // TODO: we should not use C-type casting with multiple C++ inheritance void mjCModel::CreateObjectLists() { for (int i = 0; i < mjNOBJECT; ++i) { @@ -688,28 +763,28 @@ mjCModel::~mjCModel() { compiled = false; // delete kinematic tree and all objects allocated in it - delete bodies_[0]; + bodies_[0]->Release(); // delete objects allocated in mjCModel - for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); // also deletes wraps + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); for (int i=0; iRelease(); // clear sizes and pointer lists created in Compile Clear(); @@ -933,6 +1008,7 @@ mjCPlugin* mjCModel::AddPlugin() { // append spec to spec void mjCModel::AppendSpec(mjSpec* spec) { + // TODO: check if the spec is already in the list specs_.push_back(spec); } diff --git a/src/user/user_model.h b/src/user/user_model.h index 9109018c..f1023e48 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -221,6 +221,9 @@ class mjCModel : public mjCModel_, private mjSpec { // delete object from the corresponding list void DeleteElement(mjsElement* el); + // detach subtree from model + void Detach(mjCBody* subtree); + // API for access to model elements (outside tree) int NumObjects(mjtObj type); // number of objects in specified list mjCBase* GetObject(mjtObj type, int id); // pointer to specified object @@ -307,6 +310,9 @@ class mjCModel : public mjCModel_, private mjSpec { // get the spec from which this model was created mjSpec* GetSourceSpec() const; + // set deepcopy flag + void SetDeepCopy(bool deepcopy) { deepcopy_ = deepcopy; } + private: // settings for each defaults class std::vector defaults_; @@ -351,7 +357,7 @@ class mjCModel : public mjCModel_, private mjSpec { std::vector tuples_; // list of tuple fields std::vector keys_; // list of keyframe fields std::vector plugins_; // list of plugin instances - std::vector specs_; // list of specs + std::vector specs_; // list of attached specs // pointers to objects created inside kinematic tree std::vector bodies_; // list of bodies @@ -410,5 +416,6 @@ class mjCModel : public mjCModel_, private mjSpec { mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info std::vector key_pending_; // attached keyframes + bool deepcopy_; // copy objects when attaching }; #endif // MUJOCO_SRC_USER_USER_MODEL_H_ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c96872f5..82c46acd 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -763,6 +763,7 @@ mjCBody::mjCBody(mjCModel* _model) { model = _model; if (_model) compiler = &_model->spec.compiler; + refcount = 1; mjs_defaultBody(&spec); elemtype = mjOBJ_BODY; parent = nullptr; @@ -872,22 +873,28 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { other.model->prefix = other.prefix; other.model->suffix = other.suffix; other.model->StoreKeyframes(model); + mjCModel* other_model = other.model; // attach defaults - if (other.model != model) { - mjCDef* subdef = new mjCDef(*other.model->Default()); - subdef->NameSpace(other.model); + if (other_model != model) { + mjCDef* subdef = new mjCDef(*other_model->Default()); + subdef->NameSpace(other_model); *model += *subdef; } // copy input frame mjSpec* origin = model->FindSpec(other.compiler); - frames.push_back(new mjCFrame(other)); + mjCFrame* newframe(model->deepcopy_ ? new mjCFrame(other) : (mjCFrame*)&other); + frames.push_back(newframe); frames.back()->body = this; frames.back()->model = model; frames.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; frames.back()->frame = other.frame; - frames.back()->NameSpace(other.model); + if (model->deepcopy_) { + frames.back()->NameSpace(other_model); + } else { + frames.back()->AddRef(); + } int i = frames.size(); last_attached = &frames.back()->spec; @@ -909,30 +916,43 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { CopyList(cameras, subtree->cameras, fmap, &other); CopyList(lights, subtree->lights, fmap, &other); + if (!model->deepcopy_) { + subtree->SetModel(model); + subtree->NameSpace(other_model); + } + int nbodies = (int)subtree->bodies.size(); for (int i=0; ibodies[i]->frame)) { continue; } - bodies.push_back(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call + if (model->deepcopy_) { + mjCBody* newbody(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call + bodies.push_back(newbody); + subtree->bodies[i]->ForgetKeyframes(); + bodies.back()->NameSpace_(other_model, /*propagate=*/ false); + } else { + bodies.push_back(subtree->bodies[i]); + bodies.back()->SetModel(model); + bodies.back()->ResetId(); + bodies.back()->AddRef(); + } bodies.back()->parent = this; bodies.back()->frame = subtree->bodies[i]->frame ? frames[fmap[subtree->bodies[i]->frame]] : nullptr; - bodies.back()->NameSpace_(other.model, /*propagate=*/ false); - subtree->bodies[i]->ForgetKeyframes(); } // attach referencing elements - *model += *other.model; + *model += *other_model; // leave the source model in a clean state - if (other.model != model) { - other.model->key_pending_.clear(); + if (other_model != model) { + other_model->key_pending_.clear(); } // clear namespace and return body - other.model->prefix.clear(); - other.model->suffix.clear(); + other_model->prefix.clear(); + other_model->suffix.clear(); return *this; } @@ -948,7 +968,8 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, continue; // skip if the element is not inside pframe } mjSpec* origin = model->FindSpec(src[i]->compiler); - dst.push_back(new T(*src[i])); + T* new_obj = model->deepcopy_ ? new T(*src[i]) : src[i]; + dst.push_back(new_obj); dst.back()->body = this; dst.back()->model = model; dst.back()->compiler = origin ? &origin->compiler : &model->spec.compiler; @@ -956,6 +977,11 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, dst.back()->CopyPlugin(); dst.back()->classname = src[i]->classname; + // increment refcount if shallow copy is made + if (!model->deepcopy_) { + dst.back()->AddRef(); + } + // assign dst frame to src frame dst.back()->frame = src[i]->frame ? frames[fmap[src[i]->frame]] : nullptr; @@ -981,6 +1007,73 @@ mjCBody& mjCBody::operator-=(const mjCBody& subtree) { +// set model of this body and its subtree +void mjCBody::SetModel(mjCModel* _model) { + model = _model; + mjSpec* origin = model->FindSpec(mjs_getString(model->spec.modelname)); + compiler = origin ? &origin->compiler : &model->spec.compiler; + + for (auto& body : bodies) { + body->SetModel(_model); + } + for (auto& frame : frames) { + frame->model = _model; + frame->compiler = compiler; + } + for (auto& geom : geoms) { + geom->model = _model; + geom->compiler = compiler; + } + for (auto& joint : joints) { + joint->model = _model; + joint->compiler = compiler; + } + for (auto& site : sites) { + site->model = _model; + site->compiler = compiler; + } + for (auto& camera : cameras) { + camera->model = _model; + camera->compiler = compiler; + } + for (auto& light : lights) { + light->model = _model; + light->compiler = compiler; + } +} + + + +// reset ids of all objects in this body +void mjCBody::ResetId() { + id = -1; + for (auto& body : bodies) { + body->ResetId(); + } + for (auto& frame : frames) { + frame->id = -1; + } + for (auto& geom : geoms) { + geom->id = -1; + } + for (auto& joint : joints) { + joint->id = -1; + joint->qposadr_ = -1; + joint->dofadr_ = -1; + } + for (auto& site : sites) { + site->id = -1; + } + for (auto& camera : cameras) { + camera->id = -1; + } + for (auto& light : lights) { + light->id = -1; + } +} + + + void mjCBody::PointToLocal() { spec.element = static_cast(this); spec.name = &name; @@ -1012,26 +1105,13 @@ void mjCBody::CopyPlugin() { // destructor mjCBody::~mjCBody() { - // delete objects allocated here - for (int i=0; iempty() && model) { - model->DeleteElement(spec.plugin.element); - } + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); + for (int i=0; iRelease(); } @@ -1840,19 +1920,27 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { other.model->StoreKeyframes(model); other.model->prefix = ""; other.model->suffix = ""; + mjCModel* other_model = other.model; - mjCBody* subtree = new mjCBody(other, model); - other.ForgetKeyframes(); - other.model->prefix = subtree->prefix; - other.model->suffix = subtree->suffix; + // attach or copy the subtree + mjCBody* subtree = model->deepcopy_ ? new mjCBody(other, model) : (mjCBody*)&other; + if (model->deepcopy_) { + other.ForgetKeyframes(); + } else { + subtree->SetModel(model); + subtree->ResetId(); + subtree->AddRef(); + } + other_model->prefix = subtree->prefix; + other_model->suffix = subtree->suffix; subtree->SetParent(body); subtree->SetFrame(this); - subtree->NameSpace(other.model); + subtree->NameSpace(other_model); // attach defaults - if (other.model != model) { - mjCDef* subdef = new mjCDef(*other.model->Default()); - subdef->NameSpace(other.model); + if (other_model != model) { + mjCDef* subdef = new mjCDef(*other_model->Default()); + subdef->NameSpace(other_model); *model += *subdef; } @@ -1861,16 +1949,16 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { last_attached = &body->bodies.back()->spec; // attach referencing elements - *model += *other.model; + *model += *other_model; // leave the source model in a clean state - if (other.model != model) { - other.model->key_pending_.clear(); + if (other_model != model) { + other_model->key_pending_.clear(); } // clear suffixes and return - other.model->suffix.clear(); - other.model->prefix.clear(); + other_model->suffix.clear(); + other_model->prefix.clear(); return *this; } @@ -2221,14 +2309,6 @@ mjCGeom::mjCGeom(const mjCGeom& other) { -mjCGeom::~mjCGeom() { - if (spec.plugin.active && spec.plugin.name->empty() && model) { - model->DeleteElement(spec.plugin.element); - } -} - - - mjCGeom& mjCGeom::operator=(const mjCGeom& other) { if (this != &other) { this->spec = other.spec; @@ -4591,7 +4671,7 @@ void mjCMaterial::CopyFromSpec() { void mjCMaterial::NameSpace(const mjCModel* m) { mjCBase::NameSpace(m); for (int i=0; iprefix + spec_textures_[i] + m->suffix; } } @@ -5657,14 +5737,6 @@ mjCActuator::mjCActuator(const mjCActuator& other) { -mjCActuator::~mjCActuator() { - if (spec.plugin.active && spec.plugin.name->empty() && model) { - model->DeleteElement(spec.plugin.element); - } -} - - - mjCActuator& mjCActuator::operator=(const mjCActuator& other) { if (this != &other) { this->spec = other.spec; @@ -6033,14 +6105,6 @@ mjCSensor::mjCSensor(const mjCSensor& other) { -mjCSensor::~mjCSensor() { - if (spec.plugin.active && spec.plugin.name->empty() && model) { - model->DeleteElement(spec.plugin.element); - } -} - - - mjCSensor& mjCSensor::operator=(const mjCSensor& other) { if (this != &other) { this->spec = other.spec; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ca13a741..0d093c0e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -224,9 +224,23 @@ class mjCBase : public mjCBase_ { virtual void ForgetKeyframes() {} virtual void ForgetKeyframes() const {} + // increment and decrement reference count + // release uses the argument to delete the plugin + // which may be still owned by the source spec during shallow attach + virtual void AddRef() { ++refcount; } + virtual int GetRef() { return refcount; } + virtual void Release() { + if (--refcount == 0) { + delete this; + } + } + protected: mjCBase(); // constructor mjCBase(const mjCBase& other); // copy constructor + + // reference count for allowing deleting an attached object + int refcount = 1; }; @@ -349,6 +363,15 @@ class mjCBody : public mjCBody_, private mjsBody { void SetParent(mjCBody* _body) { parent = _body; } mjCBody* GetParent() const { return parent; } + // set model of this body + void SetModel(mjCModel* _model); + + // reset ids of all objects in this body + void ResetId(); + + // getters + std::vector Bodies() const { return bodies; } + private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor mjCBody& operator=(const mjCBody& other); // copy assignment @@ -537,7 +560,6 @@ class mjCGeom : public mjCGeom_, private mjsGeom { mjCGeom(mjCModel* = nullptr, mjCDef* = nullptr); mjCGeom(const mjCGeom& other); mjCGeom& operator=(const mjCGeom& other); - ~mjCGeom(); using mjCBase::name; mjsGeom spec; // variables set by user @@ -1505,7 +1527,6 @@ class mjCActuator : public mjCActuator_, private mjsActuator { mjCActuator(mjCModel* = nullptr, mjCDef* = nullptr); mjCActuator(const mjCActuator& other); mjCActuator& operator=(const mjCActuator& other); - ~mjCActuator(); mjsActuator spec; using mjCBase::name; @@ -1567,7 +1588,6 @@ class mjCSensor : public mjCSensor_, private mjsSensor { mjCSensor(mjCModel*); mjCSensor(const mjCSensor& other); mjCSensor& operator=(const mjCSensor& other); - ~mjCSensor(); mjsSensor spec; using mjCBase::name; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index f07fa341..6fe26b27 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -945,10 +945,16 @@ void mjXReader::Parse(XMLElement* root, const mjVFS* vfs) { Keyframe(section); } + // set deepcopy flag to true to copy child specs during attach calls + mjs_setDeepCopy(spec, true); + for (XMLElement* section = FirstChildElement(root, "worldbody"); section; section = NextSiblingElement(section, "worldbody")) { Body(section, mjs_findBody(spec, "world"), nullptr, vfs); } + + // set deepcopy flag to false to disable copying during attach in all future calls + mjs_setDeepCopy(spec, false); } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 7394c7f1..31b7af43 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -262,35 +262,48 @@ TEST_F(PluginTest, AttachPlugin) { )"; std::array err; - mjSpec* spec_1 = mj_parseXMLString(xml_1, 0, err.data(), err.size()); + mjSpec* parent = mj_parseXMLString(xml_1, 0, err.data(), err.size()); + ASSERT_THAT(parent, NotNull()) << err.data(); + mjSpec* spec_1 = mj_parseXMLString(xml_2, 0, err.data(), err.size()); ASSERT_THAT(spec_1, NotNull()) << err.data(); - mjSpec* spec_2 = mj_parseXMLString(xml_2, 0, err.data(), err.size()); - ASSERT_THAT(spec_2, NotNull()) << err.data(); - mjsBody* body_1 = mjs_findBody(spec_1, "body"); + // do a copy before attaching + mjSpec* spec_2 = mj_copySpec(spec_1); + mjs_setString(spec_2->modelname, "first_copy"); + mjSpec* spec_3 = mj_copySpec(spec_1); + mjs_setString(spec_3->modelname, "second_copy"); + ASSERT_THAT(spec_3, NotNull()) << err.data(); + + // attach a body referencing the plugin to the frame and compile + mjsBody* body_1 = mjs_findBody(parent, "body"); EXPECT_THAT(body_1, NotNull()); mjsFrame* attachment_frame = mjs_addFrame(body_1, 0); EXPECT_THAT(attachment_frame, NotNull()); - - mjs_attachBody(attachment_frame, mjs_findBody(spec_2, "body"), "child-", ""); - mjModel* model_1 = mj_compile(spec_1, nullptr); + mjs_attachBody(attachment_frame, mjs_findBody(spec_1, "body"), "child-", ""); + mjModel* model_1 = mj_compile(parent, nullptr); EXPECT_THAT(model_1, NotNull()); + EXPECT_THAT(model_1->nbody, 3); - // attach it a second time to test namespacing + // attach it a second time to test namespacing and compile + ASSERT_THAT(spec_2, NotNull()) << err.data(); mjs_attachBody(attachment_frame, mjs_findBody(spec_2, "body"), "copy-", ""); - mjModel* model_2 = mj_compile(spec_1, nullptr); + mjModel* model_2 = mj_compile(parent, nullptr); EXPECT_THAT(model_2, NotNull()); + EXPECT_THAT(model_2->nbody, 4); - // attach a body not referencing the plugin - mjs_attachBody(attachment_frame, mjs_findBody(spec_2, "empty"), "empty-", ""); - mjModel* model_3 = mj_compile(spec_1, nullptr); + // attach a body not referencing the plugin and compile + mjs_attachBody(attachment_frame, mjs_findBody(spec_3, "empty"), "empty-", ""); + mjModel* model_3 = mj_compile(parent, nullptr); EXPECT_THAT(model_3, NotNull()); + EXPECT_THAT(model_3->nbody, 5); mj_deleteModel(model_1); mj_deleteModel(model_2); mj_deleteModel(model_3); + mj_deleteSpec(parent); mj_deleteSpec(spec_1); mj_deleteSpec(spec_2); + mj_deleteSpec(spec_3); } TEST_F(PluginTest, AttachExplicitPlugin) { @@ -858,6 +871,7 @@ TEST_F(MujocoTest, AttachSame) { // create parent mjSpec* parent = mj_parseXMLString(xml_child, 0, er.data(), er.size()); EXPECT_THAT(parent, NotNull()) << er.data(); + mjs_setDeepCopy(parent, true); // needed for self-attach // get frame mjsFrame* frame = mjs_findFrame(parent, "frame"); @@ -903,7 +917,7 @@ TEST_F(MujocoTest, AttachDifferent) { std::string field = ""; static constexpr char xml_parent[] = R"( - + @@ -924,7 +938,7 @@ TEST_F(MujocoTest, AttachDifferent) { )"; static constexpr char xml_result[] = R"( - + @@ -1048,7 +1062,7 @@ TEST_F(MujocoTest, AttachFrame) { std::string field = ""; static constexpr char xml_parent[] = R"( - + @@ -1063,7 +1077,7 @@ TEST_F(MujocoTest, AttachFrame) { )"; static constexpr char xml_result[] = R"( - + @@ -1371,13 +1385,15 @@ TEST_F(MujocoTest, AttachWorld) { mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size()); EXPECT_THAT(parent, NotNull()) << er.data(); - mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size()); - EXPECT_THAT(child, NotNull()) << er.data(); + mjSpec* child1 = mj_parseXMLString(xml_child, 0, er.data(), er.size()); + EXPECT_THAT(child1, NotNull()) << er.data(); + mjSpec* child2 = mj_parseXMLString(xml_child, 0, er.data(), er.size()); + EXPECT_THAT(child2, NotNull()) << er.data(); // attach a body to the frame mjsFrame* frame = mjs_findFrame(parent, "frame"); EXPECT_THAT(frame, NotNull()); - mjsBody* body = mjs_findBody(child, "sphere"); + mjsBody* body = mjs_findBody(child1, "sphere"); EXPECT_THAT(body, NotNull()); mjsBody* attached = mjs_attachBody(frame, body, "attached-", "-1"); EXPECT_THAT(attached, NotNull()); @@ -1385,7 +1401,7 @@ TEST_F(MujocoTest, AttachWorld) { EXPECT_THAT(model1, NotNull()); // attach the world to the same frame and convert it to a frame - mjsBody* world = mjs_findBody(child, "world"); + mjsBody* world = mjs_findBody(child2, "world"); EXPECT_THAT(world, NotNull()); mjsBody* child_world = mjs_attachBody(frame, world, "attached-", "-2"); EXPECT_THAT(child_world, NotNull()); @@ -1403,7 +1419,8 @@ TEST_F(MujocoTest, AttachWorld) { << "Different field: " << field << '\n'; mj_deleteSpec(parent); - mj_deleteSpec(child); + mj_deleteSpec(child1); + mj_deleteSpec(child2); mj_deleteModel(model1); mj_deleteModel(model2); mj_deleteModel(expected); @@ -1577,11 +1594,13 @@ TEST_F(MujocoTest, RecompileAttach) { mjSpec* parent = mj_makeSpec(); EXPECT_THAT(parent, NotNull()); - mjSpec* child = mj_parseXMLString(xml, 0, er.data(), er.size()); - EXPECT_THAT(child, NotNull()); + mjSpec* child1 = mj_parseXMLString(xml, 0, er.data(), er.size()); + EXPECT_THAT(child1, NotNull()); + mjSpec* child2 = mj_parseXMLString(xml, 0, er.data(), er.size()); + EXPECT_THAT(child2, NotNull()); mjsFrame* frame1 = mjs_addFrame(mjs_findBody(parent, "world"), 0); - mjs_attachBody(frame1, mjs_findBody(child, "body"), "child-", "-1"); + mjs_attachBody(frame1, mjs_findBody(child1, "body"), "child-", "-1"); mjModel* model = mj_compile(parent, 0); EXPECT_THAT(model, NotNull()); @@ -1594,7 +1613,7 @@ TEST_F(MujocoTest, RecompileAttach) { } mjsFrame* frame2 = mjs_addFrame(mjs_findBody(parent, "world"), 0); - mjs_attachBody(frame2, mjs_findBody(child, "body"), "child-", "-2"); + mjs_attachBody(frame2, mjs_findBody(child2, "body"), "child-", "-2"); EXPECT_EQ(mj_recompile(parent, 0, model, data), 0); EXPECT_THAT(model, NotNull()); @@ -1604,7 +1623,8 @@ TEST_F(MujocoTest, RecompileAttach) { mj_deleteData(data); mj_deleteModel(model); - mj_deleteSpec(child); + mj_deleteSpec(child1); + mj_deleteSpec(child2); mj_deleteSpec(parent); } @@ -1637,6 +1657,7 @@ TEST_F(MujocoTest, AttachMocap) { mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); EXPECT_THAT(spec, NotNull()) << er.data(); + mjs_setDeepCopy(spec, true); // needed for self-attach mjsBody* body = mjs_findBody(spec, "mocap"); EXPECT_THAT(body, NotNull()); @@ -1732,8 +1753,8 @@ TEST_F(MujocoTest, AttachUnnamedAssets) { EXPECT_STREQ(mj_id2name(model, mjOBJ_MESH, 0), "_cube"); mj_deleteVFS(vfs.get()); - mj_deleteSpec(child); mj_deleteSpec(spec); + mj_deleteSpec(child); mj_deleteModel(model); } @@ -1866,6 +1887,9 @@ void AttachNestedKeyframe(bool compile) { mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, er.data(), er.size()); EXPECT_THAT(gchild, NotNull()) << er.data(); + mjs_setDeepCopy(parent, true); + mjs_setDeepCopy(child, true); + // attach gchild to child mjs_attachBody(mjs_findFrame(child, "frame"), mjs_findBody(gchild, "body"), "gchild-", ""); @@ -1937,18 +1961,18 @@ TEST_F(MujocoTest, RepeatedAttachKeyframe) { )"; std::array er; - mjSpec* spec_1 = mj_parseXMLString(xml_1, 0, er.data(), er.size()); - EXPECT_THAT(spec_1, NotNull()) << er.data(); - mjSpec* spec_2 = mj_parseXMLString(xml_2, 0, er.data(), er.size()); - EXPECT_THAT(spec_2, NotNull()) << er.data(); + mjSpec* parent = mj_parseXMLString(xml_1, 0, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + mjSpec* child = mj_parseXMLString(xml_2, 0, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); - mjsBody* body_1 = mjs_findBody(spec_1, "body"); + mjsBody* body_1 = mjs_findBody(parent, "body"); mjsFrame* attachment_frame = mjs_addFrame(body_1, 0); - mjs_attachBody(attachment_frame, mjs_findBody(spec_2, "b1"), "b1-", ""); - mjModel* model_1 = mj_compile(spec_1, 0); + mjs_attachBody(attachment_frame, mjs_findBody(child, "b1"), "b1-", ""); + mjModel* model_1 = mj_compile(parent, 0); EXPECT_THAT(model_1, NotNull()); - mjs_attachBody(attachment_frame, mjs_findBody(spec_2, "b2"), "b2-", ""); - mjModel* model_2 = mj_compile(spec_1, 0); + mjs_attachBody(attachment_frame, mjs_findBody(child, "b2"), "b2-", ""); + mjModel* model_2 = mj_compile(parent, 0); EXPECT_THAT(model_2, NotNull()); EXPECT_EQ(model_1->nkey, 1); @@ -1956,8 +1980,8 @@ TEST_F(MujocoTest, RepeatedAttachKeyframe) { EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 0), "b1-home"); EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 1), "b2-home"); - mj_deleteSpec(spec_1); - mj_deleteSpec(spec_2); + mj_deleteSpec(parent); + mj_deleteSpec(child); mj_deleteModel(model_1); mj_deleteModel(model_2); } @@ -2177,5 +2201,64 @@ TEST_F(MujocoTest, CopyAttachedSpec) { mj_deleteVFS(vfs.get()); } +TEST_F(MujocoTest, ApplyNameSpaceToDefaults) { + static constexpr char xml_c[] = R"( + + + + + + + + + + + + + + + )"; + + static constexpr char xml_p[] = R"( + + + + + + )"; + + static constexpr char cube[] = R"( + v -0.500000 -0.500000 0.500000 + v 0.500000 -0.500000 0.500000 + v -0.500000 0.500000 0.500000 + v 0.500000 0.500000 0.500000 + v -0.500000 0.500000 -0.500000 + v 0.500000 0.500000 -0.500000 + v -0.500000 -0.500000 -0.500000 + v 0.500000 -0.500000 -0.500000)"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube)); + + std::array err; + mjSpec* child = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size()); + EXPECT_THAT(child, NotNull()) << err.data(); + mjSpec* parent = mj_parseXMLString(xml_p, 0, err.data(), err.size()); + EXPECT_THAT(parent, NotNull()) << err.data(); + + mjsBody* attached = mjs_attachBody(mjs_findFrame(parent, "parent"), + mjs_findBody(child, "body"), "child-", ""); + EXPECT_THAT(attached, NotNull()); + + mjModel* model = mj_compile(parent, vfs.get()); + EXPECT_THAT(model, NotNull()); + + mj_deleteSpec(child); + mj_deleteSpec(parent); + mj_deleteModel(model); + mj_deleteVFS(vfs.get()); +} + } // namespace } // namespace mujoco diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 811004a7..0c3cddac 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -545,6 +545,7 @@ TEST_F(MujocoTest, Modeldir) { // parent attaching the child mjSpec* spec = mj_makeSpec(); + mjs_setDeepCopy(spec, true); mjs_setString(spec->meshdir, "asset"); mjs_attachFrame(mjs_findBody(spec, "world"), frame, "_", ""); mjModel* model = mj_compile(spec, vfs.get()); From 26bac8c8af23e25705f08b28a409649a6f23e26b Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Thu, 16 Jan 2025 05:20:54 -0800 Subject: [PATCH 221/426] Fix changelog. PiperOrigin-RevId: 716194356 Change-Id: I23e1717867847c342d7bac82d2fae7d26df48473 --- doc/changelog.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index c3a0cac9..58a1bfe8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,9 @@ Upcoming version (not yet released) such a shallow copy. The old behavioud of creating a deep copy of the child model while attaching can be restored by setting the deep copy flag to 1. +Version 3.2.7 (Jan 14, 2025) +---------------------------- + Python bindings ^^^^^^^^^^^^^^^ 1. :ref:`rollout` now features native multi-threading. If a sequence of ``MjData`` instances From 4da2ddce29f48ecff32b6a75dc8cd6d90c3460c5 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 16 Jan 2025 06:21:34 -0800 Subject: [PATCH 222/426] Only correct box-box collisions with the mjc_BoxBox function. PiperOrigin-RevId: 716209565 Change-Id: I3419ce82f9934606bed80acd8372284fc7ac7ac8 --- src/engine/engine_collision_driver.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index f46d8157..36c81652 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1461,8 +1461,10 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { type1 = m->geom_type[g1]; type2 = m->geom_type[g2]; + mjfCollision collisionFunc = mjCOLLISIONFUNC[type1][type2]; + // return if no collision function - if (!mjCOLLISIONFUNC[type1][type2]) { + if (!collisionFunc) { return; } @@ -1503,7 +1505,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { } // call collision detector to generate contacts - num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin); + num = collisionFunc(m, d, con, g1, g2, margin); // check contacts if (!num) { @@ -1517,7 +1519,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { } // remove bad and repeated contacts in box-box - if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + if (collisionFunc == mjc_BoxBox) { // use dim field to mark: -1: bad, 0: good for (int i=0; i < num; i++) { con[i].dim = 0; From 893a993bf68aebc0d0a1bc341f6afd23ba9a2ce2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 16 Jan 2025 06:27:55 -0800 Subject: [PATCH 223/426] Fix typos. PiperOrigin-RevId: 716211176 Change-Id: Ibadfe13db89ed9047832b977e918faa3c5c6d9f1 --- include/mujoco/mjplugin.h | 2 +- src/engine/engine_callback.c | 2 +- src/engine/engine_collision_box.c | 2 +- src/engine/engine_collision_driver.c | 4 ++-- src/engine/engine_solver.c | 4 ++-- src/engine/engine_util_solve.h | 2 +- src/render/render_context.c | 2 +- src/ui/ui_main.c | 6 +++--- src/user/user_composite.cc | 2 +- src/user/user_flexcomp.cc | 4 ++-- src/user/user_model.cc | 4 ++-- src/user/user_objects.cc | 4 ++-- src/user/user_resource.cc | 2 +- src/user/user_resource.h | 2 +- src/user/user_util.cc | 2 +- src/user/user_util.h | 2 +- 16 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index c142d517..0fc31a6c 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -48,7 +48,7 @@ typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* n // callback for checking if the current resource was modified from the time // specified by the timestamp // returns 0 if the resource's timestamp matches the provided timestamp -// returns > 0 if the the resource is younger than the given timestamp +// returns > 0 if the resource is younger than the given timestamp // returns < 0 if the resource is older than the given timestamp typedef int (*mjfResourceModified)(const mjResource* resource, const char* timestamp); diff --git a/src/engine/engine_callback.c b/src/engine/engine_callback.c index 67249f2a..148f6dc3 100644 --- a/src/engine/engine_callback.c +++ b/src/engine/engine_callback.c @@ -29,7 +29,7 @@ mjfAct mjcb_act_dyn = 0; -// reset callbacks to defauls +// reset callbacks to defaults void mj_resetCallbacks(void) { mjcb_passive = 0; mjcb_control = 0; diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 1860ad4d..d4e78f00 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -475,7 +475,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, } else if (cltype >= 0 && cltype / 3 == 1) { // we are on box's edge // hacks to find the relative orientation of capsule and edge // there are 2 cases: - // c1= 2^n: edge and capsule are oriented in a T configuaration (no more contacts + // c1= 2^n: edge and capsule are oriented in a T configuration (no more contacts // c1!=2^n: oriented in a cross X configuration c1 = axisdir ^ clcorner; // same trick diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 36c81652..655d0b48 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1153,7 +1153,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { // init with pairs involving always-colliding bodies for (int b1=0; b1 < nbody; b1++) { - // cannot colide + // cannot collide if (!canCollide(m, b1)) { continue; } @@ -1163,7 +1163,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { (m->body_weldid[b1] == 0 && hasPlane(m, b1))) { // add b1:body pairs that are not welded together for (int b2=0; b2 < nbody; b2++) { - // cannot colide + // cannot collide if (!canCollide(m, b2)) { continue; } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 29f30a2e..68465374 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1408,7 +1408,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { // add nC to Hessian total nonzeros (unavoidable overcounting since H_colind is still unknown) ctx->nH = m->nC + ctx->H_rowadr[nv - 1] + ctx->H_rownnz[nv - 1]; - // shift H row adresses to make room for C + // shift H row addresses to make room for C int shift = 0; for (int r = 0; r < nv - 1; r++) { shift += d->C_rownnz[r]; @@ -1443,7 +1443,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->nL = mju_cholFactorCount(ctx->L_rownnz, HT_rownnz, HT_rowadr, HT_colind, nv, d); mj_freeStack(d); - // compute L row adresses: rowadr = cumsum(rownnz) + // compute L row addresses: rowadr = cumsum(rownnz) ctx->L_rowadr[0] = 0; for (int r=1; r < nv; r++) { ctx->L_rowadr[r] = ctx->L_rowadr[r-1] + ctx->L_rownnz[r-1]; diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 91ea13cc..66e59842 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -114,7 +114,7 @@ MJAPI void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, mjtNum** H, mjtNum** g, int n, mjtNum** lower, mjtNum** upper); -// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <=upper, explicit options (see implemetation) +// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <=upper, explicit options (see implementation) MJAPI int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n, const mjtNum* lower, const mjtNum* upper, diff --git a/src/render/render_context.c b/src/render/render_context.c index b0b2a361..6adb4f11 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -895,7 +895,7 @@ static void setVertexHaze(float* v, float az, float h, float r) { // truncated cone for haze rendering static void haze(int nSlice, float r, const float* rgba) { - // compute elevation h for transparancy transition point + // compute elevation h for transparency transition point float alpha = atan2f(1, r); float beta = (float)(0.75*mjPI) - alpha; float h = sqrtf(0.5f) * r * sinf(alpha) / sinf(beta); diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index c1146363..6cfa003b 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -228,7 +228,7 @@ static void initOpenGL(const mjrRect* r, const mjrContext* con) { -// get text width up to specificed limit (0: 0, -1: entire string) +// get text width up to specified limit (0: 0, -1: entire string) static int textwidth(const char* text, const mjrContext* con, int limit) { int i = 0, width = 0; @@ -1523,7 +1523,7 @@ static void setitemskip(mjuiSection* s, int pass) { -// Compute UI sizes: internal fuction, may be called twice per resize +// Compute UI sizes: internal function, may be called twice per resize static void tryresize(mjUI* ui, const mjrContext* con) { // scale theme sizes int w_master = SCL(ui->spacing.total, con); @@ -2969,7 +2969,7 @@ void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con) { mjr_rectangle(r, ui->color.select2[0], ui->color.select2[1], ui->color.select2[2], 1); - // hightlight row under mouse + // highlight row under mouse int k = findselect(it, ui, state, con); if (k >= 0) { mjrRect r1 = r; diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index ab34b7b8..b90245cd 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -139,7 +139,7 @@ void mjCComposite::SetDefault(void) { } } - // set all deafult groups to 3 + // set all default groups to 3 for (int i=0; igroup = 3; def[i].spec.site->group = 3; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 7b1b0116..338c1896 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -1527,7 +1527,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, // read elements, discard all tags element.reserve(numNodeTags*numElements); for (size_t i=0; i> tag >> elementType >> numTags; if (!ss.good()) { @@ -1535,7 +1535,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, } } if (numTags > 0) { - ss >> physicalEntityTag >> elmentModelEntityTag; + ss >> physicalEntityTag >> elementModelEntityTag; if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 532eece0..2a926855 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -131,7 +131,7 @@ mjCModel::mjCModel() { //------------------------ auto-computed statistics #ifndef MEMORY_SANITIZER - // initializing as best practice, but want MSAN to catch unintialized use + // initializing as best practice, but want MSAN to catch uninitialized use meaninertia_auto = 0; meanmass_auto = 0; meansize_auto = 0; @@ -1845,7 +1845,7 @@ void mjCModel::AutoSpringDamper(mjModel* m) { int adr = m->jnt_dofadr[n]; int ndim = mjCJoint::nv((mjtJoint)m->jnt_type[n]); - // get timeconst and dampratio from joint specificatin + // get timeconst and dampratio from joint specification mjtNum timeconst = (mjtNum)joints_[n]->springdamper[0]; mjtNum dampratio = (mjtNum)joints_[n]->springdamper[1]; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 82c46acd..c1f49f29 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1123,7 +1123,7 @@ void mjCBody::NameSpace(const mjCModel* m) { -// apply prefix and suffix, propagate to all descendents or only to child bodies +// apply prefix and suffix, propagate to all descendants or only to child bodies void mjCBody::NameSpace_(const mjCModel* m, bool propagate) { mjCBase::NameSpace(m); if (!plugin_instance_name.empty()) { @@ -5487,7 +5487,7 @@ void mjCTendon::Compile(void) { case mjWRAP_PULLEY: // pulley should not follow other pulley if (i>0 && path[i-1]->type==mjWRAP_PULLEY) { - throw mjCError(this, "tendon '%s' (id = %d): consequtive pulleys (pos %d)", + throw mjCError(this, "tendon '%s' (id = %d): consecutive pulleys (pos %d)", name.c_str(), id, i); } diff --git a/src/user/user_resource.cc b/src/user/user_resource.cc index 980d7ee9..b82d7c0c 100644 --- a/src/user/user_resource.cc +++ b/src/user/user_resource.cc @@ -249,7 +249,7 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) { // return 0 if the resource's timestamp matches the provided timestamp -// return > 0 if the the resource is younger than the given timestamp +// return > 0 if the resource is younger than the given timestamp // return < 0 if the resource is older than the given timestamp int mju_isModifiedResource(const mjResource* resource, const char* timestamp) { // provider is not OS filesystem diff --git a/src/user/user_resource.h b/src/user/user_resource.h index 82a70ed6..1d2093b6 100644 --- a/src/user/user_resource.h +++ b/src/user/user_resource.h @@ -42,7 +42,7 @@ MJAPI int mju_readResource(mjResource* resource, const void** buffer); MJAPI void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir); // return 0 if the resource's timestamp matches the provided timestamp -// return > 0 if the the resource is younger than the given timestamp +// return > 0 if the resource is younger than the given timestamp // return < 0 if the resource is older than the given timestamp MJAPI int mju_isModifiedResource(const mjResource* resource, const char* timestamp); diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 803c22d4..db022bfb 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -993,7 +993,7 @@ std::string FilePath::PathReduce(const std::string& str) { int j = abs_prefix.size(); for (int i = j; i < str.size(); ++i) { - if (IsSeperator(str[i])) { + if (IsSeparator(str[i])) { std::string temp = str.substr(j, i - j); j = i + 1; if (temp == ".." && !dirs.empty() && dirs.back() != "..") { diff --git a/src/user/user_util.h b/src/user/user_util.h index 8a95d61e..1d516ab8 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -211,7 +211,7 @@ class FilePath { private: static std::string AbsPrefix(const std::string& str); static std::string PathReduce(const std::string& str); - static bool IsSeperator(char c) { + static bool IsSeparator(char c) { return c == '/' || c == '\\'; } static std::string Combine(const std::string& s1, const std::string& s2); From 394cc61192923ecd69a03f612b6b7f2b29ad5c3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Thu, 16 Jan 2025 14:33:08 +0000 Subject: [PATCH 224/426] Use RST links throughout the Unity doc. --- doc/unity.rst | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/doc/unity.rst b/doc/unity.rst index 1264ea64..390ac52c 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -5,26 +5,27 @@ Unity Plug-in Introduction ------------ -The MuJoCo `Unity plug-in `_ allows the Unity Editor and +The MuJoCo `Unity plug-in `__ allows the Unity Editor and runtime to use the MuJoCo physics engine. Users can import MJCF files and edit the models in the Editor. The plug-in relies on Unity for most aspects -- assets, game logic, simulation time -- but uses MuJoCo to determine how objects move, giving the designer access to MuJoCo's full API. -An example project using MuJoCo's Unity plugin in a set of introductory tutorials is available at -https://github.com/Balint-H/mj-unity-tutorial. +An example project using MuJoCo's Unity plugin in a set of introductory tutorials are also available as a `standalone +repository `__. .. _UInstallation: Installation instructions ------------------------- -The plug-in directory (available at https://github.com/google-deepmind/mujoco/tree/main/unity) includes a +The `plug-in directory `__ includes a ``package.json`` file. Unity's package manager recognizes this file and will import the plug-in's C# codebase to your -project. In addition, Unity also needs the native MuJoCo library, which can be found in the specific platform archive at -https://github.com/google-deepmind/mujoco/releases. If you wish to simply use the plug-in and not develop it, you should -use one of the version-specific stable commits of the repository, identified by git tags. Check out the relevant version -of the cloned repository with git (``git checkout 3.X.Y`` where X and Y specify the engine version). Simply using the -``main`` branch of the repository may not be compatible with the most recent release binary of MuJoCo. +project. In addition, Unity also needs the native MuJoCo library, which can be found in the corrsponding `platform +archive `__. If you wish to simply use the plug-in and not +develop it, you should use one of the version-specific stable commits of the repository, identified by git tags. Check +out the relevant version of the cloned repository with git (``git checkout 3.X.Y`` where X and Y specify the engine +version). Simply using the ``main`` branch of the repository may not be compatible with the most recent release binary +of MuJoCo. On Unity version 2020.2 and later, the Package Manager will look for the native library file and copy it to the package directory when the package is imported. Alternatively, you can manually copy the native library to the package directory @@ -121,7 +122,7 @@ This design principle has several implications: - The layout of MuJoCo components in the GameObject hierarchy determines the layout of the resulting MuJoCo model. Therefore, we adopt a design rule that **every game object must have at most one MuJoCo component**. - We rely on Unity for spatial configuration, which requires vector components to be `swizzled - `_ since Unity uses left-handed frames with Y as the + `__ since Unity uses left-handed frames with Y as the vertical axis, while MuJoCo uses right-handed frames with Z as the vertical axis. - Unity transform scaling affects positions, orientations, and scale of the entire game object subtree. However, MuJoCo doesn’t support collision of skewed cylinders and capsules (skewed spheres are supported via the ellipsoid primitive). @@ -343,9 +344,9 @@ The current version of the Unity package does not support loading MJCF scenes th Interaction with External Processes ___________________________________ -Roboti’s `MuJoCo plug-in for Unity `_ steps the simulation in an external Python +Roboti’s `MuJoCo plug-in for Unity `__ steps the simulation in an external Python process, and uses Unity only for rendering. In contrast, our plug-in relies on Unity to step the simulation. It should be possible to use our plug-in while an external process "drives" the simulation, for example by setting ``qpos``, calling ``mj_kinematics``, synchronizing the transforms, and then using Unity to render or compute game logic. In order to establish communication with an external process, you can use Unity's `ML-Agents -`_ package. +`__ package. From f899e717d4636f16af70fe4a194256fcbc1b02ce Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 16 Jan 2025 06:39:32 -0800 Subject: [PATCH 225/426] Raise error for invalid name in MJX bind. PiperOrigin-RevId: 716214444 Change-Id: Ie9e3156b32b0c218cdbd0beb93a4a78800e07c43 --- mjx/mujoco/mjx/_src/support.py | 66 ++++++++++++++++------------- mjx/mujoco/mjx/_src/support_test.py | 6 +++ 2 files changed, 42 insertions(+), 30 deletions(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index c5ae3157..10ec9d2a 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -299,66 +299,69 @@ class BindModel(object): match spec: case mujoco.MjsBody(): self.prefix = 'body_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) case mujoco.MjsJoint(): self.prefix = 'jnt_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) case mujoco.MjsGeom(): self.prefix = 'geom_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) case mujoco.MjsSite(): self.prefix = 'site_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) case mujoco.MjsLight(): self.prefix = 'light_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) case mujoco.MjsCamera(): self.prefix = 'cam_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) case mujoco.MjsMesh(): self.prefix = 'mesh_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name) case mujoco.MjsHField(): self.prefix = 'hfield_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name) case mujoco.MjsPair(): self.prefix = 'pair_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_PAIR, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_PAIR, spec.name) case mujoco.MjsTendon(): self.prefix = 'tendon_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) case mujoco.MjsActuator(): self.prefix = 'actuator_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) case mujoco.MjsSensor(): self.prefix = 'sensor_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) case mujoco.MjsNumeric(): self.prefix = 'numeric_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_NUMERIC, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_NUMERIC, spec.name) case mujoco.MjsText(): self.prefix = 'text_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TEXT, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_TEXT, spec.name) case mujoco.MjsTuple(): self.prefix = 'tuple_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TUPLE, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_TUPLE, spec.name) case mujoco.MjsKey(): self.prefix = 'key_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_KEY, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_KEY, spec.name) case mujoco.MjsEquality(): self.prefix = 'eq_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) case mujoco.MjsExclude(): self.prefix = 'exclude_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EXCLUDE, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_EXCLUDE, spec.name) case mujoco.MjsSkin(): self.prefix = 'skin_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SKIN, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_SKIN, spec.name) case mujoco.MjsMaterial(): self.prefix = 'material_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_MATERIAL, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_MATERIAL, spec.name) case _: raise ValueError('invalid spec type') + if idx < 0: + raise KeyError(f'invalid name: {spec.name}') + ids.append(idx) if len(ids) == 1: self.id = ids[0] else: @@ -387,36 +390,39 @@ class BindData(object): match spec: case mujoco.MjsBody(): self.prefix = '' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) case mujoco.MjsJoint(): self.prefix = 'jnt_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) case mujoco.MjsGeom(): self.prefix = 'geom_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) case mujoco.MjsSite(): self.prefix = 'site_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) case mujoco.MjsLight(): self.prefix = 'light_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) case mujoco.MjsCamera(): self.prefix = 'cam_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) case mujoco.MjsTendon(): self.prefix = 'ten_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) case mujoco.MjsActuator(): self.prefix = 'actuator_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) case mujoco.MjsSensor(): self.prefix = 'sensor_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) case mujoco.MjsEquality(): self.prefix = 'eq_' - ids.append(name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name)) + idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) case _: raise ValueError('invalid spec type') + if idx < 0: + raise KeyError(f'invalid name: {spec.name}') + ids.append(idx) if len(ids) == 1: self.id = ids[0] else: diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 53c69cf9..5cfd8cd0 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -247,6 +247,12 @@ class SupportTest(parameterized.TestCase): print(dx.bind(mx, s.actuators).actuator_ctrl) with self.assertRaises(AttributeError): print(dx.bind(mx, s.actuators).set('actuator_ctrl', [1, 2, 3])) + with self.assertRaises(KeyError, msg='invalid name: invalid_actuator_name'): + s.actuators[0].name = 'invalid_actuator_name' + print(dx.bind(mx, s.actuators).set('ctrl', [1, 2, 3])) + with self.assertRaises(KeyError, msg='invalid name: invalid_geom_name'): + s.geoms[0].name = 'invalid_geom_name' + print(mx.bind(s.geoms).pos) _CONTACTS = """ From 4903d5321c253a1ad63ea1861ffb5d7a3c54f03d Mon Sep 17 00:00:00 2001 From: Silvia Cruciani Date: Thu, 16 Jan 2025 07:19:03 -0800 Subject: [PATCH 226/426] add find_geom property to mjSpec PiperOrigin-RevId: 716225290 Change-Id: Ie6e229b0e802e7df06148f994d598334592e8ad1 --- python/mujoco/specs.cc | 6 ++++++ python/mujoco/specs_test.py | 18 ++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 2f37f64f..eb8e0ea9 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -402,6 +402,12 @@ PYBIND11_MODULE(_specs, m) { return mjs_findDefault(self.ptr, classname.c_str()); }, py::return_value_policy::reference_internal); + mjSpec.def( + "find_geom", + [](MjSpec& self, std::string& name) -> raw::MjsGeom* { + return mjs_asGeom(mjs_findElement(self.ptr, mjOBJ_GEOM, name.c_str())); + }, + py::return_value_policy::reference_internal); mjSpec.def("compile", [mjmodel_from_spec_ptr](MjSpec& self) -> py::object { if (self.assets.empty()) { return mjmodel_from_spec_ptr(reinterpret_cast(self.ptr)); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index d7b86481..993fc2d4 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -691,6 +691,24 @@ class SpecsTest(absltest.TestCase): body4.name = 'body4_new' self.assertEqual(spec.bodies[4].name, 'body4_new') + def test_geom_list(self): + main_xml = """ + + + + + + """ + spec = mujoco.MjSpec.from_string(main_xml) + geom1 = spec.worldbody.add_geom(name='geom1') + geom2 = spec.worldbody.add_geom(name='geom2') + geom3 = spec.find_body('body1').add_geom(name='geom3') + + self.assertEqual(spec.geoms, [geom1, geom2, geom3]) + self.assertEqual(spec.find_geom('geom1'), geom1) + self.assertEqual(spec.find_geom('geom2'), geom2) + self.assertEqual(spec.find_geom('geom3'), geom3) + def test_iterators(self): spec = mujoco.MjSpec() geom1 = spec.worldbody.add_geom() From c16f95f62c454f4f746d89923c54df4e26d17b19 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 16 Jan 2025 08:14:01 -0800 Subject: [PATCH 227/426] Add sensordata MJX binding. PiperOrigin-RevId: 716241711 Change-Id: I11a82f58c41dca93e9a5337f0c573158026fa971 --- mjx/mujoco/mjx/_src/support.py | 13 +++++++++++++ mjx/mujoco/mjx/_src/support_test.py | 15 +++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 10ec9d2a..905a2cbf 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -381,6 +381,7 @@ class BindData(object): def __init__(self, data: Data, model: Model, specs: Sequence[Any]): self.data = data + self.model = model try: iter(specs) except TypeError: @@ -438,10 +439,22 @@ class BindData(object): return self.prefix + name def __getattr__(self, name: str): + if name == 'sensordata': + adr = self.model.sensor_adr[self.id] + num = self.model.sensor_dim[self.id] + if isinstance(self.id, list): + idx = [] + for i, n in zip(self.id, num): + idx.extend(adr[i] + j for j in range(n)) + return getattr(self.data, name)[idx, ...] + else: + return getattr(self.data, name)[adr : adr + num, ...] return getattr(self.data, self.__getname(name))[self.id, ...] def set(self, name: str, value: jax.Array) -> Data: """Set the value of an array in an MJX Data.""" + if name == 'sensordata': + raise AttributeError('sensordata is readonly') array = getattr(self.data, self.__getname(name)) try: iter(value) diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 5cfd8cd0..a61374da 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -180,6 +180,12 @@ class SupportTest(parameterized.TestCase): + + + + + + """ @@ -224,6 +230,15 @@ class SupportTest(parameterized.TestCase): dx.bind(mx, s.actuators[i]).ctrl, d.ctrl[i] ) + np.testing.assert_array_equal( + dx.bind(mx, s.sensors).sensordata, d.sensordata + ) + for i in range(m.nsensor): + np.testing.assert_array_equal( + dx.bind(mx, s.sensors[i]).sensordata, + d.sensordata[m.sensor_adr[i] : m.sensor_adr[i] + m.sensor_dim[i]], + ) + # test setting np.testing.assert_array_equal(d.ctrl, [0, 0, 0]) np.testing.assert_array_equal(dx.bind(mx, s.actuators).ctrl, d.ctrl) From 9fc1ac2c7d049d557a83be149f10d775a3541f98 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 16 Jan 2025 09:04:09 -0800 Subject: [PATCH 228/426] Fix typo in sensordata binding. PiperOrigin-RevId: 716257343 Change-Id: I4da12d8cda4342eb9f5a4df664bb43841db8e332 --- mjx/mujoco/mjx/_src/support.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 905a2cbf..748be165 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -444,8 +444,8 @@ class BindData(object): num = self.model.sensor_dim[self.id] if isinstance(self.id, list): idx = [] - for i, n in zip(self.id, num): - idx.extend(adr[i] + j for j in range(n)) + for a, n in zip(adr, num): + idx.extend(a + j for j in range(n)) return getattr(self.data, name)[idx, ...] else: return getattr(self.data, name)[adr : adr + num, ...] From 71e5e376bc8d30812f6bfff25c5a059683ee3b65 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 16 Jan 2025 12:54:26 -0800 Subject: [PATCH 229/426] Remove units from zero valued size in CSS, fix a typo. PiperOrigin-RevId: 716345043 Change-Id: I22b28e8e1d6db8ae4e482541169f1ffc46d3db34 --- doc/css/theme_overrides.css | 12 ++++++------ doc/ext/header_reader.py | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/doc/css/theme_overrides.css b/doc/css/theme_overrides.css index e51e4eb0..00b8042d 100644 --- a/doc/css/theme_overrides.css +++ b/doc/css/theme_overrides.css @@ -72,7 +72,7 @@ h4 { /* Paragraph margins don't apply to table cell contents. */ .rst-content table.docutils td>p { - margin-top: 0px; + margin-top: 0; } /* Set padding of in-line highlighted text. */ @@ -200,8 +200,8 @@ table.docutils:not(.mjcf-attributes) > tbody > tr.row-odd { /* MJCF attributes table. */ table.mjcf-attributes { border-style: none; - margin-left: 0px; - margin-right: 0px; + margin-left: 0; + margin-right: 0; width: 100%; box-shadow: none; } @@ -209,7 +209,7 @@ table.mjcf-attributes { table.mjcf-attributes tbody tr td, table.mjcf-attributes tbody tr:nth-child(2n-1) td { border-style: none; - padding: 0px 0px 0px 0px; + padding: 0 0 0 0; width: 25%; } @@ -217,13 +217,13 @@ table.mjcf-attributes tbody tr td p { margin: 0; } -div.table-wrapper.mjcf-attributes { +.table-wrapper.mjcf-attributes { margin: 0.5em; padding: 0; } table td > div.wy-table-responsive { - margin-bottom: 0px; + margin-bottom: 0; } /* Remove vertical spacing before/after code blocks. */ diff --git a/doc/ext/header_reader.py b/doc/ext/header_reader.py index 42a93a14..e81f98a8 100644 --- a/doc/ext/header_reader.py +++ b/doc/ext/header_reader.py @@ -98,7 +98,7 @@ def read(lines: List[str]) -> Dict[str, ApiDefinition]: if section is not None: if 'MJAPI FUNCTIONS' in section: # Stripped functions do not begin with MJAPI, and must be under the - # predefiend section 'MJAPI FUNCTIONS'. This is because the docs don't + # predefined section 'MJAPI FUNCTIONS'. This is because the docs don't # include this prefix, and so we need to read such functions from the # reference header. stripped_functions = True From 688d367733248b342875520e7fb7e22fd17ef1e4 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 17 Jan 2025 11:48:48 -0800 Subject: [PATCH 230/426] Modify model loading in MJX tutorial. PiperOrigin-RevId: 716745232 Change-Id: Ib89f8c00a58b493c89f72c82b47f1c8ff8326536 --- mjx/tutorial.ipynb | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index 905e2ae3..b29b20f1 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -423,6 +423,8 @@ "source": [ "#@title Humanoid Env\n", "\n", + "HUMANOID_ROOT_PATH = epath.Path(epath.resource_path('mujoco')) / 'mjx/test_data/humanoid'\n", + "\n", "class Humanoid(PipelineEnv):\n", "\n", " def __init__(\n", @@ -436,11 +438,8 @@ " exclude_current_positions_from_observation=True,\n", " **kwargs,\n", " ):\n", - " path = epath.Path(epath.resource_path('mujoco')) / (\n", - " 'mjx/test_data/humanoid'\n", - " )\n", " mj_model = mujoco.MjModel.from_xml_path(\n", - " (path / 'humanoid.xml').as_posix())\n", + " (HUMANOID_ROOT_PATH / 'humanoid.xml').as_posix())\n", " mj_model.opt.solver = mujoco.mjtSolver.mjSOL_CG\n", " mj_model.opt.iterations = 6\n", " mj_model.opt.ls_iterations = 6\n", From 240a7afdeed71baeed2b607de19c9ba3766e1590 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 17 Jan 2025 13:09:29 -0800 Subject: [PATCH 231/426] Add sensors for potential and kinetic energy. PiperOrigin-RevId: 716775375 Change-Id: Ic8ab7f1a51df970ab551dabe7c797cde13cd97fd --- doc/XMLreference.rst | 53 ++++++++++++++++- doc/XMLschema.rst | 14 +++++ doc/changelog.rst | 11 ++-- doc/includes/references.h | 2 + include/mujoco/mjmodel.h | 2 + introspect/enums.py | 8 ++- src/engine/engine_forward.c | 62 ++++++++++++++++++-- src/engine/engine_io.c | 2 + src/engine/engine_sensor.c | 21 ++++--- src/user/user_objects.cc | 8 ++- src/xml/xml_native_reader.cc | 10 +++- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 6 ++ test/engine/engine_sensor_test.cc | 88 +++++++++++++++++++++++++++- unity/Runtime/Bindings/MjBindings.cs | 8 ++- 15 files changed, 264 insertions(+), 33 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 5d290e6b..141a4d5b 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -601,9 +601,17 @@ from its default. .. _option-flag-energy: :at:`energy`: :at-val:`[disable, enable], "disable"` - This flag enables the computation of kinetic and potential energy, stored in mjData.energy and displayed in the GUI. - This feature adds some CPU time but it is usually negligible. Monitoring energy for a system that is supposed to be - energy-conserving is one of the best ways to assess the accuracy of a complex simulation. + This flag enables the computation of potential and kinetic energy in ``mjData.energy[0, 1]`` respectively, + and displayed in the simulate GUI info overlay. Potential energy includes the gravitational component summed over + all bodies :math:`\sum_b m_b g h` and energy stored in passive springs in joints, tendons and flexes + :math:`\tfrac{1}{2} k x^2`, where :math:`x` is the displacement and and :math:`k` is the spring constant. Kinetic + energy is given by :math:`\tfrac{1}{2} v^T M v`, where :math:`v` is the velocity and :math:`M` is the + mass matrix. Note that potential and kinetic energy in constraints is not accounted for. + + The extra computation (also triggered by :ref:`potential` and + :ref:`kinetic` energy sensors) adds some CPU time but it is usually negligible. Monitoring energy + for a system that is supposed to be energy-conserving is one of the best ways to assess the accuracy of a complex + simulation. .. _option-flag-fwdinv: @@ -7245,6 +7253,45 @@ See :ref:`collision-sensors` for more details about sensors of this type. :at:`name`, :at:`noise`, :at:`user` See :ref:`CSensor`. + +.. _sensor-e_potential: + +:el-prefix:`sensor/` |-| **e_potential** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates sensor that returns the potential energy. + +.. _sensor-e_potential-name: + +.. _sensor-e_potential-noise: + +.. _sensor-e_potential-cutoff: + +.. _sensor-e_potential-user: + +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` + See :ref:`CSensor`. + + +.. _sensor-e_kinetic: + +:el-prefix:`sensor/` |-| **e_kinetic** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates sensor that returns the kinetic energy. + +.. _sensor-e_kinetic-name: + +.. _sensor-e_kinetic-noise: + +.. _sensor-e_kinetic-cutoff: + +.. _sensor-e_kinetic-user: + +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` + See :ref:`CSensor`. + + .. _sensor-clock: :el-prefix:`sensor/` |-| **clock** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 2d1bc9bd..b3bbc567 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1231,6 +1231,20 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`e_potential | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`e_kinetic | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`clock | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 58a1bfe8..697604b9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,10 +5,13 @@ Changelog Upcoming version (not yet released) ----------------------------------- -- Added ``mjs_setDeepCopy`` API function. When the deep copy flag is 0, attaching a model will not copy it to the - parent, so the original references to the child allow to modify the parent as well. The default behavior is to perform - such a shallow copy. The old behavioud of creating a deep copy of the child model while attaching can be restored by - setting the deep copy flag to 1. +General +^^^^^^^ +- Added :ref:`mjs_setDeepCopy` API function. When the deep copy flag is 0, attaching a model will not copy it to the + parent, so the original references to the child can be used to modify the parent after attachment. The default + behavior is to perform such a shallow copy. The old behavior of creating a deep copy of the child model while + attaching can be restored by setting the deep copy flag to 1. +- Added :ref:`potential` and :ref:`kinetic` energy sensors. Version 3.2.7 (Jan 14, 2025) ---------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 26513c8d..c1cce18e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -674,6 +674,8 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_GEOMFROMTO, // segment between two geoms // global sensors + mjSENS_E_POTENTIAL, // potential energy + mjSENS_E_KINETIC, // kinetic energy mjSENS_CLOCK, // simulation time // plugin-controlled sensors diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index ebd752aa..b8bafe6c 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -347,6 +347,8 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_GEOMFROMTO, // segment between two geoms // global sensors + mjSENS_E_POTENTIAL, // potential energy + mjSENS_E_KINETIC, // kinetic energy mjSENS_CLOCK, // simulation time // plugin-controlled sensors diff --git a/introspect/enums.py b/introspect/enums.py index b4b07400..770716a1 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -361,9 +361,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSENS_GEOMDIST', 37), ('mjSENS_GEOMNORMAL', 38), ('mjSENS_GEOMFROMTO', 39), - ('mjSENS_CLOCK', 40), - ('mjSENS_PLUGIN', 41), - ('mjSENS_USER', 42), + ('mjSENS_E_POTENTIAL', 40), + ('mjSENS_E_KINETIC', 41), + ('mjSENS_CLOCK', 42), + ('mjSENS_PLUGIN', 43), + ('mjSENS_USER', 44), ]), )), ('mjtStage', diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 08f33625..0ece01ac 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1013,6 +1013,38 @@ void mj_implicit(const mjModel* m, mjData* d) { +// return 1 if potential energy was computed by sensor, 0 otherwise +static int energyPosSensor(const mjModel* m) { + if (mjDISABLED(mjDSBL_SENSOR)) { + return 0; + } + + for (int i=0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_E_POTENTIAL) { + return 1; + } + } + return 0; +} + + + +// return 1 if kinetic energy was computed by sensor, 0 otherwise +static int energyVelSensor(const mjModel* m) { + if (mjDISABLED(mjDSBL_SENSOR)) { + return 0; + } + + for (int i=0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_E_KINETIC) { + return 1; + } + } + return 0; +} + + + //-------------------------- top-level API --------------------------------------------------------- // forward dynamics with skip; skipstage is mjtStage @@ -1022,21 +1054,33 @@ void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) // position-dependent if (skipstage < mjSTAGE_POS) { mj_fwdPosition(m, d); + + int energyPos = 0; if (!skipsensor) { mj_sensorPos(m, d); + energyPos = energyPosSensor(m); } - if (mjENABLED(mjENBL_ENERGY)) { - mj_energyPos(m, d); + + if (!energyPos) { + if (mjENABLED(mjENBL_ENERGY)) { + mj_energyPos(m, d); + } else { + d->energy[0] = d->energy[1] = 0; + } } } // velocity-dependent if (skipstage < mjSTAGE_VEL) { mj_fwdVelocity(m, d); + + int energyVel = 0; if (!skipsensor) { mj_sensorVel(m, d); + energyVel = energyVelSensor(m); } - if (mjENABLED(mjENBL_ENERGY)) { + + if (mjENABLED(mjENBL_ENERGY) && !energyVel) { mj_energyVel(m, d); } } @@ -1111,10 +1155,18 @@ void mj_step1(const mjModel* m, mjData* d) { mj_checkVel(m, d); mj_fwdPosition(m, d); mj_sensorPos(m, d); - mj_energyPos(m, d); + if (!energyPosSensor(m)) { + if (mjENABLED(mjENBL_ENERGY)) { + mj_energyPos(m, d); + } else { + d->energy[0] = d->energy[1] = 0; + } + } mj_fwdVelocity(m, d); mj_sensorVel(m, d); - mj_energyVel(m, d); + if (mjENABLED(mjENBL_ENERGY) && !energyVelSensor(m)) { + mj_energyVel(m, d); + } if (mjcb_control) { mjcb_control(m, d); } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index ddbb3557..c78f722a 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2056,6 +2056,8 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: case mjSENS_GEOMDIST: + case mjSENS_E_POTENTIAL: + case mjSENS_E_KINETIC: case mjSENS_CLOCK: return 1; diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 56966941..baad81c4 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -458,6 +458,16 @@ void mj_sensorPos(const mjModel* m, mjData* d) { } break; + case mjSENS_E_POTENTIAL: // potential energy + mj_energyPos(m, d); + d->sensordata[adr] = d->energy[0]; + break; + + case mjSENS_E_KINETIC: // kinetic energy + mj_energyVel(m, d); + d->sensordata[adr] = d->energy[1]; + break; + case mjSENS_CLOCK: // clock d->sensordata[adr] = d->time; break; @@ -902,12 +912,6 @@ void mj_energyPos(const mjModel* m, mjData* d) { int padr; mjtNum dif[3], quat[4], stiffness; - // disabled: clear and return - if (!mjENABLED(mjENBL_ENERGY)) { - d->energy[0] = d->energy[1] = 0; - return; - } - // init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity) d->energy[0] = 0; if (!mjDISABLED(mjDSBL_GRAVITY)) { @@ -996,11 +1000,6 @@ void mj_energyPos(const mjModel* m, mjData* d) { // velocity-dependent energy (kinetic) void mj_energyVel(const mjModel* m, mjData* d) { - // return if disabled (already cleared in potential) - if (!mjENABLED(mjENBL_ENERGY)) { - return; - } - mj_markStack(d); mjtNum *vec = mjSTACKALLOC(d, m->nv, mjtNum); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c1f49f29..5d3a7272 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -6206,7 +6206,11 @@ void mjCSensor::ResolveReferences(const mjCModel* m) { ((mjCGeom*)obj)->SetNotVisual(); } - } else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) { + } else if (type != mjSENS_E_POTENTIAL && + type != mjSENS_E_KINETIC && + type != mjSENS_CLOCK && + type != mjSENS_PLUGIN && + type != mjSENS_USER) { throw mjCError(this, "invalid type in sensor"); } @@ -6539,6 +6543,8 @@ void mjCSensor::Compile(void) { } break; + case mjSENS_E_POTENTIAL: + case mjSENS_E_KINETIC: case mjSENS_CLOCK: dim = 1; needstage = mjSTAGE_POS; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6fe26b27..1a473c05 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -485,6 +485,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, + {"e_potential", "*", "4", "name", "cutoff", "noise", "user"}, + {"e_kinetic", "*", "4", "name", "cutoff", "noise", "user"}, {"clock", "*", "4", "name", "cutoff", "noise", "user"}, {"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage", "dim", "cutoff", "noise", "user"}, @@ -4172,7 +4174,13 @@ void mjXReader::Sensor(XMLElement* section) { } // global sensors - else if (type=="clock") { + else if (type=="e_potential") { + sensor->type = mjSENS_E_POTENTIAL; + sensor->objtype = mjOBJ_UNKNOWN; + } else if (type=="e_kinetic") { + sensor->type = mjSENS_E_KINETIC; + sensor->objtype = mjOBJ_UNKNOWN; + } else if (type=="clock") { sensor->type = mjSENS_CLOCK; sensor->objtype = mjOBJ_UNKNOWN; } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 41aacbbb..86612184 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -101,7 +101,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 237 +#define nMJCF 239 extern const char* MJCF[nMJCF][mjXATTRNUM]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 3ae9fc22..c18c0f70 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -2156,6 +2156,12 @@ void mjXWriter::Sensor(XMLElement* root) { break; // global sensors + case mjSENS_E_POTENTIAL: + elem = InsertEnd(section, "potential"); + break; + case mjSENS_E_KINETIC: + elem = InsertEnd(section, "kinetic"); + break; case mjSENS_CLOCK: elem = InsertEnd(section, "clock"); break; diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 005136bd..5aa2aa5b 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -21,7 +21,6 @@ #include #include #include -#include "src/engine/engine_support.h" #include "src/engine/engine_util_blas.h" #include "src/engine/engine_util_spatial.h" #include "test/fixture.h" @@ -398,6 +397,93 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { // ------------------------- general sensor tests ----------------------------- using SensorTest = MujocoTest; +TEST_F(SensorTest, EnableEnergy) { + constexpr char xml[] = R"( + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + mj_forward(model, data); + EXPECT_EQ(data->energy[0], 2*3*5); + + model->opt.enableflags &= ~mjENBL_ENERGY; + mj_forward(model, data); + EXPECT_EQ(data->energy[0], 0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, PotentialEnergy) { + constexpr char xml[] = R"( + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + mj_forward(model, data); + EXPECT_EQ(data->sensordata[0], 2*3*5); + + data->qpos[2] = 7; + mj_forward(model, data); + EXPECT_EQ(data->sensordata[0], 7*3*5); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, KineticEnergy) { + constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + while (data->time < 1.5) { + mj_step(model, data); + } + mj_forward(model, data); + + mjtNum mass = 3; + mjtNum speed = data->time * mju_norm3(model->opt.gravity); + EXPECT_FLOAT_EQ(data->sensordata[0], 0.5 * mass * speed * speed); + + mj_deleteData(data); + mj_deleteModel(model); +} + // test clock sensor TEST_F(SensorTest, Clock) { constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 59f65d87..e854f1b8 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -377,9 +377,11 @@ public enum mjtSensor : int{ mjSENS_GEOMDIST = 37, mjSENS_GEOMNORMAL = 38, mjSENS_GEOMFROMTO = 39, - mjSENS_CLOCK = 40, - mjSENS_PLUGIN = 41, - mjSENS_USER = 42, + mjSENS_E_POTENTIAL = 40, + mjSENS_E_KINETIC = 41, + mjSENS_CLOCK = 42, + mjSENS_PLUGIN = 43, + mjSENS_USER = 44, } public enum mjtStage : int{ mjSTAGE_NONE = 0, From 1b34246c6e566e8d458d06884d1c3841248632c6 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 17 Jan 2025 13:17:18 -0800 Subject: [PATCH 232/426] Update MJX notebook to point to playground. PiperOrigin-RevId: 716778081 Change-Id: Ic99734ba386c4db551766de6f209cb397e7aa967 --- mjx/tutorial.ipynb | 347 ++------------------------------------------- 1 file changed, 13 insertions(+), 334 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index b29b20f1..b8145c1d 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -886,7 +886,9 @@ "source": [ "## Quadruped Env\n", "\n", - "Let's define a quadruped environment that takes advantage of the domain randomization function. Here we use the [Barkour vb Quadruped](https://github.com/google-deepmind/mujoco_menagerie/tree/main/google_barkour_vb) from [MuJoCo Menagerie](https://github.com/google-deepmind/mujoco_menagerie). We implement an environment that trains a joystick policy with Brax." + "Let's define a quadruped environment that takes advantage of the domain randomization function. Here we use the [Barkour vb Quadruped](https://github.com/google-deepmind/mujoco_menagerie/tree/main/google_barkour_vb) from [MuJoCo Menagerie](https://github.com/google-deepmind/mujoco_menagerie). We implement an environment that trains a joystick policy with Brax.\n", + "\n", + "NOTE: for a full suite of robotic environments, many of which were transferred onto robots, check out [MuJoCo Playground](https://github.com/google-deepmind/mujoco_playground)!\n" ] }, { @@ -1604,345 +1606,22 @@ " fps=1.0 / eval_env.dt / render_every)" ] }, + { + "cell_type": "markdown", + "metadata": { + "id": "-Q5gsOOaBYd1" + }, + "source": [ + "# MuJoCo Playground: Robotics locomotion and manipulation environments + Sim-to-Real!" + ] + }, { "cell_type": "markdown", "metadata": { "id": "gluTlHURuC6i" }, "source": [ - "# Manipulation Environments and Policies\n", - "\n", - "By now, we have shown how MJX can be used to train policies for classic control and robotic locomotion. MJX can also be used for robotic manipulation!\n", - "\n", - "We demonstrate a task on the Franka Panda below, which trains a policy to pickup a cube and bring it to a mocap target position in about 3 minutes on an A100. We will be adding more support for manipulation environments in MJX (i.e. more performant collisions), so stay tuned!\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "id": "RCv16hZIu5Dm" - }, - "outputs": [], - "source": [ - "%%shell\n", - "if [ ! -d \"mujoco_menagerie\" ]; then\n", - " git clone https://github.com/google-deepmind/mujoco_menagerie\n", - "fi\n" - ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "cellView": "form", - "id": "AuU-9nUquEu4" - }, - "outputs": [], - "source": [ - "#@title Franka Panda BringToTarget Environment\n", - "\n", - "FRANKA_PANDA_ROOT_PATH = epath.Path('mujoco_menagerie/franka_emika_panda')\n", - "\n", - "\n", - "def default_config():\n", - " \"\"\"Returns reward config for the environment.\"\"\"\n", - "\n", - " return config_dict.create(\n", - " # Environment timestep. Should match the robot decision frequency.\n", - " dt=0.02,\n", - " # Lowers action magnitude for less-jerky motion. Also sometimes helps\n", - " # sample efficiency.\n", - " action_scale=0.04,\n", - " # The coefficients for all reward terms used for training.\n", - " reward_scales=config_dict.create(\n", - " # Gripper goes to the box.\n", - " gripper_box=4.0,\n", - " # Box goes to the target mocap.\n", - " box_target=8.0,\n", - " # Do not collide the gripper with the floor.\n", - " no_floor_collision=0.25,\n", - " # Arm stays close to target pose.\n", - " robot_target_qpos=0.3,\n", - " ),\n", - " )\n", - "\n", - "\n", - "def _load_sys(path: epath.Path) -> base.System:\n", - " \"\"\"Load a mujoco model from a path.\"\"\"\n", - " assets = {}\n", - " for f in path.parent.glob('*.xml'):\n", - " assets[f.name] = f.read_bytes()\n", - " for f in (path.parent / 'assets').glob('*'):\n", - " assets[f.name] = f.read_bytes()\n", - " xml = path.read_text()\n", - " model = mujoco.MjModel.from_xml_string(xml, assets)\n", - " return mjcf.load_model(model)\n", - "\n", - "\n", - "def _get_collision_info(\n", - " contact: Any, geom1: int, geom2: int) -> Tuple[jax.Array, jax.Array]:\n", - " if geom1 > geom2:\n", - " geom1, geom2 = geom2, geom1\n", - " mask = (jp.array([geom1, geom2]) == contact.geom).all(axis=1)\n", - " idx = jp.where(mask, contact.dist, 1e4).argmin()\n", - " dist = contact.dist[idx] * mask[idx]\n", - " normal = (dist < 0) * contact.frame[idx, 0, :3]\n", - " return dist, normal\n", - "\n", - "\n", - "def _geoms_colliding(\n", - " state: Optional[State], geom1: int, geom2: int\n", - ") -> jax.Array:\n", - " return _get_collision_info(state.contact, geom1, geom2)[0] < 0\n", - "\n", - "\n", - "class PandaBringToTarget(PipelineEnv):\n", - " \"\"\"Environment for training franka panda to bring an object to target.\"\"\"\n", - "\n", - " def __init__(self, **kwargs):\n", - " global root_path\n", - " sys = _load_sys(FRANKA_PANDA_ROOT_PATH / 'mjx_single_cube.xml')\n", - " self._config = config = default_config()\n", - " nsteps = int(np.round(config.dt / sys.opt.timestep))\n", - " kwargs['backend'] = 'mjx'\n", - " kwargs['n_frames'] = nsteps\n", - " super().__init__(sys, **kwargs)\n", - "\n", - " # define constants\n", - " model = sys.mj_model\n", - " arm_joints = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5',\n", - " 'joint6', 'joint7']\n", - " finger_joints = ['finger_joint1', 'finger_joint2']\n", - " all_joints = arm_joints + finger_joints\n", - " self._robot_arm_qposadr = np.array([\n", - " model.jnt_qposadr[model.joint(j).id] for j in arm_joints])\n", - " self._robot_qposadr = np.array([\n", - " model.jnt_qposadr[model.joint(j).id] for j in all_joints])\n", - " self._gripper_site = model.site('gripper').id\n", - " self._left_finger_geom = model.geom('left_finger_pad').id\n", - " self._right_finger_geom = model.geom('right_finger_pad').id\n", - " self._hand_geom = model.geom('hand_capsule').id\n", - " self._box_body = model.body('box').id\n", - " self._box_qposadr = model.jnt_qposadr[model.body('box').jntadr[0]]\n", - " # TODO(btaba): replace with mocap_pos once MJX version 3.2.3 is released.\n", - " self._target_id = model.body('mocap_target').id\n", - " self._floor_geom = model.geom('floor').id\n", - " self._init_q = sys.mj_model.keyframe('home').qpos\n", - " self._init_box_pos = jp.array(\n", - " self._init_q[self._box_qposadr : self._box_qposadr + 3],\n", - " dtype=jp.float32)\n", - " self._init_ctrl = sys.mj_model.keyframe('home').ctrl\n", - " self._lowers = model.actuator_ctrlrange[:, 0]\n", - " self._uppers = model.actuator_ctrlrange[:, 1]\n", - "\n", - " def reset(self, rng: jax.Array) -> State:\n", - " rng, rng_box, rng_target = jax.random.split(rng, 3)\n", - "\n", - " # intialize box position\n", - " box_pos = jax.random.uniform(\n", - " rng_box, (3,),\n", - " minval=jp.array([-0.2, -0.2, 0.0]),\n", - " maxval=jp.array([0.2, 0.2, 0.0])) + self._init_box_pos\n", - "\n", - " # initialize target position\n", - " target_pos = jax.random.uniform(\n", - " rng_target, (3,),\n", - " minval=jp.array([-0.2, -0.2, 0.2]),\n", - " maxval=jp.array([0.2, 0.2, 0.4])) + self._init_box_pos\n", - "\n", - " # initialize pipeline state\n", - " init_q = jp.array(self._init_q).at[\n", - " self._box_qposadr : self._box_qposadr + 3].set(box_pos)\n", - " pipeline_state = self.pipeline_init(\n", - " init_q, jp.zeros(self.sys.nv)\n", - " )\n", - " pipeline_state = pipeline_state.replace(ctrl=self._init_ctrl)\n", - " # set target mocap position\n", - " # TODO(btaba): replace with mocap_pos once MJX version 3.2.3 is released.\n", - " pipeline_state = pipeline_state.replace(\n", - " xpos=pipeline_state.xpos.at[self._target_id, :].set(target_pos))\n", - "\n", - " # initialize env state and info\n", - " metrics = {\n", - " 'out_of_bounds': jp.array(0.0),\n", - " **{k: 0.0 for k in self._config.reward_scales.keys()},\n", - " }\n", - " info = {'rng': rng, 'target_pos': target_pos, 'reached_box': 0.0}\n", - " obs = self._get_obs(pipeline_state, info)\n", - " reward, done = jp.zeros(2)\n", - " state = State(pipeline_state, obs, reward, done, metrics, info)\n", - " return state\n", - "\n", - " def step(self, state: State, action: jax.Array) -> State:\n", - " delta = action * self._config.action_scale\n", - " ctrl = state.pipeline_state.ctrl + delta\n", - " ctrl = jp.clip(ctrl, self._lowers, self._uppers)\n", - "\n", - " # step the physics\n", - " data = self.pipeline_step(state.pipeline_state, ctrl)\n", - "\n", - " # compute reward terms\n", - " target_pos = state.info['target_pos']\n", - " box_pos = data.xpos[self._box_body]\n", - " gripper_pos = data.site_xpos[self._gripper_site]\n", - " box_target = 1 - jp.tanh(5 * jp.linalg.norm(target_pos - box_pos))\n", - " gripper_box = 1 - jp.tanh(5 * jp.linalg.norm(box_pos - gripper_pos))\n", - " robot_target_qpos = 1 - jp.tanh(\n", - " jp.linalg.norm(\n", - " state.pipeline_state.qpos[self._robot_arm_qposadr]\n", - " - self._init_q[self._robot_arm_qposadr]\n", - " )\n", - " )\n", - "\n", - " hand_floor_collision = [\n", - " _geoms_colliding(state.pipeline_state, self._floor_geom, g)\n", - " for g in [\n", - " self._left_finger_geom,\n", - " self._right_finger_geom,\n", - " self._hand_geom,\n", - " ]\n", - " ]\n", - " floor_collision = sum(hand_floor_collision) > 0\n", - " no_floor_collision = 1 - floor_collision\n", - "\n", - " state.info['reached_box'] = 1.0 * jp.maximum(\n", - " state.info['reached_box'],\n", - " (jp.linalg.norm(box_pos - gripper_pos) < 0.012),\n", - " )\n", - "\n", - " rewards = {\n", - " 'box_target': box_target * state.info['reached_box'],\n", - " 'gripper_box': gripper_box,\n", - " 'no_floor_collision': no_floor_collision,\n", - " 'robot_target_qpos': robot_target_qpos,\n", - " }\n", - " rewards = {k: v * self._config.reward_scales[k] for k, v in rewards.items()}\n", - " reward = jp.clip(sum(rewards.values()), -1e4, 1e4)\n", - "\n", - " out_of_bounds = jp.any(jp.abs(box_pos) > 1.0)\n", - " out_of_bounds |= box_pos[2] < 0.0\n", - " state.metrics.update(\n", - " out_of_bounds=out_of_bounds.astype(float),\n", - " **rewards)\n", - "\n", - " obs = self._get_obs(data, state.info)\n", - " done = out_of_bounds | jp.isnan(data.qpos).any() | jp.isnan(data.qvel).any()\n", - " done = done.astype(float)\n", - " state = State(data, obs, reward, done, state.metrics, state.info)\n", - "\n", - " return state\n", - "\n", - " def _get_obs(self, data: PipelineState, info: dict[str, Any]) -> jax.Array:\n", - " gripper_pos = data.site_xpos[self._gripper_site]\n", - " gripper_mat = data.site_xmat[self._gripper_site].ravel()\n", - " obs = jp.concatenate([\n", - " data.qpos,\n", - " data.qvel,\n", - " gripper_pos,\n", - " gripper_mat[3:],\n", - " data.xmat[self._box_body].ravel()[3:],\n", - " data.xpos[self._box_body] - data.site_xpos[self._gripper_site],\n", - " info['target_pos'] - data.xpos[self._box_body],\n", - " data.ctrl - data.qpos[self._robot_qposadr[:-1]],\n", - " ])\n", - "\n", - " return obs\n", - "\n", - "envs.register_environment('PandaBringToTarget', PandaBringToTarget)" - ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "id": "76g9uILMQVkc" - }, - "outputs": [], - "source": [ - "# instantiate the environment\n", - "env_name = 'PandaBringToTarget'\n", - "env = envs.get_environment(env_name)\n", - "\n", - "# define the jit reset/step functions\n", - "jit_reset = jax.jit(env.reset)\n", - "jit_step = jax.jit(env.step)" - ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "id": "10_vs9IDvnke" - }, - "outputs": [], - "source": [ - "#@title Train Pick-up-cube Policy\n", - "\n", - "make_networks_factory = functools.partial(\n", - " ppo_networks.make_ppo_networks,\n", - " policy_hidden_layer_sizes=(32, 32, 32, 32))\n", - "\n", - "train_fn = functools.partial(\n", - " ppo.train, num_timesteps=20_000_000, num_evals=4, reward_scaling=0.1,\n", - " episode_length=150, normalize_observations=True, action_repeat=1,\n", - " unroll_length=10, num_minibatches=32, num_updates_per_batch=8,\n", - " discounting=0.97, learning_rate=1e-3, entropy_cost=2e-2, num_envs=2048,\n", - " batch_size=512, num_resets_per_eval=1,\n", - " network_factory=make_networks_factory, seed=0)\n", - "\n", - "\n", - "x_data, y_data, y_dataerr = [], [], []\n", - "times = [datetime.now()]\n", - "def progress(num_steps, metrics):\n", - " times.append(datetime.now())\n", - " x_data.append(num_steps)\n", - " y_data.append(metrics['eval/episode_reward'])\n", - " y_dataerr.append(metrics['eval/episode_reward_std'])\n", - "\n", - " plt.xlim([0, train_fn.keywords['num_timesteps'] * 1.25])\n", - " plt.ylim([0, 2000])\n", - " plt.xlabel('# environment steps')\n", - " plt.ylabel('reward per episode')\n", - " plt.title(f'y={y_data[-1]:.3f}')\n", - " plt.errorbar(x_data, y_data, yerr=y_dataerr)\n", - " plt.show()\n", - "\n", - "make_inference_fn, params, _= train_fn(environment=env, progress_fn=progress)\n", - "jit_inference_fn = jax.jit(make_inference_fn(params, deterministic=True))\n", - "\n", - "print(f'time to jit: {times[1] - times[0]}')\n", - "print(f'time to train: {times[-1] - times[1]}')\n" - ] - }, - { - "cell_type": "code", - "execution_count": 0, - "metadata": { - "id": "jDJLcI0Bv5lD" - }, - "outputs": [], - "source": [ - "# initialize the state\n", - "rng = jax.random.PRNGKey(0)\n", - "state = jit_reset(rng)\n", - "rollout = [state.pipeline_state]\n", - "\n", - "# grab a trajectory\n", - "n_steps = 150\n", - "render_every = 2\n", - "\n", - "for i in range(n_steps):\n", - " act_rng, rng = jax.random.split(rng)\n", - " ctrl, _ = jit_inference_fn(state.obs, act_rng)\n", - " state = jit_step(state, ctrl)\n", - " rollout.append(state.pipeline_state)\n", - "\n", - " if state.done:\n", - " break\n", - "\n", - "media.show_video(env.render(rollout[::render_every]), fps=1.0 / env.dt / render_every)" + "By now, we have shown how MJX can be used to train policies for classic control and robotic locomotion. For a full suite of robotic locomotion and manipulation environments we encourage you to check out [MuJoCo Playground](https://github.com/google-deepmind/mujoco_playground). Many of the robotic environments have been transferred onto robots, as described on the [website](https://playground.mujoco.org/) and [technical report](https://playground.mujoco.org/assets/playground_technical_report.pdf).\n" ] } ], From 69c66aa1bb93d40a132e6930481475a25c4cf1c6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 17 Jan 2025 15:39:02 -0800 Subject: [PATCH 233/426] Fix typo. PiperOrigin-RevId: 716820809 Change-Id: I51f8608f0c01f4fc8016b4be779d98c8972bc913 --- doc/computation/fluid.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/computation/fluid.rst b/doc/computation/fluid.rst index a066a263..b006f758 100644 --- a/doc/computation/fluid.rst +++ b/doc/computation/fluid.rst @@ -308,7 +308,7 @@ We present the following result. :math:`\mathcal{E}` with :math:`\Pi_{\mathbf{u}}` (denoted :math:`\mathcal{E}^{\cap}_{\mathbf{u}}`). An important property of :math:`\mathcal{E}^{\mathrm{proj}}_{\mathbf{u}}` is that :math:`\mathbf{u}` is tangent - tangent to the ellipsoid :math:`\mathcal{E}` at every point on :math:`\mathcal{E}^{\mathrm{proj}}_{\mathbf{u}}`. + to the ellipsoid :math:`\mathcal{E}` at every point on :math:`\mathcal{E}^{\mathrm{proj}}_{\mathbf{u}}`. We can regard :math:`\mathcal{E}` as the image of the unit sphere :math:`\mathcal{S}` under a stretching transformation :math:`T = \mathrm{diag}(r_x, r_y, r_z)`. Furthermore, if :math:`\mathbf{\tilde{u}}` is a vector From 463bc47633f6cb747366fa14f8ad3b11fc564122 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 18 Jan 2025 12:38:28 -0800 Subject: [PATCH 234/426] Clarify that equality constraints defined using the body-based semantic are assumed to be satisfied at ``mjData.qpos0``. PiperOrigin-RevId: 717050088 Change-Id: I02e82cab5a60f0af48f7f921ba9bca11837f8063 --- doc/XMLreference.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 141a4d5b..1c856cf3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4380,7 +4380,7 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o - Using :ref:`body1` and :ref:`anchor` (both required) and optionally :ref:`body2`. When using this specification, the constraint is assumed to be - satisfied in the configuration in which the model is defined. + satisfied at the configuration in which the model is defined (``mjData.qpos0``). - :ref:`site1` and :ref:`site2` (both required). When using this specification, the two sites will be pulled together by the constraint, regardless of their position in the default configuration. An example of this specification is shown in @@ -4426,8 +4426,8 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o :at:`anchor`: :at-val:`real(3), optional` Coordinates of the 3D anchor point where the two bodies are connected, in the local coordinate frame of :at:`body1`. - The constraint is assumed to be satisfied in the configuration in which the model is defined, which lets the compiler - compute the associated anchor point for :at:`body2`. + The constraint is assumed to be satisfied in the configuration at which the model is defined (``mjData.qpos0``), + which lets the compiler compute the associated anchor point for :at:`body2`. .. _equality-connect-site1: From b2d353d062409446e4ac74eb70ecf7f5cb995a6d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 18 Jan 2025 12:41:05 -0800 Subject: [PATCH 235/426] Document default visible geom groups in XMLreference. PiperOrigin-RevId: 717050528 Change-Id: I0414495d2f5a17ac2d8ba35c9353ecf0b7e8a18d --- doc/XMLreference.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 1c856cf3..9a8a6e5d 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2367,7 +2367,8 @@ helps clarify the role of bodies and geoms in MuJoCo. This attribute specifies an integer group to which the geom belongs. The only effect on the physics is at compile time, when body masses and inertias are inferred from geoms selected based on their group; see inertiagrouprange attribute of :ref:`compiler `. At runtime this attribute is used by the visualizer to enable and disable - the rendering of entire geom groups. It can also be used as a tag for custom computations. + the rendering of entire geom groups. By default, groups 0, 1 and 2 are visible, while all other groups are invisible. + The group attribute can also be used as a tag for custom computations. .. _body-geom-priority: From 203e9bc29298d250195632fd207ce4484c84a018 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Sun, 19 Jan 2025 13:24:36 +0000 Subject: [PATCH 236/426] Remove trailing spaces --- doc/unity.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/unity.rst b/doc/unity.rst index 390ac52c..78fe2ef4 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -20,10 +20,10 @@ Installation instructions The `plug-in directory `__ includes a ``package.json`` file. Unity's package manager recognizes this file and will import the plug-in's C# codebase to your -project. In addition, Unity also needs the native MuJoCo library, which can be found in the corrsponding `platform -archive `__. If you wish to simply use the plug-in and not +project. In addition, Unity also needs the native MuJoCo library, which can be found in the corrsponding `platform +archive `__. If you wish to simply use the plug-in and not develop it, you should use one of the version-specific stable commits of the repository, identified by git tags. Check -out the relevant version of the cloned repository with git (``git checkout 3.X.Y`` where X and Y specify the engine +out the relevant version of the cloned repository with git (``git checkout 3.X.Y`` where X and Y specify the engine version). Simply using the ``main`` branch of the repository may not be compatible with the most recent release binary of MuJoCo. From 1d47a91c19eced1e9de1d445205bb31e1d1cae9d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 19 Jan 2025 11:33:35 -0800 Subject: [PATCH 237/426] Add a test for mj_solveM2 PiperOrigin-RevId: 717278730 Change-Id: I7507e569620c34f250482eec8402c08c793f4e48 --- test/engine/engine_core_smooth_test.cc | 46 ++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index e5f66c38..b63e57a3 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -545,6 +545,52 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { mj_deleteModel(m); } +TEST_F(CoreSmoothTest, SolveM2) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; + + mjData* d = mj_makeData(m); + mj_forward(m, d); + + int nv = m->nv; + int nC = m->nC; + + // copy LD into LDs: CSR format + vector LDs(nC); + for (int i=0; i < nC; i++) { + LDs[i] = d->qLD[d->mapM2C[i]]; + } + + // inverse square root of D from inertia LDL decomposition + vector sqrtInvD(nv); + for (int i=0; i < nv; i++) { + sqrtInvD[i] = 1 / mju_sqrt(d->qLD[m->dof_Madr[i]]); + } + + // compare full solve and half solve + int n = 3; + vector vec(nv*n); + vector vec2(nv*n); + for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i; + for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0; + vector res(nv*n); + + mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); + mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, n, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + + // expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v) + for (int i=0; i < n; i++) { + EXPECT_FLOAT_EQ(mju_dot(vec2.data() + i*nv, vec.data() + i*nv, nv), + mju_dot(res.data() + i*nv, res.data() + i*nv, nv)); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + TEST_F(CoreSmoothTest, FactorIs) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; From f13c107cd781e49df5ddf2f340b84944e602e4e5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 19 Jan 2025 11:57:53 -0800 Subject: [PATCH 238/426] Workaround to doc build error caused by sphinx-toolbox bug. See issue for details: https://github.com/sphinx-toolbox/sphinx-toolbox/issues/176 PiperOrigin-RevId: 717282405 Change-Id: I9e02804a3571fcca74eb7f174f3c3e262b0d3b65 --- doc/requirements.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/requirements.txt b/doc/requirements.txt index b382897d..f959062f 100644 --- a/doc/requirements.txt +++ b/doc/requirements.txt @@ -6,7 +6,9 @@ sphinxcontrib-youtube==1.2.0 sphinx-copybutton==0.5.2 sphinx-favicon==1.0.1 sphinx-reredirects==0.1.1 -sphinx-toolbox==3.4.0 +# TODO: b/390995275 - Bug in sphinx-toolbox, use Kevin's workaround until fixed. +sphinx-toolbox @ git+https://github.com/kevinzakka/sphinx-toolbox.git@gh-issue-fix +# sphinx-toolbox==3.4.0 nbsphinx==0.9.1 pandoc==1.1.0 pygments==2.15.0 From 2a100547bd689249e57540e0afcc83b28d52e60a Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Sun, 19 Jan 2025 22:07:47 -0800 Subject: [PATCH 239/426] Remove match in support.py (introduced in Python 3.10). https://github.com/google-deepmind/mujoco_playground/issues/16 PiperOrigin-RevId: 717395934 Change-Id: I0be1b8285a79702a1dfc086e15e07bab0f9f6d40 --- mjx/mujoco/mjx/_src/support.py | 194 ++++++++++++++++----------------- 1 file changed, 96 insertions(+), 98 deletions(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 748be165..aeb0f7ed 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -296,71 +296,70 @@ class BindModel(object): specs = [specs] ids = [] for spec in specs: - match spec: - case mujoco.MjsBody(): - self.prefix = 'body_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) - case mujoco.MjsJoint(): - self.prefix = 'jnt_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) - case mujoco.MjsGeom(): - self.prefix = 'geom_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) - case mujoco.MjsSite(): - self.prefix = 'site_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) - case mujoco.MjsLight(): - self.prefix = 'light_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) - case mujoco.MjsCamera(): - self.prefix = 'cam_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) - case mujoco.MjsMesh(): - self.prefix = 'mesh_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name) - case mujoco.MjsHField(): - self.prefix = 'hfield_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name) - case mujoco.MjsPair(): - self.prefix = 'pair_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_PAIR, spec.name) - case mujoco.MjsTendon(): - self.prefix = 'tendon_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) - case mujoco.MjsActuator(): - self.prefix = 'actuator_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) - case mujoco.MjsSensor(): - self.prefix = 'sensor_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) - case mujoco.MjsNumeric(): - self.prefix = 'numeric_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_NUMERIC, spec.name) - case mujoco.MjsText(): - self.prefix = 'text_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_TEXT, spec.name) - case mujoco.MjsTuple(): - self.prefix = 'tuple_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_TUPLE, spec.name) - case mujoco.MjsKey(): - self.prefix = 'key_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_KEY, spec.name) - case mujoco.MjsEquality(): - self.prefix = 'eq_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) - case mujoco.MjsExclude(): - self.prefix = 'exclude_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_EXCLUDE, spec.name) - case mujoco.MjsSkin(): - self.prefix = 'skin_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_SKIN, spec.name) - case mujoco.MjsMaterial(): - self.prefix = 'material_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_MATERIAL, spec.name) - case _: - raise ValueError('invalid spec type') + if isinstance(spec, mujoco.MjsBody): + self.prefix = 'body_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) + elif isinstance(spec, mujoco.MjsJoint): + self.prefix = 'jnt_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) + elif isinstance(spec, mujoco.MjsGeom): + self.prefix = 'geom_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) + elif isinstance(spec, mujoco.MjsSite): + self.prefix = 'site_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) + elif isinstance(spec, mujoco.MjsLight): + self.prefix = 'light_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) + elif isinstance(spec, mujoco.MjsCamera): + self.prefix = 'cam_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) + elif isinstance(spec, mujoco.MjsMesh): + self.prefix = 'mesh_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_MESH, spec.name) + elif isinstance(spec, mujoco.MjsHField): + self.prefix = 'hfield_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_HFIELD, spec.name) + elif isinstance(spec, mujoco.MjsPair): + self.prefix = 'pair_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_PAIR, spec.name) + elif isinstance(spec, mujoco.MjsTendon): + self.prefix = 'tendon_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) + elif isinstance(spec, mujoco.MjsActuator): + self.prefix = 'actuator_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) + elif isinstance(spec, mujoco.MjsSensor): + self.prefix = 'sensor_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) + elif isinstance(spec, mujoco.MjsNumeric): + self.prefix = 'numeric_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_NUMERIC, spec.name) + elif isinstance(spec, mujoco.MjsText): + self.prefix = 'text_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_TEXT, spec.name) + elif isinstance(spec, mujoco.MjsTuple): + self.prefix = 'tuple_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_TUPLE, spec.name) + elif isinstance(spec, mujoco.MjsKey): + self.prefix = 'key_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_KEY, spec.name) + elif isinstance(spec, mujoco.MjsEquality): + self.prefix = 'eq_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) + elif isinstance(spec, mujoco.MjsExclude): + self.prefix = 'exclude_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_EXCLUDE, spec.name) + elif isinstance(spec, mujoco.MjsSkin): + self.prefix = 'skin_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_SKIN, spec.name) + elif isinstance(spec, mujoco.MjsMaterial): + self.prefix = 'material_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_MATERIAL, spec.name) + else: + raise ValueError('invalid spec type') if idx < 0: - raise KeyError(f'invalid name: {spec.name}') + raise KeyError(f'invalid name: {spec.name}') # pytype: disable=attribute-error ids.append(idx) if len(ids) == 1: self.id = ids[0] @@ -388,41 +387,40 @@ class BindData(object): specs = [specs] ids = [] for spec in specs: - match spec: - case mujoco.MjsBody(): - self.prefix = '' - idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) - case mujoco.MjsJoint(): - self.prefix = 'jnt_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) - case mujoco.MjsGeom(): - self.prefix = 'geom_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) - case mujoco.MjsSite(): - self.prefix = 'site_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) - case mujoco.MjsLight(): - self.prefix = 'light_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) - case mujoco.MjsCamera(): - self.prefix = 'cam_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) - case mujoco.MjsTendon(): - self.prefix = 'ten_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) - case mujoco.MjsActuator(): - self.prefix = 'actuator_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) - case mujoco.MjsSensor(): - self.prefix = 'sensor_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) - case mujoco.MjsEquality(): - self.prefix = 'eq_' - idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) - case _: - raise ValueError('invalid spec type') + if isinstance(spec, mujoco.MjsBody): + self.prefix = '' + idx = name2id(model, mujoco.mjtObj.mjOBJ_BODY, spec.name) + elif isinstance(spec, mujoco.MjsJoint): + self.prefix = 'jnt_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_JOINT, spec.name) + elif isinstance(spec, mujoco.MjsGeom): + self.prefix = 'geom_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_GEOM, spec.name) + elif isinstance(spec, mujoco.MjsSite): + self.prefix = 'site_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_SITE, spec.name) + elif isinstance(spec, mujoco.MjsLight): + self.prefix = 'light_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_LIGHT, spec.name) + elif isinstance(spec, mujoco.MjsCamera): + self.prefix = 'cam_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, spec.name) + elif isinstance(spec, mujoco.MjsTendon): + self.prefix = 'ten_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_TENDON, spec.name) + elif isinstance(spec, mujoco.MjsActuator): + self.prefix = 'actuator_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, spec.name) + elif isinstance(spec, mujoco.MjsSensor): + self.prefix = 'sensor_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, spec.name) + elif isinstance(spec, mujoco.MjsEquality): + self.prefix = 'eq_' + idx = name2id(model, mujoco.mjtObj.mjOBJ_EQUALITY, spec.name) + else: + raise ValueError('invalid spec type') if idx < 0: - raise KeyError(f'invalid name: {spec.name}') + raise KeyError(f'invalid name: {spec.name}') # pytype: disable=attribute-error ids.append(idx) if len(ids) == 1: self.id = ids[0] From 5c4c79cd6a069e4deaa17a416752aea430ca5ec5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 19 Jan 2025 22:46:38 -0800 Subject: [PATCH 240/426] Refactor LD back-substitution (CSR version). PiperOrigin-RevId: 717403904 Change-Id: Idcd5e71f03a960203c22cb737d399fac5a0ba59c --- src/engine/engine_core_smooth.c | 124 +++++++++++++++----------------- 1 file changed, 59 insertions(+), 65 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index a3e255c7..cb49b214 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1609,84 +1609,78 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // like mj_solveLD, but using the CSR representation of L void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { - // single vector - if (n == 1) { - // x <- L^-T x - for (int i=nv-1; i > 0; i--) { - // skip diagonal rows, zero elements in input vector - mjtNum x_i = x[i]; - if (x_i == 0 || diagnum[i]) { - continue; - } - - int start = rowadr[i]; - int end = start + rownnz[i] - 1; - for (int adr=start; adr < end; adr++) { - x[colind[adr]] -= qLDs[adr] * x_i; - } + // x <- L^-T x + for (int i=nv-1; i > 0; i--) { + // skip diagonal rows + if (diagnum[i]) { + continue; } - // x <- D^-1 x - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; - } - - // x <- L^-1 x - for (int i=1; i < nv; i++) { - // skip diagonal rows - if (diagnum[i]) { - i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i - continue; - } - - int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, rownnz[i] - 1, colind+adr, /*flg_unc1=*/0); - } - } - - // multiple vectors - else { - // x <- L^-T x - for (int i=nv-1; i > 0; i--) { - // skip diagonal rows - if (diagnum[i]) { - continue; - } - - int start = rowadr[i]; - int end = start + rownnz[i] - 1; - for (int adr=start; adr < end; adr++) { - int j = colind[adr]; - mjtNum val = qLDs[adr]; - for (int offset=0; offset < n*nv; offset+=nv) { - mjtNum x_i; - if ((x_i = x[i+offset])) { - x[j+offset] -= val * x_i; - } + // one vector + if (n == 1) { + mjtNum x_i; + if ((x_i = x[i])) { + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int adr=start; adr < end; adr++) { + x[colind[adr]] -= qLDs[adr] * x_i; } } } - // x <- D^-1 x - for (int i=0; i < nv; i++) { - mjtNum invD_i = qLDiagInv[i]; + // multiple vectors + else { + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int offset=0; offset < n*nv; offset+=nv) { + mjtNum x_i; + if ((x_i = x[i+offset])) { + for (int adr=start; adr < end; adr++) { + x[offset + colind[adr]] -= qLDs[adr] * x_i; + } + } + } + } + } + + // x <- D^-1 x + for (int i=0; i < nv; i++) { + mjtNum invD_i = qLDiagInv[i]; + + // one vector + if (n == 1) { + x[i] *= invD_i; + } + + // multiple vectors + else { for (int offset=0; offset < n*nv; offset+=nv) { x[i+offset] *= invD_i; } } + } - // x <- L^-1 x - for (int i=1; i < nv; i++) { - // skip diagonal rows - if (diagnum[i]) { - i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i - continue; + // x <- L^-1 x + for (int i=1; i < nv; i++) { + // skip diagonal rows + if (diagnum[i]) { + i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i + continue; + } + + int adr = rowadr[i]; + int d = rownnz[i] - 1; + if (d > 0) { + // one vector + if (n == 1) { + x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); } - int adr = rowadr[i]; - int d = rownnz[i] - 1; - for (int offset=0; offset < n*nv; offset+=nv) { - x[i+offset] -= mju_dotSparse(qLDs+adr, x+offset, d, colind+adr, /*flg_unc1=*/0); + // multiple vectors + else { + for (int offset=0; offset < n*nv; offset+=nv) { + x[i+offset] -= mju_dotSparse(qLDs+adr, x+offset, d, colind+adr, /*flg_unc1=*/0); + } } } } From 600f4acb529cc08e199d12141c182ff0a0142179 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 20 Jan 2025 08:58:45 -0800 Subject: [PATCH 241/426] Change GJK and EPA return values. PiperOrigin-RevId: 717549079 Change-Id: I42df5660bcc8336e2e9c8fce71385cbb06104848 --- src/engine/engine_collision_gjk.c | 48 ++++++++++++++++--------------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index fedaded6..48472675 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -81,8 +81,9 @@ static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj // status must have initial tetrahedrons static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); -// return the penetration depth of two convex objects; witness points are in status->{x1, x2} -static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); +// return a face of the expanded polytope that best approximates the pentration depth +// witness points are in status->{x1, x2} +static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); // -------------------------------- inlined 3D vector utils -------------------------------------- @@ -145,7 +146,7 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // GJK algorithm -static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { +static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact int backup_gjk = !get_dist; // use gjkIntersect if no geom distances needed mjtNum *simplex1 = status->simplex1; // simplex for obj1 @@ -192,7 +193,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { status->nsimplex = 0; status->nx = 0; status->dist = mjMAXVAL; - return status->dist; + return; } } else if (status->dist_cutoff < mjMAXVAL) { mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k); @@ -201,7 +202,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { status->nsimplex = 0; status->nx = 0; status->dist = mjMAXVAL; - return status->dist; + return; } } @@ -213,7 +214,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { if (ret != -1) { status->nx = 0; status->dist = ret > 0 ? 0 : mjMAXVAL; - return status->dist; + return; } k = status->gjk_iterations; backup_gjk = 0; @@ -259,7 +260,6 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { status->gjk_iterations = k; status->nsimplex = n; status->dist = mju_norm3(x_k); - return status->dist; } @@ -1294,8 +1294,9 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu -// return the penetration depth of two convex objects; witness points are in status->{x1, x2} -static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { +// return a face of the expanded polytope that best approximates the pentration depth +// witness points are in status->{x1, x2} +static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum tolerance = status->tolerance, lower, upper = FLT_MAX; int k, kmax = status->max_iterations; mjData* d = (mjData*) obj1->data; @@ -1368,7 +1369,8 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o mj_freeStack(d); status->epa_iterations = k; status->nx = 0; - return 0; + status->dist = 0; + return NULL; } // store face in map @@ -1395,7 +1397,8 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o mj_freeStack(d); status->epa_iterations = k; status->nx = 0; - return 0; + status->dist = 0; + return NULL; } // store face in map @@ -1417,7 +1420,8 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o epaWitness(pt, face, status->x1, status->x2); status->epa_iterations = k; status->nx = 1; - return face->dist; + status->dist = -face->dist; + return face; } @@ -1444,8 +1448,7 @@ static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - // set up - mjtNum dist; + // setup obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); status->gjk_iterations = 0; @@ -1488,11 +1491,11 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m } status->dist_cutoff += margin1 + margin2; - dist = gjk(status, obj1, obj2); + gjk(status, obj1, obj2); status->dist_cutoff = config->dist_cutoff; // shallow penetration, inflate contact - if (dist > 0) { + if (status->dist > 0) { inflate(status, margin1, margin2); if (status->dist > status->dist_cutoff) { status->dist = mjMAXVAL; @@ -1515,14 +1518,15 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m obj2->center(status->x2, obj2); } - dist = gjk(status, obj1, obj2); + gjk(status, obj1, obj2); // penetration recovery for contacts not needed if (!config->max_contacts) { - return dist; + return status->dist; } - if (dist <= config->tolerance && status->nsimplex > 1) { + if (status->dist <= config->tolerance && status->nsimplex > 1) { + status->dist = 0; // assume touching int N = status->max_iterations; mjData* d = (mjData*) obj1->data; mj_markStack((mjData*) obj1->data); @@ -1561,11 +1565,9 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m // simplex not on boundary (objects are penetrating) if (!ret) { - dist = -epa(status, &pt, obj1, obj2); - } else { - dist = 0; + epa(status, &pt, obj1, obj2); } mj_freeStack(d); } - return dist; + return status->dist; } From 699a6765e3e6143d5f5e6a152e8b9d5866a176af Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Jan 2025 11:01:00 -0800 Subject: [PATCH 242/426] Add find functions for joints, lights, and cameras to mjSpec bindings. PiperOrigin-RevId: 717578768 Change-Id: I2087ccbb9cf8b3b8ec03e9eb2edb0707d4a4cac1 --- python/mujoco/specs.cc | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index eb8e0ea9..560d5ba6 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -408,6 +408,27 @@ PYBIND11_MODULE(_specs, m) { return mjs_asGeom(mjs_findElement(self.ptr, mjOBJ_GEOM, name.c_str())); }, py::return_value_policy::reference_internal); + mjSpec.def( + "find_joint", + [](MjSpec& self, std::string& name) -> raw::MjsJoint* { + return mjs_asJoint( + mjs_findElement(self.ptr, mjOBJ_JOINT, name.c_str())); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_light", + [](MjSpec& self, std::string& name) -> raw::MjsLight* { + return mjs_asLight( + mjs_findElement(self.ptr, mjOBJ_LIGHT, name.c_str())); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_camera", + [](MjSpec& self, std::string& name) -> raw::MjsCamera* { + return mjs_asCamera( + mjs_findElement(self.ptr, mjOBJ_CAMERA, name.c_str())); + }, + py::return_value_policy::reference_internal); mjSpec.def("compile", [mjmodel_from_spec_ptr](MjSpec& self) -> py::object { if (self.assets.empty()) { return mjmodel_from_spec_ptr(reinterpret_cast(self.ptr)); From ff4783482c0c9fafb43da275dd143eb92cb29d64 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Jan 2025 11:12:24 -0800 Subject: [PATCH 243/426] Fix misleading line for creating list of models in rollout_test.py PiperOrigin-RevId: 717582208 Change-Id: I1b4e40fc5ccbfe68fb7dc240715c704f1f68d9fe --- python/mujoco/rollout_test.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index af8a5d3a..25689a68 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -15,6 +15,7 @@ """tests for rollout function.""" import concurrent.futures +import copy import threading from absl.testing import absltest @@ -23,6 +24,7 @@ import mujoco from mujoco import rollout import numpy as np + # -------------------------- models used for testing --------------------------- TEST_XML = r""" @@ -473,7 +475,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): def thread_initializer(): thread_local.data = mujoco.MjData(model) - model_list = [model] * nroll + model_list = [copy.copy(model) for _ in range(nroll)] def call_rollout(initial_state, control, state, sensordata): rollout.rollout( From 68cd2633374fb6145648fae5e1fb64f15a5ae4f3 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 20 Jan 2025 12:10:36 -0800 Subject: [PATCH 244/426] Change type of `light_poscom0` and `actuator_acc0'. This is required for batching over `mjx.Model` in case recompilation is used to change inertial values. PiperOrigin-RevId: 717596504 Change-Id: I4b6780903ebf12ebbf8b7bc7b8990a01609d4491 --- mjx/mujoco/mjx/_src/types.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 16fa3a81..030f4a27 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -999,7 +999,7 @@ class Model(PyTreeNode): light_castshadow: jax.Array light_pos: jax.Array light_dir: jax.Array - light_poscom0: np.ndarray = _restricted_to('mujoco') + light_poscom0: jax.Array light_pos0: np.ndarray light_dir0: np.ndarray light_cutoff: jax.Array @@ -1129,7 +1129,7 @@ class Model(PyTreeNode): actuator_actrange: jax.Array actuator_gear: jax.Array actuator_cranklength: np.ndarray - actuator_acc0: np.ndarray + actuator_acc0: jax.Array actuator_lengthrange: np.ndarray actuator_plugin: np.ndarray = _restricted_to('mujoco') sensor_type: np.ndarray From baf1c43b3cee0aa171f63112d733fa539e1bf5f5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Jan 2025 14:26:33 -0800 Subject: [PATCH 245/426] Fix more misleading line for creating list of models in rollout_test.py PiperOrigin-RevId: 717628943 Change-Id: I83a314565ce4ec683afeea18c8c8ce9e23d5452e --- python/mujoco/rollout_test.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 25689a68..23139ba8 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -532,7 +532,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): sensordata = np.empty((nroll, nstep, model.nsensordata)) control = np.random.randn(nroll, nstep, model.nu) - model_list = [model] * nroll + model_list = [copy.copy(model) for _ in range(nroll)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] rollout.rollout( @@ -561,7 +561,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): sensordata = np.empty((nroll, nstep, model.nsensordata)) control = np.random.randn(nroll, nstep, model.nu) - model_list = [model] * nroll + model_list = [copy.copy(model) for _ in range(nroll)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] with rollout.Rollout(nthread=num_workers) as rollout_: @@ -610,7 +610,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): sensordata = np.empty((nroll, nstep, model.nsensordata)) control = np.random.randn(nroll, nstep, model.nu) - model_list = [model] * nroll + model_list = [copy.copy(model) for _ in range(nroll)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] for _ in range(2): @@ -871,7 +871,7 @@ def py_rollout( nstep = control.shape[1] if isinstance(model, mujoco.MjModel): - model = [model] * nroll + model = [copy.copy(model) for _ in range(nroll)] nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) From a5ab7a95151617ae9d8ed1a3476202c95b476161 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Jan 2025 15:15:38 -0800 Subject: [PATCH 246/426] Switch mj_solveM2 to use CSR representation. PiperOrigin-RevId: 717639495 Change-Id: I59eee0f23606481480ec5acb8a3bba2c14b2ea2f --- src/engine/engine_core_smooth.c | 63 ++++++++++++++++++++------------- 1 file changed, 38 insertions(+), 25 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index cb49b214..a2a43c39 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1668,9 +1668,10 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv continue; } - int adr = rowadr[i]; - int d = rownnz[i] - 1; - if (d > 0) { + int d; + if ((d = rownnz[i] - 1) > 0) { + int adr = rowadr[i]; + // one vector if (n == 1) { x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); @@ -1769,41 +1770,53 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, const mjtNum* sqrtInvD, int n) { // local copies of key variables - mjtNum* qLD = d->qLD; - int* dof_Madr = m->dof_Madr; - int* dof_parentid = m->dof_parentid; - int nv = m->nv; + int nv = m->nv, nC = m->nC; + const int* rownnz = d->C_rownnz; + const int* rowadr = d->C_rowadr; + const int* colind = d->C_colind; + const int* diagnum = m->dof_simplenum; // x = y mju_copy(x, y, n * nv); - // loop over the n input vectors - for (int ivec=0; ivec < n; ivec++) { - int offset = ivec*nv; + // temporary: make local CSR version of qLD + mj_markStack(d); + mjtNum* qLD = mjSTACKALLOC(d, nC, mjtNum); + for (int i=0; i < nC; i++) { + qLD[i] = d->qLD[d->mapM2C[i]]; + } - // x <- inv(L') * x; skip simple, exploit sparsity of input vector - for (int i=nv-1; i >= 0; i--) { - mjtNum tmp; - if (!m->dof_simplenum[i] && (tmp = x[i+offset])) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; + // x <- L^-T x + for (int i=nv-1; i > 0; i--) { + // skip diagonal rows + if (diagnum[i]) { + continue; + } - // traverse ancestors backwards - while (j >= 0) { - x[j+offset] -= qLD[Madr_ij++] * tmp; // x(j) -= L(i,j) * x(i) + // prepare row i column address range + int start = rowadr[i]; + int end = start + rownnz[i] - 1; - // advance to parent - j = dof_parentid[j]; + // process all vectors + for (int offset=0; offset < n*nv; offset+=nv) { + mjtNum x_i; + if ((x_i = x[i+offset])) { + for (int adr=start; adr < end; adr++) { + x[offset + colind[adr]] -= qLD[adr] * x_i; } } } + } - // x <- sqrt(inv(D)) * x - for (int i=0; i < nv; i++) { - x[i+offset] *= sqrtInvD[i]; // x(i) /= sqrt(L(i,i)) + // x <- D^-1/2 x + for (int i=0; i < nv; i++) { + mjtNum invD_i = sqrtInvD[i]; + for (int offset=0; offset < n*nv; offset+=nv) { + x[i+offset] *= invD_i; } } + + mj_freeStack(d); } From 2624d524bab37c3b0a3e5aa35cd795c1c72b2c9e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Jan 2025 18:34:58 -0800 Subject: [PATCH 247/426] Switch mj_solveM_island to use CSR representation PiperOrigin-RevId: 717684720 Change-Id: I7d74a5d4aef4aa5b0c0acddf88c5b8591d63e88d --- src/engine/engine_core_smooth.c | 58 ++++++++--------- src/engine/engine_core_smooth.h | 3 +- src/engine/engine_solver.c | 4 +- test/engine/engine_core_smooth_test.cc | 87 ++++++++++++++------------ 4 files changed, 80 insertions(+), 72 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index a2a43c39..01911f0d 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1701,7 +1701,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { // in-place sparse backsubstitution for one island: x = inv(L'*D*L)*x // L is in lower triangle of qLD; D is on diagonal of qLD -void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) { +void mj_solveM_island(const mjModel* m, mjData* d, mjtNum* restrict x, int island) { // if no islands, call mj_solveLD const mjtNum* qLD = d->qLD; const mjtNum* qLDiagInv = d->qLDiagInv; @@ -1710,10 +1710,19 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int return; } - // local constants: general - const int* Madr = m->dof_Madr; - const int* parentid = m->dof_parentid; - const int* simplenum = m->dof_simplenum; + // local copies of key variables + const int* rownnz = d->C_rownnz; + const int* rowadr = d->C_rowadr; + const int* colind = d->C_colind; + const int* diagnum = m->dof_simplenum; + + // temporary: make local CSR version of qLD + int nC = m->nC; + mj_markStack(d); + mjtNum* qLDs = mjSTACKALLOC(d, nC, mjtNum); + for (int i=0; i < nC; i++) { + qLDs[i] = d->qLD[d->mapM2C[i]]; + } // local constants: island specific int ndof = d->island_dofnum[island]; @@ -1723,18 +1732,12 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int // x <- inv(L') * x; skip simple, exploit sparsity of input vector for (int k=ndof-1; k >= 0; k--) { int i = dofind[k]; - if (!simplenum[i] && x[k]) { - // init - int Madr_ij = Madr[i]+1; - int j = parentid[i]; - - // traverse ancestors backwards - // read directly from x[l] since j cannot be a parent of itself - while (j >= 0) { - x[islandind[j]] -= qLD[Madr_ij++]*x[k]; // x(j) -= L(i,j) * x(i) - - // advance to parent - j = parentid[j]; + mjtNum x_k; + if (!diagnum[i] && (x_k = x[k])) { + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int adr=end-1; adr >= start; adr--) { + x[islandind[colind[adr]]] -= qLDs[adr] * x_k; } } } @@ -1747,21 +1750,20 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int // x <- inv(L) * x; skip simple for (int k=0; k < ndof; k++) { int i = dofind[k]; - if (!simplenum[i]) { - // init - int Madr_ij = Madr[i]+1; - int j = parentid[i]; - // traverse ancestors backwards - // write directly in x[i] since i cannot be a parent of itself - while (j >= 0) { - x[k] -= qLD[Madr_ij++]*x[islandind[j]]; // x(i) -= L(i,j) * x(j) + // skip diagonal rows + if (diagnum[i]) { + continue; + } - // advance to parent - j = parentid[j]; - } + int start = rowadr[i]; + int end = start + rownnz[i] - 1; + for (int adr=end-1; adr >= start; adr--) { + x[k] -= x[islandind[colind[adr]]] * qLDs[adr]; } } + + mj_freeStack(d); } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index a710416c..ec8d168d 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -71,8 +71,9 @@ MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, i // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); +// TODO(tassa): Restore mjData const-ness. // sparse backsubstitution for one island: x = inv(L'*D*L)*x, use factorization in d -MJAPI void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* x, int island); +MJAPI void mj_solveM_island(const mjModel* m, mjData* d, mjtNum* x, int island); // half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 68465374..f23d8bf5 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -895,9 +895,9 @@ static void CGupdateConstraint(const mjModel* m, mjData* d, mjCGContext* ctx) { } - +// TODO(tassa): Restore mjData const-ness. // update grad, Mgrad -static void CGupdateGradient(const mjModel* m, const mjData* d, mjCGContext* ctx) { +static void CGupdateGradient(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = ctx->nv; const int* dofind = ctx->dofind; diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index b63e57a3..916fb625 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -353,58 +353,63 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) { static const char* const kIlslandEfcPath = "engine/testdata/island/island_efc.xml"; +static const char* const kModelPath = + "testdata/model.xml"; TEST_F(CoreSmoothTest, SolveMIsland) { - const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); - mjData* data = mj_makeData(model); - int nv = model->nv; + for (auto model_path : {kModelPath, kIlslandEfcPath}) { + const std::string xml_path = GetTestDataFilePath(model_path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + int nv = model->nv; - // allocate vec, fill with arbitrary values, copy to sol - mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); - mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); - for (int i=0; i < nv; i++) { - vec[i] = 0.2 + 0.3*i; - } - mju_copy(res, vec, nv); + // allocate vec, fill with arbitrary values, copy to sol + mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); + mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); + for (int i=0; i < nv; i++) { + vec[i] = 0.2 + 0.3*i; + } + mju_copy(res, vec, nv); - // simulate for 0.2 seconds - mj_resetData(model, data); - while (data->time < 0.2) { - mj_step(model, data); - } - mj_forward(model, data); + if (model->nkey > 0) mj_resetDataKeyframe(model, data, 0); - // divide by mass matrix: sol = M^-1 * vec - mj_solveM(model, data, res, res, 1); - - // iterate over islands - for (int i=0; i < data->nisland; i++) { - // allocate dof vectors for island - int dofnum = data->island_dofnum[i]; - mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); - - // copy values into sol_i - int* dofind = data->island_dofind + data->island_dofadr[i]; - for (int j=0; j < dofnum; j++) { - res_i[j] = vec[dofind[j]]; + for (int i=0; i < 6; i++) { + mj_step(model, data); } - // divide by mass matrix, for this island - mj_solveM_island(model, data, res_i, i); + mj_forward(model, data); - // expect corresponding values to match - for (int j=0; j < dofnum; j++) { - EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-12)); + // divide by mass matrix: sol = M^-1 * vec + mj_solveM(model, data, res, res, 1); + + // iterate over islands + for (int i=0; i < data->nisland; i++) { + // allocate dof vectors for island + int dofnum = data->island_dofnum[i]; + mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + + // copy values into sol_i + int* dofind = data->island_dofind + data->island_dofadr[i]; + for (int j=0; j < dofnum; j++) { + res_i[j] = vec[dofind[j]]; + } + + // divide by mass matrix, for this island + mj_solveM_island(model, data, res_i, i); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-14)); + } + + mju_free(res_i); } - mju_free(res_i); + mju_free(res); + mju_free(vec); + mj_deleteData(data); + mj_deleteModel(model); } - - mju_free(res); - mju_free(vec); - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kInertiaPath = "engine/testdata/inertia.xml"; From cee122116ece5a31458bf7dff5b7726c9d3b543a Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 21 Jan 2025 00:34:40 -0500 Subject: [PATCH 248/426] rollout: fix bugs in checking list lengths, add tests --- python/mujoco/rollout.cc | 12 +++-- python/mujoco/rollout.py | 2 +- python/mujoco/rollout_test.py | 82 ++++++++++++++++++++++++++++++++++- 3 files changed, 90 insertions(+), 6 deletions(-) diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index 3f27f1d6..42519189 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -254,16 +254,20 @@ class Rollout { } // check length d and nthread are consistent - if (this->nthread_ == 0 && py::len(d) > 1) { + if (py::len(d) == 0) { + std::ostringstream msg; + msg << "The list of data instances is empty"; + throw py::value_error(msg.str()); + } else if (this->nthread_ == 0 && py::len(d) > 1) { std::ostringstream msg; msg << "More than one data instance passed but " << "rollout is configured to run on main thread"; - py::value_error(msg.str()); - } else if (this->nthread_ != py::len(d)) { + throw py::value_error(msg.str()); + } else if (this->nthread_ > 0 && this->nthread_ != py::len(d)) { std::ostringstream msg; msg << "Length of data: " << py::len(d) << " not equal to nthread: " << this->nthread_; - py::value_error(msg.str()); + throw py::value_error(msg.str()); } std::vector data_ptrs(py::len(d)); diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 95b6ad3f..68a9be23 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -172,7 +172,7 @@ class Rollout: if isinstance(model, list) and nroll == 1: nroll = len(model) - if isinstance(model, list) and len(model) != nroll: + if isinstance(model, list) and len(model) > 1 and len(model) != nroll: raise ValueError( f'nroll inferred as {nroll} but model is length {len(model)}' ) diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index af8a5d3a..b4c45569 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -15,13 +15,15 @@ """tests for rollout function.""" import concurrent.futures +import copy import threading from absl.testing import absltest from absl.testing import parameterized +import numpy as np + import mujoco from mujoco import rollout -import numpy as np # -------------------------- models used for testing --------------------------- @@ -794,6 +796,84 @@ class MuJoCoRolloutTest(parameterized.TestCase): np.testing.assert_array_equal(state, state2) np.testing.assert_array_equal(sensordata, sensordata2) + def test_length_one_model_list(self): + model = mujoco.MjModel.from_xml_string(TEST_XML) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + initial_state = np.random.randn(nstate) + control = np.random.randn(3, 3, model.nu) + + state, sensordata = rollout.rollout(model, data, initial_state, control) + state2, sensordata2 = rollout.rollout([model], data, initial_state, control) + + # assert that we get same outputs + np.testing.assert_array_equal(state, state2) + np.testing.assert_array_equal(sensordata, sensordata2) + + def test_data_sizes(self): + model = mujoco.MjModel.from_xml_string(TEST_XML) + nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) + data = mujoco.MjData(model) + + initial_state = np.random.randn(nstate) + control = np.random.randn(3, 3, model.nu) + + # Test passing empty lists for data + with self.assertRaisesWithLiteralMatch( + ValueError, 'The list of data instances is empty' + ): + rollout.rollout(model, [], initial_state, control) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'The list of data instances is empty' + ): + with rollout.Rollout(nthread=0) as rollout_: + rollout_.rollout(model, [], initial_state, control) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'The list of data instances is empty' + ): + with rollout.Rollout(nthread=1) as rollout_: + rollout_.rollout(model, [], initial_state, control) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'The list of data instances is empty' + ): + with rollout.Rollout(nthread=2) as rollout_: + rollout_.rollout(model, [], initial_state, control) + + # Test checking that len(data) equals nthread + with self.assertRaisesWithLiteralMatch( + ValueError, + 'More than one data instance passed but rollout is configured to run on' + ' main thread', + ): + with rollout.Rollout(nthread=0) as rollout_: + rollout_.rollout( + model, [copy.copy(data) for i in range(2)], initial_state, control + ) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'Length of data: 1 not equal to nthread: 2' + ): + with rollout.Rollout(nthread=2) as rollout_: + rollout_.rollout(model, data, initial_state, control) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'Length of data: 1 not equal to nthread: 2' + ): + with rollout.Rollout(nthread=2) as rollout_: + rollout_.rollout(model, [data], initial_state, control) + + with self.assertRaisesWithLiteralMatch( + ValueError, 'Length of data: 3 not equal to nthread: 2' + ): + with rollout.Rollout(nthread=2) as rollout_: + rollout_.rollout( + model, [copy.copy(data) for i in range(3)], initial_state, control + ) + # -------------- Python implementation of rollout functionality ---------------- From bd1bbfe4014657f7ae930822a16351668582898e Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 21 Jan 2025 02:50:05 -0800 Subject: [PATCH 249/426] Fix bug where EPA horizon returns zero edges due to numerical inaccuracies in float32. PiperOrigin-RevId: 717825833 Change-Id: I9fa440816765d4c3eaa0c6c26ec0bd90c774fa01 --- src/engine/engine_collision_gjk.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 48472675..ec81a722 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1347,6 +1347,15 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob h.w = w; horizon(&h, face); + // unrecoverable numerical issue; at least one face was deleted so nedges is 3 or more + if (h.nedges < 3) { + mj_freeStack(d); + status->epa_iterations = k; + status->nx = 0; + status->dist = 0; + return NULL; + } + // insert w as new vertex and attach faces along the horizon int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges; From 4bb6aeb19f65920791e4907cdb6a6f29bcfa44ec Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Jan 2025 07:28:44 -0800 Subject: [PATCH 250/426] Change site and frame attach API. Old API: ``` site.attach(child_spec) frame.attach(child_spec) ``` New API: ``` parent_spec.attach(child_spec, frame=frame_name_or_object) parent_spec.attach(site_spec, site=site_name_or_object) ``` This enables accessing the parent object during attach, which will be used in a follow-up CL to automatically append the child assets to the parent. PiperOrigin-RevId: 717908589 Change-Id: I0fb27e99694c954cb8ba8a8c98484ffd53a4d6db --- doc/python.rst | 15 ++--- python/mujoco/specs.cc | 106 +++++++++++++++++++++--------------- python/mujoco/specs_test.py | 45 ++++++++++++++- 3 files changed, 114 insertions(+), 52 deletions(-) diff --git a/doc/python.rst b/doc/python.rst index 95bd409a..5a479d98 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -534,11 +534,12 @@ It is possible to combine multiple specs by using attachments. The following opt the reference to the attached body, which should be identical to the body used as input. - Attach a frame from the child spec to a body in the parent spec: ``body.attach_frame(frame, prefix, suffix)``, returns the reference to the attached frame, which should be identical to the frame used as input. -- Attach a body from the child spec to a site in the parent spec: ``site.attach(body, prefix, suffix)``, returns the - reference to the attached body, which should be identical to the body used as input. -- Attach the worldbody from the child spec to a frame in the parent spec and transform it to a frame: - ``body.attach(spec, prefix, suffix)``, returns the newly created frame that the child worldbody was transformed - into. +- Attach a child spec to a site in the parent spec: ``spec.attach(child_spec, site=site_name_or_obj)``, returns the + reference to a frame, which is the attached worldbody transformed into a frame. The site must belong to the child + spec. Prefix and suffix can also be specified as keyword arguments. +- Attach a child spec to a frame in the parent spec: ``parent_spec.attach(child_spec, frame=frame_name_or_obj)``, + returns the reference to a frame, which is the attached worldbody transformed into a frame. The frame must belong to + the child spec. Prefix and suffix can also be specified as keyword arguments. Attaching does not copy, so all the child reference are still valid in the parent and therefore modifying the child will modify the parent. This is not true for the attach :ref:`attach` an :ref:`replicate` @@ -562,8 +563,8 @@ meta-elements in MJCF, which create deep copies while attaching. # Attach the child to the parent in different ways. body_in_frame = frame.attach_body(child_body, 'child-', '') frame_in_body = body.attach_frame(child_frame, 'child-', '') - body_in_site = site.attach(child_body, 'child-', '') - worldframe_in_frame = frame.attach(child, 'child-', '') + worldframe_in_site = parent.attach(child, site=site, prefix='child-') + worldframe_in_frame = parent.attach(child, frame=frame, prefix='child-') Convenience methods ------------------- diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 560d5ba6..08716d1b 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -495,6 +495,69 @@ PYBIND11_MODULE(_specs, m) { mjSpec.def("detach_body", [](MjSpec& self, raw::MjsBody& body) { mjs_detachBody(self.ptr, &body); }); + mjSpec.def( + "attach", + [](MjSpec& self, MjSpec& child, std::optional& prefix, + std::optional& suffix, std::optional& site, + std::optional& frame) -> raw::MjsFrame* { + if (!frame.has_value() && !site.has_value()) { + throw pybind11::value_error( + "One of frame or site must be specified."); + } + if (frame.has_value() && site.has_value()) { + throw pybind11::value_error( + "Only one of frame or site can be specified."); + } + auto world = mjs_findBody(child.ptr, "world"); + if (!world) { + throw pybind11::value_error("Child does not have a world body."); + } + const char* p = prefix.has_value() ? prefix.value().c_str() : ""; + const char* s = suffix.has_value() ? suffix.value().c_str() : ""; + raw::MjsBody* attached_world = nullptr; + if (frame.has_value()) { + raw::MjsFrame* frame_ptr = nullptr; + try { + frame_ptr = frame->cast(); + } catch (const py::cast_error& e) { + frame_ptr = + mjs_findFrame(self.ptr, frame->cast().c_str()); + } + if (!frame_ptr) { + throw pybind11::value_error("Frame not found."); + } + if (mjs_getSpec(frame_ptr->element) != self.ptr) { + throw pybind11::value_error( + "Frame spec does not match parent spec."); + } + attached_world = mjs_attachBody(frame_ptr, world, p, s); + } + if (site.has_value()) { + raw::MjsSite* site_ptr = nullptr; + try { + site_ptr = site->cast(); + } catch (const py::cast_error& e) { + site_ptr = mjs_asSite(mjs_findElement( + self.ptr, mjOBJ_SITE, site->cast().c_str())); + } + if (!site_ptr) { + throw pybind11::value_error("Site not found."); + } + if (mjs_getSpec(site_ptr->element) != self.ptr) { + throw pybind11::value_error( + "Site spec does not match parent spec."); + } + attached_world = mjs_attachToSite(site_ptr, world, p, s); + } + if (!attached_world) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } + return mjs_bodyToFrame(&attached_world); + }, + py::arg("child"), py::arg("prefix") = py::none(), + py::arg("suffix") = py::none(), py::arg("site") = py::none(), + py::arg("frame") = py::none(), + py::return_value_policy::reference_internal); // ============================= MJSBODY ===================================== mjsBody.def( @@ -777,27 +840,6 @@ PYBIND11_MODULE(_specs, m) { py::arg("body"), py::arg("prefix") = py::none(), py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); - mjsFrame.def( - "attach", - [](raw::MjsFrame& self, MjSpec& spec, std::optional& prefix, - std::optional& suffix) -> raw::MjsFrame* { - auto world = mjs_findBody(spec.ptr, "world"); - if (!world) { - throw pybind11::value_error( - mjs_getError(mjs_getSpec(self.element))); - } - const char* p = prefix.has_value() ? prefix.value().c_str() : ""; - const char* s = suffix.has_value() ? suffix.value().c_str() : ""; - auto attached_world = mjs_attachBody(&self, world, p, s); - if (!attached_world) { - throw pybind11::value_error( - mjs_getError(mjs_getSpec(self.element))); - } - return mjs_bodyToFrame(&attached_world); - }, - py::arg("spec"), py::arg("prefix") = py::none(), - py::arg("suffix") = py::none(), - py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); @@ -878,28 +920,6 @@ PYBIND11_MODULE(_specs, m) { py::arg("body"), py::arg("prefix") = py::none(), py::arg("suffix") = py::none(), py::return_value_policy::reference_internal); - mjsSite.def( - "attach", - [](raw::MjsSite& self, MjSpec& spec, - std::optional& prefix, - std::optional& suffix) -> raw::MjsFrame* { - auto world = mjs_findBody(spec.ptr, "world"); - if (!world) { - throw pybind11::value_error( - mjs_getError(mjs_getSpec(self.element))); - } - const char* p = prefix.has_value() ? prefix.value().c_str() : ""; - const char* s = suffix.has_value() ? suffix.value().c_str() : ""; - auto attached_world = mjs_attachToSite(&self, world, p, s); - if (!attached_world) { - throw pybind11::value_error( - mjs_getError(mjs_getSpec(self.element))); - } - return mjs_bodyToFrame(&attached_world); - }, - py::arg("body"), py::arg("prefix") = py::none(), - py::arg("suffix") = py::none(), - py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== mjsCamera.def("delete", diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 993fc2d4..6465619b 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -939,6 +939,7 @@ class SpecsTest(absltest.TestCase): def test_attach_to_site(self): parent = mujoco.MjSpec() site = parent.worldbody.add_site(pos=[1, 2, 3], quat=[0, 0, 0, 1]) + site.name = 'site' # Attach body to site and compile. child1 = mujoco.MjSpec() @@ -954,7 +955,7 @@ class SpecsTest(absltest.TestCase): # Attach entire spec to site and compile again. child2 = mujoco.MjSpec() body2 = child2.worldbody.add_body(name='body') - self.assertIsNotNone(site.attach(child2, prefix='child-')) + self.assertIsNotNone(parent.attach(child2, site=site, prefix='child2-')) body2.pos = [-1, -1, -1] model2 = parent.compile() self.assertIsNotNone(model2) @@ -964,6 +965,26 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) + # Attach another spec to site (referenced by name) and compile again. + child3 = mujoco.MjSpec() + body3 = child3.worldbody.add_body(name='body') + self.assertIsNotNone(parent.attach(child3, site='site', prefix='child3-')) + body3.pos = [-2, -2, -2] + model3 = parent.compile() + self.assertIsNotNone(model3) + self.assertEqual(model3.nbody, 4) + np.testing.assert_array_equal(model3.body_pos[1], [0, 1, 4]) + np.testing.assert_array_equal(model3.body_pos[2], [2, 3, 2]) + np.testing.assert_array_equal(model3.body_pos[3], [3, 4, 1]) + np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1]) + np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1]) + np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1]) + + # Fail to attach to a site that does not exist. + child4 = mujoco.MjSpec() + with self.assertRaisesRegex(ValueError, 'Site not found.'): + parent.attach(child4, site='invalid_site', prefix='child3-') + def test_body_to_frame(self): spec = mujoco.MjSpec() body = spec.worldbody.add_body(pos=[1, 2, 3]) @@ -974,6 +995,7 @@ class SpecsTest(absltest.TestCase): def test_attach_to_frame(self): parent = mujoco.MjSpec() frame = parent.worldbody.add_frame(pos=[1, 2, 3], quat=[0, 0, 0, 1]) + frame.name = 'frame' # Attach body to frame and compile. child1 = mujoco.MjSpec() @@ -989,7 +1011,7 @@ class SpecsTest(absltest.TestCase): # Attach entire spec to frame and compile again. child2 = mujoco.MjSpec() body2 = child2.worldbody.add_body(name='body') - self.assertIsNotNone(frame.attach(child2, prefix='child-')) + self.assertIsNotNone(parent.attach(child2, frame=frame, prefix='child-')) body2.pos = [-1, -1, -1] model2 = parent.compile() self.assertIsNotNone(model2) @@ -999,6 +1021,25 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) + # Attach another spec to frame (referenced by name) and compile again. + child3 = mujoco.MjSpec() + body3 = child3.worldbody.add_body(name='body') + self.assertIsNotNone(parent.attach(child3, frame='frame', prefix='child3-')) + body3.pos = [-2, -2, -2] + model3 = parent.compile() + self.assertIsNotNone(model3) + self.assertEqual(model3.nbody, 4) + np.testing.assert_array_equal(model3.body_pos[1], [0, 1, 4]) + np.testing.assert_array_equal(model3.body_pos[2], [2, 3, 2]) + np.testing.assert_array_equal(model3.body_pos[3], [3, 4, 1]) + np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1]) + np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1]) + np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1]) + + # Fail to attach to a frame that does not exist. + child4 = mujoco.MjSpec() + with self.assertRaisesRegex(ValueError, 'Frame not found.'): + parent.attach(child4, frame='invalid_frame', prefix='child3-') if __name__ == '__main__': absltest.main() From 1412a29dace0ff21d2f60379629300ae18dff9df Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Jan 2025 08:56:25 -0800 Subject: [PATCH 251/426] Merge child asset dictionary into parent spec during attach(). PiperOrigin-RevId: 717939728 Change-Id: I54f8ac87062cf19f55a16af37b777701545ed865 --- python/mujoco/specs.cc | 8 ++++++++ python/mujoco/specs_test.py | 20 ++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 08716d1b..dfad9d93 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -552,6 +552,14 @@ PYBIND11_MODULE(_specs, m) { if (!attached_world) { throw pybind11::value_error(mjs_getError(self.ptr)); } + for (const auto& asset : child.assets) { + if (self.assets.contains(asset.first)) { + throw pybind11::value_error("Asset " + + asset.first.cast() + + " already exists in parent spec."); + } + self.assets[asset.first] = asset.second; + } return mjs_bodyToFrame(&attached_world); }, py::arg("child"), py::arg("prefix") = py::none(), diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 6465619b..d4aa345e 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -938,11 +938,13 @@ class SpecsTest(absltest.TestCase): def test_attach_to_site(self): parent = mujoco.MjSpec() + parent.assets = {'cube.obj': 'cube_content'} site = parent.worldbody.add_site(pos=[1, 2, 3], quat=[0, 0, 0, 1]) site.name = 'site' # Attach body to site and compile. child1 = mujoco.MjSpec() + child1.assets = {'cube1.obj': 'cube1_content'} body1 = child1.worldbody.add_body() self.assertIs(body1, site.attach_body(body1, prefix='_')) body1.pos = [1, 1, 1] @@ -951,9 +953,11 @@ class SpecsTest(absltest.TestCase): self.assertEqual(model1.nbody, 2) np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4]) np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') # Attach entire spec to site and compile again. child2 = mujoco.MjSpec() + child2.assets = {'cube2.obj': 'cube2_content'} body2 = child2.worldbody.add_body(name='body') self.assertIsNotNone(parent.attach(child2, site=site, prefix='child2-')) body2.pos = [-1, -1, -1] @@ -964,9 +968,12 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2]) np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') + self.assertEqual(parent.assets['cube2.obj'], 'cube2_content') # Attach another spec to site (referenced by name) and compile again. child3 = mujoco.MjSpec() + child3.assets = {'cube3.obj': 'cube3_content'} body3 = child3.worldbody.add_body(name='body') self.assertIsNotNone(parent.attach(child3, site='site', prefix='child3-')) body3.pos = [-2, -2, -2] @@ -979,6 +986,9 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1]) np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') + self.assertEqual(parent.assets['cube2.obj'], 'cube2_content') + self.assertEqual(parent.assets['cube3.obj'], 'cube3_content') # Fail to attach to a site that does not exist. child4 = mujoco.MjSpec() @@ -994,11 +1004,13 @@ class SpecsTest(absltest.TestCase): def test_attach_to_frame(self): parent = mujoco.MjSpec() + parent.assets = {'cube.obj': 'cube_content'} frame = parent.worldbody.add_frame(pos=[1, 2, 3], quat=[0, 0, 0, 1]) frame.name = 'frame' # Attach body to frame and compile. child1 = mujoco.MjSpec() + child1.assets = {'cube1.obj': 'cube1_content'} body1 = child1.worldbody.add_body() self.assertIs(body1, frame.attach_body(body1, prefix='_')) body1.pos = [1, 1, 1] @@ -1007,9 +1019,11 @@ class SpecsTest(absltest.TestCase): self.assertEqual(model1.nbody, 2) np.testing.assert_array_equal(model1.body_pos[1], [0, 1, 4]) np.testing.assert_array_equal(model1.body_quat[1], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') # Attach entire spec to frame and compile again. child2 = mujoco.MjSpec() + child2.assets = {'cube2.obj': 'cube2_content'} body2 = child2.worldbody.add_body(name='body') self.assertIsNotNone(parent.attach(child2, frame=frame, prefix='child-')) body2.pos = [-1, -1, -1] @@ -1020,9 +1034,12 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2]) np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') + self.assertEqual(parent.assets['cube2.obj'], 'cube2_content') # Attach another spec to frame (referenced by name) and compile again. child3 = mujoco.MjSpec() + child3.assets = {'cube3.obj': 'cube3_content'} body3 = child3.worldbody.add_body(name='body') self.assertIsNotNone(parent.attach(child3, frame='frame', prefix='child3-')) body3.pos = [-2, -2, -2] @@ -1035,6 +1052,9 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1]) np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1]) np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1]) + self.assertEqual(parent.assets['cube.obj'], 'cube_content') + self.assertEqual(parent.assets['cube2.obj'], 'cube2_content') + self.assertEqual(parent.assets['cube3.obj'], 'cube3_content') # Fail to attach to a frame that does not exist. child4 = mujoco.MjSpec() From 6a7a7a5f80d38084a5ce1f0a8b1bf05d8fadec60 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Jan 2025 10:38:14 -0800 Subject: [PATCH 252/426] Add the override_assets attribute to MjSpec. When specified, child asset with repeated names will override parent's, rather than throw an error. Default behavior is to override. PiperOrigin-RevId: 717979621 Change-Id: I18022dc266619b4ba1a8636d1c60f85b0eb5f862 --- python/mujoco/specs.cc | 15 ++++++++++++++- python/mujoco/specs_test.py | 5 ++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index dfad9d93..00dbfc89 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -81,9 +81,11 @@ struct MjSpec { // copy constructor and assignment MjSpec(const MjSpec& other) : ptr(mj_copySpec(other.ptr)) { + override_assets = other.override_assets; assets = other.assets; } MjSpec& operator=(const MjSpec& other) { + override_assets = other.override_assets; ptr = mj_copySpec(other.ptr); assets = other.assets; return *this; @@ -91,11 +93,13 @@ struct MjSpec { // move constructor and move assignment MjSpec(MjSpec&& other) : ptr(other.ptr) { + override_assets = other.override_assets; other.ptr = nullptr; assets = other.assets; other.assets.clear(); } MjSpec& operator=(MjSpec&& other) { + override_assets = other.override_assets; ptr = other.ptr; other.ptr = nullptr; assets = other.assets; @@ -108,6 +112,7 @@ struct MjSpec { } raw::MjSpec* ptr; py::dict assets; + bool override_assets = true; }; template @@ -467,6 +472,14 @@ PYBIND11_MODULE(_specs, m) { self.assets[item.first] = item.second; }; }, py::return_value_policy::reference_internal); + mjSpec.def_property( + "override_assets", + [](MjSpec& self) -> bool { + return self.override_assets; + }, + [](MjSpec& self, bool override_assets) { + self.override_assets = override_assets; + }); mjSpec.def("to_xml", [](MjSpec& self) -> std::string { int size = mj_saveXMLString(self.ptr, nullptr, 0, nullptr, 0); std::unique_ptr buf(new char[size + 1]); @@ -553,7 +566,7 @@ PYBIND11_MODULE(_specs, m) { throw pybind11::value_error(mjs_getError(self.ptr)); } for (const auto& asset : child.assets) { - if (self.assets.contains(asset.first)) { + if (self.assets.contains(asset.first) && !self.override_assets) { throw pybind11::value_error("Asset " + asset.first.cast() + " already exists in parent spec."); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index d4aa345e..4b53deed 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1039,7 +1039,7 @@ class SpecsTest(absltest.TestCase): # Attach another spec to frame (referenced by name) and compile again. child3 = mujoco.MjSpec() - child3.assets = {'cube3.obj': 'cube3_content'} + child3.assets = {'cube2.obj': 'new_cube2_content'} body3 = child3.worldbody.add_body(name='body') self.assertIsNotNone(parent.attach(child3, frame='frame', prefix='child3-')) body3.pos = [-2, -2, -2] @@ -1053,8 +1053,7 @@ class SpecsTest(absltest.TestCase): np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1]) np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1]) self.assertEqual(parent.assets['cube.obj'], 'cube_content') - self.assertEqual(parent.assets['cube2.obj'], 'cube2_content') - self.assertEqual(parent.assets['cube3.obj'], 'cube3_content') + self.assertEqual(parent.assets['cube2.obj'], 'new_cube2_content') # Fail to attach to a frame that does not exist. child4 = mujoco.MjSpec() From 67dd4827a637e91e24db230f066572a6fa0a41ad Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 21 Jan 2025 19:58:55 -0800 Subject: [PATCH 253/426] Improve printing of inertia-like matrices, allow mj_printData to take `const mjData*` PiperOrigin-RevId: 718176884 Change-Id: I057dabe6973e19db86e9ff9ce4430c6d17dfce2b --- doc/includes/references.h | 4 +- include/mujoco/mujoco.h | 4 +- introspect/functions.py | 4 +- src/engine/engine_print.c | 80 +++++++++++++++++++++++---------------- src/engine/engine_print.h | 4 +- 5 files changed, 56 insertions(+), 40 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index c1cce18e..74d6778c 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3186,9 +3186,9 @@ int mjs_activatePlugin(mjSpec* s, const char* name); int mjs_setDeepCopy(mjSpec* s, int deepcopy); void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format); void mj_printModel(const mjModel* m, const char* filename); -void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, +void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, const char* float_format); -void mj_printData(const mjModel* m, mjData* d, const char* filename); +void mj_printData(const mjModel* m, const mjData* d, const char* filename); void mju_printMat(const mjtNum* mat, int nr, int nc); void mju_printMatSparse(const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, const int* colind); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 1fefa52f..fdf30383 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -257,11 +257,11 @@ MJAPI void mj_printModel(const mjModel* m, const char* filename); // Print mjData to text file, specifying format. // float_format must be a valid printf-style format string for a single float value -MJAPI void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, +MJAPI void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, const char* float_format); // Print data to text file. -MJAPI void mj_printData(const mjModel* m, mjData* d, const char* filename); +MJAPI void mj_printData(const mjModel* m, const mjData* d, const char* filename); // Print matrix to screen. MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/introspect/functions.py b/introspect/functions.py index cdde54ff..af88f329 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -1123,7 +1123,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='d', type=PointerType( - inner_type=ValueType(name='mjData'), + inner_type=ValueType(name='mjData', is_const=True), ), ), FunctionParameterDecl( @@ -1155,7 +1155,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='d', type=PointerType( - inner_type=ValueType(name='mjData'), + inner_type=ValueType(name='mjData', is_const=True), ), ), FunctionParameterDecl( diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 7dc50712..377234b1 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -113,6 +113,43 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, +// print sparse inertia-like matrix +static void printInertia(const char* str, const mjtNum* mat, const mjModel* m, + FILE* fp, const char* float_format) { + int nv = m->nv; + // if no data, or too many rows to be visually useful, return + if (!mat || !nv || nv > 300) { + return; + } + + // get length of string produced by float_format + char test[100]; + int len = snprintf(test, sizeof(test), float_format, 0.0); + + fprintf(fp, "%s\n", str); + + for (int i=0; i < nv; i++) { + fprintf(fp, " "); + int adr = (i == nv-1) ? m->nM - 1 : m->dof_Madr[i+1] - 1; + for (int k=0; k <= i; k++) { + int j = i; + while (j != k && j >= 0) { + j = m->dof_parentid[j]; + } + if (j == k) { + fprintf(fp, " "); + fprintf(fp, float_format, mat[adr--]); + } else { + for (int d=0; d < len+1; d++) fprintf(fp, " "); + } + } + fprintf(fp, "\n"); + } + fprintf(fp, "\n"); +} + + + // print sparse matrix structure void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, const int* rownnz, const int* rowsuper, const int* colind, FILE* fp) { @@ -859,16 +896,13 @@ void mj_printModel(const mjModel* m, const char* filename) { // print mjModel to text file, specifying format. float_format must be a // valid printf-style format string for a single float value -void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, +void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, const char* float_format) { // stack in use, SHOULD NOT OCCUR if (d->pstack) { mjERROR("attempting to print mjData when stack is in use"); } - mjtNum *M = NULL; - mj_markStack(d); - // check format string if (!validateFloatFormat(float_format)) { mju_warning("WARNING: Received invalid float_format. Using default instead."); @@ -886,15 +920,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // check for nullptr if (!fp) { mju_warning("Could not open file '%s' for writing mjModel", filename); - mj_freeStack(d); return; } - // allocate full inertia if it's small - if (m->nv <= 200) { - M = mjSTACKALLOC(d, m->nv*m->nv, mjtNum); - } - #ifdef MEMORY_SANITIZER // If memory sanitizer is active, d->buffer will be marked as poisoned, even // though it's really initialized to 0. This catches unintentionally @@ -990,7 +1018,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, " solver_niter = %d\n", d->solver_niter[island]); fprintf(fp, " solver_nnz = %d\n", d->solver_nnz[island]); for (int i=0; i < niter_stat; i++) { - mjSolverStat* stat = d->solver + island*mjNSOLVER + i; + const mjSolverStat* stat = d->solver + island*mjNSOLVER + i; fprintf(fp, " %d: improvement = ", i); fprintf(fp, float_format, stat->improvement); fprintf(fp, " gradient = "); @@ -1097,16 +1125,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, d->moment_rowadr, d->moment_colind, fp, float_format); printArray("CRB", m->nbody, 10, d->crb, fp, float_format); - if (M) { - // construct and print full M matrix - mj_fullM(m, M, d->qM); - printArray("QM", m->nv, m->nv, M, fp, float_format); - - // construct and print full LD matrix - mj_fullM(m, M, d->qLD); - printArray("QLD", m->nv, m->nv, M, fp, float_format); - } + printInertia("QM", d->qM, m, fp, float_format); + printInertia("QLD", d->qLD, m, fp, float_format); printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format); // B sparse structure @@ -1204,16 +1225,13 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } fprintf(fp, "\n\n"); - if (M) { - // print qDeriv - mju_sparse2dense(M, d->qDeriv, m->nv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind); - printArray("QDERIV", m->nv, m->nv, M, fp, float_format); + // print qDeriv + printSparse("QDERIV", d->qDeriv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, + fp, float_format); - // print qLU - mju_sparse2dense(M, d->qLU, m->nv, m->nv, d->D_rownnz, d->D_rowadr, - d->D_colind); - printArray("QLU", m->nv, m->nv, M, fp, float_format); - } + // print qLU + printSparse("QLU", d->qLU, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, + fp, float_format); // contact fprintf(fp, "CONTACT\n"); @@ -1401,8 +1419,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (filename) { fclose(fp); } - - mj_freeStack(d); } @@ -1412,6 +1428,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // print mjData to text file -void mj_printData(const mjModel* m, mjData* d, const char* filename) { +void mj_printData(const mjModel* m, const mjData* d, const char* filename) { mj_printFormattedData(m, d, filename, FLOAT_FORMAT); } diff --git a/src/engine/engine_print.h b/src/engine/engine_print.h index 60a0ac17..6c1563c6 100644 --- a/src/engine/engine_print.h +++ b/src/engine/engine_print.h @@ -36,11 +36,11 @@ MJAPI void mj_printModel(const mjModel* m, const char* filename); // print mjData to text file, specifying format // float_format must be a valid printf-style format string for a single float value -MJAPI void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, +MJAPI void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, const char* float_format); // print data to text file -MJAPI void mj_printData(const mjModel* m, mjData* d, const char* filename); +MJAPI void mj_printData(const mjModel* m, const mjData* d, const char* filename); // print sparse matrix structure MJAPI void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, From 0090e1e90818eb170cb483f702ebba0fa884951a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 21 Jan 2025 21:03:08 -0800 Subject: [PATCH 254/426] Rename rollout variable `nroll` to `nbatch`. PiperOrigin-RevId: 718196100 Change-Id: Ic36b1a96ea1af2de351539115d4eee051d195aaf --- doc/python.rst | 6 +- python/least_squares.ipynb | 6 +- python/mujoco/rollout.cc | 67 +++++++------ python/mujoco/rollout.py | 66 ++++++------- python/mujoco/rollout_test.py | 178 +++++++++++++++++----------------- 5 files changed, 163 insertions(+), 160 deletions(-) diff --git a/doc/python.rst b/doc/python.rst index 5a479d98..e7499849 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -730,11 +730,11 @@ states and sensor values. The rollouts are run in parallel with an internally ma state, sensordata = rollout.rollout(model, data, initial_state, control) -``model`` is either a single instance of MjModel or a sequence of compatible MjModel of length ``nroll``. +``model`` is either a single instance of MjModel or a sequence of compatible MjModel of length ``nbatch``. ``data`` is either a single instance of MjData or a sequence of compatible MjData of length ``nthread``. -``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where +``initial_state`` is an ``nbatch x nstate`` array, with ``nbatch`` initial states of size ``nstate``, where ``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the -:ref:`full physics state`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are +:ref:`full physics state`. ``control`` is a ``nbatch x nstep x ncontrol`` array of controls. Controls are by default the ``mjModel.nu`` standard actuators, but any combination of :ref:`user input` arrays can be specified by passing an optional ``control_spec`` bitflag. diff --git a/python/least_squares.ipynb b/python/least_squares.ipynb index 80819e91..3e76d206 100644 --- a/python/least_squares.ipynb +++ b/python/least_squares.ipynb @@ -1967,8 +1967,8 @@ " data.mocap_pos[mocapid] = target\n", "\n", " # Append the mocap targets to the controls\n", - " nroll = ctrl0.shape[0]\n", - " mocap = np.tile(data.mocap_pos[mocapid], (nroll, 1))\n", + " nbatch = ctrl0.shape[0]\n", + " mocap = np.tile(data.mocap_pos[mocapid], (nbatch, 1))\n", " ctrl0 = np.hstack((ctrl0, mocap))\n", " ctrlT = np.hstack((ctrlT, mocap))\n", "\n", @@ -1988,7 +1988,7 @@ " state = np.empty(nstate)\n", " mujoco.mj_getState(model, data, state, spec)\n", "\n", - " # Perform rollouts (sensors.shape == nroll, nstep, nsensordata)\n", + " # Perform rollouts (sensors.shape == nbatch, nstep, nsensordata)\n", " states, sensors = rollout.rollout(model, data, state, control,\n", " control_spec=control_spec)\n", "\n", diff --git a/python/mujoco/rollout.cc b/python/mujoco/rollout.cc index 42519189..e591af34 100644 --- a/python/mujoco/rollout.cc +++ b/python/mujoco/rollout.cc @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include +#include #include #include "errors.h" @@ -46,31 +48,32 @@ Construct a rollout object containing a thread pool for parallel rollouts. )"; const auto rollout_doc = R"( -Roll out open-loop trajectories from initial states, get resulting states and sensor values. +Roll out batch of trajectories from initial states, get resulting states and sensor values. input arguments (required): - model list of MjModel instances of length nroll - data list of associated MjData instances of length nthread + model list of homogenous MjModel instances of length nbatch + data list of compatible MjData instances of length nthread nstep integer, number of steps to be taken for each trajectory control_spec specification of controls, ncontrol = mj_stateSize(m, control_spec) - state0 (nroll x nstate) nroll initial state vectors, - nstate = mj_stateSize(m, mjSTATE_FULLPHYSICS) + state0 (nbatch x nstate) nbatch initial state arrays, where + nstate = mj_stateSize(m, mjSTATE_FULLPHYSICS) input arguments (optional): - warmstart0 (nroll x nv) nroll qacc_warmstart vectors - control (nroll x nstep x ncontrol) nroll trajectories of nstep controls + warmstart0 (nbatch x nv) nbatch qacc_warmstart arrays + control (nbatch x nstep x ncontrol) nbatch trajectories of nstep controls output arguments (optional): - state (nroll x nstep x nstate) nroll nstep states - sensordata (nroll x nstep x nsendordata) nroll trajectories of nstep sensordata vectors - chunk_size integer, determines threadpool chunk size. If unspecified - chunk_size = max(1, nroll / (nthread * 10)) + state (nbatch x nstep x nstate) nbatch nstep states + sensordata (nbatch x nstep x nsendordata) nbatch trajectories of nstep sensordata arrays + chunk_size integer, determines threadpool chunk size. If unspecified, the default is + chunk_size = max(1, nbatch / (nthread * 10)) )"; // C-style rollout function, assumes all arguments are valid // all input fields of d are initialised, contents at call time do not matter // after returning, d will contain the last step of the last rollout -void _unsafe_rollout(std::vector& m, mjData* d, int start_roll, int end_roll, int nstep, unsigned int control_spec, - const mjtNum* state0, const mjtNum* warmstart0, const mjtNum* control, - mjtNum* state, mjtNum* sensordata) { +void _unsafe_rollout(std::vector& m, mjData* d, int start_roll, + int end_roll, int nstep, unsigned int control_spec, + const mjtNum* state0, const mjtNum* warmstart0, + const mjtNum* control, mjtNum* state, mjtNum* sensordata) { // sizes int nstate = mj_stateSize(m[0], mjSTATE_FULLPHYSICS); int ncontrol = mj_stateSize(m[0], control_spec); @@ -174,12 +177,12 @@ void _unsafe_rollout(std::vector& m, mjData* d, int start_roll, // C-style threaded version of _unsafe_rollout void _unsafe_rollout_threaded(std::vector& m, std::vector& d, - int nroll, int nstep, unsigned int control_spec, + int nbatch, int nstep, unsigned int control_spec, const mjtNum* state0, const mjtNum* warmstart0, const mjtNum* control, mjtNum* state, mjtNum* sensordata, ThreadPool* pool, int chunk_size) { - int nfulljobs = nroll / chunk_size; - int chunk_remainder = nroll % chunk_size; + int nfulljobs = nbatch / chunk_size; + int chunk_remainder = nbatch % chunk_size; int njobs = (chunk_remainder > 0) ? nfulljobs + 1 : nfulljobs; // Reset the pool counter @@ -213,7 +216,7 @@ void _unsafe_rollout_threaded(std::vector& m, std::vector> arg, - const char* name, int nroll, int nstep, int dim) { + const char* name, int nbatch, int nstep, int dim) { // if empty return nullptr if (!arg.has_value()) { return nullptr; @@ -223,7 +226,7 @@ mjtNum* get_array_ptr(std::optional> arg, py::buffer_info info = arg->request(); // check size - int expected_size = nroll * nstep * dim; + int expected_size = nbatch * nstep * dim; if (info.size != expected_size) { std::ostringstream msg; msg << name << ".size should be " << expected_size << ", got " << info.size; @@ -247,9 +250,9 @@ class Rollout { std::optional sensordata, std::optional chunk_size) { // get raw pointers - int nroll = state0.shape(0); - std::vector model_ptrs(nroll); - for (int r = 0; r < nroll; r++) { + int nbatch = state0.shape(0); + std::vector model_ptrs(nbatch); + for (int r = 0; r < nbatch; r++) { model_ptrs[r] = m[r].cast()->get(); } @@ -284,13 +287,13 @@ class Rollout { int nstate = mj_stateSize(model_ptrs[0], mjSTATE_FULLPHYSICS); int ncontrol = mj_stateSize(model_ptrs[0], control_spec); - mjtNum* state0_ptr = get_array_ptr(state0, "state0", nroll, 1, nstate); + mjtNum* state0_ptr = get_array_ptr(state0, "state0", nbatch, 1, nstate); mjtNum* warmstart0_ptr = - get_array_ptr(warmstart0, "warmstart0", nroll, 1, model_ptrs[0]->nv); + get_array_ptr(warmstart0, "warmstart0", nbatch, 1, model_ptrs[0]->nv); mjtNum* control_ptr = - get_array_ptr(control, "control", nroll, nstep, ncontrol); - mjtNum* state_ptr = get_array_ptr(state, "state", nroll, nstep, nstate); - mjtNum* sensordata_ptr = get_array_ptr(sensordata, "sensordata", nroll, + get_array_ptr(control, "control", nbatch, nstep, ncontrol); + mjtNum* state_ptr = get_array_ptr(state, "state", nbatch, nstep, nstate); + mjtNum* sensordata_ptr = get_array_ptr(sensordata, "sensordata", nbatch, nstep, model_ptrs[0]->nsensordata); // perform rollouts @@ -299,21 +302,21 @@ class Rollout { py::gil_scoped_release no_gil; // call unsafe rollout function, multi or single threaded - if (this->nthread_ > 0 && nroll > 1) { + if (this->nthread_ > 0 && nbatch > 1) { int chunk_size_final = 1; if (!chunk_size.has_value()) { - chunk_size_final = std::max(1, nroll / (10 * this->nthread_)); + chunk_size_final = std::max(1, nbatch / (10 * this->nthread_)); } else { chunk_size_final = *chunk_size; } InterceptMjErrors(_unsafe_rollout_threaded)( - model_ptrs, data_ptrs, nroll, nstep, control_spec, state0_ptr, + model_ptrs, data_ptrs, nbatch, nstep, control_spec, state0_ptr, warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr, this->pool_.get(), chunk_size_final); } else { InterceptMjErrors(_unsafe_rollout)( - model_ptrs, data_ptrs[0], 0, nroll, nstep, control_spec, state0_ptr, - warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr); + model_ptrs, data_ptrs[0], 0, nbatch, nstep, control_spec, + state0_ptr, warmstart0_ptr, control_ptr, state_ptr, sensordata_ptr); } } } diff --git a/python/mujoco/rollout.py b/python/mujoco/rollout.py index 68a9be23..24ed27e6 100644 --- a/python/mujoco/rollout.py +++ b/python/mujoco/rollout.py @@ -65,34 +65,34 @@ class Rollout: """Rolls out open-loop trajectories from initial states, get subsequent state and sensor values. Python wrapper for rollout.cc, see documentation therein. - Infers nroll and nstep. + Infers nbatch and nstep. Tiles inputs with singleton dimensions. Allocates outputs if none are given. Args: - model: An instance or length nroll sequence of MjModel with the same size signature. + model: An instance or length nbatch sequence of MjModel with the same size signature. data: Associated mjData instance or sequence of instances with length nthread. initial_state: Array of initial states from which to roll out trajectories. - ([nroll or 1] x nstate) + ([nbatch or 1] x nstate) control: Open-loop controls array to apply during the rollouts. - ([nroll or 1] x [nstep or 1] x ncontrol) + ([nbatch or 1] x [nstep or 1] x ncontrol) control_spec: mjtState specification of control vectors. skip_checks: Whether to skip internal shape and type checks. nstep: Number of steps in rollouts (inferred if unspecified). initial_warmstart: Initial qfrc_warmstart array (optional). - ([nroll or 1] x nv) + ([nbatch or 1] x nv) state: State output array (optional). - (nroll x nstep x nstate) + (nbatch x nstep x nstate) sensordata: Sensor data output array (optional). - (nroll x nstep x nsensordata) + (nbatch x nstep x nsensordata) chunk_size: Determines threadpool chunk size. If unspecified, - chunk_size = max(1, nroll / (nthread * 10)) + chunk_size = max(1, nbatch / (nthread * 10)) Returns: state: - State output array, (nroll x nstep x nstate). + State output array, (nbatch x nstep x nstate). sensordata: - Sensor data output array, (nroll x nstep x nsensordata). + Sensor data output array, (nbatch x nstep x nsensordata). Raises: RuntimeError: rollout requested after thread pool shutdown. @@ -103,7 +103,7 @@ class Rollout: raise RuntimeError('rollout requested after thread pool shutdown') # skip_checks shortcut: - # don't infer nroll/nstep + # don't infer nbatch or nstep # don't support singleton expansion # don't allocate output arrays # just call rollout and return @@ -159,8 +159,8 @@ class Rollout: state = _ensure_3d(state) sensordata = _ensure_3d(sensordata) - # infer nroll, check for incompatibilities - nroll = _infer_dimension( + # infer nbatch, check for incompatibilities + nbatch = _infer_dimension( 0, 1, initial_state=initial_state, @@ -169,12 +169,12 @@ class Rollout: state=state, sensordata=sensordata, ) - if isinstance(model, list) and nroll == 1: - nroll = len(model) + if isinstance(model, list) and nbatch == 1: + nbatch = len(model) - if isinstance(model, list) and len(model) > 1 and len(model) != nroll: + if isinstance(model, list) and len(model) > 1 and len(model) != nbatch: raise ValueError( - f'nroll inferred as {nroll} but model is length {len(model)}' + f'nbatch inferred as {nbatch} but model is length {len(model)}' ) elif not isinstance(model, list): model = [model] # Use a length 1 list to simplify code below @@ -212,16 +212,16 @@ class Rollout: _check_trailing_dimension(nsensordata, sensordata=sensordata) # tile input arrays/lists if required (singleton expansion) - model = model * nroll if len(model) == 1 else model - initial_state = _tile_if_required(initial_state, nroll) - initial_warmstart = _tile_if_required(initial_warmstart, nroll) - control = _tile_if_required(control, nroll, nstep) + model = model * nbatch if len(model) == 1 else model + initial_state = _tile_if_required(initial_state, nbatch) + initial_warmstart = _tile_if_required(initial_warmstart, nbatch) + control = _tile_if_required(control, nbatch, nstep) # allocate output if not provided if state is None: - state = np.empty((nroll, nstep, nstate)) + state = np.empty((nbatch, nstep, nstate)) if sensordata is None: - sensordata = np.empty((nroll, nstep, nsensordata)) + sensordata = np.empty((nbatch, nstep, nsensordata)) # call rollout self.rollout_.rollout( @@ -276,35 +276,35 @@ def rollout( """Rolls out open-loop trajectories from initial states, get subsequent states and sensor values. Python wrapper for rollout.cc, see documentation therein. - Infers nroll and nstep. + Infers nbatch and nstep. Tiles inputs with singleton dimensions. Allocates outputs if none are given. Args: - model: An instance or length nroll sequence of MjModel with the same size signature. + model: An instance or length nbatch sequence of MjModel with the same size signature. data: Associated mjData instance or sequence of instances with length nthread. initial_state: Array of initial states from which to roll out trajectories. - ([nroll or 1] x nstate) + ([nbatch or 1] x nstate) control: Open-loop controls array to apply during the rollouts. - ([nroll or 1] x [nstep or 1] x ncontrol) + ([nbatch or 1] x [nstep or 1] x ncontrol) control_spec: mjtState specification of control vectors. skip_checks: Whether to skip internal shape and type checks. nstep: Number of steps in rollouts (inferred if unspecified). initial_warmstart: Initial qfrc_warmstart array (optional). - ([nroll or 1] x nv) + ([nbatch or 1] x nv) state: State output array (optional). - (nroll x nstep x nstate) + (nbatch x nstep x nstate) sensordata: Sensor data output array (optional). - (nroll x nstep x nsensordata) + (nbatch x nstep x nsensordata) chunk_size: Determines threadpool chunk size. If unspecified, - chunk_size = max(1, nroll / (nthread * 10)) + chunk_size = max(1, nbatch / (nthread * 10)) persistent_pool: Determines if a persistent thread pool is created or reused. Returns: state: - State output array, (nroll x nstep x nstate). + State output array, (nbatch x nstep x nstate). sensordata: - Sensor data output array, (nroll x nstep x nsensordata). + Sensor data output array, (nbatch x nstep x nsensordata). Raises: ValueError: bad shapes or sizes. diff --git a/python/mujoco/rollout_test.py b/python/mujoco/rollout_test.py index 5c8c5fbe..f27dc02b 100644 --- a/python/mujoco/rollout_test.py +++ b/python/mujoco/rollout_test.py @@ -185,11 +185,11 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps - initial_state = np.random.randn(nroll, nstate) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + control = np.random.randn(nbatch, nstep, model.nu) state, sensordata = rollout.rollout(model, data, initial_state, control) mujoco.mj_resetData(model, data) @@ -198,108 +198,108 @@ class MuJoCoRolloutTest(parameterized.TestCase): np.testing.assert_array_equal(sensordata, py_sensordata) @parameterized.parameters(ALL_MODELS.keys()) - def test_infer_nroll_initial_state(self, model_name): + def test_infer_nbatch_initial_state(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps - initial_state = np.random.randn(nroll, nstate) + initial_state = np.random.randn(nbatch, nstate) control = np.random.randn(nstep, model.nu) state, sensordata = rollout.rollout(model, data, initial_state, control) mujoco.mj_resetData(model, data) - control = np.tile(control, (nroll, 1, 1)) + control = np.tile(control, (nbatch, 1, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @parameterized.parameters(ALL_MODELS.keys()) - def test_infer_nroll_control(self, model_name): + def test_infer_nbatch_control(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps initial_state = np.random.randn(nstate) - control = np.random.randn(nroll, nstep, model.nu) + control = np.random.randn(nbatch, nstep, model.nu) state, sensordata = rollout.rollout(model, data, initial_state, control) mujoco.mj_resetData(model, data) - initial_state = np.tile(initial_state, (nroll, 1)) + initial_state = np.tile(initial_state, (nbatch, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @parameterized.parameters(ALL_MODELS.keys()) - def test_infer_nroll_warmstart(self, model_name): + def test_infer_nbatch_warmstart(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) - initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nroll, 1)) + initial_warmstart = np.tile(data.qacc_warmstart.copy(), (nbatch, 1)) state, sensordata = rollout.rollout( model, data, initial_state, control, initial_warmstart=initial_warmstart ) mujoco.mj_resetData(model, data) - initial_state = np.tile(initial_state, (nroll, 1)) - control = np.tile(control, (nroll, 1, 1)) + initial_state = np.tile(initial_state, (nbatch, 1)) + control = np.tile(control, (nbatch, 1, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @parameterized.parameters(ALL_MODELS.keys()) - def test_infer_nroll_state(self, model_name): + def test_infer_nbatch_state(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) - state = np.empty((nroll, nstep, nstate)) + state = np.empty((nbatch, nstep, nstate)) state, sensordata = rollout.rollout( model, data, initial_state, control, state=state ) mujoco.mj_resetData(model, data) - initial_state = np.tile(initial_state, (nroll, 1)) - control = np.tile(control, (nroll, 1, 1)) + initial_state = np.tile(initial_state, (nbatch, 1)) + control = np.tile(control, (nbatch, 1, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @parameterized.parameters(ALL_MODELS.keys()) - def test_infer_nroll_sensordata(self, model_name): + def test_infer_nbatch_sensordata(self, model_name): model = mujoco.MjModel.from_xml_string(ALL_MODELS[model_name]) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 5 # number of rollouts + nbatch = 5 # number of rollouts nstep = 1 # number of steps initial_state = np.random.randn(nstate) control = np.random.randn(nstep, model.nu) - sensordata = np.empty((nroll, nstep, model.nsensordata)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) state, sensordata = rollout.rollout( model, data, initial_state, control, sensordata=sensordata ) mujoco.mj_resetData(model, data) - initial_state = np.tile(initial_state, (nroll, 1)) - control = np.tile(control, (nroll, 1, 1)) + initial_state = np.tile(initial_state, (nbatch, 1)) + control = np.tile(control, (nbatch, 1, 1)) py_state, py_sensordata = py_rollout(model, data, initial_state, control) np.testing.assert_array_equal(state, py_state) np.testing.assert_array_equal(sensordata, py_sensordata) @@ -310,13 +310,13 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 1 # number of rollouts + nbatch = 1 # number of rollouts nstep = 3 # number of steps initial_state = np.random.randn(nstate) control = np.random.randn(model.nu) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) rollout.rollout( model, data, initial_state, control, state=state, sensordata=sensordata ) @@ -332,11 +332,11 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 2 # number of initial states + nbatch = 2 # number of initial states nstep = 3 # number of timesteps - initial_state = np.random.randn(nroll, nstate) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + control = np.random.randn(nbatch, nstep, model.nu) state, sensordata = rollout.rollout(model, data, initial_state, control) py_state, py_sensordata = py_rollout(model, data, initial_state, control) @@ -345,26 +345,26 @@ class MuJoCoRolloutTest(parameterized.TestCase): @parameterized.parameters(ALL_MODELS.keys()) def test_multi_model(self, model_name): - nroll = 3 # number of initial states and models + nbatch = 3 # number of initial states and models nstep = 3 # number of timesteps spec = mujoco.MjSpec.from_string(ALL_MODELS[model_name]) if len(spec.bodies) > 1: model = [] - for i in range(nroll): + for i in range(nbatch): body = spec.bodies[1] assert body.name != 'world' body.pos = body.pos + i model.append(spec.compile()) else: - model = [spec.compile() for _ in range(nroll)] + model = [spec.compile() for _ in range(nbatch)] nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model[0]) - initial_state = np.random.randn(nroll, nstate) - control = np.random.randn(nroll, nstep, model[0].nu) + initial_state = np.random.randn(nbatch, nstate) + control = np.random.randn(nbatch, nstep, model[0].nu) state, sensordata = rollout.rollout(model, data, initial_state, control) py_state, py_sensordata = py_rollout(model, data, initial_state, control) @@ -377,12 +377,12 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 2 # number of rollouts + nbatch = 2 # number of rollouts nstep = 3 # number of timesteps - initial_state = np.random.randn(nroll, nstate) - control = np.random.randn(nroll, 1, model.nu) - state = np.empty((nroll, nstep, nstate)) + initial_state = np.random.randn(nbatch, nstate) + control = np.random.randn(nbatch, 1, model.nu) + state = np.empty((nbatch, nstep, nstate)) state, sensordata = rollout.rollout( model, data, initial_state, control, state=state ) @@ -398,10 +398,10 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 4 # number of rollouts + nbatch = 4 # number of rollouts nstep = 3 # number of timesteps - initial_state = np.random.randn(nroll, nstate) + initial_state = np.random.randn(nbatch, nstate) control_spec = ( mujoco.mjtState.mjSTATE_CTRL @@ -409,7 +409,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): | mujoco.mjtState.mjSTATE_XFRC_APPLIED ) ncontrol = mujoco.mj_stateSize(model, control_spec) - control = np.random.randn(nroll, nstep, ncontrol) + control = np.random.randn(nbatch, nstep, ncontrol) state, sensordata = rollout.rollout( model, data, initial_state, control, control_spec=control_spec @@ -426,8 +426,8 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 4 # number of rollouts - initial_state = np.empty((nroll, nstate)) + nbatch = 4 # number of rollouts + initial_state = np.empty((nbatch, nstate)) # get diverging (0, 2) and non-diverging (1, 3) states mujoco.mj_getState( @@ -446,7 +446,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstep = 10000 # divergence after ~15s, timestep = 2e-3 - state = np.random.randn(nroll, nstep, nstate) + state = np.random.randn(nbatch, nstep, nstate) rollout.rollout(model, data, initial_state, state=state) @@ -464,19 +464,19 @@ class MuJoCoRolloutTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(TEST_XML) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) num_workers = 32 - nroll = 100 + nbatch = 100 nstep = 5 - initial_state = np.random.randn(nroll, nstate) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) + control = np.random.randn(nbatch, nstep, model.nu) thread_local = threading.local() def thread_initializer(): thread_local.data = mujoco.MjData(model) - model_list = [copy.copy(model) for _ in range(nroll)] + model_list = [copy.copy(model) for _ in range(nbatch)] def call_rollout(initial_state, control, state, sensordata): rollout.rollout( @@ -490,7 +490,7 @@ class MuJoCoRolloutTest(parameterized.TestCase): sensordata=sensordata, ) - n = nroll // num_workers # integer division + n = nbatch // num_workers # integer division chunks = [] # a list of tuples, one per worker for i in range(num_workers - 1): chunks.append(( @@ -526,14 +526,14 @@ class MuJoCoRolloutTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(TEST_XML) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) num_workers = 32 - nroll = 100 + nbatch = 100 nstep = 5 - initial_state = np.random.randn(nroll, nstate) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) + control = np.random.randn(nbatch, nstep, model.nu) - model_list = [copy.copy(model) for _ in range(nroll)] + model_list = [copy.copy(model) for _ in range(nbatch)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] rollout.rollout( @@ -555,14 +555,14 @@ class MuJoCoRolloutTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(TEST_XML) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) num_workers = 32 - nroll = 100 + nbatch = 100 nstep = 5 - initial_state = np.random.randn(nroll, nstate) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) + control = np.random.randn(nbatch, nstep, model.nu) - model_list = [copy.copy(model) for _ in range(nroll)] + model_list = [copy.copy(model) for _ in range(nbatch)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] with rollout.Rollout(nthread=num_workers) as rollout_: @@ -604,14 +604,14 @@ class MuJoCoRolloutTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(TEST_XML) nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) num_workers = 32 - nroll = 100 + nbatch = 100 nstep = 5 - initial_state = np.random.randn(nroll, nstate) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model.nsensordata)) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model.nsensordata)) + control = np.random.randn(nbatch, nstep, model.nu) - model_list = [copy.copy(model) for _ in range(nroll)] + model_list = [copy.copy(model) for _ in range(nbatch)] data_list = [mujoco.MjData(model) for _ in range(num_workers)] for _ in range(2): @@ -699,11 +699,11 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 1 + nbatch = 1 nstep = 3 - initial_state = np.zeros((nroll, nstate)) - ctrl = np.zeros((nroll, nstep, model.nu)) + initial_state = np.zeros((nbatch, nstate)) + ctrl = np.zeros((nbatch, nstep, model.nu)) model.opt.solver = 10 # invalid solver type with self.assertRaisesWithLiteralMatch( @@ -716,9 +716,9 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 1 + nbatch = 1 - initial_state = np.zeros((nroll, nstate)) + initial_state = np.zeros((nbatch, nstate)) control = 'string' with self.assertRaisesWithLiteralMatch( @@ -737,31 +737,31 @@ class MuJoCoRolloutTest(parameterized.TestCase): nstate = mujoco.mj_stateSize(model, mujoco.mjtState.mjSTATE_FULLPHYSICS) data = mujoco.MjData(model) - nroll = 1 + nbatch = 1 nstep = 3 - initial_state = np.random.randn(nroll, nstate + 1) + initial_state = np.random.randn(nbatch, nstate + 1) with self.assertRaisesWithLiteralMatch( ValueError, 'trailing dimension of initial_state must be 6, got 7' ): rollout.rollout(model, data, initial_state) - initial_state = np.random.randn(nroll, nstate) + initial_state = np.random.randn(nbatch, nstate) control = np.random.randn(1, nstep, model.nu + 1) with self.assertRaisesWithLiteralMatch( ValueError, 'trailing dimension of control must be 2, got 3' ): rollout.rollout(model, data, initial_state, control) - control = np.random.randn(nroll, nstep, model.nu) - state = np.random.randn(nroll, nstep + 1, nstate) # incompatible nstep + control = np.random.randn(nbatch, nstep, model.nu) + state = np.random.randn(nbatch, nstep + 1, nstate) # incompatible nstep with self.assertRaisesWithLiteralMatch( ValueError, 'dimension 1 inferred as 3 but state has 4' ): rollout.rollout(model, data, initial_state, control, state=state) - initial_state = np.random.randn(nroll, nstate) - control = np.random.randn(nroll, nstep, model.nu) + initial_state = np.random.randn(nbatch, nstate) + control = np.random.randn(nbatch, nstep, model.nu) bad_spec = mujoco.mjtState.mjSTATE_ACT with self.assertRaisesWithLiteralMatch( ValueError, 'control_spec can only contain bits in mjSTATE_USER' @@ -946,17 +946,17 @@ def py_rollout( ): initial_state = ensure_2d(initial_state) control = ensure_3d(control) - nroll = initial_state.shape[0] + nbatch = initial_state.shape[0] nstep = control.shape[1] if isinstance(model, mujoco.MjModel): - model = [copy.copy(model) for _ in range(nroll)] + model = [copy.copy(model) for _ in range(nbatch)] nstate = mujoco.mj_stateSize(model[0], mujoco.mjtState.mjSTATE_FULLPHYSICS) - state = np.empty((nroll, nstep, nstate)) - sensordata = np.empty((nroll, nstep, model[0].nsensordata)) - for r in range(nroll): + state = np.empty((nbatch, nstep, nstate)) + sensordata = np.empty((nbatch, nstep, model[0].nsensordata)) + for r in range(nbatch): state_r, sensordata_r = one_rollout( model[r], data, initial_state[r], control[r], control_spec ) From aa505a78726912fe743787dfa3a2adb60248927a Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 22 Jan 2025 06:02:10 -0800 Subject: [PATCH 255/426] Add support for NativeCCD multiple contacts for box-box collision. PiperOrigin-RevId: 718352304 Change-Id: Icaa827e716a2d7aa7c0c644e0ccd2913b476e6fb --- src/engine/engine_collision_convex.c | 44 +- src/engine/engine_collision_gjk.c | 400 ++++++++++++++- src/engine/engine_collision_gjk.h | 3 + test/engine/engine_collision_gjk_test.cc | 600 ++++++++++++++++++++--- 4 files changed, 949 insertions(+), 98 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 712841a8..04f9ccc3 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -779,9 +779,9 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { -// find single convex-convex collision +// find convex-convex collision static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, - mjContact* con, mjtNum margin) { + mjContact* con, int max_contacts, mjtNum margin) { if (mjENABLED(mjENBL_NATIVECCD)) { mjCCDConfig config; mjCCDStatus status; @@ -789,19 +789,22 @@ static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, m // set config config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; - config.max_contacts = 1; + config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { - con->dist = margin + dist; - mju_sub3(con->frame, status.x1, status.x2); - mju_normalize3(con->frame); - con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]); - con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]); - con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]); - mju_zero3(con->frame+3); - return 1; + for (int i = 0; i < status.nx; i++) { + mjContact* c = con++; + c->dist = margin + dist; + mju_sub3(c->frame, status.x1 + 3*i, status.x2 + 3*i); + mju_normalize3(c->frame); + c->pos[0] = 0.5 * (status.x1[0 + 3*i] + status.x2[0 + 3*i]); + c->pos[1] = 0.5 * (status.x1[1 + 3*i] + status.x2[1 + 3*i]); + c->pos[2] = 0.5 * (status.x1[2 + 3*i] + status.x2[2 + 3*i]); + mju_zero3(c->frame+3); + } + return status.nx; } return 0; } @@ -884,12 +887,23 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj2, m, d, g2, margin); + int max_contacts = 1; + if (mjENABLED(mjENBL_MULTICCD)) { + // TODO(kylebayes): Support contact pruning. + max_contacts = 8; + } // find initial contact - int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); + int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin); + + // nativeccd supports multi Box-Box collision directly + if (mjENABLED(mjENBL_NATIVECCD) && m->geom_type[g1] == mjGEOM_BOX + && m->geom_type[g2] == mjGEOM_BOX) { + return ncon; + } // look for additional contacts - if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters @@ -935,7 +949,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2); // search for new contact - int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin); + int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, 1, margin); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { @@ -1583,7 +1597,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, mjc_setCCDObjFlex(&obj2, f2, e2, -1); // find contacts - return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); + return mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin); } diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index ec81a722..543d3f95 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -87,13 +87,23 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob // -------------------------------- inlined 3D vector utils -------------------------------------- -// v1 == v2 +// v1 == v2 up to 1e-15 static inline int equal3(const mjtNum v1[3], const mjtNum v2[3]) { return mju_abs(v1[0] - v2[0]) < mjMINVAL && mju_abs(v1[1] - v2[1]) < mjMINVAL && mju_abs(v1[2] - v2[2]) < mjMINVAL; } +// v1 == v2 +static inline int equalexact3(const mjtNum v1[3], const mjtNum v2[3]) { + return v1[0] == v2[0] && v1[1] == v2[1] && v1[2] == v2[2]; +} + +// res = v1 + v2 +static inline void add3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { + res[0] = v1[0] + v2[0], res[1] = v1[1] + v2[1], res[2] = v1[2] + v2[2]; +} + // res = v1 - v2 static inline void sub3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { res[0] = v1[0] - v2[0], res[1] = v1[1] - v2[1], res[2] = v1[2] - v2[2]; @@ -501,10 +511,19 @@ static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mj -// return true only when a and b are both strictly positive or both strictly negative -static inline int sameSign(mjtNum a, mjtNum b) { +// return 1 if both numbers are positive, -1 if both negative and 0 otherwise +static inline int sameSign2(mjtNum a, mjtNum b) { if (a > 0 && b > 0) return 1; - if (a < 0 && b < 0) return 1; + if (a < 0 && b < 0) return -1; + return 0; +} + + + +// return 1 if all three numbers are positive, -1 if all negative and 0 otherwise +static inline int sameSign3(mjtNum a, mjtNum b, mjtNum c) { + if (a > 0 && b > 0 && c > 0) return 1; + if (a < 0 && b < 0 && c < 0) return -1; return 0; } @@ -553,10 +572,10 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const // with vertices {s1, s2, s3, 0} - si mjtNum m_det = C41 + C42 + C43 + C44; - int comp1 = sameSign(m_det, C41), - comp2 = sameSign(m_det, C42), - comp3 = sameSign(m_det, C43), - comp4 = sameSign(m_det, C44); + int comp1 = sameSign2(m_det, C41), + comp2 = sameSign2(m_det, C42), + comp3 = sameSign2(m_det, C43), + comp4 = sameSign2(m_det, C44); // if all signs are the same then the origin is inside the simplex if (comp1 && comp2 && comp3 && comp4) { @@ -706,9 +725,9 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum C33 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; - int comp1 = sameSign(M_max, C31), - comp2 = sameSign(M_max, C32), - comp3 = sameSign(M_max, C33); + int comp1 = sameSign2(M_max, C31), + comp2 = sameSign2(M_max, C32), + comp3 = sameSign2(M_max, C33); // all the same sign, p_o is inside the 2-simplex if (comp1 && comp2 && comp3) { @@ -780,7 +799,7 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { mjtNum C2 = s1[index] - p_o[index]; // inside the simplex - if (sameSign(mu_max, C1) && sameSign(mu_max, C2)) { + if (sameSign2(mu_max, C1) && sameSign2(mu_max, C2)) { lambda[0] = C1 / mu_max; lambda[1] = C2 / mu_max; } else { @@ -1434,6 +1453,358 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob } +// ------------------------------------- MultiCCD ------------------------------------------------- + +// find the normal of a plane perpendicular to the face (given by its normal n) and intersecting the +// face edge (v1, v2) +static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], + const mjtNum n[3]) { + mjtNum v3[3], diff1[3], diff2[3]; + add3(v3, v1, n); + sub3(diff1, v2, v1); + sub3(diff2, v3, v1); + cross3(res, diff1, diff2); + return dot3(res, v1); +} + + + +// find what side of a plane a point p lies +static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) { + mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]}; + return dot3(diff, n) > 0; +} + + + +// compute the intersection of a plane with a line segment (a, b) +static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, + const mjtNum a[3], const mjtNum b[3]) { + mjtNum ab[3]; + sub3(ab, b, a); + mjtNum temp = dot3(pn, ab); + if (temp == 0.0) return mjMAXVAL; // parallel; no intersection + mjtNum t = (pd - dot3(pn, a)) / temp; + if (t >= 0.0 && t <= 1.0) { + res[0] = a[0] + t*ab[0]; + res[1] = a[1] + t*ab[1]; + res[2] = a[2] + t*ab[2]; + } + return t; +} + + + +// clip a polygon against another polygon +static void polygonClip(mjCCDStatus* status, const mjtNum face1[3 * mjMAX_SIDES], int nface1, + const mjtNum face2[3 * mjMAX_SIDES], int nface2, const mjtNum n[3], + const mjtNum dir[3]) { + // compute plane normal and distance to plane for each vertex + mjtNum pn[3 * mjMAX_SIDES], pd[mjMAX_SIDES]; + for (int i = 0; i < nface1 - 1; i++) { + pd[i] = planeNormal(&pn[3*i], &face1[3*i], &face1[3*i + 3], n); + } + pd[nface1 - 1] = planeNormal(&pn[3*(nface1 - 1)], &face1[3*(nface1 - 1)], &face1[0], n); + + // reserve 2 * max_sides as max sides for a clipped polygon + mjtNum polygon1[6 * mjMAX_SIDES], polygon2[6 * mjMAX_SIDES], *polygon, *clipped; + int npolygon = nface2, nclipped = 0; + polygon = polygon1; + clipped = polygon2; + + for (int i = 0; i < nface2; i++) { + copy3(polygon + 3*i, face2 + 3*i); + } + + // clip the polygon by one edge e at a time + for (int e = 0; e < (3 * nface1); e += 3) { + for (int i = 0; i < npolygon; i++) { + // get edge PQ of the polygon + mjtNum *P = polygon + 3*i; + mjtNum *Q = (i < npolygon - 1) ? polygon + 3*(i+1) : polygon; + + // determine if P and Q are in the halfspace of the clipping edge + int inside1 = halfspace(face1 + e, pn + e, P); + int inside2 = halfspace(face1 + e, pn + e, Q); + + // PQ entirely outside the clipping edge, skip + if (!inside1 && !inside2) { + continue; + } + + // edge PQ is inside the clipping edge, add Q + if (inside1 && inside2) { + copy3(clipped + 3*nclipped++, Q); + continue; + } + + // add new vertex to clipped polygon where PQ intersects the clipping edge + mjtNum t = planeIntersect(clipped + 3*nclipped++, pn + e, pd[e/3], P, Q); + if (t < 0.0 || t > 1.0) { + nclipped--; // no intersection in PQ + } + + // add Q as PQ is now back inside the clipping edge + if (inside2) { + copy3(clipped + 3*nclipped++, Q); + } + } + + // swap clipped and polygon + mjtNum* tmp = polygon; + polygon = clipped; + clipped = tmp; + npolygon = nclipped; + nclipped = 0; + } + + // copy final clipped polygon to status + if (npolygon > 0) { + status->nx = npolygon; + for (int i = 0; i < 3*npolygon; i += 3) { + copy3(status->x2 + i, polygon + i); + sub3(status->x1 + i, status->x2 + i, dir); + } + } +} + + + +// compute local coordinates of a global point (g1, g2, g3) +static inline void localcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3], + mjtNum g1, mjtNum g2, mjtNum g3) { + // perform matT * ((g1, g2, g3) - pos) + if (pos) { + g1 -= pos[0]; + g2 -= pos[1]; + g3 -= pos[2]; + } + res[0] = mat[0]*g1 + mat[3]*g2 + mat[6]*g3; + res[1] = mat[1]*g1 + mat[4]*g2 + mat[7]*g3; + res[2] = mat[2]*g1 + mat[5]*g2 + mat[8]*g3; +} + + + +// compute global coordinates of a local point (l1, l2, l3) +static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3], + mjtNum l1, mjtNum l2, mjtNum l3) { + // perform mat * (l1, l2, l3) + pos + res[0] = mat[0]*l1 + mat[1]*l2 + mat[2]*l3; + res[1] = mat[3]*l1 + mat[4]*l2 + mat[5]*l3; + res[2] = mat[6]*l1 + mat[7]*l2 + mat[8]*l3; + if (pos) { + res[0] += pos[0]; + res[1] += pos[1]; + res[2] += pos[2]; + } +} + + + +// compute possible face normals of a box given up to 3 vertices +static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, + const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + // box data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + + // rotate global coordinates to geom local frame + mjtNum v1_local[3], v2_local[3], v3_local[3]; + if (dim > 0) localcoord(v1_local, mat, pos, v1[0], v1[1], v1[2]); + if (dim > 1) localcoord(v2_local, mat, pos, v2[0], v2[1], v2[2]); + if (dim > 2) localcoord(v3_local, mat, pos, v3[0], v3[1], v3[2]); + + if (dim == 3) { + int x = sameSign3(v1_local[0], v2_local[0], v3_local[0]); + int y = sameSign3(v1_local[1], v2_local[1], v3_local[1]); + int z = sameSign3(v1_local[2], v2_local[2], v3_local[2]); + globalcoord(res, mat, NULL, x, y, z); + int sgn = x + y + z; + if (x) resind[0] = 0; + if (y) resind[0] = 2; + if (z) resind[0] = 4; + if (sgn == -1) resind[0]++; + return 1; + } + + if (dim == 2) { + int x = sameSign2(v1_local[0], v2_local[0]); + int y = sameSign2(v1_local[1], v2_local[1]); + int z = sameSign2(v1_local[2], v2_local[2]); + if (x) { + globalcoord(res, mat, NULL, x, 0, 0); + resind[0] = (x > 0) ? 0 : 1; + } + if (y) { + int i = (x ? 1 : 0); + globalcoord(res + 3*i, mat, NULL, 0, y, 0); + resind[i] = (y > 0) ? 2 : 3; + } + if (z) { + globalcoord(res + 3, mat, NULL, 0, 0, z); + resind[1] = (z > 0) ? 4 : 5; + } + return 2; + } + + if (dim == 1) { + mjtNum x = (v1_local[0] > 0) ? 1 : -1; + mjtNum y = (v1_local[1] > 0) ? 1 : -1; + mjtNum z = (v1_local[2] > 0) ? 1 : -1; + globalcoord(res + 0, mat, NULL, x, 0, 0); + globalcoord(res + 3, mat, NULL, 0, y, 0); + globalcoord(res + 6, mat, NULL, 0, 0, z); + resind[0] = (x > 0) ? 0 : 1; + resind[1] = (y > 0) ? 2 : 3; + resind[2] = (z > 0) ? 4 : 5; + return 3; + } + return 0; +} + + + +// recover face of a box from its index +static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { + // box data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + const mjtNum* size = obj->model->geom_size + g; + + // compute global coordinates of the box face and face normal + switch (idx) { + case 0: // right + globalcoord(res + 0, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, size[0], -size[1], size[2]); + return 4; + case 1: // left + globalcoord(res + 0, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, -size[0], size[1], size[2]); + globalcoord(res + 6, mat, pos, -size[0], -size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], -size[2]); + return 4; + case 2: // top + globalcoord(res + 0, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], size[1], size[2]); + return 4; + case 3: // bottom + globalcoord(res + 0, mat, pos, -size[0], -size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], -size[1], size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], -size[2]); + return 4; + case 4: // front + globalcoord(res + 0, mat, pos, -size[0], size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], size[2]); + return 4; + case 5: // back + globalcoord(res + 0, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, -size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, size[0], -size[1], -size[2]); + return 4; + } + return 0; +} + + + +static inline int compareNorms(int res[2], const mjtNum* v, int nv, + const mjtNum* w, int nw) { + for (int i = 0; i < nv; i++) { + for (int j = 0; j < nw; j++) { + if (dot3(v + 3*i, w + 3*j) < -0.99999872) { + res[0] = i; + res[1] = j; + return 1; + } + } + } + return 0; +} + + + +// return number of dimensions of a feature (1, 2 or 3) +static inline int simplexDim(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + int i = 1; + int same1 = equalexact3(v1, v2); + int same2 = equalexact3(v1, v3); + int same3 = equalexact3(v2, v3); + if (!same1) i++; + if (!same3 && !same2) i++; + return i; +} + + + +// recover multiple contacts from EPA polytope +static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, + mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum face1[mjMAX_SIDES * 3], face2[mjMAX_SIDES * 3]; + + // get vertices of faces from EPA + const mjtNum* v11 = pt->verts1 + face->verts[0]; + const mjtNum* v12 = pt->verts1 + face->verts[1]; + const mjtNum* v13 = pt->verts1 + face->verts[2]; + const mjtNum* v21 = pt->verts2 + face->verts[0]; + const mjtNum* v22 = pt->verts2 + face->verts[1]; + const mjtNum* v23 = pt->verts2 + face->verts[2]; + + // get dimensions of features of geoms 1 and 2 + int nface1 = simplexDim(v11, v12, v13); + int nface2 = simplexDim(v21, v22, v23); + int nnorms1 = 0, nnorms2 = 0; + mjtNum n1[9], n2[9]; // normals of possible face collisions + int idx1[3], idx2[3]; // indices of faces, so they can be recovered later + + // get all possible face normals for each geom + if (obj1->geom_type == mjGEOM_BOX) { + nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13); + } + if (obj2->geom_type == mjGEOM_BOX) { + nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23); + } + + // determine if any two normals match + int res[2]; + if (!compareNorms(res, n1, nnorms1, n2, nnorms2)) { + return; + } + int i = res[0], j = res[1]; + + // recover matching faces + if (obj1->geom_type == mjGEOM_BOX) { + nface1 = boxFace(face1, obj1, idx1[i]); + } + if (obj2->geom_type == mjGEOM_BOX) { + nface2 = boxFace(face2, obj2, idx2[j]); + } + + if (nface1 >= 3 && nface2 >= 3) { + // TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the + // single contact direction's magnitude. This is effective, but polygonClip should compute + // this for each contact point. + mjtNum diff[3], approx_dir[3]; + sub3(diff, status->x2, status->x1); + scl3(approx_dir, n2 + 3*j, mju_sqrt(dot3(diff, diff))); + + // clip the faces and store the results in status + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); + } +} + + + // inflate a contact by margin static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) { @@ -1574,7 +1945,10 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m // simplex not on boundary (objects are penetrating) if (!ret) { - epa(status, &pt, obj1, obj2); + Face* face = epa(status, &pt, obj1, obj2); + if (config->max_contacts > 1 && face) { + multicontact(&pt, face, status, obj1, obj2); + } } mj_freeStack(d); } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 5c0601c6..78e1e135 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -25,6 +25,9 @@ extern "C" { #endif +// max sides of a face of mesh supported for multiple contacts +#define mjMAX_SIDES 10 + // Status of an EPA run typedef enum { mjEPA_NOCONTACT = -1, diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index bb8daf65..234fdce2 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_collision_gjk.h" #include +#include #include #include @@ -28,11 +29,16 @@ #include #include +// uncomment to run tests with libccd +// #define TEST_WITH_LIBCCD + namespace mujoco { namespace { using ::testing::NotNull; using ::testing::ElementsAre; +using ::testing::Pointwise; +using ::testing::DoubleNear; constexpr mjtNum kTolerance = 1e-6; constexpr int kMaxIterations = 1000; @@ -78,64 +84,70 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], return dist; } -// drop in replacement for ccdMPRPenetration taken from mjc_penetration -int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, - ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { +int Penetration(mjtNum& depth, std::vector& dir, + std::vector& pos, mjModel* model, mjData* data, + int g1, int g2, mjtNum margin = 0, int max_contacts = 1) { + mjCCDObj obj1, obj2; + mjc_initCCDObj(&obj1, model, data, g1, margin); + mjc_initCCDObj(&obj2, model, data, g2, margin); + +#if defined(TEST_WITH_LIBCCD) + if (max_contacts == 1) { + ccd_t ccd; + CCD_INIT(&ccd); + ccd.mpr_tolerance = kTolerance; + ccd.epa_tolerance = kTolerance; + ccd.max_iterations = kMaxIterations; + ccd.center1 = mjccd_center; + ccd.center2 = mjccd_center; + ccd.support1 = mjccd_support; + ccd.support2 = mjccd_support; + + ccd_real_t ccd_depth; + ccd_vec3_t ccd_dir, ccd_pos; + + int ret = ccdMPRPenetration(&obj1, &obj2, &ccd, &ccd_depth, &ccd_dir, + &ccd_pos); + if (ret) return 0; + dir.resize(3); + pos.resize(3); + depth = -ccd_depth; + mju_copy3(dir.data(), ccd_dir.v); + mju_copy3(pos.data(), ccd_pos.v); + return 1; + } +#endif + mjCCDConfig config; mjCCDStatus status; // set config - config.max_iterations = ccd->max_iterations, - config.tolerance = ccd->mpr_tolerance, - config.max_contacts = 1; + config.max_iterations = kMaxIterations; + config.tolerance = kTolerance; + config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed + config.max_contacts = max_contacts; - mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); if (dist < 0) { - if (depth) *depth = -dist; - if (dir) { - mju_sub3(dir->v, status.x1, status.x2); - mju_normalize3(dir->v); + dir.resize(3 * status.nx); + pos.resize(3 * status.nx); + for (int i = 0; i < status.nx; ++i) { + // compute direction + mju_sub3(&dir[3 * i], status.x1 + 3 * i, status.x2 + 3 * i); + mju_normalize3(&dir[3 * i]); + + // compute position + pos[3 * i + 0] = 0.5 * (status.x1[0 + 3 * i] + status.x2[0 + 3 * i]); + pos[3 * i + 1] = 0.5 * (status.x1[1 + 3 * i] + status.x2[1 + 3 * i]); + pos[3 * i + 2] = 0.5 * (status.x1[2 + 3 * i] + status.x2[2 + 3 * i]); } - if (pos) { - pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]); - pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]); - pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]); - } - return 0; + depth = dist; + return status.nx; } - if (depth) *depth = 0; - if (dir) mju_zero3(dir->v); - if (pos) mju_zero3(dir->v); - return 1; -} -mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2, - mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr, - mjtNum margin = 0) { - mjCCDObj obj1, obj2; - mjc_initCCDObj(&obj1, m, d, g1, margin); - mjc_initCCDObj(&obj2, m, d, g2, margin); - - ccd_t ccd; - // CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration - ccd.mpr_tolerance = kTolerance; - ccd.epa_tolerance = kTolerance; - ccd.max_iterations = kMaxIterations; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - - ccd_real_t depth; - ccd_vec3_t ccd_dir, ccd_pos; - - int ret = PenetrationWrapper(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); - // objects not colliding, return max value as geom distance was never computed - if (ret) return mjMAXVAL; - if (dir) mju_copy3(dir, ccd_dir.v); - if (pos) mju_copy3(pos, ccd_pos.v); - return -depth; + // no contacts + return 0; } using MjGjkTest = MujocoTest; @@ -211,10 +223,11 @@ TEST_F(MjGjkTest, SphereSphereNoDist) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - EXPECT_EQ(dist, mjMAXVAL); + EXPECT_EQ(ncons, 0); mj_deleteData(data); mj_deleteModel(model); } @@ -237,8 +250,11 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + + EXPECT_EQ(ncons, 1); // penetration depth EXPECT_NEAR(dist, -2, kTolerance); @@ -275,13 +291,18 @@ TEST_F(MjGjkTest, BoxBoxDepth) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -1, kTolerance); EXPECT_NEAR(dir[0], 1, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); EXPECT_NEAR(dir[2], 0, kTolerance); + mj_deleteData(data); mj_deleteModel(model); } @@ -321,10 +342,11 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - if (dist < 0) { + if (ncons == 1) { EXPECT_NEAR(dist, -0.033401579411886845, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); @@ -387,9 +409,11 @@ TEST_F(MjGjkTest, BoxBoxDepth3) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.003066, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); @@ -417,15 +441,442 @@ TEST_F(MjGjkTest, BoxBoxTouching) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - EXPECT_EQ(dist, mjMAXVAL); + EXPECT_EQ(ncons, 0); mj_deleteData(data); mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxMultiCCD) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + EXPECT_NEAR(dist, -.1, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + + EXPECT_THAT(pos, Pointwise(DoubleNear(kTolerance), {-1.0, 1.0, 0.95, + 1.0, 1.0, 0.95, + 1.0, -1.0, 0.95, + -1.0, -1.0, 0.95})); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD2) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + EXPECT_NEAR(dist, -.1, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + + EXPECT_THAT(pos, Pointwise(DoubleNear(kTolerance), { 8.5, 10.0, 0.95, + 10.0, 10.0, 0.95, + 10.0, 8.5, 0.95, + 8.5, 8.5, 0.95})); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD3) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = 0.999999806540386004805043285160; + xmat[1] = -0.000014738590672566122784237219; + xmat[2] = 0.000621853651764864637230267874; + xmat[3] = -0.000621853434269146370175218586; + xmat[4] = 0.000014756878555191479777952690; + xmat[5] = 0.999999806540251667819063641218; + xmat[6] = -0.000014747764440060310685981504; + xmat[7] = -0.999999999782504311873765345808; + xmat[8] = 0.000014747710457105431443303178; + + xpos[0] = -0.941218618591869393696924817050; + xpos[1] = 2.209729011624415928594089564285; + xpos[2] = 1.095456702630382306296041861060; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD4) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.500063246694118501700643264485; + xmat[1] = -0.865988885078582182330819705385; + xmat[2] = -0.000015036290463686326402846169; + xmat[3] = 0.865988885208801795201338791230; + xmat[4] = 0.500063246603650646271432833601; + xmat[5] = 0.000009541064416582982810641038; + xmat[6] = -0.000000743359510433135621196039; + xmat[7] = -0.000017792396065397684211655677; + xmat[8] = 0.999999999841438502734547455475; + + xpos[0] = -0.015346718925143524800414063236; + xpos[1] = -0.023500448793229846561336771060; + xpos[2] = -4.859382717259980388746498647379; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.999999999448633714038692232862; + xmat[1] = -0.000033207420761195452995305499; + xmat[2] = -0.000000044925527333868828730462; + xmat[3] = 0.000033207420790006526530903364; + xmat[4] = 0.999999999448428988912951353996; + xmat[5] = 0.000000641458652741046316968134; + xmat[6] = 0.000000044904226121706672357864; + xmat[7] = -0.000000641460144248257277838641; + xmat[8] = 0.999999999999794386695839421009; + + xpos[0] = -0.015347749710384111718197708285; + xpos[1] = -0.023500601273213628239489025873; + xpos[2] = -4.958782854594746325460619118530; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + EXPECT_NEAR(dist, -0.00060425119242707459, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD5) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.965955045562010394810670277366; + xmat[1] = -0.258709898141739669252814337597; + xmat[2] = -0.000196358811267032467391333017; + xmat[3] = 0.258709919231419560592399875532; + xmat[4] = 0.965955055634174608591990818240; + xmat[5] = 0.000090476785846218643442895324; + xmat[6] = 0.000166266546411239724218358860; + xmat[7] = -0.000138196479997660070767120932; + xmat[8] = 0.999999976628582865068040064216; + + xpos[0] = -0.015381524498156991936914650410; + xpos[1] = -0.023527931890396581310342938309; + xpos[2] = -4.559214004409498421921398403356; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.866076536677693908927722077351; + xmat[1] = -0.499911388413602053581996642606; + xmat[2] = -0.000190658753729162216972170540; + xmat[3] = 0.499911409912061843741071243130; + xmat[4] = 0.866076540935322825021103199106; + xmat[5] = 0.000086494189368211055798235654; + xmat[6] = 0.000121885643632020743577087929; + xmat[7] = -0.000170223074359586521841353202; + xmat[8] = 0.999999978083999430111816764111; + + xpos[0] = -0.015358668590921718474784363195; + xpos[1] = -0.023542070504611382203430380855; + xpos[2] = -4.659108354876987156956147373421; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + EXPECT_NEAR(dist, -0.0001077858631973211, kTolerance); + + EXPECT_NEAR(dir[0], 0.00019065, kTolerance); + EXPECT_NEAR(dir[1], -8.6494189274575805e-05, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD6) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = -0.412617528992808124677083014831; + xmat[1] = -0.910903939143411389700588642881; + xmat[2] = -0.000887930675351447824816819576; + xmat[3] = 0.910904370383107120368038067681; + xmat[4] = -0.412617275794986082537718630192; + xmat[5] = -0.000460143975736545586020798115; + xmat[6] = 0.000052771423713213129642884969; + xmat[7] = -0.000998683403024198425301793947; + xmat[8] = 0.999999499923193035932911243435; + + xpos[0] = 0.413029898172642018217004533653; + xpos[1] = 0.190777715293135141649827346555; + xpos[2] = 0.100006658017411736993906856696; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 5); + EXPECT_NEAR(dist, -0.00009843, kTolerance); + + EXPECT_NEAR(dir[0], -0.0008879306751646528, kTolerance); + EXPECT_NEAR(dir[1], -0.00046014397575771832, kTolerance); + EXPECT_NEAR(dir[2], 1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD7) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.482851932827058627495375731087; + xmat[1] = -0.875697459006381406787511423317; + xmat[2] = 0.002823341095436950488190008812; + xmat[3] = 0.875701084072774249555948244961; + xmat[4] = 0.482853601927766051815638093103; + xmat[5] = -0.000102269990141710693382082198; + xmat[6] = -0.001273702846902712276094815635; + xmat[7] = 0.002521784120391480209927292933; + xmat[8] = 0.999996009134990648803409385437; + + xpos[0] = -0.002020740254618143012105280221; + xpos[1] = -0.022654384848980465422263463893; + xpos[2] = -4.858542902144324493463045655517; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.999985133805306514176436394337; + xmat[1] = -0.005293845271528460454113496070; + xmat[2] = 0.001306663930443651821383665990; + xmat[3] = 0.005293871114312041943616993223; + xmat[4] = 0.999985987232967277194006783247; + xmat[5] = -0.000016319793417504115210251922; + xmat[6] = -0.001306559226005186893221354794; + xmat[7] = 0.000023236861241766870316309210; + xmat[8] = 0.999999146181155484924829579541; + + xpos[0] = -0.011066235018223425159988870803; + xpos[1] = -0.023114696036485724711662115283; + xpos[2] = -4.958375812037025376355359185254; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD8) { + static constexpr char xml[] = R"( + + + + + +)"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 1.000000000000000000000000000000; + xmat[1] = 0.000000000000000000000000000000; + xmat[2] = 0.000000000000000000000000000000; + xmat[3] = 0.000000000000000000000000000000; + xmat[4] = 1.000000000000000000000000000000; + xmat[5] = 0.000000000000000000000000000000; + xmat[6] = 0.000000000000000000000000000000; + xmat[7] = 0.000000000000000000000000000000; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346500000000000765720820084; + xpos[1] = -0.023505499999999998617106200527; + xpos[2] = -4.859662640000005140450412000064; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 1.000000000000000000000000000000; + xmat[1] = 0.000000000000000000000000000000; + xmat[2] = 0.000000000000000000000000000000; + xmat[3] = 0.000000000000000000000000000000; + xmat[4] = 1.000000000000000000000000000000; + xmat[5] = -0.000000000000000015361939765351; + xmat[6] = 0.000000000000000000000000000000; + xmat[7] = 0.000000000000000015361939765351; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346500000000000765720820084; + xpos[1] = -0.023505499999999998617106200527; + xpos[2] = -4.958574289672835533338002278470; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"( @@ -465,9 +916,11 @@ TEST_F(MjGjkTest, SmallBoxMesh) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, 0, kTolerance); // direction @@ -495,9 +948,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.00022548856248122027, kTolerance); mj_deleteData(data); mj_deleteModel(model); @@ -576,9 +1031,11 @@ TEST_F(MjGjkTest, LongBox) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dir[3], pos[3]; - mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.01, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); @@ -611,8 +1068,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dist = Penetration(model, data, geom1, geom2, nullptr, nullptr, 15); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 15); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -14.245732934582151, kTolerance); mj_deleteData(data); mj_deleteModel(model); From 21d97902eef4af2a4bbf89f44f2373bfd2ca6cab Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 22 Jan 2025 11:09:06 -0800 Subject: [PATCH 256/426] Check keyframe size before appending it during attach. Fixes #2365. PiperOrigin-RevId: 718455748 Change-Id: I20c56b7c6fbd868962d19682e72e6294edb6893e --- src/user/user_model.cc | 25 ++++++++++++++++++++++++- test/user/user_api_test.cc | 26 ++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2a926855..a727c4d2 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3473,7 +3473,30 @@ void mjCModel::StoreKeyframes(mjCModel* dest) { info.mpos = !key->spec_mpos_.empty(); info.mquat = !key->spec_mquat_.empty(); dest->key_pending_.push_back(info); - ResizeKeyframe(key, qpos0.data(), body_pos0.data(), body_quat0.data()); + if (!key->spec_qpos_.empty() && key->spec_qpos_.size() != nq) { + throw mjCError(nullptr, "Keyframe '%s' has invalid qpos size, got %d, should be %d", + key->name.c_str(), key->spec_qpos_.size(), nq); + } + if (!key->spec_qvel_.empty() && key->spec_qvel_.size() != nv) { + throw mjCError(nullptr, "Keyframe %s has invalid qvel size, got %d, should be %d", + key->name.c_str(), key->spec_qvel_.size(), nv); + } + if (!key->spec_act_.empty() && key->spec_act_.size() != na) { + throw mjCError(nullptr, "Keyframe %s has invalid act size, got %d, should be %d", + key->name.c_str(), key->spec_act_.size(), na); + } + if (!key->spec_ctrl_.empty() && key->spec_ctrl_.size() != nu) { + throw mjCError(nullptr, "Keyframe %s has invalid ctrl size, got %d, should be %d", + key->name.c_str(), key->spec_ctrl_.size(), nu); + } + if (!key->spec_mpos_.empty() && key->spec_mpos_.size() != 3*nmocap) { + throw mjCError(nullptr, "Keyframe %s has invalid mpos size, got %d, should be %d", + key->name.c_str(), key->spec_mpos_.size(), 3*nmocap); + } + if (!key->spec_mquat_.empty() && key->spec_mquat_.size() != 4*nmocap) { + throw mjCError(nullptr, "Keyframe %s has invalid mquat size, got %d, should be %d", + key->name.c_str(), key->spec_mquat_.size(), 4*nmocap); + } SaveState(info.name, key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data(), key->spec_ctrl_.data(), key->spec_mpos_.data(), key->spec_mquat_.data()); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 31b7af43..39850f14 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -2056,6 +2056,32 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { mj_deleteModel(expected); } +TEST_F(MujocoTest, KeyframeSizeError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + + std::array er; + mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); + EXPECT_THAT(spec, IsNull()); + EXPECT_THAT(er.data(), HasSubstr( + "Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1")); +} + TEST_F(MujocoTest, DifferentUnitsAllowed) { static constexpr char gchild_xml[] = R"( From f9fd2e4ba4f5a2fdbdcbb486aeba61ef9445230a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 22 Jan 2025 18:24:59 -0800 Subject: [PATCH 257/426] Add a test showing how to implement scipy's 'soft_l1' loss in `minimize.least_squares` PiperOrigin-RevId: 718649580 Change-Id: I5cec0e2850c805301407c9eb543ab3ba69d42e79 --- python/mujoco/minimize_test.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/python/mujoco/minimize_test.py b/python/mujoco/minimize_test.py index 5cbf1cfd..e642a8e1 100644 --- a/python/mujoco/minimize_test.py +++ b/python/mujoco/minimize_test.py @@ -292,6 +292,34 @@ class MinimizeTest(absltest.TestCase): check_derivatives=True, ) + def test_soft_l1_norm(self) -> None: + def residual(x): + return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)]) + + class SoftL1(minimize.Norm): + """Implementation of the loss called 'soft_l1' in scipy least_squares.""" + + def value(self, r): + return np.sum(np.sqrt(r**2 + 1) - 1) + + def grad_hess(self, r, proj): + s = np.sqrt(r**2 + 1) + y_r = r / s + grad = proj.T @ y_r + y_rr = (1 - y_r ** 2) / s + hess = proj.T @ (y_rr * proj) + return grad, hess + + out = io.StringIO() + x0 = np.array((0.0, 0.0)) + x, _ = minimize.least_squares( + x0, residual, norm=SoftL1(), output=out, check_derivatives=True + ) + expected_x = np.array((1.0, 1.0)) + np.testing.assert_array_almost_equal(x, expected_x) + self.assertIn('User-provided norm gradient matches', out.getvalue()) + self.assertIn('User-provided norm Hessian matches', out.getvalue()) + if __name__ == '__main__': absltest.main() From 3d174a40349bdceb721727c8fe209b272d342f22 Mon Sep 17 00:00:00 2001 From: Andrea Gesmundo Date: Thu, 23 Jan 2025 02:51:08 -0800 Subject: [PATCH 258/426] internal change PiperOrigin-RevId: 718782821 Change-Id: I2d2ead2577767587800f3ea69dba61247105e16f --- mjx/tutorial.ipynb | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index b8145c1d..40da8958 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -416,7 +416,6 @@ "cell_type": "code", "execution_count": 0, "metadata": { - "cellView": "form", "id": "mtGMYNLE3QJN" }, "outputs": [], @@ -438,6 +437,7 @@ " exclude_current_positions_from_observation=True,\n", " **kwargs,\n", " ):\n", + "#\n", " mj_model = mujoco.MjModel.from_xml_path(\n", " (HUMANOID_ROOT_PATH / 'humanoid.xml').as_posix())\n", " mj_model.opt.solver = mujoco.mjtSolver.mjSOL_CG\n", @@ -1631,8 +1631,7 @@ "gpuClass": "premium", "gpuType": "V100", "machine_shape": "hm", - "private_outputs": true, - "toc_visible": true + "private_outputs": true }, "kernelspec": { "display_name": "Python 3", From 2546fcefa0e850265028a67ea1d88a83ff14cc10 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 23 Jan 2025 14:00:07 -0800 Subject: [PATCH 259/426] Add mujoco.MjSpec.to_zip() PiperOrigin-RevId: 719003836 Change-Id: I310ac96b0080b3313c325cee76d3bf7f81fa9df0 --- python/mujoco/__init__.py | 24 ++++++++++++++++++++++++ python/mujoco/specs_test.py | 3 +++ 2 files changed, 27 insertions(+) diff --git a/python/mujoco/__init__.py b/python/mujoco/__init__.py index 57b567b4..1637a2e6 100644 --- a/python/mujoco/__init__.py +++ b/python/mujoco/__init__.py @@ -19,7 +19,9 @@ import ctypes.util import os import platform import subprocess +from typing import Union, IO import warnings +import zipfile # Extend the path to enable multiple directories to contribute to the same # package. Without this line, the `mujoco-mjx` package would not be able to @@ -53,10 +55,32 @@ from mujoco._errors import * from mujoco._functions import * from mujoco._render import * from mujoco._specs import * +from mujoco._specs import MjSpec from mujoco._structs import * from mujoco.gl_context import * from mujoco.renderer import Renderer + +def to_zip(spec: MjSpec, file: Union[str, IO[bytes]]) -> None: + """Converts a spec to a zip file. + + Args: + spec: The mjSpec to save to a file. + file: The path to the file to save to or the file object to write to. + """ + files_to_zip = spec.assets + files_to_zip[spec.modelname + '.xml'] = spec.to_xml() + if isinstance(file, str): + directory = os.path.dirname(file) + os.makedirs(directory, exist_ok=True) + file = open(file, 'wb') + with zipfile.ZipFile(file, 'w') as zip_file: + for filename, contents in files_to_zip.items(): + zip_info = zipfile.ZipInfo(os.path.join(spec.modelname, filename)) + zip_file.writestr(zip_info, contents) + +MjSpec.to_zip = to_zip + HEADERS_DIR = os.path.join(os.path.dirname(__file__), 'include/mujoco') PLUGINS_DIR = os.path.join(os.path.dirname(__file__), 'plugin') diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 4b53deed..1302afc2 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -15,7 +15,9 @@ """Tests for mjSpec bindings.""" import inspect +import os import textwrap +import zipfile from absl.testing import absltest from etils import epath @@ -735,6 +737,7 @@ class SpecsTest(absltest.TestCase): v -1 -1 -1 v 1 -1 -1""" spec = mujoco.MjSpec() + spec.modelname = 'test' mesh = spec.add_mesh() mesh.name = 'cube' mesh.file = 'cube.obj' From 2c3d0becb2e1295fd4175d191c77157f6139a24a Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Sat, 25 Jan 2025 14:02:23 -0800 Subject: [PATCH 260/426] Empty out Model fields restricted to MuJoCo in the same way we do for Data fields. PiperOrigin-RevId: 719705508 Change-Id: I4b07433481eafaea1d95a5bdd8fd83786939188f --- mjx/mujoco/mjx/_src/io.py | 17 ++++++++++++++--- mjx/mujoco/mjx/_src/io_test.py | 5 +++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 7a000a15..f4e5aaeb 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -39,7 +39,7 @@ def _strip_weak_type(tree): def _make_option( - o: mujoco.MjOption, _full_compat: bool = False + o: mujoco.MjOption, _full_compat: bool = False # pylint: disable=invalid-name ) -> types.Option: """Returns mjx.Option given mujoco.MjOption.""" if not _full_compat: @@ -183,6 +183,15 @@ def put_model( if f.metadata.get('restricted_to') != 'mjx' } fields = {f: getattr(m, f) for f in mj_field_names} + + # zero out fields restricted to MuJoCo + if not _full_compat: + for f in types.Model.fields(): + if f.metadata.get('restricted_to') == 'mujoco' and isinstance( + fields[f.name], np.ndarray + ): + fields[f.name] = np.zeros((0,), dtype=fields[f.name].dtype) + fields['dof_hasfrictionloss'] = fields['dof_frictionloss'] > 0 fields['tendon_hasfrictionloss'] = fields['tendon_frictionloss'] > 0 fields['geom_rbound_hfield'] = fields['geom_rbound'] @@ -522,7 +531,7 @@ def _make_contact( # if we have fewer Contacts for a condim range, pad the range with zeros # build a map for where to find a dim-matching contact, or -1 if none - contact_map = np.zeros_like(dim) - 1 + contact_map = -np.ones_like(dim) for i, di in enumerate(fields['dim']): space = [j for j, dj in enumerate(dim) if di == dj and contact_map[j] == -1] if not space: @@ -672,7 +681,9 @@ def put_data( fields['_qLDiagInv_sparse'] = jp.zeros(0, dtype=float) # otherwise clear out unused arrays for f in types.Data.fields(): - if f.metadata.get('restricted_to') == 'mujoco': + if f.metadata.get('restricted_to') == 'mujoco' and isinstance( + fields[f.name], np.ndarray + ): fields[f.name] = np.zeros(0, dtype=fields[f.name].dtype) fields['contact'] = contact diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index b1730f33..cb9a9f75 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -20,7 +20,9 @@ import jax from jax import numpy as jp import mujoco from mujoco import mjx +# pylint: disable=g-importing-member from mujoco.mjx._src.types import ConeType +# pylint: enable=g-importing-member import numpy as np @@ -117,6 +119,9 @@ class ModelIOTest(parameterized.TestCase): self.assertEqual(mx.nM, m.nM) self.assertAlmostEqual(mx.opt.timestep, m.opt.timestep) + # fields restricted to MuJoCo should not be populated + self.assertEqual(mx.bvh_aabb.shape, (0,)) + np.testing.assert_allclose(mx.body_parentid, m.body_parentid) np.testing.assert_allclose(mx.geom_type, m.geom_type) np.testing.assert_allclose(mx.geom_bodyid, m.geom_bodyid) From 08edde5fd867b16de6c86b47179130ed1b93cd43 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 26 Jan 2025 11:59:53 -0800 Subject: [PATCH 261/426] Add test for armature, model will be used in upcoming improved documentation for armature. PiperOrigin-RevId: 719916567 Change-Id: I7f74bd3c3864f43f5f68d458d5b128cfee1d7e6c --- test/engine/engine_forward_test.cc | 41 +++++++++++++++++++ test/engine/testdata/armature_equivalence.xml | 34 +++++++++++++++ 2 files changed, 75 insertions(+) create mode 100644 test/engine/testdata/armature_equivalence.xml diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 9c930bc4..16a52f43 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -162,6 +162,47 @@ TEST_F(ForwardTest, DamperDampens) { mj_deleteModel(model); } +static const char* const kArmatureEquivalencePath = + "engine/testdata/armature_equivalence.xml"; + +// test that adding joint armature is equivalent to a coupled rotating mass with +// a gear ratio enforced by an equality +TEST_F(ForwardTest, ArmatureEquivalence) { + const std::string xml_path = GetTestDataFilePath(kArmatureEquivalencePath); + char error[1000]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // with actuators + mjtNum qpos_mse = 0; + int nstep = 0; + while (data->time < 4) { + data->ctrl[0] = data->ctrl[1] = mju_sin(2*data->time); + mj_step(model, data); + nstep++; + mjtNum err = data->qpos[0] - data->qpos[2]; + qpos_mse += err * err; + } + EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + + // no actuators + model->opt.disableflags |= mjDSBL_ACTUATION; + qpos_mse = 0; + nstep = 0; + mj_resetData(model, data); + while (data->time < 4) { + mj_step(model, data); + nstep++; + mjtNum err = data->qpos[0] - data->qpos[2]; + qpos_mse += err * err; + } + EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- implicit integrator ----------------------------- using ImplicitIntegratorTest = MujocoTest; diff --git a/test/engine/testdata/armature_equivalence.xml b/test/engine/testdata/armature_equivalence.xml new file mode 100644 index 00000000..78f16e02 --- /dev/null +++ b/test/engine/testdata/armature_equivalence.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 4ab274aea9bdbdbc9f02443c0d5d354b9f249b9e Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Sun, 26 Jan 2025 20:24:59 -0800 Subject: [PATCH 262/426] Fix missing arg for mj_compile in model-editing doc example code. PiperOrigin-RevId: 720005405 Change-Id: Idf71a1a12fe4cfcc450cd80c91b13ee8a75e1636 --- doc/programming/modeledit.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index fa800b67..9fa61790 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -49,7 +49,7 @@ editing corresponds to setting attributes. For example, in order to change the t mjSpec* spec = mj_makeSpec(); spec->opt.timestep = 0.01; ... - mjModel* model = mj_compile(spec); + mjModel* model = mj_compile(spec, NULL); Attributes which have variable length are C++ vectors and strings, :ref:`exposed to C as opaque types`. In C one uses the provided :ref:`getters` and :ref:`setters`: @@ -95,7 +95,7 @@ Elements cannot be created directly; they are returned to the user by the corres mjsGeom* my_geom = mjs_addGeom(world, NULL); // add a geom to the world my_geom->type = mjGEOM_BOX; // set geom type my_geom->size[0] = my_geom->size[1] = my_geom->size[2] = 0.5; // set box size - mjModel* model = mj_compile(spec); // compile to mjModel + mjModel* model = mj_compile(spec, NULL); // compile to mjModel The ``NULL`` second argument to :ref:`mjs_addGeom` is the optional default class pointer. When using defaults procedurally, default classes are passed in explicitly to element constructors. The global defaults of all elements From d86900aac1be02429694956e2c6d8f841d0a1d30 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 27 Jan 2025 05:05:57 -0800 Subject: [PATCH 263/426] Speed up `mju_cholUpdateSparse` by not checking for varying sparsity pattern where it is guaranteed to not vary. PiperOrigin-RevId: 720126576 Change-Id: I3a43bdd0259656fab462b1cd155cb422982e970d --- src/engine/engine_util_solve.c | 13 ++++--------- src/engine/engine_util_solve.h | 4 ++-- src/engine/engine_util_sparse.c | 2 +- src/engine/engine_util_sparse.h | 2 +- 4 files changed, 8 insertions(+), 13 deletions(-) diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 30fdc651..83fdb634 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -238,7 +238,8 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int // sparse reverse-order Cholesky rank-one update: L'*L +/- x*x'; return rank // x is sparse, change in sparsity pattern of mat is not allowed int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, - const int* rownnz, const int* rowadr, int* colind, int x_nnz, int* x_ind, + const int* rownnz, const int* rowadr, const int* colind, + int x_nnz, int* x_ind, mjData* d) { mj_markStack(d); int* buf_ind = mjSTACKALLOC(d, n, int); @@ -264,14 +265,8 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, mat[adr+nnz-1] = r; // update row: mat(r,1:r-1) = (mat(r,1:r-1) + s*x(1:r-1)) / c - int new_nnz = mju_combineSparse(mat + adr, x, 1 / c, (flg_plus ? s / c : -s / c), - nnz-1, i, colind + adr, x_ind, - sparse_buf, buf_ind); - - // check for size change - if (new_nnz != nnz-1) { - mjERROR("varying sparsity pattern"); - } + mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c), + nnz-1, i, colind + adr, x_ind); // update x: x(1:r-1) = c*x(1:r-1) - s*mat(r,1:r-1) int new_x_nnz = mju_combineSparse(x, mat+adr, c, -s, i, nnz-1, x_ind, diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 66e59842..6bbdc6bb 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -45,8 +45,8 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int // sparse reverse-order Cholesky rank-one update: L'*L +/i x*x'; return rank // x is sparse, change in sparsity pattern of mat is not allowed int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, - const int* rownnz, const int* rowadr, int* colind, int x_nnz, int* x_ind, - mjData* d); + const int* rownnz, const int* rowadr, const int* colind, + int x_nnz, int* x_ind, mjData* d); // band-dense Cholesky decomposition // returns minimum value in the factorized diagonal, or 0 if rank-deficient diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index e32115cf..0dfe4533 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -298,7 +298,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, // incomplete combine sparse: dst = a*dst + b*src at common indices void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) { + int dst_nnz, int src_nnz, const int* dst_ind, const int* src_ind) { // check for identical pattern if (dst_nnz == src_nnz) { if (mju_compare(dst_ind, src_ind, dst_nnz)) { diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 1473936b..745675cb 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -64,7 +64,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, // incomplete combine sparse: dst = a*dst + b*src at common indices void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind); + int dst_nnz, int src_nnz, const int* dst_ind, const int* src_ind); // dst += scl * src, only at common non-zero indices void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, From d21f624264d938cd722d761ad8b695c0383a29f6 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 27 Jan 2025 05:46:36 -0800 Subject: [PATCH 264/426] Fix binding issue with scalar attributes. PiperOrigin-RevId: 720136044 Change-Id: I0846a93ef36a23371d1bd504adff02dcce0abcf9 --- mjx/mujoco/mjx/_src/support.py | 2 +- mjx/mujoco/mjx/_src/support_test.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index aeb0f7ed..0e4e21ec 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -367,7 +367,7 @@ class BindModel(object): self.id = ids def __getattr__(self, name: str): - return getattr(self.model, self.prefix + name)[self.id, :] + return getattr(self.model, self.prefix + name)[self.id, ...] def _bind_model(self: Model, obj: Sequence[Any]) -> BindModel: diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index a61374da..da046c70 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -219,6 +219,7 @@ class SupportTest(parameterized.TestCase): ) np.testing.assert_array_equal(mx.bind(s.joints).axis, m.jnt_axis) + np.testing.assert_array_equal(mx.bind(s.joints).qposadr, m.jnt_qposadr) for i in range(m.njnt): np.testing.assert_array_equal(m.bind(s.joints[i]).axis, m.jnt_axis[i, :]) np.testing.assert_array_equal(mx.bind(s.joints[i]).axis, m.jnt_axis[i, :]) From 96059906480ab3f301568f53ca22b1b578b07743 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 27 Jan 2025 10:23:59 -0800 Subject: [PATCH 265/426] Replace custom sort of sparse fixed tendons with `mju_combineSparse`. PiperOrigin-RevId: 720219887 Change-Id: I0491b3cb8987c119c3864fc0b5c38ca303d8f7f0 --- src/engine/engine_core_smooth.c | 28 +++------------ test/engine/engine_core_smooth_test.cc | 50 ++++++++++++++++++++++++++ 2 files changed, 55 insertions(+), 23 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 01911f0d..88702a06 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -665,7 +665,7 @@ void mj_tendon(const mjModel* m, mjData* d) { rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0); } - // process joint tendon + // process fixed tendon if (m->wrap_type[adr] == mjWRAP_JOINT) { // process all defined joints for (int j=0; j < tendon_num; j++) { @@ -677,9 +677,10 @@ void mj_tendon(const mjModel* m, mjData* d) { // add to moment if (issparse) { - J[rowadr[i] + rownnz[i]] = m->wrap_prm[adr+j]; - colind[rowadr[i] + rownnz[i]] = m->jnt_dofadr[k]; - rownnz[i]++; + rownnz[i] = mju_combineSparse(J+rowadr[i], &m->wrap_prm[adr+j], 1, 1, + rownnz[i], 1, + colind+rowadr[i], &m->jnt_dofadr[k], + sparse_buf, buf_ind); } // add to moment: dense @@ -688,25 +689,6 @@ void mj_tendon(const mjModel* m, mjData* d) { } } - // sort on colind if sparse: custom insertion sort - if (issparse) { - int x, *list = colind+rowadr[i], nnz = rownnz[i]; - mjtNum y, *listy = J+rowadr[i]; - - for (int k=1; k < nnz; k++) { - x = list[k]; - y = listy[k]; - int j = k-1; - while (j >= 0 && list[j] > x) { - list[j+1] = list[j]; - listy[j+1] = listy[j]; - j--; - } - list[j+1] = x; - listy[j+1] = y; - } - } - continue; } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 916fb625..47e1ff49 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -109,6 +109,56 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { mj_deleteModel(model); } +// ----------------------------- mj_tendon ------------------------------------- + +TEST_F(CoreSmoothTest, FixedTendonSortedIndices) { + constexpr char xml[] = R"( + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + ASSERT_EQ(model->ntendon, 1); + ASSERT_EQ(model->nwrap, 3); + + mjData* data = mj_makeData(model); + mj_fwdPosition(model, data); + + int rowadr = data->ten_J_rowadr[0]; + int* colind = data->ten_J_colind + rowadr; + mjtNum* J = data->ten_J + rowadr; + + EXPECT_THAT(vector(J, J + 3), ElementsAre(1, 2, 3)); + EXPECT_THAT(vector(colind, colind + 3), ElementsAre(0, 1, 2)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- connect constraint ------------------------------ // test that bodies hanging on connects lead to expected force sensor readings From 8c22181156f5eb5232f30ab7d96b5951187f7e02 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 27 Jan 2025 10:57:06 -0800 Subject: [PATCH 266/426] Fix documentation typos. PiperOrigin-RevId: 720232771 Change-Id: I176db4d4168c37b2819df14b439c4cde838e2025 --- doc/APIreference/APItypes.rst | 4 +-- doc/APIreference/functions.rst | 2 +- doc/APIreference/functions_override.rst | 2 +- doc/XMLreference.rst | 18 ++++++------- doc/changelog.rst | 12 ++++----- doc/computation/fluid.rst | 2 +- doc/computation/index.rst | 12 ++++----- doc/mjx.rst | 2 +- doc/modeling.rst | 12 ++++----- doc/overview.rst | 4 +-- doc/programming/modeledit.rst | 2 +- doc/python.rst | 35 ++++++++++++++----------- doc/templates/layout.html | 6 ++--- doc/unity.rst | 2 +- 14 files changed, 59 insertions(+), 56 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 3f11ee5e..7e2befa0 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -28,7 +28,7 @@ MuJoCo defines a large number of types: - :ref:`mjOption` (embedded in :ref:`mjModel`). - :ref:`mjData`. - - :ref:`Auxillary struct types`, also used by the engine. + - :ref:`Auxiliary struct types`, also used by the engine. - Structs for collecting :ref:`simulation statistics`. - Structs for :ref:`abstract visualization`. - Structs used by the :ref:`openGL renderer`. @@ -752,7 +752,7 @@ modifiable inputs and write their outputs. .. _tyAuxStructure: -Auxillary +Auxiliary ^^^^^^^^^ These struct types are used in the engine and their names are prefixed with ``mj``. :ref:`mjVisual` diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 19d792fd..e2f922c7 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -504,7 +504,7 @@ found, the function will return ``distmax`` and ``fromto``, if given, will be se .. TODO: b/339596989 - Improve mjc_Convex. For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. This is considered a bug to be fixed in a future release. In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of `engine_collision_driver.c `__. diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index d34fa3f9..9d425623 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -228,7 +228,7 @@ found, the function will return ``distmax`` and ``fromto``, if given, will be se .. TODO: b/339596989 - Improve mjc_Convex. For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. This is considered a bug to be fixed in a future release. In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of `engine_collision_driver.c `__. diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9a8a6e5d..3daec8d9 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -591,7 +591,7 @@ from its default. .. _option-flag-autoreset: :at:`autoreset`: :at-val:`[disable, enable], "enable"` - This flag disables the automatic reseting of the simulation state when numerical issues are detected. + This flag disables the automatic resetting of the simulation state when numerical issues are detected. .. _option-flag-override: @@ -604,7 +604,7 @@ from its default. This flag enables the computation of potential and kinetic energy in ``mjData.energy[0, 1]`` respectively, and displayed in the simulate GUI info overlay. Potential energy includes the gravitational component summed over all bodies :math:`\sum_b m_b g h` and energy stored in passive springs in joints, tendons and flexes - :math:`\tfrac{1}{2} k x^2`, where :math:`x` is the displacement and and :math:`k` is the spring constant. Kinetic + :math:`\tfrac{1}{2} k x^2`, where :math:`x` is the displacement and :math:`k` is the spring constant. Kinetic energy is given by :math:`\tfrac{1}{2} v^T M v`, where :math:`v` is the velocity and :math:`M` is the mass matrix. Note that potential and kinetic energy in constraints is not accounted for. @@ -1271,7 +1271,7 @@ The full list of processing steps applied by the compiler to each mesh is as fol :at:`maxhullvert`: :at-val:`int, "-1"` Maximum number of vertices in a mesh's convex hull. Currently this is implemented by asking qhull - `to teminate `__ after :at:`maxhullvert` vertices. The default + `to terminate `__ after :at:`maxhullvert` vertices. The default value of -1 means "unlimited". Positive values must be larger than 3. .. _asset-mesh-vertex: @@ -2122,7 +2122,7 @@ rotations as unit quaternions. (``mjData.qfrc_actuator``) rather than passive forces (``mjData.qfrc_passive``). Notionally, this means that gravity compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint, - including gravity compensation, is guaranteed to not exceeed the specified limits. See :ref:`CForceRange` and + including gravity compensation, is guaranteed to not exceed the specified limits. See :ref:`CForceRange` and :ref:`actuatorfrcrange` for more details on this type of force limit. .. _body-joint-margin: @@ -2505,7 +2505,7 @@ helps clarify the role of bodies and geoms in MuJoCo. `. The frame position is in the middle between the end points. If this attribute is specified, the remaining position and orientation-related attributes are ignored. The image on the right demonstrates use of :at:`fromto` with the four supported geoms, using identical Z values. The model is `here <_static/fromto.xml>`__. - Note that the :at:`fromto` semantics of *capsule* are unique: the two end points specify the segement around which + Note that the :at:`fromto` semantics of *capsule* are unique: the two end points specify the segment around which the radius defines the capsule surface. .. _body-geom-pos: @@ -2736,7 +2736,7 @@ and the +Y axis points up. Thus the frame position and orientation are the key a :at:`mode`: :at-val:`[fixed, track, trackcom, targetbody, targetbodycom], "fixed"` This attribute specifies how the camera position and orientation in world coordinates are computed in forward kinematics (which in turn determine what the camera sees). "fixed" means that the position and orientation specified - below are fixed relative to the the body where the camera is defined. "track" means that the camera position is at a + below are fixed relative to the body where the camera is defined. "track" means that the camera position is at a constant offset from the body in world coordinates, while the camera orientation is constant in world coordinates. These constants are determined by applying forward kinematics in qpos0 and treating the camera as fixed. Tracking can be used for example to position a camera above a body, point it down so it sees the body, and have it always remain @@ -4052,7 +4052,7 @@ cases, the user will specify a :el:`flexcomp` which will then automatically cons which is why the number of indices equals (dim+1) times the number of elements. In 2D, the vertices should be listed in counter-clockwise order. In 1D and 3D the order is irrelevant; in 3D the model compiler will rearrange the vertices as needed. Repeated vertex indices within a flex element are not allowed. The topology of the flex is not - enforced; it could corespond to a continuous soft body, or a collection of disconnected stretchable elements, or + enforced; it could correspond to a continuous soft body, or a collection of disconnected stretchable elements, or anything in-between. .. _deformable-flex-flatskin: @@ -4140,7 +4140,7 @@ stress-strain relationship.. See also :ref:`deformable ` objects. :at:`thickness`: :at-val:`real(1), "-1"` Shell thickness, units of length; only for used 2D flexes. Used to scale the stretching stiffness. This thickness can be set equal to 2 times the :ref:`radius ` in order to match the geometry, - but is exposed seperately since the radius might be constrained by considerations related to collision detection. + but is exposed separately since the radius might be constrained by considerations related to collision detection. .. _flex-contact: @@ -7099,7 +7099,7 @@ pipeline. These 3 sensors share some common properties: .. TODO: b/339596989 - Improve mjc_Convex. For some colliders, a positive :at:`cutoff` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate. + pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. This is considered a bug to be fixed in a future release. In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of `engine_collision_driver.c `__. diff --git a/doc/changelog.rst b/doc/changelog.rst index 697604b9..00c92e4d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -88,7 +88,7 @@ General 4. The not-useful ``convexhull`` compiler option (to disable computation of mesh convex hulls) has been removed. 5. Removed the deprecated ``mju_rotVecMat``, ``mju_rotVecMatT`` and ``mjv_makeConnector`` functions. 6. Sorting now uses a faster, native sort function (fixes :github:issue:`1638`). -7. The PBR texture layers introduced in 3.2.1 were refactored from seperate sub-elements to a single +7. The PBR texture layers introduced in 3.2.1 were refactored from separate sub-elements to a single :ref:`layer` sub-element. 8. The composite types box, cylinder, and sphere have been removed. Users should instead use the equivalent types available in :ref:`flexcomp`. @@ -178,7 +178,7 @@ General 4. Added the :ref:`nativeccd` flag. When this flag is enabled, general convex collision detection is handled with a new native code path, rather than `libccd `__. - This feature is in early stages of testing, but users who've experienced issues related to collsion detection are + This feature is in early stages of testing, but users who've experienced issues related to collision detection are welcome to experiment with it and report any issues. .. youtube:: kcM_oauk3ZA @@ -316,7 +316,7 @@ General The older functions have been removed from the Python bindings and will be removed from the C API in the next release. 5. Removed the ``actuator_actdim`` callback from actuator plugins. They now have the ``actdim`` attribute, which - must be used with actuators that write state to the ``act`` array. This fixed a crash which happend when + must be used with actuators that write state to the ``act`` array. This fixed a crash which happened when keyframes were used in a model with stateful actuator plugins. The PID plugin will give an error when the wrong value of actdim is provided. @@ -577,7 +577,7 @@ General 1. Improved the :ref:`discardvisual` compiler flag, which now discards all visual-only assets. See :ref:`discardvisual` for details. 2. Removed the :ref:`timer` for midphase colllision detection, it is now folded in with the narrowphase - timer. This is because timing the two phases seperately required fine-grained timers inside the collision + timer. This is because timing the two phases separately required fine-grained timers inside the collision functions; these functions are so small and fast that the timer itself was incurring a measurable cost. 3. Added the flag :ref:`bvactive` to ``visual/global``, allowing users to turn off visualisation of active bounding volumes (the red/green boxes in this :ref:`this changelog item`). For @@ -1050,7 +1050,7 @@ Simulate :width: 240px 6. Added Visualization tab to simulate UI, corresponding to elements of the :ref:`visual` MJCF element. After - modifying values in the GUI, a saved XML will contain the new values. The modifyable members of + modifying values in the GUI, a saved XML will contain the new values. The modifiable members of :ref:`mjStatistic` (:ref:`extent`, :ref:`meansize` and :ref:`center`) are computed by the compiler and therefore do not have defaults. In order for these attributes to appear in the saved XML, a value must be specified in the loaded XML. @@ -1169,7 +1169,7 @@ Python bindings passive viewer now also requires an explicit call to ``sync`` on its handle to pick up any update to the physics state. This is to avoid race conditions that can result in visual artifacts. See :ref:`documentation` for details. -#. The ``viewer.launch_repl`` function has been removed since its functionality is superceded by ``launch_passive``. +#. The ``viewer.launch_repl`` function has been removed since its functionality is superseded by ``launch_passive``. #. Added a small number of missing struct fields discovered through the new ``introspect`` metadata. Bug fixes diff --git a/doc/computation/fluid.rst b/doc/computation/fluid.rst index b006f758..24c259c2 100644 --- a/doc/computation/fluid.rst +++ b/doc/computation/fluid.rst @@ -135,7 +135,7 @@ also disables the inertia-based model for the parent body. The Elements of the model are a generalization of :cite:t:`andersen2005b` to 3 dimensions. The force :math:`\mathbf{f}_{\text{ellipsoid}}` and torque :math:`\mathbf{g}_{\text{ellipsoid}}` exerted by the fluid onto the solid are -the sum of of the terms +the sum of the terms .. math:: \begin{align*} diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 048ac199..f6e84273 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -241,7 +241,7 @@ The computation of the constraint force is the hard part and will be described l description of the general framework by summarizing how the above quantities up to the constraint Jacobian are computed. - The applied force :math:`\tau` includes :ref:`passive ` forces from spring-dampers and fluid dynamics, - :ref:`actuation ` forces, and additonal forces specified by the user. + :ref:`actuation ` forces, and additional forces specified by the user. - The bias force :math:`c` includes Coriolis, centrifugal and gravitational forces. Their sum is computed using the Recursive Newton-Euler (RNE) algorithm with acceleration set to 0. - The joint-space inertia matrix :math:`M` is computed using the Composite Rigid-Body (CRB) algorithm. This matrix is @@ -414,7 +414,7 @@ with MuJoCo's operation as long as such user forces depend only on position and MuJoCo can compute three types of passive forces: -- Spring-dampers in joints and tendons. See the following attribues for details. +- Spring-dampers in joints and tendons. See the following attributes for details. |br| **Joints:** :ref:`stiffness`, :ref:`springref`, :ref:`damping`, :ref:`springdamper`. @@ -576,8 +576,8 @@ Fast implicit-in-velocity (``implicitfast``) increased stability, and is therefore a strict improvement. It is the recommended integrator for most models. **implicit**: The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces, including - gyroscopic forces. The most common case where integrating such forces implicitly leads to noticable improvement is - when free objects with assymetric inertia are spinning quickly. `gyroscopic.xml <../_static/gyroscopic.xml>`__ + gyroscopic forces. The most common case where integrating such forces implicitly leads to noticeable improvement is + when free objects with asymmetric inertia are spinning quickly. `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an inclined plane which quickly diverges with ``implicitfast`` but is stable with ``implicit``. **RK4**: @@ -646,7 +646,7 @@ Control: ``ctrl`` generalized forces directly (stateless actuators), or affect the actuator activations in ``mjData.act``, which then produce forces. -Auxillary Controls: ``qfrc_applied`` and ``xfrc_applied`` +Auxiliary Controls: ``qfrc_applied`` and ``xfrc_applied`` | ``mjData.qfrc_applied`` are directly applied generalized forces. | ``mjData.xfrc_applied`` are Cartesian wrenches applied to the CoM of individual bodies. This field is used for example, by the :ref:`native viewer` to apply mouse perturbations. @@ -1722,7 +1722,7 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu Derivatives ----------- -MuJoCo's entire computational pipline including its constraint solver are analytically differentiable in principle. +MuJoCo's entire computational pipeline including its constraint solver are analytically differentiable in principle. Writing efficient implementations of these derivatives is a long term goal of the development team. Analytic derivatives of the smooth dynamics (excluding constraints) with respect to velocity are already computed and enable the two :ref:`implicit integrators`. diff --git a/doc/mjx.rst b/doc/mjx.rst index cc50fe2e..55f9aab3 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -50,7 +50,7 @@ The recommended way to install this package is via `PyPI ` attribute, which can be changed at runtime by setting the :ref:`mjOption.disableactuator` integer bitfield, allows the user to disable sets of actuators according to their :ref:`group`. This feature is convenient when one would like to use multiple types of -actuators for the same kinematic tree. For example consider a robot with firmware that supports mutiple control modes +actuators for the same kinematic tree. For example consider a robot with firmware that supports multiple control modes e.g., torque-control and position-control. In this case, one can define both types of actuators in the same MJCF model, assigning one type of actuator to group 0 and the other to group 1. @@ -1452,7 +1452,7 @@ elements available in MuJoCo. In addition to standard URDF files, MuJoCo can loa viewpoint of URDF) :el:`mujoco` element as a child of the top-level element :el:`robot`. This custom element can have sub-elements :ref:`compiler `, :ref:`option