Planner height input doesn't produce visible knee bend in IDLE mode.
Instead, add a direct offset to both knee motor targets (+5 deg default).
Runtime-adjustable via keys 3/4 (±1 deg). This shifts the standing
equilibrium lower without changing the NN's learned dynamics.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Adds knee bend by passing explicit height (0.72m vs 0.789m default) to the
planner for all standing locomotion modes. Runtime-adjustable via keys 5/6
(±1cm). IMU pitch offset default changed from 0 to -2 deg to correct the
forward-tilt bias observed on the physical robot.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Kp override approach doesn't work — changing PD gains breaks the NN's
trained dynamics. Need to fix perception (IMU offset) not actuation.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Motor test confirmed waist pitch (motor 14) physically moves when
commanded at Kp=60 but not at Kp=28.5. Real-world friction threshold
is ~3 Nm. Doubling Kp from 28.5 to 57 should allow the NN's waist
commands to actually reach their targets.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Now logs: raw NN output, motor command (deg), and measured position (deg)
for all 3 waist joints every second.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Log the decoder's raw action values for waist joints every 5s to
determine whether the policy actively commands waist motion or
outputs near-zero (suggesting locked-waist training).
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
The waist shows dynamic movement during walking (swagger), meaning
the NN IS trained to control it. Overriding Kp amplifies the NN's
calibrated actions and causes oscillation. Revert to default gains.
Keep optional offset keys for future tuning.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Stiffening both axes caused shakiness during walking. Only pitch
(forward/backward tilt) was the original problem. Roll left at
default Kp=28.5 to avoid fighting lateral balance during gait.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Kp=200 caused waist shaking. Back off to 125/7 — still 4x stiffer
than default (28.5) but within stable range.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Higher stiffness better simulates the missing physical waist clamp.
Saturates at ~7° error which is tight enough for rigid behavior.
Kd scaled proportionally for proper damping.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Robot lacks the waist clamp expected by the locked-waist policy
(mode_machine=6). Default Kp=28.5 is intentionally low to protect
gears against a physical lock, but without the clamp the waist flops
under dynamic loads (walking/running), causing backward tilt.
Bump waist_roll and waist_pitch Kp to 100, Kd to 5 so the motors
hold the waist rigid. Remove the 8-degree offset — the waist default
position is correct, it just couldn't hold it. Keep offset keys 7/8
available for tuning if needed. Remove temporary debug logging.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Log cmd_q, hw_q, kp for motor[14] every 2s to diagnose
why the waist motor isn't tracking the position command.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
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>
zhengyiluo