hello_robot_stretch_3: the stowed wrist starts 48 mm inside base_link's collision hull (398 kN self-contact at t=0) #341

Which model is the issue affecting?

hello_robot_stretch_3/stretch.xml (loaded by scene.xml).

What is the issue?

At the default qpos — which is where every rollout starts, because both keyframes (home, stow) specify only ctrl and leave qpos at zero — link_DW3_wrist_pitch sits inside the collision hull of base_link. Three body pairs interpenetrate at t=0:

None of these three pairs appears in <contact><exclude>. The existing <exclude> list already covers link_lift↔link_mast, link_lift↔link_head, every arm-segment pair, and both gripper fingers against link_DW3_wrist_pitch / link_wrist_yaw — so the stowed-arm self-contacts were clearly considered, and the wrist-vs-torso pair looks like an omission rather than intent.

Why the overlap is this deep: base_link uses base_link_collision, a 40,336-vertex mesh, as its collision geom. Mesh collision runs on the compile-time convex hull, and that hull fills the notch the arm retracts into. At qpos = 0 the wrist body origin is at (-0.041, -0.145, 0.139) m in the base frame, comfortably inside the hull's half-extents (0.081, 0.170, 0.173).

Measured consequences (MuJoCo 3.16, scene.xml, keyframe 0):

So it is a startup transient rather than a permanent deformation — but every rollout begins with a ~400 kN internal impulse, the wrist is driven past its own joint limit, and the settled end-effector pose still differs by 11 mm in y from the same run with the pair excluded. Any policy trained from t=0 sees that impulse as its first observation.

Reproduce (from the repository root):

import mujoco, numpy as np

m = mujoco.MjModel.from_xml_path("hello_robot_stretch_3/scene.xml")
d = mujoco.MjData(m)
mujoco.mj_resetDataKeyframe(m, d, 0)      # keyframe 0 = "home"
mujoco.mj_forward(m, d)

F = 0.0
for k in range(d.ncon):
    f = np.zeros(6)
    mujoco.mj_contactForce(m, d, k, f)
    F = max(F, abs(f[0]))
worst = min(c.dist for c in d.contact[:d.ncon])
print(f"{d.ncon} contacts, deepest {-worst * 1000:.2f} mm, peak {F:.0f} N")

Output:

14 contacts, deepest 48.28 mm, peak 68450 N

Possible fix — three excludes:

<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"/>

With those added, the deepest t=0 contact is the floor at 0 mm, the first-step peak joint speed drops from 37.1 to 0.11 rad/s, and the wrist_pitch transient peak drops from 0.618 to 0.051 rad. Nothing else about the model changes.

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

Is there any additional context you can provide (e.g., a spec sheet or a URDF to show a value mismatch)?

hello_robot_stretch (v1) does not have this: at qpos = 0 it reports a single contact (a caster on the floor) and no self-collision at all. So this looks specific to the v3 model.

I found this by running a linter I wrote over the whole Menagerie collection (it checks for silent failures — models that load and render correctly but behave wrongly). 82 models scanned; this was the one hard error it reported. Numbers above were re-verified by hand with the snippet, not just taken from the tool.