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>