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

Mocap bodies are the weld root of their own kinematic subtree.

Browse Source 
PiperOrigin-RevId: 960212286
Change-Id: Ibeff129c3110576c8846c76ba6ab756f23c0fa2e
This commit is contained in:
Yuval Tassa
2026-08-06 04:09:41 -07:00
committed by Copybara-Service
parent 0accc5b3c7
commit ed13bf5647
19 changed files with 260 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+4 -1
View File
@@ -2090,7 +2090,10 @@ defined. Its body name is automatically defined as "world".
time step. The size of these arrays is adjusted by the compiler so as to match the number of mocap bodies in the
model. This mechanism can be used to stream motion capture data into the simulation. Mocap bodies can also be moved
via mouse perturbations in the interactive visualizer, even in dynamic simulation mode. This can be useful for
creating props with adjustable position and orientation.
creating props with adjustable position and orientation. Mocap bodies are the weld root of their own kinematic
subtree rather than being welded to the world: their children receive standard parent-child collision filtering,
they do not generate contacts with static geometry, and contact with a mocap body wakes
:ref:`sleeping<Sleeping>` bodies. See :ref:`mocap bodies<CMocap>` for more details.
.. _body-pos:
doc/changelog.rst
+6
View File
@@ -53,6 +53,12 @@ Engine
- Changed the default value of :ref:`bvactive<visual-global-bvactive>` from "true" to "false". This avoids
unnecessarily clearing bounding volume hierarchy visualization flags at every simulation step, which can be a
bottleneck for models with large meshes.
- Mocap bodies and their dof-less descendants are now the root of their own weld group: ``mjModel.body_weldid`` of a
mocap body equals its own id rather than 0. Consequences: dragging a mocap body into sleeping objects now wakes
them; children of mocap bodies receive standard :ref:`parent-child collision filtering<SurprisingCollisions>`;
mocap bodies no longer count as static geometry for ray casting, and contact-matching sensors aggregate their
contacts under the mocap body rather than the world; and geom pairs where neither body can move no longer generate
contacts.
Models
^^^^^^
doc/computation/index.rst
+5 -2
View File
@@ -1666,8 +1666,11 @@ Filtering
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.
single body for the purposes of this test. :ref:`Mocap bodies<CMocap>` and their dof-less descendants form their
own weld group, distinct from the world weld, so the parent-child exclusion applies to children of mocap bodies as
usual. Additionally, geom pairs where neither body can move (both weld groups have no degrees of freedom) are
skipped, so mocap bodies do not generate contacts with static geometry or with each other. The parent-filter test
can be disabled by the user, while the same-body test cannot be disabled.
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:
doc/includes/references.h
+1 -1
View File
@@ -698,7 +698,7 @@ typedef struct mjModel_ {
// bodies
int* body_parentid; // id of body's parent (nbody x 1)
int* body_rootid; // ancestor that is direct child of world (nbody x 1)
int* body_weldid; // top ancestor with no dofs to this body (nbody x 1)
int* body_weldid; // top dof-less ancestor; mocap: own root (nbody x 1)
int* body_mocapid; // id of mocap data; -1: none (nbody x 1)
int* body_jntnum; // number of joints for this body (nbody x 1)
int* body_jntadr; // start addr of joints; -1: no joints (nbody x 1)
doc/modeling.rst
+7
View File
@@ -1544,6 +1544,13 @@ virtual objects cannot push on your physical hand, so your hand (and thereby the
violate the simulated physics. But at the same time we want the resulting simulation to be reasonable. How do we do
this?
Mocap bodies and their dof-less descendants form their own *weld group*, rooted at the mocap body
(``mjModel.body_weldid`` equals the mocap body's own id rather than 0, the world). This has several consequences:
children of mocap bodies receive the standard parent-child collision exclusion; mocap bodies do not generate contacts
with static geometry or with each other; and when :ref:`sleeping<Sleeping>` is enabled, mocap bodies count as awake —
contact with a mocap body, or an active equality constraint connecting to one, wakes sleeping objects, so dragging a
mocap body through a pile of sleeping objects behaves as expected.
The first step is to define a mocap body in the MJCF model, and implement code that reads the data stream at runtime and
sets :ref:`mjData.mocap_pos <siMocap>` and :ref:`mjData.mocap_quat <siMocap>` to the position and orientation received
from the motion capture system. The :ref:`simulate.cc <saSimulate>` code sample uses the mouse as a motion capture
doc/overview.rst
+8 -4
View File
@@ -804,13 +804,17 @@ section, prevents objects from falling through the floor or moving through walls
the following situation:
The user comments out the root joint of a floating-base model, perhaps in order to prevent it from falling; now that the
base body is counted as static, new collisions appear that were not there before and the user is confused. There are two
easy ways to avoid this problem:
base body is counted as static, new collisions appear that were not there before and the user is confused. There are
three easy ways to avoid this problem:
1. Don't remove the root joint. Perhaps it is enough to :ref:`disable gravity<option-flag>` and possibly add some
1. Make the newly static body a :ref:`mocap body<body-mocap>`. Mocap bodies are the root of their own body group for
the purposes of collision filtering, rather than being welded to the world, so the parent-child exclusion applies to
their children as usual. As a bonus, the body can be moved around interactively.
2. Don't remove the root joint. Perhaps it is enough to :ref:`disable gravity<option-flag>` and possibly add some
:ref:`fluid viscosity<option>` in order to prevent your model from moving around too much.
2. Use :ref:`collision filtering<Collision>` to explicitly disable the unwanted collisions, either by setting the
3. Use :ref:`collision filtering<Collision>` to explicitly disable the unwanted collisions, either by setting the
relevant :at:`contype` and :at:`conaffinity` attributes, or by using a contact :ref:`exclude <contact-exclude>`
directive.
doc/programming/simulation.rst
+2
View File
@@ -1300,6 +1300,8 @@ criteria:
- It comes into contact with an awake tree. Waking due to contact leads to collision detection being run *twice*, but
only on the timestep when it occurs. This is required in order to detect contacts inside the island and between the
island and the world, which were skipped in the first run when it was deemed asleep.
- It comes into contact with a :ref:`mocap body<CMocap>`, or is connected to one by an active equality constraint.
Mocap bodies count as awake, since the user can move them at any time.
- It is connected to an awake tree by an active equality constraint or limited tendon.
- It is connected by an equality constraint to a sleeping tree in a different island. For this to occur, the equality
must have been disabled when both trees were put to sleep.
include/mujoco/mjmodel.h
+1 -1
View File
@@ -370,7 +370,7 @@ typedef struct mjModel_ {
// bodies
int* body_parentid; // id of body's parent (nbody x 1)
int* body_rootid; // ancestor that is direct child of world (nbody x 1)
int* body_weldid; // top ancestor with no dofs to this body (nbody x 1)
int* body_weldid; // top dof-less ancestor; mocap: own root (nbody x 1)
int* body_mocapid; // id of mocap data; -1: none (nbody x 1)
int* body_jntnum; // number of joints for this body (nbody x 1)
int* body_jntadr; // start addr of joints; -1: no joints (nbody x 1)
python/mujoco/introspect/structs.py
+1 -1
View File
@@ -1497,7 +1497,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='top ancestor with no dofs to this body',
doc='top dof-less ancestor; mocap: own root',
array_extent=('nbody',),
),
StructFieldDecl(
src/engine/engine_collision_driver.c
+26 -15
View File
@@ -285,20 +285,25 @@ static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum m
// filter body pair; 1: discard, 0: proceed
static int filterBodyPair(int weldbody1, int weldparent1, int asleep1,
int weldbody2, int weldparent2, int asleep2,
static int filterBodyPair(int weldbody1, int weldparent1, int asleep1, int dofnum1,
int weldbody2, int weldparent2, int asleep2, int dofnum2,
int dsbl_filterparent) {
// same weldbody check
if (weldbody1 == weldbody2) {
return 1;
}
// both dof-less: no forces can act, skip
if (dofnum1 == 0 && dofnum2 == 0) {
return 1;
}
// both asleep check
if (asleep1 && asleep2) {
return 1;
}
// asleep and static check
// asleep and world-static check (mocap has weldbody != 0, still triggers wake)
if ((asleep1 && !weldbody2) || (asleep2 && !weldbody1)) {
return 1;
}
@@ -1024,8 +1029,8 @@ static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int nstack = 1;
stack[0].node1 = stack[0].node2 = 0;
// for body:flex, if body has planes, call mj_collidePlaneFlex directly
if (isbody1 && !isbody2 && m->body_weldid[bf1] == 0) {
// for body:flex, if dof-less body has planes, call mj_collidePlaneFlex directly
if (isbody1 && !isbody2 && m->body_dofnum[m->body_weldid[bf1]] == 0) {
for (int i=m->body_geomadr[bf1]; i < m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) {
if (m->geom_type[i] == mjGEOM_PLANE) {
mj_collidePlaneFlex(m, d, i, f2);
@@ -1075,7 +1080,6 @@ static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
margin + gap, NULL, NULL, &initialize)) {
if (filterCollisionPair(m, d, nodeid1, nodeid2, -1, merged, startadr, pairadr)) {
int n1 = nodeid1, n2 = nodeid2;
if (m->geom_type[n1] > m->geom_type[n2]) {
@@ -1579,9 +1583,13 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
continue;
}
// b1 is world body with geoms, or world-welded body with plane
// b1 is world body with geoms, or dof-less body with plane (static or mocap)
if ((b1 == 0 && m->body_geomnum[b1] > 0) ||
(m->body_weldid[b1] == 0 && hasPlane(m, b1))) {
(m->body_dofnum[m->body_weldid[b1]] == 0 && hasPlane(m, b1))) {
int weld1 = m->body_weldid[b1];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int dofnum1 = m->body_dofnum[weld1];
// add b1:b2 pairs that are not welded together
for (int b2=0; b2 < nbody; b2++) {
// cannot collide
@@ -1589,11 +1597,13 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
continue;
}
// welded together
int weld2 = m->body_weldid[b2];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
int asleep2 = sleep_filter ? d->body_awake[b2] == mjS_ASLEEP : 0;
if (filterBodyPair(0, 0, 1, weld2, parent_weld2, asleep2, dsbl_filterparent)) {
int dofnum2 = m->body_dofnum[weld2];
if (filterBodyPair(weld1, parent_weld1, 0, dofnum1,
weld2, parent_weld2, asleep2, dofnum2,
dsbl_filterparent)) {
continue;
}
@@ -1684,15 +1694,16 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
// body pair: prune based on sleep filter and weld filter
if (bf1 < nbody && bf2 < nbody) {
int asleep1 = sleep_filter ? d->body_awake[bf1] == mjS_ASLEEP : 0;
int asleep2 = sleep_filter ? d->body_awake[bf2] == mjS_ASLEEP : 0;
int weld1 = m->body_weldid[bf1];
int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(weld1, parent_weld1, asleep1,
weld2, parent_weld2, asleep2,
int asleep1 = sleep_filter ? d->body_awake[bf1] == mjS_ASLEEP : 0;
int asleep2 = sleep_filter ? d->body_awake[bf2] == mjS_ASLEEP : 0;
int dofnum1 = m->body_dofnum[weld1];
int dofnum2 = m->body_dofnum[weld2];
if (filterBodyPair(weld1, parent_weld1, asleep1, dofnum1,
weld2, parent_weld2, asleep2, dofnum2,
dsbl_filterparent)) {
continue;
}
src/engine/engine_core_util.c
+16 -16
View File
@@ -59,8 +59,8 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2, int flg_skipcomm
b1 = m->body_weldid[b1];
b2 = m->body_weldid[b2];
// neither body is movable: empty chain
if (b1 == 0 && b2 == 0) {
// neither weld root has dofs: empty chain
if (m->body_dofnum[b1] == 0 && m->body_dofnum[b2] == 0) {
return 0;
}
@@ -143,8 +143,8 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) {
// skip fixed bodies
body = m->body_weldid[body];
// not movable: empty chain
if (body == 0) {
// weld root has no dofs: empty chain
if (m->body_dofnum[body] == 0) {
return 0;
}
@@ -190,8 +190,8 @@ void mj_jac(const mjModel* m, const mjData* d,
// skip fixed bodies
body = m->body_weldid[body];
// no movable body found: nothing to do
if (!body) {
// weld root has no dofs: nothing to do
if (m->body_dofnum[body] == 0) {
return;
}
@@ -319,8 +319,8 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
// skip fixed bodies
body = m->body_weldid[body];
// no movable body found: nothing to do
if (!body) {
// weld root has no dofs: nothing to do
if (m->body_dofnum[body] == 0) {
return;
}
@@ -622,8 +622,8 @@ void mj_jacDot(const mjModel* m, const mjData* d,
// skip fixed bodies
body = m->body_weldid[body];
// no movable body found: nothing to do
if (!body) {
// weld root has no dofs: nothing to do
if (m->body_dofnum[body] == 0) {
return;
}
@@ -695,8 +695,8 @@ void mj_jacDotSparse(const mjModel* m, const mjData* d,
// skip fixed bodies
body = m->body_weldid[body];
// no movable body found: nothing to do
if (!body) {
// weld root has no dofs: nothing to do
if (m->body_dofnum[body] == 0) {
return;
}
@@ -877,8 +877,8 @@ void mj_objectVelocity(const mjModel* m, const mjData* d,
mjERROR("invalid object type %d", objtype);
}
// static body: quick return
if (m->body_weldid[bodyid] == 0) {
// dof-less body (static or mocap): quick return
if (m->body_dofnum[m->body_weldid[bodyid]] == 0) {
mju_zero(res, 6);
return;
}
@@ -951,8 +951,8 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
mjERROR("invalid object type %d", objtype);
}
// static body: quick return
if (m->body_weldid[bodyid] == 0) {
// dof-less body (static or mocap): quick return
if (m->body_dofnum[m->body_weldid[bodyid]] == 0) {
mju_zero(res, 6);
return;
}
src/engine/engine_setconst.c
+2 -2
View File
@@ -905,8 +905,8 @@ static void set0(mjModel* m, mjData* d) {
// compute body_invweight0
m->body_invweight0[0] = m->body_invweight0[1] = 0.0;
for (int i=1; i < m->nbody; i++) {
// static bodies: zero invweight0
if (m->body_weldid[i] == 0) {
// bodies with no dofs (static and mocap): zero invweight0
if (m->body_dofnum[m->body_weldid[i]] == 0) {
m->body_invweight0[2*i] = m->body_invweight0[2*i+1] = 0;
}
src/engine/engine_sleep.c
+22 -10
View File
@@ -333,8 +333,13 @@ int mj_wakeCollision(const mjModel* m, mjData* d) {
int tree1 = m->body_treeid[b1];
int tree2 = m->body_treeid[b2];
// contact with static body, nothing to do
// contact with a dof-less body: wake if it is marked awake (mocap), otherwise nothing to do
if (tree1 < 0 || tree2 < 0) {
int tree = tree1 < 0 ? tree2 : tree1;
int b = tree1 < 0 ? b1 : b2;
if (tree >= 0 && !d->tree_awake[tree] && d->body_awake[b] == mjS_AWAKE) {
nwoke += mj_wakeIsland(d->tree_asleep, ntree, tree, kAwake, "mocap contact with", d->time);
}
continue;
}
@@ -408,22 +413,27 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
int id1 = m->eq_obj1id[i];
int id2 = m->eq_obj2id[i];
int tree1, tree2;
int body1 = -1, body2 = -1;
switch (eqtype) {
case mjEQ_CONNECT:
case mjEQ_WELD:
if (m->eq_objtype[i] == mjOBJ_BODY) {
tree1 = m->body_treeid[id1];
tree2 = m->body_treeid[id2];
body1 = id1;
body2 = id2;
} else {
tree1 = m->body_treeid[m->site_bodyid[id1]];
tree2 = m->body_treeid[m->site_bodyid[id2]];
body1 = m->site_bodyid[id1];
body2 = m->site_bodyid[id2];
}
tree1 = m->body_treeid[body1];
tree2 = m->body_treeid[body2];
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;
body1 = id1 >= 0 ? m->jnt_bodyid[id1] : -1;
body2 = id2 >= 0 ? m->jnt_bodyid[id2] : -1;
tree1 = body1 >= 0 ? m->body_treeid[body1] : -1;
tree2 = body2 >= 0 ? m->body_treeid[body2] : -1;
break;
case mjEQ_TENDON:
@@ -475,9 +485,11 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
continue;
}
// get sleep state
mjtSleepState s1 = tree1 >= 0 ? d->tree_awake[tree1] : mjS_STATIC;
mjtSleepState s2 = tree2 >= 0 ? d->tree_awake[tree2] : mjS_STATIC;
// get sleep state; dof-less bodies marked awake (mocap) count as awake
mjtSleepState s1 = tree1 >= 0 ? (mjtSleepState)d->tree_awake[tree1]
: (body1 >= 0 ? (mjtSleepState)d->body_awake[body1] : mjS_STATIC);
mjtSleepState s2 = tree2 >= 0 ? (mjtSleepState)d->tree_awake[tree2]
: (body2 >= 0 ? (mjtSleepState)d->body_awake[body2] : mjS_STATIC);
// neither is asleep, nothing to do
if (s1 != mjS_ASLEEP && s2 != mjS_ASLEEP) {
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -153,9 +153,9 @@ static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) {
}
// a body is static if it is welded to the world and is not a mocap body or descendant thereof
// a body is static if it is welded to the world (mocap subtrees are their own weld)
static int bodycategory(const mjModel* m, int bodyid) {
if (m->body_weldid[bodyid] == 0 && m->body_mocapid[m->body_rootid[bodyid]] == -1) {
if (m->body_weldid[bodyid] == 0) {
return mjCAT_STATIC;
} else {
return mjCAT_DYNAMIC;
src/render/filament/support/model_renderables.cc
+2 -2
View File
@@ -64,8 +64,8 @@ static float GetPlaneTileSize(const mjModel* model, int matid,
}
static mjtCatBit GetBodyCategory(const mjModel* m, int bodyid) {
if (m->body_weldid[bodyid] == 0 &&
m->body_mocapid[m->body_rootid[bodyid]] == -1) {
// mocap subtrees are their own weld, hence not static
if (m->body_weldid[bodyid] == 0) {
return mjCAT_STATIC;
} else {
return mjCAT_DYNAMIC;
src/user/user_model.cc
+2 -2
View File
@@ -4508,8 +4508,8 @@ void mjCModel::FuseReindex(mjCBody* body) {
// set parentid and weldid of children
for (int i=0; i < body->bodies.size(); i++) {
body->bodies[i]->parent = body;
body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ?
body->bodies[i]->id : body->weldid);
bool weld_root = !body->bodies[i]->joints.empty() || body->bodies[i]->spec.mocap;
body->bodies[i]->weldid = (weld_root ? body->bodies[i]->id : body->weldid);
}
makelistid(joints_, body->joints);
src/user/user_objects.cc
+10 -3
View File
@@ -2689,7 +2689,8 @@ void mjCBody::Compile(void) {
// set parentid and weldid of children
for (int i=0; i < bodies.size(); i++) {
bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid);
bool weld_root = !bodies[i]->joints.empty() || bodies[i]->spec.mocap;
bodies[i]->weldid = (weld_root ? bodies[i]->id : weldid);
}
// check and process orientation alternatives for body
@@ -3962,9 +3963,15 @@ void mjCGeom::Compile(void) {
throw mjCError(this, "hfield geom '%s' (id = %d) must have valid hfieldid", name.c_str(), id);
}
// plane only allowed in static bodies
// plane only allowed in bodies with no dofs (static, including mocap)
if (type == mjGEOM_PLANE && body->weldid != 0) {
throw mjCError(this, "plane only allowed in static bodies");
const mjCBody* weld = body;
while (weld->id != weld->weldid) {
weld = weld->parent;
}
if (!weld->spec.mocap) {
throw mjCError(this, "plane only allowed in static bodies");
}
}
// check if can collide
test/engine/engine_sleep_test.cc
+141
View File
@@ -790,5 +790,146 @@ TEST_F(SleepTest, FlexMocapContact) {
mj_deleteModel(m);
}
// mocap bodies are their own weld root: dragging into a sleeping tree wakes it
TEST_F(SleepTest, MocapWakes) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 .1"/>
<body name="hand" mocap="true" pos="2 0 .1">
<geom type="sphere" size=".1"/>
<body name="finger">
<geom type="sphere" size=".05" pos=".1 0 0"/>
</body>
</body>
<body name="box" pos="0 0 .1">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
// mocap body is its own weld root, static child inherits it,
// box is its own (jointed)
int hand = mj_name2id(m.get(), mjOBJ_BODY, "hand");
int finger = mj_name2id(m.get(), mjOBJ_BODY, "finger");
EXPECT_EQ(m->body_weldid[hand], hand);
EXPECT_EQ(m->body_weldid[finger], hand);
// let the box fall asleep
MjDataPtr d = MakeData(m);
int steps = 0;
while (d->ntree_awake > 0 && steps++ < 10000) {
mj_step(m.get(), d.get());
}
ASSERT_EQ(d->ntree_awake, 0);
// drag the mocap hand into the sleeping box: box wakes
d->mocap_pos[0] = 0;
mj_step(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1);
}
// mocap bodies do not generate contacts with static bodies or each other
TEST_F(SleepTest, MocapNoStaticContacts) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="plane" size="5 5 .1"/>
<body mocap="true" pos="0 0 .05">
<geom type="sphere" size=".1"/>
</body>
<body mocap="true" pos=".05 0 .05">
<geom type="sphere" size=".1"/>
</body>
<geom type="box" size=".1 .1 .1" pos="-.05 0 .05"/>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// everything interpenetrates, nothing can move: no contacts
mj_forward(m.get(), d.get());
EXPECT_EQ(d->ncon, 0);
}
// a plane on a mocap body collides like a plane on a static body
TEST_F(SleepTest, MocapPlane) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body mocap="true">
<geom type="plane" size="5 5 .1"/>
</body>
<body pos="0 0 .5">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// box falls onto the mocap plane and rests on it
while (d->time < 2) {
mj_step(m.get(), d.get());
}
EXPECT_GT(d->ncon, 0);
EXPECT_NEAR(d->qpos[2], 0.1, 1e-3);
}
// a tree welded by equality to a mocap body is woken when asleep
TEST_F(SleepTest, MocapWeldEqualityWakes) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag sleep="enable"/>
</option>
<worldbody>
<geom type="plane" size="5 5 .1"/>
<body name="target" mocap="true" pos="2 0 .5">
<geom type="sphere" size=".05" contype="0" conaffinity="0"/>
</body>
<body name="box" pos="0 0 .1">
<freejoint/>
<geom type="box" size=".1 .1 .1"/>
</body>
</worldbody>
<equality>
<weld name="grab" body1="target" body2="box" active="false"/>
</equality>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// equality inactive: box falls asleep on the floor
int steps = 0;
while (d->ntree_awake > 0 && steps++ < 10000) {
mj_step(m.get(), d.get());
}
ASSERT_EQ(d->ntree_awake, 0);
// activate the weld to the mocap body: box wakes
d->eq_active[mj_name2id(m.get(), mjOBJ_EQUALITY, "grab")] = 1;
mj_step(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1);
}
} // namespace
} // namespace mujoco
test/user/user_model_test.cc
+2 -1
View File
@@ -100,8 +100,9 @@ TEST_F(UserModelTest, WeldRootID) {
EXPECT_THAT(AsVector(model->body_rootid, model->nbody),
ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9));
// mocap bodies (7, 9) are their own weld roots, inherited by static children (8)
EXPECT_THAT(AsVector(model->body_weldid, model->nbody),
ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10));
ElementsAre(0, 1, 2, 3, 3, 0, 0, 7, 7, 9, 10));
}
TEST_F(UserModelTest, RepeatedNames) {