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

Fix multiple bugs related to connect and weld constraints with site semantics. Fixes #2179

Browse Source 
The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints.

PiperOrigin-RevId: 690670420
Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6
This commit is contained in:
Yuval Tassa
2024-10-28 10:43:09 -07:00
committed by Copybara-Service
parent 2c86136dd6
commit 864b805a6e
10 changed files with 234 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+28 -2
View File
@@ -1117,16 +1117,30 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// process according to equality-constraint type
switch (m->eq_type[id]) {
case mjEQ_CONNECT:
// body translation
b1 = m->eq_obj1id[id];
b2 = m->eq_obj2id[id];
// get body ids if using site semantics
if (m->eq_objtype[id] == mjOBJ_SITE) {
b1 = m->site_bodyid[b1];
b2 = m->site_bodyid[b2];
}
// body translation
dA[i] = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
break;
case mjEQ_WELD: // distinguish translation and rotation inertia
// body translation or rotation depending on weldcnt
b1 = m->eq_obj1id[id];
b2 = m->eq_obj2id[id];
// get body ids if using site semantics
if (m->eq_objtype[id] == mjOBJ_SITE) {
b1 = m->site_bodyid[b1];
b2 = m->site_bodyid[b2];
}
// body translation or rotation depending on weldcnt
dA[i] = m->body_invweight0[2*b1 + (weldcnt > 2)] +
m->body_invweight0[2*b2 + (weldcnt > 2)];
weldcnt = (weldcnt + 1) % 6;
@@ -1650,6 +1664,12 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
break;
}
// get body ids if using site semantics
if (m->eq_objtype[i] == mjOBJ_SITE) {
id[0] = m->site_bodyid[id[0]];
id[1] = m->site_bodyid[id[1]];
}
NV = mj_jacDifPairCount(m, chain, id[1], id[0], issparse);
break;
@@ -1659,6 +1679,12 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
break;
}
// get body ids if using site semantics
if (m->eq_objtype[i] == mjOBJ_SITE) {
id[0] = m->site_bodyid[id[0]];
id[1] = m->site_bodyid[id[1]];
}
NV = mj_jacDifPairCount(m, chain, id[1], id[0], issparse);
break;