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

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
+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>