Captures VP wrist poses and robot FK poses when tracking starts (A/r),
then maps all subsequent VP input as deltas from the user's reference
onto the robot's reference frame. This eliminates the arm skew that
occurred because the system was mapping absolute VP positions directly.
- Add compute_fk_pose() returning full 4x4 SE(3) matrices from Pinocchio FK
- Calibrate on every START transition (initial + resume after pause)
- Position: target = robot_ref + (vp_current - vp_ref)
- Rotation: target_R = (vp_R @ vp_ref_R^T) @ robot_ref_R
- Arm reset (X) auto-pauses and invalidates calibration
- I-term error now uses calibrated target instead of raw VP
- --no-calibration flag preserves old absolute-mode behavior
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Joe DiPrima
206830c530