Fix self-collision between the stowed arm and base_link in Stretch 3

[ericzhubonnie-glitch]

What this fixes

Fixes #341.

At qpos = 0 — which is where both keyframes start, since home and stow set ctrl but leave qpos at zero — link_DW3_wrist_pitch is 48.28 mm inside the collision hull of base_link. Three pairs interpenetrate at t=0 and none of them is in <contact><exclude>:

deepest	body pair	contact points
48.28 mm	base_link ↔ link_DW3_wrist_pitch	4
14.02 mm	link_right_wheel ↔ link_DW3_wrist_pitch	1
2.24 mm	link_aruco_right_base ↔ link_DW3_wrist_pitch	1

base_link uses base_link_collision (40,336 vertices) as its collision geom, and mesh collision runs on the compile-time convex hull, which fills the notch the arm retracts into. The remaining stowed-arm pairs are already excluded (link_arm_* against link_lift, link_wrist_yaw against every arm segment, both fingers against the wrist), so this looks like an omission rather than intent. hello_robot_stretch (v1) has no self-collision at qpos = 0 at all.

The change

Three lines in hello_robot_stretch_3/stretch.xml:

<exclude body1="base_link" body2="link_DW3_wrist_pitch"/>
<exclude body1="link_right_wheel" body2="link_DW3_wrist_pitch"/>
<exclude body1="link_aruco_right_base" body2="link_DW3_wrist_pitch"/>

Measured effect

MuJoCo 3.16.0, hello_robot_stretch_3/scene.xml, mj_resetDataKeyframe(m, d, 0):

quantity	before	after
contacts at t=0	14	1 (a caster on the floor)
deepest contact at t=0	48.28 mm	0 mm
peak |normal force| at t=0	68.5 kN	0 N (resting weight only)
joint_wrist_pitch speed on the first step	37.10 rad/s	0.11 rad/s
joint_wrist_pitch excursion over the first 0.5 s	0.618 rad (limit 0.56)	0.051 rad

Five seconds in, the settled link_DW3_wrist_pitch pose differs between the two runs by 11.3 mm in y — same command, same seed, so the impulse leaves a residue in where the arm ends up.

The model is 23.7 kg, i.e. ~232 N of weight, so 68.5 kN is an internal startup impulse rather than anything physical. It does not persist (the model pushes itself out within 5 s), but every rollout starts with it, and the wrist is driven past its own joint limit on the way.

The alternative fix — replacing base_link_collision with primitives or a decimated hull — would also cut 40k vertices out of every collision query, but that is a much larger change and is not required to resolve the overlap.

Checks

• python format_xml.py --check hello_robot_stretch_3/stretch.xml → clean
• pytest test/ -k stretch → 10 passed
• mjcf-doctor check hello_robot_stretch_3/scene.xml → exit 0, no errors (was the only hard error across the 82 models I scanned)

No other model directory is touched.

[google-cla]

Thanks for your pull request! It looks like this may be your first contribution to a Google open source project. Before we can look at your pull request, you'll need to sign a Contributor License Agreement (CLA).

View this failed invocation of the CLA check for more information.

For the most up to date status, view the checks section at the bottom of the pull request.