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
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