From f62792594fcb062943ff34601b46bac63317fa96 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Mon, 22 Jul 2024 03:21:09 -0700 Subject: [PATCH 1/2] Add a new disable flag `mjDSBL_AUTORESETNAN`. PiperOrigin-RevId: 654677473 Change-Id: Id92383fe413729bf6697353aa6e0bc8a946ec7e8 --- doc/changelog.rst | 5 +++-- doc/includes/references.h | 3 ++- include/mujoco/mjmodel.h | 3 ++- introspect/enums.py | 3 ++- simulate/simulate.cc | 6 ++++++ src/engine/engine_forward.c | 16 ++++++++++++---- src/engine/engine_support.c | 3 ++- unity/Runtime/Bindings/MjBindings.cs | 3 ++- 8 files changed, 31 insertions(+), 11 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index ab2bdbdc..6c818847 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,15 +9,16 @@ General ^^^^^^^ 1. Renamed ``mjModel.tex_rbg`` to ``mjModel.tex_data``. +2. Added a new disable flag ``mjDSBL_AUTORESETNAN`` to disable automatic reset when NaNs or infinities are produced. MJX ^^^ -2. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs. +3. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs. Python bindings ^^^^^^^^^^^^^^^ -2. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and +4. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and ``mujoco.spec.compile``. Version 3.2.0 (Jul 15, 2024) diff --git a/doc/includes/references.h b/doc/includes/references.h index cce47515..10e62a77 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -418,8 +418,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator + mjDSBL_AUTORESETNAN = 1<<15, // automatic reset when numerical issues are detected - mjNDISABLE = 15 // number of disable flags + mjNDISABLE = 16 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index b559a98c..73697a66 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -61,8 +61,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator + mjDSBL_AUTORESETNAN = 1<<15, // automatic reset when numerical issues are detected - mjNDISABLE = 15 // number of disable flags + mjNDISABLE = 16 // number of disable flags } mjtDisableBit; diff --git a/introspect/enums.py b/introspect/enums.py index 7308b664..52bf4c6a 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -42,7 +42,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_SENSOR', 4096), ('mjDSBL_MIDPHASE', 8192), ('mjDSBL_EULERDAMP', 16384), - ('mjNDISABLE', 15), + ('mjDSBL_AUTORESETNAN', 32768), + ('mjNDISABLE', 16), ]), )), ('mjtEnableBit', diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 17ae8db6..9fe5ae7c 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -739,12 +739,18 @@ void MakePhysicsSection(mj::Simulate* sim) { for (int i=0; idisable + i; + if ((1 << i) == mjDSBL_AUTORESETNAN) { + defFlag[0].state = 0; + } else { + defFlag[0].state = 2; + } mjui_add(&sim->ui0, defFlag); } mjui_add(&sim->ui0, defEnableFlags); for (int i=0; ienable + i; + defFlag[0].state = 2; mjui_add(&sim->ui0, defFlag); } // add contact override diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 15f5c5e7..6c78663e 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -52,7 +52,9 @@ void mj_checkPos(const mjModel* m, mjData* d) { for (int i=0; i < m->nq; i++) { if (mju_isBad(d->qpos[i])) { mj_warning(d, mjWARN_BADQPOS, i); - mj_resetData(m, d); + if (!(m->opt.disableflags & mjDSBL_AUTORESETNAN)) { + mj_resetData(m, d); + } d->warning[mjWARN_BADQPOS].number++; d->warning[mjWARN_BADQPOS].lastinfo = i; return; @@ -67,7 +69,9 @@ void mj_checkVel(const mjModel* m, mjData* d) { for (int i=0; i < m->nv; i++) { if (mju_isBad(d->qvel[i])) { mj_warning(d, mjWARN_BADQVEL, i); - mj_resetData(m, d); + if (!(m->opt.disableflags & mjDSBL_AUTORESETNAN)) { + mj_resetData(m, d); + } d->warning[mjWARN_BADQVEL].number++; d->warning[mjWARN_BADQVEL].lastinfo = i; return; @@ -82,10 +86,14 @@ void mj_checkAcc(const mjModel* m, mjData* d) { for (int i=0; i < m->nv; i++) { if (mju_isBad(d->qacc[i])) { mj_warning(d, mjWARN_BADQACC, i); - mj_resetData(m, d); + if (!(m->opt.disableflags & mjDSBL_AUTORESETNAN)) { + mj_resetData(m, d); + } d->warning[mjWARN_BADQACC].number++; d->warning[mjWARN_BADQACC].lastinfo = i; - mj_forward(m, d); + if (!(m->opt.disableflags & mjDSBL_AUTORESETNAN)) { + mj_forward(m, d); + } return; } } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index c57a2e8b..b0e86887 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -57,7 +57,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Refsafe", "Sensor", "Midphase", - "Eulerdamp" + "Eulerdamp", + "AutoResetNaN" }; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index dea70955..46f89ab6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -157,7 +157,8 @@ public enum mjtDisableBit : int{ mjDSBL_SENSOR = 4096, mjDSBL_MIDPHASE = 8192, mjDSBL_EULERDAMP = 16384, - mjNDISABLE = 15, + mjDSBL_AUTORESETNAN = 32768, + mjNDISABLE = 16, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, From e79efa60d6afd191610f5462dc9a9b47aac98278 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 22 Jul 2024 08:29:31 -0700 Subject: [PATCH 2/2] Reset tree indices before recompiling. Fixes #1822. PiperOrigin-RevId: 654762203 Change-Id: I7f656f7dad376d4899229a8741315c8f821bfa56 --- src/user/user_model.cc | 1 + test/user/user_api_test.cc | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f365a994..377ed8e6 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3206,6 +3206,7 @@ mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) { bodies_[i]->subtreedofs = 0; } mjCBody* world = bodies_[0]; + ResetTreeLists(); Clear(); bodies_.push_back(world); } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 093c07c1..39f224f6 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -186,6 +186,38 @@ TEST_F(PluginTest, RecompileCompare) { } } +TEST_F(PluginTest, RecompileEdit) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + std::array er; + mjSpec *spec = mj_parseXMLString(xml, 0, er.data(), er.size()); + EXPECT_THAT(spec, NotNull()) << er.data(); + mjModel *m1 = mj_compile(spec, nullptr); + EXPECT_THAT(m1, NotNull()); + + // add a geom + mjsBody *world = mjs_findBody(spec, "world"); + mjsGeom *geom = mjs_addGeom(world, nullptr); + geom->size[0] = 1; + + // compile again + mjModel *m2 = mj_compile(spec, nullptr); + EXPECT_THAT(m2, NotNull()); + + mj_deleteModel(m1); + mj_deleteModel(m2); + mj_deleteSpec(spec); +} + // ------------------- test cache with modified assets ------------------------- TEST_F(PluginTest, RecompileCompareObjCache) {