Fix measured context: override only frame 0, keep trajectory in frames 1-3

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>

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

@@ -577,7 +577,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();
+            UpdateContextFromMeasuredState(motion_sequence);
         } else {
             UpdateContextFromMotion(motion_sequence);
         }
@@ -703,23 +703,27 @@ public:
     }
 
     /**
-     * @brief Fill planner context from measured robot state (motor encoders + IMU).
+     * @brief Ground planner context frame 0 in measured robot state.
      *
-     * 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.
+     * 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.
      */
-    void UpdateContextFromMeasuredState()
+    void UpdateContextFromMeasuredState(const std::shared_ptr<const MotionSequence>& motion_sequence)
     {
+        // Fill all 4 frames from planner trajectory (provides velocity/continuity)
+        UpdateContextFromMotion(motion_sequence);
+
+        // Override frame 0 with measured state (grounds starting point in reality)
         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);
+        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;
@@ -735,7 +739,6 @@ public:
                 static_cast<float>(measured_joint_positions_[i]);
         }
     }
-    }
 
     /**
      * @brief Common method signatures that derived classes should implement