Revert measured context experiment

Reverts commits 8ebca6e, 094dcc5, 4c7db7f. The planner context
grounding approach (blending measured state into planner context)
is not worth the risk to walking/transitions for a marginal
improvement in IDLE knee angle.

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,10 +75,6 @@ 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
@@ -235,16 +231,6 @@ 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,14 +269,6 @@ 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_;
@@ -295,19 +287,6 @@ 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)
         {
@@ -576,11 +555,7 @@ public:
         
         // Update context and get frame information
         gen_frame_ = gen_frame + config_.motion_look_ahead_steps;
-        if (use_measured_context_ && has_measured_state_) {
-            UpdateContextFromMeasuredState(motion_sequence);
-        } else {
-            UpdateContextFromMotion(motion_sequence);
-        }
+        UpdateContextFromMotion(motion_sequence);
 
         auto gather_input_end_time = std::chrono::steady_clock::now();
 
@@ -702,41 +677,6 @@ public:
         }
     }
 
-    /**
-     * @brief Blend planner context joint positions toward measured robot state.
-     *
-     * Fills all 4 context frames from the planner's trajectory first (preserving
-     * position, quaternion, and inter-frame velocity), then blends ONLY the joint
-     * positions toward measured values uniformly across all 4 frames. Uniform
-     * blending preserves inter-frame velocity (scaled by 1-alpha) while pulling
-     * joint angles toward reality. This prevents self-feedback convergence
-     * (knee straightening) without creating velocity discontinuities.
-     *
-     * alpha=0: pure self-feedback (original behavior)
-     * alpha=1: fully measured joints (may lose velocity info)
-     * alpha=0.5: balanced blend (recommended starting point)
-     */
-    void UpdateContextFromMeasuredState(const std::shared_ptr<const MotionSequence>& motion_sequence)
-    {
-        // Fill all 4 frames from planner trajectory (full state + velocity info)
-        UpdateContextFromMotion(motion_sequence);
-
-        // Blend joint positions toward measured values in all 4 frames uniformly.
-        // Position and quaternion stay from planner trajectory (no discontinuity).
-        constexpr float alpha = 0.5f;
-        auto context_qpos_values = GetContextBuffer();
-        for (int n = 0; n < 4; ++n) {
-            int base = n * (G1_NUM_MOTOR + 7);
-            // Only blend joints [7..35], leave position [0..2] and quat [3..6] from planner
-            for (int i = 0; i < G1_NUM_MOTOR; i++) {
-                float planner_val = context_qpos_values[base + 7 + i];
-                float measured_val = static_cast<float>(measured_joint_positions_[i]);
-                context_qpos_values[base + 7 + i] =
-                    (1.0f - alpha) * planner_val + alpha * measured_val;
-            }
-        }
-    }
-
     /**
      * @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,23 +3453,6 @@ 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