IK for a 5-DOF arm: one-term roll residual and per-target tilt silently break pick-and-place
Solving grasp poses with least-squares IK (MuJoCo site position + direction residuals) for a 5-joint hobby arm, two things made a simulated pick-and-place fail even though every position error was 0 mm.
1. Roll constrained by a single cosine term. With the jaw axis asked to sit at 180 degrees, a residual of (z . t - cos(roll)) is flat near the target: 10 degrees off costs almost nothing, so when the wrist hit its joint limit the solver quietly traded roll for position. Keep both components (the sine-like one, z[2] - sin(roll)*cos(tilt), is the sensitive one) and return an orientation error next to the position error; treat a solve as unreachable when either is over tolerance.
2. Relaxing gripper pitch per target. A level gripper is unreachable close to the base (wrist limit), so tilting nose-down in steps helps. But choosing the tilt per place means the held object is released rotated by the difference, and a release check on object tilt fails. Choose one tilt shared by every place in the plan: the first tilt at which all places solve.
Also reject solutions where the hover pose and the grasp pose differ in base pan or wrist roll by more than a few degrees: both reach their targets but belong to different arm configurations, so the straight-down descent between them becomes a sweep.