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

Use body_weldid to find the first movable body in a chain.

Browse Source 
PiperOrigin-RevId: 863774053
Change-Id: I8df96d5af7cc80d78a6f35560c3c31bed8be550e
This commit is contained in:
Yuval Tassa
2026-01-31 17:15:48 -08:00
committed by Copybara-Service
parent 7418d86eda
commit 815dbb996e
1 changed files with 8 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_util.c
+8 -19
View File
@@ -56,12 +56,8 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
int da1, da2, NV = 0; int da1, da2, NV = 0;
// skip fixed bodies // skip fixed bodies
while (b1 && !m->body_dofnum[b1]) { b1 = m->body_weldid[b1];
b1 = m->body_parentid[b1]; b2 = m->body_weldid[b2];
}
while (b2 && !m->body_dofnum[b2]) {
b2 = m->body_parentid[b2];
}
// neither body is movable: empty chain // neither body is movable: empty chain
if (b1 == 0 && b2 == 0) { if (b1 == 0 && b2 == 0) {
@@ -105,7 +101,8 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
} }
// init // init
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2]; int n1 = m->body_dofnum[b1];
int n2 = m->body_dofnum[b2];
// both fixed: nothing to do // both fixed: nothing to do
if (n1 == 0 && n2 == 0) { if (n1 == 0 && n2 == 0) {
@@ -140,9 +137,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) {
// general case // general case
else { else {
// skip fixed bodies // skip fixed bodies
while (body && !m->body_dofnum[body]) { body = m->body_weldid[body];
body = m->body_parentid[body];
}
// not movable: empty chain // not movable: empty chain
if (body == 0) { if (body == 0) {
@@ -189,9 +184,7 @@ void mj_jac(const mjModel* m, const mjData* d,
} }
// skip fixed bodies // skip fixed bodies
while (body && !m->body_dofnum[body]) { body = m->body_weldid[body];
body = m->body_parentid[body];
}
// no movable body found: nothing to do // no movable body found: nothing to do
if (!body) { if (!body) {
@@ -320,9 +313,7 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]); mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
// skip fixed bodies // skip fixed bodies
while (body && !m->body_dofnum[body]) { body = m->body_weldid[body];
body = m->body_parentid[body];
}
// no movable body found: nothing to do // no movable body found: nothing to do
if (!body) { if (!body) {
@@ -601,9 +592,7 @@ void mj_jacDot(const mjModel* m, const mjData* d,
} }
// skip fixed bodies // skip fixed bodies
while (body && !m->body_dofnum[body]) { body = m->body_weldid[body];
body = m->body_parentid[body];
}
// no movable body found: nothing to do // no movable body found: nothing to do
if (!body) { if (!body) {