Default Kp=28.5 generates only ~4 Nm at 8° error, insufficient to
overcome gravity on the upper body. Bump to Kp=100, Kd=5 (matching
hip-level stiffness) so the waist motor actually drives to the
commanded position. Motor effort limit is 25 Nm, so 14 Nm command
at 8° error is well within range.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
The SONIC policy was likely trained on mode_machine=6 (locked waist variant)
where waist_roll and waist_pitch are fixed joints. On our mode_machine=5
robot the waist is free but the NN outputs near-zero for it. This adds a
configurable waist_pitch offset (default +8°) applied directly to the motor
command. Keys 7/8 adjust ±1° at runtime via tmux.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Planner NN was trained with SLOW_WALK range [0.2, 0.8] m/s per NVIDIA
docs. Sending 0.0 m/s is out-of-distribution. Restored 0.2 minimum,
ramp now 0.2->0.8 over ~2s at 100Hz input loop.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Input thread runs at 100Hz not 200Hz. Corrected:
- Rotation: 0.008 rad/frame = 45 deg/s at full stick
- Speed ramp: 0.002/frame = 4s to reach 0.8 m/s
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
- Rescale SLOW_WALK speed from [0.2, 0.8] to [0.0, 0.8] so dead zone
edge produces zero speed instead of jumping to 0.2 m/s
- Add acceleration rate limiter (0.01/frame = 0.4s ramp at 200Hz)
that smooths speed increases but allows instant deceleration
- Fixes exaggerated first steps caused by instant IDLE to 0.2 m/s jump
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
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