From 4c7db7fd9ae361e259b088b41acbcc731d65a6a0 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 24 Feb 2026 18:48:28 -0600 Subject: [PATCH] Fix measured context v3: uniform joint blend across all 4 frames MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- .../include/localmotion_kplanner.hpp | 51 +++++++++---------- 1 file changed, 24 insertions(+), 27 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 15150cd..4733c09 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 @@ -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& 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(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]); + 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(measured_joint_positions_[i]); + context_qpos_values[base + 7 + i] = + (1.0f - alpha) * planner_val + alpha * measured_val; + } } }