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

Restrict midpoint integration to unconstrained free bodies in implicitfast

Browse Source 
PiperOrigin-RevId: 908750768
Change-Id: I9a45a160ac757cc82bfe54871609956769988369
This commit is contained in:
Yuval Tassa
2026-05-01 08:40:53 -07:00
committed by Copybara-Service
parent 8287d9d152
commit 910b3336ed
5 changed files with 221 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+71 -11
View File
@@ -1597,16 +1597,75 @@ static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContex
// return 1 if free joint is eligible for midpoint quaternion integration:
// standalone 6-DOF tree with no children
static int midpoint_eligible(const mjModel* m, int jnt) {
if (m->jnt_type[jnt] == mjJNT_FREE) {
int body = m->jnt_bodyid[jnt];
int treeid = m->dof_treeid[m->jnt_dofadr[jnt]];
return m->tree_dofnum[treeid] == 6 &&
m->body_subtreemass[body] == m->body_mass[body];
// standalone 6-DOF tree with no children, awake, and unconstrained
static int midpoint_eligible(const mjModel* m, const mjData* d, int jnt) {
if (m->jnt_type[jnt] != mjJNT_FREE) {
return 0;
}
return 0;
int body = m->jnt_bodyid[jnt];
int adr = m->jnt_dofadr[jnt];
int tree = m->dof_treeid[adr];
// must be standalone 6-DOF tree with no children
if (m->tree_dofnum[tree] != 6 ||
m->body_subtreemass[body] != m->body_mass[body]) {
return 0;
}
// must be awake
if (!d->tree_awake[tree]) {
return 0;
}
// must be unconstrained
if (d->nefc) {
// islands enabled: O(1) lookup
if (!mjDISABLED(mjDSBL_ISLAND)) {
if (d->dof_island[adr] >= 0) {
return 0;
}
}
// islands disabled: check if any constraint involves this tree
else {
for (int c=0; c < d->nefc; c++) {
int type = d->efc_type[c];
int id = d->efc_id[c];
// contact: check if either geom belongs to this body
if (type == mjCNSTR_CONTACT_FRICTIONLESS ||
type == mjCNSTR_CONTACT_PYRAMIDAL ||
type == mjCNSTR_CONTACT_ELLIPTIC) {
int g1 = d->contact[id].geom[0];
int g2 = d->contact[id].geom[1];
if (g1 >= 0 && m->geom_bodyid[g1] == body) return 0;
if (g2 >= 0 && m->geom_bodyid[g2] == body) return 0;
}
// connect or weld: check if either body is this body
else if (type == mjCNSTR_EQUALITY &&
(m->eq_type[id] == mjEQ_CONNECT || m->eq_type[id] == mjEQ_WELD)) {
int b1 = m->eq_obj1id[id];
int b2 = m->eq_obj2id[id];
if (m->eq_objtype[id] == mjOBJ_SITE) {
b1 = m->site_bodyid[b1];
b2 = m->site_bodyid[b2];
}
if (b1 == body || b2 == body) return 0;
}
// tendon limit or friction: check first two trees
else if (type == mjCNSTR_LIMIT_TENDON || type == mjCNSTR_FRICTION_TENDON) {
if (m->tendon_treeid[2*id] == tree ||
m->tendon_treeid[2*id+1] == tree) return 0;
}
}
}
}
// otherwise eligible
return 1;
}
@@ -1980,11 +2039,12 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
// count and list joints of free bodies eligible for midpoint integration
int nfree = 0;
int* free_jntid = NULL;
if (!mjENABLED(mjENBL_INVDISCRETE)) {
if (!mjENABLED(mjENBL_INVDISCRETE) &&
m->opt.integrator == mjINT_IMPLICITFAST &&
m->opt.density == 0 && m->opt.viscosity == 0) {
free_jntid = mjSTACKALLOC(d, m->njnt, int);
for (int j=0; j < m->njnt; j++) {
// add to list if eligible and awake
if (midpoint_eligible(m, j) && d->tree_awake[m->dof_treeid[m->jnt_dofadr[j]]]) {
if (midpoint_eligible(m, d, j)) {
free_jntid[nfree++] = j;
}
}