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

Fix setting wrong value in efc_diagApprox for rotational components of weld constraints.

Browse Source 
PiperOrigin-RevId: 572276016
Change-Id: Ic985a342777aca976f5c4bc781133fb4a8976e3f
This commit is contained in:
Google DeepMind
2023-10-10 09:33:42 -07:00
committed by Copybara-Service
parent 8af09eb70d
commit 1bd6c94e6d
2 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+2 -7
View File
@@ -1035,11 +1035,6 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// get constraint id
id = d->efc_id[i];
// clear weld counter
if (d->efc_type[i] != mjEQ_WELD) {
weldcnt = 0;
}
// process according to constraint type
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
@@ -1052,13 +1047,13 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
dA[i] = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
break;
case mjEQ_WELD: // distingush translation and rotation inertia
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];
dA[i] = m->body_invweight0[2*b1 + (weldcnt > 2)] +
m->body_invweight0[2*b2 + (weldcnt > 2)];
weldcnt++;
weldcnt = (weldcnt + 1) % 6;
break;
case mjEQ_JOINT: