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,37 +703,40 @@ 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);
-            // 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]);
-            }
+        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]);
         }
     }