Fix measured context v3: uniform joint blend across all 4 frames

Previous attempts failed:
- v1 (all 4 frames = measured): zero velocity → slow then jerky
- v2 (frame 0 override): 26deg discontinuity → instant chaos

New approach: fill all 4 frames from planner trajectory first, then
blend ONLY joint positions toward measured values uniformly (alpha=0.5).
Position/quaternion stay from planner. Uniform blending preserves
inter-frame velocity (scaled by 0.5) with no discontinuities.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

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

@@ -703,40 +703,37 @@ public:
     }
 
     /**
-     * @brief Ground planner context frame 0 in measured robot state.
+     * @brief Blend planner context joint positions toward measured robot state.
      *
-     * Hybrid approach: first fills all 4 context frames from the planner's
-     * own trajectory (preserving velocity/continuity in frames 1-3), then
-     * overrides frame 0 with actual motor encoder + IMU data. This prevents
-     * the self-feedback convergence (knee straightening) while maintaining
-     * the temporal trajectory information the planner needs for smooth motion.
+     * 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 (provides velocity/continuity)
+        // Fill all 4 frames from planner trajectory (full state + velocity info)
         UpdateContextFromMotion(motion_sequence);
 
-        // Override frame 0 with measured state (grounds starting point in reality)
+        // 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();
-
-        // 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_);
-
-        int base = 0;  // Frame 0 only
-        // 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]);
+        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;
+            }
         }
     }