From 7062c6f6772909ec573deb43481b588acaa856b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Fri, 17 Nov 2023 14:43:10 +0000 Subject: [PATCH 01/44] Enable mouse spring perturbations when clicking on a geom in scene view --- unity/Editor/Components/MjMouseSpring.cs | 8 ++++++-- unity/Editor/Components/MjShapeComponentEditor.cs | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/unity/Editor/Components/MjMouseSpring.cs b/unity/Editor/Components/MjMouseSpring.cs index b2264727..c502e551 100644 --- a/unity/Editor/Components/MjMouseSpring.cs +++ b/unity/Editor/Components/MjMouseSpring.cs @@ -23,7 +23,7 @@ namespace Mujoco { // and left mouse drag will apply a force on the body. Holding shift down will change the applied // force direction between World XZ plane and Y[camera-up]. - [CustomEditor(typeof(MjBody))] + [CustomEditor(typeof(MjComponent), true)] public class MjMouseSpring : Editor { private bool _lastShiftKeyState = false; @@ -102,7 +102,11 @@ namespace Mujoco { return; } - MjBody body = target as MjBody; + var targetObject = target as MjComponent; + MjBody body = targetObject.GetComponentInParent(); + if(!body) + return; + Vector3 bodyPosition = body != null ? body.transform.position : Vector3.zero; var scene = MjScene.Instance; diff --git a/unity/Editor/Components/MjShapeComponentEditor.cs b/unity/Editor/Components/MjShapeComponentEditor.cs index 41b8ba22..3d0d687c 100644 --- a/unity/Editor/Components/MjShapeComponentEditor.cs +++ b/unity/Editor/Components/MjShapeComponentEditor.cs @@ -22,7 +22,7 @@ namespace Mujoco { [CustomEditor(typeof(MjShapeComponent), true)] [CanEditMultipleObjects] -public class MjShapeComponentEditor : Editor { +public class MjShapeComponentEditor : MjMouseSpring { public override void OnInspectorGUI() { From f99e9abc3c8dde43eb51aa2b59322568de7e23ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Fri, 17 Nov 2023 15:00:19 +0000 Subject: [PATCH 02/44] Remove no longer necessary directive for editor only compilation (solved by assembly definition) --- unity/Editor/Components/MjMouseSpring.cs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/unity/Editor/Components/MjMouseSpring.cs b/unity/Editor/Components/MjMouseSpring.cs index c502e551..33ead857 100644 --- a/unity/Editor/Components/MjMouseSpring.cs +++ b/unity/Editor/Components/MjMouseSpring.cs @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#if UNITY_EDITOR using System; using UnityEditor; using UnityEngine; @@ -191,5 +190,4 @@ namespace Mujoco { } } } -} -#endif +} \ No newline at end of file From 08c62339122b428ead44c7ad0a975f7010dc2b2b Mon Sep 17 00:00:00 2001 From: limymy <72388584+limymy@users.noreply.github.com> Date: Thu, 7 Dec 2023 14:36:29 +0800 Subject: [PATCH 03/44] fix gcc warning "stringop-truncation" --- 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 9154fa4d..393bd509 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -62,7 +62,7 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { } // copy result into label - strncpy(label, txt, 99); + strncpy(label, txt, 100); label[99] = '\0'; } From 14e9607d3a1dc666637435193d5017a6eb2e0b84 Mon Sep 17 00:00:00 2001 From: Jamie Mair Date: Thu, 7 Dec 2023 11:16:10 +0000 Subject: [PATCH 04/44] Edited README to update the entry for the Julia 3rd party bindings --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 21a3cb81..cb19a639 100644 --- a/README.md +++ b/README.md @@ -164,7 +164,7 @@ These packages give users of various languages access to MuJoCo functionality: by [Manoj Velmurugan](https://github.com/vmanoj1996). - **Swift**: [swift-mujoco](https://github.com/liuliu/swift-mujoco) - **Java**: [mujoco-java](https://github.com/CommonWealthRobotics/mujoco-java) -- **Julia**: [Lyceum](https://github.com/Lyceum/MuJoCo.jl) (unmaintained) +- **Julia**: [MuJoCo.jl](https://github.com/JamieMair/MuJoCo.jl) ### Converters From d12fe594d98ee0500fa6bb7513f3faf3d9f7488a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 7 Dec 2023 06:45:55 -0800 Subject: [PATCH 05/44] Add suffix to duplicated assets. PiperOrigin-RevId: 588775467 Change-Id: I3a0d2a14601cf018b0c8c56728bd3ae22adb8ec2 --- src/user/user_model.cc | 74 +++++++++++++++++-------------- src/user/user_model.h | 5 ++- test/user/testdata/load_twice.xml | 10 +++++ test/user/user_model_test.cc | 22 +++++++++ 4 files changed, 76 insertions(+), 35 deletions(-) create mode 100644 test/user/testdata/load_twice.xml diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 693287fa..fcc0c354 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -842,57 +843,56 @@ void mjCModel::IndexAssets(void) { // if asset name is missing, set to filename -void mjCModel::SetDefaultNames(void) { +template +void mjCModel::SetDefaultNames(std::vector& assets) { string stripped; + std::map> names; - // meshes - for (int i=0; iname.empty()) { - stripped = mjuu_strippath(meshes[i]->file()); - meshes[i]->name = mjuu_stripext(stripped); - - // name cannot be empty - if (meshes[i]->name.empty()) { - throw mjCError(meshes[i], "empty name in mesh"); - } + // use filename if name is missing + for (int i=0; iname.empty()) { + stripped = mjuu_strippath(assets[i]->get_file()); + assets[i]->name = mjuu_stripext(stripped); + names[assets[i]->name].push_back(i); } } - // skins - for (int i=0; iname.empty()) { - stripped = mjuu_strippath(skins[i]->file); - skins[i]->name = mjuu_stripext(stripped); + // add suffix if duplicates + for (auto const& [name, indices] : names) { + if (indices.size() > 1) { + for (int i=0; iname += "_" + std::to_string(i); + } + } + } +} + + + +// throw error if a name is missing +void mjCModel::CheckEmptyNames(void) { + // meshes + for (int i=0; iname.empty()) { + throw mjCError(meshes[i], "empty name in mesh"); } } // hfields for (int i=0; iname.empty()) { - stripped = mjuu_strippath(hfields[i]->file); - hfields[i]->name = mjuu_stripext(stripped); - - // name cannot be empty - if (hfields[i]->name.empty()) { - throw mjCError(hfields[i], "empty name in height field"); - } + throw mjCError(hfields[i], "empty name in height field"); } } // textures for (int i=0; iname.empty()) { - stripped = mjuu_strippath(textures[i]->file); - textures[i]->name = mjuu_stripext(stripped); - - // name cannot be empty, except for skybox - if (textures[i]->name.empty() && textures[i]->type!=mjTEXTURE_SKYBOX) { - throw mjCError(textures[i], "empty name in texture"); - } + if (textures[i]->name.empty() && textures[i]->type!=mjTEXTURE_SKYBOX) { + throw mjCError(textures[i], "empty name in texture"); } } - // materials: name check only + // materials for (int i=0; iname.empty()) { throw mjCError(materials[i], "empty name in material"); @@ -2785,6 +2785,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // make lists of objects created in kinematic tree MakeLists(bodies[0]); + // fill missing names and check that they are all filled + SetDefaultNames(meshes); + SetDefaultNames(skins); + SetDefaultNames(hfields); + SetDefaultNames(textures); + CheckEmptyNames(); + // set object ids and default names, check for repeated names processlist(bodies, "body"); processlist(joints, "joint"); @@ -2810,8 +2817,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { processlist(keys, "key"); processlist(plugins, "plugin"); - // set default names, convert names into indices - SetDefaultNames(); + // convert names into indices IndexAssets(); // mark meshes that need convex hull diff --git a/src/user/user_model.h b/src/user/user_model.h index 77d9d718..f013d832 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -176,10 +176,13 @@ class mjCModel { template // add object of any type, with def parameter T* AddObjectDef(std::vector& list, std::string type, mjCDef* def); + template // if asset name is missing, set to filename + void SetDefaultNames(std::vector& assets); + //------------------------ compile phases void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites void IndexAssets(void); // convert asset names into indices - void SetDefaultNames(void); // if mesh or hfield name is missing, set to filename + void CheckEmptyNames(void); // check empty names void SetSizes(void); // compute sizes void AutoSpringDamper(mjModel*);// automatic stiffness and damping computation void LengthRange(mjModel*, mjData*); // compute actuator lengthrange diff --git a/test/user/testdata/load_twice.xml b/test/user/testdata/load_twice.xml new file mode 100644 index 00000000..4e9304a7 --- /dev/null +++ b/test/user/testdata/load_twice.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 3abae496..db1059ef 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -14,6 +14,7 @@ // Tests for user/user_model.cc. +#include #include #include @@ -21,6 +22,7 @@ #include #include #include +#include "src/cc/array_safety.h" #include "test/fixture.h" namespace mujoco { @@ -190,6 +192,26 @@ TEST_F(UserDataTest, AutoNUserSensor) { mj_deleteModel(m); } +// ------------- test duplicate names ------------------------------------------ +TEST_F(UserDataTest, DuplicateNames) { + static const char* const kFilePath = "user/testdata/load_twice.xml"; + const std::string xml_path = GetTestDataFilePath(kFilePath); + + std::array error; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_THAT(m->nmesh, 2); + + for (int i = 0; i < m->nmesh; i++) { + char mesh_name[mjMAXUINAME] = ""; + util::strcat_arr(mesh_name, m->names + m->name_meshadr[i]); + EXPECT_THAT(std::string(mesh_name), "cube_" + std::to_string(i)); + } + + mj_deleteModel(m); +} + // ------------- test fusestatic ----------------------------------------------- using FuseStaticTest = MujocoTest; From 1db0a9946fca461ed17493a34e03dcbcf8694ab9 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 7 Dec 2023 08:35:46 -0800 Subject: [PATCH 06/44] Fix frame accumulation order for `` meta-element. PiperOrigin-RevId: 588803425 Change-Id: Id0289c34d1a6cfb6007540a29cc1f32d879e43ed --- src/user/user_objects.cc | 14 ++++---- src/user/user_util.cc | 31 ++++++++++------ src/user/user_util.h | 18 ++++++---- test/user/user_objects_test.cc | 64 +++++++++++++++++++++++----------- 4 files changed, 82 insertions(+), 45 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 009ecebc..84a9c8c8 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -993,7 +993,7 @@ void mjCBody::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // accumulate rbound, contype, conaffinity over geoms @@ -1101,7 +1101,7 @@ void mjCFrame::Compile() { // compile parents and accumulate result if (frame) { frame->Compile(); - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } mjuu_normvec(quat, 4); @@ -1263,7 +1263,7 @@ int mjCJoint::Compile(void) { mjuu_zerovec(pos, 3); } else if (frame) { double qunit[4] = {1, 0, 0, 0}; - mjuu_frameaccum(pos, qunit, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, qunit); } // convert reference angles to radians for hinge joints @@ -1878,7 +1878,7 @@ void mjCGeom::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } } @@ -1990,7 +1990,7 @@ void mjCSite::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize quaternion @@ -2055,7 +2055,7 @@ void mjCCamera::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize quaternion @@ -2159,7 +2159,7 @@ void mjCLight::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize direction, make sure it is not zero diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 4c75ccce..35e5568d 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -367,8 +367,8 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl // invert frame transformation -void mjuu_frameinvert(double* newpos, double* newquat, - const double* oldpos, const double* oldquat) { +void mjuu_frameinvert(double newpos[3], double newquat[4], + const double oldpos[3], const double oldquat[4]) { // position mjuu_localaxis(newpos, oldpos, oldquat); newpos[0] = -newpos[0]; @@ -384,28 +384,39 @@ void mjuu_frameinvert(double* newpos, double* newquat, // accumulate frame transformations (forward kinematics) -void mjuu_frameaccum(double* pos, double* quat, - const double* addpos, const double* addquat) { +void mjuu_frameaccum(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]) { double mat[9], vec[3], qtmp[4]; mjuu_quat2mat(mat, quat); - mjuu_mulvecmat(vec, addpos, mat); + mjuu_mulvecmat(vec, childpos, mat); pos[0] += vec[0]; pos[1] += vec[1]; pos[2] += vec[2]; - mjuu_mulquat(qtmp, quat, addquat); + mjuu_mulquat(qtmp, quat, childquat); mjuu_copyvec(quat, qtmp, 4); } +// accumulate frame transformation in second frame +void mjuu_frameaccumChild(const double pos[3], const double quat[4], + double childpos[3], double childquat[4]) { + double p[] = {pos[0], pos[1], pos[2]}; + double q[] = {quat[0], quat[1], quat[2], quat[3]}; + mjuu_frameaccum(p, q, childpos, childquat); + mjuu_copyvec(childpos, p, 3); + mjuu_copyvec(childquat, q, 4); +} + + // invert frame accumulation -void mjuu_frameaccuminv(double* pos, double* quat, - const double* addpos, const double* addquat) { +void mjuu_frameaccuminv(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]) { double mat[9], vec[3], qtmp[4]; - double qneg[4] = {addquat[0], -addquat[1], -addquat[2], -addquat[3]}; + double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]}; mjuu_mulquat(qtmp, quat, qneg); mjuu_copyvec(quat, qtmp, 4); mjuu_quat2mat(mat, quat); - mjuu_mulvecmat(vec, addpos, mat); + mjuu_mulvecmat(vec, childpos, mat); pos[0] -= vec[0]; pos[1] -= vec[1]; pos[2] -= vec[2]; diff --git a/src/user/user_util.h b/src/user/user_util.h index fa230137..2cede571 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -104,16 +104,20 @@ void mjuu_z2quat(double* quat, const double* vec); void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z); // invert frame transformation -void mjuu_frameinvert(double* newpos, double* newquat, - const double* oldpos, const double* oldquat); +void mjuu_frameinvert(double newpos[3], double newquat[4], + const double oldpos[3], const double oldquat[4]); -// accumulate frame transformations -void mjuu_frameaccum(double* pos, double* quat, - const double* addpos, const double* addquat); +// accumulate frame transformation into parent frame +void mjuu_frameaccum(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]); + +// accumulate frame transformation into child frame +void mjuu_frameaccumChild(const double pos[3], const double quat[4], + double childpos[3], double childquat[4]); // invert frame accumulation -void mjuu_frameaccuminv(double* pos, double* quat, - const double* addpos, const double* addquat); +void mjuu_frameaccuminv(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]); // convert local_inertia[3] to global_inertia[6] void mjuu_globalinertia(double* global, const double* local, const double* quat); diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 37154b13..28750da0 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1404,38 +1404,47 @@ TEST_F(MujocoTest, Frame) { - + - - - + + + - + - + - + + + + + + + + + )"; + constexpr mjtNum eps = 1e-14; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(m, testing::NotNull()) << error.data(); - EXPECT_EQ(m->nbody, 4); + EXPECT_EQ(m->nbody, 5); // geom quat transformed to euler = 0 0 50 EXPECT_NEAR(m->geom_quat[0], mju_cos(25. * mjPI / 180.), 1e-3); @@ -1444,15 +1453,15 @@ TEST_F(MujocoTest, Frame) { EXPECT_NEAR(m->geom_quat[3], mju_sin(25. * mjPI / 180.), 1e-3); // geom transformed to frame 0 1 0, 0 0 1, 1 0 0 - EXPECT_NEAR(m->geom_quat[4], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[5], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[6], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[7], .5, 1e-6); + EXPECT_NEAR(m->geom_quat[4], .5, eps); + EXPECT_NEAR(m->geom_quat[5], .5, eps); + EXPECT_NEAR(m->geom_quat[6], .5, eps); + EXPECT_NEAR(m->geom_quat[7], .5, eps); // geom pos transformed from 0 1 0 to 0 2 0 - EXPECT_EQ(m->geom_pos[6], 0); - EXPECT_EQ(m->geom_pos[7], 2); - EXPECT_EQ(m->geom_pos[8], 0); + EXPECT_EQ(m->geom_pos[ 9], 0); + EXPECT_EQ(m->geom_pos[10], 2); + EXPECT_EQ(m->geom_pos[11], 0); // body pos transformed from 1 0 0 to 1 1 0 EXPECT_EQ(m->body_pos[6], 1); @@ -1460,16 +1469,29 @@ TEST_F(MujocoTest, Frame) { EXPECT_EQ(m->body_pos[8], 0); // nested geom pos not transformed - EXPECT_EQ(m->geom_pos[ 9], 0); - EXPECT_EQ(m->geom_pos[10], 0); - EXPECT_EQ(m->geom_pos[11], 1); + EXPECT_EQ(m->geom_pos[12], 0); + EXPECT_EQ(m->geom_pos[13], 0); + EXPECT_EQ(m->geom_pos[14], 1); // joint axis transformed to 0 -1 0 - EXPECT_NEAR(m->jnt_axis[0], 0, 1e-6); - EXPECT_NEAR(m->jnt_axis[1], -1, 1e-6); - EXPECT_NEAR(m->jnt_axis[2], 0, 1e-6); + EXPECT_NEAR(m->jnt_axis[0], 0, eps); + EXPECT_NEAR(m->jnt_axis[1], -1, eps); + EXPECT_NEAR(m->jnt_axis[2], 0, eps); + + mjData* d = mj_makeData(m); + mj_kinematics(m, d); + + // body and frame equivalence geom 2 vs 6 + EXPECT_NEAR(d->geom_xpos[6], d->geom_xpos[18], eps); + EXPECT_NEAR(d->geom_xpos[7], d->geom_xpos[19], eps); + EXPECT_NEAR(d->geom_xpos[8], d->geom_xpos[20], eps); + EXPECT_NEAR(d->geom_xmat[18], d->geom_xmat[54], eps); + EXPECT_NEAR(d->geom_xmat[19], d->geom_xmat[55], eps); + EXPECT_NEAR(d->geom_xmat[20], d->geom_xmat[56], eps); + EXPECT_NEAR(d->geom_xmat[21], d->geom_xmat[57], eps); mj_deleteModel(m); + mj_deleteData(d); } } // namespace From b1a912e9e22ae8cca170a029af13256052197ab4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 8 Dec 2023 01:18:16 -0800 Subject: [PATCH 07/44] Improve readability of test. PiperOrigin-RevId: 589048504 Change-Id: I5087cfd67481df90103da2d7f5f06e421fc28e11 --- test/user/user_objects_test.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 28750da0..75fc9422 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1413,6 +1413,10 @@ TEST_F(MujocoTest, Frame) { + + + + @@ -1432,10 +1436,6 @@ TEST_F(MujocoTest, Frame) { - - - - From 623201ec76cdfd52d8ceced9d5f81a309ee88af1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 8 Dec 2023 05:40:26 -0800 Subject: [PATCH 08/44] Throw error if scale attribute is used with an SDF. Fixes #1210. PiperOrigin-RevId: 589101807 Change-Id: I3f2796de3438fbcdaf9221bddc5c9bd82aedb7cb --- src/user/user_mesh.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index cc2536bf..edd36263 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -296,6 +296,11 @@ void mjCMesh::LoadSDF() { name.c_str(), id); } + if (scale_[0] != 1 || scale_[1] != 1 || scale_[2] != 1) { + throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)", + name.c_str(), id); + } + model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { From a006b15958ece085b609b01fcc3da391bf5a31ff Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 8 Dec 2023 08:13:17 -0800 Subject: [PATCH 09/44] Add map from objects names to their index. Also some minor cleanup of repeated code in user_model. PiperOrigin-RevId: 589135294 Change-Id: I16f22c4dbdde961cedc042af3bdc9d3577d33459 --- src/user/user_model.cc | 148 ++++++++++++++++++++++------------------- src/user/user_model.h | 9 +++ 2 files changed, 88 insertions(+), 69 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index fcc0c354..4645644e 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -136,6 +137,7 @@ mjCModel::mjCModel() { nuser_sensor = -1; //------------------------ private variables + ids.clear(); cameras.clear(); lights.clear(); flexes.clear(); @@ -201,6 +203,7 @@ mjCModel::~mjCModel() { for (int i=0; i -static T* findobject(string name, vector& list) { - for (unsigned int i=0; iname == name) { - return list[i]; +static T* findobject(std::string_view name, const vector& list, const mjKeyMap& ids) { + // this can occur in the URDF parser + if (ids.empty()) { + for (unsigned int i=0; iname == name) { + return list[i]; + } } + return nullptr; } - return 0; + // during model compilation + auto id = ids.find(name); + if (id == ids.end()) { + return nullptr; + } + return list[id->second]; } // find object in global lists given string type and name @@ -647,49 +659,49 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) { switch (type) { case mjOBJ_BODY: case mjOBJ_XBODY: - return findobject(name, bodies); + return findobject(name, bodies, ids["body"]); case mjOBJ_JOINT: - return findobject(name, joints); + return findobject(name, joints, ids["joint"]); case mjOBJ_GEOM: - return findobject(name, geoms); + return findobject(name, geoms, ids["geom"]); case mjOBJ_SITE: - return findobject(name, sites); + return findobject(name, sites, ids["site"]); case mjOBJ_CAMERA: - return findobject(name, cameras); + return findobject(name, cameras, ids["camera"]); case mjOBJ_LIGHT: - return findobject(name, lights); + return findobject(name, lights, ids["light"]); case mjOBJ_FLEX: - return findobject(name, flexes); + return findobject(name, flexes, ids["flex"]); case mjOBJ_MESH: - return findobject(name, meshes); + return findobject(name, meshes, ids["mesh"]); case mjOBJ_SKIN: - return findobject(name, skins); + return findobject(name, skins, ids["skin"]); case mjOBJ_HFIELD: - return findobject(name, hfields); + return findobject(name, hfields, ids["hfield"]); case mjOBJ_TEXTURE: - return findobject(name, textures); + return findobject(name, textures, ids["texture"]); case mjOBJ_MATERIAL: - return findobject(name, materials); + return findobject(name, materials, ids["material"]); case mjOBJ_PAIR: - return findobject(name, pairs); + return findobject(name, pairs, ids["pair"]); case mjOBJ_EXCLUDE: - return findobject(name, excludes); + return findobject(name, excludes, ids["exclude"]); case mjOBJ_EQUALITY: - return findobject(name, equalities); + return findobject(name, equalities, ids["equality"]); case mjOBJ_TENDON: - return findobject(name, tendons); + return findobject(name, tendons, ids["tendon"]); case mjOBJ_ACTUATOR: - return findobject(name, actuators); + return findobject(name, actuators, ids["actuator"]); case mjOBJ_SENSOR: - return findobject(name, sensors); + return findobject(name, sensors, ids["sensor"]); case mjOBJ_NUMERIC: - return findobject(name, numerics); + return findobject(name, numerics, ids["numeric"]); case mjOBJ_TEXT: - return findobject(name, texts); + return findobject(name, texts, ids["text"]); case mjOBJ_TUPLE: - return findobject(name, tuples); + return findobject(name, tuples, ids["tuple"]); case mjOBJ_PLUGIN: - return findobject(name, plugins); + return findobject(name, plugins, ids["plugin"]); default: return 0; } @@ -2357,6 +2369,14 @@ void mjCModel::CopyObjects(mjModel* m) { //------------------------------- FUSE STATIC ------------------------------------------------------ +template +static void makelistid(std::vector& dest, std::vector& source) { + for (int i=0; iid = (int)dest.size(); + dest.push_back(source[i]); + } +} + // change frame to parent body static void changeframe(double childpos[3], double childquat[4], const double bodypos[3], const double bodyquat[4]) { @@ -2379,23 +2399,9 @@ void mjCModel::FuseReindex(mjCBody* body) { body->bodies[i]->id : body->weldid); } - // joints - for (int i=0; ijoints.size(); i++) { - body->joints[i]->id = (int)joints.size(); - joints.push_back(body->joints[i]); - } - - // geoms - for (int i=0; igeoms.size(); i++) { - body->geoms[i]->id = (int)geoms.size(); - geoms.push_back(body->geoms[i]); - } - - // sites - for (int i=0; isites.size(); i++) { - body->sites[i]->id = (int)sites.size(); - sites.push_back(body->sites[i]); - } + makelistid(joints, body->joints); + makelistid(geoms, body->geoms); + makelistid(sites, body->sites); // process children recursively for (int i=0; ibodies.size(); i++) { @@ -2632,7 +2638,8 @@ static void reassignid(vector& list) { // set ids, check for repeated names template -static void processlist(vector& list, string defname, bool checkrepeat=true) { +static void processlist(mjListKeyMap& ids, vector& list, + string defname, bool checkrepeat = true) { // loop over list elements for (int i=0; i<(int)list.size(); i++) { // check for incompatible id setting; SHOULD NOT OCCUR @@ -2642,6 +2649,9 @@ static void processlist(vector& list, string defname, bool checkrepeat=true) // id equals position in array list[i]->id = i; + + // add to ids map + ids[defname][list[i]->name] = i; } // check for repeated names @@ -2792,30 +2802,30 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { SetDefaultNames(textures); CheckEmptyNames(); - // set object ids and default names, check for repeated names - processlist(bodies, "body"); - processlist(joints, "joint"); - processlist(geoms, "geom"); - processlist(sites, "site"); - processlist(cameras, "camera"); - processlist(lights, "light"); - processlist(flexes, "flex"); - processlist(meshes, "mesh"); - processlist(skins, "skin"); - processlist(hfields, "hfield"); - processlist(textures, "texture"); - processlist(materials, "material"); - processlist(pairs, "pair"); - processlist(excludes, "exclude"); - processlist(equalities, "equality"); - processlist(tendons, "tendon"); - processlist(actuators, "actuator"); - processlist(sensors, "sensor"); - processlist(numerics, "numeric"); - processlist(texts, "text"); - processlist(tuples, "tuple"); - processlist(keys, "key"); - processlist(plugins, "plugin"); + // set object ids, check for repeated names + processlist(ids, bodies, "body"); + processlist(ids, joints, "joint"); + processlist(ids, geoms, "geom"); + processlist(ids, sites, "site"); + processlist(ids, cameras, "camera"); + processlist(ids, lights, "light"); + processlist(ids, flexes, "flex"); + processlist(ids, meshes, "mesh"); + processlist(ids, skins, "skin"); + processlist(ids, hfields, "hfield"); + processlist(ids, textures, "texture"); + processlist(ids, materials, "material"); + processlist(ids, pairs, "pair"); + processlist(ids, excludes, "exclude"); + processlist(ids, equalities, "equality"); + processlist(ids, tendons, "tendon"); + processlist(ids, actuators, "actuator"); + processlist(ids, sensors, "sensor"); + processlist(ids, numerics, "numeric"); + processlist(ids, texts, "text"); + processlist(ids, tuples, "tuple"); + processlist(ids, keys, "key"); + processlist(ids, plugins, "plugin"); // convert names into indices IndexAssets(); diff --git a/src/user/user_model.h b/src/user/user_model.h index f013d832..d771dd42 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_USER_USER_MODEL_H_ #define MUJOCO_SRC_USER_USER_MODEL_H_ +#include +#include #include #include #include @@ -30,6 +32,10 @@ typedef enum _mjtInertiaFromGeom { mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing } mjtInertiaFromGeom; +// TODO: convert mjListKeyMap to mjKeyMap[mjNOBJECT] by adding mjNOBJECT to mjtObj +typedef std::map > mjKeyMap; +typedef std::map > mjListKeyMap; + //---------------------------------- class mjCModel ------------------------------------------------ @@ -286,6 +292,9 @@ class mjCModel { std::vector cameras; // list of cameras std::vector lights; // list of lights + // map from object names to ids + mjListKeyMap ids; + //------------------------ internal variables bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator bool compiled; // already compiled flag (cannot be compiled again) From 7942fe957ea046750f936ac002d49425e31d9dab Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 8 Dec 2023 10:38:39 -0800 Subject: [PATCH 10/44] Circumvent bug in mjc_BoxBox. The box-box collider sometimes outputs bad contact points which are far outside one of the two boxes. This happens at configurations which are very rare numerically, but rare configurations (e.g. perfect box alignment) are often induced by the designer and the physics, making them actually not-so-rare. Since the box collision functions are very difficult to understand, we circumvent the issue by extending the post processing code to detect and remove such contacts. PiperOrigin-RevId: 589178197 Change-Id: I82b8a888ef04f9196111c960757e3314d0ab6d8e --- src/engine/engine_collision_driver.c | 36 ++- src/engine/engine_collision_primitive.h | 45 ++-- src/engine/engine_util_misc.c | 20 ++ src/engine/engine_util_misc.h | 4 + test/engine/CMakeLists.txt | 3 + test/engine/engine_collision_box_test.cc | 226 ++++++++++++++++++ .../testdata/collision_box/boxbox_bad0.xml | 36 +++ .../testdata/collision_box/boxbox_bad1.xml | 24 ++ .../collision_box/boxbox_duplicate.xml | 11 + 9 files changed, 381 insertions(+), 24 deletions(-) create mode 100644 test/engine/engine_collision_box_test.cc create mode 100644 test/engine/testdata/collision_box/boxbox_bad0.xml create mode 100644 test/engine/testdata/collision_box/boxbox_bad1.xml create mode 100644 test/engine/testdata/collision_box/boxbox_duplicate.xml diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 10920162..d42a4f2e 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1444,6 +1444,9 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], // test two geoms for collision, apply filters, add to contact list void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { + // relative distance (1%) outside of which box-box contacts are removed + static mjtNum kBoxRemoveMargin = 1.01; + TM_START; int num, type1, type2, condim; @@ -1524,16 +1527,44 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } - // remove repeated contacts in box-box + // remove bad and repeated contacts in box-box if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { // use dim field to mark: -1: bad, 0: good for (int i=0; i < num; i++) { con[i].dim = 0; } - // find bad + // get box info + const mjtNum* pos1 = d->geom_xpos + 3 * g1; + const mjtNum* mat1 = d->geom_xmat + 9 * g1; + const mjtNum* size1 = m->geom_size + 3 * g1; + const mjtNum* pos2 = d->geom_xpos + 3 * g2; + const mjtNum* mat2 = d->geom_xmat + 9 * g2; + const mjtNum* size2 = m->geom_size + 3 * g2; + + // find bad: contacts outside one of the boxes + for (int i=0; i < num; i++) { + // box sizes with margin + mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, size1[2] + margin}; + mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, size2[2] + margin}; + mju_scl3(sz1, sz1, kBoxRemoveMargin); + mju_scl3(sz2, sz2, kBoxRemoveMargin); + // mark as bad if outside box + if (mju_outsideBox(con[i].pos, pos1, mat1, sz1) || + mju_outsideBox(con[i].pos, pos2, mat2, sz2)) { + con[i].dim = -1; + } + } + + // find duplicates for (int i=0; i < num-1; i++) { + if (con[i].dim == -1) { + continue; // already marked bad: skip + } for (int j=i+1; j < num; j++) { + if (con[j].dim == -1) { + continue; // already marked bad: skip + } if (con[i].pos[0] == con[j].pos[0] && con[i].pos[1] == con[j].pos[1] && con[i].pos[2] == con[j].pos[2]) { @@ -1546,6 +1577,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // consolidate good int i = 0; for (int j=0; j < num; j++) { + // good: maybe copy if (con[j].dim == 0) { // different: copy if (i < j) { diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index 49946707..8156905e 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_PRIMITIVE_H_ #include +#include #include // define and extract geom info @@ -47,32 +48,32 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); // plane collisions -int mjc_PlaneSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // sphere and capsule collisions -int mjc_SphereSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // box collisions: from engine_collision_box.c -int mjc_CapsuleBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_BoxBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_BoxBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); #ifdef __cplusplus } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 30eb0d2a..29e2e0f8 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -837,6 +837,26 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) +// return 1 if point is outside box given by pos, mat, size +int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3]) { + // vector from pos to point, projected to box frame + mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]}; + mju_rotVecMatT(vec, vec, mat); + + // outside + if (vec[0] > size[0] || vec[0] < -size[0] || + vec[1] > size[1] || vec[1] < -size[1] || + vec[2] > size[2] || vec[2] < -size[2]) { + return 1; + } + + // inside + return 0; +} + + + // print matrix to screen void mju_printMat(const mjtNum* mat, int nr, int nc) { for (int r=0; r < nr; r++) { diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 32417f2c..c103aba1 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -78,6 +78,10 @@ MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, // integrate spring-damper analytically, return pos(dt) MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); +// return 1 if point is outside box given by pos, mat, size +MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3]); + // print matrix MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 1e083b36..fb77205d 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -12,6 +12,9 @@ # See the License for the specific language governing permissions and # 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) diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc new file mode 100644 index 00000000..0247335d --- /dev/null +++ b/test/engine/engine_collision_box_test.cc @@ -0,0 +1,226 @@ +// Copyright 2023 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. + +// Tests for engine/engine_collision_box.c. + +#include + +#include +#include +#include +#include +#include "test/fixture.h" +#include "src/engine/engine_collision_primitive.h" +#include "src/engine/engine_util_misc.h" + + +namespace mujoco { +namespace { + +using MjCollisionBoxTest = MujocoTest; +using ::testing::NotNull; + +static const char* const kBad0FilePath = + "engine/testdata/collision_box/boxbox_bad0.xml"; +static const char* const kBad1FilePath = + "engine/testdata/collision_box/boxbox_bad1.xml"; + +TEST_F(MjCollisionBoxTest, BadContacts) { + for (const char* local_path : {kBad0FilePath, kBad1FilePath}) { + const std::string xml_path = GetTestDataFilePath(local_path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num); + + // get box info + const mjtNum* pos1 = data->geom_xpos + 3 * g1; + const mjtNum* mat1 = data->geom_xmat + 9 * g1; + const mjtNum* size1 = model->geom_size + 3 * g1; + const mjtNum* pos2 = data->geom_xpos + 3 * g2; + const mjtNum* mat2 = data->geom_xmat + 9 * g2; + const mjtNum* size2 = model->geom_size + 3 * g2; + + // loop over raw contacts, find removed + for (int i = 0; i < num; i++) { + if (!match_raw[i]) { + // === check if outside + + // get margin and adjusted sizes + const mjtNum kBoxRemoveMargin = 1.01; + mjtNum sz1[3], sz2[3]; + mju_scl3(sz1, size1, kBoxRemoveMargin); + mju_scl3(sz2, size2, kBoxRemoveMargin); + + // is contact outside one of the boxes + bool outside = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1) || + mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2); + + // expect that removed contact was either outside + EXPECT_TRUE(outside); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); + } +} + +static const char* const kDuplicateFilePath = + "engine/testdata/collision_box/boxbox_duplicate.xml"; + +TEST_F(MjCollisionBoxTest, DuplicateContacts) { + const std::string xml_path = GetTestDataFilePath(kDuplicateFilePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num); + + // loop over raw contacts, find removed + for (int i = 0; i < num; i++) { + if (!match_raw[i]) { + // === check if duplicate + bool duplicate = false; + for (int j = 0; j < num; j++) { + if (duplicate || i == j) { + continue; + } + if (con_raw[i].pos[0] == con_raw[j].pos[0] && + con_raw[i].pos[1] == con_raw[j].pos[1] && + con_raw[i].pos[2] == con_raw[j].pos[2]) { + duplicate = true; + } + } + + // expect that removed contact was duplicated + EXPECT_TRUE(duplicate); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); +} + + +} // namespace +} // namespace mujoco diff --git a/test/engine/testdata/collision_box/boxbox_bad0.xml b/test/engine/testdata/collision_box/boxbox_bad0.xml new file mode 100644 index 00000000..a3e08973 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad0.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_bad1.xml b/test/engine/testdata/collision_box/boxbox_bad1.xml new file mode 100644 index 00000000..04f16290 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_duplicate.xml b/test/engine/testdata/collision_box/boxbox_duplicate.xml new file mode 100644 index 00000000..6582f5cf --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_duplicate.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + From e7d637f375d809f6a83b7e3e3d5d698151edfd56 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Fri, 8 Dec 2023 16:43:31 -0800 Subject: [PATCH 11/44] Fix issue with foot vel in MJX notebook PiperOrigin-RevId: 589276253 Change-Id: I2c8c24e42d037d8c59667364a298f8b669d779b6 --- mjx/tutorial.ipynb | 46 +++++++++++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index 16aa9c32..522b0208 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -923,7 +923,9 @@ " 'physics_steps_per_control_step', physics_steps_per_control_step)\n", " super().__init__(mj_model=mj_model, **kwargs)\n", "\n", - " self.torso_idx = 1\n", + " self.torso_idx = mujoco.mj_name2id(\n", + " mj_model, mujoco.mjtObj.mjOBJ_BODY.value, 'torso'\n", + " )\n", " self._action_scale = action_scale\n", " self._obs_noise = obs_noise\n", " self._reset_horizon = 500\n", @@ -940,6 +942,7 @@ " self.reward_config = get_config()\n", " self.lowers = self._default_ap_pose - jp.array([0.2, 0.8, 0.8] * 4)\n", " self.uppers = self._default_ap_pose + jp.array([0.2, 0.8, 0.8] * 4)\n", + " self._foot_radius = 0.014\n", "\n", " def sample_command(self, rng: jax.Array) -\u003e jax.Array:\n", " lin_vel_x = [-0.6, 1.0] # min max [m/s]\n", @@ -1023,12 +1026,15 @@ " joint_vel = qvel[6:]\n", "\n", " # foot contact data based on z-position\n", - " foot_contact = 0.017 - self._get_feet_pos_vel(x, xd)[0][:, 2]\n", - " contact = foot_contact \u003e -1e-3 # a mm or less off the floor\n", + " foot_contact_pos = (\n", + " self._get_feet_pos_vel(x, xd)[0][:, 2]\n", + " - self._foot_radius\n", + " )\n", + " contact = foot_contact_pos \u003c 1e-3 # a mm or less off the floor\n", " contact_filt_mm = jp.logical_or(contact, state.info['last_contact'])\n", " contact_filt_cm = jp.logical_or(\n", - " foot_contact \u003e -1e-2, state.info['last_contact']\n", - " )\n", + " foot_contact_pos \u003c 3e-2, state.info['last_contact']\n", + " ) # 3cm or less off the floor\n", " first_contact = (state.info['feet_air_time'] \u003e 0) * (contact_filt_mm)\n", " state.info['feet_air_time'] += self.dt\n", "\n", @@ -1100,29 +1106,26 @@ " state.info.update(rng=rng)\n", "\n", " # resetting logic if joint limits are reached or robot is falling\n", - " done = 0.0\n", " up = jp.array([0.0, 0.0, 1.0])\n", - " done = jp.where(jp.dot(math.rotate(up, x.rot[0]), up) \u003c 0, 1.0, done)\n", - " done = jp.where(jp.logical_or(\n", - " jp.any(joint_angles \u003c .98 * self.lowers),\n", - " jp.any(joint_angles \u003e .98 * self.uppers)), 1.0, done)\n", - " done = jp.where(x.pos[self.torso_idx, 2] \u003c 0.18, 1.0, done)\n", + " done = jp.dot(math.rotate(up, x.rot[0]), up) \u003c 0\n", + " done |= jp.any(joint_angles \u003c 0.98 * self.lowers)\n", + " done |= jp.any(joint_angles \u003e 0.98 * self.uppers)\n", + " done |= x.pos[0, 2] \u003c 0.18\n", "\n", " # termination reward\n", - " reward += jp.where(\n", - " (done == 1.0) \u0026 (state.info['step'] \u003c self._reset_horizon),\n", - " self.reward_config.rewards.scales.termination,\n", - " 0.0,\n", + " reward += (\n", + " done * (state.info['step'] \u003c self._reset_horizon) *\n", + " self.reward_config.rewards.scales.termination\n", " )\n", "\n", " # when done, sample new command if more than _reset_horizon timesteps\n", " # achieved\n", " state.info['command'] = jp.where(\n", - " (done == 1.0) \u0026 (state.info['step'] \u003e self._reset_horizon),\n", + " done \u0026 (state.info['step'] \u003e self._reset_horizon),\n", " self.sample_command(cmd_rng), state.info['command'])\n", " # reset the step counter when done\n", " state.info['step'] = jp.where(\n", - " (done == 1.0) | (state.info['step'] \u003e self._reset_horizon), 0,\n", + " done | (state.info['step'] \u003e self._reset_horizon), 0,\n", " state.info['step']\n", " )\n", "\n", @@ -1133,7 +1136,7 @@ "\n", " state = state.replace(\n", " pipeline_state=data, obs=obs + obs_noise, reward=reward,\n", - " done=done)\n", + " done=done * 1.0)\n", " return state\n", "\n", " def _get_obs(self, qpos: jax.Array, x: Transform, xd: Motion,\n", @@ -1233,7 +1236,8 @@ " self, x: Transform, xd: Motion) -\u003e Tuple[jax.Array, jax.Array]:\n", " offset = Transform.create(pos=self._feet_pos)\n", " pos = x.take(self._feet_index).vmap().do(offset).pos\n", - " vel = offset.vmap().do(xd.take(self._feet_index)).vel\n", + " world_offset = Transform.create(pos=pos - x.take(self._feet_index).pos)\n", + " vel = world_offset.vmap().do(xd.take(self._feet_index)).vel\n", " return pos, vel\n", "\n", " def _reward_foot_slip(\n", @@ -1405,8 +1409,8 @@ "private_outputs": true, "provenance": [ { - "file_id": "1brcF4_qCRS2ASc-QQw1rsEwl5IjzGvq2", - "timestamp": 1697763780236 + "file_id": "1QsuS7EJhdPEHxxAu9XwozvA7eb4ZnlAb", + "timestamp": 1701993737024 } ], "toc_visible": true From b22919543ddf096377bce27e4a638b779478ed54 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 9 Dec 2023 16:05:43 -0800 Subject: [PATCH 12/44] Fix bugs related to saving of values. Before this change: - User-modified values of `mjModel.stat.{meanmass, meaninertia}` would not be saved to XML. - Non-user-modified values of `mjModel.stat.{meansize, extent, center}` would always get saved to XML. After this change values are saved to XML only if provided by the user, either in the XML or by changing values in `mjModel` before saving. PiperOrigin-RevId: 589473351 Change-Id: I4dcf4892ef61c35edf424226e731232d861bad8d --- src/user/user_model.cc | 31 ++++++++++++++++++++++++++---- src/user/user_model.h | 10 +++++++++- test/xml/xml_native_writer_test.cc | 28 ++++++++++++++++++++++++++- 3 files changed, 63 insertions(+), 6 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 4645644e..c1c0300e 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -114,6 +114,16 @@ mjCModel::mjCModel() { center[0] = mjNAN; center[1] = center[2] = 0; + //------------------------ auto-computed statistics +#ifndef MEMORY_SANITIZER + // initializing as best practice, but want MSAN to catch unintialized use + meaninertia_auto = 0; + meanmass_auto = 0; + meansize_auto = 0; + extent_auto = 0; + center_auto[0] = center_auto[1] = center_auto[2] = 0; +#endif + //------------------------ engine data modelname = "MuJoCo Model"; mj_defaultOption(&option); @@ -3157,6 +3167,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // actuator lengthrange computation LengthRange(m, d); + // save automatically-computed statistics, to disambiguate when saving + extent_auto = m->stat.extent; + meaninertia_auto = m->stat.meaninertia; + meanmass_auto = m->stat.meanmass; + meansize_auto = m->stat.meansize; + copyvec(center_auto, m->stat.center, 3); + // override model statistics if defined by user if (mjuu_defined(extent)) m->stat.extent = (mjtNum)extent; if (mjuu_defined(meaninertia)) m->stat.meaninertia = (mjtNum)meaninertia; @@ -3233,10 +3250,16 @@ bool mjCModel::CopyBack(const mjModel* m) { option = m->opt; visual = m->vis; - // runtime-modifiable members of mjStatistic - meansize = m->stat.meansize; - extent = m->stat.extent; - mju_copy3(center, m->stat.center); + // runtime-modifiable members of mjStatistic, if different from computed values + if (m->stat.meaninertia != meaninertia_auto) meaninertia = m->stat.meaninertia; + if (m->stat.meanmass != meanmass_auto) meanmass = m->stat.meanmass; + if (m->stat.meansize != meansize_auto) meansize = m->stat.meansize; + if (m->stat.extent != extent_auto) extent = m->stat.extent; + if (m->stat.center[0] != center_auto[0] || + m->stat.center[1] != center_auto[1] || + m->stat.center[2] != center_auto[2]) { + mju_copy3(center, m->stat.center); + } // qpos0, qpos_spring for (int i=0; i cameras; // list of cameras std::vector lights; // list of lights + //------------------------ internal variables + + // statistics, as computed by mj_setConst + double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst + double meanmass_auto; // mean body mass, as computed by mj_setConst + double meansize_auto; // mean body size, as computed by mj_setConst + double extent_auto; // spatial extent, as computed by mj_setConst + double center_auto[3]; // center of model, as computed by mj_setConst + // map from object names to ids mjListKeyMap ids; - //------------------------ internal variables bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator bool compiled; // already compiled flag (cannot be compiled again) mjCError errInfo; // last error info diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 52b21923..f5989279 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1135,7 +1135,7 @@ using DecompilerTest = MujocoTest; TEST_F(DecompilerTest, SavesStatitics) { static constexpr char xml[] = R"( - + )"; mjModel* model = LoadModelFromString(xml); @@ -1145,10 +1145,36 @@ TEST_F(DecompilerTest, SavesStatitics) { model->stat.center[0] = 9; model->stat.center[1] = 10; model->stat.center[2] = 11; + model->stat.meanmass = 12; + model->stat.meaninertia = 13; std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("meansize=\"7\"")); EXPECT_THAT(saved_xml, HasSubstr("extent=\"8\"")); EXPECT_THAT(saved_xml, HasSubstr("center=\"9 10 11\"")); + EXPECT_THAT(saved_xml, HasSubstr("meanmass=\"12\"")); + EXPECT_THAT(saved_xml, HasSubstr("meaninertia=\"13\"")); + mj_deleteModel(model); +} + +TEST_F(DecompilerTest, DoesntSaveInferredStatitics) { + static constexpr char xml[] = R"( + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, Not(HasSubstr("meansize"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("meanmass"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("meaninertia"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("center"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("extent"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("statistic"))); mj_deleteModel(model); } From 1885d518ebf80b6fe6c2ba595be06027ce7306da Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 10 Dec 2023 03:26:49 -0800 Subject: [PATCH 13/44] Use the string `(rot:lin)` in mjdata.h to clarify where 6D motion vectors use the Featherstone convention. Also add clarifying text to the appropriate note in the documentation. Fixes #982. PiperOrigin-RevId: 589556339 Change-Id: I3e7ff185a089715a386694321d4b89d09d481cf9 --- doc/APIreference/APItypes.rst | 6 +++--- doc/includes/references.h | 6 +++--- include/mujoco/mjdata.h | 6 +++--- introspect/structs.py | 6 +++--- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 684e810b..ee8de505 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1219,9 +1219,9 @@ a frame at the center-of-mass of the local kinematic subtree (``mjData.subtree_c This choice increases the precision of kinematic computations for mechanisms that are distant from the global origin. ``cdof``: - These 6D motion vectors describe the instantaneous axis of a degree-of-freedom and are used by all Jacobian functions. - Therefore, the minimal computation required for analytic Jacobians is :ref:`mj_kinematics` followed by - :ref:`mj_comPos`. + These 6D motion vectors (3 rotation, 3 translation) describe the instantaneous axis of a degree-of-freedom and are + used by all Jacobian functions. The minimal computation required for analytic Jacobians is :ref:`mj_kinematics` + followed by :ref:`mj_comPos`. ``cinert``: These 10-vectors describe the inertial properties of a body in the c-frame and are used by the Composite Rigid Body diff --git a/doc/includes/references.h b/doc/includes/references.h index f4aedde9..79302849 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -237,7 +237,7 @@ struct mjData_ { // computed by mj_fwdPosition/mj_comPos mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) - mjtNum* cdof; // com-based motion axis of each dof (nv x 6) + mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) mjtNum* cinert; // com-based body inertia and mass (nbody x 10) // computed by mj_fwdPosition/mj_flex @@ -285,8 +285,8 @@ struct mjData_ { mjtNum* actuator_velocity; // actuator velocities (nu x 1) // computed by mj_fwdVelocity/mj_comVel - mjtNum* cvel; // com-based velocity [3D rot; 3D tran] (nbody x 6) - mjtNum* cdof_dot; // time-derivative of cdof (nv x 6) + mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) // computed by mj_fwdVelocity/mj_rne (without acceleration) mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index a3609b8b..1fc6fe19 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -265,7 +265,7 @@ struct mjData_ { // computed by mj_fwdPosition/mj_comPos mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) - mjtNum* cdof; // com-based motion axis of each dof (nv x 6) + mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) mjtNum* cinert; // com-based body inertia and mass (nbody x 10) // computed by mj_fwdPosition/mj_flex @@ -313,8 +313,8 @@ struct mjData_ { mjtNum* actuator_velocity; // actuator velocities (nu x 1) // computed by mj_fwdVelocity/mj_comVel - mjtNum* cvel; // com-based velocity [3D rot; 3D tran] (nbody x 6) - mjtNum* cdof_dot; // time-derivative of cdof (nv x 6) + mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) // computed by mj_fwdVelocity/mj_rne (without acceleration) mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) diff --git a/introspect/structs.py b/introspect/structs.py index a1fa3159..eea7f6cc 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4493,7 +4493,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='com-based motion axis of each dof (nv x 6)', # pylint: disable=line-too-long + doc='com-based motion axis of each dof (rot:lin) (nv x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='cinert', @@ -4703,14 +4703,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='com-based velocity [3D rot; 3D tran] (nbody x 6)', # pylint: disable=line-too-long + doc='com-based velocity (rot:lin) (nbody x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='cdof_dot', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='time-derivative of cdof (nv x 6)', # pylint: disable=line-too-long + doc='time-derivative of cdof (rot:lin) (nv x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='qfrc_bias', From 07e05b4c55c0a179e5a62e4aa7af67ca4e6e8cb5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 11 Dec 2023 08:41:07 -0800 Subject: [PATCH 14/44] Fix typo. Fixes #1221 PiperOrigin-RevId: 589835888 Change-Id: Iacedbe642709d5fbd43b7f0f1f9daa6447ce6155 --- doc/XMLreference.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 2f423abf..6ca8c13b 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -172,7 +172,7 @@ This element does not strictly belong to MJCF. Instead it is a meta-element, use files in a single document object model (DOM) before parsing. The included file must be a valid XML file with a unique top-level element. This top-level element is removed by the parser, and the elements below it are inserted at the location of the :el:`include` element. At least one element must be inserted as a result of this procedure. The -:el:`include` element can be used where ever an XML element is expected in the MJFC file. Nested includes are allowed, +:el:`include` element can be used where ever an XML element is expected in the MJCF file. Nested includes are allowed, however a given XML file can be included at most once in the entire model. After all the included XML files have been assembled into a single DOM, it must correspond to a valid MJCF model. Other than that, it is up to the user to decide how to use includes and how to modularize large files if desired. From aed4254d65d54d61a1d357df62c917ee4ec390a1 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 12 Dec 2023 03:41:29 -0800 Subject: [PATCH 15/44] Fix the table formatting in plugin/actuator/README.md. The line continuation format that was used is not supported on GitHub. PiperOrigin-RevId: 590141362 Change-Id: I0ab0dabfb8977bdf79682476376dda435787eba0 --- plugin/actuator/README.md | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/plugin/actuator/README.md b/plugin/actuator/README.md index 80676921..9139aed1 100644 --- a/plugin/actuator/README.md +++ b/plugin/actuator/README.md @@ -38,16 +38,10 @@ You can use it like: The available options are: -|Attribute|Default |Meaning | -|---------|--------|-------------------------------------------------------------------------------------------------------------------------------------| -|`kp` |0 |**P** gain for the controller. | -|`ki` |0 |**I** gain for the controller. | -: : : : -: : :If nonzero, one activation variable will be added to `mjData.act`, containing the current I term (in units of force). : -|`kd` |0 |**D** gain for the controller. | -|`imax` |Optional|If specified, the force produced by the I term will be clipped to the range `[-imax, -imax]`. | -|`slewmax`|Optional|The maximum rate at which the setpoint for the PID controller can change. | -: : : : -: : :If a bigger change is requested between two timesteps, it will be clipped to the range `[ctrl - slewmax * dt, ctrl + slewmax * dt]` : -: : : : -: : :If specified, one activation variable will be added to `mjData.act` containing the previous value of `ctrl`. : +|Attribute | Default | Meaning | +|----------|---------|---------| +|`kp` | 0 | **P** gain for the controller. | +|`ki` | 0 | **I** gain for the controller.

If nonzero, one activation variable will be added to `mjData.act`, containing the current I term (in units of force). | +|`kd` | 0 | **D** gain for the controller. | +|`imax` | Optional | If specified, the force produced by the I term will be clipped to the range `[-imax, -imax]`. | +|`slewmax` | Optional | The maximum rate at which the setpoint for the PID controller can change.

If a bigger change is requested between two timesteps, it will be clipped to the range `[ctrl - slewmax * dt, ctrl + slewmax * dt]`

If specified, one activation variable will be added to `mjData.act` containing the previous value of `ctrl`. | From 64c33cefcfb6edfcaf424927615621d94ad39d21 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 12 Dec 2023 03:55:19 -0800 Subject: [PATCH 16/44] Correct formula for `solref` direct stiffness. Fixes #732. PiperOrigin-RevId: 590144795 Change-Id: I94def777c7456243509374caecea2a335859f1f2 --- doc/modeling.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/modeling.rst b/doc/modeling.rst index c43b9a04..681534cd 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -403,7 +403,7 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math: .. math:: \begin{aligned} b &= \text{damping} / d_\text{width} \\ - k &= \text{stiffness} / d_\text{width}^2 \\ + k &= \text{stiffness} \cdot d(r) / d_\text{width}^2 \\ \end{aligned} .. tip:: From 28a9e6d9a28afdf303a20415d7946f9362f78cd5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 12 Dec 2023 05:13:54 -0800 Subject: [PATCH 17/44] Add `mjNOBJECT` value to `mjtObj` enum. PiperOrigin-RevId: 590164716 Change-Id: I500741aa4fb97fe2c4f5d2086188bb2c9fdfd1a0 --- doc/includes/references.h | 4 +++- include/mujoco/mjmodel.h | 4 +++- introspect/enums.py | 1 + src/engine/engine_io.c | 2 ++ unity/Runtime/Bindings/MjBindings.cs | 1 + 5 files changed, 10 insertions(+), 2 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 79302849..c89a4363 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -570,7 +570,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_TEXT, // text mjOBJ_TUPLE, // tuple mjOBJ_KEY, // keyframe - mjOBJ_PLUGIN // plugin instance + mjOBJ_PLUGIN, // plugin instance + + mjNOBJECT // number of object types } mjtObj; typedef enum mjtConstraint_ { // type of constraint mjCNSTR_EQUALITY = 0, // equality constraint diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 663e191f..49312dd1 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -246,7 +246,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_TEXT, // text mjOBJ_TUPLE, // tuple mjOBJ_KEY, // keyframe - mjOBJ_PLUGIN // plugin instance + mjOBJ_PLUGIN, // plugin instance + + mjNOBJECT // number of object types } mjtObj; diff --git a/introspect/enums.py b/introspect/enums.py index bb039f65..4982cf36 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -265,6 +265,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjOBJ_TUPLE', 23), ('mjOBJ_KEY', 24), ('mjOBJ_PLUGIN', 25), + ('mjNOBJECT', 26), ]), )), ('mjtConstraint', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 32994e9c..ca55358a 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1858,6 +1858,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) { return m->nkey; case mjOBJ_PLUGIN: return m->nplugin; + case mjNOBJECT: + return -2; } return -2; } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a17a2694..91bf8765 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -301,6 +301,7 @@ public enum mjtObj : int{ mjOBJ_TUPLE = 23, mjOBJ_KEY = 24, mjOBJ_PLUGIN = 25, + mjNOBJECT = 26, } public enum mjtConstraint : int{ mjCNSTR_EQUALITY = 0, From 35f095d7c5a644bf82c827513d87c7fc4afc5308 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 12 Dec 2023 06:42:40 -0800 Subject: [PATCH 18/44] When paused in `simulate`, add index of history scrubber if not 0. PiperOrigin-RevId: 590184966 Change-Id: Id0914c41a1339cf71ebdea02ec486c52c3d37179 --- simulate/simulate.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 0b2a150a..db0821d2 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -2476,7 +2476,14 @@ void Simulate::Render() { // show pause/loading label if (!this->run || this->loadrequest) { - const char* label = this->loadrequest ? "LOADING..." : "PAUSE"; + char label[30] = {'\0'}; + if (this->loadrequest) { + std::snprintf(label, sizeof(label), "LOADING..."); + } if (this->scrub_index == 0) { + std::snprintf(label, sizeof(label), "PAUSE"); + } else { + std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index); + } mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, label, nullptr, &this->platform_ui->mjr_context()); } From e3df84ec3fd157d6bb28a206791a4d5f3436d91c Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 12 Dec 2023 08:40:20 -0800 Subject: [PATCH 19/44] Include the size limit in error message from mjr_addAux. This is to help debug #1274. PiperOrigin-RevId: 590215990 Change-Id: I356675be84f9cb2892d31d4dc7639adf49fe4a0f --- src/render/render_context.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/render/render_context.c b/src/render/render_context.c index afb1377f..ec349b2a 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -1675,8 +1675,17 @@ void mjr_addAux(int index, int width, int height, int samples, mjrContext* con) // check max size int maxSize = 0; glGetIntegerv(GL_MAX_RENDERBUFFER_SIZE, &maxSize); - if (width > maxSize || height > maxSize) { - mju_error("Auxiliary buffer size exceeds maximum allowed by OpenGL implementation"); + if (width > maxSize) { + mju_error( + "Auxiliary buffer width exceeds maximum allowed by OpenGL " + "implementation: %d > %d", + width, maxSize); + } + if (height > maxSize) { + mju_error( + "Auxiliary buffer height exceeds maximum allowed by OpenGL " + "implementation: %d > %d", + height, maxSize); } // clamp samples request From 53883ce86a7da0f7df7016c009d4ce79eefc283e Mon Sep 17 00:00:00 2001 From: Kevin Zakka Date: Tue, 12 Dec 2023 08:58:49 -0800 Subject: [PATCH 20/44] Add a `close()` method to `mujoco.Renderer` which explicitly frees the gl context. PiperOrigin-RevId: 590221484 Change-Id: I07392f9c53f662a37a57ab23acfef9aa5a98a49b --- python/mujoco/cgl/__init__.py | 19 +++--- python/mujoco/glfw/__init__.py | 2 +- python/mujoco/renderer.py | 37 ++++++++++++ python/mujoco/renderer_test.py | 104 ++++++++++++++++----------------- 4 files changed, 100 insertions(+), 62 deletions(-) diff --git a/python/mujoco/cgl/__init__.py b/python/mujoco/cgl/__init__.py index ba1e36a6..36037116 100644 --- a/python/mujoco/cgl/__init__.py +++ b/python/mujoco/cgl/__init__.py @@ -57,15 +57,18 @@ class GLContext: def free(self): """Frees resources associated with this context.""" - if self._context: - cgl.CGLUnlockContext(self._context) - cgl.CGLSetCurrentContext(None) - cgl.CGLReleaseContext(self._context) - self._context = None + try: + if self._context: + cgl.CGLUnlockContext(self._context) + cgl.CGLSetCurrentContext(None) + cgl.CGLReleaseContext(self._context) + self._context = None - if self._pix: - cgl.CGLReleasePixelFormat(self._pix) - self._context = None + if self._pix: + cgl.CGLReleasePixelFormat(self._pix) + self._pix = None + except Exception: # pylint: disable=broad-exception-caught + pass def __del__(self): self.free() diff --git a/python/mujoco/glfw/__init__.py b/python/mujoco/glfw/__init__.py index a5693bec..0fd8bd9a 100644 --- a/python/mujoco/glfw/__init__.py +++ b/python/mujoco/glfw/__init__.py @@ -35,7 +35,7 @@ class GLContext: if glfw.get_current_context() == self._context: glfw.make_context_current(None) glfw.destroy_window(self._context) - self._context = None + self._context = None def __del__(self): self.free() diff --git a/python/mujoco/renderer.py b/python/mujoco/renderer.py index f34c8243..dfb387c6 100644 --- a/python/mujoco/renderer.py +++ b/python/mujoco/renderer.py @@ -136,6 +136,9 @@ the clause: A new numpy array holding the pixels with shape `(H, W)` or `(H, W, 3)`, depending on the value of `self._depth_rendering` unless `out is None`, in which case a reference to `out` is returned. + + Raises: + RuntimeError: if this method is called after the close method. """ original_flags = self._scene.flags.copy() @@ -145,6 +148,8 @@ the clause: self._scene.flags[_enums.mjtRndFlag.mjRND_SEGMENT] = True self._scene.flags[_enums.mjtRndFlag.mjRND_IDCOLOR] = True + if self._gl_context is None: + raise RuntimeError('render cannot be called after close.') self._gl_context.make_current() if self._depth_rendering: @@ -288,3 +293,35 @@ the clause: camera, _enums.mjtCatBit.mjCAT_ALL.value, self._scene, ) + + def close(self) -> None: + """Frees the resources used by the renderer. + + This method can be used directly: + + ```python + renderer = Renderer(...) + # Use renderer. + renderer.close() + ``` + + or via a context manager: + + ```python + with Renderer(...) as renderer: + # Use renderer. + ``` + """ + if self._gl_context: + self._gl_context.free() + self._gl_context = None + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + del exc_type, exc_value, traceback # Unused. + self.close() + + def __del__(self) -> None: + self.close() diff --git a/python/mujoco/renderer_test.py b/python/mujoco/renderer_test.py index 8aa44a97..e8022006 100644 --- a/python/mujoco/renderer_test.py +++ b/python/mujoco/renderer_test.py @@ -33,10 +33,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, r'camera "b" does not exist'): - renderer.update_scene(data, 'b') + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, r'camera "b" does not exist'): + renderer.update_scene(data, 'b') def test_renderer_camera_under_range(self): xml = """ @@ -48,10 +48,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, '-2 is out of range'): - renderer.update_scene(data, -2) + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, '-2 is out of range'): + renderer.update_scene(data, -2) def test_renderer_camera_over_range(self): xml = """ @@ -63,10 +63,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, '1 is out of range'): - renderer.update_scene(data, 1) + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, '1 is out of range'): + renderer.update_scene(data, 1) def test_renderer_renders_scene(self): xml = """ @@ -79,19 +79,19 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - renderer.update_scene(data, 'closeup') + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + renderer.update_scene(data, 'closeup') - pixels = renderer.render().flatten() - not_all_black = False + pixels = renderer.render().flatten() + not_all_black = False - # Pixels should all be a neutral color. - for pixel in pixels: - if pixel > 0: - not_all_black = True - break - self.assertTrue(not_all_black) + # Pixels should all be a neutral color. + for pixel in pixels: + if pixel > 0: + not_all_black = True + break + self.assertTrue(not_all_black) def test_renderer_output_without_out(self): xml = """ @@ -105,25 +105,25 @@ class MuJoCoRendererTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) mujoco.mj_forward(model, data) - renderer = mujoco.Renderer(model, 50, 50) - renderer.update_scene(data, 'closeup') - pixels = [renderer.render()] - - colors = ( - (1.0, 0.0, 0.0, 1.0), - (0.0, 1.0, 0.0, 1.0), - (0.0, 0.0, 1.0, 1.0), - ) - - for i, color in enumerate(colors): - model.geom_rgba[0, :] = color - mujoco.mj_forward(model, data) + with mujoco.Renderer(model, 50, 50) as renderer: renderer.update_scene(data, 'closeup') - pixels.append(renderer.render()) - self.assertIsNot(pixels[-2], pixels[-1]) + pixels = [renderer.render()] - # Pixels should change over steps. - self.assertFalse((pixels[i + 1] == pixels[i]).all()) + colors = ( + (1.0, 0.0, 0.0, 1.0), + (0.0, 1.0, 0.0, 1.0), + (0.0, 0.0, 1.0, 1.0), + ) + + for i, color in enumerate(colors): + model.geom_rgba[0, :] = color + mujoco.mj_forward(model, data) + renderer.update_scene(data, 'closeup') + pixels.append(renderer.render()) + self.assertIsNot(pixels[-2], pixels[-1]) + + # Pixels should change over steps. + self.assertFalse((pixels[i + 1] == pixels[i]).all()) def test_renderer_output_with_out(self): xml = """ @@ -139,23 +139,21 @@ class MuJoCoRendererTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) mujoco.mj_forward(model, data) - renderer = mujoco.Renderer(model, *render_size) - renderer.update_scene(data, 'closeup') + with mujoco.Renderer(model, *render_size) as renderer: + renderer.update_scene(data, 'closeup') - self.assertTrue(np.all(render_out == 0)) + self.assertTrue(np.all(render_out == 0)) - pixels = renderer.render(out=render_out) + pixels = renderer.render(out=render_out) - # Pixels should always refer to the same `render_out` array. - self.assertIs(pixels, render_out) - self.assertFalse(np.all(render_out == 0)) + # Pixels should always refer to the same `render_out` array. + self.assertIs(pixels, render_out) + self.assertFalse(np.all(render_out == 0)) - failing_render_size = (10, 10) - self.assertNotEqual(failing_render_size, render_size) - with self.assertRaises(ValueError): - pixels = renderer.render( - out=np.zeros((*failing_render_size, 3), np.uint8) - ) + failing_render_size = (10, 10) + self.assertNotEqual(failing_render_size, render_size) + with self.assertRaises(ValueError): + renderer.render(out=np.zeros((*failing_render_size, 3), np.uint8)) if __name__ == '__main__': From fcebdf4d0962b9b2cc5071c52cbd7a775ea084d0 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Tue, 12 Dec 2023 10:36:48 -0800 Subject: [PATCH 21/44] Update MuJoCo to v3.1.0 for release. PiperOrigin-RevId: 590256664 Change-Id: I42ad788d609dde0ff00b13a2e456b2a8d3d9e952 --- 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 c9723a29..bfc599f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.0.2 + VERSION 3.1.0 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index e52e3530..484597a9 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,0,2,0 -PRODUCTVERSION 3,0,2,0 +FILEVERSION 3,1,0,0 +PRODUCTVERSION 3,1,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.0.2" + VALUE "ProductVersion", "3.1.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.0.2" + VALUE "FileVersion", "3.1.0" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 4835c979..ae73084e 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,0,2,0 -PRODUCTVERSION 3,0,2,0 +FILEVERSION 3,1,0,0 +PRODUCTVERSION 3,1,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.0.2" + VALUE "ProductVersion", "3.1.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.0.2" + VALUE "FileVersion", "3.1.0" 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 33c5401c..3101f9b2 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -522,7 +522,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`` - - 302 + - 310 - 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 457c2cd7..f3a97af7 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.0.2.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.0.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.0.2/lib/libmujoco.so.3.0.2`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.1.0/lib/libmujoco.so.3.1.0`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 5f32cbc7..2935d5a8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -24,7 +24,7 @@ extern "C" { #endif // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 302 +#define mjVERSION_HEADER 310 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index a385e9af..d615108d 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.0.2" +version = "3.1.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -31,13 +31,13 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.0.2.dev0", + "mujoco>=3.1.0.dev0", "scipy", "trimesh", ] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.0.2" +Documentation = "https://mujoco.readthedocs.io/en/3.1.0" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.0.2/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.0/changelog.html" diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 256adb10..b4dc3f28 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.0.2.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.0.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.0.2 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.1.0 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 6c6d68b2..18a27c2f 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.0.2 + 3.1.0 CFBundleGetInfoString - 3.0.2 + 3.1.0 CFBundleLongVersionString - 3.0.2 + 3.1.0 CFBundleShortVersionString - 3.0.2 + 3.1.0 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 81b7da8e..6f934c1d 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.0.2" +version = "3.1.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.0.2" +Documentation = "https://mujoco.readthedocs.io/en/3.1.0" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.0.2/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.0/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 3dceb85c..0e989480 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.0.2 + VERSION 3.1.0 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 464fc4c5..e16584e0 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.0.2 + VERSION 3.1.0 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index b223a3e2..f887d1ca 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -38,8 +38,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 302 -#define mjVERSIONSTRING "3.0.2" + #define mjVERSION 310 +#define mjVERSIONSTRING "3.1.0" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index f670431b..8ed2ac7a 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.0.2.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.1.0.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.0.2/lib/libmujoco.so.3.0.2", + "/.mujoco/mujoco-3.1.0/lib/libmujoco.so.3.1.0", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 91bf8765..6f797926 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -108,7 +108,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 = 302; +public const int mjVERSION_HEADER = 310; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 9c093316..f8f658d7 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.0.2", + "version": "3.1.0", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From 89d15c48e8b2f41b4f909767dd0ab569edabbf35 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Tue, 12 Dec 2023 10:41:28 -0800 Subject: [PATCH 22/44] Update version number in third_party/mujoco/doc/changelog.rst for v3.1.0 PiperOrigin-RevId: 590258202 Change-Id: I1220eb48e7c3871fdf785cf74819aca12d0c75c0 --- doc/changelog.rst | 52 +++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 56904ed8..5a90b5f6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,48 +2,48 @@ Changelog ========= -Upcoming version (not yet released) +Version 3.1.0 (December 12, 2023) ----------------------------------- General ^^^^^^^ -- Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function - for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more - robust for very small or large geom sizes. -- Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation - on its direct children, without requiring a :ref:`body`. -- Added the :at:`kv` attribute to the :ref:`position` and :ref:`intvelocity` - actuators, for specifying actuator-applied damping. This can be used to implement a PD controller with 0 reference - velocity. When using this attribute, it is recommended to use the implicitfast or implicit - :ref:`integrators`. +1. Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function + for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more + robust for very small or large geom sizes. +2. Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation + on its direct children, without requiring a :ref:`body`. +3. Added the :at:`kv` attribute to the :ref:`position` and :ref:`intvelocity` + actuators, for specifying actuator-applied damping. This can be used to implement a PD controller with 0 reference + velocity. When using this attribute, it is recommended to use the implicitfast or implicit + :ref:`integrators`. Plugins ^^^^^^^ -- Allow actuator plugins to use activation variables in ``mjData.act`` as their internal state, rather than - ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation - variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. +4. Allow actuator plugins to use activation variables in ``mjData.act`` as their internal state, rather than + ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation + variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. -- Added the `pid `__ actuator plugin, a - configurable PID controller that implements the Integral term, which is not available with native MuJoCo actuators. +5. Added the `pid `__ actuator plugin, a + configurable PID controller that implements the Integral term, which is not available with native MuJoCo actuators. MJX ^^^ -- Added ``site_xpos`` and ``site_xmat`` to MJX. -- Added ``put_data``, ``put_model``, ``get_data`` to replace ``device_put`` and ``device_get_into``, which will be - deprecated. These new functions correctly translate fields that are the result of intermediate calculations such as - ``efc_J``. +6. Added ``site_xpos`` and ``site_xmat`` to MJX. +7. Added ``put_data``, ``put_model``, ``get_data`` to replace ``device_put`` and ``device_get_into``, which will be + deprecated. These new functions correctly translate fields that are the result of intermediate calculations such as + ``efc_J``. Bug fixes ^^^^^^^^^ -- Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. - Before this fix such actuators could lead to non-conservation of momentum. -- Fix bug that prevented using flex with the :ref:`passive viewer`. -- Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. -- Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum - supported version is still 11.0, but we release these wheels to fix compatibility for those users. See - :github:issue:`1213`. +8. Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. + Before this fix such actuators could lead to non-conservation of momentum. +9. Fix bug that prevented using flex with the :ref:`passive viewer`. +10. Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. +11. Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum + supported version is still 11.0, but we release these wheels to fix compatibility for those users. See + :github:issue:`1213`. Version 3.0.1 (November 15, 2023) --------------------------------- From 0df1c9c733ea0cc90ba392ce21050914f53832ef Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Tue, 12 Dec 2023 13:16:54 -0800 Subject: [PATCH 23/44] Updated version number in cmakelist, mujcodepencies.cmake for v 3.1.0 PiperOrigin-RevId: 590312529 Change-Id: Ie4fb2a76f88d5032c53365995a2bb7b691485e96 --- cmake/MujocoDependencies.cmake | 4 ++-- python/mujoco/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 87bb13a7..86c860a2 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 - aa6964bf3a34fd607837dd8123bc42465185c4f8 + 454f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 CACHE STRING "Version of `Eigen3` to be fetched." ) @@ -54,7 +54,7 @@ set(MUJOCO_DEP_VERSION_gtest ) set(MUJOCO_DEP_VERSION_benchmark - 344117638c8ff7e239044fd0fa7085839fc03021 # v1.8.3 + e45585a4b8e75c28479fa4107182c28172799640 # v1.8.3 CACHE STRING "Version of `benchmark` to be fetched." ) diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index b4dc3f28..1575f33b 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 - aa6964bf3a34fd607837dd8123bc42465185c4f8 + 54f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL From 0d37670698b49e78594c635b7f54682ce51ec95d Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Tue, 12 Dec 2023 14:21:49 -0800 Subject: [PATCH 24/44] Fix eigen3 commit hash. PiperOrigin-RevId: 590335535 Change-Id: I90819d420e5ef54dfc5b202ce0407e1ea833a8a4 --- python/mujoco/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 1575f33b..c9ed748e 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 - 54f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 + 454f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL From b82c382c53d866b391fde85914ee172634270ef5 Mon Sep 17 00:00:00 2001 From: Tyler Lindberg Date: Tue, 12 Dec 2023 16:28:36 -0800 Subject: [PATCH 25/44] Use positional arguments with jp.where --- mjx/tutorial.ipynb | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index 522b0208..f27461c6 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -416,10 +416,8 @@ " forward_reward = self._forward_reward_weight * velocity[0]\n", "\n", " min_z, max_z = self._healthy_z_range\n", - " is_healthy = jp.where(data.qpos[2] \u003c min_z, x=0.0, y=1.0)\n", - " is_healthy = jp.where(\n", - " data.qpos[2] \u003e max_z, x=0.0, y=is_healthy\n", - " )\n", + " is_healthy = jp.where(data.qpos[2] \u003c min_z, 0.0, 1.0)\n", + " is_healthy = jp.where(data.qpos[2] \u003e max_z, 0.0, is_healthy)\n", " if self._terminate_when_unhealthy:\n", " healthy_reward = self._healthy_reward\n", " else:\n", From 5b2c98f8f1fa7b0ddbce34ab7c5382bf52f41901 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 13 Dec 2023 13:12:29 -0800 Subject: [PATCH 26/44] When saving XMLs, don't round floats that are bigger than INT_MAX. Fixes #1278 PiperOrigin-RevId: 590692020 Change-Id: Icd11d20f47bc4163eb4378f4cb031b8fd9d13612 --- src/xml/xml_util.cc | 4 +++- test/xml/xml_native_writer_test.cc | 24 ++++++++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 0d1a7f75..d7a09afc 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -1004,7 +1005,8 @@ void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, con } // append number - if (isint(data[i])) { + double doubledata = static_cast(data[i]); + if (doubledata < INT_MAX && doubledata > -INT_MAX && isint(data[i])) { stream << Round(data[i]); } else { stream << data[i]; diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index f5989279..4b5b7f9e 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1178,5 +1178,29 @@ TEST_F(DecompilerTest, DoesntSaveInferredStatitics) { mj_deleteModel(model); } +TEST_F(DecompilerTest, VeryLargeNumbers) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model); + // note, focal is float and loses precision 16777217 -> 16777216 + EXPECT_THAT(saved_xml, HasSubstr("focal=\"16777216 1\"")); + EXPECT_THAT(saved_xml, HasSubstr("pos=\"1e+20 0 0\"")); + EXPECT_THAT(saved_xml, HasSubstr("range=\"-1e+10 1e+10\"")); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From 1c3455363179c86ec03f4f01ba47d085f3b4a3db Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 13 Dec 2023 13:13:12 -0800 Subject: [PATCH 27/44] Add shortcuts for power function in impedance computation. PiperOrigin-RevId: 590692245 Change-Id: I1a49475c9a8299f4845e4ec33c9a7ce21a9c5d2b --- src/engine/engine_core_constraint.c | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d1cb4a6c..fbb8ff9a 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1325,6 +1325,19 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int +// return a to the power of b, quick return for powers 1 and 2 +// solimp[4] == 2 is the default, so these branches are common +static mjtNum power(mjtNum a, mjtNum b) { + if (b == 1) { + return a; + } else if (b == 2) { + return a*a; + } + return mju_pow(a, b); +} + + + // compute impedance and derivative for one constraint static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, mjtNum* imp, mjtNum* impP) { @@ -1359,16 +1372,16 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, // y(x) = a*x^p if x<=midpoint else if (x <= solimp[3]) { - mjtNum a = 1/mju_pow(solimp[3], solimp[4]-1); - y = a*mju_pow(x, solimp[4]); - yP = solimp[4] * a*mju_pow(x, solimp[4]-1); + mjtNum a = 1/power(solimp[3], solimp[4]-1); + y = a*power(x, solimp[4]); + yP = solimp[4] * a*power(x, solimp[4]-1); } // y(x) = 1-b*(1-x)^p is x>midpoint else { - mjtNum b = 1/mju_pow(1-solimp[3], solimp[4]-1); - y = 1-b*mju_pow(1-x, solimp[4]); - yP = solimp[4] * b*mju_pow(1-x, solimp[4]-1); + mjtNum b = 1/power(1-solimp[3], solimp[4]-1); + y = 1-b*power(1-x, solimp[4]); + yP = solimp[4] * b*power(1-x, solimp[4]-1); } // scale From 537d649e95d5de559786ae55a0f3bb83cffd1426 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 13 Dec 2023 13:21:43 -0800 Subject: [PATCH 28/44] Improvements to transmission documentation. PiperOrigin-RevId: 590694542 Change-Id: I9dc25655116c9300061095c2a2b701530bbbbeca --- doc/computation/index.rst | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 8afdb7cf..788cdd14 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -300,19 +300,22 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, Slider-cranks can also be modeled explicitly by creating MuJoCo bodies and coupling them with equality constraints to the rest of the system, but that would be less efficient. -:at:`site` - :at:`site` transmission (without a :at:`refsite`, see below) and :at:`body` transmission targets have a fixed zero - length :math:`l_i(q) = 0`. They can therefore not be used to maintain a desired length, but can be used to apply - forces. Site transmissions correspond to applying a Cartsian force/torque at the site, and are useful for modeling - jets and propellors. :el:`body` transmissions correspond to applying forces at contact points belonging to a body, in +:at:`body` + :el:`body` transmission corresponds to applying forces at contact points belonging to a body, in order to model vacuum grippers and biomechanical adhesive appendages. For more information about adhesion, see the - :ref:`adhesion` actuator documentation. + :ref:`adhesion` actuator documentation. These transmission targets have a fixed zero length + :math:`l_i(q) = 0`. - If a :at:`site` transmission target is defined with the optional :at:`refsite` attribute, forces and torques are - applied in the frame of the reference site rather than the site's own frame. If a reference site is defined then - the length of the actuator is nonzero and corresponds to the pose difference of the two sites. This length can then - be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the - :ref:`refsite` documentation for more details. +:at:`site` + Site transmissions correspond to applying a Cartsian force/torque in the frame of a site, and are useful for + modeling jets and propellors. When a :at:`refsite` is not defined (see below), these targets have a fixed zero + length :math:`l_i(q) = 0`. + + If a :at:`site` transmission is defined with the optional :at:`refsite` attribute, forces and torques are applied in + the frame of the reference site rather than the site's own frame. If a reference site is defined, the length of the + actuator is nonzero and corresponds to the pose difference of the two sites, projected onto a chosen direction in the + reference frame. This length can then be controlled with a :el:`position` actuator, allowing for Cartesian + end-effector control. See the :ref:`refsite` documentation for more details. .. _geActivation: From c2c76195dd5c706e4427915b0aa374df4f13e36e Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Wed, 13 Dec 2023 14:25:10 -0800 Subject: [PATCH 29/44] Updating Mujoco Release version number to v3.1.1 PiperOrigin-RevId: 590712172 Change-Id: Ibf32322222212c1bf098e75bbb2e145344c57b93 --- 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 bfc599f8..40931b19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.1.0 + VERSION 3.1.1 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 484597a9..6a7fb828 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,1,0,0 -PRODUCTVERSION 3,1,0,0 +FILEVERSION 3,1,1,0 +PRODUCTVERSION 3,1,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.1.0" + VALUE "ProductVersion", "3.1.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.1.0" + VALUE "FileVersion", "3.1.1" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index ae73084e..14627b0b 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,1,0,0 -PRODUCTVERSION 3,1,0,0 +FILEVERSION 3,1,1,0 +PRODUCTVERSION 3,1,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.1.0" + VALUE "ProductVersion", "3.1.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.1.0" + VALUE "FileVersion", "3.1.1" 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 3101f9b2..c43f3145 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -522,7 +522,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`` - - 310 + - 311 - 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 f3a97af7..0a608df9 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.1.0.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.1.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.1.0/lib/libmujoco.so.3.1.0`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.1.1/lib/libmujoco.so.3.1.1`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 2935d5a8..ea861353 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -24,7 +24,7 @@ extern "C" { #endif // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 310 +#define mjVERSION_HEADER 311 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index d615108d..a558c2ed 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.1.0" +version = "3.1.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -31,13 +31,13 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.1.0.dev0", + "mujoco>=3.1.1.dev0", "scipy", "trimesh", ] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.1.0" +Documentation = "https://mujoco.readthedocs.io/en/3.1.1" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.1.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.1/changelog.html" diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index c9ed748e..e782497d 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.1.0.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.1.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.1.0 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.1.1 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 18a27c2f..7b2dfa5e 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.1.0 + 3.1.1 CFBundleGetInfoString - 3.1.0 + 3.1.1 CFBundleLongVersionString - 3.1.0 + 3.1.1 CFBundleShortVersionString - 3.1.0 + 3.1.1 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 6f934c1d..ab298dc4 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.1.0" +version = "3.1.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.1.0" +Documentation = "https://mujoco.readthedocs.io/en/3.1.1" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.1.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.1/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 0e989480..0d2c2c7f 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.1.0 + VERSION 3.1.1 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index e16584e0..fb416535 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.1.0 + VERSION 3.1.1 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index f887d1ca..819dc693 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -38,8 +38,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 310 -#define mjVERSIONSTRING "3.1.0" + #define mjVERSION 311 +#define mjVERSIONSTRING "3.1.1" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index 8ed2ac7a..3e9b9bfc 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.1.0.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.1.1.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.1.0/lib/libmujoco.so.3.1.0", + "/.mujoco/mujoco-3.1.1/lib/libmujoco.so.3.1.1", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 6f797926..c4969e46 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -108,7 +108,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 = 310; +public const int mjVERSION_HEADER = 311; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index f8f658d7..a6f629b9 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.1.0", + "version": "3.1.1", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From da211ddf29400ffc7bbd981d21a7325cc113d4f8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 13 Dec 2023 16:05:19 -0800 Subject: [PATCH 30/44] Improve description of the collision detection pipeline. PiperOrigin-RevId: 590742091 Change-Id: Idfcc37cece21dcf46401a147a43aa8ec4eeb05aa --- doc/computation/index.rst | 49 ++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 26 deletions(-) diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 788cdd14..190be5ac 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -307,9 +307,9 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, :math:`l_i(q) = 0`. :at:`site` - Site transmissions correspond to applying a Cartsian force/torque in the frame of a site, and are useful for - modeling jets and propellors. When a :at:`refsite` is not defined (see below), these targets have a fixed zero - length :math:`l_i(q) = 0`. + Site transmissions correspond to applying a Cartsian force/torque in the frame of a site. When a :at:`refsite` is not + defined (see below), these targets have a fixed zero length :math:`l_i(q) = 0` and are useful for modeling jets and + propellors: forces and torques which are fixed to the site frame. If a :at:`site` transmission is defined with the optional :at:`refsite` attribute, forces and torques are applied in the frame of the reference site rather than the site's own frame. If a reference site is defined, the length of the @@ -1458,48 +1458,45 @@ others can be pruned quickly without a detailed check. MuJoCo has flexible mecha checked in detail. The decision process involves two stages: generation and filtering. Generation - First we generate a list of candidate geom pairs in one of two ways: "pair" or "dynamic". The user can also specify - "all" which merges both sources (and is the default). This is done via the setting ``mjModel.opt.collision``. "Pair" - refers to an explicit list of geom pairs defined with the :ref:`pair ` element in MJCF. It gives the - user full control, however it is a static mechanism (independent of the spatial arrangement of the geoms at runtime) - and can be tedious for large models. It is normally used to supplement the output of the "dynamic" mechanism. Dynamic - generation works with bodies rather than geoms; when a body pair is included this means that all geoms attached to - one body can collide with all geoms attached to the other body. + First we generate a list of candidate geom pairs by merging from two sources: pairs of bodies that might contain + colliding geoms and the explicit list of geom pairs defined with the :ref:`pair ` element in MJCF. The body pairs are generated via broad-phase collision detection based on a modified sweep-and-prune algorithm. The modification is that the axis for sorting is chosen as the principal eigenvector of the covariance matrix of all geom - centers - which maximizes the spread. Then, for each body pair, a mid-phase collision detection using a static - bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB) is performed. Each body is - equipped with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes, + centers -- which maximizes the spread. Then, for each body pair, mid-phase collision detection is performed using a + static bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB). Each body is equipped + with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes, respectively. - Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude ` element - in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of - this step we have a list of geoms pairs that is typically much smaller than :math:`n (n-1)/2`, but can still be - pruned further before detailed collision checking. + Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude ` element in + MJCF. At the end of this step we have a list of geoms pairs that is typically much smaller than :math:`n (n-1)/2`, + but can still be pruned further before detailed collision checking. Filtering Next we apply four filters to the list generated in the previous step. Filters 1 and 2 are applied to all geom pairs. - Filters 3 and 4 are applied only to pairs generated by the "dynamic" mechanism, thereby allowing the user to bypass + Filters 3 and 4 are applied only to pairs generated by the body-pair mechanism, thereby allowing the user to bypass those filters by specifying geom pairs explicitly. - #. The types of the two geoms must correspond to a collision function that is capable of performing the detailed + 1. The types of the two geoms must correspond to a collision function that is capable of performing the detailed check. This is usually the case but there are exceptions (for example plane-plane collisions are not supported), and furthermore the user may override the default table of collision functions with NULL pointers, effectively disabling collisions between certain geom types. - #. A bounding sphere test is applied, taking into account the contact margin. If one of the geoms in the pair is a + 2. A bounding sphere test is applied, taking into account the contact margin. If one of the geoms in the pair is a plane, this becomes a plane-sphere test. - #. The two geoms cannot belong to the same body. Furthermore, they cannot belong to a parent and a child body, unless + 3. The two geoms cannot belong to the same body. Furthermore, they cannot belong to a parent and a child body, unless the parent is the world body. The motivation is to avoid permanent contacts within bodies and joints. Note that if several bodies are welded together in the sense that there are no joints between them, they are treated as a single body for the purposes of this test. The parent-filter test can be disabled by the user, while the same-body test cannot be disabled. - #. The two geoms must be "compatible" in the following sense. Each geom has integer parameters ``contype`` and + 4. The two geoms must be "compatible" in the following sense. Each geom has integer parameters ``contype`` and ``conaffinity``. The boolean expression below must be true for the test to pass: - ``(contype1 & conaffinity2) || (contype2 & conaffinity1)`` This requires the ``contype`` of one geom and the - ``conaffinity`` of the other geom to have a common bit set to 1. This is a powerful mechanism borrowed from the - Open Dynamics Engine. The default setting for all geoms is ``contype = conaffinity = 1`` which always passes the - test, so the user can ignore this mechanism if it is confusing at first. + + ``(contype1 & conaffinity2) || (contype2 & conaffinity1)`` + + This requires the ``contype`` of one geom and the ``conaffinity`` of the other geom to have a common bit set to 1. + This is a powerful mechanism borrowed from Open Dynamics Engine. The default setting for all geoms is + ``contype = conaffinity = 1`` which always passes the test, so the user can ignore this mechanism if it is + confusing at first. .. _coChecking: From 0915d69c3f5610ca22be7997e2328cc1261fefe9 Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Wed, 13 Dec 2023 16:33:44 -0800 Subject: [PATCH 31/44] Correctly transform site_xmat in MJX get_data/put_data. PiperOrigin-RevId: 590750273 Change-Id: I836e1f5d20725ed9b93bf86fe3e8e61954a56b47 --- mjx/mujoco/mjx/_src/io.py | 6 +++--- mjx/mujoco/mjx/_src/io_test.py | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 9658ad53..9396d7f3 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -217,7 +217,7 @@ def _get_contact( value = value.reshape((-1, 9)) getattr(c, field.name)[:] = value - ncon = con_id.shape[0] + ncon = cx.dist.shape[0] c.efc_address[:] = np.arange(efc_start, efc_start + ncon * 4, 4)[con_id] @@ -257,7 +257,7 @@ def get_data( value = getattr(dx_i, field.name) - if field.name in ('xmat', 'ximat', 'geom_xmat'): + if field.name in ('xmat', 'ximat', 'geom_xmat', 'site_xmat'): value = value.reshape((-1, 9)) if field.name in ('efc_frictionloss', 'efc_D', 'efc_aref', 'efc_force'): @@ -318,7 +318,7 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: if f.type is jax.Array } - for fname in ('xmat', 'ximat', 'geom_xmat'): + for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat'): fields[fname] = fields[fname].reshape((-1, 3, 3)) # pad efc fields: MuJoCo efc arrays are sparse for inactive constraints. diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 4c7b0065..a16a8780 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -71,6 +71,7 @@ _MULTIPLE_CONSTRAINTS = """ + @@ -318,9 +319,11 @@ class IoTest(parameterized.TestCase): self.assertEqual(dx.xmat.shape, (3, 3, 3)) self.assertEqual(dx.ximat.shape, (3, 3, 3)) self.assertEqual(dx.geom_xmat.shape, (3, 3, 3)) + self.assertEqual(dx.site_xmat.shape, (1, 3, 3)) np.testing.assert_allclose(dx.xmat.reshape((3, 9)), d.xmat) np.testing.assert_allclose(dx.ximat.reshape((3, 9)), d.ximat) np.testing.assert_allclose(dx.geom_xmat.reshape((3, 9)), d.geom_xmat) + np.testing.assert_allclose(dx.site_xmat.reshape((1, 9)), d.site_xmat) # efc_ are also shape transformed and padded self.assertEqual(dx.efc_J.shape, (21, 8)) # nefc, nv @@ -369,13 +372,15 @@ class IoTest(parameterized.TestCase): self.assertEqual(d_2.contact.frame.shape, (1, 9)) np.testing.assert_allclose(d_2.contact.frame, d.contact.frame) - # xmat, ximat, geom_xmat are all shape transformed + # xmat, ximat, geom_xmat, site_xmat are all shape transformed self.assertEqual(d_2.xmat.shape, (3, 9)) self.assertEqual(d_2.ximat.shape, (3, 9)) self.assertEqual(d_2.geom_xmat.shape, (3, 9)) + self.assertEqual(d_2.site_xmat.shape, (1, 9)) np.testing.assert_allclose(d_2.xmat, d.xmat) np.testing.assert_allclose(d_2.ximat, d.ximat) np.testing.assert_allclose(d_2.geom_xmat, d.geom_xmat) + np.testing.assert_allclose(d_2.site_xmat, d.site_xmat) # efc_* are also shape transformed and filtered self.assertEqual(d_2.efc_J.shape, (64,)) # nefc * nv From 9f6be3135ecc077700ab69d60f0826bee43f24a9 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 14 Dec 2023 11:11:00 -0800 Subject: [PATCH 32/44] Modify mjListKeyMap to be a fixed-size array, and add internal object_lists variable to mjCModel to remove redundant code. PiperOrigin-RevId: 590997348 Change-Id: I5f4d7de605f44d40a75eb1e9ceec208325fa3be5 --- src/user/user_model.cc | 234 ++++++++--------------------------- src/user/user_model.h | 6 +- test/user/user_model_test.cc | 27 +++- 3 files changed, 80 insertions(+), 187 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index c1c0300e..a4f5a2dd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -147,7 +147,6 @@ mjCModel::mjCModel() { nuser_sensor = -1; //------------------------ private variables - ids.clear(); cameras.clear(); lights.clear(); flexes.clear(); @@ -183,6 +182,35 @@ mjCModel::mjCModel() { world->name = "world"; world->def = defaults[0]; bodies.push_back(world); + + for (int i = 0; i < mjNOBJECT; ++i) { + object_lists[i] = nullptr; + } + + object_lists[mjOBJ_BODY] = (std::vector*) &bodies; + object_lists[mjOBJ_XBODY] = (std::vector*) &bodies; + object_lists[mjOBJ_JOINT] = (std::vector*) &joints; + object_lists[mjOBJ_GEOM] = (std::vector*) &geoms; + object_lists[mjOBJ_SITE] = (std::vector*) &sites; + object_lists[mjOBJ_CAMERA] = (std::vector*) &cameras; + object_lists[mjOBJ_LIGHT] = (std::vector*) &lights; + object_lists[mjOBJ_FLEX] = (std::vector*) &flexes; + object_lists[mjOBJ_MESH] = (std::vector*) &meshes; + object_lists[mjOBJ_SKIN] = (std::vector*) &skins; + object_lists[mjOBJ_HFIELD] = (std::vector*) &hfields; + object_lists[mjOBJ_TEXTURE] = (std::vector*) &textures; + object_lists[mjOBJ_MATERIAL] = (std::vector*) &materials; + object_lists[mjOBJ_PAIR] = (std::vector*) &pairs; + object_lists[mjOBJ_EXCLUDE] = (std::vector*) &excludes; + object_lists[mjOBJ_EQUALITY] = (std::vector*) &equalities; + object_lists[mjOBJ_TENDON] = (std::vector*) &tendons; + object_lists[mjOBJ_ACTUATOR] = (std::vector*) &actuators; + object_lists[mjOBJ_SENSOR] = (std::vector*) &sensors; + object_lists[mjOBJ_NUMERIC] = (std::vector*) &numerics; + object_lists[mjOBJ_TEXT] = (std::vector*) &texts; + object_lists[mjOBJ_TUPLE] = (std::vector*) &tuples; + object_lists[mjOBJ_KEY] = (std::vector*) &keys; + object_lists[mjOBJ_PLUGIN] = (std::vector*) &plugins; } @@ -213,7 +241,6 @@ mjCModel::~mjCModel() { for (int i=0; isize(); } // get pointer to specified object mjCBase* mjCModel::GetObject(mjtObj type, int id) { - if (id>=0 && id= NumObjects(type)) { + return nullptr; } - - return 0; + return (*object_lists[type])[id]; } @@ -666,55 +595,10 @@ static T* findobject(std::string_view name, const vector& list, const mjKeyM // find object in global lists given string type and name mjCBase* mjCModel::FindObject(mjtObj type, string name) { - switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - return findobject(name, bodies, ids["body"]); - case mjOBJ_JOINT: - return findobject(name, joints, ids["joint"]); - case mjOBJ_GEOM: - return findobject(name, geoms, ids["geom"]); - case mjOBJ_SITE: - return findobject(name, sites, ids["site"]); - case mjOBJ_CAMERA: - return findobject(name, cameras, ids["camera"]); - case mjOBJ_LIGHT: - return findobject(name, lights, ids["light"]); - case mjOBJ_FLEX: - return findobject(name, flexes, ids["flex"]); - case mjOBJ_MESH: - return findobject(name, meshes, ids["mesh"]); - case mjOBJ_SKIN: - return findobject(name, skins, ids["skin"]); - case mjOBJ_HFIELD: - return findobject(name, hfields, ids["hfield"]); - case mjOBJ_TEXTURE: - return findobject(name, textures, ids["texture"]); - case mjOBJ_MATERIAL: - return findobject(name, materials, ids["material"]); - case mjOBJ_PAIR: - return findobject(name, pairs, ids["pair"]); - case mjOBJ_EXCLUDE: - return findobject(name, excludes, ids["exclude"]); - case mjOBJ_EQUALITY: - return findobject(name, equalities, ids["equality"]); - case mjOBJ_TENDON: - return findobject(name, tendons, ids["tendon"]); - case mjOBJ_ACTUATOR: - return findobject(name, actuators, ids["actuator"]); - case mjOBJ_SENSOR: - return findobject(name, sensors, ids["sensor"]); - case mjOBJ_NUMERIC: - return findobject(name, numerics, ids["numeric"]); - case mjOBJ_TEXT: - return findobject(name, texts, ids["text"]); - case mjOBJ_TUPLE: - return findobject(name, tuples, ids["tuple"]); - case mjOBJ_PLUGIN: - return findobject(name, plugins, ids["plugin"]); - default: - return 0; + if (!object_lists[type]) { + return nullptr; } + return findobject(name, *object_lists[type], ids[type]); } @@ -2649,37 +2533,37 @@ static void reassignid(vector& list) { // set ids, check for repeated names template static void processlist(mjListKeyMap& ids, vector& list, - string defname, bool checkrepeat = true) { + mjtObj type, bool checkrepeat = true) { // loop over list elements - for (int i=0; i<(int)list.size(); i++) { + for (size_t i=0; i < list.size(); i++) { // check for incompatible id setting; SHOULD NOT OCCUR if (list[i]->id!=-1 && list[i]->id!=i) { - throw mjCError(list[i], "incompatible id in %s array, position %d", defname.c_str(), i); + throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i); } // id equals position in array list[i]->id = i; // add to ids map - ids[defname][list[i]->name] = i; + ids[type][list[i]->name] = i; } // check for repeated names if (checkrepeat) { // created vectors with all names vector allnames; - for (int i=0; i<(int)list.size(); i++) { + for (size_t i=0; i < list.size(); i++) { if (!list[i]->name.empty()) { allnames.push_back(list[i]->name); } } // sort and check for duplicates - if (allnames.size()>1) { + if (allnames.size() > 1) { std::sort(allnames.begin(), allnames.end()); auto adjacent = std::adjacent_find(allnames.begin(), allnames.end()); - if (adjacent!=allnames.end()) { - string msg = "repeated name '" + *adjacent + "' in " + defname; + if (adjacent != allnames.end()) { + string msg = "repeated name '" + *adjacent + "' in " + mju_type2Str(type); throw mjCError(NULL, msg.c_str()); } } @@ -2813,29 +2697,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { CheckEmptyNames(); // set object ids, check for repeated names - processlist(ids, bodies, "body"); - processlist(ids, joints, "joint"); - processlist(ids, geoms, "geom"); - processlist(ids, sites, "site"); - processlist(ids, cameras, "camera"); - processlist(ids, lights, "light"); - processlist(ids, flexes, "flex"); - processlist(ids, meshes, "mesh"); - processlist(ids, skins, "skin"); - processlist(ids, hfields, "hfield"); - processlist(ids, textures, "texture"); - processlist(ids, materials, "material"); - processlist(ids, pairs, "pair"); - processlist(ids, excludes, "exclude"); - processlist(ids, equalities, "equality"); - processlist(ids, tendons, "tendon"); - processlist(ids, actuators, "actuator"); - processlist(ids, sensors, "sensor"); - processlist(ids, numerics, "numeric"); - processlist(ids, texts, "text"); - processlist(ids, tuples, "tuple"); - processlist(ids, keys, "key"); - processlist(ids, plugins, "plugin"); + for (int i = 0; i < mjNOBJECT; i++) { + if (i != mjOBJ_XBODY && object_lists[i]) { + processlist(ids, *object_lists[i], (mjtObj) i); + } + } // convert names into indices IndexAssets(); diff --git a/src/user/user_model.h b/src/user/user_model.h index 85ae8ec9..222cbccf 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -32,9 +32,8 @@ typedef enum _mjtInertiaFromGeom { mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing } mjtInertiaFromGeom; -// TODO: convert mjListKeyMap to mjKeyMap[mjNOBJECT] by adding mjNOBJECT to mjtObj typedef std::map > mjKeyMap; -typedef std::map > mjListKeyMap; +typedef std::array mjListKeyMap; @@ -294,6 +293,9 @@ class mjCModel { //------------------------ internal variables + // array of pointers to each object list (enumerated by type) + std::array*, mjNOBJECT> object_lists; + // statistics, as computed by mj_setConst double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst double meanmass_auto; // mean body mass, as computed by mj_setConst diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index db1059ef..2079b1d1 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -30,16 +30,41 @@ namespace { using ::testing::DoubleNear; using ::testing::ElementsAre; +using ::testing::HasSubstr; +using ::testing::IsNull; using ::testing::NotNull; -using UserDataTest = MujocoTest; static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { return std::vector(array + ncolumn * row, array + ncolumn * (row + 1)); } +// ----------------------------- test mjCModel -------------------------------- + +using UserCModelTest = MujocoTest; + +TEST_F(UserCModelTest, RepeatedNames) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom")); +} + // ------------- test automatic inference of nuser_xxx ------------------------- +using UserDataTest = MujocoTest; + TEST_F(UserDataTest, AutoNUserBody) { static constexpr char xml[] = R"( From f2a967348c64197b9a29fb9e1c7dd9879984e10b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 14 Dec 2023 13:29:22 -0800 Subject: [PATCH 33/44] Fix bug introduced in 7942fe957ea046750f936ac002d49425e31d9dab That change removed some spurious contacts returned by mjc_BoxBox, but also removed some desirable contacts that occur during very deep penetration of two boxes (when one box is completely inside another box). This is now fixed. PiperOrigin-RevId: 591036252 Change-Id: I84b51f2179fd6fe29618a4e908e86934c6fc6941 --- src/engine/engine_collision_driver.c | 18 ++++---- src/engine/engine_util_misc.c | 40 ++++++++++++++---- src/engine/engine_util_misc.h | 6 ++- test/engine/engine_collision_box_test.cc | 42 ++++++++++++++----- .../testdata/collision_box/boxbox_deep.xml | 11 +++++ 5 files changed, 90 insertions(+), 27 deletions(-) create mode 100644 test/engine/testdata/collision_box/boxbox_deep.xml diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index d42a4f2e..f6132b74 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1444,9 +1444,6 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], // test two geoms for collision, apply filters, add to contact list void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { - // relative distance (1%) outside of which box-box contacts are removed - static mjtNum kBoxRemoveMargin = 1.01; - TM_START; int num, type1, type2, condim; @@ -1547,11 +1544,16 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // box sizes with margin mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, size1[2] + margin}; mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, size2[2] + margin}; - mju_scl3(sz1, sz1, kBoxRemoveMargin); - mju_scl3(sz2, sz2, kBoxRemoveMargin); - // mark as bad if outside box - if (mju_outsideBox(con[i].pos, pos1, mat1, sz1) || - mju_outsideBox(con[i].pos, pos2, mat2, sz2)) { + + // relative distance from surface (1%) outside of which box-box contacts are removed + static mjtNum kRemoveRatio = 1.01; + + // is the contact outside: 1, inside: -1, within the removal width: 0 + int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio); + int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio); + + // mark as bad if outside one box and not inside the other box + if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) { con[i].dim = -1; } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 29e2e0f8..37eca9e4 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -837,21 +837,47 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) -// return 1 if point is outside box given by pos, mat, size +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], - const mjtNum size[3]) { + const mjtNum size[3], mjtNum inflate) { + // check inflation coefficient + if (inflate < 1) { + mjERROR("inflation coefficient must be >= 1") + } + // vector from pos to point, projected to box frame mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]}; mju_rotVecMatT(vec, vec, mat); - // outside - if (vec[0] > size[0] || vec[0] < -size[0] || - vec[1] > size[1] || vec[1] < -size[1] || - vec[2] > size[2] || vec[2] < -size[2]) { + // big: inflated box + mjtNum big[3] = {size[0], size[1], size[2]}; + if (inflate > 1) { + mju_scl3(big, big, inflate); + } + + // check if outside big box + if (vec[0] > big[0] || vec[0] < -big[0] || + vec[1] > big[1] || vec[1] < -big[1] || + vec[2] > big[2] || vec[2] < -big[2]) { return 1; } - // inside + // quick return if no inflation + if (inflate == 1) { + return -1; + } + + // check if inside small (deflated) box + mjtNum small[3] = {size[0]/inflate, size[1]/inflate, size[2]/inflate}; + if (vec[0] < small[0] && vec[0] > -small[0] && + vec[1] < small[1] && vec[1] > -small[1] && + vec[2] < small[2] && vec[2] > -small[2]) { + return -1; + } + + // within margin between small and big box return 0; } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index c103aba1..cb317258 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -78,9 +78,11 @@ MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, // integrate spring-damper analytically, return pos(dt) MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); -// return 1 if point is outside box given by pos, mat, size +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], - const mjtNum size[3]); + const mjtNum size[3], mjtNum inflate); // print matrix MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index 0247335d..5477189d 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -94,7 +94,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) { } // expect some contacts to have been removed - EXPECT_LT(nmatched, num); + EXPECT_LT(nmatched, num) << local_path; // get box info const mjtNum* pos1 = data->geom_xpos + 3 * g1; @@ -103,23 +103,27 @@ TEST_F(MjCollisionBoxTest, BadContacts) { const mjtNum* pos2 = data->geom_xpos + 3 * g2; const mjtNum* mat2 = data->geom_xmat + 9 * g2; const mjtNum* size2 = model->geom_size + 3 * g2; + mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[g2]); // loop over raw contacts, find removed for (int i = 0; i < num; i++) { if (!match_raw[i]) { // === check if outside - // get margin and adjusted sizes - const mjtNum kBoxRemoveMargin = 1.01; - mjtNum sz1[3], sz2[3]; - mju_scl3(sz1, size1, kBoxRemoveMargin); - mju_scl3(sz2, size2, kBoxRemoveMargin); + mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin}; + mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin}; - // is contact outside one of the boxes - bool outside = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1) || - mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2); + // relative distance (1%) outside of which contacts are removed + static mjtNum kRatio = 1.01; - // expect that removed contact was either outside + // is the contact outside: 1, inside: -1, within the removal width: 0 + int out1 = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1, kRatio); + int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio); + + // mark as bad if outside one box and not inside the other box + bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); + + // expect that removed contact was outside EXPECT_TRUE(outside); } } @@ -222,5 +226,23 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { } +static const char* const kDeepFilePath = + "engine/testdata/collision_box/boxbox_deep.xml"; + +TEST_F(MjCollisionBoxTest, DeepPenetration) { + const std::string xml_path = GetTestDataFilePath(kDeepFilePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // expect 4 contact + EXPECT_EQ(data->ncon ,4); + + mj_deleteData(data); + mj_deleteModel(model); +} + + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/collision_box/boxbox_deep.xml b/test/engine/testdata/collision_box/boxbox_deep.xml new file mode 100644 index 00000000..65f14e5f --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_deep.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + From 38fa756a20b8edc218bea622f21c50a333ab02cb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 15 Dec 2023 05:35:34 -0800 Subject: [PATCH 34/44] Add `mjGEOM_LINEBOX`, used in visualization of collision trees. PiperOrigin-RevId: 591224734 Change-Id: I09ad8c4b711021ff4dd5548ba5381d25d5a3d7f8 --- doc/includes/references.h | 3 +- include/mujoco/mjmodel.h | 3 +- introspect/enums.py | 9 +-- introspect/enums_test.py | 2 +- python/mujoco/bindings_test.py | 2 +- src/engine/engine_vis_visualize.c | 96 ++++++++++------------------ src/render/render_gl3.c | 35 ++++++++++ unity/Runtime/Bindings/MjBindings.cs | 9 +-- 8 files changed, 84 insertions(+), 75 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index c89a4363..7224e3a3 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -459,10 +459,11 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_ARROW1, // arrow without wedges mjGEOM_ARROW2, // arrow in both directions mjGEOM_LINE, // line + mjGEOM_LINEBOX, // box with line edges mjGEOM_FLEX, // flex mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label - mjGEOM_TRIANGLE, // triangle connecting a frame + mjGEOM_TRIANGLE, // triangle mjGEOM_NONE = 1001 // missing geom type } mjtGeom; diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 49312dd1..d3417683 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -109,10 +109,11 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_ARROW1, // arrow without wedges mjGEOM_ARROW2, // arrow in both directions mjGEOM_LINE, // line + mjGEOM_LINEBOX, // box with line edges mjGEOM_FLEX, // flex mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label - mjGEOM_TRIANGLE, // triangle connecting a frame + mjGEOM_TRIANGLE, // triangle mjGEOM_NONE = 1001 // missing geom type } mjtGeom; diff --git a/introspect/enums.py b/introspect/enums.py index 4982cf36..8502dbdb 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -90,10 +90,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGEOM_ARROW1', 101), ('mjGEOM_ARROW2', 102), ('mjGEOM_LINE', 103), - ('mjGEOM_FLEX', 104), - ('mjGEOM_SKIN', 105), - ('mjGEOM_LABEL', 106), - ('mjGEOM_TRIANGLE', 107), + ('mjGEOM_LINEBOX', 104), + ('mjGEOM_FLEX', 105), + ('mjGEOM_SKIN', 106), + ('mjGEOM_LABEL', 107), + ('mjGEOM_TRIANGLE', 108), ('mjGEOM_NONE', 1001), ]), )), diff --git a/introspect/enums_test.py b/introspect/enums_test.py index b4144420..3580d4c1 100644 --- a/introspect/enums_test.py +++ b/introspect/enums_test.py @@ -61,7 +61,7 @@ class EnumsTest(absltest.TestCase): self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100) self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101) self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102) - self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 107) + self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 108) # Skip a few... self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 1222569c..3d2e0ac6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -840,7 +840,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102) - self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 107) + self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 108) self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001) def test_enum_from_int(self): diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 393bd509..d2e2de20 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -509,56 +509,6 @@ static int bodycategory(const mjModel* m, int bodyid) { -// draw bounding box -static void drawBoundingBox(mjData* d, mjvScene* scn, - const mjtNum aabb[6], const mjtNum xpos[3], - const mjtNum xmat[9], const float rgba[4]) { - mjtNum x[3]; - mjtNum dist[3][3]; - mjvGeom* thisgeom; - int category = mjCAT_DECOR; - int objtype = mjOBJ_UNKNOWN; - int i = -1; - - if (xmat != NULL) { - mju_rotVecMat(x, aabb, xmat); - mju_addTo3(x, xpos); - for (int j=0; j < 3; j++) { - for (int k=0; k < 3; k++) { - dist[k][j] = aabb[k+3] * xmat[3*j+k]; - } - } - } else { - mju_copy3(x, aabb); - mju_addTo3(x, xpos); - for (int j=0; j < 3; j++) { - mju_zero3(dist[j]); - dist[j][j] = aabb[j+3]; - } - } - - int split[3] = {1, 2, 4}; - for (int v=0; v < 8; v++) { - mjtNum from[3] = {x[0], x[1], x[2]}; - for (int k=0; k < 3; k++) { - mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1); - } - - mjtNum to[3]; - for (int k=0; k < 3; k++) { - mju_addScl3(to, from, dist[k], 2); - if (!(v&split[k])) { - START - mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to); - f2f(thisgeom->rgba, rgba, 4); - FINISH - } - } - } -} - - - // computes the camera frustum static void getFrustum(float zver[2], float zhor[2], float znear, const float K[4], const float sensorsize[2]) { @@ -683,6 +633,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // body BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_BODYBVH]) { float rgba[] = {1, 0, 0, 1}; for (int i = 0; i < m->nbvhstatic; i++) { @@ -708,20 +660,30 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, break; } - // compute transformation - mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; - + // get xpos, xmat, size const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid; const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid; + const mjtNum *size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3; + + // offset xpos with aabb center (not always at frame origin) + const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; + mjtNum pos[3]; + mju_rotVecMat(pos, center, xmat); + mju_addTo3(pos, xpos); rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - drawBoundingBox(d, scn, aabb, xpos, xmat, rgba); + START + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); + FINISH + } } // flex BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_FLEXBVH]) { float rgba[] = {1, 0, 0, 0.1}; for (int f=0; f < m->nflex; f++) { @@ -741,10 +703,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - // b/304453879 : add LINEBOX geom for bounding box visualization - START - mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba); + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba); FINISH } } @@ -752,6 +712,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // mesh BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_MESHBVH]) { float rgba[] = {1, 0, 0, 1}; for (int geomid = 0; geomid < m->ngeom; geomid++) { @@ -770,11 +732,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } } - // compute transformation - const mjtNum *aabb = m->bvh_aabb + 6*i; - const mjtNum* xpos = d->geom_xpos + 3 * geomid; - const mjtNum* xmat = d->geom_xmat + 9 * geomid; - if (!d->bvh_active[i]) { continue; } @@ -782,7 +739,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - drawBoundingBox(d, scn, aabb, xpos, xmat, rgba); + // get xpos, xmat, size + const mjtNum* xpos = d->geom_xpos + 3 * geomid; + const mjtNum* xmat = d->geom_xmat + 9 * geomid; + const mjtNum *size = m->bvh_aabb + 6*i + 3; + + // offset xpos with aabb center (not always at geom origin) + const mjtNum *center = m->bvh_aabb + 6*i; + mjtNum pos[3]; + mju_rotVecMat(pos, center, xmat); + mju_addTo3(pos, xpos); + + START + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); + FINISH } } } diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 4e108c0d..6535874a 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -404,6 +404,41 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, } break; + case mjGEOM_LINEBOX: // box with line edges + glLineWidth(1.5*con->lineWidth); + lighting = glIsEnabled(GL_LIGHTING); + glDisable(GL_LIGHTING); + // bottom face + glBegin(GL_LINE_LOOP); + glVertex3f(-size[0], -size[1], -size[2]); + glVertex3f( size[0], -size[1], -size[2]); + glVertex3f( size[0], size[1], -size[2]); + glVertex3f(-size[0], size[1], -size[2]); + glEnd(); + // top face + glBegin(GL_LINE_LOOP); + glVertex3f(-size[0], -size[1], size[2]); + glVertex3f( size[0], -size[1], size[2]); + glVertex3f( size[0], size[1], size[2]); + glVertex3f(-size[0], size[1], size[2]); + glEnd(); + // vertical edges + glBegin(GL_LINES); + glVertex3f(-size[0], -size[1], -size[2]); + glVertex3f(-size[0], -size[1], size[2]); + glVertex3f( size[0], -size[1], -size[2]); + glVertex3f( size[0], -size[1], size[2]); + glVertex3f( size[0], size[1], -size[2]); + glVertex3f( size[0], size[1], size[2]); + glVertex3f(-size[0], size[1], -size[2]); + glVertex3f(-size[0], size[1], size[2]); + glEnd(); + glLineWidth(con->lineWidth); + if (lighting) { + glEnable(GL_LIGHTING); + } + break; + case mjGEOM_TRIANGLE: // triangle glBegin(GL_TRIANGLES); glVertex3f(0, 0, 0); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c4969e46..d773cf87 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -191,10 +191,11 @@ public enum mjtGeom : int{ mjGEOM_ARROW1 = 101, mjGEOM_ARROW2 = 102, mjGEOM_LINE = 103, - mjGEOM_FLEX = 104, - mjGEOM_SKIN = 105, - mjGEOM_LABEL = 106, - mjGEOM_TRIANGLE = 107, + mjGEOM_LINEBOX = 104, + mjGEOM_FLEX = 105, + mjGEOM_SKIN = 106, + mjGEOM_LABEL = 107, + mjGEOM_TRIANGLE = 108, mjGEOM_NONE = 1001, } public enum mjtCamLight : int{ From dc0d0c59d40bf768282a47bdc465c83c2b7fb5f1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 15 Dec 2023 08:11:26 -0800 Subject: [PATCH 35/44] Fix bug in simulate where the "LOADING..." label was not properly showing. PiperOrigin-RevId: 591257979 Change-Id: Id09754b1e5a87decfb68b9fe305388aa8f0f5ed3 --- simulate/simulate.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index db0821d2..abe5a23b 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -2479,7 +2479,7 @@ void Simulate::Render() { char label[30] = {'\0'}; if (this->loadrequest) { std::snprintf(label, sizeof(label), "LOADING..."); - } if (this->scrub_index == 0) { + } else if (this->scrub_index == 0) { std::snprintf(label, sizeof(label), "PAUSE"); } else { std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index); From 8ecb16abfa16beb3a92f1840bfb31c0bf7d11cae Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Mon, 18 Dec 2023 03:33:05 -0800 Subject: [PATCH 36/44] Don't run particle.xml in testspeed_test under ASAN. Reduce the number of steps for a couple of other slow tests. PiperOrigin-RevId: 591844569 Change-Id: If722567b91163f1f3c451302f9543a43d05872a1 --- test/sample/testspeed_test.sh | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/test/sample/testspeed_test.sh b/test/sample/testspeed_test.sh index 942c2a98..f48342bc 100755 --- a/test/sample/testspeed_test.sh +++ b/test/sample/testspeed_test.sh @@ -26,8 +26,17 @@ test_model() { echo "Testing $model" >&2 local iterations=10 - if [[ "$model" == */composite/particle.xml && ${TESTSPEED_ASAN:-0} != 0 ]]; then - iterations=2 + # for particularly slow models, only run 2 steps under ASAN, or skip. + if [[ ${TESTSPEED_ASAN:-0} != 0 ]]; then + if [[ "$model" == */composite/particle.xml ]]; then + # this test can take several minutes under ASAN + return 0 + fi + if [[ "$model" == */benchmark/testdata/humanoid200.xml + || "$model" == */engine/testdata/collision_convex/stacked_boxes.xml + ]]; then + iterations=2 + fi fi # run testspeed, writing its output to stderr. From 7cb7c87f70a8dc6718283c3fec90f8b28497debf Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Mon, 18 Dec 2023 07:32:20 -0800 Subject: [PATCH 37/44] Fix a few issues with the passive viewer. 1. Create an arena for the mjData instance used by the passive viewer visualization. When using the passive viewer, stuff gets copied from the real mjData into a minimal struct. That struct didn't have a stack, and now visualization for Flex does stack allocs. 2. Add missing a missing field in scene state for flex visualization. 3. Fix a memory leak where mjvScene wasn't released on exit in the passive viewer. 4. Add some locks in places where the render thread and Simulate::Sync collide. This fixes #1280. PiperOrigin-RevId: 591891676 Change-Id: I592f286cab9719c8d9af84e42f9756ea1f9f1971 --- doc/includes/references.h | 3 +++ include/mujoco/mjvisualize.h | 3 +++ include/mujoco/mjxmacro.h | 2 +- introspect/structs.py | 19 +++++++++++++++++++ simulate/simulate.cc | 9 ++++++--- src/engine/engine_vis_state.c | 16 ++++++++++++++++ test/engine/CMakeLists.txt | 7 ++++++- test/engine/engine_vis_state_test.cc | 8 ++++---- unity/Runtime/Bindings/MjBindings.cs | 3 +++ 9 files changed, 61 insertions(+), 9 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 7224e3a3..1c788150 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2170,6 +2170,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -2383,6 +2384,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -2394,6 +2396,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index d693df9c..dc8a4bc6 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -436,6 +436,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -649,6 +650,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -660,6 +662,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index f4088314..86f61e0c 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -614,7 +614,7 @@ X ( mjtNum, qLD, nM, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \ - X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ + XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ XMJV( mjtByte, bvh_active, nbvh, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ X ( mjtNum, ten_velocity, ntendon, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index eea7f6cc..43e7cdc9 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -6334,6 +6334,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='', ), + StructFieldDecl( + name='narena', + type=ValueType(name='int'), + doc='', + ), StructFieldDecl( name='opt', type=ValueType(name='mjOption'), @@ -7596,6 +7601,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='bvh_aabb_dyn', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), StructFieldDecl( name='bvh_active', type=PointerType( @@ -7659,6 +7671,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='arena', + type=PointerType( + inner_type=ValueType(name='void'), + ), + doc='', + ), ), ), doc='', diff --git a/simulate/simulate.cc b/simulate/simulate.cc index abe5a23b..226e290d 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1817,6 +1817,9 @@ void Simulate::Sync() { if (!m_) { return; } + if (this->exitrequest.load()) { + return; + } bool update_profiler = this->profiler && (this->pause_update || this->run); bool update_sensor = this->sensor && (this->pause_update || this->run); @@ -2714,9 +2717,9 @@ void Simulate::RenderLoop() { } } - if (!is_passive_){ - mjv_freeScene(&this->scn); - } else { + const MutexLock lock(this->mtx); + mjv_freeScene(&this->scn); + if (is_passive_) { mjv_freeSceneState(&scnstate_); } diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 5ef3c274..55d35497 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -86,6 +86,11 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + // create an arena in the scnstate, to allow visualization code to use the stack. + // TODO: Consider allocating way less than narena, since stack allocations in + // visualization code are much smaller than the arena space required by the model, + // typically. + scnstate->nbuffer += roundUpToCacheLine(m->narena); // buffer space required for contacts int condimmax = mj_isPyramidal(m) ? 10 : 6; scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->contact) * maxgeom); @@ -118,6 +123,10 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + scnstate->model.narena = m->narena; + scnstate->data.arena = (void*)ptr; + ptr += roundUpToCacheLine(m->narena); + scnstate->data.contact = (mjContact*)ptr; ptr += roundUpToCacheLine(sizeof(*scnstate->data.contact) * scnstate->maxgeom); @@ -177,6 +186,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* m->opt = scnstate->model.opt; m->vis = scnstate->model.vis; m->stat = scnstate->model.stat; + m->narena = scnstate->model.narena; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) m->var = scnstate->model.var; @@ -194,10 +204,16 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* #endif memcpy(d->warning, scnstate->data.warning, sizeof(d->warning)); + d->threadpool = 0; d->nefc = scnstate->data.nefc; d->ncon = scnstate->data.ncon; d->nisland = scnstate->data.nisland; d->time = scnstate->data.time; + d->narena = scnstate->model.narena; + d->arena = scnstate->data.arena; + d->parena = 0; + d->pbase = 0; + d->pstack = 0; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) d->var = scnstate->data.var; diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index fb77205d..727cf955 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -99,5 +99,10 @@ target_link_libraries(engine_util_spatial_test fixture gmock) mujoco_test(engine_vfs_test) target_link_libraries(engine_vfs_test fixture gmock) -mujoco_test(engine_vis_state_test) +mujoco_test( + engine_vis_state_test + PROPERTIES + ENVIRONMENT + "MUJOCO_PLUGIN_DIR=$" +) target_link_libraries(engine_vis_state_test fixture gmock) diff --git a/test/engine/engine_vis_state_test.cc b/test/engine/engine_vis_state_test.cc index 48d48efd..6fefde47 100644 --- a/test/engine/engine_vis_state_test.cc +++ b/test/engine/engine_vis_state_test.cc @@ -35,17 +35,17 @@ static const char* const kTendonPath = "engine/testdata/island/tendon_wrap.xml"; static const char* const kFrustumPath = "engine/testdata/vis_visualize/frustum.xml"; -static const char* const kModelPath = - "testdata/model.xml"; +static const char* const kFlex = "testdata/flex.xml"; +static const char* const kModelPath = "testdata/model.xml"; #define EXPECT_ZERO(exp) EXPECT_EQ(0, exp); TEST_F(MjvSceneStateTest, CanUpdateFromState) { for (const char* path : - {kHammockPath, kTendonPath, kModelPath, kFrustumPath}) { + {kHammockPath, kTendonPath, kModelPath, kFrustumPath, kFlex}) { const std::string xml_path = GetTestDataFilePath(path); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - ASSERT_THAT(model, NotNull()); + ASSERT_THAT(model, NotNull()) << "Failed to load model from " << path; mjData* data = mj_makeData(model); while (data->time < 2) { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d773cf87..213cba7a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6095,6 +6095,7 @@ public unsafe struct model { public int nnames; public int npaths; public int nsensordata; + public int narena; public mjOption_ opt; public mjVisual_ vis; public mjStatistic_ stat; @@ -6287,6 +6288,7 @@ public unsafe struct data { public int* wrap_obj; public double* ten_length; public double* wrap_xpos; + public double* bvh_aabb_dyn; public byte* bvh_active; public int* island_dofadr; public int* island_dofind; @@ -6296,6 +6298,7 @@ public unsafe struct data { public double* flexvert_xpos; public mjContact_* contact; public double* efc_force; + public void* arena; } [StructLayout(LayoutKind.Sequential)] From d39ed1d3c8890f30037cc454a0f415f506cf907f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 18 Dec 2023 10:04:25 -0800 Subject: [PATCH 38/44] Update changelog. PiperOrigin-RevId: 591932297 Change-Id: I9131400fee6839e9311baf28a069fc1e7237ee86 --- doc/changelog.rst | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 5a90b5f6..016d028b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,9 +2,23 @@ Changelog ========= -Version 3.1.0 (December 12, 2023) + +Upcoming version (not yet released) ----------------------------------- +Bug fixes +^^^^^^^^^ +- Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts is one box was deeply embedded in the + other. +- Fixed a bug in :ref:`simulate` where the "LOADING..." message was not showing correctly. +- Fixed a crash in the Python :ref:`passive viewer`, when used with models containing Flex objects. +- Fixed a bug in MJX where ``site_xmat`` was ignored in ``get_data`` and ``put_data`` +- Fixed a bug in MJX where ``efc_address`` was sometimes incorrectly calculated in ``get_data``. + + +Version 3.1.0 (December 12, 2023) +--------------------------------- + General ^^^^^^^ 1. Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function @@ -39,7 +53,7 @@ Bug fixes ^^^^^^^^^ 8. Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. Before this fix such actuators could lead to non-conservation of momentum. -9. Fix bug that prevented using flex with the :ref:`passive viewer`. +9. Fix bug that prevented using flex with :ref:`simulate`. 10. Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. 11. Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum supported version is still 11.0, but we release these wheels to fix compatibility for those users. See From 3ed81f864c75957b507a9f9453d112f2cb9d243f Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 18 Dec 2023 11:34:33 -0800 Subject: [PATCH 39/44] Update version number in third_party/mujoco/doc/changelog.rst for v3.1.1 PiperOrigin-RevId: 591962782 Change-Id: I7c498e1339013e9207ef8c21e793c1d80f55d16d --- doc/changelog.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 016d028b..c4970235 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -3,17 +3,17 @@ Changelog ========= -Upcoming version (not yet released) +Version 3.1.1 (December 18, 2023) ----------------------------------- Bug fixes ^^^^^^^^^ -- Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts is one box was deeply embedded in the +1. Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts is one box was deeply embedded in the other. -- Fixed a bug in :ref:`simulate` where the "LOADING..." message was not showing correctly. -- Fixed a crash in the Python :ref:`passive viewer`, when used with models containing Flex objects. -- Fixed a bug in MJX where ``site_xmat`` was ignored in ``get_data`` and ``put_data`` -- Fixed a bug in MJX where ``efc_address`` was sometimes incorrectly calculated in ``get_data``. +2. Fixed a bug in :ref:`simulate` where the "LOADING..." message was not showing correctly. +3. Fixed a crash in the Python :ref:`passive viewer`, when used with models containing Flex objects. +4. Fixed a bug in MJX where ``site_xmat`` was ignored in ``get_data`` and ``put_data`` +5. Fixed a bug in MJX where ``efc_address`` was sometimes incorrectly calculated in ``get_data``. Version 3.1.0 (December 12, 2023) From 1c31676e2d37d3d7808d1511318ce6afce3dc91b Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 18 Dec 2023 14:56:12 -0800 Subject: [PATCH 40/44] Fix formatting error. PiperOrigin-RevId: 592018049 Change-Id: If0b59d18c380997c4c94795e275b56d2f6293c22 --- doc/changelog.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index c4970235..b70c7e9e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,8 +8,7 @@ Version 3.1.1 (December 18, 2023) Bug fixes ^^^^^^^^^ -1. Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts is one box was deeply embedded in the - other. +1. Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts if one box was deeply embedded in the other. 2. Fixed a bug in :ref:`simulate` where the "LOADING..." message was not showing correctly. 3. Fixed a crash in the Python :ref:`passive viewer`, when used with models containing Flex objects. 4. Fixed a bug in MJX where ``site_xmat`` was ignored in ``get_data`` and ``put_data`` From 7fb448613fbcd205f742628c353b3f20223b3c88 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 18 Dec 2023 15:02:35 -0800 Subject: [PATCH 41/44] Post release version update: Update to V3.1.2 PiperOrigin-RevId: 592019826 Change-Id: I64fd7da628bf3312ede4970b9b10ef5d0cc75b45 --- 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 40931b19..57b9ddd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.1.1 + VERSION 3.1.2 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 6a7fb828..2c2d58c6 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,1,1,0 -PRODUCTVERSION 3,1,1,0 +FILEVERSION 3,1,2,0 +PRODUCTVERSION 3,1,2,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.1.1" + VALUE "ProductVersion", "3.1.2" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.1.1" + VALUE "FileVersion", "3.1.2" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 14627b0b..255cb59c 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,1,1,0 -PRODUCTVERSION 3,1,1,0 +FILEVERSION 3,1,2,0 +PRODUCTVERSION 3,1,2,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.1.1" + VALUE "ProductVersion", "3.1.2" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.1.1" + VALUE "FileVersion", "3.1.2" 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 c43f3145..483383c4 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -522,7 +522,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`` - - 311 + - 312 - 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 0a608df9..4e443f40 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.1.1.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.2.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.1.1/lib/libmujoco.so.3.1.1`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.1.2/lib/libmujoco.so.3.1.2`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index ea861353..5ab8a363 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -24,7 +24,7 @@ extern "C" { #endif // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 311 +#define mjVERSION_HEADER 312 // needed to define size_t, fabs and log10 #include diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index a558c2ed..b0672ead 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.1.1" +version = "3.1.2" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -31,13 +31,13 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.1.1.dev0", + "mujoco>=3.1.2.dev0", "scipy", "trimesh", ] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.1.1" +Documentation = "https://mujoco.readthedocs.io/en/3.1.2" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.1.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.2/changelog.html" diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index e782497d..ed90bd7d 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.1.1.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.2.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.1.1 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.1.2 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 7b2dfa5e..e4cb8947 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.1.1 + 3.1.2 CFBundleGetInfoString - 3.1.1 + 3.1.2 CFBundleLongVersionString - 3.1.1 + 3.1.2 CFBundleShortVersionString - 3.1.1 + 3.1.2 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index ab298dc4..e8a31471 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.1.1" +version = "3.1.2" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.1.1" +Documentation = "https://mujoco.readthedocs.io/en/3.1.2" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.1.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.2/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 0d2c2c7f..3df0c6ce 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.1.1 + VERSION 3.1.2 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index fb416535..670aa6e2 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.1.1 + VERSION 3.1.2 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 819dc693..dfa91d0a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -38,8 +38,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 311 -#define mjVERSIONSTRING "3.1.1" + #define mjVERSION 312 +#define mjVERSIONSTRING "3.1.2" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index 3e9b9bfc..2d6e9ea0 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.1.1.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.1.2.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.1.1/lib/libmujoco.so.3.1.1", + "/.mujoco/mujoco-3.1.2/lib/libmujoco.so.3.1.2", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 213cba7a..fd2c33bf 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -108,7 +108,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 = 311; +public const int mjVERSION_HEADER = 312; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index a6f629b9..cff13baf 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.1.1", + "version": "3.1.2", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From 80f50c943c4fc500f8a7ea5bd8705836588260b4 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 18 Dec 2023 15:27:16 -0800 Subject: [PATCH 42/44] Add filterexact to MJX. PiperOrigin-RevId: 592026712 Change-Id: Ia73d67f36db5a4a1786a2532b5daa9e9a36f714c --- doc/changelog.rst | 8 ++++++ doc/mjx.rst | 8 +++--- mjx/mujoco/mjx/_src/forward.py | 41 +++++++++++++++++++++-------- mjx/mujoco/mjx/_src/forward_test.py | 40 ++++++++++++++++++++++++++++ mjx/mujoco/mjx/_src/test_util.py | 12 +++++++++ mjx/mujoco/mjx/_src/types.py | 10 +++---- 6 files changed, 99 insertions(+), 20 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index b70c7e9e..ad621fb4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,14 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +MJX +^^^ + +1. Add :ref:`dyntype` ``filterexact``. + Version 3.1.1 (December 18, 2023) ----------------------------------- diff --git a/doc/mjx.rst b/doc/mjx.rst index 92a709b6..cc052182 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -183,7 +183,7 @@ The following features are **fully supported** in MJX: * - :ref:`Transmission ` - ``TRN_JOINT`` * - :ref:`Actuator Dynamics ` - - ``NONE``, ``INTEGRATOR``, ``FILTER`` + - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT`` * - :ref:`Actuator Gain ` - ``FIXED``, ``AFFINE`` * - :ref:`Actuator Bias ` @@ -218,7 +218,7 @@ The following features are **in development** and coming soon: * - Dynamics - :ref:`Inverse ` * - :ref:`Transmission ` - - ``TRN_TENDON`` + - ``TRN_SITE``, ``TRN_TENDON`` * - :ref:`Actuator Dynamics ` - ``MUSCLE`` * - :ref:`Actuator Gain ` @@ -257,9 +257,9 @@ The following features are **unsupported**: * - Category - Feature * - :ref:`Transmission ` - - ``TRN_JOINTINPARENT``, ``TRN_SLIDERCRANK``, ``TRN_SITE``, ``TRN_BODY`` + - ``TRN_JOINTINPARENT``, ``TRN_SLIDERCRANK``, ``TRN_BODY`` * - :ref:`Actuator Dynamics ` - - ``FILTEREXACT``, ``USER`` + - ``USER`` * - :ref:`Actuator Gain ` - ``USER`` * - :ref:`Actuator Bias ` diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 827c8f31..1ff268e7 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -107,7 +107,7 @@ def fwd_actuation(m: Model, d: Data) -> Data: act_dot = jp.array(0.0) elif dyn_typ == DynType.INTEGRATOR: act_dot = ctrl - elif dyn_typ == DynType.FILTER: + elif dyn_typ in (DynType.FILTER, DynType.FILTEREXACT): act_dot = (ctrl - act) / jp.clip(dyn_prm[0], mujoco.mjMINVAL) else: raise NotImplementedError(f'dyntype {dyn_typ.name} not implemented.') @@ -228,6 +228,34 @@ def _integrate_pos( return jp.concatenate(qs) if qs else jp.empty((0,)) +def _next_activation(m: Model, d: Data, act_dot: jax.Array) -> jax.Array: + """Returns the next act given the current act_dot, after clamping.""" + act = d.act + + if not m.na: + return act + + actrange = jp.where( + m.actuator_actlimited[:, None], + m.actuator_actrange, + jp.array([-jp.inf, jp.inf]), + ) + + def fn(dyntype, dynprm, act, act_dot, actrange): + if dyntype == DynType.FILTEREXACT: + tau = jp.clip(dynprm[0], a_min=mujoco.mjMINVAL) + act = act + act_dot * tau * (1 - jp.exp(-m.opt.timestep / tau)) + else: + act = act + act_dot * m.opt.timestep + act = jp.clip(act, actrange[0], actrange[1]) + return act + + args = (m.actuator_dyntype, m.actuator_dynprm, act, act_dot, actrange) + act = scan.flat(m, fn, 'uuaau', 'a', *args, group_by='u') + + return act.reshape(m.na) + + @named_scope def _advance( m: Model, @@ -237,16 +265,7 @@ def _advance( qvel: Optional[jax.Array] = None, ) -> Data: """Advance state and time given activation derivatives and acceleration.""" - act = d.act - if m.na: - act = d.act + act_dot * m.opt.timestep - actrange = jp.where( - m.actuator_actlimited[:, None], - m.actuator_actrange, - jp.array([-jp.inf, jp.inf]), - ) - fn = lambda act, actrange: jp.clip(act, actrange[0], actrange[1]) - act = scan.flat(m, fn, 'au', 'a', act, actrange, group_by='u') + act = _next_activation(m, d, act_dot) # advance velocities d = d.replace(qvel=d.qvel + qacc * m.opt.timestep) diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index fbe28c9d..cc767ae8 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -134,5 +134,45 @@ class ForwardTest(absltest.TestCase): np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) +class ActuatorTest(absltest.TestCase): + _DYN_XML = """ + + + + + + + + + + + + + + + + + + + """ + + def test_dyntype(self): + m = mujoco.MjModel.from_xml_string(self._DYN_XML) + 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]) + + 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') + + mujoco.mj_Euler(m, d) + dx = jax.jit(mjx.euler)(mx, dx) + _assert_attr_eq(d, dx, 'act') + + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 870621f7..85cdb5dc 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -29,6 +29,8 @@ TEST_FILES: List[str] = [ ] _ACTUATOR_TYPES = ['motor', 'velocity', 'position', 'general', 'intvelocity'] +_DYN_TYPES = ['none', 'integrator', 'filter', 'filterexact'] +_DYN_PRMS = ['0.189', '2.1'] _JOINT_TYPES = ['free', 'hinge', 'slide', 'ball'] _JOINT_AXES = ['1 0 0', '0 1 0', '0 0 1'] _FRICTIONS = ['1.2 0.003 0.0002', '0.2 0.0001 0.0005'] @@ -124,6 +126,8 @@ def _make_geom( def _make_actuator(actuator_type: str, joint: str) -> Dict[str, str]: """Returns attributes for an actuator.""" attr = {'joint': joint} + + # set actuator type if actuator_type == 'motor': attr['gear'] = np.random.choice(_GEARS) elif actuator_type == 'position': @@ -139,10 +143,18 @@ def _make_actuator(actuator_type: str, joint: str) -> Dict[str, str]: elif actuator_type == 'velocity': attr['kv'] = np.random.choice(_KV_VEL) + # set dyntype + if actuator_type == 'general': + attr['dyntype'] = np.random.choice(_DYN_TYPES) + if attr['dyntype'] != 'none': + attr['dynprm'] = np.random.choice(_DYN_PRMS) + + # ctrlrange if p(50) and actuator_type != 'intvelocity': lb, ub = -np.random.uniform(), np.random.uniform() attr['ctrlrange'] = f'{lb:.2f} {ub:.2f}' + # forcerange if p(50): lb, ub = -np.random.uniform(), np.random.uniform() attr['forcerange'] = f'{lb*10:.2f} {ub*10:.2f}' diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 19b37630..9ccbe29a 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -20,10 +20,7 @@ from typing import Sequence import jax import jax.numpy as jp import mujoco -# pylint: disable=g-importing-member -from mujoco.mjx._src import dataclasses -from mujoco.mjx._src.dataclasses import PyTreeNode -# pylint: enable=g-importing-member +from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member import numpy as np @@ -167,11 +164,14 @@ class DynType(enum.IntEnum): Attributes: NONE: no internal dynamics; ctrl specifies force INTEGRATOR: integrator: da/dt = u + FILTER: linear filter: da/dt = (u-a) / tau + FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration """ NONE = mujoco.mjtDyn.mjDYN_NONE INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR FILTER = mujoco.mjtDyn.mjDYN_FILTER - # unsupported: FILTEREXACT, MUSCLE, USER + FILTEREXACT = mujoco.mjtDyn.mjDYN_FILTEREXACT + # unsupported: MUSCLE, USER class GainType(enum.IntEnum): From 77b4132c47fe652df27d014884b39b410301fb1c Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Tue, 19 Dec 2023 16:18:01 -0800 Subject: [PATCH 43/44] Add site transmission to MJX. PiperOrigin-RevId: 592373042 Change-Id: I54d941f4ee6f74404fe2aea7253f5b426d097e23 --- doc/changelog.rst | 1 + doc/mjx.rst | 4 +-- mjx/mujoco/mjx/_src/device_test.py | 6 ---- mjx/mujoco/mjx/_src/io.py | 4 +++ mjx/mujoco/mjx/_src/io_test.py | 41 +++++++++++++++++----- mjx/mujoco/mjx/_src/scan.py | 40 +++++++++++++++++----- mjx/mujoco/mjx/_src/smooth.py | 55 ++++++++++++++++++++---------- mjx/mujoco/mjx/_src/smooth_test.py | 35 +++++++++++++++++++ mjx/mujoco/mjx/_src/test_util.py | 33 +++++++++++++----- mjx/mujoco/mjx/_src/types.py | 4 ++- 10 files changed, 171 insertions(+), 52 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index ad621fb4..52592511 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,7 @@ MJX ^^^ 1. Add :ref:`dyntype` ``filterexact``. +2. Add :at:`site` transmission. Version 3.1.1 (December 18, 2023) diff --git a/doc/mjx.rst b/doc/mjx.rst index cc052182..c9263fa9 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -181,7 +181,7 @@ The following features are **fully supported** in MJX: * - :ref:`Joint ` - ``FREE``, ``BALL``, ``SLIDE``, ``HINGE`` * - :ref:`Transmission ` - - ``TRN_JOINT`` + - ``TRN_JOINT``, ``TRN_SITE`` * - :ref:`Actuator Dynamics ` - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT`` * - :ref:`Actuator Gain ` @@ -218,7 +218,7 @@ The following features are **in development** and coming soon: * - Dynamics - :ref:`Inverse ` * - :ref:`Transmission ` - - ``TRN_SITE``, ``TRN_TENDON`` + - ``TRN_TENDON`` * - :ref:`Actuator Dynamics ` - ``MUSCLE`` * - :ref:`Actuator Gain ` diff --git a/mjx/mujoco/mjx/_src/device_test.py b/mjx/mujoco/mjx/_src/device_test.py index 9f518d91..b4b9a0cd 100644 --- a/mjx/mujoco/mjx/_src/device_test.py +++ b/mjx/mujoco/mjx/_src/device_test.py @@ -129,12 +129,6 @@ class ValidateInputTest(absltest.TestCase): with self.assertRaises(NotImplementedError): mjx.device_put(m) - def test_trn(self): - m = test_util.load_test_file('pendula.xml') - m.actuator_trntype[0] = mujoco.mjtTrn.mjTRN_SITE - with self.assertRaises(NotImplementedError): - mjx.device_put(m) - def test_dyn(self): m = test_util.load_test_file('pendula.xml') m.actuator_dyntype[0] = mujoco.mjtDyn.mjDYN_MUSCLE diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 9396d7f3..ab255cfa 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -103,6 +103,10 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model: f'{[mj_type(m) for m in missing]} not supported' ) + # TODO: implement reference sites. + if any(m.actuator_trnid[:, 1] != -1): + raise NotImplementedError('refsite is not supported') + opt = _put_option(m.opt, device=device) stat = _put_statistic(m.stat, device=device) diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index a16a8780..08dec57a 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -82,7 +82,8 @@ _MULTIPLE_CONSTRAINTS = """ """ -class IoTest(parameterized.TestCase): +class ModelIOTest(parameterized.TestCase): + """IO tests for mjx.Model.""" def test_put_model(self): m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) @@ -133,7 +134,7 @@ class IoTest(parameterized.TestCase): ) self.assertTrue(m.opt.has_fluid_params) - def test_put_model_implicit_not_implemented(self): + def test_implicit_not_implemented(self): """Test that MJX guards against models with unimplemented features.""" with self.assertRaises(NotImplementedError): @@ -143,7 +144,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_cone_not_implemented(self): + def test_cone_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model( mujoco.MjModel.from_xml_string( @@ -151,7 +152,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_pgs_not_implemented(self): + def test_pgs_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model( mujoco.MjModel.from_xml_string( @@ -159,7 +160,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_site_actuator_not_implemented(self): + def test_site_actuator_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -174,7 +175,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_tendon_not_implemented(self): + def test_tendon_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -191,7 +192,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_condim_not_implemented(self): + def test_condim_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -207,7 +208,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_cylinder_not_implemented(self): + def test_cylinder_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -223,6 +224,29 @@ class IoTest(parameterized.TestCase): """)) + def test_refsite_not_implemented(self): + """Tests that site transmissions with refsites are not implemented.""" + with self.assertRaises(NotImplementedError): + mjx.put_model(mujoco.MjModel.from_xml_string(""" + + + + + + + + + + + + + + """)) + + +class DataIOTest(parameterized.TestCase): + """IO tests for mjx.Data.""" + def test_make_data(self): """Test that make_data returns the correct shapes.""" @@ -407,5 +431,6 @@ class IoTest(parameterized.TestCase): self.assertEqual(ds[0].ncon, 1) self.assertEqual(ds[1].ncon, 0) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/scan.py b/mjx/mujoco/mjx/_src/scan.py index ae0eeb30..9035c98e 100644 --- a/mjx/mujoco/mjx/_src/scan.py +++ b/mjx/mujoco/mjx/_src/scan.py @@ -49,7 +49,9 @@ def _take(obj: Y, idx: np.ndarray) -> Y: def take(x): # TODO(erikfrey): if this helps perf, add support for striding too - if ( + if not x.shape[0]: + return x + elif ( len(idx.shape) == 1 and idx.size > 0 and (idx == np.arange(idx[0], idx[0] + idx.size)).all() @@ -113,8 +115,12 @@ def _nvmap(f: Callable[..., Y], *args) -> Y: if isinstance(arg, np.ndarray) and not np.all(arg == arg[0]): raise RuntimeError(f'numpy arg elements do not match: {arg}') + # split out numpy and jax args np_args = [a[0] if isinstance(a, np.ndarray) else None for a in args] args = [a if n is None else None for n, a in zip(np_args, args)] + + # remove empty args that we should not vmap over + args = jax.tree_map(lambda a: a if a.shape[0] else None, args) in_axes = [None if a is None else 0 for a in args] def outer_f(*args, np_args=np_args): @@ -126,7 +132,15 @@ def _nvmap(f: Callable[..., Y], *args) -> Y: def _check_input(m: Model, args: Any, in_types: str) -> None: """Checks that scan input has the right shape.""" - size = {'b': m.nbody, 'j': m.njnt, 'q': m.nq, 'v': m.nv, 'u': m.nu, 'a': m.na} + size = { + 'b': m.nbody, + 'j': m.njnt, + 'q': m.nq, + 'v': m.nv, + 'u': m.nu, + 'a': m.na, + 's': m.nsite, + } for idx, (arg, typ) in enumerate(zip(args, in_types)): if len(arg) != size[typ]: raise IndexError( @@ -162,7 +176,7 @@ def flat( ) -> Y: r"""Scan a function across bodies or actuators. - Scan group data according to type and batch shape then calls vmap(f) on it. + Scan group data according to type and batch shape then calls vmap(f) on it.\ Args: m: an mjx model @@ -223,14 +237,22 @@ def flat( 'j': ( m.actuator_trnid[i, 0] if m.actuator_trntype[i] == TrnType.JOINT - else np.array(-1) + else -1 + ), + 's': ( + m.actuator_trnid[i, 0] + if m.actuator_trntype[i] == TrnType.SITE + else -1 ), } - # v/q associated with joint transmissions - typ_ids.update({ - 'v': np.nonzero(m.dof_jntid == typ_ids['j'])[0], - 'q': np.nonzero(_q_jointid(m) == typ_ids['j'])[0], - }) + v, q = np.array([-1]), np.array([-1]) + if m.actuator_trntype[i] == TrnType.JOINT: + # v/q are associated with the joint transmissions only + v = np.nonzero(m.dof_jntid == typ_ids['j'])[0] + q = np.nonzero(_q_jointid(m) == typ_ids['j'])[0] + + typ_ids.update({'v': v, 'q': q}) + return typ_ids # build up a grouping of type take-ids in body/actuator order diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index ba470bed..81c53213 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -19,11 +19,13 @@ from jax import numpy as jp import mujoco from mujoco.mjx._src import math from mujoco.mjx._src import scan +from mujoco.mjx._src import support # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import Model +from mujoco.mjx._src.types import TrnType # pylint: enable=g-importing-member @@ -432,38 +434,55 @@ def rne(m: Model, d: Data) -> Data: def transmission(m: Model, d: Data) -> Data: """Computes actuator/transmission lengths and moments.""" + # TODO: consider combining transmission calculation into fwd_actuation. if not m.nu: return d - def fn(gear, jnt_typ, m_j, qpos): - # handles joint transmissions only - if jnt_typ == JointType.FREE: + def fn(trntype, trnid, gear, jnt_typ, m_j, qpos, site_xpos, site_xmat): + if trntype == TrnType.JOINT: + if jnt_typ == JointType.FREE: + length = jp.zeros(1) + moment = gear + m_j = m_j + jp.arange(6) + elif jnt_typ == JointType.BALL: + axis, angle = math.quat_to_axis_angle(qpos) + length = jp.dot(axis * angle, gear[:3])[None] + moment = gear[:3] + m_j = m_j + jp.arange(3) + elif jnt_typ in (JointType.SLIDE, JointType.HINGE): + length = qpos * gear[0] + moment = gear[:1] + m_j = m_j[None] + else: + raise RuntimeError(f'unrecognized joint type: {JointType(jnt_typ)}') + + moment = jp.zeros((m.nv,)).at[m_j].set(moment) + elif trntype == TrnType.SITE: length = jp.zeros(1) - moment = gear - m_j = m_j + jp.arange(6) - elif jnt_typ == JointType.BALL: - axis, angle = math.quat_to_axis_angle(qpos) - length = jp.dot(axis * angle, gear[:3])[None] - moment = gear[:3] - m_j = m_j + jp.arange(3) - elif jnt_typ in (JointType.SLIDE, JointType.HINGE): - length = qpos * gear[0] - moment = gear[:1] - m_j = m_j[None] + jacp, jacr = support.jac( + m, d, site_xpos, jp.array(m.site_bodyid)[trnid[0]] + ) + jac = jp.concatenate((jacp, jacr), axis=1) + wrench = jp.concatenate((site_xmat @ gear[:3], site_xmat @ gear[3:])) + moment = jac @ wrench else: - raise RuntimeError(f'unrecognized joint type: {jnt_typ}') - moment = jp.zeros((m.nv,)).at[m_j].set(moment) + raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}') + return length, moment length, moment = scan.flat( m, fn, - 'ujjq', - 'uuuu', + 'uuujjqss', + 'uu', + m.actuator_trntype, + jp.array(m.actuator_trnid), m.actuator_gear, m.jnt_type, jp.array(m.jnt_dofadr), d.qpos, + d.site_xpos, + d.site_xmat, group_by='u', ) length = length.reshape((m.nu,)) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 203cb654..e83779ad 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -132,5 +132,40 @@ class SmoothTest(absltest.TestCase): dx = jax.jit(mjx.rne)(mx, dx) np.testing.assert_allclose(dx.qfrc_bias, 0) + def test_site_transmission(self): + m = mujoco.MjModel.from_xml_string(""" + + + + + + + + + + + + + + + + + + + + + + """) + d = mujoco.MjData(m) + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + + mujoco.mj_transmission(m, d) + dx = mjx.transmission(mx, dx) + _assert_attr_eq(d, dx, 'actuator_length') + _assert_attr_eq(d, dx, 'actuator_moment') + + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 85cdb5dc..66bab2f7 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -47,7 +47,7 @@ _SOLIMPS = [ _DIMS = ['3'] _MARGINS = ['0.0', '0.01', '0.02'] _GAPS = ['0.0', '0.005'] -_GEARS = ['20', '50', '100'] +_GEARS = ['2.1 0.0 3.3 0 2.3 0', '5.0 3.1 0 2.3 0.0 1.1'] def p(pct: int) -> bool: @@ -123,14 +123,21 @@ def _make_geom( return attr -def _make_actuator(actuator_type: str, joint: str) -> Dict[str, str]: +def _make_actuator( + actuator_type: str, joint: str | None = None, site: str | None = None +) -> Dict[str, str]: """Returns attributes for an actuator.""" - attr = {'joint': joint} + if joint: + attr = {'joint': joint} + elif site: + attr = {'site': site} + else: + raise ValueError('must provide a joint or site name') + + attr['gear'] = np.random.choice(_GEARS) # set actuator type - if actuator_type == 'motor': - attr['gear'] = np.random.choice(_GEARS) - elif actuator_type == 'position': + if actuator_type == 'position': attr['kp'] = np.random.choice(_KP_POS) elif actuator_type == 'general': attr['biastype'] = 'affine' @@ -245,6 +252,7 @@ def create_mjcf( pos = f'{body_pos[0]:.3f} {body_pos[1]:.3f} {body_pos[2] + z_pos:.3f}' n_bodies = len(list(mjcf.iter('body'))) child = ET.SubElement(body, 'body', {'pos': pos, 'name': f'body{n_bodies}'}) + ET.SubElement(child, 'site', {'name': f'site{n_bodies}'}) n_joints = len(list(mjcf.iter('joint'))) for nj in range(np.random.randint(1, max_stacked_joints + 1)): @@ -282,17 +290,28 @@ def create_mjcf( for _ in range(num_trees): make_tree(world, 0) + bodies = list(mjcf.iter('body')) + n_bodies = len(bodies) + # actuators if add_actuators: actuator = ET.SubElement(mjcf, 'actuator') n_joints = len(list(mjcf.iter('joint'))) nu = np.random.randint(1, n_joints + 1) actuators = [] + + # joint transmission for i in range(nu): actuator_type = np.random.choice(_ACTUATOR_TYPES) attr = _make_actuator(actuator_type, joint=f'joint{i}') actuators.append((actuator_type, attr)) + # site transmission + for i in range(np.random.randint(0, n_bodies)): + actuator_type = np.random.choice(_ACTUATOR_TYPES) + attr = _make_actuator(actuator_type, site=f'site{i}') + actuators.append((actuator_type, attr)) + np.random.shuffle(actuators) for typ, attr in actuators: ET.SubElement(actuator, typ, attr) @@ -320,9 +339,7 @@ def create_mjcf( ET.SubElement(contact, 'pair', attr) # exclude contacts - bodies = list(mjcf.iter('body')) body_names = [b.get('name') for b in bodies] - n_bodies = len(bodies) for _ in range(min(max_contact_excludes, (n_bodies * (n_bodies - 1) // 2))): if p(50): continue diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 9ccbe29a..ece81623 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -153,9 +153,11 @@ class TrnType(enum.IntEnum): Attributes: JOINT: force on joint + SITE: force on site """ JOINT = mujoco.mjtTrn.mjTRN_JOINT - # unsupported: JOINTINPARENT, SLIDERCRANK, TENDON, SITE, BODY + SITE = mujoco.mjtTrn.mjTRN_SITE + # unsupported: JOINTINPARENT, SLIDERCRANK, TENDON, BODY class DynType(enum.IntEnum): From 2a46ca37874181bbae6ac148ba9797a53101d0b9 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Wed, 20 Dec 2023 05:01:55 -0800 Subject: [PATCH 44/44] Update the documentation on the maximum number of contacts from hfield collisions. The docs were incorrectly asserting that the limit is 9 contacts, which hasn't been true since version 1.5. This fixes #1285. PiperOrigin-RevId: 592526249 Change-Id: Id45d73666fec8dcfd18d11605975e787f2cfeba2 --- doc/XMLreference.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 6ca8c13b..8fb1edd3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1378,9 +1378,9 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe | For collision detection, a height field is treated as a union of triangular prisms. Collisions between height fields and other geoms (except for planes and other height fields which are not supported) are computed by first selecting the sub-grid of prisms that could collide with the geom based on its bounding box, and then using the general convex - collider. The number of possible contacts between a height field and a geom is limited to 9; any contacts beyond that - are discarded. To avoid penetration due to discarded contacts, the spatial features of the height field should be - large compared to the geoms it collides with. + collider. The number of possible contacts between a height field and a geom is limited to 50 + (:ref:`mjMAXCONPAIR `); any contacts beyond that are discarded. To avoid penetration due to discarded + contacts, the spatial features of the height field should be large compared to the geoms it collides with. .. _asset-hfield-name: