From bd95a8962053fa2009b4dcc991061b317fff7814 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 24 Feb 2026 19:31:37 -0600 Subject: [PATCH] Revert measured context experiment Reverts commits 8ebca6e, 094dcc5, 4c7db7f. The planner context grounding approach (blending measured state into planner context) is not worth the risk to walking/transitions for a marginal improvement in IDLE knee angle. Co-Authored-By: Claude Opus 4.6 --- .../input_interface/gamepad_manager.hpp | 14 ----- .../include/localmotion_kplanner.hpp | 62 +------------------ .../src/g1_deploy_onnx_ref.cpp | 17 ----- 3 files changed, 1 insertion(+), 92 deletions(-) diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp index 7f1b14a..8512141 100644 --- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp +++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp @@ -75,10 +75,6 @@ inline std::atomic g_ankle_pitch_offset_deg{0.0}; /// Used for visual joint inspection (diagnosing hardware offsets). inline std::atomic g_neutral_hold_mode{false}; -/// Toggle: use measured robot state for planner context. Key 'm' to toggle. -/// true = measured state (breaks self-feedback), false = original self-feedback. -inline std::atomic g_use_measured_context{true}; - #if HAS_ROS2 #include "ros2_input_handler.hpp" #endif @@ -235,16 +231,6 @@ class GamepadManager : public InputInterface { is_manager_key = true; break; } - case 'm': - case 'M': { - bool cur = g_use_measured_context.load(); - g_use_measured_context.store(!cur); - std::cout << "[PLANNER] Context: " - << (!cur ? "MEASURED (grounded)" : "SELF-FEEDBACK (original)") - << std::endl; - is_manager_key = true; - break; - } case '1': { double val = g_hip_pitch_offset_deg.load() - 1.0; g_hip_pitch_offset_deg.store(val); 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 4733c09..8dc5737 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 @@ -269,14 +269,6 @@ public: int gen_frame_; bool motion_available_ = false; - // Measured robot state for context grounding (breaks planner self-feedback loop). - // When enabled, planner context is filled from motor encoders + IMU instead of - // the planner's own previous output. Toggle at runtime with 'm' key. - bool use_measured_context_ = true; - std::array measured_base_quat_ = {1.0, 0.0, 0.0, 0.0}; - std::array measured_joint_positions_ = {}; - bool has_measured_state_ = false; - protected: // Common configuration PlannerConfig config_; @@ -295,19 +287,6 @@ public: return planner_state_.initialized && planner_state_.enabled; } - /** - * @brief Set measured robot state for context grounding. - * Called from the planner thread before UpdatePlanning(). - */ - void SetMeasuredRobotState( - const std::array& base_quat, - const std::array& joint_positions) - { - measured_base_quat_ = base_quat; - measured_joint_positions_ = joint_positions; - has_measured_state_ = true; - } - int GetValidModeValueRange() const { if (config_.version == 2) { @@ -576,11 +555,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(motion_sequence); - } else { - UpdateContextFromMotion(motion_sequence); - } + UpdateContextFromMotion(motion_sequence); auto gather_input_end_time = std::chrono::steady_clock::now(); @@ -702,41 +677,6 @@ public: } } - /** - * @brief Blend planner context joint positions toward measured robot state. - * - * 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 (full state + velocity info) - UpdateContextFromMotion(motion_sequence); - - // 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(); - 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; - } - } - } - /** * @brief Common method signatures that derived classes should implement */ diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp index 89e94c9..8d58416 100644 --- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp +++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp @@ -3453,23 +3453,6 @@ class G1Deploy { last_movement_state_.height = movement_state_data.data->height; } - // Ground planner context in measured robot state (breaks self-feedback loop) - planner_->use_measured_context_ = g_use_measured_context.load(); - if (ls) { - std::array base_quat = float_to_double<4>(ls->imu_state().quaternion()); - // Apply same IMU pitch correction as GatherRobotStateToLogger (line 2660) - double pitch_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0; - double half = pitch_rad / 2.0; - std::array pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0}; - base_quat = quat_mul_d(base_quat, pitch_quat); - - std::array joint_positions; - for (int i = 0; i < G1_NUM_MOTOR; i++) { - joint_positions[i] = ls->motor_state()[i].q(); - } - planner_->SetMeasuredRobotState(base_quat, joint_positions); - } - try { // Update planning with current movement parameters