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>
- A button and r key now toggle tracking (start/pause) instead of one-way start
- X/h arm reset works regardless of tracking state (can pause then reset)
- I-term accumulators cleared when paused to prevent stale drift correction
- Main loop stays alive while paused (buttons still polled, can resume with A)
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Map physical R3 buttons (A/B/X/Y/Start) to teleop actions so the
operator can control tracking, arm reset, I-term toggle, and recording
without needing a separate SSH keyboard session.
- Store wireless_remote bytes from rt/lowstate in G1_29_LowState
- Add get_wireless_remote() accessor to arm controller
- Parse button bitmasks with edge detection (fire on 0→1 only)
- A=start, B=stop, X=reset arms, Y=toggle I-term, Start=toggle recording
- Buttons work in both pre-tracking wait loop and main control loop
- Keyboard shortcuts (r/q/h/i/s) still work alongside R3 buttons
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Adds a closed-loop integral (I) term to compensate for arm drift caused
by the 250Hz velocity clipper. When the user moves faster than the clip
limit, the robot falls behind; the I-term accumulates the task-space
position error (FK vs VP target) and biases future IK targets to catch up.
New features:
- --ki/--i-clamp/--i-decay CLI args for tunable gain, anti-windup, decay
- 'h' key resets arms to home position without exiting the program
- 'i' key toggles integral correction on/off mid-session
- Per-second logging of error magnitude, I accumulator, and correction
- compute_fk() method on G1_29_ArmIK for Pinocchio forward kinematics
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Issues caused:
• Import executed only at runtime → inconsistent behavior
• IDE & linters cannot detect missing modules
• Violates PEP8: imports must appear at top of file
• Could cause crashes if execution flow skips the lazy import
Joe DiPrima
孙昌贺
silencht
dejun.huang
Yunjai, Lee
libsxm-dev