Ground planner context in measured robot state

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>

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -75,6 +75,10 @@ inline std::atomic<double> g_ankle_pitch_offset_deg{0.0};
 /// Used for visual joint inspection (diagnosing hardware offsets).
 inline std::atomic<bool> g_neutral_hold_mode{false};
 
+/// Toggle: use measured robot state for planner context. Key 'm' to toggle.
+/// true = measured state (breaks self-feedback), false = original self-feedback.
+inline std::atomic<bool> g_use_measured_context{true};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -231,6 +235,16 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             break;
           }
+          case 'm':
+          case 'M': {
+            bool cur = g_use_measured_context.load();
+            g_use_measured_context.store(!cur);
+            std::cout << "[PLANNER] Context: "
+                      << (!cur ? "MEASURED (grounded)" : "SELF-FEEDBACK (original)")
+                      << std::endl;
+            is_manager_key = true;
+            break;
+          }
           case '1': {
             double val = g_hip_pitch_offset_deg.load() - 1.0;
             g_hip_pitch_offset_deg.store(val);

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/localmotion_kplanner.hpp

@@ -269,6 +269,14 @@ public:
     int gen_frame_;
     bool motion_available_ = false;
 
+    // Measured robot state for context grounding (breaks planner self-feedback loop).
+    // When enabled, planner context is filled from motor encoders + IMU instead of
+    // the planner's own previous output. Toggle at runtime with 'm' key.
+    bool use_measured_context_ = true;
+    std::array<double, 4> measured_base_quat_ = {1.0, 0.0, 0.0, 0.0};
+    std::array<double, 29> measured_joint_positions_ = {};
+    bool has_measured_state_ = false;
+
 protected:
     // Common configuration
     PlannerConfig config_;
@@ -287,6 +295,19 @@ public:
         return planner_state_.initialized && planner_state_.enabled;
     }
 
+    /**
+     * @brief Set measured robot state for context grounding.
+     * Called from the planner thread before UpdatePlanning().
+     */
+    void SetMeasuredRobotState(
+        const std::array<double, 4>& base_quat,
+        const std::array<double, 29>& joint_positions)
+    {
+        measured_base_quat_ = base_quat;
+        measured_joint_positions_ = joint_positions;
+        has_measured_state_ = true;
+    }
+
     int GetValidModeValueRange() const {
         if (config_.version == 2)
         {
@@ -555,7 +576,11 @@ public:
         
         // Update context and get frame information
         gen_frame_ = gen_frame + config_.motion_look_ahead_steps;
-        UpdateContextFromMotion(motion_sequence);
+        if (use_measured_context_ && has_measured_state_) {
+            UpdateContextFromMeasuredState();
+        } else {
+            UpdateContextFromMotion(motion_sequence);
+        }
 
         auto gather_input_end_time = std::chrono::steady_clock::now();
 
@@ -677,6 +702,41 @@ public:
         }
     }
 
+    /**
+     * @brief Fill planner context from measured robot state (motor encoders + IMU).
+     *
+     * Replaces the self-feedback loop of UpdateContextFromMotion where the planner
+     * reads its own previous output. All 4 context frames are set to the same
+     * measured snapshot. Follows the same write pattern as InitializeContext:
+     * motor_state is MuJoCo order, context buffer is MuJoCo order, direct write.
+     */
+    void UpdateContextFromMeasuredState()
+    {
+        auto context_qpos_values = GetContextBuffer();
+
+        // Strip yaw from IMU quaternion — planner expects zero-yaw frame.
+        auto heading_inv = calc_heading_quat_inv_d(measured_base_quat_);
+        auto quat_no_yaw = quat_mul_d(heading_inv, measured_base_quat_);
+
+        for (int n = 0; n < 4; ++n) {
+            int base = n * (G1_NUM_MOTOR + 7);
+            // Root position: (0, 0, standing_height) — no global position tracking
+            context_qpos_values[base + 0] = 0.0f;
+            context_qpos_values[base + 1] = 0.0f;
+            context_qpos_values[base + 2] = static_cast<float>(config_.default_height);
+            // Root quaternion: pitch+roll from IMU, yaw stripped
+            context_qpos_values[base + 3] = static_cast<float>(quat_no_yaw[0]);
+            context_qpos_values[base + 4] = static_cast<float>(quat_no_yaw[1]);
+            context_qpos_values[base + 5] = static_cast<float>(quat_no_yaw[2]);
+            context_qpos_values[base + 6] = static_cast<float>(quat_no_yaw[3]);
+            // Joint positions: motor_state is MuJoCo order, context is MuJoCo order
+            for (int i = 0; i < G1_NUM_MOTOR; i++) {
+                context_qpos_values[base + 7 + i] =
+                    static_cast<float>(measured_joint_positions_[i]);
+            }
+        }
+    }
+
     /**
      * @brief Common method signatures that derived classes should implement
      */

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@@ -3453,6 +3453,23 @@ class G1Deploy {
               last_movement_state_.height = movement_state_data.data->height;
             }
             
+            // Ground planner context in measured robot state (breaks self-feedback loop)
+            planner_->use_measured_context_ = g_use_measured_context.load();
+            if (ls) {
+              std::array<double, 4> base_quat = float_to_double<4>(ls->imu_state().quaternion());
+              // Apply same IMU pitch correction as GatherRobotStateToLogger (line 2660)
+              double pitch_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
+              double half = pitch_rad / 2.0;
+              std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0};
+              base_quat = quat_mul_d(base_quat, pitch_quat);
+
+              std::array<double, 29> joint_positions;
+              for (int i = 0; i < G1_NUM_MOTOR; i++) {
+                joint_positions[i] = ls->motor_state()[i].q();
+              }
+              planner_->SetMeasuredRobotState(base_quat, joint_positions);
+            }
+
             try {
               // Update planning with current movement parameters