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 8512141..7f1b14a 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,6 +75,10 @@ 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 @@ -231,6 +235,16 @@ 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 8dc5737..e6ec136 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,6 +269,14 @@ 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_; @@ -287,6 +295,19 @@ 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) { @@ -555,7 +576,11 @@ public: // Update context and get frame information gen_frame_ = gen_frame + config_.motion_look_ahead_steps; - UpdateContextFromMotion(motion_sequence); + if (use_measured_context_ && has_measured_state_) { + UpdateContextFromMeasuredState(); + } else { + UpdateContextFromMotion(motion_sequence); + } auto gather_input_end_time = std::chrono::steady_clock::now(); @@ -677,6 +702,41 @@ public: } } + /** + * @brief Fill planner context from measured robot state (motor encoders + IMU). + * + * 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. + */ + void UpdateContextFromMeasuredState() + { + 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]); + } + } + } + /** * @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 8d58416..89e94c9 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,6 +3453,23 @@ 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