chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Allow flex sleeping

Browse Source 
PiperOrigin-RevId: 917817500
Change-Id: Ia3bd5e52e7c2eaa3f70c81352d82c130b1d357f6
This commit is contained in:
Yuval Tassa
2026-05-19 07:14:45 -07:00
committed by Copybara-Service
parent ddc4c54d16
commit f712eed4ce
21 changed files with 660 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+1 -1
View File
@@ -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<ElemFlex>` are not allowed to sleep (not overridable).
- Constraint-free :ref:`flexes<ElemFlex>` 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.
doc/changelog.rst
+1
View File
@@ -16,6 +16,7 @@ General
- The pseudo-random constraint visitation order in the :ref:`PGS solver<soAlgorithms>`, 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
doc/programming/simulation.rst
-12
View File
@@ -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 <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:
model/flex/hollow_vs_solid.xml
+26 -26
View File
@@ -31,6 +31,32 @@
</default>
<worldbody>
<body name="gripper_solid" pos="0.2 0.45 0.2">
<joint name="lift_solid" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_r" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_r" axis="0 1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger_r" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_r" axis="0 -1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<body name="gripper_hollow" pos="0.2 0 0.2">
<joint name="lift_hollow" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_s" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_s" axis="0 1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
<body name="right_finger_s" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_s" axis="0 -1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
</body>
<body name="solid" pos=".32 0 .2">
<flexcomp
origin="0 0 0" count="8 2 12" cellcount="6 1 6"
@@ -59,32 +85,6 @@
solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
<body name="gripper_solid" pos="0.2 0.45 0.2">
<joint name="lift_solid" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_r" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_r" axis="0 1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger_r" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_r" axis="0 -1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<body name="gripper_hollow" pos="0.2 0 0.2">
<joint name="lift_hollow" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_s" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_s" axis="0 1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
<body name="right_finger_s" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_s" axis="0 -1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
</body>
</worldbody>
<equality>
simulate/simulate.cc
+1 -1
View File
@@ -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");
src/engine/engine_collision_driver.c
+16 -1
View File
@@ -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);
}
src/engine/engine_core_constraint.c
+10 -1
View File
@@ -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
src/engine/engine_island.c
+4 -3
View File
@@ -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];
src/engine/engine_setconst.c
+6 -1
View File
@@ -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];
src/engine/engine_sleep.c
+94 -11
View File
@@ -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:
src/engine/engine_sleep.h
+3
View File
@@ -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);
src/engine/engine_vis_visualize.c
+39
View File
@@ -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;
test/engine/engine_sleep_test.cc
+225
View File
@@ -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
test/engine/testdata/sleep/flex_contact.xml
Vendored
+23
View File
@@ -0,0 +1,23 @@
<mujoco>
<option integrator="implicitfast" viscosity="2" sleep_tolerance="0.1">
<flag sleep="enable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 .01"/>
<!-- constrained flex on the floor, will go to sleep quickly -->
<body name="flex_body" pos="0 0 0.01">
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="true" solref="0.15 0.2"/>
</flexcomp>
</body>
<!-- ball that will collide with the flex -->
<body name="ball" pos="0 0.01 0.5">
<freejoint/>
<geom type="sphere" size=".05"/>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/flex_edge.xml
Vendored
+21
View File
@@ -0,0 +1,21 @@
<mujoco>
<custom>
<numeric name="duration" data="2"/>
</custom>
<option integrator="implicitfast" viscosity="10"
sleep_tolerance="0.01">
<flag sleep="enable" gravity="disable"/>
</option>
<worldbody>
<body name="flex_body">
<freejoint/>
<inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/flex_mocap.xml
Vendored
+22
View File
@@ -0,0 +1,22 @@
<mujoco>
<option integrator="implicitfast" viscosity="1" sleep_tolerance="1e-2">
<flag sleep="enable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 .01"/>
<!-- mocap body with a sphere -->
<body name="mocap_obstacle" pos="0 0 -.02" mocap="true">
<geom type="sphere" size=".04"/>
</body>
<!-- constrained flex on the floor, will go to sleep -->
<body name="flex_body" pos="0 0 0.01">
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="true" solref="0.15 0.2"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/flex_nocnstr.xml
Vendored
+15
View File
@@ -0,0 +1,15 @@
<mujoco>
<option integrator="implicitfast" viscosity="10"
sleep_tolerance="0.01">
<flag sleep="enable" gravity="disable"/>
</option>
<worldbody>
<body name="flex_body">
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="false" stiffness="100" damping="10"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/flex_state.xml
Vendored
+16
View File
@@ -0,0 +1,16 @@
<mujoco>
<option>
<flag sleep="enable" constraint="disable" contact="disable" gravity="disable"/>
</option>
<worldbody>
<body name="flex_body">
<freejoint/>
<inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="true" solref="0.15 0.2"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/flex_static.xml
Vendored
+22
View File
@@ -0,0 +1,22 @@
<mujoco>
<option integrator="implicitfast" viscosity="2" sleep_tolerance="0.1">
<flag sleep="enable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 .01"/>
<!-- static body (no joint) with a sphere touching the flex -->
<body name="static_obstacle" pos="0 0 0.05">
<geom type="sphere" size=".04"/>
</body>
<!-- constrained flex on the floor, will go to sleep -->
<body name="flex_body" pos="0 0 0.01">
<flexcomp name="f1" type="grid" dim="1" count="3 1 1"
spacing=".05 .05 .05" radius=".01" mass="1">
<edge equality="true" solref="0.15 0.2"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
test/engine/testdata/sleep/hollow_vs_solid.xml
Vendored
+89
View File
@@ -0,0 +1,89 @@
<mujoco>
<include file="scene.xml"/>
<option cone="elliptic" solver="CG" sleep_tolerance=".01">
<flag sleep="enable"/>
</option>
<default>
<default class="gripper">
<geom type="box" size="0.02 0.15 0.02" rgba="0.5 0.5 0.5 1"
solref="0.001 1" friction="2 0.1 0.1"/>
<joint type="slide" axis="0 0 1" range="-1 1"/>
<default class="finger">
<geom size="0.01 0.06 0.03"/>
<joint type="slide" range="0 0.1"/>
</default>
</default>
</default>
<worldbody>
<body name="gripper_solid" pos="0.2 0.45 0.2">
<joint name="lift_solid" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_r" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_r" axis="0 1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger_r" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_r" axis="0 -1 0"/>
<geom rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<body name="gripper_hollow" pos="0.2 0 0.2">
<joint name="lift_hollow" class="gripper"/>
<geom class="gripper"/>
<body name="left_finger_s" pos="0.05 -0.1 0" childclass="finger">
<joint name="left_slide_s" axis="0 1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
<body name="right_finger_s" pos="0.05 0.1 0" childclass="finger">
<joint name="right_slide_s" axis="0 -1 0"/>
<geom rgba="0.2 0.8 0.2 1"/>
</body>
</body>
<body name="solid" pos=".32 0 .2">
<flexcomp
origin="0 0 0" count="8 2 12" cellcount="2 1 2"
type="grid" name="soft_mesh" dim="3" spacing=".025 .05 .025"
mass="0.43" radius="0.005" dof="trilinear" rgba="0.7 0.9 0.7 1">
<edge equality="strain" solref="0.001 1"/>
<contact selfcollide="none"
internal="false"
friction="2.0 0.5 0.01"
solref="0.001 1"
solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
<body name="hollow" pos=".32 .45 .2">
<flexcomp
origin="0 0 0" count="8 2 12" cellcount="2 1 2"
type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025"
mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1">
<elasticity young="1e5" poisson="0.3" damping="0" elastic2d="bend" thickness="0.03"/>
<edge equality="strain" solref="0.001 1" solimp="0.99 0.999 0.001"/>
<contact selfcollide="none"
internal="false"
friction="2.0 0.5 0.01"
solref="0.001 1"
solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
</worldbody>
<equality>
<joint joint1="right_slide_r" joint2="left_slide_r"/>
<joint joint1="right_slide_s" joint2="left_slide_s"/>
</equality>
<actuator>
<position name="lift_r" joint="lift_solid" kp="400" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp_r" joint="left_slide_r" kp="10" dampratio="1" ctrlrange="0 5"/>
<position name="lift_s" joint="lift_hollow" kp="400" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp_s" joint="left_slide_s" kp="10" dampratio="1" ctrlrange="0 5"/>
</actuator>
</mujoco>
test/engine/testdata/sleep/scene.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<compiler meshdir="asset" texturedir="asset"/>
<statistic meansize=".05"/>
<visual>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10" texuniform="true"/>
</asset>
<worldbody>
<light diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 2.0" dir="0 0 -1" castshadow="false"/>
<light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
</worldbody>
</mujoco>