Fix 1: Replace frozen head_ref with EMA-smoothed head position.
Fast head movements (looking around) are compensated instantly,
but slow body movements (stepping) are absorbed by the EMA filter
so robot arms stay stable. --head-smooth-alpha 0.02 (default).
Fix 2: Increase --rot-blend default from 0.3 to 0.7. With the
rotation I-term now active, we can track 70% of wrist rotation
instead of 30%, producing more natural wrist poses while the
I-term corrects any residual drift.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Captures head position at calibration time and compensates for head
drift each frame. tv_wrapper subtracts live head position from wrist
poses, which causes robot arms to move when operator looks around or
walks. Head decoupling adds back (current_head - ref_head) to cancel
this coupling, so only actual hand movement drives the robot arms.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Adds integral correction in axis-angle space to actively correct wrist
rotation drift over time. Complements the existing rotation blend
dampening with closed-loop feedback.
- rotation_to_rotvec() and rotvec_to_rotation() helpers (refactored from scale_rotation)
- --ki-rot (default 0.3) and --i-rot-clamp (default 0.3 rad) CLI args
- Rotation error computed as rotvec(target_R @ actual_R^T), same leaky integrator as position
- Magnitude-based anti-windup clamping (preserves axis direction)
- Switched I-term FK from compute_fk to compute_fk_pose for full SE(3)
- Updated periodic logging to include rotation I-term stats
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
1. Settle ramp: after calibration, smoothly blend VP deltas from 0%
to 100% over --settle-time (default 0.5s). Prevents the initial
arm jerk caused by VP tracking jitter on the first frame.
2. Rotation blend: scale VP rotation deltas by --rot-blend (default
0.3) to prevent wrist joints from drifting to odd positions.
The VP-to-Unitree rotation convention may be wrong for Vision Pro
(tuned for PICO), so dampening rotation tracking avoids the IK
solver fighting a bad rotation target. Use --rot-blend 0 for
position-only tracking, --rot-blend 1 for full rotation.
Both parameters ramp together during settle: rot_alpha = rot_blend * settle_alpha
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
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