Replacing all 4 context frames with identical measured snapshots gave the
planner zero velocity information, causing extremely slow motion that
degraded into jerky behavior. Now fills all 4 frames from planner trajectory
first (preserving continuity), then overrides only frame 0 with measured
state to ground the starting point in reality.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Replace planner's self-feedback loop with actual motor encoder + IMU data.
The planner was reading its own previous output as context, causing IDLE mode
to converge on straight knees (~12 deg) instead of the natural bent-knee
posture (~38 deg). Now reads measured joint positions and IMU quaternion
(with yaw stripped) each replan cycle. Toggle with 'm' key for A/B comparison.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Keys 5/6 now initialize from 0.789m (planner default) instead of
operating on -1.0, which caused key 5 to jump to 0.55m and key 6
to have no effect.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Standing height was 0.72m (should be -1.0 = planner default 0.789m).
Hip pitch offset was 3.0 deg (should be 0.0 = no post-NN bias).
Both were experimental overrides that didn't fix knee straightening
and may have been causing subtle planner input mismatches.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
5x wasn't enough to overcome ankle parallel linkage friction.
Robot is harness-supported so higher gains are safe.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Ankle KP was only ~28.5, too weak to hold against body weight.
5x multiplier gives ankles ~142, hips ~495 for proper static hold.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Ankle pitch measured 6.8° off on both sides in neutral hold diagnostic.
Adds runtime ankle offset alongside existing hip/knee/waist/IMU controls.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Bypasses NN output and commands default_angles directly at 500Hz.
Used for visual diagnosis of hardware joint offsets on new pelvis.
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
Theory: newer G1 pelvis has shifted hip pitch zero point, causing
backward lean. IMU offset masked the symptom but caused tippy-toe
stance. Hip pitch offset applied post-NN to motor commands (NN is
blind to adjustment). Default +3 deg more flexion on both hips.
- IMU offset reset to 0 (was -2)
- Knee offset reset to 0 (was 5)
- Hip pitch offset: +3 deg default, keys 1/2 for +/- 1 deg
- Positive = more flexion = legs forward = counteracts backward lean
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
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>
