Ignores X/Y mode selection — all stick input maps to SLOW_WALK with
proportional speed 0.2-0.8 m/s (matching stock Gamepad range).
Crawl/kneel modes preserved.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Left-multiply applied correction in world frame, cross-coupling
pitch into roll depending on robot heading. Right-multiply applies
in body frame — pure pitch regardless of yaw.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Mathematically verified: gravity[0] = 2*(w*y - x*z) responds to qy,
so Y-axis quaternion [cos(h),0,sin(h),0] is the correct pitch axis.
Left-multiply, default -6°. Keys 9/0 adjust ±1° at runtime.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Remove quaternion correction, global offset variable, and 9/0 key
handlers. Return to the last known working state (proportional stick
control without IMU modification).
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
X-axis was producing roll (sideways tilt). Y-axis is the correct
pitch axis in SONIC's quaternion frame.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Replace fixed 6° correction with live-adjustable offset starting at 0°.
Fix quaternion multiplication order (body-frame). Press 9 to decrease
pitch offset, 0 to increase, printed to console.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
G1's physical IMU is mounted with ~6° forward pitch bias. SONIC's NN
was trained with a perfect virtual IMU, causing backward lean compensation.
Pre-rotate base_quat by -6° about Y axis before it enters observations.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Shows the actual locomotion mode and speed value being sent to the
planner when the stick is active (not IDLE). Helps diagnose whether
SLOW_WALK mode is being selected and speeds are in-distribution.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Walk mode (X): SLOW_WALK gait at 0.1-0.8 m/s (lower half of stick),
WALK gait at 0.8-2.0 m/s (upper half). Run mode (Y): WALK gait at
0.8-2.5 m/s (lower half), RUN gait at 2.5-4.5 m/s (upper half).
Ensures planner always receives in-distribution speeds matching the
neural network's training clip ranges per locomotion mode.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Left stick magnitude now maps to movement speed instead of binary on/off.
Removed 30-degree angle binning for smooth continuous direction control.
WALK: 0.3-1.5 m/s, RUN: 2.0-4.5 m/s, CRAWL: 0.3-1.2 m/s proportional
to stick deflection. Dead zone behavior unchanged (IDLE when released).
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Joe DiPrima
zhengyiluo