From f712eed4cefef98d729c1948f0d03b18d738d46c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 19 May 2026 07:14:45 -0700 Subject: [PATCH] Allow flex sleeping PiperOrigin-RevId: 917817500 Change-Id: Ia3bd5e52e7c2eaa3f70c81352d82c130b1d357f6 --- doc/XMLreference.rst | 2 +- doc/changelog.rst | 1 + doc/programming/simulation.rst | 12 - model/flex/hollow_vs_solid.xml | 52 ++-- simulate/simulate.cc | 2 +- src/engine/engine_collision_driver.c | 17 +- src/engine/engine_core_constraint.c | 11 +- src/engine/engine_island.c | 7 +- src/engine/engine_setconst.c | 7 +- src/engine/engine_sleep.c | 105 +++++++- src/engine/engine_sleep.h | 3 + src/engine/engine_vis_visualize.c | 39 +++ test/engine/engine_sleep_test.cc | 225 ++++++++++++++++++ test/engine/testdata/sleep/flex_contact.xml | 23 ++ test/engine/testdata/sleep/flex_edge.xml | 21 ++ test/engine/testdata/sleep/flex_mocap.xml | 22 ++ test/engine/testdata/sleep/flex_nocnstr.xml | 15 ++ test/engine/testdata/sleep/flex_state.xml | 16 ++ test/engine/testdata/sleep/flex_static.xml | 22 ++ .../engine/testdata/sleep/hollow_vs_solid.xml | 89 +++++++ test/engine/testdata/sleep/scene.xml | 26 ++ 21 files changed, 660 insertions(+), 57 deletions(-) create mode 100644 test/engine/testdata/sleep/flex_contact.xml create mode 100644 test/engine/testdata/sleep/flex_edge.xml create mode 100644 test/engine/testdata/sleep/flex_mocap.xml create mode 100644 test/engine/testdata/sleep/flex_nocnstr.xml create mode 100644 test/engine/testdata/sleep/flex_state.xml create mode 100644 test/engine/testdata/sleep/flex_static.xml create mode 100644 test/engine/testdata/sleep/hollow_vs_solid.xml create mode 100644 test/engine/testdata/sleep/scene.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 75d6fe03..85b889df 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2113,7 +2113,7 @@ defined. Its body name is automatically defined as "world". - Trees which are connected by tendons which have non-zero stiffness and damping are not allowed to sleep (overridable). - Trees which are connected by tendons which connect more than two trees are not allowed to sleep (not overridable). - - :ref:`flexes` are not allowed to sleep (not overridable). + - Constraint-free :ref:`flexes` are not allowed to sleep (not overridable). - All other trees are allowed to sleep (overridable). The policies :at-val:`never` and :at-val:`allowed` constitute user overrides of the automatic compiler policy. diff --git a/doc/changelog.rst b/doc/changelog.rst index b05d0c2f..93d6d365 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,7 @@ General - The pseudo-random constraint visitation order in the :ref:`PGS solver`, introduced in the previous release, now uses a fixed seed. The previous implementation seeded with ``mjData.time``, which introduced subtle yet undesirable time dependence. +- Flexes are now allowed to sleep, with the exception of completely passive (constraint-free) flexes. .. admonition:: Breaking API changes :class: attention diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index 8b1e21bb..702a9768 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -1176,11 +1176,6 @@ which are initialized asleep. These can be placed in mid-air or in deep collisio Notes ^^^^^ -.. admonition:: Subject to change - :class: warning - - Sleeping is a new feature (Nov 2025) that is subject to change and may have latent bugs. - **Sleeping actuators** As explained in the :ref:`body/sleep ` documentation, trees with actuators are by default not allowed to sleep, but this can be overridden by the user. The reason sleeping is not allowed by default is that once an actuator @@ -1269,13 +1264,6 @@ Notes **RK4 integrator** The RK4 integrator is not currently supported, due to the subtleties of waking inside the sub-steps. -**Latent bugs** - Sleeping may have latent bugs. These bugs may generally come in two varieties: - - - Quantities which could be skipped are instead recomputed. The only observable effect of such a bug would be that - the simulation is slower than it could be. This type of bug can only be diagnosed with detailed profiling. - - Actual bugs. Hopefully these will lead to informative runtime errors, please report any to the development team. - .. _siCoordinate: diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml index e9ac9d8a..9ea1fbcc 100644 --- a/model/flex/hollow_vs_solid.xml +++ b/model/flex/hollow_vs_solid.xml @@ -31,6 +31,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 76e2b788..4249d7a9 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -756,7 +756,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d, } // add islands if enabled - if (!mjDISABLED(mjDSBL_ISLAND) && d->nisland > 0) { + if (!mjDISABLED(mjDSBL_ISLAND)) { mju::sprintf_arr(tmp, "\n%d", d->nisland); mju::strcat_arr(content, tmp); mju::strcat_arr(title, "\nIslands"); diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 2d5ad3ea..5c31b16c 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -30,6 +30,7 @@ #include "engine/engine_inline.h" #include "engine/engine_macro.h" #include "engine/engine_memory.h" +#include "engine/engine_sleep.h" #include "engine/engine_sort.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -437,6 +438,7 @@ void mj_collision(const mjModel* m, mjData* d) { int nexclude = m->nexclude, npair = m->npair, nbody = m->nbody; int nbodyflex = m->nbody + m->nflex; + int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < nbody; // reset the size of the contact array and invalidate efc arrays d->ncon = 0; @@ -627,6 +629,9 @@ void mj_collision(const mjModel* m, mjData* d) { // flex self-collisions for (int f=0; f < m->nflex; f++) { if (!m->flex_rigid[f] && (m->flex_contype[f] & m->flex_conaffinity[f])) { + // skip if flex is asleep + if (sleep_filter && mj_sleepState(m, d, mjOBJ_FLEX, f) == mjS_ASLEEP) continue; + // internal collisions if (m->flex_internal[f]) { int ncon_before = d->ncon; @@ -1403,8 +1408,9 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { add_pair(m, b1, b2, &npair, bfpair, maxpair); } - // add all b1:flex pairs + // add body:flex pairs, skip if flex asleep for (int f=0; f < nflex; f++) { + if (sleep_filter && mj_sleepState(m, d, mjOBJ_FLEX, f) == mjS_ASLEEP) continue; add_pair(m, b1, nbody+f, &npair, bfpair, maxpair); } } @@ -1499,6 +1505,15 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { } } + // flex pair: skip if neither side is dynamically awake + else if (sleep_filter) { + int awake1 = (bf1 >= nbody) ? mj_sleepState(m, d, mjOBJ_FLEX, bf1-nbody) == mjS_AWAKE + : d->body_awake[bf1] == mjS_AWAKE && m->body_treeid[bf1] >= 0; + int awake2 = (bf2 >= nbody) ? mj_sleepState(m, d, mjOBJ_FLEX, bf2-nbody) == mjS_AWAKE + : d->body_awake[bf2] == mjS_AWAKE && m->body_treeid[bf2] >= 0; + if (!awake1 && !awake2) continue; + } + // add bodyflex pair if there is room in buffer add_pair(m, bf1, bf2, &npair, bfpair, maxpair); } diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 6600c183..2b7a1dbf 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1463,7 +1463,7 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di m->geom_bodyid[con->geom[side]] : m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; } - // compute Jacobian differences, skipping common dofs + // compute Jacobian differences, skipping common DOFs if (dim > 3) { return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr, mj_isSparse(m), 1); @@ -2492,6 +2492,15 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { mjERROR("contact %d involves sleeping geom %d", i, asleep1 ? g1 : g2); } } + + // check flex contact sides + for (int side = 0; side < 2; side++) { + if (con->geom[side] >= 0) continue; + int b = mj_flexBody(m, con, side); + if (d->body_awake[m->body_weldid[b]] == mjS_ASLEEP) { + mjERROR("contact %d involves sleeping flex %d", i, con->flex[side]); + } + } } // compute NV only if nnz requested diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index ebda019d..a8aff1ce 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -331,7 +331,8 @@ static int findEdges(const mjModel* m, const mjData* d, // unless it is a flex equality, where the tree pattern changes per dof if (!(efc_type == mjCNSTR_EQUALITY && (m->eq_type[efc_id] == mjEQ_FLEX || - m->eq_type[efc_id] == mjEQ_FLEXVERT))) { + m->eq_type[efc_id] == mjEQ_FLEXVERT || + m->eq_type[efc_id] == mjEQ_FLEXSTRAIN))) { // copy tree assignment from previous constraint and continue efc_tree[i] = efc_tree[i-1]; continue; @@ -482,7 +483,7 @@ void mj_island(const mjModel* m, mjData* d) { // compute dof_island, island_nv mju_zeroInt(d->island_nv, nisland); for (int i=0; i < nv; i++) { - // assign dofs to islands + // assign DOFs to islands int island = tree_island[m->dof_treeid[i]]; // -1 if unconstrained d->dof_island[i] = island; @@ -499,7 +500,7 @@ void mj_island(const mjModel* m, mjData* d) { } // compute dof <-> idof maps - int* island_nv2 = mjSTACKALLOC(d, nisland + 1, int); // last element counts unconstrained dofs + int* island_nv2 = mjSTACKALLOC(d, nisland + 1, int); // last element counts unconstrained DOFs mju_zeroInt(island_nv2, nisland + 1); for (int dof=0; dof < nv; dof++) { int island = d->dof_island[dof]; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index d47c570e..06f5f313 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -291,8 +291,13 @@ static void setFixed(mjModel* m, mjData* d) { } } - // flexes: trees containing bodies that are part of any flex are not allowed to sleep + // flexes: constraint-free trees are not allowed to sleep for (int i = 0; i < m->nflex; ++i) { + // constrained flexes are allowed to sleep + if (m->flex_edgeequality[i]) { + continue; + } + // node-based flex if (m->flex_interp[i]) { int nodenum = m->flex_nodenum[i]; diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index 9805e0ab..55c9d732 100644 --- a/src/engine/engine_sleep.c +++ b/src/engine/engine_sleep.c @@ -275,7 +275,32 @@ int mj_wake(const mjModel* m, mjData* d) { } -// wake sleeping trees that touch awake trees, return number of woke trees +// get a representative body from a flex contact side +int mj_flexBody(const mjModel* m, const mjContact* con, int side) { + int f = con->flex[side]; + + // flex vertex contact (non-interpolated) + if (con->vert[side] >= 0 && m->flex_interp[f] == 0) { + return m->flex_vertbodyid[m->flex_vertadr[f] + con->vert[side]]; + } + + // flex element contact + if (con->elem[side] >= 0) { + if (m->flex_interp[f] == 0) { + int dim = m->flex_dim[f]; + const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(dim+1); + return m->flex_vertbodyid[m->flex_vertadr[f] + edata[0]]; + } else { + return m->flex_nodebodyid[m->flex_nodeadr[f]]; + } + } + + // flex vertex contact (interpolated): use first node + return m->flex_nodebodyid[m->flex_nodeadr[f]]; +} + + +// wake sleeping trees with collision contact, return number of woke trees int mj_wakeCollision(const mjModel* m, mjData* d) { int ntree = m->ntree, ncon = d->ncon, nwoke = 0; @@ -287,13 +312,10 @@ int mj_wakeCollision(const mjModel* m, mjData* d) { for (int i=0; i < ncon; i++) { const mjContact* con = d->contact + i; - // only geom-geom contacts are handled - if (con->geom[0] < 0 || con->geom[1] < 0) { - continue; - } + // resolve body on each side + int b1 = con->geom[0] >= 0 ? m->geom_bodyid[con->geom[0]] : mj_flexBody(m, con, 0); + int b2 = con->geom[1] >= 0 ? m->geom_bodyid[con->geom[1]] : mj_flexBody(m, con, 1); - int b1 = m->geom_bodyid[con->geom[0]]; - int b2 = m->geom_bodyid[con->geom[1]]; int tree1 = m->body_treeid[b1]; int tree2 = m->body_treeid[b2]; @@ -391,17 +413,59 @@ int mj_wakeEquality(const mjModel* m, mjData* d) { tree2 = m->body_treeid[m->site_bodyid[id2]]; } break; + case mjEQ_JOINT: tree1 = id1 >= 0 ? m->body_treeid[m->jnt_bodyid[id1]] : -1; tree2 = id2 >= 0 ? m->body_treeid[m->jnt_bodyid[id2]] : -1; break; + case mjEQ_TENDON: mjERROR("tendon equality does not yet support sleeping"); continue; + case mjEQ_FLEX: case mjEQ_FLEXVERT: - mjERROR("flex equality does not yet support sleeping"); + case mjEQ_FLEXSTRAIN: { + int f = id1; + int num, adr; + const int* bodyid; + if (m->flex_interp[f]) { + num = m->flex_nodenum[f]; + adr = m->flex_nodeadr[f]; + bodyid = m->flex_nodebodyid; + } else { + num = m->flex_vertnum[f]; + adr = m->flex_vertadr[f]; + bodyid = m->flex_vertbodyid; + } + + // find the first awake tree, if any + int awake_tree = -1; + for (int j = 0; j < num; j++) { + int treeid = m->body_treeid[bodyid[adr+j]]; + if (treeid >= 0 && d->tree_awake[treeid]) { + awake_tree = treeid; + break; + } + } + + // wake sleeping island: find first sleeping tree, wakeTree wakes them all + if (awake_tree >= 0) { + int wakeval = d->tree_asleep[awake_tree]; + for (int j = 0; j < num; j++) { + int treeid = m->body_treeid[bodyid[adr+j]]; + if (treeid >= 0 && !d->tree_awake[treeid]) { + nwoke += mj_wakeTree(d->tree_asleep, m->ntree, treeid, wakeval); + #ifdef MJ_DEBUG_SLEEP + printf("woke tree %d due to flex equality %d at t=%g\n", treeid, i, d->time); + #endif + break; + } + } + } continue; + } + default: continue; } @@ -643,6 +707,7 @@ static mjtSleepState mj_equalitySleepState(const mjModel* m, const mjData* d, in break; case mjEQ_FLEX: case mjEQ_FLEXVERT: + case mjEQ_FLEXSTRAIN: objtype = mjOBJ_FLEX; break; default: @@ -751,9 +816,27 @@ mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int case mjOBJ_SENSOR: return mj_sensorSleepState(m, d, i); - // always awake - case mjOBJ_FLEX: - return mjS_AWAKE; + case mjOBJ_FLEX: { + // all dynamic bodies share sleep state: find and check the first one + int num, adr; + const int* bodyid; + if (m->flex_interp[i]) { + num = m->flex_nodenum[i]; + adr = m->flex_nodeadr[i]; + bodyid = m->flex_nodebodyid; + } else { + num = m->flex_vertnum[i]; + adr = m->flex_vertadr[i]; + bodyid = m->flex_vertbodyid; + } + for (int j = 0; j < num; j++) { + int b = bodyid[adr+j]; + if (m->body_treeid[b] >= 0) { + return (mjtSleepState) d->body_awake[b]; + } + } + return mjS_STATIC; + } // undefined sleep state, return AWAKE case mjOBJ_UNKNOWN: diff --git a/src/engine/engine_sleep.h b/src/engine/engine_sleep.h index 4908d7c3..5beb23f0 100644 --- a/src/engine/engine_sleep.h +++ b/src/engine/engine_sleep.h @@ -53,6 +53,9 @@ int mj_wakeEquality(const mjModel* m, mjData* d); // put trees to sleep according to tolerance, return number of slept trees int mj_sleep(const mjModel* m, mjData* d); +// get a representative body from a flex contact side +int mj_flexBody(const mjModel* m, const mjContact* con, int side); + // return sleep state of object i MJAPI mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int i); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 0e058bd1..28ccb448 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -772,6 +772,45 @@ static void addFlexGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, thisgeom->size[0] = m->flex_radius[i]; setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags); + // override if visualizing islands + if (vopt->flags[mjVIS_ISLAND]) { + // find first dynamic body in flex + int bodyid = -1; + if (m->flex_interp[i]) { + int nodeadr = m->flex_nodeadr[i]; + for (int j=0; j < m->flex_nodenum[i] && bodyid < 0; j++) { + int b = m->flex_nodebodyid[nodeadr+j]; + if (m->body_treeid[b] >= 0) bodyid = b; + } + } else { + int vertadr = m->flex_vertadr[i]; + for (int j=0; j < m->flex_vertnum[i] && bodyid < 0; j++) { + int b = m->flex_vertbodyid[vertadr+j]; + if (m->body_treeid[b] >= 0) bodyid = b; + } + } + + if (bodyid >= 0) { + // strip material + thisgeom->matid = -1; + + int weld_id = m->body_weldid[bodyid]; + int dof = m->body_dofadr[weld_id]; + int island = d->nisland ? d->dof_island[dof] : -1; + int h = island >= 0 ? d->island_dofadr[island] : -1; + int awake = d->body_awake[bodyid]; + + // if sleep is enabled, color by first tree dof + if (h == -1 && mjENABLED(mjENBL_SLEEP)) { + int tree = m->dof_treeid[dof]; + if (!awake) tree = mj_sleepCycle(d->tree_asleep, m->ntree, tree); + h = m->tree_dofadr[tree]; + } + + islandColor(thisgeom->rgba, h, awake); + } + } + // set texcoord if (m->flex_texcoordadr[i] >= 0) { thisgeom->texcoord = 1; diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 1245dc83..f8b2e9c1 100644 --- a/test/engine/engine_sleep_test.cc +++ b/test/engine/engine_sleep_test.cc @@ -600,5 +600,230 @@ TEST_F(SleepTest, InitIslandFail) { "the first tree that could not be slept.")); } +// Test that a constrained flex eventually goes to sleep. +TEST_F(SleepTest, FlexEdgeSleep) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_edge.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_GT(m->flex_edgeequality[0], 0); + mjData* d = mj_makeData(m); + + // give initial velocity + d->qvel[0] = 0.1; + + // step until body goes to sleep + for (int step = 0; step < 2000; step++) { + mj_step(m, d); + if (d->ntree_awake == 0) break; + } + + EXPECT_EQ(d->ntree_awake, 0) << "flex did not go to sleep"; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test that a constraint-free flex never sleeps. +TEST_F(SleepTest, FlexNoConstraintNeverSleeps) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_nocnstr.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_EQ(m->flex_edgeequality[0], 0); + mjData* d = mj_makeData(m); + + // step for a while + for (int step = 0; step < 500; step++) { + mj_step(m, d); + } + + // tree should have AUTO_NEVER policy + EXPECT_EQ(m->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); + EXPECT_GT(d->ntree_awake, 0) << "constraint-free flex should not sleep"; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test that mj_sleepState returns correct values for flex objects. +TEST_F(SleepTest, FlexSleepState) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_state.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // initially awake + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, 0), mjS_AWAKE); + + // step until flex goes to sleep + for (int step = 0; step < 1000; step++) { + mj_step(m, d); + if (d->ntree_awake == 0) break; + } + + // flex should be asleep + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, 0), mjS_ASLEEP); + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test that a sleeping flex wakes on contact with a falling ball, +// then both go back to sleep after the ball rolls off. +TEST_F(SleepTest, FlexWakeContact) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_contact.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + int flexid = mj_name2id(m, mjOBJ_FLEX, "f1"); + ASSERT_GE(flexid, 0); + + // phase 1: flex settles and goes to sleep + for (int step = 0; step < 5000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, flexid) == mjS_ASLEEP) break; + } + ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, flexid), mjS_ASLEEP) + << "flex did not go to sleep"; + + // phase 2: ball hits flex, flex wakes up + for (int step = 0; step < 5000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, flexid) == mjS_AWAKE) break; + } + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, flexid), mjS_AWAKE) + << "flex should have been woken by ball contact"; + + // phase 3: ball rolls off, everything goes back to sleep + for (int step = 0; step < 10000; step++) { + mj_step(m, d); + if (d->ntree_awake == 0) break; + } + EXPECT_EQ(d->ntree_awake, 0) << "all trees should be asleep again"; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test full sleep/wake lifecycle with two grippers and two flex objects. +TEST_F(SleepTest, HollowVsSolidSleep) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/hollow_vs_solid.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // look up flex IDs + int solid_flex = mj_name2id(m, mjOBJ_FLEX, "soft_mesh"); + int hollow_flex = mj_name2id(m, mjOBJ_FLEX, "soft_mesh_2"); + ASSERT_GE(solid_flex, 0); + ASSERT_GE(hollow_flex, 0); + + // look up actuator IDs + int grasp_r = mj_name2id(m, mjOBJ_ACTUATOR, "grasp_r"); + int grasp_s = mj_name2id(m, mjOBJ_ACTUATOR, "grasp_s"); + ASSERT_GE(grasp_r, 0); + ASSERT_GE(grasp_s, 0); + + // phase 1: everything starts awake + mj_forward(m, d); + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_AWAKE); + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_AWAKE); + + // phase 2: both flexes go to sleep within 500 steps + for (int step = 0; step < 500; step++) { + mj_step(m, d); + } + ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP) + << "solid flex did not go to sleep"; + ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_ASLEEP) + << "hollow flex did not go to sleep"; + + // phase 3: close gripper_solid (grasp_r), it wakes soft_mesh_2 (hollow) + d->ctrl[grasp_r] = 1; + for (int step = 0; step < 2000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex) == mjS_AWAKE) break; + } + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_AWAKE) + << "hollow flex should be woken by gripper_solid"; + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP) + << "solid flex should still be asleep"; + + // phase 4: close gripper_hollow (grasp_s), it wakes soft_mesh (solid) + d->ctrl[grasp_s] = 1; + for (int step = 0; step < 2000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, solid_flex) == mjS_AWAKE) break; + } + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_AWAKE) + << "solid flex should be woken by gripper_hollow"; + + // phase 5: open gripper_solid (grasp_r=0), hollow flex goes back to sleep + d->ctrl[grasp_r] = 0; + for (int step = 0; step < 2000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex) == mjS_ASLEEP) break; + } + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_ASLEEP) + << "hollow flex should go back to sleep after gripper release"; + + // phase 6: open gripper_hollow (grasp_s=0), solid flex goes back to sleep + d->ctrl[grasp_s] = 0; + for (int step = 0; step < 2000; step++) { + mj_step(m, d); + if (mj_sleepState(m, d, mjOBJ_FLEX, solid_flex) == mjS_ASLEEP) break; + } + EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP) + << "solid flex should go back to sleep after gripper release"; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test that a sleeping flex touching a non-world static body doesn't crash. +TEST_F(SleepTest, FlexStaticContact) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_static.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // step until flex goes to sleep, should not throw + for (int step = 0; step < 1000; step++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test that a sleeping flex near a mocap body doesn't crash. +TEST_F(SleepTest, FlexMocapContact) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/sleep/flex_mocap.xml"); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + for (int step = 0; step < 200; step++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/sleep/flex_contact.xml b/test/engine/testdata/sleep/flex_contact.xml new file mode 100644 index 00000000..28dac939 --- /dev/null +++ b/test/engine/testdata/sleep/flex_contact.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/flex_edge.xml b/test/engine/testdata/sleep/flex_edge.xml new file mode 100644 index 00000000..f44a57ac --- /dev/null +++ b/test/engine/testdata/sleep/flex_edge.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/flex_mocap.xml b/test/engine/testdata/sleep/flex_mocap.xml new file mode 100644 index 00000000..591655c1 --- /dev/null +++ b/test/engine/testdata/sleep/flex_mocap.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/flex_nocnstr.xml b/test/engine/testdata/sleep/flex_nocnstr.xml new file mode 100644 index 00000000..e2f7767b --- /dev/null +++ b/test/engine/testdata/sleep/flex_nocnstr.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/flex_state.xml b/test/engine/testdata/sleep/flex_state.xml new file mode 100644 index 00000000..43a19ab8 --- /dev/null +++ b/test/engine/testdata/sleep/flex_state.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/flex_static.xml b/test/engine/testdata/sleep/flex_static.xml new file mode 100644 index 00000000..adb213ac --- /dev/null +++ b/test/engine/testdata/sleep/flex_static.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/hollow_vs_solid.xml b/test/engine/testdata/sleep/hollow_vs_solid.xml new file mode 100644 index 00000000..c4931110 --- /dev/null +++ b/test/engine/testdata/sleep/hollow_vs_solid.xml @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/sleep/scene.xml b/test/engine/testdata/sleep/scene.xml new file mode 100644 index 00000000..2f259f57 --- /dev/null +++ b/test/engine/testdata/sleep/scene.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + +