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
test/engine/testdata/equality_site_body_compare.xml
Vendored
+58
View File
@@ -0,0 +1,58 @@
<mujoco model="Equivalent Connects and Welds defined with sites and bodies">
<option viscosity="1">
<flag contact="disable"/>
</option>
<visual>
<scale framewidth="0.05" framelength="0.4"/>
</visual>
<default>
<default class="box">
<geom type="box" size=".2" fromto=".2 .2 0 .2 .2 -1" rgba=".4 .7 .6 1"/>
</default>
<site rgba=".0 .7 0 1" size="0.05"/>
</default>
<worldbody>
<geom pos="0 0 -2" type="plane" size="10 10 .01"/>
<light pos="0 0 20"/>
<site name="a1" pos="-2 0 0" euler="-90 0 0"/>
<body name="a" pos="-2 1 0">
<freejoint/>
<geom class="box"/>
<site name="a2" pos="0 -1 0"/>
<body pos="0 0 -1">
<geom class="box" size=".15"/>
<site name="a_sensor"/>
</body>
</body>
<site name="b1" euler="-90 0 0"/>
<body name="b" pos="0 1 0">
<freejoint/>
<geom class="box"/>
<site name="b2" pos="0 -1 0"/>
<body pos="0 0 -1">
<geom class="box" size=".15"/>
<site name="b_sensor"/>
</body>
</body>
</worldbody>
<equality>
<!-- equivalent connect and weld constraints defined with sites (enabled) and bodies (disabled) -->
<connect name="connect site" site1="a1" site2="a2" active="true"/>
<connect name="connect body" body1="a" body2="world" active="false" anchor="0 -1 0"/>
<weld name="weld site" site1="b1" site2="b2" active="true"/>
<weld name="weld body" body1="world" body2="b" active="false" anchor="0 -1 0" relpose="0 0 0 1 -1 0 0"/>
</equality>
<sensor>
<force name="a_force" site="a_sensor"/>
<torque name="a_torque" site="a_sensor"/>
<force name="b_force" site="b_sensor"/>
<torque name="b_torque" site="b_sensor"/>
</sensor>
</mujoco>
test/engine/testdata/island/island_efc.xml
Vendored
+4 -1
View File
@@ -1,6 +1,6 @@
<!--
test model for constraint islanding:
- 4 equalities (1:joint, 3:connect)
- 7 equalities (1:joint, 6:connect)
- 1 limit
- 2 friction constraints
- 1 unconstrained dof
@@ -65,14 +65,17 @@
<geom type="box" size=".03 .03 .03" pos="0.01 0.01 0.01"/>
</body>
<site name="0" pos="-.45 -.05 .35"/>
<body pos="-.5 0 .3" name="connect">
<freejoint/>
<geom type="box" size=".05 .05 .05"/>
<site name="1" pos=".05 -.05 .05"/>
</body>
</worldbody>
<equality>
<joint joint1="hinge1" joint2="hinge2"/>
<connect body1="connect" body2="world" anchor="-.05 -.05 .05"/>
<connect site1="0" site2="1"/>
</equality>
</mujoco>