From 094dcc5a3a157b95cdceaef58028996747ebca47 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 24 Feb 2026 18:36:33 -0600 Subject: [PATCH] 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 --- .../include/localmotion_kplanner.hpp | 49 ++++++++++--------- 1 file changed, 26 insertions(+), 23 deletions(-) diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/localmotion_kplanner.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/localmotion_kplanner.hpp index e6ec136..15150cd 100644 --- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/localmotion_kplanner.hpp +++ b/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& 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(config_.default_height); - // Root quaternion: pitch+roll from IMU, yaw stripped - context_qpos_values[base + 3] = static_cast(quat_no_yaw[0]); - context_qpos_values[base + 4] = static_cast(quat_no_yaw[1]); - context_qpos_values[base + 5] = static_cast(quat_no_yaw[2]); - context_qpos_values[base + 6] = static_cast(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(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(config_.default_height); + // Root quaternion: pitch+roll from IMU, yaw stripped + context_qpos_values[base + 3] = static_cast(quat_no_yaw[0]); + context_qpos_values[base + 4] = static_cast(quat_no_yaw[1]); + context_qpos_values[base + 5] = static_cast(quat_no_yaw[2]); + context_qpos_values[base + 6] = static_cast(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(measured_joint_positions_[i]); } }