Add files using upload-large-folder tool

.gitattributes

@@ -544,3 +544,10 @@ GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/meshes/g1/left_w
 GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/meshes/g1/pelvis_contour_link.STL filter=lfs diff=lfs merge=lfs -text
 GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/meshes/g1/right_hand_middle_0_link.STL filter=lfs diff=lfs merge=lfs -text
 GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/meshes/g1/NVBP.STL filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_roll_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_ankle_pitch_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/right_knee_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_yaw_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_wrist_roll_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_hip_pitch_link.stl filter=lfs diff=lfs merge=lfs -text
+GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_pitch_link.stl filter=lfs diff=lfs merge=lfs -text

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_ankle_pitch_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:42bf18934f633451187365d240b97b0412041e06d3583ec2e93d9e84d0e51f8f
+size 430684

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_hip_pitch_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ca7361b81500c3d738bc40fb40c624294b19a3147d6334e888d020ea2be6c0cb
+size 1619884

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_pitch_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0f70851b1ba071d00504fed43f213937d250bf61524bb2e6efbec4cf32052a06
+size 9459584

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_roll_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9994d592925ff408ec0590fe438d74045ea86ae83d7e2d620741811bcc002d66
+size 749884

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_shoulder_yaw_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a183a0969a0f5e8fe6364515ab8cebfef9498a267c2e0541213d7e2098b921b
+size 1202984

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/left_wrist_roll_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:aac4f99935429b9bedc1a1df7e664885a9406c99cc692a1fc3556c59e475bb12
+size 622484

GR00T-WholeBodyControl/gear_sonic/data/assets/robot_description/urdf/h2/meshes/right_knee_link.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3cce66e474d1679da1ba570804ee10b2cd0adfddbb6e5bfd3d32b42ac3806d64
+size 747184

GR00T-WholeBodyControl/gear_sonic_deploy/policy/release/observation_config.yaml

@@ -0,0 +1,80 @@
+# G1 Deploy - Observation Configuration
+# ====================================
+# Total dimension: 436 (64+12+116+116+116+12)
+
+observations:
+
+  - name: "token_state"
+    enabled: true
+
+  - name: "his_base_angular_velocity_10frame_step1"
+    enabled: true
+    
+  - name: "his_body_joint_positions_10frame_step1"
+    enabled: true
+    
+  - name: "his_body_joint_velocities_10frame_step1"
+    enabled: true
+    
+  - name: "his_last_actions_10frame_step1"
+    enabled: true
+
+  - name: "his_gravity_dir_10frame_step1"
+    enabled: true
+
+encoder:
+  dimension: 64
+  use_fp16: false
+  encoder_observations:
+    - name: "encoder_mode_4"
+      enabled: true
+    - name: "motion_joint_positions_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_10frame_step5"
+      enabled: true
+    - name: "motion_root_z_position_10frame_step5"
+      enabled: true
+    - name: "motion_root_z_position"
+      enabled: true
+    - name: "motion_anchor_orientation"
+      enabled: true
+    - name: "motion_anchor_orientation_10frame_step5"
+      enabled: true
+    - name: "motion_joint_positions_lowerbody_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_lowerbody_10frame_step5"
+      enabled: true
+    - name: "vr_3point_local_target"
+      enabled: true
+    - name: "vr_3point_local_orn_target"
+      enabled: true
+    - name: "smpl_joints_10frame_step1"
+      enabled: true
+    - name: "smpl_anchor_orientation_10frame_step1" 
+      enabled: true
+    - name: "motion_joint_positions_wrists_10frame_step1" 
+      enabled: true
+  encoder_modes:
+    - name: "g1"
+      mode_id: 0
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_10frame_step5
+        - motion_joint_velocities_10frame_step5
+        - motion_anchor_orientation_10frame_step5
+    - name: "teleop"
+      mode_id: 1
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_lowerbody_10frame_step5
+        - motion_joint_velocities_lowerbody_10frame_step5
+        - vr_3point_local_target
+        - vr_3point_local_orn_target
+        - motion_anchor_orientation
+    - name: "smpl"
+      mode_id: 2
+      required_observations:
+        - encoder_mode_4
+        - smpl_joints_10frame_step1
+        - smpl_anchor_orientation_10frame_step1
+        - motion_joint_positions_wrists_10frame_step1

GR00T-WholeBodyControl/gear_sonic_deploy/policy/release/observation_config_low_latency.yaml

@@ -0,0 +1,92 @@
+# SONIC Release (Low-Latency) - Observation Configuration
+# =======================================================
+# For ONNX models exported from the low-latency sonic_release checkpoint
+# (smpl 4-frame lookahead; g1/teleop lookahead at step1, body-frame orientation).
+#
+# Encoder input dimension: 1247   Decoder/policy dimension: 994   token: 64
+# Uses the unitoken_all_noz tokenizer config (3 encoders: g1, teleop, smpl)
+#
+# Requires the smpl_*_4frame_step1 entries in GetObservationRegistry()
+# (src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp) + a rebuild to load.
+
+observations:
+
+  - name: "token_state"
+    enabled: true
+
+  - name: "his_base_angular_velocity_10frame_step1"
+    enabled: true
+
+  - name: "his_body_joint_positions_10frame_step1"
+    enabled: true
+
+  - name: "his_body_joint_velocities_10frame_step1"
+    enabled: true
+
+  - name: "his_last_actions_10frame_step1"
+    enabled: true
+
+  - name: "his_gravity_dir_10frame_step1"
+    enabled: true
+
+encoder:
+  dimension: 64
+  use_fp16: false
+  encoder_observations:
+    # Shared
+    - name: "encoder_mode_4"
+      enabled: true
+    # g1 encoder: command_multi_future_nonflat (positions + velocities), step1
+    - name: "motion_joint_positions_10frame_step1"
+      enabled: true
+    - name: "motion_joint_velocities_10frame_step1"
+      enabled: true
+    # g1 encoder: motion_anchor_ori_b_mf_nonflat (body, multi-frame FIRST), step1
+    - name: "motion_anchor_orientation_10frame_step1"
+      enabled: true
+    # teleop encoder: motion_anchor_ori_b (body, single-frame SECOND)
+    - name: "motion_anchor_orientation"
+      enabled: true
+    # teleop encoder: command_multi_future_lower_body, step1
+    - name: "motion_joint_positions_lowerbody_10frame_step1"
+      enabled: true
+    - name: "motion_joint_velocities_lowerbody_10frame_step1"
+      enabled: true
+    # teleop encoder: vr_3point
+    - name: "vr_3point_local_target"
+      enabled: true
+    - name: "vr_3point_local_orn_target"
+      enabled: true
+    # smpl encoder: smpl_joints_multi_future_local_nonflat (4 frames, step1)  <-- LOW-LATENCY
+    - name: "smpl_joints_4frame_step1"
+      enabled: true
+    # smpl encoder: smpl_root_ori_b_multi_future (body, 4 frames, step1)      <-- LOW-LATENCY
+    - name: "smpl_anchor_orientation_4frame_step1"
+      enabled: true
+    # smpl encoder: joint_pos_multi_future_wrist_for_smpl (4 frames, step1)   <-- LOW-LATENCY
+    - name: "motion_joint_positions_wrists_4frame_step1"
+      enabled: true
+  encoder_modes:
+    - name: "g1"
+      mode_id: 0
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_10frame_step1
+        - motion_joint_velocities_10frame_step1
+        - motion_anchor_orientation_10frame_step1
+    - name: "teleop"
+      mode_id: 1
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_lowerbody_10frame_step1
+        - motion_joint_velocities_lowerbody_10frame_step1
+        - vr_3point_local_target
+        - vr_3point_local_orn_target
+        - motion_anchor_orientation
+    - name: "smpl"
+      mode_id: 2
+      required_observations:
+        - encoder_mode_4
+        - smpl_joints_4frame_step1
+        - smpl_anchor_orientation_4frame_step1
+        - motion_joint_positions_wrists_4frame_step1

GR00T-WholeBodyControl/gear_sonic_deploy/policy/release/observation_config_sonic_release.yaml

@@ -0,0 +1,79 @@
+# SONIC Release - Observation Configuration
+# ==========================================
+# For ONNX models exported from sonic_release checkpoint via:
+#   python gear_sonic/eval_agent_trl.py +checkpoint=sonic_release/last.pt \
+#       +headless=True ++num_envs=1 +export_onnx_only=true
+#
+# Encoder input dimension: 1751 (1 encoder_index + 1750 tokenizer obs)
+# Uses the unitoken_all_noz tokenizer config (3 encoders: g1, teleop, smpl)
+
+observations:
+
+  - name: "token_state"
+    enabled: true
+
+  - name: "his_base_angular_velocity_10frame_step1"
+    enabled: true
+
+  - name: "his_body_joint_positions_10frame_step1"
+    enabled: true
+
+  - name: "his_body_joint_velocities_10frame_step1"
+    enabled: true
+
+  - name: "his_last_actions_10frame_step1"
+    enabled: true
+
+  - name: "his_gravity_dir_10frame_step1"
+    enabled: true
+
+encoder:
+  dimension: 64
+  use_fp16: false
+  encoder_observations:
+    # Used by all encoder modes
+    - name: "encoder_index"
+      enabled: true
+    # G1 mode
+    - name: "command_multi_future_nonflat"
+      enabled: true
+    - name: "motion_anchor_ori_b_mf_nonflat"
+      enabled: true
+    # Teleop mode
+    - name: "command_multi_future_lower_body"
+      enabled: true
+    - name: "vr_3point_local_target"
+      enabled: true
+    - name: "vr_3point_local_orn_target"
+      enabled: true
+    - name: "motion_anchor_ori_b"
+      enabled: true
+    # SMPL mode
+    - name: "smpl_joints_multi_future_local_nonflat"
+      enabled: true
+    - name: "smpl_root_ori_b_multi_future"
+      enabled: true
+    - name: "joint_pos_multi_future_wrist_for_smpl"
+      enabled: true
+  encoder_modes:
+    - name: "g1"
+      mode_id: 0
+      required_observations:
+        - encoder_index
+        - command_multi_future_nonflat
+        - motion_anchor_ori_b_mf_nonflat
+    - name: "teleop"
+      mode_id: 1
+      required_observations:
+        - encoder_index
+        - command_multi_future_lower_body
+        - vr_3point_local_target
+        - vr_3point_local_orn_target
+        - motion_anchor_ori_b
+    - name: "smpl"
+      mode_id: 2
+      required_observations:
+        - encoder_index
+        - smpl_joints_multi_future_local_nonflat
+        - smpl_root_ori_b_multi_future
+        - joint_pos_multi_future_wrist_for_smpl

GR00T-WholeBodyControl/gear_sonic_deploy/policy/release/observation_config_sonic_v1_1.yaml

@@ -0,0 +1,74 @@
+# SONIC v1.1 - Observation Configuration
+# =================================================
+# Matches sonic_v1_1/model_encoder.onnx and model_decoder.onnx.
+#
+# Encoder input: 1751   Decoder input: 994   Token: 64
+# G1/teleop references use 10 frames at step5. SMPL and wrist references use
+# 10 frames at step1. Root orientations are normalized by robot heading.
+
+observations:
+  - name: "token_state"
+    enabled: true
+  - name: "his_base_angular_velocity_10frame_step1"
+    enabled: true
+  - name: "his_body_joint_positions_10frame_step1"
+    enabled: true
+  - name: "his_body_joint_velocities_10frame_step1"
+    enabled: true
+  - name: "his_last_actions_10frame_step1"
+    enabled: true
+  - name: "his_gravity_dir_10frame_step1"
+    enabled: true
+
+encoder:
+  dimension: 64
+  use_fp16: false
+  encoder_observations:
+    - name: "encoder_mode_4"
+      enabled: true
+    - name: "motion_joint_positions_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_10frame_step5"
+      enabled: true
+    - name: "motion_anchor_orientation_heading_10frame_step5"
+      enabled: true
+    - name: "motion_anchor_orientation_heading"
+      enabled: true
+    - name: "motion_joint_positions_lowerbody_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_lowerbody_10frame_step5"
+      enabled: true
+    - name: "vr_3point_local_target"
+      enabled: true
+    - name: "vr_3point_local_orn_target"
+      enabled: true
+    - name: "smpl_joints_10frame_step1"
+      enabled: true
+    - name: "smpl_anchor_orientation_heading_10frame_step1"
+      enabled: true
+    - name: "motion_joint_positions_wrists_10frame_step1"
+      enabled: true
+  encoder_modes:
+    - name: "g1"
+      mode_id: 0
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_10frame_step5
+        - motion_joint_velocities_10frame_step5
+        - motion_anchor_orientation_heading_10frame_step5
+    - name: "teleop"
+      mode_id: 1
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_lowerbody_10frame_step5
+        - motion_joint_velocities_lowerbody_10frame_step5
+        - vr_3point_local_target
+        - vr_3point_local_orn_target
+        - motion_anchor_orientation_heading
+    - name: "smpl"
+      mode_id: 2
+      required_observations:
+        - encoder_mode_4
+        - smpl_joints_10frame_step1
+        - smpl_anchor_orientation_heading_10frame_step1
+        - motion_joint_positions_wrists_10frame_step1

GR00T-WholeBodyControl/gear_sonic_deploy/policy/sonic_v1_1/observation_config.yaml

@@ -0,0 +1,74 @@
+# SONIC v1.1 - Observation Configuration
+# =================================================
+# Matches sonic_v1_1/model_encoder.onnx and model_decoder.onnx.
+#
+# Encoder input: 1751   Decoder input: 994   Token: 64
+# G1/teleop references use 10 frames at step5. SMPL and wrist references use
+# 10 frames at step1. Root orientations are normalized by robot heading.
+
+observations:
+  - name: "token_state"
+    enabled: true
+  - name: "his_base_angular_velocity_10frame_step1"
+    enabled: true
+  - name: "his_body_joint_positions_10frame_step1"
+    enabled: true
+  - name: "his_body_joint_velocities_10frame_step1"
+    enabled: true
+  - name: "his_last_actions_10frame_step1"
+    enabled: true
+  - name: "his_gravity_dir_10frame_step1"
+    enabled: true
+
+encoder:
+  dimension: 64
+  use_fp16: false
+  encoder_observations:
+    - name: "encoder_mode_4"
+      enabled: true
+    - name: "motion_joint_positions_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_10frame_step5"
+      enabled: true
+    - name: "motion_anchor_orientation_heading_10frame_step5"
+      enabled: true
+    - name: "motion_anchor_orientation_heading"
+      enabled: true
+    - name: "motion_joint_positions_lowerbody_10frame_step5"
+      enabled: true
+    - name: "motion_joint_velocities_lowerbody_10frame_step5"
+      enabled: true
+    - name: "vr_3point_local_target"
+      enabled: true
+    - name: "vr_3point_local_orn_target"
+      enabled: true
+    - name: "smpl_joints_10frame_step1"
+      enabled: true
+    - name: "smpl_anchor_orientation_heading_10frame_step1"
+      enabled: true
+    - name: "motion_joint_positions_wrists_10frame_step1"
+      enabled: true
+  encoder_modes:
+    - name: "g1"
+      mode_id: 0
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_10frame_step5
+        - motion_joint_velocities_10frame_step5
+        - motion_anchor_orientation_heading_10frame_step5
+    - name: "teleop"
+      mode_id: 1
+      required_observations:
+        - encoder_mode_4
+        - motion_joint_positions_lowerbody_10frame_step5
+        - motion_joint_velocities_lowerbody_10frame_step5
+        - vr_3point_local_target
+        - vr_3point_local_orn_target
+        - motion_anchor_orientation_heading
+    - name: "smpl"
+      mode_id: 2
+      required_observations:
+        - encoder_mode_4
+        - smpl_joints_10frame_step1
+        - smpl_anchor_orientation_heading_10frame_step1
+        - motion_joint_positions_wrists_10frame_step1

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad.hpp

@@ -0,0 +1,777 @@
+/**
+ * @file gamepad.hpp
+ * @brief Unitree wireless gamepad driver and InputInterface implementation.
+ *
+ * This file contains:
+ *  1. Low-level data structures that map directly onto the raw 40-byte
+ *     wireless-remote packet received from the Unitree SDK
+ *     (xKeySwitchUnion, xRockerBtnDataStruct, REMOTE_DATA_RX).
+ *  2. A simple edge-detecting Button helper class.
+ *  3. The Gamepad class – a full InputInterface that translates raw button
+ *     and analog-stick data into motion / planner commands.
+ *
+ * The Gamepad supports two operational modes:
+ *  - **Non-planner mode** – uses the face buttons and bumpers to cycle through
+ *    pre-loaded reference motions and play/pause/restart them.
+ *  - **Planner mode** (toggled with F1) – the left stick controls movement
+ *    direction, the right stick controls facing direction, and the bumpers
+ *    switch between locomotion modes (idle / slow walk / walk / run / squat /
+ *    kneel).  L2/R2 adjust speed or height depending on the current mode.
+ *
+ * See the button-mapping comment block inside the class for the complete mapping.
+ */
+
+#ifndef GAMEPAD_HPP
+#define GAMEPAD_HPP
+
+#include <cmath>
+#include <iostream>
+#include <array>
+#include <thread>
+#include <chrono>
+#include <cstdlib>
+#include "input_interface.hpp"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace unitree::common {
+
+// =========================================================================
+// Raw wireless-remote data structures (match the Unitree SDK wire format)
+// =========================================================================
+
+/**
+ * @brief Bit-field union for the 16-bit button state word.
+ *
+ * Each member is a single-bit flag corresponding to one physical button.
+ * The `value` member gives access to all 16 bits as a single uint16_t.
+ */
+typedef union {
+    struct {
+        uint8_t R1 : 1;      ///< Right bumper
+        uint8_t L1 : 1;      ///< Left bumper
+        uint8_t start : 1;   ///< Start button
+        uint8_t select : 1;  ///< Select / back button
+        uint8_t R2 : 1;      ///< Right trigger (digital)
+        uint8_t L2 : 1;      ///< Left trigger (digital)
+        uint8_t F1 : 1;      ///< Function 1 button
+        uint8_t F2 : 1;      ///< Function 2 button
+        uint8_t A : 1;       ///< A face button
+        uint8_t B : 1;       ///< B face button
+        uint8_t X : 1;       ///< X face button
+        uint8_t Y : 1;       ///< Y face button
+        uint8_t up : 1;      ///< D-pad up
+        uint8_t right : 1;   ///< D-pad right
+        uint8_t down : 1;    ///< D-pad down
+        uint8_t left : 1;    ///< D-pad left
+    } components;
+
+    uint16_t value;  ///< Raw 16-bit button word.
+} xKeySwitchUnion;
+
+/**
+ * @brief 40-byte raw joystick packet (24 bytes used, 16 idle/reserved).
+ *
+ * Layout matches the Unitree wireless remote receiver format.
+ */
+typedef struct {
+    uint8_t head[2];        ///< 2-byte packet header
+    xKeySwitchUnion btn;    ///< 16-bit button state
+    float lx;               ///< Left stick horizontal  (−1..+1)
+    float rx;               ///< Right stick horizontal  (−1..+1)
+    float ry;               ///< Right stick vertical    (−1..+1)
+    float L2;               ///< Left trigger analog     (0..+1)
+    float ly;               ///< Left stick vertical     (−1..+1)
+
+    uint8_t idle[16];       ///< Reserved / unused bytes
+} xRockerBtnDataStruct;
+
+/**
+ * @brief Union overlay giving byte-array access to the full 40-byte packet.
+ *
+ * `RF_RX` provides structured access; `buff` gives raw byte-level access
+ * (useful for memcpy from the SDK callback).
+ */
+typedef union {
+    xRockerBtnDataStruct RF_RX;
+    uint8_t buff[40] = {0};
+} REMOTE_DATA_RX;
+
+/**
+ * @brief Simple edge-detecting button helper.
+ *
+ * Call update() once per frame with the current digital state.
+ * After the call:
+ *   - `pressed`    – true while the button is held down.
+ *   - `on_press`   – true only on the frame the button transitions from released → pressed.
+ *   - `on_release` – true only on the frame the button transitions from pressed → released.
+ */
+class Button {
+  public:
+    Button() {}
+
+    /// Update the button state for this frame.
+    void update(bool state) {
+      on_press = state ? state != pressed : false;
+      on_release = state ? false : state != pressed;
+      pressed = state;
+    }
+
+    bool pressed = false;     ///< Currently held down.
+    bool on_press = false;    ///< Rising edge this frame.
+    bool on_release = false;  ///< Falling edge this frame.
+};
+
+/**
+ * @class Gamepad
+ * @brief InputInterface implementation for the Unitree wireless gamepad.
+ *
+ * Reads raw joystick packets via `gamepad_data` (populated externally by
+ * InterfaceManager::UpdateGamepadRemoteData) and translates button presses /
+ * stick deflections into per-frame action flags.
+ *
+ * Supports two modes:
+ *  - Reference-motion mode (default): L1/R1 cycle motions, A plays, B restarts.
+ *  - Planner mode (toggled by F1):    sticks control direction / facing,
+ *    L1/R1 change locomotion mode, L2/R2 adjust speed/height.
+ */
+class Gamepad : public InputInterface {
+  public:
+    /// Compile-time toggle for debug log output.  Set to false and rebuild to suppress.
+    static constexpr bool DEBUG_LOGGING = false;
+    
+    // ------------------------------------------------------------------
+    // Per-frame action flags (reset at the start of every update() call)
+    // ------------------------------------------------------------------
+    bool motion_prev = false;      ///< Switch to previous pre-loaded motion.
+    bool motion_next = false;      ///< Switch to next pre-loaded motion.
+    bool play_motion = false;      ///< Start / resume motion playback.
+    bool motion_restart = false;   ///< Reset current motion to frame 0 (paused).
+
+    bool start_control = false;    ///< Request control-system start.
+    bool stop_control = false;     ///< Request emergency stop.
+
+    bool delta_left = false;       ///< Nudge heading left  (+0.1 rad).
+    bool delta_right = false;      ///< Nudge heading right (−0.1 rad).
+
+    bool reinitialize = false;     ///< Recapture the IMU base quaternion.
+
+    bool use_planner = false;      ///< True when planner mode is active (toggle via F1).
+
+    // ------------------------------------------------------------------
+    // Planner-mode state (persists across frames)
+    // ------------------------------------------------------------------
+    int planner_use_movement_mode = 1;         ///< Locomotion mode index (1=slow walk, 2=walk, 3=run, …).
+    double planner_use_movement_speed = -1;    ///< Desired speed (−1 = mode default).
+    double planner_use_height = -1;            ///< Desired body height (−1 = mode default).
+    bool planner_emergency_stop = false;       ///< Immediate halt flag (B button in planner mode).
+    double planner_facing_angle = 0.0;         ///< Accumulated facing angle from right stick (radians).
+    double planner_moving_direction = 0.0;     ///< Current movement direction from left stick (radians).
+
+    /// Raw wireless-remote data buffer.  Written externally (e.g. by InterfaceManager).
+    REMOTE_DATA_RX gamepad_data;
+
+    explicit Gamepad() : InputInterface() {
+      type_ = InputType::GAMEPAD;
+    }
+
+    /*
+     * Gamepad Button Mappings:
+     * 
+     * Common (both modes):
+     * - A Button      - Play motion
+     * - X/Y Buttons   - Reinitialize base quaternion and delta heading
+     * - Start Button  - Start control
+     * - Select Button - Emergency Stop (kills all motion)
+     * - F1 Button - Toggle planner on/off
+     * - D-pad L/R     - Delta heading left/right (+/-0.1 rad)
+     * 
+     * Non-Planner Mode:
+     * - B Button      - Restart current motion and pause (emergency stop)
+     * - L1/R1 Buttons - Previous/Next motion
+     * 
+     * Planner Mode:
+     * - B Button      - Emergency stop
+     * - L1/R1 Buttons - Change movement mode (0-4: idle, slow walk, walk, run, boxing)
+     * - L2/R2 Buttons - Decrease/Increase movement speed
+     * - Left Stick    - Movement direction (lx, ly)
+     * - Right Stick   - Facing direction (rx, ry)
+     */
+
+    // Flag to trigger safety reset in handle_input
+    bool trigger_safety_reset = false;
+
+    // Override the update function from InputInterface
+    void update() override {
+      // Check for safety reset trigger from manager
+      if (CheckAndClearSafetyReset()) {
+        use_planner = false;
+        trigger_safety_reset = true;
+        std::cout << "[Gamepad] Safety reset triggered: will disable planner and return to reference motion" << std::endl;
+      }
+
+      // Reset input flags each frame
+      start_control = false;
+      stop_control = false;
+      motion_prev = false;
+      motion_next = false;
+      play_motion = false;
+      motion_restart = false;
+      delta_left = false;
+      delta_right = false;
+      reinitialize = false;
+      planner_emergency_stop = false;
+
+      // Process gamepad input and set flags based on current button states
+      update_gamepad_data(gamepad_data.RF_RX);
+
+      // Debug: Log analog stick values if they're above dead zone
+      if constexpr (DEBUG_LOGGING) {
+        if (std::abs(lx) > dead_zone || std::abs(ly) > dead_zone) {
+          std::cout << "[GAMEPAD DEBUG] Left stick: lx=" << lx << ", ly=" << ly << std::endl;
+        }
+        if (std::abs(rx) > dead_zone || std::abs(ry) > dead_zone) {
+          std::cout << "[GAMEPAD DEBUG] Right stick: rx=" << rx << ", ry=" << ry << std::endl;
+        }
+      }
+
+      // Button mappings:
+
+      if (!use_planner) {
+        // Y - Reinitialize
+        if (Y.on_press || X.on_press) { 
+          reinitialize = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] X/Y pressed - Reinitialize" << std::endl;
+          }
+        }
+        // B - Reset motion
+        if (B.on_press) { 
+          motion_restart = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] B pressed - Motion restart" << std::endl;
+          }
+        }
+        // A - Play/Pause motion
+        if (A.on_press) { 
+          play_motion = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] A pressed - Play/Pause motion" << std::endl;
+          }
+        }
+        // L1/R1 -Motion prev/next
+        if (L1.on_press) { 
+          motion_prev = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] L1 pressed - Previous motion" << std::endl;
+          }
+        }
+        if (R1.on_press) { 
+          motion_next = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] R1 pressed - Next motion" << std::endl;
+          }
+        }
+      }
+      else {
+        // Y - Reinitialize
+        if (Y.on_press || X.on_press) { 
+          reinitialize = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] X/Y pressed - Reinitialize" << std::endl;
+          }
+        }
+        // A - Play/Pause motion
+        if (A.on_press) { 
+          play_motion = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] A pressed - Play/Pause motion" << std::endl;
+          }
+        }
+        // B - Reset motion
+        if (B.on_press) { 
+          planner_emergency_stop = true; 
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] B pressed - Planner emergency stop" << std::endl;
+          }
+        }
+        // L1/R1 - change movement mode
+        if (L1.on_press) { 
+          planner_use_movement_mode -= 1; 
+          if (planner_use_movement_mode < 0) { planner_use_movement_mode = 6; }
+          // If movement mode is 6, i.e. switch from idle to kneel, set height to 0.8
+          if (planner_use_movement_mode == 6) { planner_use_height = 0.8;}
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] L1 pressed - Movement mode changed to: " << planner_use_movement_mode << std::endl;
+          }
+        }
+        if (R1.on_press) { 
+          planner_use_movement_mode += 1; 
+          if (planner_use_movement_mode > 6) { planner_use_movement_mode = 0; }
+          // If movement mode is 4, i.e. switch from run to squat, set height to 0.8
+          if (planner_use_movement_mode == 4) { planner_use_height = 0.8;}
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GAMEPAD DEBUG] R1 pressed - Movement mode changed to: " << planner_use_movement_mode << std::endl;
+          }
+        }
+        // L2/R2 - change movement speed
+        if (R2.pressed) { 
+          if (planner_use_movement_mode < 4) {
+            planner_use_movement_speed += 0.02;
+            if constexpr (DEBUG_LOGGING) {
+              if (R2.pressed) std::cout << "[GAMEPAD DEBUG] R2 pressed - Speed increasing" << "Current speed: " << planner_use_movement_speed << std::endl;
+            }
+          } else {
+            planner_use_height += 0.02;
+            if constexpr (DEBUG_LOGGING) {
+              if (R2.pressed) std::cout << "[GAMEPAD DEBUG] R2 pressed - Height increasing" << "Current height: " << planner_use_height << std::endl;
+            }
+          }
+
+        }
+        if (L2.pressed) { 
+          if (planner_use_movement_mode < 4) {
+            planner_use_movement_speed -= 0.02;
+            if constexpr (DEBUG_LOGGING) {
+              if (L2.pressed) std::cout << "[GAMEPAD DEBUG] L2 pressed - Speed decreasing" << "Current speed: " << planner_use_movement_speed << std::endl;
+            }
+          } else {
+            planner_use_height -= 0.02;
+            if constexpr (DEBUG_LOGGING) {
+              if (L2.pressed) std::cout << "[GAMEPAD DEBUG] L2 pressed - Height decreasing" << "Current height: " << planner_use_height << std::endl;
+            }
+          }
+
+        }
+        // Limit movement speed and height to the range of the movement mode
+        switch (planner_use_movement_mode) {
+          case 0:
+            planner_use_movement_speed = -1.0;
+            planner_use_height = -1.0;
+            break;
+          case 1: // slow walk: 0.1 - 0.8
+            planner_use_movement_speed = std::max(planner_use_movement_speed, 0.2);
+            planner_use_movement_speed = std::min(planner_use_movement_speed, 0.8);
+            planner_use_height = -1.0;
+            break;
+          case 2: // walk: 0.8 - 2.5
+            planner_use_movement_speed = std::max(planner_use_movement_speed, 0.8);
+            planner_use_movement_speed = std::min(planner_use_movement_speed, 1.5);
+            planner_use_height = -1.0;
+            break;
+          case 3: // run: 2.5 - 7.5
+            planner_use_movement_speed = std::max(planner_use_movement_speed, 1.5);
+            planner_use_movement_speed = std::min(planner_use_movement_speed, 3.0);
+            planner_use_height = -1.0;
+            break;
+          case 4: // squat: 
+            planner_use_movement_speed = -1.0;
+            planner_use_height = std::max(planner_use_height, 0.1);
+            planner_use_height = std::min(planner_use_height, 0.8);
+            break;
+          case 5: // kneel two legs: 
+            planner_use_movement_speed = -1.0;
+            planner_use_height = std::max(planner_use_height, 0.1);
+            planner_use_height = std::min(planner_use_height, 0.8);
+            break;
+          case 6: // kneel:
+            planner_use_movement_speed = -1.0;
+            planner_use_height = std::max(planner_use_height, 0.1);
+            planner_use_height = std::min(planner_use_height, 0.8);
+            break;
+        }
+        // Analog sticks - change movement and facing direction (with dead zone)
+        
+        if (std::abs(rx) > dead_zone || std::abs(ry) > dead_zone) {
+            planner_facing_angle = planner_facing_angle - 0.02 * rx;
+            if constexpr (DEBUG_LOGGING) {
+              {
+                std::cout << "[GAMEPAD DEBUG] Right stick - Facing angle: " << planner_facing_angle << " rad ("
+                          << (planner_facing_angle * 180.0 / M_PI) << " deg)" << std::endl;
+              }
+            }
+        }
+        
+        if (std::abs(lx) > dead_zone || std::abs(ly) > dead_zone) {
+            planner_moving_direction = atan2(ly, lx) - M_PI/2 + planner_facing_angle;
+            if constexpr (DEBUG_LOGGING) {
+              {
+                std::cout << "[GAMEPAD DEBUG] Left stick - Moving direction: " << planner_moving_direction << " rad ("
+                          << (planner_moving_direction * 180.0 / M_PI) << " deg)" << std::endl;
+              }
+            }
+        }
+        
+        // Log speed changes after limit processing
+        if constexpr (DEBUG_LOGGING) {
+          static double prev_speed = planner_use_movement_speed;
+          if (std::abs(planner_use_movement_speed - prev_speed) > 0.01) {
+            std::cout << "[GAMEPAD DEBUG] Movement speed: " << planner_use_movement_speed << std::endl;
+            prev_speed = planner_use_movement_speed;
+          }
+        }
+      }
+
+
+      // F1 - Toggle planner
+      if (F1.on_press) { 
+        use_planner = !use_planner; 
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GAMEPAD DEBUG] F1 pressed - Planner toggled to: " << (use_planner ? "ON" : "OFF") << std::endl;
+        }
+      }
+
+      // start - Start control
+      if (start.on_press) { 
+        start_control = true; 
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GAMEPAD DEBUG] Start pressed - Start control" << std::endl;
+        }
+      }
+      // select - Emergency stop
+      if (select.on_press) { 
+        stop_control = true; 
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GAMEPAD DEBUG] Select pressed - Emergency Stop" << std::endl;
+        }
+      }       
+      // left/right - Delta heading left/right
+      if (left.on_press) { 
+        delta_left = true; 
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GAMEPAD DEBUG] D-pad Left pressed - Delta heading left" << std::endl;
+        }
+      }
+      if (right.on_press) { 
+        delta_right = true; 
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GAMEPAD DEBUG] D-pad Right pressed - Delta heading right" << std::endl;
+        }
+      }
+    }
+
+    /**
+     * @brief Decode raw joystick packet into smoothed analog values and Button states.
+     *
+     * Analog values are smoothed with an exponential moving average (controlled
+     * by `smooth`) and dead-zone filtered.  Each Button is updated with edge
+     * detection so callers can distinguish press / release / held.
+     */
+    void update_gamepad_data(xRockerBtnDataStruct& key_data) {
+      lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
+      rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
+      ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
+      l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
+      ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
+
+      R1.update(key_data.btn.components.R1);
+      L1.update(key_data.btn.components.L1);
+      start.update(key_data.btn.components.start);
+      select.update(key_data.btn.components.select);
+      R2.update(key_data.btn.components.R2);
+      L2.update(key_data.btn.components.L2);
+      F1.update(key_data.btn.components.F1);
+      F2.update(key_data.btn.components.F2);
+      A.update(key_data.btn.components.A);
+      B.update(key_data.btn.components.B);
+      X.update(key_data.btn.components.X);
+      Y.update(key_data.btn.components.Y);
+      up.update(key_data.btn.components.up);
+      right.update(key_data.btn.components.right);
+      down.update(key_data.btn.components.down);
+      left.update(key_data.btn.components.left);
+    }
+
+    // Override the handle_input function from InputInterface
+    // This processes the gamepad input flags and performs actions using the provided parameters
+    void handle_input(MotionDataReader& motion_reader,
+                     std::shared_ptr<const MotionSequence>& current_motion,
+                     int& current_frame,
+                     OperatorState& operator_state,
+                     bool& reinitialize_heading,
+                     DataBuffer<HeadingState>& heading_state_buffer,
+                     bool has_planner,
+                     PlannerState& planner_state,
+                     DataBuffer<MovementState>& movement_state_buffer,
+                     std::mutex& current_motion_mutex,
+                     bool& report_temperature) override {
+      
+
+      // Handle safety reset from interface manager
+      if (trigger_safety_reset) {
+        trigger_safety_reset = false;
+        movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+          reinitialize_heading = true;
+          auto temp_motion = std::make_shared<MotionSequence>(*current_motion);
+          temp_motion->name = "temporary_motion";
+          current_motion = temp_motion;
+          if (has_planner && planner_state.enabled) {
+            planner_state.enabled = false;
+            planner_state.initialized = false;
+            std::cout << "Safety reset: Planner disabled" << std::endl;
+          }
+        }
+        std::cout << "Safety reset: Returned to reference motion at frame 0" << std::endl;
+      }
+
+      // Handle motion control commands
+      if (this->motion_prev && !motion_reader.motions.empty()) {
+          motion_reader.current_motion_index_ =
+              (motion_reader.current_motion_index_ - 1 + motion_reader.motions.size()) % motion_reader.motions.size();
+          std::string motion_name;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+            current_frame = 0;
+            motion_name = current_motion->name;
+            reinitialize_heading = true;
+          }
+          std::cout << "Switched to motion " << motion_reader.current_motion_index_ << ": " << motion_name
+                    << " (paused at frame 0)" << std::endl;
+      }
+
+      if (this->motion_next && !motion_reader.motions.empty()) {
+          motion_reader.current_motion_index_ = (motion_reader.current_motion_index_ + 1) % motion_reader.motions.size();
+          std::string motion_name;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+            current_frame = 0;
+            motion_name = current_motion->name;
+            reinitialize_heading = true;
+          }
+          std::cout << "Switched to motion " << motion_reader.current_motion_index_ << ": " << motion_name
+                    << " (paused at frame 0)" << std::endl;
+      }
+
+      if (this->play_motion) {
+          if (!operator_state.play) {
+              int frame_copy;
+              size_t timesteps_copy;
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = true;
+                frame_copy = current_frame;
+                timesteps_copy = current_motion ? current_motion->timesteps : 0;
+              }
+              std::cout << "Playing motion " << motion_reader.current_motion_index_ << " from frame " << frame_copy << " to end ("
+                        << timesteps_copy << " total frames)" << std::endl;
+          }
+      }
+
+      if (this->motion_restart) {
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_frame = 0;
+            reinitialize_heading = true;
+          }
+          std::cout << "Reset motion " << motion_reader.current_motion_index_ << " to frame 0 (paused)" << std::endl;
+      }
+
+      if (this->stop_control) { operator_state.stop = true; }
+
+      if (this->start_control) { operator_state.start = true; }
+
+      // Handle delta heading controls
+      if (this->delta_left) {
+          auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+          HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+          double new_delta = current_state.delta_heading + 0.1;
+          heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+          std::cout << "Delta heading left: " << new_delta << " rad" << std::endl;
+      }
+
+      if (this->delta_right) {
+          auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+          HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+          double new_delta = current_state.delta_heading - 0.1;
+          heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+          std::cout << "Delta heading right: " << new_delta << " rad" << std::endl;
+      }
+
+      // Handle reinitialize command
+      if (this->reinitialize) {
+        std::lock_guard<std::mutex> lock(current_motion_mutex); 
+        reinitialize_heading = true;
+      }
+
+      // Handle planner control - copy the toggle state from gamepad
+      if (this->use_planner && !has_planner) {
+          std::cout << "Planner not loaded - cannot enable" << std::endl;
+          this->use_planner = false;
+          movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+            current_frame = 0;
+          }
+      } else if (has_planner && planner_state.enabled != this->use_planner) {
+        planner_state.enabled = this->use_planner;
+        if (planner_state.enabled) {
+          std::cout << "Planner enabled" << std::endl;
+          planner_facing_angle = 0.0;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+          }
+          // Wait for planner to be initialized with timeout (5 seconds)
+          auto wait_start = std::chrono::steady_clock::now();
+          constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+          while (planner_state.enabled) {
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              if (current_motion->name == "planner_motion") {
+                std::cout << "[Gamepad] motion name is planner_motion" << std::endl;
+                break;
+              }
+            }
+              std::this_thread::sleep_for(std::chrono::milliseconds(100));
+              auto elapsed = std::chrono::steady_clock::now() - wait_start;
+              if (elapsed > PLANNER_INIT_TIMEOUT) {
+                  std::cerr << "[Gamepad] Planner initialization timeout after 5 seconds" << std::endl;
+                  this->use_planner = false;
+                  movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+                  {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    operator_state.play = false;
+                    current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+                    current_frame = 0;
+                  }
+                  break;
+              }
+              std::cout << "[Gamepad] Waiting for planner to be initialized" << std::endl;
+          }
+          // Check if planner is enabled and initialized
+          if (!planner_state.enabled || !planner_state.initialized) {
+              std::cout << "[Gamepad] Planner failed to initialize." << std::endl;
+              this->use_planner = false;
+              movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+                current_frame = 0;
+              }
+          } else {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = true;
+          }
+        } else {
+            std::cout << "Planner disabled" << std::endl;
+            planner_state.initialized = false; // Reset planner initialization when disabled
+            movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              operator_state.play = false;
+              reinitialize_heading = true;
+              current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+              current_frame = 0;
+            }
+        }
+      }
+
+      if (has_planner && planner_state.enabled && planner_state.initialized) {
+          
+          // Set final movement values based on momentum
+          int final_mode = this->planner_use_movement_mode;
+          std::array<double, 3> final_movement = {double(cos(planner_moving_direction)), double(sin(planner_moving_direction)), 0.0};
+          std::array<double, 3> final_facing_direction = {double(cos(planner_facing_angle)), double(sin(planner_facing_angle)), 0.0};
+          double final_speed = this->planner_use_movement_speed;
+          double final_height = this->planner_use_height;
+
+          // If left sticks are in the dead zone, idle mode
+          if (std::abs(lx) < dead_zone && std::abs(ly) < dead_zone) {
+            if constexpr (DEBUG_LOGGING) {
+              std::cout << "Both left sticks in the dead zone - Idle mode" << std::endl;
+              std::cout << "[GAMEPAD DEBUG] Left stick: lx=" << lx << ", ly=" << ly << std::endl;
+            }
+            if (is_standing_motion_mode(LocomotionMode(final_mode))) {
+              final_mode = static_cast<int>(LocomotionMode::IDLE);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = -1.0f;
+              final_height = -1.0f;
+            } else {
+              final_mode = final_mode;
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = 0;
+              final_height = final_height;
+            }
+          }
+
+          // Handle emergency stop (immediate momentum reset)
+          if (this->planner_emergency_stop) {
+              planner_use_movement_mode = static_cast<int>(LocomotionMode::IDLE);
+              final_mode = static_cast<int>(LocomotionMode::IDLE);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = -1.0f;
+              final_height = -1.0f;
+              if constexpr (DEBUG_LOGGING) {
+                std::cout << "Emergency Stop! Movement momentum reset." << std::endl;
+              }
+          }
+
+          // Debug: Log final computed values being sent to planner
+          if constexpr (DEBUG_LOGGING) {
+            static int debug_counter = 0;
+            debug_counter++;
+            if (debug_counter % 50 == 0) {  // Log every 50 calls to avoid spam
+              std::cout << "[GAMEPAD DEBUG] Final planner values:" << std::endl;
+              std::cout << "  Mode: " << final_mode << " (0=idle, 1=slow, 2=walk, 3=run, 4=box)" << std::endl;
+              std::cout << "  Speed: " << final_speed << std::endl;
+              std::cout << "  Height: " << final_height << std::endl;
+              std::cout << "  Movement direction: [" << final_movement[0] << ", " << final_movement[1] << ", " << final_movement[2] << "]" << std::endl;
+              std::cout << "  Facing direction: [" << final_facing_direction[0] << ", " << final_facing_direction[1] << ", " << final_facing_direction[2] << "]" << std::endl;
+            }
+          }
+
+          // Update thread-safe buffer (single source of truth for planner thread)
+          MovementState mode_state(final_mode, final_movement, final_facing_direction, final_speed, final_height);
+          movement_state_buffer.SetData(mode_state);
+      }
+
+    }
+
+    // ------------------------------------------------------------------
+    // Smoothed analog-stick values (exponential moving average)
+    // ------------------------------------------------------------------
+    float lx = 0.f;   ///< Left stick horizontal  (smoothed, dead-zone filtered)
+    float rx = 0.f;   ///< Right stick horizontal  (smoothed)
+    float ry = 0.f;   ///< Right stick vertical    (smoothed)
+    float l2 = 0.f;   ///< Left trigger analog     (smoothed)
+    float ly = 0.f;   ///< Left stick vertical     (smoothed)
+
+    float smooth = 0.3f;       ///< EMA smoothing factor (0 = no update, 1 = no smoothing).
+    float dead_zone = 0.05f;   ///< Analog values below this are zeroed.
+
+    // ------------------------------------------------------------------
+    // Edge-detecting button states
+    // ------------------------------------------------------------------
+    Button R1;      ///< Right bumper
+    Button L1;      ///< Left bumper
+    Button start;   ///< Start button
+    Button select;  ///< Select / back button
+    Button R2;      ///< Right trigger (digital)
+    Button L2;      ///< Left trigger (digital)
+    Button F1;      ///< Function 1 (planner toggle)
+    Button F2;      ///< Function 2
+    Button A;       ///< A face button (play motion / play in planner)
+    Button B;       ///< B face button (restart / emergency stop in planner)
+    Button X;       ///< X face button (reinitialize)
+    Button Y;       ///< Y face button (reinitialize)
+    Button up;      ///< D-pad up
+    Button right;   ///< D-pad right (delta heading)
+    Button down;    ///< D-pad down
+    Button left;    ///< D-pad left  (delta heading)
+};
+} // namespace unitree::common
+
+#endif

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -0,0 +1,1078 @@
+/**
+ * @file gamepad_manager.hpp
+ * @brief Planner-centric gamepad manager with ZMQ delegate and D-pad motion-set selection.
+ *
+ * GamepadManager is a specialised InputInterface that:
+ *  - Reads the Unitree wireless gamepad **directly** (no nested Gamepad class).
+ *  - Operates **exclusively in planner mode** – the planner must always be loaded.
+ *  - Can delegate to ZMQEndpointInterface via F1 button toggle.
+ *  - D-pad selects one of 4 motion sets; face buttons cycle within the set.
+ *  - Boxing set uses direct key selection instead of cycling.
+ *
+ * ## Gamepad Button Mapping (Planner Mode)
+ *
+ *   Button  | Action
+ *   --------|-------
+ *   Start   | Start control (enable planner, wait for init, auto-play)
+ *   Select  | Emergency stop
+ *   A       | Emergency stop (immediate halt)
+ *   D-Up    | Select Standing motion set (SLOW_WALK default)
+ *   D-Down  | Select Squat/Crawl motion set (IDEL_SQUAT default)
+ *   D-Left  | Select Boxing motion set (IDEL_BOXING default)
+ *   D-Right | Select Styled Walking motion set (LEDGE_WALKING default)
+ *   F1      | Toggle ZMQ streaming
+ *
+ *   --- Standing / Squat / Styled sets (loop cycling) ---
+ *   X       | Next mode in current set (wraps)
+ *   Y       | Previous mode in current set (wraps)
+ *   B       | Reset to set's default mode
+ *   L1/R1   | Facing angle ±π/4
+ *   L2/R2   | Height ±0.1 (Squat set only; disabled otherwise)
+ *
+ *   --- Boxing set (direct selection) ---
+ *   X       | WALK_BOXING
+ *   Y       | RANDOM_PUNCH
+ *   B       | IDEL_BOXING (reset)
+ *   L1      | LEFT_PUNCH
+ *   R1      | RIGHT_PUNCH
+ *   L2      | LEFT_HOOK
+ *   R2      | RIGHT_HOOK
+ *
+ *   L stick | Movement direction (binned to nearest 30° increment)
+ *   R stick | Facing direction (continuous)
+ */
+
+#ifndef GAMEPAD_MANAGER_HPP
+#define GAMEPAD_MANAGER_HPP
+
+#include <memory>
+#include <vector>
+#include <iostream>
+#include <cstring>
+#include <cmath>
+#include <array>
+#include <thread>
+#include <chrono>
+
+#include "input_interface.hpp"
+#include "zmq_endpoint_interface.hpp"
+#include "gamepad.hpp"
+#include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @class GamepadManager
+ * @brief Planner-only gamepad controller with ZMQ delegate and D-pad motion-set selection.
+ *
+ * When in GAMEPAD mode, button presses and stick positions are translated
+ * directly into MovementState commands for the locomotion planner.
+ * D-pad selects one of 4 motion sets; face buttons cycle within the set
+ * (except Boxing which uses direct key selection).
+ * When in ZMQ mode (toggled via F1), all calls are forwarded to ZMQ.
+ */
+class GamepadManager : public InputInterface {
+  public:
+    // ========================================
+    // DEBUG CONTROL FLAG
+    // ========================================
+    static constexpr bool DEBUG_LOGGING = true;
+
+    enum class ManagedType {
+      GAMEPAD = 0,
+      ZMQ = 1
+    };
+
+    GamepadManager(
+      const std::string& zmq_host,
+      int zmq_port,
+      const std::string& zmq_topic,
+      bool zmq_conflate,
+      bool zmq_verbose
+    ) : InputInterface(), zmq_host_(zmq_host), zmq_port_(zmq_port), zmq_topic_(zmq_topic),
+        zmq_conflate_(zmq_conflate), zmq_verbose_(zmq_verbose) {
+      type_ = InputType::GAMEPAD;  // Default to gamepad mode
+      buildInterfaces();
+      active_ = ManagedType::GAMEPAD;
+      current_ = nullptr;  // Gamepad mode doesn't use delegate
+
+      // Initialize with Standing motion set, SLOW_WALK as default
+      motion_set_index_ = 0;
+      current_motion_set_ = get_motion_set(motion_set_index_);
+      mode_index_in_set_ = 0;
+      applyModeFromSet();
+    }
+
+    void update() override {
+      // Reset per-frame flags
+      emergency_stop_ = false;
+      report_temperature_flag_ = false;
+      start_control_ = false;
+      stop_control_ = false;
+      reinitialize_ = false;
+      planner_emergency_stop_ = false;
+
+      // Handle stdin shortcuts for switching and emergency stop
+      char ch;
+      while (ReadStdinChar(ch)) {
+        bool is_manager_key = false;
+        switch (ch) {
+          case '@':
+            SetActiveInterface(ManagedType::GAMEPAD);
+            is_manager_key = true;
+            break;
+          case '#':
+            SetActiveInterface(ManagedType::ZMQ);
+            is_manager_key = true;
+            break;
+          case 'o':
+          case 'O':
+            emergency_stop_ = true;
+            is_manager_key = true;
+            std::cout << "[GamepadManager] EMERGENCY STOP triggered (O/o key pressed)" << std::endl;
+            break;
+          case 'f':
+          case 'F':
+            report_temperature_flag_ = true;
+            is_manager_key = true;
+            break;
+        }
+
+        if (!is_manager_key && current_) {
+          current_->PushStdinChar(ch);
+        }
+      }
+
+      // Update gamepad data and buttons
+      update_gamepad_data(gamepad_data_.RF_RX);
+
+      // F1 - Toggle ZMQ streaming
+      bool trigger_ZMQ_toggle = false;
+      if (F1_.on_press) {
+        if (active_ != ManagedType::ZMQ) {
+          SetActiveInterface(ManagedType::ZMQ);
+        }
+        trigger_ZMQ_toggle = true;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] F1 pressed - ZMQ toggle" << std::endl;
+        }
+      }
+
+      // D-pad - Motion set selection (switches back to GAMEPAD if in ZMQ)
+      if (up_.on_press) {
+        if (active_ != ManagedType::GAMEPAD) { SetActiveInterface(ManagedType::GAMEPAD); }
+        selectMotionSet(0);  // Standing
+      }
+      if (down_.on_press) {
+        if (active_ != ManagedType::GAMEPAD) { SetActiveInterface(ManagedType::GAMEPAD); }
+        selectMotionSet(1);  // Squat/Crawl
+      }
+      if (left_.on_press) {
+        if (active_ != ManagedType::GAMEPAD) { SetActiveInterface(ManagedType::GAMEPAD); }
+        selectMotionSet(2);  // Boxing
+      }
+      if (right_.on_press) {
+        if (active_ != ManagedType::GAMEPAD) { SetActiveInterface(ManagedType::GAMEPAD); }
+        selectMotionSet(3);  // Styled Walking
+      }
+
+      // Select - Emergency Stop
+      if (select_.on_press) {
+        stop_control_ = true;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] Select pressed - Emergency Stop" << std::endl;
+        }
+      }
+
+      // If not in gamepad mode, update the active interface
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        current_->update();
+        if (trigger_ZMQ_toggle && zmq_) {
+            zmq_->TriggerZMQToggle();
+        }
+      } else {
+        processGamepadPlannerControls();
+      }
+    }
+
+    void handle_input(MotionDataReader& motion_reader,
+                      std::shared_ptr<const MotionSequence>& current_motion,
+                      int& current_frame,
+                      OperatorState& operator_state,
+                      bool& reinitialize_heading,
+                      DataBuffer<HeadingState>& heading_state_buffer,
+                      bool has_planner,
+                      PlannerState& planner_state,
+                      DataBuffer<MovementState>& movement_state_buffer,
+                      std::mutex& current_motion_mutex,
+                      bool& report_temperature) override {
+      // Check if planner is loaded (required for GamepadManager)
+      if (!has_planner) {
+        std::cerr << "[GamepadManager ERROR] Planner not loaded - GamepadManager requires planner. Stopping control." << std::endl;
+        operator_state.stop = true;
+        return;
+      }
+
+      // NOTE: Encoder mode safety check removed - encoder mode is now a property of the motion
+      // If a motion has an incompatible encoder mode for gamepad mode, the user should
+      // switch to a different motion with a compatible encoder mode
+
+      // Global emergency stop
+      if (emergency_stop_) {
+        operator_state.stop = true;
+      }
+
+      // Global temperature report (F key)
+      if (report_temperature_flag_) {
+        report_temperature = true;
+        report_temperature_flag_ = false;
+      }
+
+      // Handle stop control
+      if (stop_control_) { operator_state.stop = true; }
+
+      // If in gamepad mode, handle planner-only controls
+      if (active_ == ManagedType::GAMEPAD) {
+        handleGamepadPlannerInput(motion_reader, current_motion, current_frame,
+                                  operator_state, reinitialize_heading, heading_state_buffer,
+                                  planner_state, movement_state_buffer, current_motion_mutex);
+      } else {
+        // Delegate to ZMQ
+        if (current_) {
+          current_->handle_input(motion_reader, current_motion, current_frame, operator_state,
+                                reinitialize_heading, heading_state_buffer, has_planner,
+                                planner_state, movement_state_buffer, current_motion_mutex, report_temperature);
+        }
+      }
+    }
+
+    bool HasVR3PointControl() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->HasVR3PointControl();
+      }
+      return has_vr_3point_control_;
+    }
+
+    bool HasHandJoints() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->HasHandJoints();
+      }
+      return has_hand_joints_;
+    }
+
+    bool HasExternalTokenState() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->HasExternalTokenState();
+      }
+      return has_external_token_state_;
+    }
+
+    std::pair<bool, std::array<double, 9>> GetVR3PointPosition() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->GetVR3PointPosition();
+      }
+      return InputInterface::GetVR3PointPosition();
+    }
+
+    std::pair<bool, std::array<double, 12>> GetVR3PointOrientation() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->GetVR3PointOrientation();
+      }
+      return InputInterface::GetVR3PointOrientation();
+    }
+
+    std::array<double, 3> GetVR3PointCompliance() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->GetVR3PointCompliance();
+      }
+      return InputInterface::GetVR3PointCompliance();
+    }
+
+    std::pair<bool, std::array<double, 7>> GetHandPose(bool is_left) const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->GetHandPose(is_left);
+      }
+      return InputInterface::GetHandPose(is_left);
+    }
+
+    std::pair<bool, std::vector<double>> GetExternalTokenState() const override {
+      if (active_ != ManagedType::GAMEPAD && current_) {
+        return current_->GetExternalTokenState();
+      }
+      return InputInterface::GetExternalTokenState();
+    }
+
+    // Receive raw wireless remote data for gamepad
+    void UpdateGamepadRemoteData(const uint8_t* buff, size_t size) {
+      if (buff == nullptr || size == 0) { return; }
+      size_t copy_size = std::min<size_t>(size, sizeof(gamepad_data_.buff));
+      std::memcpy(gamepad_data_.buff, buff, copy_size);
+    }
+
+    void SetActiveInterface(ManagedType t) {
+      for (size_t i = 0; i < order_.size(); ++i) {
+        if (order_[i] == t) {
+          setActiveIndex(static_cast<int>(i));
+          return;
+        }
+      }
+    }
+
+    ManagedType GetActiveInterface() const { return active_; }
+
+  private:
+    void buildInterfaces() {
+      order_.push_back(ManagedType::GAMEPAD);
+
+      zmq_ = std::make_unique<ZMQEndpointInterface>(
+        zmq_host_, zmq_port_, zmq_topic_, zmq_conflate_, zmq_verbose_
+      );
+      order_.push_back(ManagedType::ZMQ);
+    }
+
+    void setActiveIndex(int idx) {
+      if (order_.empty()) { return; }
+      if (idx < 0) { idx = static_cast<int>(order_.size()) - 1; }
+      if (idx >= static_cast<int>(order_.size())) { idx = 0; }
+
+      // Trigger safety reset on all managed interfaces when switching
+      TriggerSafetyReset();  // Self (for gamepad mode)
+      if (zmq_) zmq_->TriggerSafetyReset();
+
+      active_index_ = idx;
+      active_ = order_[static_cast<size_t>(active_index_)];
+
+      switch (active_) {
+        case ManagedType::GAMEPAD:
+          current_ = nullptr;  // Gamepad mode is handled directly
+          type_ = InputType::GAMEPAD;
+          std::cout << "[GamepadManager] Switched to: GAMEPAD (safety reset triggered)" << std::endl;
+          break;
+        case ManagedType::ZMQ:
+          current_ = zmq_.get();
+          type_ = InputType::NETWORK;
+          std::cout << "[GamepadManager] Switched to: ZMQ (safety reset triggered)" << std::endl;
+          break;
+      }
+    }
+
+    // Update gamepad analog and button data
+    void update_gamepad_data(unitree::common::xRockerBtnDataStruct& key_data) {
+      lx_ = lx_ * (1 - smooth_) + (std::fabs(key_data.lx) < dead_zone_ ? 0.0f : key_data.lx) * smooth_;
+      rx_ = rx_ * (1 - smooth_) + (std::fabs(key_data.rx) < dead_zone_ ? 0.0f : key_data.rx) * smooth_;
+      ry_ = ry_ * (1 - smooth_) + (std::fabs(key_data.ry) < dead_zone_ ? 0.0f : key_data.ry) * smooth_;
+      l2_ = l2_ * (1 - smooth_) + (std::fabs(key_data.L2) < dead_zone_ ? 0.0f : key_data.L2) * smooth_;
+      ly_ = ly_ * (1 - smooth_) + (std::fabs(key_data.ly) < dead_zone_ ? 0.0f : key_data.ly) * smooth_;
+
+      R1_.update(key_data.btn.components.R1);
+      L1_.update(key_data.btn.components.L1);
+      start_.update(key_data.btn.components.start);
+      select_.update(key_data.btn.components.select);
+      R2_.update(key_data.btn.components.R2);
+      L2_.update(key_data.btn.components.L2);
+      F1_.update(key_data.btn.components.F1);
+      F2_.update(key_data.btn.components.F2);
+      A_.update(key_data.btn.components.A);
+      B_.update(key_data.btn.components.B);
+      X_.update(key_data.btn.components.X);
+      Y_.update(key_data.btn.components.Y);
+      up_.update(key_data.btn.components.up);
+      right_.update(key_data.btn.components.right);
+      down_.update(key_data.btn.components.down);
+      left_.update(key_data.btn.components.left);
+    }
+
+    // Select a motion set by index and reset to its default mode
+    void selectMotionSet(int set_index) {
+      bool was_crawling = isInCrawlingMode();
+
+      motion_set_index_ = set_index;
+      current_motion_set_ = get_motion_set(motion_set_index_);
+      mode_index_in_set_ = 0;
+      boxing_revert_time_ = {};  // Clear boxing timer on set switch
+
+      if (was_crawling) {
+        // Soft exit: crawl → kneel → target (staged)
+        LocomotionMode target = current_motion_set_[0];
+        beginExitCrawlingTransition(target);
+      } else {
+        // Clear any pending transition and apply directly
+        transition_target_mode_ = LocomotionMode::IDLE;
+        transition_final_mode_ = LocomotionMode::IDLE;
+        transition_start_time_ = {};
+        applyModeFromSet();
+      }
+
+      // Boxing set enters with IDEL_BOXING → auto-revert to WALK_BOXING after 1s
+      if (motion_set_index_ == 2) {
+        boxing_revert_time_ = std::chrono::steady_clock::now();
+      }
+      if constexpr (DEBUG_LOGGING) {
+        std::cout << "[GamepadManager DEBUG] D-pad - Motion set " << motion_set_index_
+                  << ", mode: " << planner_use_movement_mode_ << std::endl;
+      }
+    }
+
+    // Apply mode/speed/height from the current mode_index_in_set_
+    void applyModeFromSet() {
+      if (current_motion_set_.empty()) return;
+      LocomotionMode mode = current_motion_set_[mode_index_in_set_];
+      planner_use_movement_mode_ = static_cast<int>(mode);
+      applySpeedAndHeight(mode);
+    }
+
+    // Set fixed speed and height for a given mode
+    void applySpeedAndHeight(LocomotionMode mode) {
+      switch (mode) {
+        case LocomotionMode::SLOW_WALK:
+          planner_use_movement_speed_ = 0.4;
+          planner_use_height_ = -1.0;
+          break;
+        case LocomotionMode::RUN:
+          planner_use_movement_speed_ = 1.5;
+          planner_use_height_ = -1.0;
+          break;
+        case LocomotionMode::CRAWLING:
+          planner_use_movement_speed_ = 0.7;
+          planner_use_height_ = 0.4;
+          break;
+        case LocomotionMode::ELBOW_CRAWLING:
+          planner_use_movement_speed_ = 0.7;
+          planner_use_height_ = 0.3;
+          break;
+        case LocomotionMode::IDEL_SQUAT:
+        case LocomotionMode::IDEL_KNEEL_TWO_LEGS:
+        case LocomotionMode::IDEL_KNEEL:
+          planner_use_movement_speed_ = -1.0;
+          planner_use_height_ = 0.4;
+          break;
+        case LocomotionMode::IDEL_BOXING:
+        case LocomotionMode::WALK_BOXING:
+        case LocomotionMode::LEFT_PUNCH:
+        case LocomotionMode::RIGHT_PUNCH:
+        case LocomotionMode::RANDOM_PUNCH:
+        case LocomotionMode::LEFT_HOOK:
+        case LocomotionMode::RIGHT_HOOK:
+          planner_use_movement_speed_ = 0.7;
+          planner_use_height_ = -1.0;
+          break;
+        default:
+          // WALK, styled walking, and other modes use defaults
+          planner_use_movement_speed_ = -1.0;
+          planner_use_height_ = -1.0;
+          break;
+      }
+    }
+
+    // Begin a staged crawling transition to target_mode, skipping completed stages
+    void beginCrawlingTransition(LocomotionMode target_mode) {
+      LocomotionMode current = static_cast<LocomotionMode>(planner_use_movement_mode_);
+
+      if (target_mode == LocomotionMode::CRAWLING) {
+        if (current == LocomotionMode::CRAWLING) return; // Already there
+        if (current == LocomotionMode::IDEL_KNEEL_TWO_LEGS ||
+            current == LocomotionMode::IDEL_KNEEL) {
+          // Already kneeling, transition directly to crawling after delay
+          transition_target_mode_ = LocomotionMode::CRAWLING;
+          transition_start_time_ = std::chrono::steady_clock::now();
+        } else {
+          // Need to kneel first
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+          applySpeedAndHeight(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+          transition_target_mode_ = LocomotionMode::CRAWLING;
+          transition_start_time_ = std::chrono::steady_clock::now();
+        }
+      } else if (target_mode == LocomotionMode::ELBOW_CRAWLING) {
+        if (current == LocomotionMode::ELBOW_CRAWLING) return; // Already there
+        if (current == LocomotionMode::CRAWLING) {
+          // Already crawling, transition to elbow crawling after delay
+          transition_target_mode_ = LocomotionMode::ELBOW_CRAWLING;
+          transition_start_time_ = std::chrono::steady_clock::now();
+        } else if (current == LocomotionMode::IDEL_KNEEL_TWO_LEGS ||
+                   current == LocomotionMode::IDEL_KNEEL) {
+          // Already kneeling, go to crawling first then elbow
+          transition_target_mode_ = LocomotionMode::CRAWLING;
+          transition_final_mode_ = LocomotionMode::ELBOW_CRAWLING;
+          transition_start_time_ = std::chrono::steady_clock::now();
+        } else {
+          // Need to kneel first, then crawl, then elbow
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+          applySpeedAndHeight(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+          transition_target_mode_ = LocomotionMode::CRAWLING;
+          transition_final_mode_ = LocomotionMode::ELBOW_CRAWLING;
+          transition_start_time_ = std::chrono::steady_clock::now();
+        }
+      }
+      if constexpr (DEBUG_LOGGING) {
+        std::cout << "[GamepadManager DEBUG] Crawling transition started -> target: "
+                  << static_cast<int>(target_mode) << std::endl;
+      }
+    }
+
+    // Begin a staged exit from crawling/elbow crawling to target_mode (reverse of enter)
+    // ELBOW_CRAWLING → (immediate CRAWLING) → (2s) KNEEL → (2s) target
+    // CRAWLING → (immediate KNEEL) → (2s) target
+    void beginExitCrawlingTransition(LocomotionMode target_mode) {
+      LocomotionMode current = static_cast<LocomotionMode>(planner_use_movement_mode_);
+
+      if (current == LocomotionMode::ELBOW_CRAWLING) {
+        // Step down to CRAWLING immediately, then KNEEL after 2s, then target after 2s
+        planner_use_movement_mode_ = static_cast<int>(LocomotionMode::CRAWLING);
+        applySpeedAndHeight(LocomotionMode::CRAWLING);
+        transition_target_mode_ = LocomotionMode::IDEL_KNEEL_TWO_LEGS;
+        transition_final_mode_ = target_mode;
+        transition_start_time_ = std::chrono::steady_clock::now();
+      } else if (current == LocomotionMode::CRAWLING) {
+        // Step down to KNEEL immediately, then target after 2s
+        planner_use_movement_mode_ = static_cast<int>(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+        applySpeedAndHeight(LocomotionMode::IDEL_KNEEL_TWO_LEGS);
+        transition_target_mode_ = target_mode;
+        transition_start_time_ = std::chrono::steady_clock::now();
+      }
+
+      if constexpr (DEBUG_LOGGING) {
+        std::cout << "[GamepadManager DEBUG] Exit crawling transition -> target: "
+                  << static_cast<int>(target_mode) << std::endl;
+      }
+    }
+
+    // Check if current mode is crawling or elbow crawling
+    bool isInCrawlingMode() const {
+      return planner_use_movement_mode_ == static_cast<int>(LocomotionMode::CRAWLING) ||
+             planner_use_movement_mode_ == static_cast<int>(LocomotionMode::ELBOW_CRAWLING);
+    }
+
+    // Process timed crawling transitions
+    void processCrawlingTransitions() {
+      if (transition_start_time_.time_since_epoch().count() == 0) return;
+
+      auto elapsed = std::chrono::steady_clock::now() - transition_start_time_;
+      if (elapsed < std::chrono::seconds(2)) return;
+
+      // Time to advance to the next stage
+      LocomotionMode next = transition_target_mode_;
+      planner_use_movement_mode_ = static_cast<int>(next);
+      applySpeedAndHeight(next);
+
+      if constexpr (DEBUG_LOGGING) {
+        std::cout << "[GamepadManager DEBUG] Transition -> " << static_cast<int>(next) << std::endl;
+      }
+
+      // Check if there's a further stage (elbow crawling after crawling)
+      if (transition_final_mode_ != LocomotionMode::IDLE && next != transition_final_mode_) {
+        transition_target_mode_ = transition_final_mode_;
+        transition_final_mode_ = LocomotionMode::IDLE;
+        transition_start_time_ = std::chrono::steady_clock::now();
+      } else {
+        // Done
+        transition_target_mode_ = LocomotionMode::IDLE;
+        transition_final_mode_ = LocomotionMode::IDLE;
+        transition_start_time_ = {};
+      }
+    }
+
+    // Process gamepad inputs for planner controls (called from update())
+    void processGamepadPlannerControls() {
+      // Start button
+      if (start_.on_press) {
+        start_control_ = true;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] Start pressed" << std::endl;
+        }
+      }
+
+      // A - Emergency Stop (always available)
+      if (A_.on_press) {
+        planner_emergency_stop_ = true;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] A pressed - Emergency Stop" << std::endl;
+        }
+      }
+
+      // ---- Boxing set: direct key selection (no cycling) ----
+      if (motion_set_index_ == 2) {
+        if (X_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::WALK_BOXING);
+          applySpeedAndHeight(LocomotionMode::WALK_BOXING);
+          boxing_revert_time_ = {};  // WALK_BOXING is the default, no revert needed
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] X - WALK_BOXING" << std::endl; }
+        }
+        if (Y_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::RANDOM_PUNCH);
+          applySpeedAndHeight(LocomotionMode::RANDOM_PUNCH);
+          boxing_revert_time_ = {};  // RANDOM_PUNCH stays until user changes
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] Y - RANDOM_PUNCH" << std::endl; }
+        }
+        // B - IDEL_BOXING (auto-reverts to WALK_BOXING after 1s)
+        if (B_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::IDEL_BOXING);
+          applySpeedAndHeight(LocomotionMode::IDEL_BOXING);
+          boxing_revert_time_ = std::chrono::steady_clock::now();
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] B - IDEL_BOXING (auto-revert 1s)" << std::endl; }
+        }
+        // L1/R1/L2/R2 - Punches/hooks (auto-revert to WALK_BOXING after 1s)
+        if (L1_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::LEFT_PUNCH);
+          applySpeedAndHeight(LocomotionMode::LEFT_PUNCH);
+          boxing_revert_time_ = std::chrono::steady_clock::now();
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] L1 - LEFT_PUNCH (auto-revert 1s)" << std::endl; }
+        }
+        if (R1_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::RIGHT_PUNCH);
+          applySpeedAndHeight(LocomotionMode::RIGHT_PUNCH);
+          boxing_revert_time_ = std::chrono::steady_clock::now();
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] R1 - RIGHT_PUNCH (auto-revert 1s)" << std::endl; }
+        }
+        if (L2_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::LEFT_HOOK);
+          applySpeedAndHeight(LocomotionMode::LEFT_HOOK);
+          boxing_revert_time_ = std::chrono::steady_clock::now();
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] L2 - LEFT_HOOK (auto-revert 1s)" << std::endl; }
+        }
+        if (R2_.on_press) {
+          planner_use_movement_mode_ = static_cast<int>(LocomotionMode::RIGHT_HOOK);
+          applySpeedAndHeight(LocomotionMode::RIGHT_HOOK);
+          boxing_revert_time_ = std::chrono::steady_clock::now();
+          if constexpr (DEBUG_LOGGING) { std::cout << "[GamepadManager DEBUG] R2 - RIGHT_HOOK (auto-revert 1s)" << std::endl; }
+        }
+
+        // Auto-revert to WALK_BOXING after 1s if no new mode key pressed
+        if (boxing_revert_time_.time_since_epoch().count() != 0) {
+          auto elapsed = std::chrono::steady_clock::now() - boxing_revert_time_;
+          if (elapsed > std::chrono::milliseconds(500)) {
+            planner_use_movement_mode_ = static_cast<int>(LocomotionMode::WALK_BOXING);
+            applySpeedAndHeight(LocomotionMode::WALK_BOXING);
+            boxing_revert_time_ = {};
+            if constexpr (DEBUG_LOGGING) {
+              std::cout << "[GamepadManager DEBUG] Boxing auto-revert -> WALK_BOXING" << std::endl;
+            }
+          }
+        }
+      } else {
+        // ---- Standing / Squat / Styled sets: loop cycling ----
+        if (X_.on_press) {
+          // Next mode (wraps)
+          bool was_crawling = isInCrawlingMode();
+          mode_index_in_set_ = (mode_index_in_set_ + 1) % static_cast<int>(current_motion_set_.size());
+          LocomotionMode target = current_motion_set_[mode_index_in_set_];
+          if (target == LocomotionMode::CRAWLING || target == LocomotionMode::ELBOW_CRAWLING) {
+            beginCrawlingTransition(target);
+          } else if (was_crawling) {
+            beginExitCrawlingTransition(target);
+          } else {
+            transition_start_time_ = {};  // Cancel any pending transition
+            transition_target_mode_ = LocomotionMode::IDLE;
+            transition_final_mode_ = LocomotionMode::IDLE;
+            applyModeFromSet();
+          }
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] X - Next mode [" << mode_index_in_set_
+                      << "]: " << planner_use_movement_mode_ << std::endl;
+          }
+        }
+        if (Y_.on_press) {
+          // Previous mode (wraps)
+          bool was_crawling = isInCrawlingMode();
+          mode_index_in_set_ = (mode_index_in_set_ - 1 + static_cast<int>(current_motion_set_.size()))
+                               % static_cast<int>(current_motion_set_.size());
+          LocomotionMode target = current_motion_set_[mode_index_in_set_];
+          if (target == LocomotionMode::CRAWLING || target == LocomotionMode::ELBOW_CRAWLING) {
+            beginCrawlingTransition(target);
+          } else if (was_crawling) {
+            beginExitCrawlingTransition(target);
+          } else {
+            transition_start_time_ = {};
+            transition_target_mode_ = LocomotionMode::IDLE;
+            transition_final_mode_ = LocomotionMode::IDLE;
+            applyModeFromSet();
+          }
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] Y - Prev mode [" << mode_index_in_set_
+                      << "]: " << planner_use_movement_mode_ << std::endl;
+          }
+        }
+        if (B_.on_press) {
+          // Reset to set's default (index 0)
+          bool was_crawling = isInCrawlingMode();
+          mode_index_in_set_ = 0;
+          LocomotionMode target = current_motion_set_[0];
+          if (was_crawling && target != LocomotionMode::CRAWLING && target != LocomotionMode::ELBOW_CRAWLING) {
+            beginExitCrawlingTransition(target);
+          } else {
+            transition_start_time_ = {};
+            transition_target_mode_ = LocomotionMode::IDLE;
+            transition_final_mode_ = LocomotionMode::IDLE;
+            applyModeFromSet();
+          }
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] B - Reset to default mode: "
+                      << planner_use_movement_mode_ << std::endl;
+          }
+        }
+
+        // L1/R1 - Facing angle (not in boxing set)
+        if (L1_.on_press) {
+          planner_facing_angle_ += M_PI / 4;
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] L1 - Facing angle: " << planner_facing_angle_ << " rad" << std::endl;
+          }
+        }
+        if (R1_.on_press) {
+          planner_facing_angle_ -= M_PI / 4;
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] R1 - Facing angle: " << planner_facing_angle_ << " rad" << std::endl;
+          }
+        }
+
+        // L2/R2 - Height control (squat set only)
+        if (motion_set_index_ == 1) {
+          if (L2_.on_press) {
+            planner_use_height_ -= 0.1;
+            planner_use_height_ = std::max(planner_use_height_, 0.2);
+            if constexpr (DEBUG_LOGGING) {
+              std::cout << "[GamepadManager DEBUG] L2 - Height: " << planner_use_height_ << std::endl;
+            }
+          }
+          if (R2_.on_press) {
+            planner_use_height_ += 0.1;
+            planner_use_height_ = std::min(planner_use_height_, 0.8);
+            if constexpr (DEBUG_LOGGING) {
+              std::cout << "[GamepadManager DEBUG] R2 - Height: " << planner_use_height_ << std::endl;
+            }
+          }
+        }
+      }
+
+      // Process timed crawling transitions (kneel → crawl → elbow)
+      processCrawlingTransitions();
+
+      // Analog sticks - facing and movement direction
+      if (std::abs(rx_) > dead_zone_ || std::abs(ry_) > dead_zone_) {
+        planner_facing_angle_ = planner_facing_angle_ - 0.02 * rx_;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] Right stick - Facing angle: " << planner_facing_angle_ << " rad" << std::endl;
+        }
+      }
+
+      if (std::abs(lx_) > dead_zone_ || std::abs(ly_) > dead_zone_) {
+        double raw_angle = atan2(ly_, lx_);
+        double bin_size = M_PI / 4.0;  // 8 directions (45° bins)
+        double binned_angle = std::round(raw_angle / bin_size) * bin_size;
+        planner_moving_direction_ = binned_angle - M_PI / 2 + planner_facing_angle_;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] Left stick - Raw: " << raw_angle
+                    << ", Binned: " << binned_angle
+                    << ", Moving: " << planner_moving_direction_ << " rad" << std::endl;
+        }
+      }
+    }
+
+    // Handle gamepad planner input (called from handle_input())
+    void handleGamepadPlannerInput(MotionDataReader& motion_reader,
+                                   std::shared_ptr<const MotionSequence>& current_motion,
+                                   int& current_frame,
+                                   OperatorState& operator_state,
+                                   bool& reinitialize_heading,
+                                   DataBuffer<HeadingState>& heading_state_buffer,
+                                   PlannerState& planner_state,
+                                   DataBuffer<MovementState>& movement_state_buffer,
+                                   std::mutex& current_motion_mutex) {
+      // Handle safety reset from interface manager
+      if (CheckAndClearSafetyReset()) {
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+        }
+        if (operator_state.start) {
+          if (planner_state.enabled && planner_state.initialized) {
+            // Planner is already on, keep it as is (don't touch initialized flag)
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              if (current_motion->GetEncodeMode() >= 0) {
+                current_motion->SetEncodeMode(0);
+              }
+              operator_state.play = true;
+            }
+            auto current_facing = movement_state_buffer.GetDataWithTime().data->facing_direction;
+            planner_facing_angle_ = std::atan2(current_facing[1], current_facing[0]);
+            std::cout << "[GamepadManager] Safety reset: Planner kept enabled with current state" << std::endl;
+          } else {
+            // Planner was disabled, set initial movement state
+            movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), 
+                                                        {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+
+            // Now enable planner
+            planner_state.enabled = true;
+            planner_facing_angle_ = 0.0;
+            std::cout << "[GamepadManager] Planner enabled" << std::endl;
+
+            // Wait for planner to be initialized with timeout (5 seconds)
+            auto wait_start = std::chrono::steady_clock::now();
+            constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+            while (planner_state.enabled) {
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                if (current_motion->name == "planner_motion") {
+                  std::cout << "[GamepadManager] motion name is planner_motion" << std::endl;
+                  break;
+                }
+              }
+              std::this_thread::sleep_for(std::chrono::milliseconds(100));
+              auto elapsed = std::chrono::steady_clock::now() - wait_start;
+              if (elapsed > PLANNER_INIT_TIMEOUT) {
+                std::cerr << "[GamepadManager ERROR] Planner initialization timeout after 5 seconds" << std::endl;
+                operator_state.stop = true;
+                return;
+              }
+              std::cout << "[GamepadManager] Waiting for planner to be initialized" << std::endl;
+            }
+
+            // Check if planner is enabled and initialized
+            if (!planner_state.enabled || !planner_state.initialized) {
+              std::cerr << "[GamepadManager ERROR] Planner failed to initialize. Stopping control." << std::endl;
+              operator_state.stop = true;
+              return;
+            }
+
+            // Play motion
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              operator_state.play = true;
+            }
+          }
+        }
+        return;
+      }
+
+      // Handle control start (Start button)
+      if (start_control_) {
+        // Start control
+        operator_state.start = true;
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+          // Reinitialize base quaternion and reset delta heading
+          reinitialize_heading = true;
+        }
+        
+        // Ensure planner is enabled (always required in GamepadManager mode)
+        if (!planner_state.enabled) {
+          planner_state.enabled = true;
+          planner_facing_angle_ = 0.0;
+          std::cout << "[GamepadManager] Planner enabled" << std::endl;
+        }
+
+        // Wait for planner to be initialized with timeout (5 seconds)
+        auto wait_start = std::chrono::steady_clock::now();
+        constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+        while (planner_state.enabled) {
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            if (current_motion->name == "planner_motion") {
+              std::cout << "[GamepadManager] motion name is planner_motion" << std::endl;
+              break;
+            }
+          }
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));
+          auto elapsed = std::chrono::steady_clock::now() - wait_start;
+          if (elapsed > PLANNER_INIT_TIMEOUT) {
+            std::cerr << "[GamepadManager ERROR] Planner initialization timeout after 5 seconds" << std::endl;
+            operator_state.stop = true;
+            return;
+          }
+          std::cout << "[GamepadManager] Waiting for planner to be initialized" << std::endl;
+        }
+
+        // Check if planner is enabled and initialized
+        if (!planner_state.enabled || !planner_state.initialized) {
+          std::cerr << "[GamepadManager ERROR] Planner failed to initialize. Stopping control." << std::endl;
+          operator_state.stop = true;
+          return;
+        }
+
+        // Play motion
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = true;
+        }
+      }
+
+      // Handle reinitialize command
+      if (reinitialize_) {
+        std::lock_guard<std::mutex> lock(current_motion_mutex);
+        reinitialize_heading = true;
+        if constexpr (DEBUG_LOGGING) {
+          std::cout << "[GamepadManager DEBUG] Reinitialized base quaternion and facing angle" << std::endl;
+        }
+      }
+
+      // If planner is enabled and initialized, send movement commands
+      if (planner_state.enabled && planner_state.initialized) {
+        int final_mode = planner_use_movement_mode_;
+        std::array<double, 3> final_movement = {double(cos(planner_moving_direction_)),
+                                                 double(sin(planner_moving_direction_)), 0.0};
+        std::array<double, 3> final_facing_direction = {double(cos(planner_facing_angle_)),
+                                                         double(sin(planner_facing_angle_)), 0.0};
+        double final_speed = planner_use_movement_speed_;
+        double final_height = planner_use_height_;
+
+        // If left sticks in dead zone — idle logic depends on motion set
+        if (std::abs(lx_) < dead_zone_ && std::abs(ly_) < dead_zone_) {
+          if (motion_set_index_ == 0) {
+            // Standing set → go to IDLE
+            final_mode = static_cast<int>(LocomotionMode::IDLE);
+            final_movement = {0.0f, 0.0f, 0.0f};
+            final_speed = -1.0f;
+            final_height = -1.0f;
+          } else if (motion_set_index_ == 1) {
+            // Squat set → stay in mode, zero movement
+            final_movement = {0.0f, 0.0f, 0.0f};
+            LocomotionMode cur = static_cast<LocomotionMode>(planner_use_movement_mode_);
+            if (cur == LocomotionMode::CRAWLING || cur == LocomotionMode::ELBOW_CRAWLING) {
+              final_speed = 0.0f;
+            } else {
+              final_speed = -1.0f;
+            }
+            final_height = planner_use_height_;
+          } else if (motion_set_index_ == 2) {
+            // Boxing set → stay in mode, zero movement
+            final_movement = {0.0f, 0.0f, 0.0f};
+            LocomotionMode cur = static_cast<LocomotionMode>(planner_use_movement_mode_);
+            if (cur == LocomotionMode::LEFT_PUNCH || cur == LocomotionMode::RIGHT_PUNCH ||
+                cur == LocomotionMode::LEFT_HOOK || cur == LocomotionMode::RIGHT_HOOK) {
+              // Punches use facing direction as movement
+              final_movement = final_facing_direction;
+            }
+            final_speed = 0.0f;
+            final_height = -1.0f;
+          } else if (motion_set_index_ == 3) {
+            // Styled walking → stay in mode, zero movement
+            final_movement = {0.0f, 0.0f, 0.0f};
+            final_speed = 0.0f;
+            final_height = -1.0f;
+          }
+        }
+
+        // Emergency stop
+        if (planner_emergency_stop_) {
+          if (motion_set_index_ == 0) {
+            final_mode = static_cast<int>(LocomotionMode::IDLE);
+            final_movement = {0.0f, 0.0f, 0.0f};
+            final_speed = -1.0f;
+            final_height = -1.0f;
+          } else {
+            final_movement = {0.0f, 0.0f, 0.0f};
+            final_speed = 0.0f;
+            final_height = (motion_set_index_ == 1) ? planner_use_height_ : -1.0f;
+          }
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] Emergency stop - movement reset" << std::endl;
+          }
+        }
+
+        // Static kneel modes always zero movement
+        if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::IDEL_KNEEL_TWO_LEGS) ||
+            planner_use_movement_mode_ == static_cast<int>(LocomotionMode::IDEL_KNEEL) ||
+            planner_use_movement_mode_ == static_cast<int>(LocomotionMode::IDEL_SQUAT)) {
+          final_movement = {0.0f, 0.0f, 0.0f};
+          final_speed = 0.0f;
+          final_height = planner_use_height_;
+        }
+
+        MovementState mode_state(final_mode, final_movement, final_facing_direction, final_speed, final_height);
+        movement_state_buffer.SetData(mode_state);
+      }
+    }
+
+  private:
+    // ------------------------------------------------------------------
+    // Owned delegate interfaces (gamepad mode is handled inline, not delegated)
+    // ------------------------------------------------------------------
+    std::unique_ptr<ZMQEndpointInterface> zmq_;   ///< ZMQ streaming handler.
+
+    InputInterface* current_ = nullptr;  ///< Non-owning pointer to active delegate (nullptr in gamepad mode).
+
+    // ------------------------------------------------------------------
+    // Active-selection bookkeeping
+    // ------------------------------------------------------------------
+    std::vector<ManagedType> order_;         ///< Insertion-order of available modes.
+    int active_index_ = 0;                   ///< Index into order_.
+    ManagedType active_ = ManagedType::GAMEPAD;  ///< Currently-active mode tag.
+
+    // ZMQ configuration (stored for deferred construction)
+    std::string zmq_host_;
+    int zmq_port_;
+    std::string zmq_topic_;
+    bool zmq_conflate_ = false;
+    bool zmq_verbose_ = false;
+
+    /// Global emergency-stop flag, set by 'O'/'o' keyboard shortcut.
+    bool emergency_stop_ = false;
+    /// Global temperature report flag, set by 'F'/'f' keyboard shortcut.
+    bool report_temperature_flag_ = false;
+
+    // ------------------------------------------------------------------
+    // Per-frame gamepad action flags (reset at the start of update())
+    // ------------------------------------------------------------------
+    bool start_control_ = false;           ///< Start-button pressed this frame.
+    bool stop_control_ = false;            ///< Select-button pressed this frame.
+    bool reinitialize_ = false;            ///< X/Y-button reinitialize heading.
+    bool planner_emergency_stop_ = false;  ///< A-button emergency stop.
+
+    // ------------------------------------------------------------------
+    // Gamepad hardware state
+    // ------------------------------------------------------------------
+    unitree::common::REMOTE_DATA_RX gamepad_data_ = unitree::common::REMOTE_DATA_RX();  ///< Raw 40-byte packet.
+
+    // Smoothed analog stick values
+    float lx_ = 0.0f;   ///< Left stick horizontal (smoothed).
+    float rx_ = 0.0f;   ///< Right stick horizontal (smoothed).
+    float ry_ = 0.0f;   ///< Right stick vertical (smoothed).
+    float l2_ = 0.0f;   ///< Left trigger analog (smoothed).
+    float ly_ = 0.0f;   ///< Left stick vertical (smoothed).
+    float smooth_ = 0.3f;       ///< EMA smoothing factor.
+    float dead_zone_ = 0.05f;   ///< Analog dead-zone threshold.
+
+    // Edge-detecting buttons
+    unitree::common::Button R1_, L1_, start_, select_, R2_, L2_;
+    unitree::common::Button F1_, F2_, A_, B_, X_, Y_;
+    unitree::common::Button up_, right_, down_, left_;
+
+    // ------------------------------------------------------------------
+    // Motion set state
+    // ------------------------------------------------------------------
+    int motion_set_index_ = 0;                    ///< Active motion set (0=Standing, 1=Squat, 2=Boxing, 3=Styled).
+    std::vector<LocomotionMode> current_motion_set_;  ///< Modes in the active set.
+    int mode_index_in_set_ = 0;                   ///< Current mode index within the set.
+
+    // ------------------------------------------------------------------
+    // Planner control state (persists across frames)
+    // ------------------------------------------------------------------
+    int planner_use_movement_mode_ = static_cast<int>(LocomotionMode::SLOW_WALK);  ///< Current locomotion mode.
+    double planner_use_movement_speed_ = 0.4;    ///< Desired speed (fixed per mode).
+    double planner_use_height_ = -1.0;            ///< Desired body height (−1 = mode default).
+    double planner_facing_angle_ = 0.0;           ///< Accumulated facing direction (radians).
+    double planner_moving_direction_ = 0.0;       ///< Current movement direction (radians).
+
+    // ------------------------------------------------------------------
+    // Staged crawling transition state
+    // ------------------------------------------------------------------
+    /// Target mode for the current transition stage (e.g., CRAWLING after kneeling).
+    LocomotionMode transition_target_mode_ = LocomotionMode::IDLE;
+    /// Final mode for multi-stage transitions (e.g., ELBOW_CRAWLING via kneel→crawl→elbow).
+    LocomotionMode transition_final_mode_ = LocomotionMode::IDLE;
+    /// Timestamp when the current transition stage began.
+    std::chrono::time_point<std::chrono::steady_clock> transition_start_time_{};
+
+    // ------------------------------------------------------------------
+    // Boxing auto-revert state
+    // ------------------------------------------------------------------
+    /// Timestamp when a punch/hook/idle was triggered; reverts to WALK_BOXING after 1s.
+    std::chrono::time_point<std::chrono::steady_clock> boxing_revert_time_{};
+};
+
+#endif // GAMEPAD_MANAGER_HPP
+
+

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/input_command.hpp

@@ -0,0 +1,108 @@
+/**
+ * @file input_command.hpp
+ * @brief Lightweight message structs used for inter-component communication
+ *        between ZMQ subscribers and the input managers.
+ *
+ * Two message types are defined:
+ *  - CommandMessage  – carries high-level control signals (start / stop /
+ *                      planner-mode toggle) received on the ZMQ "command" topic.
+ *  - PlannerMessage  – carries per-frame locomotion commands (mode, movement
+ *                      direction, facing direction, speed, height, and optional
+ *                      upper-body / hand data) received on the ZMQ "planner" topic.
+ *
+ * Both structs are plain-old-data (POD-like) value types designed to be written
+ * under a mutex by a background ZMQ thread and read by the main control loop.
+ */
+
+#pragma once
+
+#include <array>
+#include <chrono>
+#include <optional>
+
+#include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
+
+// ---------------------------------------------------------------------------
+// CommandMessage
+// ---------------------------------------------------------------------------
+/**
+ * @brief Wire format for the ZMQ "command" topic.
+ *
+ * Packed binary layout sent by the remote controller:
+ *   { start: bool, stop: bool, planner: bool, delta_heading?: f32/f64 }
+ *
+ * Multiple messages between two update() calls are accumulated using OR logic
+ * for start/stop (so a transient pulse is never lost), while the planner flag
+ * is overwritten with the latest value.
+ */
+struct CommandMessage {
+  bool start = false;     ///< When true, request the control system to start.
+  bool stop = false;      ///< When true, request an emergency / graceful stop.
+  bool planner = false;   ///< true  → planner mode  (use planner topic for locomotion)
+                          ///< false → streamed-motion mode  (use pose topic)
+  /// Optional absolute heading override (radians).  When set, the value is
+  /// written directly into HeadingState.delta_heading.
+  std::optional<double> delta_heading;
+  bool valid = false;     ///< Set to true once a message has been decoded successfully.
+};
+
+// ---------------------------------------------------------------------------
+// PlannerMessage
+// ---------------------------------------------------------------------------
+/**
+ * @brief Wire format for the ZMQ "planner" topic.
+ *
+ * Required fields (must be present in every message):
+ *   - mode      : int32  – LocomotionMode enum cast (IDLE, WALK, RUN, …)
+ *   - movement  : float[3] – desired movement direction unit vector (x, y, z)
+ *   - facing    : float[3] – desired facing direction unit vector  (x, y, z)
+ *
+ * Optional fields (may or may not be present):
+ *   - speed              : float – desired locomotion speed (-1.0 = use default)
+ *   - height             : float – desired body height      (-1.0 = use default)
+ *   - upper_body_position: float[17] – target upper-body joint positions  (radians)
+ *   - upper_body_velocity: float[17] – target upper-body joint velocities (rad/s)
+ *   - left_hand_joints   : float[7]  – Dex3 left-hand joint positions
+ *   - right_hand_joints  : float[7]  – Dex3 right-hand joint positions
+ *
+ * The `timestamp` field is set locally on receipt and used for timeout
+ * detection (planner messages older than ~1 s are considered stale).
+ */
+struct PlannerMessage {
+  bool valid = false;  ///< True once this struct contains a successfully decoded message.
+
+  /// Locomotion mode (cast of LocomotionMode enum). Defaults to IDLE.
+  int mode = static_cast<int>(LocomotionMode::IDLE);
+
+  /// Desired movement direction as a 3D unit vector [x, y, z].
+  /// Zeroed when the robot should stand still.
+  std::array<double, 3> movement = {0.0, 0.0, 0.0};
+
+  /// Desired facing direction as a 3D unit vector [x, y, z].
+  /// Defaults to facing forward along the +X axis.
+  std::array<double, 3> facing = {1.0, 0.0, 0.0};
+
+  /// Optional upper-body joint target positions (17 DOF, radians).
+  /// Present when the remote controller provides whole-body commands.
+  std::optional<std::array<double, 17>> upper_body_position;
+
+  /// Optional upper-body joint target velocities (17 DOF, rad/s).
+  std::optional<std::array<double, 17>> upper_body_velocity;
+
+  /// Optional left-hand Dex3 joint positions (7 DOF).
+  std::optional<std::array<double, 7>> left_hand_joints;
+
+  /// Optional right-hand Dex3 joint positions (7 DOF).
+  std::optional<std::array<double, 7>> right_hand_joints;
+
+  /// Desired locomotion speed.  -1.0 means "use the default for the current mode".
+  double speed = -1.0;
+
+  /// Desired body height.  -1.0 means "use the default for the current mode".
+  double height = -1.0;
+
+  /// Local steady-clock timestamp recorded when the message was received.
+  /// Used to detect planner timeouts (stale data → fallback to IDLE).
+  std::chrono::steady_clock::time_point timestamp{};
+};
+

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/input_interface.hpp

@@ -0,0 +1,505 @@
+/**
+ * @file input_interface.hpp
+ * @brief Abstract base class for all input sources that can drive the G1 robot.
+ *
+ * InputInterface defines the polymorphic contract that every concrete input
+ * handler (keyboard, gamepad, ZMQ, ROS2, …) must implement.  Two pure-virtual
+ * methods form the core of the interface:
+ *
+ *   1. update()       – Poll the hardware / network for new data and latch it
+ *                        into internal flags (called once per control-loop tick).
+ *   2. handle_input() – Translate the latched flags into system-state changes
+ *                        (motion switching, planner commands, heading adjustments,
+ *                         operator start / stop, etc.).
+ *
+ * The base class also provides:
+ *   - Thread-safe DataBuffer members for VR 3-point tracking, upper-body
+ *     joint targets, hand joint targets, and external token state.
+ *   - Shared stdin buffering so the InterfaceManager can pre-read keys and
+ *     dispatch them to the correct active interface.
+ *   - A safety-reset mechanism used by managers when switching active
+ *     interfaces to prevent stale commands from carrying over.
+ *   - Keyboard-controlled compliance and hand-close-ratio helpers that are
+ *     shared across all input modes (g/h/b/v and x/c keys).
+ *
+ * @note The class is non-copyable and non-movable by design.  Instances are
+ *       always managed via std::unique_ptr by the owning manager.
+ */
+
+#ifndef INPUT_INTERFACE_HPP
+#define INPUT_INTERFACE_HPP
+
+#include <unistd.h>
+#include <atomic>
+#include <queue>
+#include <memory>
+#include <optional>
+#include "../utils.hpp"              // For DataBuffer  
+#include "../robot_parameters.hpp"   // For HeadingState, OperatorState
+#include "../motion_data_reader.hpp" // For MotionDataReader, MotionSequence
+#include "../math_utils.hpp"         // For float_to_double
+#include "../localmotion_kplanner.hpp" // For PlannerState, MovementState
+
+/**
+ * @class InputInterface
+ * @brief Abstract base class for all input sources (keyboard, gamepad, ZMQ, ROS2).
+ *
+ * Concrete sub-classes must implement update() and handle_input().
+ * The base class owns shared state (VR buffers, compliance values, stdin queue)
+ * that all implementations can use.
+ */
+class InputInterface {
+public:
+
+    /// Identifies the physical / logical source of this input interface.
+    enum class InputType {
+      KEYBOARD,   ///< Terminal stdin keyboard
+      GAMEPAD,    ///< Unitree wireless gamepad (raw joystick data)
+      ROS2,       ///< ROS 2 topics (msgpack-serialised ControlGoalMsg)
+      NETWORK,    ///< ZMQ packed-message protocol (pose / planner topics)
+      UNKNOWN     ///< Default / uninitialised
+    };
+
+    /// Virtual destructor ensures correct cleanup of derived classes.
+    virtual ~InputInterface() = default;
+
+    // ------------------------------------------------------------------
+    // Pure-virtual interface
+    // ------------------------------------------------------------------
+
+    /**
+     * @brief Poll the input source for new data and latch it internally.
+     *
+     * Called once per control-loop tick **before** handle_input().
+     * Implementations should:
+     *  - Read all available bytes / messages from their source.
+     *  - Update per-frame boolean flags (e.g. start_control, delta_left).
+     *  - NOT modify shared system state – that is done in handle_input().
+     */
+    virtual void update() = 0;
+
+    /**
+     * @brief Apply latched input flags to the system state.
+     *
+     * Called once per control-loop tick **after** update().
+     *
+     * @param motion_reader         Reference to the pre-loaded motion library.
+     * @param current_motion        Currently-active MotionSequence (may be swapped).
+     * @param current_frame         Playback cursor within current_motion.
+     * @param operator_state        High-level operator signals (start/stop/play).
+     * @param reinitialize_heading  Set to true to recapture the IMU heading.
+     * @param heading_state_buffer  Thread-safe heading buffer (delta heading, init quat).
+     * @param has_planner           Whether a locomotion planner is loaded.
+     * @param planner_state         Planner enable / initialise state.
+     * @param movement_state_buffer Thread-safe buffer for locomotion commands.
+     * @param current_motion_mutex  Guards concurrent access to current_motion/frame.
+     */
+    virtual void handle_input(MotionDataReader& motion_reader,
+                            std::shared_ptr<const MotionSequence>& current_motion,
+                            int& current_frame,
+                            OperatorState& operator_state,
+                            bool& reinitialize_heading,
+                            DataBuffer<HeadingState>& heading_state_buffer,
+                            bool has_planner,
+                            PlannerState& planner_state,
+                            DataBuffer<MovementState>& movement_state_buffer,
+                            std::mutex& current_motion_mutex,
+                            bool& report_temperature) = 0;
+
+    // ------------------------------------------------------------------
+    // Capability queries – overridden by sub-classes as needed
+    // ------------------------------------------------------------------
+
+    /// @return The InputType tag for this concrete implementation.
+    virtual InputType GetType() {
+      return type_;
+    }
+
+    /// @return True if this interface provides upper-body joint targets (17 DOF).
+    virtual bool HasUpperBodyControl() const {
+      return has_upper_body_control_;
+    }
+    
+    /// @return True if this interface provides VR 3-point tracking data
+    ///         (left wrist, right wrist, head – 9 position + 12 orientation values).
+    virtual bool HasVR3PointControl() const {
+      return has_vr_3point_control_;
+    }
+    
+    /// @return True if this interface provides VR 5-point tracking data
+    ///         (3-point + 2 extra trackers – 15 position + 20 orientation values).
+    virtual bool HasVR5PointControl() const {
+      return has_vr_5point_control_;
+    }
+
+    /// @return True if this interface provides Dex3 hand joint targets (7 DOF per hand).
+    virtual bool HasHandJoints() const {
+      return has_hand_joints_;
+    }
+    
+    /// @return True if an external token-state vector is available (e.g. from ROS2/ZMQ).
+    virtual bool HasExternalTokenState() const {
+      return has_external_token_state_;
+    }
+    
+    /**
+     * @brief Retrieve the latest external token-state vector (if any).
+     * @return A pair: {true, vector} if data is available; {false, {}} otherwise.
+     */
+    virtual std::pair<bool, std::vector<double>> GetExternalTokenState() const {
+      if (!has_external_token_state_) {
+        return {false, {}};
+      }
+      auto buffered_data = external_token_state_.GetDataWithTime();
+      if (buffered_data.data) {
+        return {true, *buffered_data.data};
+      }
+      return {false, {}};
+    }
+    
+    // ------------------------------------------------------------------
+    // VR 3-point tracking data accessors
+    // Layout: [left_wrist(xyz), right_wrist(xyz), head(xyz)]
+    //         [left_quat(wxyz), right_quat(wxyz), head_quat(wxyz)]
+    // ------------------------------------------------------------------
+
+    /// Write new VR 3-point position data into the thread-safe buffer.
+    virtual void SetVR3PointPosition(const std::array<double, 9>& position) {
+        vr_3point_position_.SetData(position);
+    }
+    
+    /// Write new VR 3-point orientation data into the thread-safe buffer.
+    virtual void SetVR3PointOrientation(const std::array<double, 12>& orientation) {
+        vr_3point_orientation_.SetData(orientation);
+    }
+
+    /**
+     * @brief Get the latest VR 3-point position data.
+     * @return {true, positions} if VR tracking is active and data is available;
+     *         {false, default_positions} otherwise.
+     *
+     * Default positions (metres, in body frame):
+     *   left wrist  = ( 0.0903,  0.1615, -0.2411)
+     *   right wrist = ( 0.1280, -0.1522, -0.2461)
+     *   head        = ( 0.0241, -0.0081,  0.4028)
+     */
+    virtual std::pair<bool, std::array<double, 9>> GetVR3PointPosition() const {
+        if(!has_vr_3point_control_) {
+            return {false, {0.0903,  0.1615, -0.2411, 
+                    0.1280, -0.1522, -0.2461, 
+                    0.0241, -0.0081,  0.4028}};
+        }
+        auto buffered_data = vr_3point_position_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.0903,  0.1615, -0.2411, 
+                0.1280, -0.1522, -0.2461, 
+                0.0241, -0.0081,  0.4028}};
+    }
+    
+    /**
+     * @brief Get the latest VR 3-point orientation data (quaternions w,x,y,z).
+     * @return {true, orientations} if VR tracking is active and data is available;
+     *         {false, default_orientations} otherwise.
+     */
+    virtual std::pair<bool, std::array<double, 12>> GetVR3PointOrientation() const {
+        if(!has_vr_3point_control_) {
+            return {false, {0.7295,  0.3145,  0.5533, -0.2506, 
+                    0.7320, -0.2639,  0.5395,  0.3217, 
+                    0.9991,  0.011,  0.0402, -0.0002}};
+        }
+        auto buffered_data = vr_3point_orientation_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.7295,  0.3145,  0.5533, -0.2506, 
+                0.7320, -0.2639,  0.5395,  0.3217, 
+                0.9991,  0.011,  0.0402, -0.0002}};
+    }
+    
+    /**
+     * @brief Get VR 3-point compliance values (keyboard-controlled).
+     *
+     * Compliance is always controlled via keyboard (g/h keys for left, b/v for right)
+     * and is intentionally **not** overwritten by external sources (ROS2 / ZMQ).
+     * @return [left_arm_compliance, right_arm_compliance, head_compliance] in [0, 0.5].
+     */
+    virtual std::array<double, 3> GetVR3PointCompliance() const {
+        auto buffered_data = vr_3point_compliance_.GetDataWithTime();
+        if (buffered_data.data) {
+            return *buffered_data.data;
+        }
+        return {0.5, 0.5, 0.0};
+    }
+
+    /// Set initial VR 3-point compliance values (called at startup from command-line args).
+    virtual void SetVR3PointCompliance(const std::array<double, 3>& compliance) {
+        vr_3point_compliance_.SetData(compliance);
+    }
+    
+    /// Adjust left-arm (index 0) compliance by @p delta, clamped to [0.0, 0.5].
+    /// Only takes effect if the active policy observes 'vr_3point_compliance'.
+    virtual void AdjustLeftHandCompliance(double delta) {
+        auto compliance = GetVR3PointCompliance();
+        compliance[0] = std::clamp(compliance[0] + delta, 0.0, 0.5);
+        SetVR3PointCompliance(compliance);
+        std::cout << "[Compliance] Left hand: " <<  GetVR3PointCompliance()[0] << std::endl;
+    }
+    
+    /// Adjust right-arm (index 1) compliance by @p delta, clamped to [0.0, 0.5].
+    /// Only takes effect if the active policy observes 'vr_3point_compliance'.
+    virtual void AdjustRightHandCompliance(double delta) {
+        auto compliance = GetVR3PointCompliance();
+        compliance[1] = std::clamp(compliance[1] + delta, 0.0, 0.5);
+        SetVR3PointCompliance(compliance);
+        std::cout << "[Compliance] Right hand: " <<  GetVR3PointCompliance()[1] << std::endl;
+    }
+
+    // =========================================================================
+    // Hand max close ratio control (keyboard-controlled via X/C keys)
+    // Controls how much the Dex3 hands can close (0.2 = 80% open, 1.0 = fully closed)
+    // =========================================================================
+    
+    // Get the current max close ratio (keyboard-controlled)
+    virtual double GetMaxCloseRatio() const {
+        return max_close_ratio_;
+    }
+    
+    // Set initial max close ratio (from command line or initialization)
+    virtual void SetMaxCloseRatio(double ratio) {
+        max_close_ratio_.store(std::clamp(ratio, 0.2, 1.0), std::memory_order_relaxed);
+    }
+    
+    // Adjust max close ratio by delta (X = +0.1, C = -0.1), clipped to [0.2, 1.0]
+    virtual void AdjustMaxCloseRatio(double delta) {
+        double new_val = std::clamp(max_close_ratio_.load(std::memory_order_relaxed) + delta, 0.2, 1.0);
+        max_close_ratio_.store(new_val, std::memory_order_relaxed);
+        std::cout << "[InputInterface] Max close ratio adjusted to: " << new_val 
+                  << " (range: 0.2-1.0, higher = more closed)" << std::endl;
+    }
+
+    // ------------------------------------------------------------------
+    // VR 5-point tracking (3-point + 2 extra body trackers)
+    // Layout: [left_wrist, right_wrist, head, tracker1, tracker2] × xyz
+    // ------------------------------------------------------------------
+
+    /**
+     * @brief Get VR 5-point position data (15 doubles).
+     * @return {true, positions} if 5-point tracking is active; {false, defaults} otherwise.
+     */
+    virtual std::pair<bool, std::array<double, 15>> GetVR5PointPosition() const {
+        if(!has_vr_5point_control_) {
+            return {false, {0.0903,  0.1615, -0.2411, 
+                    0.1280, -0.1522, -0.2461, 
+                    0.0, 0.0, 0.0, 
+                    0.0, 0.0, 0.0, 
+                    0.0, 0.0, 0.0}};
+        }
+        auto buffered_data = vr_5point_position_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.0903,  0.1615, -0.2411, 
+                0.1280, -0.1522, -0.2461, 
+                0.0, 0.0, 0.0, 
+                0.0, 0.0, 0.0, 
+                0.0, 0.0, 0.0}};
+    }
+
+    /// @brief Get VR 5-point orientation data (20 doubles = 5 quaternions × wxyz).
+    virtual std::pair<bool, std::array<double, 20>> GetVR5PointOrientation() const {
+        if(!has_vr_5point_control_) {
+            return {false, {0.7295,  0.3145,  0.5533, -0.2506, 
+                    0.7320, -0.2639,  0.5395,  0.3217, 
+                    0.9991,  0.011,  0.0402, -0.0002, 
+                    1.0, 0.0, 0.0, 0.0,
+                    1.0, 0.0, 0.0, 0.0}};
+        }
+        auto buffered_data = vr_5point_orientation_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.7295,  0.3145,  0.5533, -0.2506, 
+                0.7320, -0.2639,  0.5395,  0.3217, 
+                0.9991,  0.011,  0.0402, -0.0002, 
+                1.0, 0.0, 0.0, 0.0,
+                1.0, 0.0, 0.0, 0.0}};
+    }
+
+    // ------------------------------------------------------------------
+    // Hand / upper-body joint accessors
+    // ------------------------------------------------------------------
+
+    /**
+     * @brief Get 7-DOF Dex3 hand joint positions.
+     * @param is_left  true → left hand, false → right hand.
+     * @return {true, joints} if hand joint data is available; {false, defaults} otherwise.
+     *         Default left  = {0, 0,  1.75, -1.57, -1.75, -1.57, -1.75}
+     *         Default right = {0, 0, -1.75,  1.57,  1.75,  1.57,  1.75}
+     */
+    virtual std::pair<bool, std::array<double, 7>> GetHandPose(bool is_left) const {
+        if(!has_hand_joints_) {
+            if(is_left) {
+                return {false, {0, 0, 1.75, -1.57, -1.75, -1.57, -1.75 }};
+            } else {
+                return {false, {0, 0, -1.75,  1.57,  1.75,  1.57,  1.75 }};
+            }
+        }
+        if(is_left) {
+            auto buffered_data = left_hand_joint_.GetDataWithTime();
+            if (buffered_data.data) {
+                return {true, *buffered_data.data};
+            }
+            else {
+                return {false, {0, 0, 1.75, -1.57, -1.75, -1.57, -1.75 }};
+            }
+        } else {
+            auto buffered_data = right_hand_joint_.GetDataWithTime();
+            if (buffered_data.data) {
+                return {true, *buffered_data.data};
+            } else {
+                return {false, {0, 0, -1.75,  1.57,  1.75,  1.57,  1.75 }};
+            }
+        }
+    }
+
+    /// @brief Get upper-body joint target positions (17 DOF, radians).
+    /// @return {true, positions} if upper-body data is available; {false, zeros} otherwise.
+    virtual std::pair<bool, std::array<double, 17>> GetUpperBodyJointPositions() const {
+        if(!has_upper_body_control_) {
+            return {false, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        }
+        auto buffered_data = upper_body_joint_positions_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+    }
+
+    /// @brief Get upper-body joint target velocities (17 DOF, rad/s).
+    /// @return {true, velocities} if upper-body data is available; {false, zeros} otherwise.
+    virtual std::pair<bool, std::array<double, 17>> GetUpperBodyJointVelocities() const {
+        if(!has_upper_body_control_) {
+            return {false, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        }
+        auto buffered_data = upper_body_joint_velocities_.GetDataWithTime();
+        if (buffered_data.data) {
+            return {true, *buffered_data.data};
+        }
+        return {false, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+    }
+
+    /// @brief Get the last update time of the input interface.
+    virtual std::optional<std::chrono::steady_clock::time_point> GetLastUpdateTime() const {
+        // Return empty optional as a default, implementers should override this if they have a timestamp
+        return {};
+    }
+
+    // ------------------------------------------------------------------
+    // Shared stdin buffering
+    // ------------------------------------------------------------------
+    /**
+     * @brief Read one character, trying the manager-pushed buffer first, then raw stdin.
+     *
+     * When an InterfaceManager is active, it pre-reads stdin and dispatches
+     * keys: manager-level shortcuts are consumed, everything else is queued
+     * via PushStdinChar() for the active interface to read later.
+     *
+     * @param[out] ch  The character that was read.
+     * @return True if a character was available, false otherwise.
+     */
+    bool ReadStdinChar(char& ch) {
+        // First check if we have buffered keys from manager
+        if (!stdin_buffer_.empty()) {
+            ch = stdin_buffer_.front();
+            stdin_buffer_.pop();
+            return true;
+        }
+        // Otherwise read directly from stdin (non-blocking)
+        return read(STDIN_FILENO, &ch, 1) > 0;
+    }
+
+    /// Queue a key into this interface's stdin buffer (used by managers).
+    void PushStdinChar(char ch) {
+        stdin_buffer_.push(ch);
+    }
+
+    // ------------------------------------------------------------------
+    // Safety-reset mechanism  (used when switching active interfaces)
+    // ------------------------------------------------------------------
+
+    /// Flag that tells the interface to abandon its current mode and return
+    /// to a safe default (disable planner, stop streaming, etc.).
+    std::atomic<bool> reset_to_safe_state_{false};
+
+    /// Called by the manager to request a safety reset on this interface.
+    void TriggerSafetyReset() {
+        reset_to_safe_state_.store(true, std::memory_order_release);
+    }
+
+    /// Atomically check and clear the safety-reset flag.
+    /// @return True if a reset was requested since the last call.
+    bool CheckAndClearSafetyReset() {
+        return reset_to_safe_state_.exchange(false, std::memory_order_acq_rel);
+    }
+
+protected:
+    /// Protected default constructor – only concrete sub-classes may instantiate.
+    InputInterface() = default;
+
+    // Non-copyable, non-movable (instances are managed via unique_ptr).
+    InputInterface(const InputInterface&) = delete;
+    InputInterface& operator=(const InputInterface&) = delete;
+    InputInterface(InputInterface&&) = delete;
+    InputInterface& operator=(InputInterface&&) = delete;
+
+    // ------------------------------------------------------------------
+    // Capability flags – set by sub-class constructors / update methods
+    // ------------------------------------------------------------------
+    InputType type_ = InputType::UNKNOWN;             ///< Concrete input source tag.
+    std::atomic<bool> has_vr_3point_control_{false};  ///< VR 3-point tracking available.
+    std::atomic<bool> has_vr_5point_control_{false};  ///< VR 5-point tracking available.
+    std::atomic<bool> has_hand_joints_{false};        ///< Dex3 hand joint data available.
+    std::atomic<bool> has_external_token_state_{false}; ///< External token-state vector available.
+    std::atomic<bool> has_upper_body_control_{false}; ///< Upper-body 17-DOF targets available.
+
+    // ------------------------------------------------------------------
+    // Thread-safe data buffers (written by input threads, read by control loop)
+    // ------------------------------------------------------------------
+
+    /// VR 3-point positions [left_wrist xyz, right_wrist xyz, head xyz].
+    DataBuffer<std::array<double, 9>> vr_3point_position_;
+    /// VR 3-point orientations [left quat wxyz, right quat wxyz, head quat wxyz].
+    DataBuffer<std::array<double, 12>> vr_3point_orientation_;
+    /// VR 3-point compliance [left_arm, right_arm, head] – keyboard-controlled.
+    DataBuffer<std::array<double, 3>> vr_3point_compliance_;
+    /// VR 5-point positions (3-point + 2 extra trackers × xyz = 15 values).
+    DataBuffer<std::array<double, 15>> vr_5point_position_;
+    /// VR 5-point orientations (5 quaternions × wxyz = 20 values).
+    DataBuffer<std::array<double, 20>> vr_5point_orientation_;
+
+    /// Upper-body target joint positions (17 DOF, radians).
+    DataBuffer<std::array<double, 17>> upper_body_joint_positions_;
+    /// Upper-body target joint velocities (17 DOF, rad/s).
+    DataBuffer<std::array<double, 17>> upper_body_joint_velocities_;
+    
+    /// Left-hand Dex3 joint positions (7 DOF).
+    DataBuffer<std::array<double, 7>> left_hand_joint_;
+    /// Right-hand Dex3 joint positions (7 DOF).
+    DataBuffer<std::array<double, 7>> right_hand_joint_;
+    
+    /// Arbitrary external token-state vector (e.g. latent codes from a remote model).
+    DataBuffer<std::vector<double>> external_token_state_;
+
+    /// Keyboard-controlled max close ratio for Dex3 hands.
+    /// Adjusted via keyboard (X = +0.1, C = −0.1), clamped to [0.2, 1.0].
+    /// 1.0 = fully closed allowed (default); use --max-close-ratio CLI arg to limit.
+    std::atomic<double> max_close_ratio_{1.0};
+
+    /// Shared stdin buffer – the InterfaceManager pushes non-manager keys here
+    /// for the currently-active interface to consume via ReadStdinChar().
+    std::queue<char> stdin_buffer_;
+};
+
+#endif // INPUT_INTERFACE_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/interface_manager.hpp

@@ -0,0 +1,396 @@
+/**
+ * @file interface_manager.hpp
+ * @brief Multiplexer that owns several concrete InputInterface instances and
+ *        delegates all calls to the currently-active one.
+ *
+ * InterfaceManager is itself an InputInterface so it can be used transparently
+ * by the main control loop.  It creates and owns:
+ *   - SimpleKeyboard   (index 0, default)
+ *   - Gamepad          (index 1)
+ *   - ZMQEndpointInterface (index 2)
+ *   - ROS2InputHandler (index 3, only when compiled with HAS_ROS2)
+ *
+ * Switching is done via **keyboard shortcuts** typed into the terminal:
+ *   '!' → Keyboard  |  '@' → Gamepad  |  '#' → ZMQ  |  '$' → ROS2
+ *
+ * The manager also handles several **global** controls that work regardless of
+ * which interface is active:
+ *   - 'O'/'o'  → emergency stop
+ *   - g/G, h/H → left-hand compliance ±0.1
+ *   - b/B, v/V → right-hand compliance ±0.1
+ *   - x/X, c/C → hand max-close ratio ±0.1
+ *
+ * When switching interfaces, a **safety reset** is triggered on *all* managed
+ * interfaces to prevent stale planner / streaming state from carrying over.
+ */
+
+#ifndef INTERFACE_MANAGER_HPP
+#define INTERFACE_MANAGER_HPP
+
+#include <memory>
+#include <vector>
+#include <iostream>
+#include <cstring>
+#include <cstdlib>
+
+#include "input_interface.hpp"
+#include "keyboard_handler.hpp"
+#include "gamepad.hpp"
+#include "zmq_endpoint_interface.hpp"
+
+#if HAS_ROS2
+#include "ros2_input_handler.hpp"
+#endif
+
+/**
+ * @class InterfaceManager
+ * @brief Composite InputInterface that delegates to one of several concrete
+ *        implementations, switchable at run-time via keyboard shortcuts.
+ *
+ * All getter methods (HasVR3PointControl, GetHandPose, etc.) are forwarded to
+ * the currently-active delegate so the control loop always sees live values.
+ */
+class InterfaceManager : public InputInterface {
+  public:
+    /// Identifies which concrete interface is currently active.
+    enum class ManagedType {
+      KEYBOARD = 0,  ///< SimpleKeyboard (stdin)
+      GAMEPAD = 1,   ///< Unitree wireless gamepad
+      ZMQ = 2,       ///< ZMQ packed-message streaming
+      ROS2 = 3       ///< ROS 2 teleop (requires HAS_ROS2)
+    };
+
+    /**
+     * @brief Construct the manager, creating all sub-interfaces.
+     * @param zmq_host     ZMQ server hostname (passed to ZMQEndpointInterface).
+     * @param zmq_port     ZMQ server port.
+     * @param zmq_topic    ZMQ subscription topic.
+     * @param zmq_conflate Whether to enable ZMQ conflate (latest-only) mode.
+     * @param zmq_verbose  Enable verbose ZMQ logging.
+     */
+    InterfaceManager(
+      const std::string& zmq_host,
+      int zmq_port,
+      const std::string& zmq_topic,
+      bool zmq_conflate,
+      bool zmq_verbose
+    ) : InputInterface(), zmq_host_(zmq_host), zmq_port_(zmq_port), zmq_topic_(zmq_topic),
+        zmq_conflate_(zmq_conflate), zmq_verbose_(zmq_verbose) {
+      type_ = InputType::UNKNOWN;
+      buildInterfaces();
+      setActiveIndex(0); // default to keyboard (index 0)
+    }
+
+    void update() override {
+      // Reset per-frame flags
+      emergency_stop_ = false;
+      report_temperature_flag_ = false;
+      
+      // Read stdin using shared buffering mechanism, check for manager keys
+      char ch;
+      while (ReadStdinChar(ch)) {
+        bool is_manager_key = false;
+        switch (ch) {
+          case '!': 
+            SetActiveInterface(ManagedType::KEYBOARD); 
+            is_manager_key = true;
+            break;
+          case '@': 
+            SetActiveInterface(ManagedType::GAMEPAD); 
+            is_manager_key = true;
+            break;
+          case '#': 
+            SetActiveInterface(ManagedType::ZMQ); 
+            is_manager_key = true;
+            break;
+          case '$': 
+            SetActiveInterface(ManagedType::ROS2); 
+            is_manager_key = true;
+            break;
+          case 'o':
+          case 'O':
+            // Global emergency stop - works for all interfaces (especially gamepad)
+            emergency_stop_ = true;
+            is_manager_key = true;
+            std::cout << "[InterfaceManager] EMERGENCY STOP triggered (O/o key pressed)" << std::endl;
+            break;
+          // Global compliance controls - work across ALL interfaces (keyboard, planner, ROS2, ZMQ)
+          case 'g':
+          case 'G':
+            // Increase left hand compliance by 0.1
+            AdjustLeftHandCompliance(0.1);
+            is_manager_key = true;
+            break;
+          case 'h':
+          case 'H':
+            // Decrease left hand compliance by 0.1
+            AdjustLeftHandCompliance(-0.1);
+            is_manager_key = true;
+            break;
+          case 'b':
+          case 'B':
+            // Increase right hand compliance by 0.1
+            AdjustRightHandCompliance(0.1);
+            is_manager_key = true;
+            break;
+          case 'v':
+          case 'V':
+            // Decrease right hand compliance by 0.1
+            AdjustRightHandCompliance(-0.1);
+            is_manager_key = true;
+            break;
+          // Global hand max close ratio controls - work across ALL interfaces (x/c keys)
+          case 'x':
+          case 'X':
+            // Increase max close ratio by 0.1 (allow hands to close more)
+            AdjustMaxCloseRatio(0.1);
+            is_manager_key = true;
+            break;
+          case 'c':
+          case 'C':
+            // Decrease max close ratio by 0.1 (keep hands more open)
+            AdjustMaxCloseRatio(-0.1);
+            is_manager_key = true;
+            break;
+          case 'f':
+          case 'F':
+            // Global temperature report
+            report_temperature_flag_ = true;
+            is_manager_key = true;
+            break;
+        }
+        
+        // Buffer non-manager keys for the active interface to read
+        // Note: 'O'/'o' is NOT passed through since it's handled globally
+        if (!is_manager_key) {
+          current_->PushStdinChar(ch);
+        }
+      }
+
+      // Run the actual active interface update (it will read buffered keys)
+      current_->update();
+
+      // Note: We don't cache data here anymore - just forward calls to active interface
+      // This ensures we always get the most current values
+    }
+
+    void handle_input(MotionDataReader& motion_reader,
+                      std::shared_ptr<const MotionSequence>& current_motion,
+                      int& current_frame,
+                      OperatorState& operator_state,
+                      bool& reinitialize_heading,
+                      DataBuffer<HeadingState>& heading_state_buffer,
+                      bool has_planner,
+                      PlannerState& planner_state,
+                      DataBuffer<MovementState>& movement_state_buffer,
+                      std::mutex& current_motion_mutex,
+                      bool& report_temperature) override {
+      
+      // Handle global emergency stop (works for all interfaces, especially gamepad)
+      if (emergency_stop_) {
+        operator_state.stop = true;
+      }
+
+      // Handle global temperature report (F key)
+      if (report_temperature_flag_) {
+        report_temperature = true;
+        report_temperature_flag_ = false;
+      }
+
+      // Check if ROS2 is active but planner is not available - switch to keyboard
+      if (active_ == ManagedType::ROS2 && !has_planner) {
+        std::cout << "[InterfaceManager] ROS2 requires planner but planner not loaded. Switching to KEYBOARD" << std::endl;
+        SetActiveInterface(ManagedType::KEYBOARD);
+      }
+
+      // Delegate to the active interface
+      current_->handle_input(motion_reader, current_motion, current_frame, operator_state,
+                             reinitialize_heading, heading_state_buffer, has_planner, planner_state,
+                             movement_state_buffer, current_motion_mutex, report_temperature);
+    }
+    
+    // Override all getters to forward directly to the active interface
+    // This makes the manager a transparent proxy that always returns live values
+    
+    
+    bool HasVR3PointControl() const override {
+      if (current_) {
+        return current_->HasVR3PointControl();
+      }
+      return has_vr_3point_control_;
+    }
+    
+    bool HasHandJoints() const override {
+      if (current_) {
+        return current_->HasHandJoints();
+      }
+      return has_hand_joints_;
+    }
+    
+    bool HasExternalTokenState() const override {
+      if (current_) {
+        return current_->HasExternalTokenState();
+      }
+      return has_external_token_state_;
+    }
+    
+    std::pair<bool, std::array<double, 9>> GetVR3PointPosition() const override {
+      if (current_) {
+        return current_->GetVR3PointPosition();
+      }
+      return InputInterface::GetVR3PointPosition();  // Fallback to base class
+    }
+    
+    std::pair<bool, std::array<double, 12>> GetVR3PointOrientation() const override {
+      if (current_) {
+        return current_->GetVR3PointOrientation();
+      }
+      return InputInterface::GetVR3PointOrientation();  // Fallback to base class
+    }
+    
+    std::pair<bool, std::array<double, 7>> GetHandPose(bool is_left) const override {
+      if (current_) {
+        return current_->GetHandPose(is_left);
+      }
+      return InputInterface::GetHandPose(is_left);  // Fallback to base class
+    }
+    
+    std::pair<bool, std::vector<double>> GetExternalTokenState() const override {
+      if (current_) {
+        return current_->GetExternalTokenState();
+      }
+      return InputInterface::GetExternalTokenState();  // Fallback to base class
+    }
+
+    /// Forward raw wireless-remote byte buffer to the internal Gamepad instance.
+    /// Called by the Unitree SDK callback whenever new joystick data arrives.
+    void UpdateGamepadRemoteData(const uint8_t* buff, size_t size) {
+      if (!gamepad_ || buff == nullptr || size == 0) { return; }
+      size_t copy_size = std::min<size_t>(size, sizeof(gamepad_->gamepad_data.buff));
+      std::memcpy(gamepad_->gamepad_data.buff, buff, copy_size);
+    }
+
+    /// Programmatically switch to a specific interface type.
+    /// Triggers safety reset on all interfaces and prints a log message.
+    void SetActiveInterface(ManagedType t) {
+      for (size_t i = 0; i < order_.size(); ++i) {
+        if (order_[i] == t) {
+          setActiveIndex(static_cast<int>(i));
+          return;
+        }
+      }
+    }
+
+    ManagedType GetActiveInterface() const { return active_; }
+
+  private:
+    /// Instantiate all concrete interfaces and register them in order_.
+    void buildInterfaces() {
+      keyboard_ = std::make_unique<SimpleKeyboard>();
+      order_.push_back(ManagedType::KEYBOARD);
+
+      gamepad_ = std::make_unique<unitree::common::Gamepad>();
+      order_.push_back(ManagedType::GAMEPAD);
+
+      zmq_ = std::make_unique<ZMQEndpointInterface>(
+        zmq_host_, zmq_port_, zmq_topic_, zmq_conflate_, zmq_verbose_
+      );
+      order_.push_back(ManagedType::ZMQ);
+
+#if HAS_ROS2
+      ros2_ = std::make_unique<ROS2InputHandler>(true, "g1_deploy_ros2_handler");
+      order_.push_back(ManagedType::ROS2);
+#endif
+    }
+
+    /// Set the active interface by numeric index (wraps around).
+    /// Triggers safety reset on ALL interfaces to prevent stale state.
+    void setActiveIndex(int idx) {
+      if (order_.empty()) { return; }
+      if (idx < 0) { idx = static_cast<int>(order_.size()) - 1; }
+      if (idx >= static_cast<int>(order_.size())) { idx = 0; }
+      
+      // Trigger safety reset on ALL interfaces when switching
+      keyboard_->TriggerSafetyReset();
+      gamepad_->TriggerSafetyReset();
+      zmq_->TriggerSafetyReset();
+#if HAS_ROS2
+      if (ros2_) ros2_->TriggerSafetyReset();
+#endif
+
+      active_index_ = idx;
+      active_ = order_[static_cast<size_t>(active_index_)];
+
+      switch (active_) {
+        case ManagedType::KEYBOARD:
+          current_ = keyboard_.get();
+          type_ = InputType::KEYBOARD;
+          std::cout << "[InterfaceManager] Switched to: KEYBOARD (safety reset triggered)" << std::endl;
+          break;
+        case ManagedType::GAMEPAD:
+          current_ = gamepad_.get();
+          type_ = InputType::GAMEPAD;
+          std::cout << "[InterfaceManager] Switched to: GAMEPAD (safety reset triggered)" << std::endl;
+          break;
+        case ManagedType::ZMQ:
+          current_ = zmq_.get();
+          type_ = InputType::NETWORK;
+          std::cout << "[InterfaceManager] Switched to: ZMQ (safety reset triggered)" << std::endl;
+          break;
+        case ManagedType::ROS2:
+#if HAS_ROS2
+          current_ = ros2_.get();
+          type_ = InputType::ROS2;
+          std::cout << "[InterfaceManager] Switched to: ROS2 (safety reset triggered)" << std::endl;
+          break;
+#else
+          // Should never happen when ROS2 disabled; fall back to keyboard
+          current_ = keyboard_.get();
+          type_ = InputType::KEYBOARD;
+          active_ = ManagedType::KEYBOARD;
+          std::cout << "[InterfaceManager] ROS2 not available. Falling back to KEYBOARD (safety reset triggered)" << std::endl;
+          break;
+#endif
+      }
+    }
+
+    void nextInterface() { setActiveIndex(active_index_ + 1); }  ///< Cycle forward.
+    void prevInterface() { setActiveIndex(active_index_ - 1); }  ///< Cycle backward.
+
+  private:
+    // ------------------------------------------------------------------
+    // Owned concrete delegates (kept alive to preserve state across switches)
+    // ------------------------------------------------------------------
+    std::unique_ptr<SimpleKeyboard> keyboard_;              ///< Keyboard handler.
+    std::unique_ptr<unitree::common::Gamepad> gamepad_;     ///< Gamepad handler.
+    std::unique_ptr<ZMQEndpointInterface> zmq_;             ///< ZMQ streaming handler.
+#if HAS_ROS2
+    std::unique_ptr<ROS2InputHandler> ros2_;                ///< ROS 2 teleop handler.
+#endif
+
+    InputInterface* current_ = nullptr;  ///< Non-owning pointer to the active delegate.
+
+    // ------------------------------------------------------------------
+    // Active-selection bookkeeping
+    // ------------------------------------------------------------------
+    std::vector<ManagedType> order_;         ///< Insertion-order of managed types.
+    int active_index_ = 0;                   ///< Index into order_.
+    ManagedType active_ = ManagedType::KEYBOARD;  ///< Currently-active type tag.
+
+    // ZMQ configuration (stored for deferred construction)
+    std::string zmq_host_;
+    int zmq_port_;
+    std::string zmq_topic_;
+    bool zmq_conflate_ = false;
+    bool zmq_verbose_ = false;
+    
+    /// Global emergency-stop flag, set by 'O'/'o' key.
+    /// Applies to ALL interfaces (especially useful when gamepad is active
+    /// and has no physical stop button readily accessible).
+    bool emergency_stop_ = false;
+    bool report_temperature_flag_ = false;
+};
+
+#endif // INTERFACE_MANAGER_HPP
+
+

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/keyboard_handler.hpp

@@ -0,0 +1,701 @@
+/**
+ * @file keyboard_handler.hpp
+ * @brief Terminal-based keyboard input handler for the G1 robot.
+ *
+ * SimpleKeyboard reads single characters from stdin (set to non-blocking,
+ * non-canonical mode) and translates them into per-frame action flags.
+ *
+ * ## Two Operational Modes
+ *
+ * ### Reference-motion mode (default)
+ *   Key | Action
+ *   ----|-------
+ *   P/p | Previous motion
+ *   N/n | Next motion
+ *   T/t | Play / resume playback
+ *   R/r | Restart motion (frame 0, paused)
+ *   ]   | Start control system
+ *   O/o | Emergency stop
+ *   Q/q | Delta heading left  (−π/12)
+ *   E/e | Delta heading right (+π/12)
+ *   I/i | Reinitialise heading (recapture IMU)
+ *   Enter | Toggle planner mode
+ *   Z/z | Toggle encoder mode
+ *
+ * ### Planner mode (Enter to toggle)
+ *   Key | Action
+ *   ----|-------
+ *   W/S | Move forward / backward
+ *   A/D | Adjust-left / adjust-right (slight turn + forward)
+ *   ,/. | Strafe left / right
+ *   Q/E | Heading left / right (±π/6)
+ *   1-8 | Select locomotion mode from current motion set
+ *   N/P | Next / previous motion set
+ *   9/0 | Decrease / increase speed
+ *   -/= | Decrease / increase height
+ *   R/` | Emergency stop (reset momentum)
+ *   T/t | Play motion
+ *   Z/z | Toggle encoder mode
+ *
+ * Movement uses a momentum system: pressing a direction key sets momentum to
+ * 1.0; each frame without input decays it by `momentum_decay_rate`.  Below
+ * `momentum_threshold` the robot transitions to IDLE (or stays in the current
+ * static pose for squat / boxing sets).
+ */
+
+#ifndef KEYBOARD_HANDLER_HPP
+#define KEYBOARD_HANDLER_HPP
+
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <iostream>
+#include <array>
+#include <cmath>
+#include <thread>
+#include <chrono>
+#include <cstdlib>
+#include "input_interface.hpp"
+
+/**
+ * @class SimpleKeyboard
+ * @brief stdin-based InputInterface for interactive control from a terminal.
+ */
+class SimpleKeyboard : public InputInterface {
+  public:
+    // ------------------------------------------------------------------
+    // Per-frame action flags  (reset at the start of every update() call)
+    // ------------------------------------------------------------------
+    bool motion_prev = false;      ///< Switch to previous pre-loaded motion.
+    bool motion_next = false;      ///< Switch to next pre-loaded motion.
+    bool play_motion = false;      ///< Start / resume motion playback.
+    bool motion_restart = false;   ///< Reset current motion to frame 0 (paused).
+
+    bool start_control = false;    ///< Request control-system start.
+    bool stop_control = false;     ///< Request emergency stop.
+
+    bool delta_left = false;       ///< Nudge heading left  (−π/12 rad per press).
+    bool delta_right = false;      ///< Nudge heading right (+π/12 rad per press).
+
+    bool reinitialize = false;     ///< Recapture the IMU base quaternion.
+
+    // ------------------------------------------------------------------
+    // Planner-mode state
+    // ------------------------------------------------------------------
+    bool use_planner = false;      ///< True while planner mode is active.
+
+    /// Index into the predefined motion-set table
+    /// (0 = standing, 1 = squat, 2 = boxing, 3 = styled walk).
+    int motion_set_index = 0;
+    /// Locomotion modes available in the current motion set.
+    std::vector<LocomotionMode> current_motion_set = get_motion_set(motion_set_index);
+
+    // Directional movement flags (per-frame, planner mode only)
+    bool planner_move_forward = false;     ///< W key pressed this frame.
+    bool planner_move_backward = false;    ///< S key pressed this frame.
+    bool planner_move_adj_left = false;    ///< A key – slight left turn + forward.
+    bool planner_move_adj_right = false;   ///< D key – slight right turn + forward.
+    bool planner_move_left = false;        ///< ',' key – strafe left.
+    bool planner_move_right = false;       ///< '.' key – strafe right.
+    bool planner_heading_left = false;     ///< E key – heading left (−π/6).
+    bool planner_heading_right = false;    ///< Q key – heading right (+π/6).
+
+    LocomotionMode planner_use_movement_mode = LocomotionMode::IDLE;  ///< Current locomotion mode.
+    double planner_use_movement_speed = -1;  ///< Desired speed (−1 = mode default).
+    double planner_use_height = -1;          ///< Desired body height (−1 = mode default).
+    bool planner_emergency_stop = false;     ///< Immediate halt flag (R / ` key).
+
+    bool encoder_mode_toggle = false;  ///< Toggle encoder-mode (Z key) this frame.
+    bool report_temperature = false;   ///< Print motor temperatures (F key) this frame.
+
+    // Persistent planner state
+    double planner_facing_angle = 0.0;   ///< Accumulated facing direction (radians).
+
+    /// Movement momentum (0 = stopped, 1 = full speed).
+    /// Decays each frame by `momentum_decay_rate` when no movement key is held.
+    double movement_momentum = 0.0;
+    const double momentum_decay_rate = 0.999;  ///< Per-frame multiplicative decay.
+    const double momentum_threshold = 0.1;     ///< Below this, transition to IDLE.
+
+    /**
+     * @brief Construct the keyboard handler.
+     *
+     * Puts stdin into non-canonical, non-echo, non-blocking mode so that
+     * individual key-presses can be read without waiting for Enter.
+     * The original terminal settings are restored in the destructor.
+     */
+    explicit SimpleKeyboard() : InputInterface() {
+      tcgetattr(STDIN_FILENO, &old_termios_);
+      struct termios new_termios = old_termios_;
+      new_termios.c_lflag &= ~(ICANON | ECHO);  // Disable line buffering and echo
+      tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
+      fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);  // Non-blocking reads
+      type_ = InputType::KEYBOARD;
+    }
+
+    /// Restore original terminal settings on destruction.
+    ~SimpleKeyboard() {
+      tcsetattr(STDIN_FILENO, TCSANOW, &old_termios_);
+    }
+
+    // Flag to trigger safety reset in handle_input
+    bool trigger_safety_reset = false;
+
+    // Override the update function from InputInterface
+    void update() override {
+      // Check for safety reset trigger from manager
+      if (CheckAndClearSafetyReset()) {
+        use_planner = false;
+        trigger_safety_reset = true;
+        std::cout << "[SimpleKeyboard] Safety reset triggered: will disable planner and return to reference motion" << std::endl;
+      }
+
+      // Reset input flags each frame
+      start_control = false;
+      stop_control = false;
+      motion_prev = false;
+      motion_next = false;
+      play_motion = false;
+      motion_restart = false;
+      delta_left = false;
+      delta_right = false;
+      reinitialize = false;
+      planner_move_forward = false;
+      planner_move_backward = false;
+      planner_move_adj_left = false;
+      planner_move_adj_right = false;
+      planner_move_left = false;
+      planner_move_right = false;
+      planner_heading_left = false;
+      planner_heading_right = false;
+      planner_emergency_stop = false;
+      encoder_mode_toggle = false;
+      report_temperature = false;
+
+      // Read keyboard input (using shared buffered reading)
+      char ch;
+      while (ReadStdinChar(ch)) {
+        if (use_planner) {
+            switch (ch) {
+                case 'r':
+                case 'R':
+                case '`':
+                case '~': planner_emergency_stop = true; break; // Emergency stop - immediate halt
+                case '1':
+                case '!': if (current_motion_set.size() > 0) planner_use_movement_mode = current_motion_set[0]; break; // Use standing motion set
+                case '2':
+                case '@': if (current_motion_set.size() > 1) planner_use_movement_mode = current_motion_set[1]; break; // Use squat motion set
+                case '3':
+                case '#': if (current_motion_set.size() > 2) planner_use_movement_mode = current_motion_set[2]; break; // Use boxing motion set
+                case '4':
+                case '$': if (current_motion_set.size() > 3) planner_use_movement_mode = current_motion_set[3]; break; // Use boxing or idele squat mode
+                case '5':
+                case '%': if (current_motion_set.size() > 4) planner_use_movement_mode = current_motion_set[4]; break; // Use kneel two legs mode
+                case '6':
+                case '^': if (current_motion_set.size() > 5) planner_use_movement_mode = current_motion_set[5]; break; // Use kneel mode
+                case '7':
+                case '&': if (current_motion_set.size() > 6) planner_use_movement_mode = current_motion_set[6]; break; // Use lying face down mode
+                case '8':
+                case '*': if (current_motion_set.size() > 7) planner_use_movement_mode = current_motion_set[7]; break; // Use crawling mode
+                case '-':
+                case '_': if(planner_use_height != -1) { planner_use_height = planner_use_height - 0.1; } break; // Delta height down
+                case '=':
+                case '+': if(planner_use_height != -1) { planner_use_height = planner_use_height + 0.1; } break; // Delta height up
+                case '9':
+                case '(': if(planner_use_movement_speed != -1) { planner_use_movement_speed = planner_use_movement_speed - 0.1; } break; // Delta speed up
+                case '0':
+                case ')': if(planner_use_movement_speed != -1) { planner_use_movement_speed = planner_use_movement_speed + 0.1; } break; // Delta speed down
+                case 'n':
+                case 'N': motion_set_index = (motion_set_index + 1) % 4; 
+                          current_motion_set = get_motion_set(motion_set_index); 
+                          planner_use_movement_mode = current_motion_set[0]; 
+                          std::cout << "Motion set: " << motion_set_index << std::endl;
+                          if (motion_set_index == 1) {
+                            planner_use_height = 0.8;
+                          }
+                          break; // Next motion set
+                case 'p':
+                case 'P': motion_set_index = (motion_set_index - 1 + 4) % 4; 
+                          current_motion_set = get_motion_set(motion_set_index); 
+                          planner_use_movement_mode = current_motion_set[0]; 
+                          std::cout << "Motion set: " << motion_set_index << std::endl;
+                          if (motion_set_index == 1) {
+                            planner_use_height = 0.8;
+                          }
+                          break; // Previous motion set
+                case 'j':
+                case 'J': delta_left = true; break; // Delta heading left
+                case 'l':
+                case 'L': delta_right = true; break; // Delta heading right
+                case 'w':
+                case 'W': planner_move_forward = true; break; // Move forward
+                case 's':
+                case 'S': planner_move_backward = true; break; // Move backward
+                case ',':
+                case '<': planner_move_left = true; break; // Move left
+                case '.': 
+                case '>': planner_move_right = true; break; // Move right
+                case 'a':
+                case 'A': planner_move_adj_left = true; break; // Move adj left
+                case 'd':
+                case 'D': planner_move_adj_right = true; break; // Move adj right
+                case 'e':
+                case 'E': planner_heading_left = true; break; // Delta heading left (-0.1)
+                case 'q':
+                case 'Q': planner_heading_right = true; break; // Delta heading right (+0.1)
+                case ']': start_control = true; break; // Start control system
+                case 'o':
+                case 'O': stop_control = true; break; // Stop/Exit
+                case '\n': use_planner = !use_planner; break; // Use planner
+                case 'i':
+                case 'I': reinitialize = true; break; // Reinitialize base quaternion and delta heading
+                case 't':
+                case 'T': play_motion = true; break; // Play motion to end
+                case 'z':
+                case 'Z': encoder_mode_toggle = true; break; // Toggle encoder mode
+                case 'f':
+                case 'F': report_temperature = true; break; // Report motor temperatures
+            }
+
+            // Limit movement speed and height to the range of the movement mode
+            if (is_standing_motion_mode(planner_use_movement_mode)) {
+              planner_use_height = -1.0;
+            } else {
+              planner_use_height = std::max(planner_use_height, 0.2);
+              planner_use_height = std::min(planner_use_height, 0.8);
+            }
+
+            if (is_static_motion_mode(planner_use_movement_mode)) {
+              planner_use_movement_speed = -1.0;
+            } else {
+              if (planner_use_movement_mode == LocomotionMode::SLOW_WALK) {
+                planner_use_movement_speed = std::max(planner_use_movement_speed, 0.2);
+                planner_use_movement_speed = std::min(planner_use_movement_speed, 0.8);
+              } else if (planner_use_movement_mode == LocomotionMode::RUN) {
+                planner_use_movement_speed = std::max(planner_use_movement_speed, 1.5);
+                planner_use_movement_speed = std::min(planner_use_movement_speed, 3.0);
+              } else if (planner_use_movement_mode == LocomotionMode::CRAWLING) {
+                planner_use_movement_speed = std::max(planner_use_movement_speed, 0.4);
+                planner_use_movement_speed = std::min(planner_use_movement_speed, 1.0);
+              } else if (planner_use_movement_mode == LocomotionMode::ELBOW_CRAWLING) {
+                planner_use_movement_speed = std::max(planner_use_movement_speed, 0.7);
+                planner_use_movement_speed = std::min(planner_use_movement_speed, 1.0);
+              } else if (planner_use_movement_mode == LocomotionMode::WALK_BOXING || 
+                          planner_use_movement_mode == LocomotionMode::LEFT_PUNCH || 
+                          planner_use_movement_mode == LocomotionMode::RIGHT_PUNCH || 
+                          planner_use_movement_mode == LocomotionMode::RANDOM_PUNCH ||
+                          planner_use_movement_mode == LocomotionMode::LEFT_HOOK ||
+                          planner_use_movement_mode == LocomotionMode::RIGHT_HOOK) {
+                planner_use_movement_speed = std::max(planner_use_movement_speed, 0.7);
+                planner_use_movement_speed = std::min(planner_use_movement_speed, 1.5);
+              }
+              else {
+                planner_use_movement_speed = -1;
+              }
+            }
+        } else {
+          switch (ch) {
+            case 'p':
+            case 'P': motion_prev = true; break; // Previous motion
+            case 'n':
+            case 'N': motion_next = true; break; // Next motion
+            case 't':
+            case 'T': play_motion = true; break; // Play motion to end
+            case 'r':
+            case 'R': motion_restart = true; break; // Restart motion
+            case ']': start_control = true; break; // Start control system
+            case 'o':
+            case 'O': stop_control = true; break; // Stop/Exit
+            case 'q':
+            case 'Q': delta_left = true; break; // Delta heading left (-0.1)
+            case 'e':
+            case 'E': delta_right = true; break; // Delta heading right (+0.1)
+            case 'i':
+            case 'I': reinitialize = true; break; // Reinitialize base quaternion and delta heading
+            case '\n': use_planner = !use_planner; break; // Use planner
+            case 'z':
+            case 'Z': encoder_mode_toggle = true; break; // Toggle encoder mode
+            case 'h':
+            case 'H': report_temperature = true; break; // Report motor temperatures
+          }
+        }
+        
+      }
+    }
+
+    // Override the handle_input function from InputInterface
+    // This processes the keyboard input flags and performs actions using the provided parameters
+    void handle_input(MotionDataReader& motion_reader,
+                     std::shared_ptr<const MotionSequence>& current_motion,
+                     int& current_frame,
+                     OperatorState& operator_state,
+                     bool& reinitialize_heading,
+                     DataBuffer<HeadingState>& heading_state_buffer,
+                     bool has_planner,
+                     PlannerState& planner_state,
+                     DataBuffer<MovementState>& movement_state_buffer,
+                     std::mutex& current_motion_mutex,
+                     bool& report_temperature) override {
+      
+
+      
+      // Handle safety reset from interface manager
+      if (trigger_safety_reset) {
+        trigger_safety_reset = false;
+        movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+          reinitialize_heading = true;
+          auto temp_motion = std::make_shared<MotionSequence>(*current_motion);
+          temp_motion->name = "temporary_motion";
+          current_motion = temp_motion;
+          if (has_planner && planner_state.enabled) {
+            planner_state.enabled = false;
+            planner_state.initialized = false;
+            std::cout << "Safety reset: Planner disabled" << std::endl;
+          }
+        }
+
+        std::cout << "Safety reset: Returned to reference motion at frame 0" << std::endl;
+      }
+
+      // Handle encoder mode toggle
+      if (encoder_mode_toggle) {
+        int current_encoder_mode = current_motion->GetEncodeMode();
+        if (current_encoder_mode == -2) {
+          std::cout << "⚠ No encoder configured - cannot toggle encoder mode" << std::endl;
+        } else if (current_encoder_mode == -1) {
+          std::cout << "⚠ No encoder loaded - cannot toggle encoder mode" << std::endl;
+        } else {
+          int new_encoder_mode = (current_encoder_mode == 0) ? 1 : 0;  // Toggle between mode 0 and 1
+          std::cout << "Encoder mode: " << (new_encoder_mode == 0 ? "mode 0" : "mode 1") << std::endl;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            current_motion->SetEncodeMode(new_encoder_mode);
+          }
+        }
+      }
+
+      // Handle motion control commands
+      if (this->motion_prev && !motion_reader.motions.empty()) {
+          motion_reader.current_motion_index_ =
+              (motion_reader.current_motion_index_ - 1 + motion_reader.motions.size()) % motion_reader.motions.size();
+          std::string motion_name;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+            current_frame = 0;
+            motion_name = current_motion->name;
+            reinitialize_heading = true;
+          }
+      }
+
+      if (this->motion_next && !motion_reader.motions.empty()) {
+          motion_reader.current_motion_index_ = (motion_reader.current_motion_index_ + 1) % motion_reader.motions.size();
+          std::string motion_name;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+            current_frame = 0;
+            motion_name = current_motion->name;
+            reinitialize_heading = true;
+          }
+      }
+
+      if (this->play_motion) {
+          if (!operator_state.play) {
+              int frame_copy;
+              size_t timesteps_copy;
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = true;
+                frame_copy = current_frame;
+                timesteps_copy = current_motion ? current_motion->timesteps : 0;
+              }
+              std::cout << "Playing motion " << motion_reader.current_motion_index_ << " from frame " << frame_copy << " to end ("
+                        << timesteps_copy << " total frames)" << std::endl;
+          }
+      }
+
+      if (this->motion_restart) {
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+            current_frame = 0;
+            reinitialize_heading = true;
+          }
+          std::cout << "Reset motion " << motion_reader.current_motion_index_ << " to frame 0 (paused)" << std::endl;
+      }
+
+      if (this->stop_control) { operator_state.stop = true; }
+
+      if (this->report_temperature) { report_temperature = true; }
+
+      if (this->start_control) { operator_state.start = true; }
+
+      // Handle delta heading controls
+      if (this->delta_left) {
+          auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+          HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+          double new_delta = current_state.delta_heading + M_PI / 12;
+          heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+          std::cout << "Delta heading left: " << new_delta << " rad" << std::endl;
+      }
+
+      if (this->delta_right) {
+          auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+          HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+          double new_delta = current_state.delta_heading - M_PI / 12;
+          heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+          std::cout << "Delta heading right: " << new_delta << " rad" << std::endl;
+      }
+
+      // Handle reinitialize command
+      if (this->reinitialize) {
+        std::lock_guard<std::mutex> lock(current_motion_mutex);
+        reinitialize_heading = true;
+      }
+
+      // Handle planner control - copy the toggle state from keyboard
+      if (this->use_planner && !has_planner) {
+        std::cout << "Planner not loaded - cannot enable" << std::endl;
+        this->use_planner = false;
+        movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+          current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+          current_frame = 0;
+        }
+      } else if (has_planner && planner_state.enabled != this->use_planner) {
+        planner_state.enabled = this->use_planner;
+        if (planner_state.enabled) {
+          std::cout << "Planner enabled" << std::endl;
+          planner_facing_angle = 0.0;
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = false;
+          }
+          // Wait for planner to be initialized with timeout (5 seconds)
+          auto wait_start = std::chrono::steady_clock::now();
+          constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+          while (planner_state.enabled) {
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              if (current_motion->name == "planner_motion") {
+                std::cout << "[Keyboard] motion name is planner_motion" << std::endl;
+                break;
+              }
+            }
+              std::this_thread::sleep_for(std::chrono::milliseconds(100));
+              auto elapsed = std::chrono::steady_clock::now() - wait_start;
+              if (elapsed > PLANNER_INIT_TIMEOUT) {
+                  std::cerr << "[Keyboard] Planner initialization timeout after 5 seconds" << std::endl;
+                  this->use_planner = false;
+                  movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+                  {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    operator_state.play = false;
+                    current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+                    current_frame = 0;
+                  }
+                  break;
+              }
+              std::cout << "[Keyboard] Waiting for planner to be initialized" << std::endl;
+          }
+          // Check if planner is enabled and initialized
+          if (!planner_state.enabled || !planner_state.initialized) {
+              std::cout << "[Keyboard] Planner failed to initialize." << std::endl;
+              this->use_planner = false;
+              movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+                current_frame = 0;
+              }
+          } else {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            operator_state.play = true;
+          }
+        } else {
+            std::cout << "Planner disabled" << std::endl;
+            planner_state.initialized = false; // Reset planner initialization when disabled
+            movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+            movement_momentum = 0.0;
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              operator_state.play = false;
+              reinitialize_heading = true;
+              current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);  // Update current motion directly
+              current_frame = 0;
+            }
+        }
+      }
+
+      if (has_planner && planner_state.enabled && planner_state.initialized) {
+          // Handle emergency stop (immediate momentum reset)
+          if (this->planner_emergency_stop) {
+              movement_momentum = 0.0;
+              std::cout << "Emergency Stop! Movement momentum reset." << std::endl;
+          }
+          
+          // Get current state from buffer (single source of truth)
+          auto current_mode_data = movement_state_buffer.GetDataWithTime();
+          int current_mode = current_mode_data.data ? current_mode_data.data->locomotion_mode : static_cast<int>(LocomotionMode::IDLE);
+          std::array<double, 3> current_movement = current_mode_data.data ? current_mode_data.data->movement_direction : std::array<double, 3>{0.0, 0.0, 0.0};
+          std::array<double, 3> current_facing = current_mode_data.data ? current_mode_data.data->facing_direction : std::array<double, 3>{1.0, 0.0, 0.0};
+          
+          // Use local variables for all processing - avoid updating globals until the end
+          std::array<double, 3> local_target_movement = current_movement;
+          std::array<double, 3> local_facing_direction = current_facing;
+          
+          // Handle heading controls (independent of movement momentum)
+          if (this->planner_heading_left) {
+              planner_facing_angle -= M_PI / 6;
+              local_facing_direction[0] = cos(planner_facing_angle);
+              local_facing_direction[1] = sin(planner_facing_angle);
+          }
+          if (this->planner_heading_right) {
+              planner_facing_angle += M_PI / 6;
+              local_facing_direction[0] = cos(planner_facing_angle);
+              local_facing_direction[1] = sin(planner_facing_angle);
+          }
+          bool movement_key_pressed = false;
+          
+          if (this->planner_move_forward && !is_static_motion_mode(planner_use_movement_mode)) {
+              // Just indicate we want to move - actual mode will be determined later based on momentum
+              local_target_movement[0] = local_facing_direction[0];
+              local_target_movement[1] = local_facing_direction[1];
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          if (this->planner_move_backward && !is_static_motion_mode(planner_use_movement_mode)) {
+              local_target_movement[0] = -local_facing_direction[0];
+              local_target_movement[1] = -local_facing_direction[1];
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          if (this->planner_move_adj_left && !is_static_motion_mode(planner_use_movement_mode)) {
+              planner_facing_angle += 0.1;
+              local_facing_direction[0] = cos(planner_facing_angle);
+              local_facing_direction[1] = sin(planner_facing_angle);
+              local_facing_direction[2] = 0.0f;
+              local_target_movement[0] = local_facing_direction[0];
+              local_target_movement[1] = local_facing_direction[1];
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          if (this->planner_move_adj_right && !is_static_motion_mode(planner_use_movement_mode)) {
+              planner_facing_angle -= 0.1;
+              local_facing_direction[0] = cos(planner_facing_angle);
+              local_facing_direction[1] = sin(planner_facing_angle);
+              local_facing_direction[2] = 0.0f;
+              local_target_movement[0] = local_facing_direction[0];
+              local_target_movement[1] = local_facing_direction[1];
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          if (this->planner_move_left && !is_static_motion_mode(planner_use_movement_mode)) {
+              local_target_movement[0] = -sin(planner_facing_angle);
+              local_target_movement[1] = cos(planner_facing_angle);
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          if (this->planner_move_right && !is_static_motion_mode(planner_use_movement_mode)) {
+              local_target_movement[0] = sin(planner_facing_angle);
+              local_target_movement[1] = -cos(planner_facing_angle);
+              local_target_movement[2] = 0.0f;
+              movement_momentum = 1.0;  // Full momentum
+              movement_key_pressed = true;
+          }
+          
+          // Handle mode changes - just update the mode, keep momentum
+          // No need to reset momentum when switching between walk/run modes
+          
+          // Apply momentum decay if no movement key is pressed
+          if (!movement_key_pressed) {
+              movement_momentum *= momentum_decay_rate;
+          }
+          
+          // Set final movement values based on momentum
+          int final_mode;
+          std::array<double, 3> final_movement;
+          double final_speed;
+          double final_height;
+          if (movement_momentum > momentum_threshold) {
+              // Determine final mode based on keyboard setting only when we have momentum
+              final_mode = static_cast<int>(this->planner_use_movement_mode);
+              final_movement = local_target_movement;
+              final_speed = this->planner_use_movement_speed;
+              final_height = this->planner_use_height;
+          } else {
+            if (motion_set_index == 1) {
+              // Below threshold - maintain current squat mode
+              final_mode = static_cast<int>(this->planner_use_movement_mode);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = -1.0f;
+              final_height = this->planner_use_height;
+              if (this->planner_use_movement_mode == LocomotionMode::CRAWLING) {
+                final_speed = 0;
+              } else if (this->planner_use_movement_mode == LocomotionMode::ELBOW_CRAWLING) {
+                final_speed = 0;
+              }
+            } else if (motion_set_index == 2) {
+              // Below threshold - maintain current boxing mode
+              final_mode = static_cast<int>(this->planner_use_movement_mode);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = -1.0f;
+              final_height = -1.0f;
+              if (this->planner_use_movement_mode == LocomotionMode::WALK_BOXING) {
+                final_speed = 0;
+              } else if (this->planner_use_movement_mode == LocomotionMode::LEFT_PUNCH) {
+                final_speed = 0;
+                final_movement = local_facing_direction;
+              } else if (this->planner_use_movement_mode == LocomotionMode::RIGHT_PUNCH) {
+                final_speed = 0;
+                final_movement = local_facing_direction;
+              } else if (this->planner_use_movement_mode == LocomotionMode::RANDOM_PUNCH) {
+                final_speed = 0;
+              } else if (this->planner_use_movement_mode == LocomotionMode::LEFT_HOOK) {
+                final_speed = 0;
+                final_movement = local_facing_direction;
+              } else if (this->planner_use_movement_mode == LocomotionMode::RIGHT_HOOK) {
+                final_speed = 0;
+                final_movement = local_facing_direction;
+              }
+            } else if (motion_set_index == 3) {
+              // Below threshold - maintain current styled walking mode
+              final_mode = static_cast<int>(this->planner_use_movement_mode);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = 0.0f;
+              final_height = -1.0f;
+            } else {
+              // Below threshold - switch to IDLE
+              final_mode = static_cast<int>(LocomotionMode::IDLE);
+              final_movement = {0.0f, 0.0f, 0.0f};
+              final_speed = -1.0f;
+              final_height = -1.0f;
+            }
+          }
+         
+          // Update thread-safe buffer (single source of truth for planner thread)
+          MovementState mode_state(final_mode, final_movement, local_facing_direction, final_speed, final_height);
+          movement_state_buffer.SetData(mode_state);
+      }
+    }
+
+
+  private:
+    struct termios old_termios_;
+};
+
+#endif // KEYBOARD_HANDLER_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/ros2_input_handler.hpp

@@ -0,0 +1,1278 @@
+/**
+ * @file ros2_input_handler.hpp
+ * @brief ROS 2 input handler for receiving teleop commands over DDS.
+ *
+ * ROS2InputHandler creates a lightweight ROS 2 node and subscribes to a single
+ * topic (`ControlPolicy/upper_body_pose`) that carries msgpack-serialised
+ * ControlGoalMsg payloads inside a `std_msgs/ByteMultiArray`.
+ *
+ * ## Data Flow
+ *
+ *   Python teleop script  ──(msgpack/ROS2)──►  ROS2InputHandler
+ *       ControlGoalMsg                           ↓
+ *                                           update()  (spin + buffer → local state)
+ *                                           handle_input()  (local state → system state)
+ *
+ * ## Operational Requirements
+ *
+ * - A locomotion **planner must be loaded** – ROS2 mode always operates through
+ *   the planner (no reference-motion playback).
+ * - VR 3-point tracking is always enabled; the handler populates position and
+ *   orientation buffers from the received wrist / head data.
+ * - Supports two IK modes controlled at construction time:
+ *   - `use_ik_mode = true`  – uses IK-processed transformation matrices
+ *     (left_wrist_after_ik, right_wrist_after_ik, head_after_ik) with
+ *     configurable wrist offsets.
+ *   - `use_ik_mode = false` – uses raw wrist matrices (left_wrist, right_wrist).
+ *
+ * ## Locomotion Modes
+ *
+ *   base_height_command | Mode
+ *   --------------------|------
+ *   0.72 – 0.88         | WALK (or SLOW_WALK depending on locomotion_mode flag)
+ *   0.50 – 0.72         | SQUAT (static)
+ *   0.10 – 0.50         | KNEEL (static)
+ *
+ * ## Edge-Triggered Commands
+ *
+ * `toggle_policy_action` is accumulated with OR logic in the callback so that
+ * a transient toggle pulse is never lost between update() cycles.
+ *
+ * ## Thread Safety
+ *
+ * The subscriber callback runs on the ROS 2 executor thread.  Data is copied
+ * into `control_goal_buffer_` under `control_goal_mutex_` and consumed by
+ * update() on the main thread.
+ */
+
+#ifndef ROS2_INPUT_HANDLER_HPP
+#define ROS2_INPUT_HANDLER_HPP
+
+#if HAS_ROS2
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/exceptions/exceptions.hpp>
+#include <std_msgs/msg/byte_multi_array.hpp>  // For msgpack-serialized messages
+#include <array>
+#include <memory>
+#include <atomic>
+#include <mutex>
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <map>
+#include <vector>
+#include <variant>
+#include <cstdlib>
+
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <msgpack.hpp>
+
+#include "input_interface.hpp"
+#include "../math_utils.hpp"
+#include "../policy_parameters.hpp"  // For isaaclab_to_mujoco and default_angles
+
+/**
+ * @brief Deserialized control-goal message received from the Python teleop script.
+ *
+ * Fields are populated by parse_msgpack_control_goal() and stored in the
+ * receiving buffer for consumption by update().
+ */
+struct ControlGoalMsg {
+    /// Navigation velocity command [lin_vel_x, lin_vel_y, ang_vel_z] (m/s, m/s, rad/s).
+    std::array<double, 3> navigate_cmd = {0.0f, 0.0f, 0.0f};
+    /// Wrist pose in [x,y,z, qw,qx,qy,qz] × 2 (left then right).
+    std::array<double, 14> wrist_pose = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
+                                          0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
+    
+    // IK-processed wrist poses (4×4 transformation matrices, row-major flattened)
+    std::array<double, 16> left_wrist_after_ik = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};   ///< Left wrist after IK.
+    std::array<double, 16> right_wrist_after_ik = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};  ///< Right wrist after IK.
+    std::array<double, 16> head_after_ik = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};         ///< Head after IK.
+    bool has_ik_data = false;  ///< True if the IK-processed matrices are present.
+    
+    // Non-IK raw wrist matrices (4×4 transformation matrices, row-major flattened)
+    std::array<double, 16> left_wrist = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};   ///< Raw left wrist transform.
+    std::array<double, 16> right_wrist = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};  ///< Raw right wrist transform.
+    bool has_wrist_matrices = false;  ///< True if the raw wrist matrices are present.
+    
+    double base_height_command = 0.78;        ///< Desired base height (metres, valid range 0.1–0.88).
+    bool toggle_policy_action = false;        ///< Edge-triggered toggle: maps to start/stop control.
+    int locomotion_mode = 0;                  ///< 0 = slow walk (custom speed), 1 = fast walk (default speed).
+    
+    /// Dex3 hand joint positions (7 DOF per hand).
+    std::array<double, 7> left_hand_joint = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    std::array<double, 7> right_hand_joint = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    bool has_hand_joints = false;  ///< True if hand joint data is present.
+    
+    double ros_timestamp = 0.0;  ///< ROS time in seconds (for synchronisation with other components).
+    bool valid = false;           ///< True once the message has been successfully parsed.
+};
+
+/**
+ * @class ROS2InputHandler
+ * @brief InputInterface driven by ROS 2 DDS messages (msgpack-serialised ControlGoalMsg).
+ *
+ * Architecture:
+ *  1. **ROS 2 callback** (`control_goal_callback`) receives ByteMultiArray messages,
+ *     deserialises them via msgpack, and stores the result in `control_goal_buffer_`
+ *     under `control_goal_mutex_`.
+ *  2. **update()** spins the ROS 2 node, reads the buffer, and converts the data
+ *     into local state (VR buffers, navigate_cmd, control flags).
+ *  3. **handle_input()** translates local state into system-state changes
+ *     (planner commands, operator start/stop, movement buffer updates).
+ *
+ * A 1-second timeout (`CONTROL_GOAL_TIMEOUT`) resets the handler if messages
+ * stop arriving, and any ROS 2 errors trigger an immediate emergency stop.
+ */
+class ROS2InputHandler : public InputInterface {
+public:
+    // ========================================
+    // DEBUG CONTROL FLAG
+    // ========================================
+    static constexpr bool DEBUG_LOGGING = true;  // Set to false to disable debug logs
+
+    // Constructor - initializes ROS2 node and subscribers
+    explicit ROS2InputHandler(bool use_ik_mode = true, const std::string& node_name = "g1_input_handler") 
+        : InputInterface() {
+        // Set terminal to non-blocking mode for keyboard input (emergency stop)
+        tcgetattr(STDIN_FILENO, &old_termios_);
+        struct termios new_termios = old_termios_;
+        new_termios.c_lflag &= ~(ICANON | ECHO);
+        tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
+        fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
+        
+        // Initialize ROS2 - this is the only ROS2 component in the system
+        if (!rclcpp::ok()) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 DEBUG] Initializing ROS2" << std::endl;
+            }
+            rclcpp::init(0, nullptr);  // Initialize with no command line arguments
+        }
+        
+        try {
+            // Initialize ROS2 node
+            node_ = rclcpp::Node::make_shared(node_name);
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 DEBUG] ROS2InputHandler node '" << node_name << "' initialized" << std::endl;
+            }
+            
+            // Setup subscriber for control goal topic
+            setup_subscribers();
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 DEBUG] Subscribed to topic: ControlPolicy/upper_body_pose" << std::endl;
+            }
+        } catch (const std::exception& e) {
+            std::cerr << "[ROS2 ERROR] Failed to initialize ROS2InputHandler: " << e.what() << std::endl;
+            throw;
+        }
+        type_ = InputType::ROS2;
+        has_vr_3point_control_ = true;
+        use_ik_mode_ = use_ik_mode;
+    }
+
+    // Destructor
+    ~ROS2InputHandler() {
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ROS2 DEBUG] ROS2InputHandler destructor called" << std::endl;
+        }
+        
+        // Restore terminal before ROS2 cleanup
+        tcsetattr(STDIN_FILENO, TCSANOW, &old_termios_);
+        
+        // CRITICAL: Proper shutdown order to avoid EntityDelegate assertion
+        try {
+            // Step 1: Stop spinning to prevent new callbacks
+            if (node_) {
+                // Ensure no more spin operations happen
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            }
+            
+            // Step 2: Reset subscribers BEFORE resetting node (proper DDS entity order)
+            if (control_goal_sub_) {
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ROS2 DEBUG] Resetting control goal subscriber" << std::endl;
+                }
+                control_goal_sub_.reset();
+            }
+            
+            // Step 3: Allow DDS cleanup time (critical for preventing assertion)
+            std::this_thread::sleep_for(std::chrono::milliseconds(50));
+            
+            // Step 4: Reset node AFTER subscribers are cleaned up
+            if (node_) {
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ROS2 DEBUG] Resetting ROS2 node" << std::endl;
+                }
+                node_.reset();
+            }
+            
+            // Step 5: Only shutdown if we're the last ROS2 component
+            // Note: Be careful about calling shutdown() if other ROS2 components exist
+            if (rclcpp::ok()) {
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ROS2 DEBUG] Shutting down ROS2 context" << std::endl;
+                }
+                rclcpp::shutdown();
+            }
+            
+        } catch (const std::exception& e) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 ERROR] Exception during cleanup: " << e.what() << std::endl;
+            }
+            // Don't rethrow in destructor
+        } catch (...) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 ERROR] Unknown exception during cleanup" << std::endl;
+            }
+        }
+    }
+
+    // Flag to trigger safety reset in handle_input
+    bool trigger_safety_reset = false;
+    
+    // Flag to trigger emergency stop (set internally on ROS2 errors/timeout)
+    bool emergency_stop_ = false;
+
+    // Override the update function from InputInterface
+    // Reads from control goal buffer (updated by callback) and updates local state
+    // 
+    // EDGE-TRIGGERED BEHAVIOR:
+    // Toggle commands like toggle_policy_action are accumulated in the callback using OR logic.
+    // Once a command arrives as 'true', it stays 'true' in the buffer until processed here.
+    // After reading, these commands are cleared from the buffer to prevent re-execution.
+    void update() override {
+        // Reset emergency stop flag each frame
+        emergency_stop_ = false;
+        
+        // Check for safety reset trigger from manager
+        if (CheckAndClearSafetyReset()) {
+            trigger_safety_reset = true;
+            std::cout << "[ROS2InputHandler] Safety reset triggered: will disable planner and return to reference motion" << std::endl;
+        }
+
+        // Check if ROS2 is still healthy
+        if (!rclcpp::ok()) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 ERROR] ROS2 context is not OK - triggering emergency stop" << std::endl;
+            }
+            emergency_stop_ = true;
+            return;  // Skip further processing
+        }
+
+        // First, spin ROS2 node to process any pending messages and trigger callbacks
+        if (node_) {
+            try {
+                rclcpp::spin_some(node_);
+            } catch (const rclcpp::exceptions::RCLError& e) {
+                std::cerr << "[ROS2 ERROR] RCL error during spin: " << e.what() << " - triggering emergency stop" << std::endl;
+                emergency_stop_ = true;
+                return;  // Skip further processing
+            } catch (const std::exception& e) {
+                std::cerr << "[ROS2 ERROR] Failed to spin node: " << e.what() << " - triggering emergency stop" << std::endl;
+                emergency_stop_ = true;
+                return;  // Skip further processing
+            }
+        }
+        
+        // Reset input flags each frame
+        start_control_ = false;
+        stop_control_ = false;
+        report_temperature_flag_ = false;
+
+        // Read keyboard input for emergency stop ('O'/'o' key)
+        // This works in standalone mode; when managed by InterfaceManager, it's also handled there
+        char ch;
+        while (ReadStdinChar(ch)) {
+            switch (ch) {
+                case 'o':
+                case 'O':
+                    stop_control_ = true;
+                    std::cout << "[ROS2] Emergency stop triggered (O/o key pressed)" << std::endl;
+                    break;
+                case 'f':
+                case 'F':
+                    report_temperature_flag_ = true;
+                    break;
+            }
+        }
+        
+        // Check for control goal timeout (using steady_clock for monotonic timing)
+        if (received_control_goal_.load()) {
+            int64_t current_time_ns = std::chrono::steady_clock::now().time_since_epoch().count();
+            int64_t last_msg_time_ns = last_control_goal_time_ns_.load();
+            double time_since_last_msg = (current_time_ns - last_msg_time_ns) / 1e9;  // Convert ns to seconds
+            if (time_since_last_msg > CONTROL_GOAL_TIMEOUT) {
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ROS2 WARNING] Control goal timeout (" << time_since_last_msg 
+                              << "s since last message). Resetting flags." << std::endl;
+                }
+                reset_data_flags();  // Reset both flag and timestamp consistently
+                use_teleop_navigate_cmd_ = false;
+                planner_facing_angle_ = 0.0; // Reset facing angle
+            }
+        }
+
+        // Read from control goal buffer (teleop commands from Python) - thread-safe
+        if (received_control_goal_.load()) {
+            std::lock_guard<std::mutex> lock(control_goal_mutex_);
+            // Update navigate_cmd and base_height_command from control goal
+            navigate_cmd_from_teleop_ = control_goal_buffer_.navigate_cmd;
+            base_height_command_ = control_goal_buffer_.base_height_command;
+            use_teleop_navigate_cmd_ = true;
+            
+            // Handle toggle_policy_action (edge-triggered toggle between start/stop)
+            if (control_goal_buffer_.toggle_policy_action) {
+                // Toggle the control state
+                control_is_active_ = !control_is_active_;
+                
+                if (control_is_active_) {
+                    start_control_ = true;
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ROS2 DEBUG] toggle_policy_action: START control" << std::endl;
+                    }
+                } else {
+                    stop_control_ = true;
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ROS2 DEBUG] toggle_policy_action: STOP control" << std::endl;
+                    }
+                }
+                
+                // Clear the trigger after reading (edge-triggered behavior)
+                control_goal_buffer_.toggle_policy_action = false;
+            }
+            
+            // Handle locomotion_mode (direct state: 0 = slow walk, 1 = fast walk)
+            locomotion_mode_is_fast_ = (control_goal_buffer_.locomotion_mode == 1);
+            
+            if constexpr (DEBUG_LOGGING) {
+                static int prev_locomotion_mode = -1;
+                if (prev_locomotion_mode != control_goal_buffer_.locomotion_mode) {
+                    if (locomotion_mode_is_fast_) {
+                        std::cout << "[ROS2 DEBUG] locomotion_mode: FAST WALK (default speed, mode 2)" << std::endl;
+                    } else {
+                        std::cout << "[ROS2 DEBUG] locomotion_mode: SLOW WALK (custom speed, mode 1)" << std::endl;
+                    }
+                    prev_locomotion_mode = control_goal_buffer_.locomotion_mode;
+                }
+            }
+            
+            // Update hand poses from teleop
+            if (control_goal_buffer_.has_hand_joints) {
+                left_hand_joint_.SetData(control_goal_buffer_.left_hand_joint);
+                right_hand_joint_.SetData(control_goal_buffer_.right_hand_joint);
+                has_hand_joints_ = true;
+            }
+            
+            // Update VR 3-point control data based on use_ik_mode_ flag
+            // Build arrays first, then call SetData() on buffers
+            std::array<double, 9> vr_position;
+            std::array<double, 12> vr_orientation;
+            
+            if (use_ik_mode_ && control_goal_buffer_.has_ik_data) {
+                // IK mode: Use IK-processed transformation matrices
+                // Offsets from local_vr_tracking_bm.py (lines 39-41)
+                // NOTE the left and right hand offsets are opposite in sign for the y-axis
+                constexpr std::array<double, 3> LEFT_HAND_OFFSET = {0.18, -0.025, 0.0};
+                constexpr std::array<double, 3> RIGHT_HAND_OFFSET = {0.18, +0.025, 0.0};
+                constexpr std::array<double, 3> HEAD_OFFSET = {0.0, 0.0, 0.35};
+                
+                // Extract left wrist
+                auto left_rot = extract_rotation_from_transform(control_goal_buffer_.left_wrist_after_ik);
+                auto left_pos = extract_position_from_transform(control_goal_buffer_.left_wrist_after_ik);
+                auto left_offset_rotated = apply_rotation_to_offset(left_rot, LEFT_HAND_OFFSET);
+                vr_position[0] = left_pos[0] + left_offset_rotated[0];
+                vr_position[1] = left_pos[1] + left_offset_rotated[1];
+                vr_position[2] = left_pos[2] + left_offset_rotated[2];
+                
+                // Extract right wrist
+                auto right_rot = extract_rotation_from_transform(control_goal_buffer_.right_wrist_after_ik);
+                auto right_pos = extract_position_from_transform(control_goal_buffer_.right_wrist_after_ik);
+                auto right_offset_rotated = apply_rotation_to_offset(right_rot, RIGHT_HAND_OFFSET);
+                vr_position[3] = right_pos[0] + right_offset_rotated[0];
+                vr_position[4] = right_pos[1] + right_offset_rotated[1];
+                vr_position[5] = right_pos[2] + right_offset_rotated[2];
+                
+                // Extract head
+                auto head_rot = extract_rotation_from_transform(control_goal_buffer_.head_after_ik);
+                auto head_pos = extract_position_from_transform(control_goal_buffer_.head_after_ik);
+                auto head_offset_rotated = apply_rotation_to_offset(head_rot, HEAD_OFFSET);
+                vr_position[6] = head_pos[0] + head_offset_rotated[0];
+                vr_position[7] = head_pos[1] + head_offset_rotated[1];
+                vr_position[8] = head_pos[2] + head_offset_rotated[2];
+                
+                // Convert rotation matrices to quaternions using math_utils.hpp
+                auto left_quat = rotation_matrix_to_quat_d(left_rot);
+                auto right_quat = rotation_matrix_to_quat_d(right_rot);
+                auto head_quat = rotation_matrix_to_quat_d(head_rot);
+                
+                // Store quaternions (w, x, y, z format)
+                vr_orientation[0] = left_quat[0];   // left qw
+                vr_orientation[1] = left_quat[1];   // left qx
+                vr_orientation[2] = left_quat[2];   // left qy
+                vr_orientation[3] = left_quat[3];   // left qz
+                vr_orientation[4] = right_quat[0];  // right qw
+                vr_orientation[5] = right_quat[1];  // right qx
+                vr_orientation[6] = right_quat[2];  // right qy
+                vr_orientation[7] = right_quat[3];  // right qz
+                vr_orientation[8] = head_quat[0];   // head qw
+                vr_orientation[9] = head_quat[1];   // head qx
+                vr_orientation[10] = head_quat[2];  // head qy
+                vr_orientation[11] = head_quat[3];  // head qz
+                
+                // Update buffers
+                vr_3point_position_.SetData(vr_position);
+                vr_3point_orientation_.SetData(vr_orientation);
+            } else if (!use_ik_mode_ && control_goal_buffer_.has_wrist_matrices) {
+                // Non-IK mode with wrist matrices: Use left_wrist and right_wrist matrices
+                // Extract left wrist (matches Python: left_wrist_matrix[:3, 3:] for position)
+                auto left_rot = extract_rotation_from_transform(control_goal_buffer_.left_wrist);
+                auto left_pos = extract_position_from_transform(control_goal_buffer_.left_wrist);
+                vr_position[0] = left_pos[0];
+                vr_position[1] = left_pos[1];
+                vr_position[2] = left_pos[2];
+                
+                // Extract right wrist
+                auto right_rot = extract_rotation_from_transform(control_goal_buffer_.right_wrist);
+                auto right_pos = extract_position_from_transform(control_goal_buffer_.right_wrist);
+                vr_position[3] = right_pos[0];
+                vr_position[4] = right_pos[1];
+                vr_position[5] = right_pos[2];
+                // Head position uses defaults
+                vr_position[6] = 0.0241;
+                vr_position[7] = -0.0081;
+                vr_position[8] = 0.4028;
+                
+                // Convert rotation matrices to quaternions using math_utils.hpp
+                auto left_quat = rotation_matrix_to_quat_d(left_rot);
+                auto right_quat = rotation_matrix_to_quat_d(right_rot);
+                
+                // Store quaternions (w, x, y, z format)
+                vr_orientation[0] = left_quat[0];   // left qw
+                vr_orientation[1] = left_quat[1];   // left qx
+                vr_orientation[2] = left_quat[2];   // left qy
+                vr_orientation[3] = left_quat[3];   // left qz
+                vr_orientation[4] = right_quat[0];  // right qw
+                vr_orientation[5] = right_quat[1];  // right qx
+                vr_orientation[6] = right_quat[2];  // right qy
+                vr_orientation[7] = right_quat[3];  // right qz
+                // Head orientation uses defaults
+                vr_orientation[8] = 0.9991;
+                vr_orientation[9] = 0.011;
+                vr_orientation[10] = 0.0402;
+                vr_orientation[11] = -0.0002;
+                
+                // Update buffers
+                vr_3point_position_.SetData(vr_position);
+                vr_3point_orientation_.SetData(vr_orientation);
+            } else {
+                // Fallback: Use standard wrist_pose format (14 doubles)
+                vr_position[0] = control_goal_buffer_.wrist_pose[0];  // left wrist x
+                vr_position[1] = control_goal_buffer_.wrist_pose[1];  // left wrist y
+                vr_position[2] = control_goal_buffer_.wrist_pose[2];  // left wrist z
+                vr_position[3] = control_goal_buffer_.wrist_pose[7];  // right wrist x
+                vr_position[4] = control_goal_buffer_.wrist_pose[8];  // right wrist y
+                vr_position[5] = control_goal_buffer_.wrist_pose[9];  // right wrist z
+                // Head position uses defaults
+                vr_position[6] = 0.0241;
+                vr_position[7] = -0.0081;
+                vr_position[8] = 0.4028;
+                
+                // Update VR 3-point orientation data (wrist quaternions)
+                vr_orientation[0] = control_goal_buffer_.wrist_pose[3];   // left wrist qw
+                vr_orientation[1] = control_goal_buffer_.wrist_pose[4];   // left wrist qx
+                vr_orientation[2] = control_goal_buffer_.wrist_pose[5];   // left wrist qy
+                vr_orientation[3] = control_goal_buffer_.wrist_pose[6];   // left wrist qz
+                vr_orientation[4] = control_goal_buffer_.wrist_pose[10];  // right wrist qw
+                vr_orientation[5] = control_goal_buffer_.wrist_pose[11];  // right wrist qx
+                vr_orientation[6] = control_goal_buffer_.wrist_pose[12];  // right wrist qy
+                vr_orientation[7] = control_goal_buffer_.wrist_pose[13];  // right wrist qz
+                // Head orientation uses defaults
+                vr_orientation[8] = 0.9991;
+                vr_orientation[9] = 0.011;
+                vr_orientation[10] = 0.0402;
+                vr_orientation[11] = -0.0002;
+                
+                // Update buffers
+                vr_3point_position_.SetData(vr_position);
+                vr_3point_orientation_.SetData(vr_orientation);
+            }
+
+            if constexpr (DEBUG_LOGGING) {
+                static int goal_debug_counter = 0;
+                goal_debug_counter++;
+                if (goal_debug_counter % 50 == 0) {  // Log every 50 calls to avoid spam
+                    std::cout << "[ROS2 DEBUG] Control goal update:" << std::endl;
+                    std::cout << "  Navigate cmd: [" << navigate_cmd_from_teleop_[0] << ", " 
+                              << navigate_cmd_from_teleop_[1] << ", " << navigate_cmd_from_teleop_[2] << "]" << std::endl;
+                    std::cout << "  Base height: " << control_goal_buffer_.base_height_command << std::endl;
+                    std::cout << "  Toggle policy action: " << (control_goal_buffer_.toggle_policy_action ? "true" : "false") << std::endl;
+                    
+                    // Data availability flags
+                    std::cout << "  Data flags: has_ik_data=" << (control_goal_buffer_.has_ik_data ? "true" : "false")
+                              << ", has_wrist_matrices=" << (control_goal_buffer_.has_wrist_matrices ? "true" : "false")
+                              << ", has_hand_joints=" << (has_hand_joints_ ? "true" : "false")
+                              << ", use_ik_mode=" << (use_ik_mode_ ? "true" : "false") << std::endl;
+                    
+                    // VR 3-point positions (left wrist, right wrist, head)
+                    std::cout << "  VR 3-point positions:" << std::endl;
+                    std::cout << "    Left wrist:  [" << vr_position[0] << ", " << vr_position[1] << ", " << vr_position[2] << "]" << std::endl;
+                    std::cout << "    Right wrist: [" << vr_position[3] << ", " << vr_position[4] << ", " << vr_position[5] << "]" << std::endl;
+                    std::cout << "    Head:        [" << vr_position[6] << ", " << vr_position[7] << ", " << vr_position[8] << "]" << std::endl;
+                    
+                    // VR 3-point orientations (quaternions: w, x, y, z)
+                    std::cout << "  VR 3-point orientations:" << std::endl;
+                    std::cout << "    Left wrist:  [" << vr_orientation[0] << ", " << vr_orientation[1] << ", " 
+                              << vr_orientation[2] << ", " << vr_orientation[3] << "]" << std::endl;
+                    std::cout << "    Right wrist: [" << vr_orientation[4] << ", " << vr_orientation[5] << ", " 
+                              << vr_orientation[6] << ", " << vr_orientation[7] << "]" << std::endl;
+                    std::cout << "    Head:        [" << vr_orientation[8] << ", " << vr_orientation[9] << ", " 
+                              << vr_orientation[10] << ", " << vr_orientation[11] << "]" << std::endl;
+                    
+                    if (has_hand_joints_) {
+                        auto [has_left, left_hand] = GetHandPose(true);
+                        auto [has_right, right_hand] = GetHandPose(false);
+                        std::cout << "  Left hand pose: [";
+                        for (size_t i = 0; i < 7; ++i) {
+                            std::cout << left_hand[i];
+                            if (i < 6) std::cout << ", ";
+                        }
+                        std::cout << "]" << std::endl;
+                        std::cout << "  Right hand pose: [";
+                        for (size_t i = 0; i < 7; ++i) {
+                            std::cout << right_hand[i];
+                            if (i < 6) std::cout << ", ";
+                        }
+                        std::cout << "]" << std::endl;
+                    }
+                }
+            }
+        } else {
+            // No control goal data available
+            use_teleop_navigate_cmd_ = false;
+        }
+    }
+
+    // Override the handle_input function from InputInterface
+    // Uses local boolean flags (set by update()) to perform actions on system state
+    void handle_input(MotionDataReader& motion_reader,
+                     std::shared_ptr<const MotionSequence>& current_motion,
+                     int& current_frame,
+                     OperatorState& operator_state,
+                     bool& reinitialize_heading,
+                     DataBuffer<HeadingState>& heading_state_buffer,
+                     bool has_planner,
+                     PlannerState& planner_state,
+                     DataBuffer<MovementState>& movement_state_buffer,
+                     std::mutex& current_motion_mutex,
+                     bool& report_temperature) override {
+        
+        // Handle emergency stop (triggered by ROS2 errors/timeout)
+        if (emergency_stop_) {
+            std::cout << "[ROS2 EMERGENCY STOP] Triggering emergency stop due to ROS2 failure" << std::endl;
+            operator_state.stop = true;
+            return;  // Skip all other processing
+        }
+        
+        // Handle safety reset from interface manager
+        // ROS2 requires planner to be enabled
+        if (trigger_safety_reset) {
+            trigger_safety_reset = false;
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+            }
+            if (has_planner && operator_state.start) {
+                control_is_active_ = true;
+                if (planner_state.enabled && planner_state.initialized) {
+                    // Planner is already on, keep it as is (don't touch initialized flag)
+                    {
+                        std::lock_guard<std::mutex> lock(current_motion_mutex);
+                        if (current_motion->GetEncodeMode() >= 0) {
+                            current_motion->SetEncodeMode(1);
+                        }
+                        operator_state.play = true;
+                    }
+                    auto current_facing = movement_state_buffer.GetDataWithTime().data->facing_direction;
+                    planner_facing_angle_ = std::atan2(current_facing[1], current_facing[0]);
+                    std::cout << "Safety reset: Planner kept enabled with current state" << std::endl;
+                } else {
+                    // Planner was disabled, go to first motion and set initial heading and movement state
+                    // Set initial heading and movement state
+                    movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+                    // Set current motion and frame to reference motion (lock mutex)
+                    {
+                        std::lock_guard<std::mutex> lock(current_motion_mutex);
+                        auto temp_motion = std::make_shared<MotionSequence>(*current_motion);
+                        temp_motion->name = "temporary_motion";
+                        current_motion = temp_motion;
+                    }
+
+                    // Now it is safe to enable planner
+                    // Ensure planner is enabled (always required in ROS2 mode)
+                    planner_state.enabled = true;
+                    planner_facing_angle_ = 0.0;
+                    std::cout << "[ROS2] Planner enabled" << std::endl;
+                    // Wait for planner to be initialized with timeout (5 seconds)
+                    auto wait_start = std::chrono::steady_clock::now();
+                    constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+                    while (planner_state.enabled) {
+                        {
+                            std::lock_guard<std::mutex> lock(current_motion_mutex);
+                            if (current_motion->name == "planner_motion") {
+                            std::cout << "[ROS2] motion name is planner_motion" << std::endl;
+                            break;
+                            }
+                        }
+                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                        auto elapsed = std::chrono::steady_clock::now() - wait_start;
+                        if (elapsed > PLANNER_INIT_TIMEOUT) {
+                            std::cerr << "[ROS2 ERROR] Planner initialization timeout after 5 seconds" << std::endl;
+                            operator_state.stop = true;
+                            return;
+                        }
+                        std::cout << "[ROS2] Waiting for planner to be initialized" << std::endl;
+                    }
+                    // Check if planner is enabled and initialized
+                    if (!planner_state.enabled || !planner_state.initialized) {
+                        std::cerr << "[ROS2 ERROR] Planner failed to initialize - ROS2 mode requires planner. Stopping control." << std::endl;
+                        operator_state.stop = true;
+                        return;
+                    }
+                    // Play motion
+                    {
+                        std::lock_guard<std::mutex> lock(current_motion_mutex);
+                        if (current_motion->GetEncodeMode() == 0) {
+                            current_motion->SetEncodeMode(1);
+                        }
+                        operator_state.play = true;
+                    }
+                }
+            }
+        }
+
+        // Check if planner is loaded (required for ROS2 mode)
+        if (!has_planner) {
+            std::cerr << "[ROS2 ERROR] Planner not loaded - ROS2 mode requires planner. Stopping control." << std::endl;
+            operator_state.stop = true;
+            return;
+        }
+
+        // Handle control start/stop
+        if (this->stop_control_) { operator_state.stop = true; }
+        if (this->report_temperature_flag_) { report_temperature = true; }
+
+        // Handle control start
+        if (this->start_control_) { 
+            // Start control
+            operator_state.start = true;
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                reinitialize_heading = true;
+            }
+            // Ensure planner is enabled (always required in ROS2 mode)
+            if (!planner_state.enabled) {
+                planner_state.enabled = true;
+                planner_facing_angle_ = 0.0;
+                std::cout << "[ROS2] Planner enabled" << std::endl;
+            }
+            // Wait for planner to be initialized with timeout (5 seconds)
+            auto wait_start = std::chrono::steady_clock::now();
+            constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+            while (planner_state.enabled) {
+                {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    if (current_motion->name == "planner_motion") {
+                        std::cout << "[ROS2] motion name is planner_motion" << std::endl;
+                        break;
+                    }
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                auto elapsed = std::chrono::steady_clock::now() - wait_start;
+                if (elapsed > PLANNER_INIT_TIMEOUT) {
+                    std::cerr << "[ROS2 ERROR] Planner initialization timeout after 5 seconds" << std::endl;
+                    operator_state.stop = true;
+                    return;
+                }
+                std::cout << "[ROS2] Waiting for planner to be initialized" << std::endl;
+            }
+            // Check if planner is enabled and initialized
+            if (!planner_state.enabled || !planner_state.initialized) {
+                std::cerr << "[ROS2 ERROR] Planner failed to initialize - ROS2 mode requires planner. Stopping control." << std::endl;
+                operator_state.stop = true;
+                return;
+            }
+            // Play motion
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = true;
+            }
+        }
+
+        if (planner_state.enabled && planner_state.initialized) {
+            
+            // Set final movement values from navigate_cmd
+            int final_mode = static_cast<int>(LocomotionMode::IDLE);
+            std::array<double, 3> final_movement = {0.0, 0.0, 0.0};
+            std::array<double, 3> final_facing_direction = {1.0, 0.0, 0.0};
+            double final_speed = 0.0;
+            double final_height = -1.0;
+            
+            // Get and process base_height_command (thread-safe copy)
+            double base_height = base_height_command_;
+            // Clip to valid range [0.1, 0.88]
+            base_height = std::clamp(base_height, 0.1, 0.88);
+            
+            // Convert navigate_cmd to movement direction and mode
+            if (use_teleop_navigate_cmd_) {
+                // navigate_cmd format: [lin_vel_x, lin_vel_y, ang_vel_z]
+                // Convert to movement_direction and mode
+                double lin_vel_x = navigate_cmd_from_teleop_[0];
+                double lin_vel_y = navigate_cmd_from_teleop_[1];
+                double ang_vel_z = navigate_cmd_from_teleop_[2];
+                
+                // Update facing angle based on angular velocity (similar to gamepad)
+                if (std::abs(ang_vel_z) > 0.01f) {
+                    // Negative sign matches gamepad behavior (positive ang_vel_z = turn left/CCW)
+                    planner_facing_angle_ += ang_vel_z * 0.02;
+                }
+                
+                // Always compute facing direction from maintained angle (not from navigation topic)
+                final_facing_direction[0] = std::cos(planner_facing_angle_);
+                final_facing_direction[1] = std::sin(planner_facing_angle_);
+                final_facing_direction[2] = 0.0f;
+                
+                // Calculate movement magnitude
+                double movement_mag = std::sqrt(lin_vel_x * lin_vel_x + lin_vel_y * lin_vel_y);
+
+                double planner_moving_direction = planner_facing_angle_;
+                
+                // Determine locomotion mode based on base_height_command first
+                if (base_height >= 0.72f) {
+                    // Height 0.72-0.88: Normal walking modes
+                    if (movement_mag > 0.01f) {
+                        // Moving: use walk modes
+                        // Compute moving direction (same as gamepad logic)
+                        planner_moving_direction = std::atan2(lin_vel_y, lin_vel_x) + planner_moving_direction;
+                        
+                        // Bin the moving direction to 8 evenly spaced directions and get corresponding speed
+                        auto [binned_angle, direction_speed] = bin_angle_to_8_directions(planner_moving_direction);
+                        planner_moving_direction = binned_angle;
+                        
+                        // Compute normalized movement direction from binned angle
+                        final_movement[0] = std::cos(planner_moving_direction);
+                        final_movement[1] = std::sin(planner_moving_direction);
+                        final_movement[2] = 0.0f;
+                        
+                        if (locomotion_mode_is_fast_) {
+                            // Normal walk mode: default speed (-1)
+                            final_mode = static_cast<int>(LocomotionMode::WALK);
+                            final_speed = -1.0f;
+                        } else {
+                            // Slow walk mode: speed varies by direction (faster forward/lateral, slower backward)
+                            final_mode = static_cast<int>(LocomotionMode::SLOW_WALK);
+                            final_speed = direction_speed;
+                        }
+                    } else {
+                        // No movement: idle
+                        final_mode = static_cast<int>(LocomotionMode::IDLE);
+                        final_movement = {0.0f, 0.0f, 0.0f};
+                        final_speed = -1.0f;
+                    }
+                    final_height = -1.0f;  // Use default height for walking
+                    
+                } else if (base_height >= 0.5f) {
+                    // Height 0.5-0.72: Squat mode (static pose, no movement)
+                    final_mode = static_cast<int>(LocomotionMode::IDEL_SQUAT);
+                    final_movement = {0.0f, 0.0f, 0.0f};
+                    final_speed = -1.0f;  // Use default speed (no walking while squatting)
+                    final_height = base_height;  // Pass actual height command
+                    
+                } else {
+                    // Height 0.1-0.5: Kneel mode (static pose, no movement)
+                    final_mode = static_cast<int>(LocomotionMode::IDEL_KNEEL);
+                    final_movement = {0.0f, 0.0f, 0.0f};
+                    final_speed = -1.0f;  // Use default speed (no walking while kneeling)
+                    final_height = base_height;  // Pass actual height command
+                }
+            }
+
+            // Debug: Log final computed values being sent to planner
+            if constexpr (DEBUG_LOGGING) {
+                static int debug_counter = 0;
+                debug_counter++;
+                if (debug_counter % 50 == 0) {  // Log every 50 calls to avoid spam
+                    std::cout << "[ROS2 DEBUG] Planner control values:" << std::endl;
+                    if (use_teleop_navigate_cmd_) {
+                        std::cout << "  Input navigate_cmd: [" << navigate_cmd_from_teleop_[0] << ", " 
+                                  << navigate_cmd_from_teleop_[1] << ", " << navigate_cmd_from_teleop_[2] << "]" << std::endl;
+                        std::cout << "  Facing angle: " << planner_facing_angle_ << " rad (" 
+                                  << (planner_facing_angle_ * 180.0 / M_PI) << " deg)" << std::endl;
+                    }
+                    std::cout << "  Base height command: " << base_height_command_ 
+                              << " (clamped: " << std::clamp(base_height_command_, 0.1, 0.88) << ")" << std::endl;
+                    std::cout << "  Final mode: " << final_mode << " (0=idle, 1=slow, 2=walk, 3=run, 4=squat, 6=kneel)" << std::endl;
+                    std::cout << "  Final speed: " << final_speed << std::endl;
+                    std::cout << "  Final height: " << final_height << std::endl;
+                    std::cout << "  Movement direction: [" << final_movement[0] << ", " << final_movement[1] << ", " << final_movement[2] << "]" << std::endl;
+                    std::cout << "  Facing direction: [" << final_facing_direction[0] << ", " << final_facing_direction[1] << ", " << final_facing_direction[2] << "]" << std::endl;
+                }
+            }
+
+            // Update thread-safe buffer (single source of truth for planner thread)
+            MovementState mode_state(final_mode, final_movement, final_facing_direction, final_speed, final_height);
+            movement_state_buffer.SetData(mode_state);
+        }
+    }
+
+    // Get ROS2 node for external use (e.g., spinning)
+    std::shared_ptr<rclcpp::Node> get_node() const { return node_; }
+    
+    // Check if ROS2 node is healthy and receiving data
+    bool is_receiving_control_goal_data() const { return received_control_goal_.load(); }
+    bool is_ros2_ok() const { return rclcpp::ok() && node_ != nullptr; }
+    
+    // Reset data received flags (useful for debugging)
+    void reset_data_flags() {
+        received_control_goal_.store(false);
+        last_control_goal_time_ns_.store(0);
+    }
+
+    // Get ROS timestamp in seconds (for state logging)
+    // This is ROS2-specific and not part of the InputInterface base class
+    double GetROSTimestamp() const {
+        if (node_) {
+            return node_->get_clock()->now().nanoseconds() / 1e9;
+        }
+        return 0.0;
+    }
+
+private:
+    // ------------------------------------------------------------------
+    // ROS 2 infrastructure
+    // ------------------------------------------------------------------
+    std::shared_ptr<rclcpp::Node> node_;  ///< Lightweight ROS 2 node for this handler.
+
+    /// Subscription to `ControlPolicy/upper_body_pose` (ByteMultiArray, msgpack).
+    rclcpp::Subscription<std_msgs::msg::ByteMultiArray>::SharedPtr control_goal_sub_;
+
+    // ------------------------------------------------------------------
+    // Thread-safe receiving buffer (written by callback, read by update())
+    // ------------------------------------------------------------------
+    ControlGoalMsg control_goal_buffer_;               ///< Latest deserialized message.
+    std::mutex control_goal_mutex_;                     ///< Guards control_goal_buffer_.
+    std::atomic<bool> received_control_goal_{false};   ///< True once at least one message arrived.
+    std::atomic<int64_t> last_control_goal_time_ns_{0}; ///< Monotonic timestamp of last message (ns).
+    static constexpr double CONTROL_GOAL_TIMEOUT = 1.0; ///< Seconds before a timeout reset.
+    
+    /// When true, use IK-processed transformation matrices for VR position;
+    /// when false, use raw left_wrist / right_wrist matrices.
+    bool use_ik_mode_ = false;
+
+    // ------------------------------------------------------------------
+    // Control-toggle state
+    // ------------------------------------------------------------------
+    bool control_is_active_ = false;       ///< Tracks the toggle state for toggle_policy_action.
+    bool locomotion_mode_is_fast_ = false;  ///< false = SLOW_WALK (custom speed), true = WALK (default speed).
+    
+    // ------------------------------------------------------------------
+    // Per-frame control flags (reset in update())
+    // ------------------------------------------------------------------
+    bool start_control_ = false;   ///< Start control this frame.
+    bool stop_control_ = false;    ///< Stop control this frame.
+    bool report_temperature_flag_ = false;  ///< Report temperature this frame (F key).
+
+    // ------------------------------------------------------------------
+    // Teleop state (updated from control_goal_buffer_ in update())
+    // ------------------------------------------------------------------
+    std::array<double, 3> navigate_cmd_from_teleop_ = {0.0, 0.0, 0.0};  ///< [lin_x, lin_y, ang_z].
+    bool use_teleop_navigate_cmd_ = false;   ///< True while navigate_cmd is valid.
+    double base_height_command_ = 0.78;      ///< Thread-safe copy of base_height_command.
+    
+    /// Accumulated facing angle (radians), integrated from ang_vel_z each frame.
+    double planner_facing_angle_ = 0.0;
+    
+    struct termios old_termios_;  ///< Saved terminal state for restoration on destruction.
+    
+    /**
+     * @brief Deserialize a msgpack-encoded control-goal payload.
+     * @param data  Raw bytes from the ByteMultiArray message.
+     * @return Parsed ControlGoalMsg (valid == true on success).
+     *
+     * Expected keys: navigate_cmd, wrist_pose, left_wrist_after_ik,
+     * right_wrist_after_ik, head_after_ik, left_wrist, right_wrist,
+     * base_height_command, toggle_policy_action, locomotion_mode,
+     * left_hand_joint, right_hand_joint, ros_timestamp.
+     */
+    ControlGoalMsg parse_msgpack_control_goal(const std::vector<uint8_t>& data) {
+        ControlGoalMsg msg;
+        msg.valid = false;
+        
+        try {
+            // Use msgpack-c library for clean, efficient parsing
+            msgpack::object_handle oh = msgpack::unpack(reinterpret_cast<const char*>(data.data()), data.size());
+            msgpack::object deserialized = oh.get();
+            
+            // Convert to map
+            if (deserialized.type != msgpack::type::MAP) {
+                return msg;
+            }
+            
+            std::map<std::string, msgpack::object> map_data;
+            deserialized.convert(map_data);
+            
+            // Extract navigate_cmd (array of 3 doubles)
+            if (map_data.count("navigate_cmd") && map_data["navigate_cmd"].type == msgpack::type::ARRAY) {
+                auto nav_arr = map_data["navigate_cmd"].as<std::vector<double>>();
+                if (nav_arr.size() >= 3) {
+                    msg.navigate_cmd[0] = nav_arr[0];
+                    msg.navigate_cmd[1] = nav_arr[1];
+                    msg.navigate_cmd[2] = nav_arr[2];
+                }
+            }
+            
+            // Extract wrist_pose (array of 14 doubles)
+            if (map_data.count("wrist_pose") && map_data["wrist_pose"].type == msgpack::type::ARRAY) {
+                auto wrist_arr = map_data["wrist_pose"].as<std::vector<double>>();
+                if (wrist_arr.size() >= 14) {
+                    std::copy_n(wrist_arr.begin(), 14, msg.wrist_pose.begin());
+                }
+            }
+            
+            // Extract IK-processed matrices (16 doubles each)
+            if (map_data.count("left_wrist_after_ik") && map_data["left_wrist_after_ik"].type == msgpack::type::ARRAY) {
+                try {
+                    auto nested_arr = map_data["left_wrist_after_ik"].as<std::vector<std::vector<double>>>();
+                    if (nested_arr.size() == 4 && nested_arr[0].size() == 4) {
+                        // Flatten 4x4 matrix to 1D array (row-major)
+                        size_t idx = 0;
+                        for (const auto& row : nested_arr) {
+                            for (double val : row) {
+                                msg.left_wrist_after_ik[idx++] = val;
+                            }
+                        }
+                        msg.has_ik_data = true;
+                    }
+                } catch (const std::exception& e) {
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cerr << "[ROS2 ERROR] Failed to parse left_wrist_after_ik: " << e.what() << std::endl;
+                    }
+                }
+            }
+            
+            if (map_data.count("right_wrist_after_ik") && map_data["right_wrist_after_ik"].type == msgpack::type::ARRAY) {
+                try {
+                    auto nested_arr = map_data["right_wrist_after_ik"].as<std::vector<std::vector<double>>>();
+                    if (nested_arr.size() == 4 && nested_arr[0].size() == 4) {
+                        // Flatten 4x4 matrix to 1D array (row-major)
+                        size_t idx = 0;
+                        for (const auto& row : nested_arr) {
+                            for (double val : row) {
+                                msg.right_wrist_after_ik[idx++] = val;
+                            }
+                        }
+                        msg.has_ik_data = true;
+                    }
+                } catch (const std::exception& e) {
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cerr << "[ROS2 ERROR] Failed to parse right_wrist_after_ik: " << e.what() << std::endl;
+                    }
+                }
+            }
+            
+            if (map_data.count("head_after_ik") && map_data["head_after_ik"].type == msgpack::type::ARRAY) {
+                try {
+                    auto nested_arr = map_data["head_after_ik"].as<std::vector<std::vector<double>>>();
+                    if (nested_arr.size() == 4 && nested_arr[0].size() == 4) {
+                        // Flatten 4x4 matrix to 1D array (row-major)
+                        size_t idx = 0;
+                        for (const auto& row : nested_arr) {
+                            for (double val : row) {
+                                msg.head_after_ik[idx++] = val;
+                            }
+                        }
+                        msg.has_ik_data = true;
+                    }
+                } catch (const std::exception& e) {
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cerr << "[ROS2 ERROR] Failed to parse head_after_ik: " << e.what() << std::endl;
+                    }
+                }
+            }
+            
+            // Extract non-IK wrist matrices (16 doubles each)
+            if (map_data.count("left_wrist") && map_data["left_wrist"].type == msgpack::type::ARRAY) {
+                try {
+                    auto nested_arr = map_data["left_wrist"].as<std::vector<std::vector<double>>>();
+                    if (nested_arr.size() == 4 && nested_arr[0].size() == 4) {
+                        // Flatten 4x4 matrix to 1D array (row-major)
+                        size_t idx = 0;
+                        for (const auto& row : nested_arr) {
+                            for (double val : row) {
+                                msg.left_wrist[idx++] = val;
+                            }
+                        }
+                        msg.has_wrist_matrices = true;
+                    }
+                } catch (const std::exception& e) {
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cerr << "[ROS2 ERROR] Failed to parse left_wrist: " << e.what() << std::endl;
+                    }
+                }
+            }
+            
+            if (map_data.count("right_wrist") && map_data["right_wrist"].type == msgpack::type::ARRAY) {
+                try {
+                    auto nested_arr = map_data["right_wrist"].as<std::vector<std::vector<double>>>();
+                    if (nested_arr.size() == 4 && nested_arr[0].size() == 4) {
+                        // Flatten 4x4 matrix to 1D array (row-major)
+                        size_t idx = 0;
+                        for (const auto& row : nested_arr) {
+                            for (double val : row) {
+                                msg.right_wrist[idx++] = val;
+                            }
+                        }
+                        msg.has_wrist_matrices = true;
+                    }
+                } catch (const std::exception& e) {
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cerr << "[ROS2 ERROR] Failed to parse right_wrist: " << e.what() << std::endl;
+                    }
+                }
+            }
+            
+            // Extract base_height_command (double)
+            if (map_data.count("base_height_command")) {
+                msg.base_height_command = map_data["base_height_command"].as<double>();
+            }
+            
+            // Extract toggle_policy_action (bool)
+            if (map_data.count("toggle_policy_action")) {
+                msg.toggle_policy_action = map_data["toggle_policy_action"].as<bool>();
+            }
+            
+            // Extract locomotion_mode (int: 0 = slow walk, 1 = fast walk)
+            if (map_data.count("locomotion_mode")) {
+                msg.locomotion_mode = map_data["locomotion_mode"].as<int>();
+            }
+            
+            // Extract left_hand_joint (7 doubles - joint positions)
+            if (map_data.count("left_hand_joint") && map_data["left_hand_joint"].type == msgpack::type::ARRAY) {
+                auto left_hand_arr = map_data["left_hand_joint"].as<std::vector<double>>();
+                if (left_hand_arr.size() >= 7) {
+                    std::copy_n(left_hand_arr.begin(), 7, msg.left_hand_joint.begin());
+                    msg.has_hand_joints = true;
+                }
+            }
+            
+            // Extract right_hand_joint (7 doubles - joint positions)
+            if (map_data.count("right_hand_joint") && map_data["right_hand_joint"].type == msgpack::type::ARRAY) {
+                auto right_hand_arr = map_data["right_hand_joint"].as<std::vector<double>>();
+                if (right_hand_arr.size() >= 7) {
+                    std::copy_n(right_hand_arr.begin(), 7, msg.right_hand_joint.begin());
+                    msg.has_hand_joints = true;
+                }
+            }
+            
+            // Extract ros_timestamp (double - ROS time in seconds)
+            if (map_data.count("ros_timestamp")) {
+                msg.ros_timestamp = map_data["ros_timestamp"].as<double>();
+            }
+            
+            msg.valid = true;
+            
+        } catch (const std::exception& e) {
+            std::cerr << "[ROS2 ERROR] msgpack parsing failed: " << e.what() << std::endl;
+            msg.valid = false;
+        }
+        
+        return msg;
+    }
+    
+    /**
+     * @brief Extract the 3×3 rotation matrix from a flattened row-major 4×4 transform.
+     * @return 3×3 rotation matrix compatible with math_utils::rotation_matrix_to_quat_d().
+     */
+    std::array<std::array<double, 3>, 3> extract_rotation_from_transform(const std::array<double, 16>& transform) {
+        // 4x4 matrix in row-major: [R00,R01,R02,tx, R10,R11,R12,ty, R20,R21,R22,tz, 0,0,0,1]
+        return {{
+            {transform[0], transform[1], transform[2]},    // Row 0
+            {transform[4], transform[5], transform[6]},    // Row 1
+            {transform[8], transform[9], transform[10]}    // Row 2
+        }};
+    }
+    
+    /// Extract the translation vector [x, y, z] from a flattened row-major 4×4 transform.
+    std::array<double, 3> extract_position_from_transform(const std::array<double, 16>& transform) {
+        // 4x4 matrix in row-major: position is at [3, 7, 11]
+        return {transform[3], transform[7], transform[11]};
+    }
+    
+    /// Multiply a 3×3 rotation matrix by a 3D offset vector (R × v).
+    std::array<double, 3> apply_rotation_to_offset(
+        const std::array<std::array<double, 3>, 3>& rot_mat, 
+        const std::array<double, 3>& offset) {
+        return {
+            rot_mat[0][0] * offset[0] + rot_mat[0][1] * offset[1] + rot_mat[0][2] * offset[2],  // Row 0 * offset
+            rot_mat[1][0] * offset[0] + rot_mat[1][1] * offset[1] + rot_mat[1][2] * offset[2],  // Row 1 * offset
+            rot_mat[2][0] * offset[0] + rot_mat[2][1] * offset[1] + rot_mat[2][2] * offset[2]   // Row 2 * offset
+        };
+    }
+    
+    /**
+     * @brief Quantise an angle to the nearest 45° bin and return a direction-dependent speed.
+     *
+     * The 8 bins: 0° (forward), ±45° (forward-diagonal), ±90° (lateral),
+     * ±135° (backward-diagonal), 180° (backward).
+     *
+     * @param angle  Input angle in radians (will be normalised to [−π, π]).
+     * @return {binned_angle, slow_walk_speed} – angle snapped to nearest bin,
+     *         and the corresponding speed for SLOW_WALK mode (faster forward,
+     *         slower backward).
+     */
+    std::pair<double, double> bin_angle_to_8_directions(double angle) {
+        constexpr double BIN_SIZE = M_PI / 4.0;  // 45 degrees in radians
+        constexpr int NUM_BINS = 8;
+        
+        // Normalize angle to [-π, π]
+        while (angle > M_PI) angle -= 2.0 * M_PI;
+        while (angle < -M_PI) angle += 2.0 * M_PI;
+        
+        // Find nearest bin
+        int bin_index = static_cast<int>(std::round(angle / BIN_SIZE));
+        
+        // Handle wrap-around (bin_index can be -4 to 4)
+        if (bin_index > 4) bin_index -= NUM_BINS;
+        if (bin_index < -4) bin_index += NUM_BINS;
+        
+        // Convert back to angle
+        double binned_angle = bin_index * BIN_SIZE;
+        
+        // Determine speed based on direction bin for slow walk mode
+        // Faster forward/lateral, slower backward
+        double slow_walk_speed;
+        switch (bin_index) {
+            case 0:   // Forward (0°)
+            case 1:   // Forward-right (45°)
+            case -1:  // Forward-left (-45°)
+                slow_walk_speed = 0.3f;
+                break;
+            case 2:   // Right (90°)
+            case -2:  // Left (-90°)
+                slow_walk_speed = 0.35f;
+                break;
+            case 3:   // Back-right (135°)
+            case -3:  // Back-left (-135°)
+                slow_walk_speed = 0.25f;
+                break;
+            case 4:   // Backward (180°)
+            case -4:  // Backward (-180°)
+                slow_walk_speed = 0.2f;
+                break;
+            default:
+                slow_walk_speed = 0.2f;  // Fallback
+                break;
+        }
+        
+        return {binned_angle, slow_walk_speed};
+    }
+
+    /**
+     * @brief ROS 2 subscriber callback (runs on the executor thread).
+     *
+     * Deserialises the ByteMultiArray payload via msgpack, stores the result
+     * in `control_goal_buffer_` under `control_goal_mutex_`, and sets the
+     * `received_control_goal_` flag.  Edge-triggered commands (toggle_policy_action)
+     * are accumulated with OR logic to prevent lost pulses.
+     */
+    void control_goal_callback(std::shared_ptr<const std_msgs::msg::ByteMultiArray> msg) {
+        try {
+            // Convert ByteMultiArray to vector<uint8_t>
+            // msg->data is already std::vector<uint8_t>, so just copy it
+            std::vector<uint8_t> data(msg->data.begin(), msg->data.end());
+            
+            // Parse msgpack data
+            ControlGoalMsg goal_msg = parse_msgpack_control_goal(data);
+            
+            if (goal_msg.valid) {
+                // Store in receiving buffer (thread-safe)
+                {
+                    std::lock_guard<std::mutex> lock(control_goal_mutex_);
+                    // Edge-triggered commands: accumulate with OR (like mode_control)
+                    // This prevents losing a toggle if it arrives between update() cycles
+                    bool prev_toggle_policy = control_goal_buffer_.toggle_policy_action;
+                    control_goal_buffer_ = goal_msg;
+                    control_goal_buffer_.toggle_policy_action = prev_toggle_policy || goal_msg.toggle_policy_action;
+                    // locomotion_mode is a direct state value (0 or 1), not accumulated
+                }
+                received_control_goal_.store(true);
+                // Update timestamp for timeout tracking (using steady_clock for monotonic timing)
+                last_control_goal_time_ns_.store(std::chrono::steady_clock::now().time_since_epoch().count());
+                
+                if constexpr (DEBUG_LOGGING) {
+                    static int goal_counter = 0;
+                    goal_counter++;
+                    if (goal_counter % 50 == 0) {  // Log every 50 messages to avoid spam
+                        std::cout << "[ROS2 DEBUG] Control goal message received:" << std::endl;
+                        std::cout << "  navigate_cmd: [" << goal_msg.navigate_cmd[0] << ", " 
+                                  << goal_msg.navigate_cmd[1] << ", " << goal_msg.navigate_cmd[2] << "]" << std::endl;
+                        std::cout << "  base_height: " << goal_msg.base_height_command << std::endl;
+                    }
+                }
+            } else {
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ROS2 DEBUG] Invalid control goal message received" << std::endl;
+                }
+            }
+        } catch (const std::exception& e) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ROS2 ERROR] Failed to process control goal message: " << e.what() << std::endl;
+            }
+        }
+    }
+
+    // Helper method to initialize ROS2 subscriber
+    void setup_subscribers() {
+        // Create subscriber for control goal topic (teleop commands from Python)
+        control_goal_sub_ = node_->create_subscription<std_msgs::msg::ByteMultiArray>(
+            "ControlPolicy/upper_body_pose",
+            1,  // QoS depth = 1 for some buffering
+            [this](std::shared_ptr<const std_msgs::msg::ByteMultiArray> msg) {
+                this->control_goal_callback(msg);
+            }
+        );
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ROS2 DEBUG] Control goal subscriber created for topic: ControlPolicy/upper_body_pose" << std::endl;
+        }
+    }
+
+    /*
+     * ROS2 Topic Structure:
+     * 
+     * ControlPolicy/upper_body_pose (std_msgs/ByteMultiArray) - msgpack-serialized ControlGoalMsg:
+     *    - navigate_cmd: double[3] (navigation velocities [lin_vel_x, lin_vel_y, ang_vel_z])
+     *    - wrist_pose: double[14] (left + right wrist poses: [x,y,z,qw,qx,qy,qz] * 2)
+     *    - left_wrist_after_ik: double[16] (4x4 transformation matrix, flattened row-major)
+     *    - right_wrist_after_ik: double[16] (4x4 transformation matrix, flattened row-major)
+     *    - head_after_ik: double[16] (4x4 transformation matrix, flattened row-major)
+     *    - left_wrist: double[16] (4x4 transformation matrix, flattened row-major, non-IK)
+     *    - right_wrist: double[16] (4x4 transformation matrix, flattened row-major, non-IK)
+     *    - left_hand_joint: double[7] (7 DOF joint positions for left hand)
+     *    - right_hand_joint: double[7] (7 DOF joint positions for right hand)
+     *    - base_height_command: double (desired base height)
+     *    - toggle_policy_action: bool (toggle between start/stop control)
+     *    - locomotion_mode: int (0 = slow walk with custom speed, 1 = fast walk with default speed)
+     *    - ros_timestamp: double (ROS time in seconds for synchronization)
+     *    - valid: bool (message validity flag)
+     */
+};
+
+#endif // HAS_ROS2
+
+#endif // ROS2_INPUT_HANDLER_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/streamed_motion_merger.hpp

@@ -0,0 +1,517 @@
+/**
+ * @file streamed_motion_merger.hpp
+ * @brief Reusable sliding-window merger for streamed motion data.
+ *
+ * StreamedMotionMerger receives chunks of motion frames (joint positions /
+ * velocities, body quaternions, SMPL data) from any streaming source (ZMQ,
+ * ROS2, etc.) and merges them into a single growing MotionSequence using a
+ * sliding-window approach.
+ *
+ * ## Key Concepts
+ *
+ * - **Frame indices**: Each incoming chunk carries a vector of monotonically
+ *   increasing integer indices that identify frames in a global timeline.
+ *   The merger uses these to align new data with the existing window.
+ *
+ * - **Frame step**: The stride between consecutive frame indices (e.g. 2 if
+ *   the sender runs at 60 Hz and the consumer at 30 Hz).  Detected
+ *   automatically from the first two indices of each chunk.
+ *
+ * - **Sliding window**: The merger maintains a window of frames centred
+ *   around the current playback position.  It keeps `HISTORY_FRAMES` past
+ *   frames for smooth interpolation and appends new frames from the
+ *   incoming chunk.  Old frames that fall behind the window are discarded.
+ *
+ * - **Catch-up reset**: If the gap between the playback position and the
+ *   incoming data exceeds `MAX_GAP_FRAMES`, the merger resets the window
+ *   to the start of the incoming chunk and signals the caller to reset
+ *   the playback cursor to frame 0.  This prevents unbounded buffering
+ *   when the network falls behind.
+ *
+ * - **Protocol versions**: The merger itself is version-agnostic – it
+ *   merges whatever data fields are present in IncomingData.  Protocol-
+ *   version validation (rejecting changes mid-session, etc.) is left to
+ *   the caller (e.g. ZMQEndpointInterface).
+ *
+ * ## Thread Safety
+ *
+ * The merger is **not** thread-safe.  All calls must be serialised by the
+ * caller (typically by holding the data_mutex_ in ZMQEndpointInterface).
+ */
+
+#ifndef STREAMED_MOTION_MERGER_HPP
+#define STREAMED_MOTION_MERGER_HPP
+
+#include <memory>
+#include <vector>
+#include <array>
+#include <algorithm>
+#include <limits>
+#include <iostream>
+#include <iomanip>
+#include <cstring>
+
+// Forward declaration – MotionSequence is defined in motion_data_reader.hpp.
+struct MotionSequence;
+
+/**
+ * @class StreamedMotionMerger
+ * @brief Merges incoming motion-frame chunks into a sliding-window
+ *        MotionSequence for real-time playback.
+ */
+class StreamedMotionMerger {
+public:
+    /// Compile-time toggle for debug log output.
+    static constexpr bool DEBUG_LOGGING = true;
+    /// Number of already-consumed frames to retain before the playback cursor
+    /// (provides look-back for interpolation / blending).
+    static constexpr int HISTORY_FRAMES = 5;
+    /// Maximum tolerated gap (in current-rate frames) before a catch-up reset.
+    static constexpr int MAX_GAP_FRAMES = 200;
+    
+    /// Returned by MergeIncomingData() to communicate what happened.
+    struct MergeResult {
+        std::shared_ptr<MotionSequence> motion;  ///< Merged motion (nullptr on failure).
+        int window_start = 0;                    ///< Global frame index of motion[0].
+        int frame_offset_adjustment = 0;         ///< Subtract from current_frame to compensate for window shift.
+        bool did_catchup_reset = false;           ///< True → caller should reset playback to frame 0.
+        int frame_step = 1;                       ///< Detected stride between consecutive frame indices.
+        int protocol_version = 0;                 ///< Protocol version of the incoming data (1, 2, or 3).
+    };
+    
+    /// All the data needed for one merge operation, decoded by the caller.
+    struct IncomingData {
+        // -- Joint data (required in v1 & v3, optional in v2) --
+        std::vector<std::vector<double>> joint_pos;  ///< [frame][joint] positions (radians).
+        std::vector<std::vector<double>> joint_vel;  ///< [frame][joint] velocities (rad/s).
+        
+        // -- Body quaternions (required for all versions) --
+        std::vector<std::vector<std::array<double, 4>>> body_quat;  ///< [frame][body][w,x,y,z].
+        
+        // -- SMPL data (required in v2 & v3, optional in v1) --
+        std::vector<std::vector<std::array<double, 3>>> smpl_joints;  ///< [frame][joint][x,y,z].
+        std::vector<std::vector<std::array<double, 3>>> smpl_pose;    ///< [frame][pose][axis-angle x,y,z].
+        
+        std::vector<int64_t> frame_indices;  ///< Monotonic global frame indices (required).
+        
+        int protocol_version = 1;    ///< Protocol version (1, 2, or 3).
+        bool catch_up_enabled = true; ///< true → use MAX_GAP_FRAMES; false → allow infinite delay.
+        
+        // Derived dimensions (must match the vector sizes above)
+        int num_frames = 0;       ///< Number of frames in this chunk.
+        int num_joints = 0;       ///< Joints per frame (joint_pos / joint_vel width).
+        int num_quat_bodies = 0;  ///< Number of rigid bodies per frame (body_quat width).
+        int num_smpl_joints = 0;  ///< SMPL joints per frame.
+        int num_smpl_poses = 0;   ///< SMPL pose parameters per frame.
+    };
+    
+    StreamedMotionMerger() {
+        Reset();
+    }
+    
+    // Reset the merger state (clear all buffered data)
+    void Reset() {
+        streamed_motion_ = std::make_shared<MotionSequence>();
+        streamed_motion_->name = "streamed";
+        streamed_motion_->ReserveCapacity(15000, 29, 1, 1, 0, 0);
+        stream_window_start_ = 0;
+    }
+    
+    // Main merging method: merge incoming data with existing buffered data
+    // Returns MergeResult containing the merged motion and playback adjustments
+    // 
+    // Note: Protocol version validation should be done by the caller before calling this method.
+    // The merger doesn't care about protocol versions - it just merges the data.
+    MergeResult MergeIncomingData(const IncomingData& data, int current_playback_frame) {
+        MergeResult result;
+        
+        // Validate incoming data
+        if (!ValidateIncomingData(data)) {
+            std::cerr << "[StreamedMotionMerger] Invalid incoming data" << std::endl;
+            return result;
+        }
+        
+        // Extract frame step and validate
+        int frame_step = CalculateFrameStep(data.frame_indices);
+        int incoming_frame_start = static_cast<int>(data.frame_indices[0]);
+        int incoming_frame_end = static_cast<int>(data.frame_indices[data.num_frames - 1]);
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[StreamedMotionMerger] Processing " << data.num_frames << " frames, "
+                      << "incoming_frame_start=" << incoming_frame_start 
+                      << ", frame_step=" << frame_step << std::endl;
+        }
+        
+        // Calculate sliding window parameters
+        int global_playback_frame = stream_window_start_ + frame_step * std::max(0, current_playback_frame - HISTORY_FRAMES);
+        int new_window_start = stream_window_start_;
+        int merge_dst_frame = 0;
+        bool did_catchup = false;
+        
+        CalculateSlidingWindow(
+            incoming_frame_start,
+            incoming_frame_end,
+            frame_step,
+            current_playback_frame,
+            global_playback_frame,
+            data.catch_up_enabled,
+            new_window_start,
+            merge_dst_frame,
+            did_catchup
+        );
+        
+        // Create new motion sequence
+        auto new_motion = CreateNewMotion(data);
+        
+        // Copy old data to fill gap before incoming data
+        if (merge_dst_frame > 0) {
+            CopyOldDataToNewMotion(
+                streamed_motion_,
+                stream_window_start_,
+                new_motion,
+                new_window_start,
+                incoming_frame_start,
+                frame_step,
+                data
+            );
+        }
+        
+        // Copy incoming data to new motion
+        CopyIncomingDataToMotion(data, new_motion, merge_dst_frame);
+        
+        // Update total timesteps
+        new_motion->timesteps = merge_dst_frame + data.num_frames;
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[StreamedMotionMerger] Merged motion: " << new_motion->timesteps 
+                      << " frames (copied: " << merge_dst_frame << " + incoming: " << data.num_frames << ")" << std::endl;
+        }
+        
+        // Calculate frame offset adjustment BEFORE updating state
+        int old_window_start = stream_window_start_;
+        int window_shift_ticks = new_window_start - old_window_start;
+        int window_shift = (frame_step > 0) ? (window_shift_ticks / frame_step) : 0;
+        
+        // Update state
+        streamed_motion_ = new_motion;
+        stream_window_start_ = new_window_start;
+        
+        // Build result
+        result.motion = new_motion;
+        result.window_start = new_window_start;
+        result.frame_offset_adjustment = did_catchup ? 0 : window_shift;
+        result.did_catchup_reset = did_catchup;
+        result.frame_step = frame_step;
+        result.protocol_version = data.protocol_version;
+        
+        return result;
+    }
+    
+private:
+    std::shared_ptr<MotionSequence> streamed_motion_;
+    int stream_window_start_ = 0;
+    
+    // Validate incoming data structure
+    bool ValidateIncomingData(const IncomingData& data) const {
+        // Check required fields
+        if (data.body_quat.empty() || data.frame_indices.empty()) {
+            std::cerr << "[StreamedMotionMerger] Missing required fields (body_quat or frame_indices)" << std::endl;
+            return false;
+        }
+        
+        // Validate protocol-specific requirements
+        if (data.protocol_version == 3) {
+            // Version 3: requires both SMPL data AND joint data
+            if (data.smpl_joints.empty() || data.smpl_pose.empty()) {
+                std::cerr << "[StreamedMotionMerger] Protocol v3 missing smpl_joints or smpl_pose" << std::endl;
+                return false;
+            }
+            if (data.joint_pos.empty() || data.joint_vel.empty()) {
+                std::cerr << "[StreamedMotionMerger] Protocol v3 missing joint_pos or joint_vel" << std::endl;
+                return false;
+            }
+        } else if (data.protocol_version == 2) {
+            // Version 2: requires SMPL data (joint data optional)
+            if (data.smpl_joints.empty() || data.smpl_pose.empty()) {
+                std::cerr << "[StreamedMotionMerger] Protocol v2 missing smpl_joints or smpl_pose" << std::endl;
+                return false;
+            }
+        } else if (data.protocol_version == 1) {
+            // Version 1: requires joint data (SMPL data optional)
+            if (data.joint_pos.empty() || data.joint_vel.empty()) {
+                std::cerr << "[StreamedMotionMerger] Protocol v1 missing joint_pos or joint_vel" << std::endl;
+                return false;
+            }
+        } else {
+            std::cerr << "[StreamedMotionMerger] Unsupported protocol version: " << data.protocol_version << std::endl;
+            return false;
+        }
+        
+        return true;
+    }
+    
+    // Calculate frame step from frame indices
+    int CalculateFrameStep(const std::vector<int64_t>& frame_indices) const {
+        if (frame_indices.size() < 2) {
+            return 1;
+        }
+        int64_t step = std::abs(frame_indices[1] - frame_indices[0]);
+        return step > 0 ? static_cast<int>(step) : 1;
+    }
+    
+    // Calculate sliding window parameters
+    void CalculateSlidingWindow(
+        int incoming_frame_start,
+        int incoming_frame_end,
+        int frame_step,
+        int current_playback_frame,
+        int global_playback_frame,
+        bool catch_up_enabled,
+        int& new_window_start,
+        int& merge_dst_frame,
+        bool& did_catchup
+    ) {
+        // Special case: first packet
+        if (!streamed_motion_ || streamed_motion_->timesteps <= 0) {
+            new_window_start = incoming_frame_start;
+            merge_dst_frame = 0;
+            did_catchup = true;
+            return;
+        }
+        
+        // Calculate max gap based on catch_up flag
+        int max_gap_frames = catch_up_enabled 
+            ? (MAX_GAP_FRAMES + HISTORY_FRAMES) 
+            : std::numeric_limits<int>::max();
+        
+
+        int stream_window_end = stream_window_start_ + frame_step * (streamed_motion_->timesteps - 1);
+
+        if (DEBUG_LOGGING) {
+            std::cout << "[StreamedMotionMerger] incoming_frame_start: " << incoming_frame_start
+                      << ", incoming_frame_end: " << incoming_frame_end
+                      << ", stream_window_start_: " << stream_window_start_
+                      << ", stream_window_end: " << stream_window_end
+                      << ", frame_step: " << frame_step
+                      << ", global_playback_frame: " << global_playback_frame
+                      << ", streamed_motion_->timesteps: " << streamed_motion_->timesteps
+                      << std::endl;
+        }
+
+        // Check for incoming data older than current window
+        if (incoming_frame_start <= stream_window_start_) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[StreamedMotionMerger] WARNING: incoming_frame_start (" << incoming_frame_start
+                          << ") < stream_window_start_ (" << stream_window_start_ << ") - forcing catch-up" << std::endl;
+            }
+            new_window_start = incoming_frame_start;
+            merge_dst_frame = 0;
+            did_catchup = true;
+            return;
+        } else if (incoming_frame_end <= stream_window_end) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[StreamedMotionMerger] WARNING: incoming_frame_end (" << incoming_frame_end
+                          << ") <= stream_window_end (" << stream_window_end << ") - forcing catch-up" << std::endl;
+            }
+            new_window_start = incoming_frame_start;
+            merge_dst_frame = 0;
+            did_catchup = true;
+            return;
+        }
+        
+        // Tentative window aligned to playback
+        int desired_window_start = global_playback_frame;
+        int tentative_window_start = std::min(desired_window_start, incoming_frame_start);
+        int delta_to_incoming = incoming_frame_start - tentative_window_start;
+        int tentative_merge_dst = (frame_step > 0) ? (delta_to_incoming / frame_step) : 0;
+        
+        // Check for large gap
+        bool large_gap_from_old = incoming_frame_start > stream_window_end + frame_step;
+        
+        if (tentative_merge_dst > max_gap_frames || large_gap_from_old) {
+            // Catch-up: reset window to incoming frame
+            new_window_start = incoming_frame_start;
+            merge_dst_frame = 0;
+            did_catchup = true;
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[StreamedMotionMerger] CATCH-UP: gap too large or old data expired" << std::endl;
+            }
+        } else {
+            // Normal merge
+            new_window_start = tentative_window_start;
+            merge_dst_frame = tentative_merge_dst;
+        }
+    }
+    
+    // Create new motion sequence with appropriate capacity
+    std::shared_ptr<MotionSequence> CreateNewMotion(const IncomingData& data) const {
+        auto new_motion = std::make_shared<MotionSequence>();
+        new_motion->name = "streamed";
+        
+        int joints_to_reserve = data.num_joints;
+        int bodies_to_reserve = 1;
+        int body_quaternions_to_reserve = data.num_quat_bodies;
+        int smpl_joints_to_reserve = data.num_smpl_joints;
+        int smpl_poses_to_reserve = data.num_smpl_poses;
+        
+        new_motion->ReserveCapacity(
+            15000,
+            joints_to_reserve,
+            bodies_to_reserve,
+            body_quaternions_to_reserve,
+            smpl_joints_to_reserve,
+            smpl_poses_to_reserve
+        );
+        
+        // Initialize body_part_indexes (typically just root for streaming)
+        new_motion->SetBodyPartIndexes({0});
+        
+        return new_motion;
+    }
+    
+    // Copy old data to new motion to fill gap before incoming data
+    void CopyOldDataToNewMotion(
+        std::shared_ptr<MotionSequence> old_motion,
+        int old_window_start,
+        std::shared_ptr<MotionSequence> new_motion,
+        int new_window_start,
+        int incoming_frame_start,
+        int frame_step,
+        const IncomingData& data
+    ) {
+        if (!old_motion || old_motion->timesteps <= 0) {
+            return;
+        }
+        
+        int old_window_end = old_window_start + frame_step * old_motion->timesteps;
+        
+        // Find overlap between old data and needed range
+        int need_start_global = new_window_start;
+        int need_end_global = incoming_frame_start;
+        int overlap_start_global = std::max(need_start_global, old_window_start);
+        int overlap_end_global = std::min(need_end_global, old_window_end);
+        
+        if (overlap_start_global >= overlap_end_global) {
+            return;  // No overlap
+        }
+        
+        // Calculate copy parameters
+        int start_offset_old = overlap_start_global - old_window_start;
+        int start_offset_new = overlap_start_global - new_window_start;
+        int overlap_span = overlap_end_global - overlap_start_global;
+        int copy_src_idx = (frame_step > 0) ? (start_offset_old / frame_step) : 0;
+        int copy_dst_idx = (frame_step > 0) ? (start_offset_new / frame_step) : 0;
+        int copy_count = (frame_step > 0) ? (overlap_span / frame_step) : 0;
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[StreamedMotionMerger] Copying old data: "
+                      << "global [" << overlap_start_global << ".." << (overlap_end_global-1) << "] → "
+                      << "new_motion[" << copy_dst_idx << ".." << (copy_dst_idx + copy_count - 1) << "]" << std::endl;
+        }
+        
+        // Copy joint data if present
+        if (data.num_joints > 0 && old_motion->GetNumJoints() > 0) {
+            int joints_to_copy = std::min(data.num_joints, old_motion->GetNumJoints());
+            for (int i = 0; i < copy_count; ++i) {
+                for (int joint = 0; joint < joints_to_copy; ++joint) {
+                    new_motion->JointPositions(copy_dst_idx + i)[joint] = 
+                        old_motion->JointPositions(copy_src_idx + i)[joint];
+                    new_motion->JointVelocities(copy_dst_idx + i)[joint] = 
+                        old_motion->JointVelocities(copy_src_idx + i)[joint];
+                }
+            }
+        }
+        
+        // Copy body quaternions
+        int old_quat_bodies = old_motion->GetNumBodyQuaternions();
+        int quat_bodies_to_copy = std::min(data.num_quat_bodies, old_quat_bodies);
+        for (int i = 0; i < copy_count; ++i) {
+            for (int b = 0; b < quat_bodies_to_copy; ++b) {
+                for (int q = 0; q < 4; ++q) {
+                    new_motion->BodyQuaternions(copy_dst_idx + i)[b][q] = 
+                        old_motion->BodyQuaternions(copy_src_idx + i)[b][q];
+                }
+            }
+        }
+        
+        // Copy SMPL data if present
+        if (data.num_smpl_joints > 0 && old_motion->GetNumSmplJoints() > 0) {
+            int smpl_joints_to_copy = std::min(data.num_smpl_joints, old_motion->GetNumSmplJoints());
+            for (int i = 0; i < copy_count; ++i) {
+                for (int joint = 0; joint < smpl_joints_to_copy; ++joint) {
+                    for (int xyz = 0; xyz < 3; ++xyz) {
+                        new_motion->SmplJoints(copy_dst_idx + i)[joint][xyz] = 
+                            old_motion->SmplJoints(copy_src_idx + i)[joint][xyz];
+                    }
+                }
+            }
+        }
+        
+        if (data.num_smpl_poses > 0 && old_motion->GetNumSmplPoses() > 0) {
+            int smpl_poses_to_copy = std::min(data.num_smpl_poses, old_motion->GetNumSmplPoses());
+            for (int i = 0; i < copy_count; ++i) {
+                for (int p = 0; p < smpl_poses_to_copy; ++p) {
+                    for (int xyz = 0; xyz < 3; ++xyz) {
+                        new_motion->SmplPoses(copy_dst_idx + i)[p][xyz] = 
+                            old_motion->SmplPoses(copy_src_idx + i)[p][xyz];
+                    }
+                }
+            }
+        }
+    }
+    
+    // Copy incoming data to motion sequence
+    void CopyIncomingDataToMotion(
+        const IncomingData& data,
+        std::shared_ptr<MotionSequence> motion,
+        int dst_frame_offset
+    ) {
+        // Copy joint data if present
+        if (!data.joint_pos.empty() && !data.joint_vel.empty()) {
+            for (int frame = 0; frame < data.num_frames; ++frame) {
+                for (int joint = 0; joint < data.num_joints; ++joint) {
+                    motion->JointPositions(dst_frame_offset + frame)[joint] = data.joint_pos[frame][joint];
+                    motion->JointVelocities(dst_frame_offset + frame)[joint] = data.joint_vel[frame][joint];
+                }
+            }
+        }
+        
+        // Copy body quaternions (always present)
+        for (int frame = 0; frame < data.num_frames; ++frame) {
+            for (int body = 0; body < data.num_quat_bodies; ++body) {
+                for (int q = 0; q < 4; ++q) {
+                    motion->BodyQuaternions(dst_frame_offset + frame)[body][q] = 
+                        data.body_quat[frame][body][q];
+                }
+            }
+        }
+        
+        // Copy SMPL joints if present
+        if (!data.smpl_joints.empty()) {
+            for (int frame = 0; frame < data.num_frames; ++frame) {
+                for (int joint = 0; joint < data.num_smpl_joints; ++joint) {
+                    for (int xyz = 0; xyz < 3; ++xyz) {
+                        motion->SmplJoints(dst_frame_offset + frame)[joint][xyz] = 
+                            data.smpl_joints[frame][joint][xyz];
+                    }
+                }
+            }
+        }
+        
+        // Copy SMPL poses if present
+        if (!data.smpl_pose.empty()) {
+            for (int frame = 0; frame < data.num_frames; ++frame) {
+                for (int pose = 0; pose < data.num_smpl_poses; ++pose) {
+                    for (int xyz = 0; xyz < 3; ++xyz) {
+                        motion->SmplPoses(dst_frame_offset + frame)[pose][xyz] = 
+                            data.smpl_pose[frame][pose][xyz];
+                    }
+                }
+            }
+        }
+    }
+};
+
+#endif // STREAMED_MOTION_MERGER_HPP
+

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/zmq_endpoint_interface.hpp

@@ -0,0 +1,1865 @@
+/**
+ * @file zmq_endpoint_interface.hpp
+ * @brief ZMQ-based input interface for receiving streamed pose / motion data.
+ *
+ * ZMQEndpointInterface combines SimpleKeyboard-style local controls with
+ * network-streamed motion data received via the ZMQ packed-message protocol.
+ * Pressing **Enter** toggles between pre-loaded reference motions and live
+ * ZMQ streaming.
+ *
+ * ## Keyboard Controls (when this interface is active)
+ *
+ *   Key    | Action
+ *   -------|-------
+ *   Enter  | Toggle ZMQ streaming on/off
+ *   P/p    | Previous motion (non-streaming mode)
+ *   N/n    | Next motion
+ *   T/t    | Play / resume
+ *   R/r    | Restart (frame 0, paused)
+ *   ]      | Start control
+ *   O/o    | Emergency stop
+ *   Q/q    | Delta heading left
+ *   E/e    | Delta heading right
+ *   I/i    | Reinitialise heading
+ *
+ * ## Protocol Versions
+ *
+ * All versions carry `body_quat` and `frame_index` as required fields.
+ * Additionally:
+ *
+ *   Version | Required                         | Optional
+ *   --------|----------------------------------|---------------------------
+ *   1       | joint_pos, joint_vel             | smpl_joints, smpl_pose
+ *   2       | smpl_joints, smpl_pose           | joint_pos, joint_vel
+ *   3       | joint_pos, joint_vel, smpl_joints, smpl_pose | —
+ *
+ * ## Optional Fields (all versions)
+ *
+ *   - `left_hand_joints`, `right_hand_joints` – 7-DOF Dex3 joint values.
+ *   - `vr_position` (9 doubles) – enables VR 3-point tracking mode.
+ *   - `vr_orientation` (12 doubles) – defaults used if absent.
+ *   - `vr_compliance` (3 doubles) – **IGNORED** (compliance is keyboard-controlled).
+ *   - `catch_up` (bool, default true) – controls gap tolerance for motion sync.
+ *   - `heading_increment` (scalar) – incremental heading adjustment per message.
+ *
+ * ## Streaming Architecture
+ *
+ *   1. A background ZMQPackedMessageSubscriber thread receives messages and
+ *      copies them into `buffered_header_` / `buffered_buffers_` under `data_mutex_`.
+ *   2. update() reads keyboard input and resets per-frame flags.
+ *   3. handle_input() checks `has_new_data_`, decodes the buffered message via
+ *      DecodeIntoMotionSequence() (which delegates to StreamedMotionMerger for
+ *      sliding-window logic), and swaps the current_motion pointer.
+ */
+
+#ifndef ZMQ_ENDPOINT_INTERFACE_HPP
+#define ZMQ_ENDPOINT_INTERFACE_HPP
+
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <iostream>
+#include <iomanip>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <algorithm>
+#include <cstdlib>
+#include <limits>
+
+#include "input_interface.hpp"
+#include "zmq_packed_message_subscriber.hpp"
+#include "streamed_motion_merger.hpp"
+
+/**
+ * @class ZMQEndpointInterface
+ * @brief InputInterface that streams pose / motion data over ZMQ and merges
+ *        it into a MotionSequence for real-time playback.
+ *
+ * Can operate standalone (keyboard + network) or as a delegate inside
+ * InterfaceManager / GamepadManager / ZMQManager.
+ *
+ * Protocol Version 4 additionally supports token-only streaming:
+ *   REQUIRED: token_state (motion token array)
+ *   OPTIONAL: frame_index, left_hand_joints, right_hand_joints, body_quat_w
+ *   STORES:   token_state → external_token_state_ (for policy input)
+ *             hand joints → left_hand_joint_/right_hand_joint_ (for robot control)
+ */
+class ZMQEndpointInterface : public InputInterface {
+public:
+    /// Compile-time toggle for debug log output.
+    static constexpr bool DEBUG_LOGGING = true;
+    
+    // ------------------------------------------------------------------
+    // Per-frame action flags (reset at the start of every update() call)
+    // ------------------------------------------------------------------
+    bool motion_prev = false;      ///< Previous pre-loaded motion.
+    bool motion_next = false;      ///< Next pre-loaded motion.
+    bool play_motion = false;      ///< Play / resume.
+    bool motion_restart = false;   ///< Restart (frame 0, paused).
+    bool start_control = false;    ///< Start control system.
+    bool stop_control = false;     ///< Emergency stop.
+    bool delta_left = false;       ///< Heading nudge left.
+    bool delta_right = false;      ///< Heading nudge right.
+    bool reinitialize = false;     ///< Recapture IMU heading.
+    bool toggle_zmq_mode = false;  ///< Toggle ZMQ streaming on/off (Enter key).
+    bool report_temperature = false; ///< Report motor temperatures (F key).
+
+    /// When true, handle_input() reads from the ZMQ stream instead of
+    /// pre-loaded reference motions.
+    bool use_zmq_stream = false;
+    
+    /// Reusable sliding-window merger that handles frame alignment, gap
+    /// detection, and catch-up logic for streamed motion data.
+    StreamedMotionMerger motion_merger_;
+    
+    /// Protocol version established by the first received ZMQ message.
+    /// −1 = not yet established.  Changing mid-session is an error.
+    int active_protocol_version_ = -1;
+    
+    /// Shared pointer to the latest merged motion sequence from ZMQ data.
+    std::shared_ptr<MotionSequence> streamed_motion_;
+    /// Global frame index corresponding to streamed_motion_[0].
+    int stream_window_start_ = 0;
+
+    static constexpr std::string_view LOCALHOST = "localhost";
+
+    ZMQEndpointInterface(
+        const std::string& host = std::string(LOCALHOST),
+        int port = 5556,
+        const std::string& topic = "pose",
+        bool use_conflate = false,
+        bool verbose = false
+    ) : InputInterface(), host_(host), port_(port), topic_(topic), verbose_(verbose), is_localhost_(host == LOCALHOST) {
+        type_ = InputType::NETWORK;
+        
+        // Set terminal to non-blocking mode (same as SimpleKeyboard)
+        tcgetattr(STDIN_FILENO, &old_termios_);
+        struct termios new_termios = old_termios_;
+        new_termios.c_lflag &= ~(ICANON | ECHO);
+        tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
+        fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
+        
+        // Create ZMQ subscriber
+        subscriber_ = std::make_unique<ZMQPackedMessageSubscriber>(
+            host, port, topic,
+            /*timeout_ms=*/100,
+            verbose,
+            use_conflate,
+            /*rcv_hwm=*/ use_conflate ? 1 : 3
+        );
+        
+        // Setup callback to receive and buffer pose data
+        subscriber_->SetOnDecodedMessage(
+            [this](const std::string& topic,
+                   const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+                   const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+                this->OnPoseDataReceived(topic, hdr, bufs);
+            }
+        );
+        
+        // Start background receiving thread
+        subscriber_->Start();
+        
+        // Initialize streamed motion buffer (reserve large capacity for streaming)
+        ResetStreamedMotion();
+        
+        std::cout << "[ZMQEndpointInterface] Connected to " << host << ":" << port 
+                  << " topic='" << topic << "'" << std::endl;
+        std::cout << "[ZMQEndpointInterface] Press ENTER to toggle between loaded motions and ZMQ stream" << std::endl;
+    }
+    
+    ~ZMQEndpointInterface() {
+        if (subscriber_) {
+            subscriber_->Stop();
+        }
+        // Restore terminal
+        tcsetattr(STDIN_FILENO, TCSANOW, &old_termios_);
+    }
+    
+    // Flag to trigger safety reset in handle_input
+    bool trigger_safety_reset = false;
+
+    // Update is called each frame - read keyboard and check for network data
+    void update() override {
+        // Check for safety reset trigger from manager
+        if (CheckAndClearSafetyReset()) {
+            use_zmq_stream = false;
+            trigger_safety_reset = true;
+            std::cout << "[ZMQEndpointInterface] Safety reset triggered: will disable ZMQ streaming and return to reference motion" << std::endl;
+        }
+
+        // Reset input flags each frame
+        start_control = false;
+        stop_control = false;
+        motion_prev = false;
+        motion_next = false;
+        play_motion = false;
+        motion_restart = false;
+        delta_left = false;
+        delta_right = false;
+        reinitialize = false;
+        toggle_zmq_mode = false;
+        report_temperature = false;
+
+        // Read keyboard input (same as SimpleKeyboard, but without planner keys)
+        // Using shared buffered reading
+        char ch;
+        while (ReadStdinChar(ch)) {
+            switch (ch) {
+                case 'p':
+                case 'P': motion_prev = true; break;
+                case 'n':
+                case 'N': motion_next = true; break;
+                case 't':
+                case 'T': play_motion = true; break;
+                case 'r':
+                case 'R': motion_restart = true; break;
+                case ']': start_control = true; break;
+                case 'o':
+                case 'O': stop_control = true; break;
+                case 'f':
+                case 'F': report_temperature = true; break;
+                case 'q':
+                case 'Q': delta_left = true; break;
+                case 'e':
+                case 'E': delta_right = true; break;
+                case 'i':
+                case 'I': reinitialize = true; break;
+                case '\n': toggle_zmq_mode = true; break; // Toggle ZMQ streaming
+            }
+        }
+
+    }
+    
+    /// Disable ZMQ streaming, reset to reference motion, and clear external token state.
+    /// Called when an unrecoverable protocol error is detected during ZMQ processing.
+    void DisableZmqAndReset(
+        MotionDataReader& motion_reader,
+        std::shared_ptr<const MotionSequence>& current_motion,
+        int& current_frame,
+        OperatorState& operator_state,
+        bool& reinitialize_heading,
+        std::mutex& current_motion_mutex,
+        const std::string& reason)
+    {
+        std::cerr << "✗✗✗ ERROR: " << reason << std::endl;
+        std::cerr << "✗✗✗ This is not allowed. Exiting ZMQ streaming mode for safety." << std::endl;
+
+        use_zmq_stream = false;
+
+        {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            has_external_token_state_ = false;
+            external_token_state_.SetData({});
+            operator_state.play = false;
+            reinitialize_heading = true;
+            current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);
+            current_frame = 0;
+            if (current_motion->GetEncodeMode() >= 0) {
+                current_motion->SetEncodeMode(0);
+            }
+        }
+
+        std::cout << "=====================================" << std::endl;
+        std::cout << "ZMQ STREAMING MODE: FORCE DISABLED" << std::endl;
+        std::cout << "=====================================" << std::endl;
+        std::cout << "Returned to reference motion. Re-enable ZMQ mode to continue." << std::endl;
+    }
+
+    // Handle input and update motion data
+    void handle_input(MotionDataReader& motion_reader,
+                     std::shared_ptr<const MotionSequence>& current_motion,
+                     int& current_frame,
+                     OperatorState& operator_state,
+                     bool& reinitialize_heading,
+                     DataBuffer<HeadingState>& heading_state_buffer,
+                     bool has_planner,
+                     PlannerState& planner_state,
+                     DataBuffer<MovementState>& movement_state_buffer,
+                     std::mutex& current_motion_mutex,
+                     bool& report_temperature) override {
+        
+        // Handle safety reset from interface manager
+        if (trigger_safety_reset) {
+            trigger_safety_reset = false;
+            
+            movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                // Encoder mode will be read from the motion's encode_mode
+                has_external_token_state_ = false;
+                external_token_state_.SetData({});
+                operator_state.play = false;
+                reinitialize_heading = true;
+                auto temp_motion = std::make_shared<MotionSequence>(*current_motion);
+                temp_motion->name = "temporary_motion";
+                current_motion = temp_motion;
+                if (has_planner && planner_state.enabled) {
+                    planner_state.enabled = false;
+                    planner_state.initialized = false;
+                    std::cout << "Safety reset: Planner disabled" << std::endl;
+                }
+            }
+
+            // Disable ZMQ streaming and return to reference motion
+            use_zmq_stream = false;
+            ResetStreamedMotion(); // Reset motion merger and protocol version
+            
+            std::cout << "Safety reset: ZMQ streaming disabled, returned to reference motion at frame 0" << std::endl;
+        }
+
+        // Handle ZMQ mode toggle
+        if (toggle_zmq_mode) {
+            use_zmq_stream = !use_zmq_stream;
+            if (use_zmq_stream) {
+                std::cout << "=====================================" << std::endl;
+                std::cout << "ZMQ STREAMING MODE: ENABLED" << std::endl;
+                std::cout << "=====================================" << std::endl;
+                std::cout << "Using pose data from " << host_ << ":" << port_ << std::endl;
+                std::cout << "Press ENTER again to return to loaded motions" << std::endl;
+                // reset the heading state
+                {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    operator_state.play = false;
+                    reinitialize_heading = true; // reset the heading state
+                }
+                // reset streaming buffers when enabling to avoid mixing with stale data
+                ResetStreamedMotion(); // This also resets protocol version in the merger
+                has_new_data_ = false;
+            } else {
+                std::cout << "=====================================" << std::endl;
+                std::cout << "ZMQ STREAMING MODE: DISABLED" << std::endl;
+                std::cout << "=====================================" << std::endl;
+                std::cout << "Using pre-loaded motion data" << std::endl;
+                
+                // Encoder mode will be read from the motion's encode_mode
+                
+                // reset the current motion and frame
+                {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    has_external_token_state_ = false;
+                    external_token_state_.SetData({});
+                    operator_state.play = false;
+                    reinitialize_heading = true;
+                    current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_); // current motion is the pre-loaded motion
+                    current_frame = 0; // current frame is 0
+                    if (current_motion->GetEncodeMode() >= 0) {
+                        current_motion->SetEncodeMode(0);
+                    }
+                }
+                // reset the streamed motion (also resets protocol version)
+                ResetStreamedMotion();
+                has_new_data_ = false;
+            }
+        }
+        if (stop_control) { operator_state.stop = true; }
+        if (this->report_temperature) { report_temperature = true; }
+        if (start_control) { operator_state.start = true; }
+
+        // Handle delta heading controls
+        if (delta_left) {
+            auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+            HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+            double new_delta = current_state.delta_heading + 0.1;
+            heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+            std::cout << "Delta heading left: " << new_delta << " rad" << std::endl;
+        }
+
+        if (delta_right) {
+            auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+            HeadingState current_state = current_heading_state ? *current_heading_state : HeadingState();
+            double new_delta = current_state.delta_heading - 0.1;
+            heading_state_buffer.SetData(HeadingState(current_state.init_base_quat, new_delta));
+            std::cout << "Delta heading right: " << new_delta << " rad" << std::endl;
+        }
+
+        // If ZMQ mode is active, use streamed motion data
+        if (use_zmq_stream) {
+            // Check and decode new network data if available
+            std::shared_ptr<MotionSequence> new_motion;
+            int frame_offset_adjustment = 0;
+            bool did_catchup = false;
+            int protocol_version_for_mode_update = -1;
+            {
+                std::lock_guard<std::mutex> lock(data_mutex_);
+                if (has_new_data_) {
+                    has_new_data_ = false; // consumed
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ZMQEndpointInterface] *** Starting ZMQ processing ***" << std::endl;
+                    }
+                    // Decode into a new MotionSequence with current playback position
+                    auto result = DecodeIntoMotionSequence(current_frame, streamed_motion_, stream_window_start_, heading_state_buffer);
+                    
+                    // Handle Protocol v4 (token-only) - no motion, just tokens
+                    if (result.protocol_version == 4) {
+                        if (result.motion) {
+                            DisableZmqAndReset(motion_reader, current_motion, current_frame,
+                                               operator_state, reinitialize_heading, current_motion_mutex,
+                                               "Protocol version 4 with motion data is impossible!");
+                            return;
+                        }
+
+                        if (result.token_data.empty()) {
+                            DisableZmqAndReset(motion_reader, current_motion, current_frame,
+                                               operator_state, reinitialize_heading, current_motion_mutex,
+                                               "Protocol version 4 with empty token data!");
+                            return;
+                        }
+                        
+                        // Keep robot active
+                        {
+                            std::lock_guard<std::mutex> lock(current_motion_mutex);
+                            external_token_state_.SetData(result.token_data);
+                            has_external_token_state_ = true;
+                            operator_state.play = true; // this should be redundant because the robot never read reference motion
+                        }
+                        
+                        // Skip motion handling and keyboard controls
+                        return;
+                    }
+                    
+                    // Check if protocol version change was detected (error case)
+                    if (!result.motion && result.protocol_version != 0) {
+                        DisableZmqAndReset(motion_reader, current_motion, current_frame,
+                                           operator_state, reinitialize_heading, current_motion_mutex,
+                                           "Protocol version changed from " + std::to_string(active_protocol_version_)
+                                           + " to " + std::to_string(result.protocol_version)
+                                           + " during active ZMQ session!");
+                        return;
+                    }
+                    
+                    if (result.motion) {
+                        // Determine encode_mode based on protocol version (only once when first established)
+                        // Version 1: Use encoder mode 0 (joint-based)
+                        // Version 2/3: Use encoder mode 2 (SMPL-based)
+                        if constexpr (DEBUG_LOGGING) {
+                            std::cout << "[ZMQEndpointInterface] active_protocol_version_=" << active_protocol_version_ << std::endl;
+                            std::cout << "[ZMQEndpointInterface] result.motion->GetEncodeMode()=" << result.motion->GetEncodeMode() << std::endl;
+                        }
+                    
+                        
+                        new_motion = result.motion;
+                        std::cout << "[ZMQEndpointInterface] motion name: " << new_motion->name << std::endl;
+                        stream_window_start_ = result.window_start;
+                        frame_offset_adjustment = result.frame_offset_adjustment;
+                        did_catchup = result.did_catchup_reset;
+                        
+                        if constexpr (DEBUG_LOGGING) {
+                            int window_end_msg_idx = stream_window_start_ + result.frame_step * (new_motion->timesteps - 1);
+                            std::cout << "[ZMQEndpointInterface] Merged streamed data: " 
+                                      << new_motion->timesteps << " current-rate frames, "
+                                      << "window [" << stream_window_start_ << ".." << window_end_msg_idx << "] (message-index)"
+                                      << ", frame_step=" << result.frame_step
+                                      << ", frame_offset_adjustment=" << frame_offset_adjustment
+                                      << ", did_catchup=" << did_catchup << std::endl;
+                        }
+                    }
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ZMQEndpointInterface] *** End of ZMQ decoding processing ***" << std::endl;
+                    }
+                }
+            }
+            
+            // update streamed_motion_ and current_frame if we have new data
+            if (new_motion) {
+                streamed_motion_ = new_motion;
+                
+                // Handle catch-up reset: when window was reset due to large gap, start from beginning
+                if (did_catchup) {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    current_frame = 0;
+                    current_motion = streamed_motion_;  // Assign shared_ptr directly for thread safety
+                    operator_state.play = true; // Auto-play when entering ZMQ mode
+                    reinitialize_heading = true;
+                    
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ZMQEndpointInterface] Catch-up: Reset to frame 0 at global frame " 
+                                  << stream_window_start_ << std::endl;
+                    }
+                } else {
+                    // Normal case: Adjust current_frame to maintain global playback position after window shift
+                    // current_frame represents "the next frame to be read" (not yet consumed)
+                    int adjusted_frame = current_frame - frame_offset_adjustment;
+                    
+                    // Validate the adjustment doesn't cause discontinuities due to clamping
+                    if (adjusted_frame < 0) {
+                        if constexpr (DEBUG_LOGGING) {
+                            std::cout << "[ZMQEndpointInterface] WARNING: Window shifted past playback position. "
+                                      << "Skipping from global frame " << (stream_window_start_ - frame_offset_adjustment + current_frame)
+                                      << " to " << stream_window_start_ << std::endl;
+                        }
+                        adjusted_frame = 0; // Start from beginning of new window
+                    } else if (adjusted_frame >= streamed_motion_->timesteps) {
+                        if constexpr (DEBUG_LOGGING) {
+                            std::cout << "[ZMQEndpointInterface] WARNING: Playback position beyond new window. "
+                                      << "Clamping to last frame." << std::endl;
+                        }
+                        // Safety: ensure we don't set negative frame index if timesteps is 0
+                        adjusted_frame = (streamed_motion_->timesteps > 0) ? (streamed_motion_->timesteps - 1) : 0;
+                    }
+                    
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    current_frame = adjusted_frame;
+                    current_motion = streamed_motion_;  // Assign shared_ptr directly for thread safety
+                    operator_state.play = true; // Auto-play when entering ZMQ mode
+                }
+                
+            }
+            return; // Skip keyboard motion controls when in ZMQ mode
+        }
+        
+        // Standard keyboard controls (same as SimpleKeyboard, without planner)
+        if (motion_prev && !motion_reader.motions.empty()) {
+            motion_reader.current_motion_index_ =
+                (motion_reader.current_motion_index_ - 1 + motion_reader.motions.size()) % motion_reader.motions.size();
+            std::string motion_name;
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);
+                current_frame = 0;
+                motion_name = current_motion->name;
+                reinitialize_heading = true;
+            }
+        }
+
+        if (motion_next && !motion_reader.motions.empty()) {
+            motion_reader.current_motion_index_ = (motion_reader.current_motion_index_ + 1) % motion_reader.motions.size();
+            std::string motion_name;
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                current_motion = motion_reader.GetMotionShared(motion_reader.current_motion_index_);
+                current_frame = 0;
+                motion_name = current_motion->name;
+                reinitialize_heading = true;
+            }
+        }
+
+        if (play_motion) {
+            if (!operator_state.play) {
+                int frame_copy;
+                size_t timesteps_copy;
+                {
+                    std::lock_guard<std::mutex> lock(current_motion_mutex);
+                    operator_state.play = true;
+                    frame_copy = current_frame;
+                    timesteps_copy = current_motion ? current_motion->timesteps : 0;
+                }
+                std::cout << "Playing motion " << motion_reader.current_motion_index_ << " from frame " << frame_copy << " to end ("
+                          << timesteps_copy << " total frames)" << std::endl;
+            }
+        }
+
+        if (motion_restart) {
+            {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                operator_state.play = false;
+                current_frame = 0;
+                reinitialize_heading = true;
+            }
+            std::cout << "Reset motion " << motion_reader.current_motion_index_ << " to frame 0 (paused)" << std::endl;
+        }
+
+        // Handle reinitialize command
+        if (reinitialize) {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            reinitialize_heading = true;
+            std::cout << "Reinitialized base quaternion and reset delta heading to 0" << std::endl;
+        }
+    }
+
+    // Public method to trigger ZMQ mode toggle (for programmatic control from GamepadManager)
+    void TriggerZMQToggle() {
+        toggle_zmq_mode = true;
+    }
+
+    std::optional<std::chrono::steady_clock::time_point> GetLastUpdateTime() const override {
+      if (is_localhost_) {
+        return data_timestamp_;
+      }
+      return last_receive_time_;
+    }
+    
+private:
+    /// Reset the streamed motion buffer, merger state, and protocol version.
+    /// Called on construction, when toggling ZMQ mode, and on safety reset.
+    void ResetStreamedMotion() {
+        motion_merger_.Reset();
+        active_protocol_version_ = -1;  // Reset protocol version tracking
+        // Update legacy fields for backward compatibility
+        streamed_motion_ = std::make_shared<MotionSequence>();
+        streamed_motion_->name = "streamed";
+        streamed_motion_->ReserveCapacity(15000, 29, 1, 1, 0, 0); // max 15k frames, 29 joints, 1 body, 1 quat
+        stream_window_start_ = 0;
+        data_timestamp_.reset();
+        last_receive_time_.reset();
+    }
+    
+    /// Outcome of DecodeIntoMotionSequence().
+    struct DecodeResult {
+        std::shared_ptr<MotionSequence> motion;  ///< Merged motion (nullptr on failure / version change).
+        int window_start = 0;                     ///< Global frame index of motion[0].
+        int frame_offset_adjustment = 0;          ///< Subtract from current_frame for window shift.
+        bool did_catchup_reset = false;            ///< True → caller should reset playback to frame 0.
+        int frame_step = 1;                        ///< Detected stride between frame indices.
+        int protocol_version = 0;                  ///< Protocol version from the message (1, 2, or 3).
+        std::vector<double> token_data;            ///< Token data from the message.
+    };
+    
+    /**
+     * @brief Decode buffered network data into a new MotionSequence.
+     *
+     * Called from handle_input() (with data_mutex_ held) whenever `has_new_data_`
+     * is true.  This method:
+     *  1. Parses the buffered JSON header to determine field indices and dtypes.
+     *  2. Validates required fields for the detected protocol version.
+     *  3. Decodes binary buffers into typed C++ containers (joint_pos, body_quat, …).
+     *  4. Delegates to StreamedMotionMerger::MergeIncomingData() for sliding-window logic.
+     *  5. Sets encoder mode on the resulting motion based on protocol version.
+     *  6. Updates VR / hand-joint buffers if the corresponding optional fields are present.
+     *
+     * @param current_playback_frame  Current playback cursor in the old motion.
+     * @param old_motion              Previous streamed motion (for window overlap).
+     * @param old_window_start        Global frame index of old_motion[0].
+     * @param heading_state_buffer    Heading buffer (for heading_increment field).
+     * @return DecodeResult describing the merged motion and playback adjustments.
+     */
+    DecodeResult DecodeIntoMotionSequence(int current_playback_frame, 
+                                          std::shared_ptr<MotionSequence> old_motion,
+                                          int old_window_start,
+                                          DataBuffer<HeadingState>& heading_state_buffer) {
+        DecodeResult result;
+        if (buffered_buffers_.empty()) {
+            std::cerr << "[ZMQEndpointInterface] No buffered buffers" << std::endl;
+            return result;
+        }
+        
+        // Track timing between decode calls
+        uint64_t decode_start_time = std::chrono::steady_clock::now().time_since_epoch().count() / 1000000; // milliseconds
+        
+        // Check protocol version
+        int protocol_version = buffered_header_.version;
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ZMQEndpointInterface] Protocol version: " << protocol_version << std::endl;
+        }
+        
+        // Find expected fields by name (including frame_index for alignment)
+        int joint_pos_idx = -1, joint_vel_idx = -1, body_quat_idx = -1, frame_index_idx = -1, smpl_joints_idx = -1, smpl_pose_idx = -1;
+        int left_hand_joints_idx = -1, right_hand_joints_idx = -1, catch_up_idx = -1;
+        int token_state_idx = -1;  // Protocol v4: token-only streaming
+        int heading_increment_idx = -1;
+        int timestamp_monotonic_idx = -1;
+        // VR 3-point tracking fields (optional)
+        int vr_position_idx = -1, vr_orientation_idx = -1, vr_compliance_idx = -1;
+        
+        for (size_t i = 0; i < buffered_header_.fields.size(); ++i) {
+            const auto& f = buffered_header_.fields[i];
+            if (f.name == "joint_pos") joint_pos_idx = static_cast<int>(i);
+            else if (f.name == "joint_vel") joint_vel_idx = static_cast<int>(i);
+            else if (f.name == "body_quat_w" || f.name == "body_quat") body_quat_idx = static_cast<int>(i);
+            else if (f.name == "frame_index" || f.name == "last_smpl_global_frames") frame_index_idx = static_cast<int>(i);
+            else if (f.name == "smpl_joints") smpl_joints_idx = static_cast<int>(i);
+            else if (f.name == "smpl_pose") smpl_pose_idx = static_cast<int>(i);
+            else if (f.name == "left_hand_joints") left_hand_joints_idx = static_cast<int>(i);
+            else if (f.name == "right_hand_joints") right_hand_joints_idx = static_cast<int>(i);
+            else if (f.name == "catch_up") catch_up_idx = static_cast<int>(i);
+            else if (f.name == "token_state") token_state_idx = static_cast<int>(i);
+            else if (f.name == "heading_increment") heading_increment_idx = static_cast<int>(i);
+            else if (f.name == "timestamp_monotonic") timestamp_monotonic_idx = static_cast<int>(i);
+            // VR 3-point tracking fields
+            else if (f.name == "vr_position") vr_position_idx = static_cast<int>(i);
+            else if (f.name == "vr_orientation") vr_orientation_idx = static_cast<int>(i);
+            else if (f.name == "vr_compliance") vr_compliance_idx = static_cast<int>(i);
+        }
+        
+        // ===== PROTOCOL VERSION 4: Token-Only Streaming (check first, has different requirements) =====
+        if (protocol_version == 4) {
+            // Token-only mode - no motion data, just tokens for the policy
+            if (token_state_idx < 0) {
+                std::cerr << "[ZMQEndpointInterface] Version 4 missing required field 'token_state'" << std::endl;
+                return result;
+            }
+
+            // Check protocol version before decoding token_state
+            if (active_protocol_version_ == -1) {
+                // First message - establish protocol version
+                active_protocol_version_ = protocol_version;
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Protocol version " << active_protocol_version_ << " established" << std::endl;
+                }
+            } else if (active_protocol_version_ != protocol_version) {
+                // Protocol version changed - this is an error
+                std::cerr << "[ZMQEndpointInterface] ERROR: Protocol version changed from " 
+                        << active_protocol_version_ << " to " << protocol_version << std::endl;
+                result.protocol_version = protocol_version;  // Signal the change to caller
+                return result;
+            }
+
+            // Decode token_state field
+            const auto& token_field = buffered_header_.fields[static_cast<size_t>(token_state_idx)];
+            const auto& token_buf = buffered_buffers_[static_cast<size_t>(token_state_idx)];
+            
+            // Calculate token dimension from shape
+            size_t token_dim = 1;
+            for (size_t d : token_field.shape) {
+                token_dim *= d;
+            }
+            
+            std::vector<double> token_data(token_dim);
+            bool needs_swap = buffered_header_.NeedsByteSwap();
+            
+            if (token_field.dtype == "f32") {
+                for (size_t i = 0; i < token_dim; ++i) {
+                    float val;
+                    std::memcpy(&val, token_buf.data() + i * sizeof(float), sizeof(float));
+                    if (needs_swap) val = byte_swap(val);
+                    token_data[i] = static_cast<double>(val);
+                }
+            } else if (token_field.dtype == "f64") {
+                for (size_t i = 0; i < token_dim; ++i) {
+                    double val;
+                    std::memcpy(&val, token_buf.data() + i * sizeof(double), sizeof(double));
+                    if (needs_swap) val = byte_swap(val);
+                    token_data[i] = val;
+                }
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Version 4: unsupported dtype '" << token_field.dtype << "' for token_state" << std::endl;
+                return result;
+            }
+            
+            // Log for debugging (show first token value and frame info if available)
+            std::string frame_info = "";
+            if (frame_index_idx >= 0) {
+                const auto& frame_idx_field = buffered_header_.fields[static_cast<size_t>(frame_index_idx)];
+                const auto& frame_idx_buf = buffered_buffers_[static_cast<size_t>(frame_index_idx)];
+                if (frame_idx_field.dtype == "i64" && frame_idx_buf.size() >= sizeof(int64_t)) {
+                    int64_t frame_val;
+                    std::memcpy(&frame_val, frame_idx_buf.data(), sizeof(int64_t));
+                    if (needs_swap) frame_val = byte_swap(frame_val);
+                    frame_info = ", frame_index: " + std::to_string(frame_val);
+                } else if (frame_idx_field.dtype == "i64" && frame_idx_buf.size() > sizeof(int64_t)) {
+                    // Chunk mode: show range
+                    int num_frames = frame_idx_buf.size() / sizeof(int64_t);
+                    int64_t first_frame, last_frame;
+                    std::memcpy(&first_frame, frame_idx_buf.data(), sizeof(int64_t));
+                    std::memcpy(&last_frame, frame_idx_buf.data() + (num_frames - 1) * sizeof(int64_t), sizeof(int64_t));
+                    if (needs_swap) {
+                        first_frame = byte_swap(first_frame);
+                        last_frame = byte_swap(last_frame);
+                    }
+                    frame_info = ", frames: " + std::to_string(first_frame) + " to " + std::to_string(last_frame) 
+                               + " (chunk_size: " + std::to_string(num_frames) + ")";
+                }
+            }
+            std::cout << "[ZMQEndpointInterface] Protocol v4: Received " << token_dim 
+                      << "D token (latent action), tokens[0]=" << token_data[0] << frame_info << std::endl;
+            
+            // Store tokens in the external token state buffer (inherited from InputInterface)
+            result.token_data = std::move(token_data);
+            
+            // Decode hand joint positions if present (7 DOF joint values) - same as protocol v2/v3
+            bool has_left_hand_joints = (left_hand_joints_idx >= 0);
+            bool has_right_hand_joints = (right_hand_joints_idx >= 0);
+            auto [has_left_hand_v4, left_hand_joint_values] = GetHandPose(true);
+            auto [has_right_hand_v4, right_hand_joint_values] = GetHandPose(false);
+            
+            if (has_left_hand_joints) {
+                const auto& left_hand_field = buffered_header_.fields[left_hand_joints_idx];
+                const auto& left_hand_buf = buffered_buffers_[left_hand_joints_idx];
+                
+                // Validate shape: expect [7] or [N, 7] (for chunks, use first frame)
+                int num_hand_joints = 0;
+                if (left_hand_field.shape.size() == 1 && left_hand_field.shape[0] == 7) {
+                    num_hand_joints = 7;
+                } else if (left_hand_field.shape.size() == 2 && left_hand_field.shape[1] == 7) {
+                    num_hand_joints = 7;
+                }
+                
+                if (num_hand_joints == 7) {
+                    // Decode 7 joint values (from first frame if chunked [N, 7])
+                    if (left_hand_field.dtype == "f32") {
+                        for (int j = 0; j < 7; ++j) {
+                            float val;
+                            std::memcpy(&val, left_hand_buf.data() + j * sizeof(float), sizeof(float));
+                            if (needs_swap) val = byte_swap(val);
+                            left_hand_joint_values[j] = static_cast<double>(val);
+                        }
+                    } else if (left_hand_field.dtype == "f64") {
+                        for (int j = 0; j < 7; ++j) {
+                            double val;
+                            std::memcpy(&val, left_hand_buf.data() + j * sizeof(double), sizeof(double));
+                            if (needs_swap) val = byte_swap(val);
+                            left_hand_joint_values[j] = val;
+                        }
+                    }
+                } else {
+                    std::cerr << "[ZMQEndpointInterface] Protocol v4: Invalid left_hand_joints shape" << std::endl;
+                    has_left_hand_joints = false;
+                }
+            }
+            
+            if (has_right_hand_joints) {
+                const auto& right_hand_field = buffered_header_.fields[right_hand_joints_idx];
+                const auto& right_hand_buf = buffered_buffers_[right_hand_joints_idx];
+                
+                // Validate shape: expect [7] or [N, 7] (for chunks, use first frame)
+                int num_hand_joints = 0;
+                if (right_hand_field.shape.size() == 1 && right_hand_field.shape[0] == 7) {
+                    num_hand_joints = 7;
+                } else if (right_hand_field.shape.size() == 2 && right_hand_field.shape[1] == 7) {
+                    num_hand_joints = 7;
+                }
+                
+                if (num_hand_joints == 7) {
+                    // Decode 7 joint values (from first frame if chunked [N, 7])
+                    if (right_hand_field.dtype == "f32") {
+                        for (int j = 0; j < 7; ++j) {
+                            float val;
+                            std::memcpy(&val, right_hand_buf.data() + j * sizeof(float), sizeof(float));
+                            if (needs_swap) val = byte_swap(val);
+                            right_hand_joint_values[j] = static_cast<double>(val);
+                        }
+                    } else if (right_hand_field.dtype == "f64") {
+                        for (int j = 0; j < 7; ++j) {
+                            double val;
+                            std::memcpy(&val, right_hand_buf.data() + j * sizeof(double), sizeof(double));
+                            if (needs_swap) val = byte_swap(val);
+                            right_hand_joint_values[j] = val;
+                        }
+                    }
+                } else {
+                    std::cerr << "[ZMQEndpointInterface] Protocol v4: Invalid right_hand_joints shape" << std::endl;
+                    has_right_hand_joints = false;
+                }
+            }
+            
+            // Set hand joints if present
+            if (has_left_hand_joints || has_right_hand_joints) {
+                has_hand_joints_ = true;
+                
+                if (has_left_hand_joints) {
+                    left_hand_joint_.SetData(left_hand_joint_values);
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ZMQEndpointInterface] Protocol v4: Left hand joints set: [";
+                        for (int j = 0; j < 7; ++j) {
+                            if (j > 0) std::cout << ", ";
+                            std::cout << std::fixed << std::setprecision(4) << left_hand_joint_values[j];
+                        }
+                        std::cout << "]" << std::endl;
+                    }
+                }
+                
+                if (has_right_hand_joints) {
+                    right_hand_joint_.SetData(right_hand_joint_values);
+                    if constexpr (DEBUG_LOGGING) {
+                        std::cout << "[ZMQEndpointInterface] Protocol v4: Right hand joints set: [";
+                        for (int j = 0; j < 7; ++j) {
+                            if (j > 0) std::cout << ", ";
+                            std::cout << std::fixed << std::setprecision(4) << right_hand_joint_values[j];
+                        }
+                        std::cout << "]" << std::endl;
+                    }
+                }
+            }
+            
+            // Return success with protocol version but no motion (token-only)
+            result.protocol_version = 4;
+            return result;
+        }
+        
+        // Validate required fields based on protocol version (for motion protocols v1/v2/v3)
+        // body_quat and frame_index are required for motion protocols
+        if (body_quat_idx < 0) {
+            std::cerr << "[ZMQEndpointInterface] Missing required field 'body_quat' (or 'body_quat_w')" << std::endl;
+            return result;
+        }
+        
+        if (frame_index_idx < 0) {
+            std::cerr << "[ZMQEndpointInterface] Missing required field 'frame_index' (or 'last_smpl_global_frames')" << std::endl;
+            return result;
+        }
+        
+        if (protocol_version == 2 || protocol_version == 3) {
+            // Version 2/3: require smpl_joints, smpl_pose (joint_pos/joint_vel optional for v2, required for v3)
+            if (smpl_joints_idx < 0) {
+                std::cerr << "[ZMQEndpointInterface] Version " << protocol_version
+                          << " missing required field 'smpl_joints' " << std::endl;
+                return result;
+            }
+            if (smpl_pose_idx < 0) {
+                std::cerr << "[ZMQEndpointInterface] Version " << protocol_version
+                          << " missing required field 'smpl_pose'" << std::endl;
+                return result;
+            }
+            if (protocol_version == 3) {
+                // Version 3 additionally requires joint_pos and joint_vel
+                if (joint_pos_idx < 0 ) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 missing required field 'joint_pos'" << std::endl;
+                    return result;
+                }
+                if (joint_vel_idx < 0) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 missing required field 'joint_vel'" << std::endl;
+                    return result;
+                }
+            }
+        } else if (protocol_version == 1) {
+            // Version 1: requires joint_pos and joint_vel (smpl_joints optional)
+            if (joint_pos_idx < 0 || joint_vel_idx < 0) {
+                std::cerr << "[ZMQEndpointInterface] Version 1 missing required fields (joint_pos, joint_vel)" << std::endl;
+                return result;
+            }
+        } else {
+            // Protocol v4 is handled above, before body_quat/frame_index validation
+            std::cerr << "[ZMQEndpointInterface] Unsupported protocol version: " << protocol_version << std::endl;
+            return result;
+        }
+        
+        // Determine num_frames and num_joints from available fields
+        int num_frames = 0;
+        int num_joints = 0;
+        
+        // Get num_frames from the primary required field for each version
+        if (protocol_version == 2 || protocol_version == 3) {
+            // Version 2/3: Get num_frames from smpl_joints (required)
+            const auto& smpl_field = buffered_header_.fields[smpl_joints_idx];
+            if (smpl_field.shape.size() < 2) {
+                std::cerr << "[ZMQEndpointInterface] Invalid smpl_joints shape" << std::endl;
+                return result;
+            }
+            int num_frames_smpl = static_cast<int>(smpl_field.shape[0]);
+            if (num_frames_smpl <= 0) {
+                std::cerr << "[ZMQEndpointInterface] Invalid number of frames from smpl_joints: " << num_frames_smpl << std::endl;
+                return result;
+            }
+
+            // For version 3, also validate that joint_pos has consistent frame count
+            if (protocol_version == 3) {
+                const auto& joint_pos_field = buffered_header_.fields[joint_pos_idx];
+                if (joint_pos_field.shape.size() != 2) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 has invalid joint_pos shape (expected [N, num_joints])" << std::endl;
+                    return result;
+                }
+                const auto& joint_vel_field = buffered_header_.fields[joint_vel_idx];
+                if (joint_vel_field.shape.size() != 2) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 has invalid joint_vel shape (expected [N, num_joints])" << std::endl;
+                    return result;
+                }
+                int num_frames_joint = static_cast<int>(joint_pos_field.shape[0]);
+                if (num_frames_joint != num_frames_smpl) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 frame count mismatch between smpl_joints (" 
+                              << num_frames_smpl << ") and joint_pos (" << num_frames_joint << ")" << std::endl;
+                    return result;
+                }
+                int num_frames_joint_vel = static_cast<int>(joint_vel_field.shape[0]);
+                if (num_frames_joint_vel != num_frames_smpl) {
+                    std::cerr << "[ZMQEndpointInterface] Version 3 frame count mismatch between smpl_joints (" 
+                              << num_frames_smpl << ") and joint_vel (" << num_frames_joint_vel << ")" << std::endl;
+                    return result;
+                }
+            }
+            num_frames = num_frames_smpl;
+        } else if (protocol_version == 1) {
+            // Version 1: Get num_frames from joint_pos (required)
+            const auto& joint_pos_field = buffered_header_.fields[joint_pos_idx];
+            if (joint_pos_field.shape.size() != 2) {
+                std::cerr << "[ZMQEndpointInterface] Invalid joint_pos shape" << std::endl;
+                return result;
+            }
+            const auto& joint_vel_field = buffered_header_.fields[joint_vel_idx];
+            if (joint_vel_field.shape.size() != 2) {
+                std::cerr << "[ZMQEndpointInterface] Invalid joint_vel shape" << std::endl;
+                return result;
+            }
+            num_frames = static_cast<int>(joint_pos_field.shape[0]);
+            if (num_frames != static_cast<int>(joint_vel_field.shape[0])) {
+                std::cerr << "[ZMQEndpointInterface] Frame count mismatch between joint_pos and joint_vel" << std::endl;
+                return result;
+            }
+        }
+        
+        if (num_frames <= 0) {
+            std::cerr << "[ZMQEndpointInterface] Invalid number of frames: " << num_frames << std::endl;
+            return result;
+        }
+        
+        // Get num_joints if joint data is present
+        if (joint_pos_idx >= 0 && joint_vel_idx >= 0) {
+            const auto& joint_pos_field = buffered_header_.fields[joint_pos_idx];
+            const auto& joint_vel_field = buffered_header_.fields[joint_vel_idx];
+            
+            // Validate shapes: expect [N, num_joints]
+            if (joint_pos_field.shape.size() == 2 && joint_vel_field.shape.size() == 2) {
+                num_joints = static_cast<int>(joint_pos_field.shape[1]);
+                if (num_joints <= 0) {
+                    std::cerr << "[ZMQEndpointInterface] Invalid number of joints: " << num_joints << std::endl;
+                    return result;
+                }
+            }
+        }
+        
+        bool needs_swap = buffered_header_.NeedsByteSwap();
+        
+        // ===== STEP 1: Decode all incoming data into temporary buffers =====
+        
+        // Decode joint positions and velocities if present
+        std::vector<std::vector<double>> decoded_joint_pos;
+        std::vector<std::vector<double>> decoded_joint_vel;
+        bool has_joint_data = (joint_pos_idx >= 0 && joint_vel_idx >= 0 && num_joints > 0);
+        
+        if (has_joint_data) {
+            // Decode joint positions
+            decoded_joint_pos.resize(num_frames, std::vector<double>(num_joints));
+            const auto& joint_pos_field = buffered_header_.fields[joint_pos_idx];
+            const auto& pos_buf = buffered_buffers_[joint_pos_idx];
+            if (joint_pos_field.dtype == "f32") {
+                for (int frame = 0; frame < num_frames; ++frame) {
+                    for (int joint = 0; joint < num_joints; ++joint) {
+                        float val;
+                        std::memcpy(&val, pos_buf.data() + (frame * num_joints + joint) * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_joint_pos[frame][joint] = static_cast<double>(val);
+                    }
+                }
+            } else if (joint_pos_field.dtype == "f64") {
+                for (int frame = 0; frame < num_frames; ++frame) {
+                    for (int joint = 0; joint < num_joints; ++joint) {
+                        double val;
+                        std::memcpy(&val, pos_buf.data() + (frame * num_joints + joint) * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_joint_pos[frame][joint] = val;
+                    }
+                }
+            }
+            
+            // Decode joint velocities
+            decoded_joint_vel.resize(num_frames, std::vector<double>(num_joints));
+            const auto& joint_vel_field = buffered_header_.fields[joint_vel_idx];
+            const auto& vel_buf = buffered_buffers_[joint_vel_idx];
+            if (joint_vel_field.dtype == "f32") {
+                for (int frame = 0; frame < num_frames; ++frame) {
+                    for (int joint = 0; joint < num_joints; ++joint) {
+                        float val;
+                        std::memcpy(&val, vel_buf.data() + (frame * num_joints + joint) * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_joint_vel[frame][joint] = static_cast<double>(val);
+                    }
+                }
+            } else if (joint_vel_field.dtype == "f64") {
+                for (int frame = 0; frame < num_frames; ++frame) {
+                    for (int joint = 0; joint < num_joints; ++joint) {
+                        double val;
+                        std::memcpy(&val, vel_buf.data() + (frame * num_joints + joint) * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_joint_vel[frame][joint] = val;
+                    }
+                }
+            }
+        }
+        
+        // Decode body quaternions (required for both versions)
+        // Support shapes: [N, num_quat_bodies, 4] or [N, 4] for single body
+        const auto& quat_field = buffered_header_.fields[body_quat_idx];
+        const auto& quat_buf = buffered_buffers_[body_quat_idx];
+        
+        // Determine number of quaternion bodies from shape
+        int num_quat_bodies = 1;
+        if (quat_field.shape.size() == 3) {
+            num_quat_bodies = static_cast<int>(quat_field.shape[1]);
+        } else if (quat_field.shape.size() == 2) {
+            num_quat_bodies = 1;
+        }
+        
+        // Decode quaternions: [frame][body][xyzw]
+        std::vector<std::vector<std::array<double, 4>>> decoded_body_quat(num_frames);
+        for (int frame = 0; frame < num_frames; ++frame) {
+            decoded_body_quat[frame].resize(num_quat_bodies, {1.0, 0.0, 0.0, 0.0});
+        }
+        
+        int quat_stride = num_quat_bodies * 4;
+        
+        if (quat_field.dtype == "f32") {
+            for (int frame = 0; frame < num_frames; ++frame) {
+                for (int body = 0; body < num_quat_bodies; ++body) {
+                    for (int q = 0; q < 4; ++q) {
+                        float val;
+                        std::memcpy(&val, quat_buf.data() + (frame * quat_stride + body * 4 + q) * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_body_quat[frame][body][q] = static_cast<double>(val);
+                    }
+                }
+            }
+        } else if (quat_field.dtype == "f64") {
+            for (int frame = 0; frame < num_frames; ++frame) {
+                for (int body = 0; body < num_quat_bodies; ++body) {
+                    for (int q = 0; q < 4; ++q) {
+                        double val;
+                        std::memcpy(&val, quat_buf.data() + (frame * quat_stride + body * 4 + q) * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        decoded_body_quat[frame][body][q] = val;
+                    }
+                }
+            }
+        }
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ZMQEndpointInterface] Decoded body quaternions: " << num_quat_bodies << " bodies per frame" << std::endl;
+        }
+        
+        // Decode SMPL joints if present
+        // Expected shape: [N, num_smpl_joints, 3] or [N, 3] for single joint
+        std::vector<std::vector<std::array<double, 3>>> decoded_smpl_joints; // [frame][joint][xyz]
+        int num_smpl_joints = 0;
+        bool has_smpl_joints = (smpl_joints_idx >= 0);
+        
+        if (has_smpl_joints) {
+            const auto& smpl_field = buffered_header_.fields[smpl_joints_idx];
+            const auto& smpl_buf = buffered_buffers_[smpl_joints_idx];
+            
+            // Determine shape: [N, num_smpl_joints, 3] or [N, 3]
+            if (smpl_field.shape.size() == 3) {
+                num_smpl_joints = static_cast<int>(smpl_field.shape[1]);
+            } else if (smpl_field.shape.size() == 2) {
+                num_smpl_joints = 1;
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid smpl_joints shape dimensions: " 
+                          << smpl_field.shape.size() << std::endl;
+                has_smpl_joints = false; // Invalid shape, skip decoding
+            }
+            
+            if (has_smpl_joints && num_smpl_joints > 0) {
+                decoded_smpl_joints.resize(num_frames);
+                
+                int stride = num_smpl_joints * 3;
+                
+                if (smpl_field.dtype == "f32") {
+                    for (int frame = 0; frame < num_frames; ++frame) {
+                        decoded_smpl_joints[frame].resize(num_smpl_joints);
+                        for (int joint = 0; joint < num_smpl_joints; ++joint) {
+                            for (int xyz = 0; xyz < 3; ++xyz) {
+                                float val;
+                                std::memcpy(&val, smpl_buf.data() + (frame * stride + joint * 3 + xyz) * sizeof(float), sizeof(float));
+                                if (needs_swap) val = byte_swap(val);
+                                decoded_smpl_joints[frame][joint][xyz] = static_cast<double>(val);
+                            }
+                        }
+                    }
+                } else if (smpl_field.dtype == "f64") {
+                    for (int frame = 0; frame < num_frames; ++frame) {
+                        decoded_smpl_joints[frame].resize(num_smpl_joints);
+                        for (int joint = 0; joint < num_smpl_joints; ++joint) {
+                            for (int xyz = 0; xyz < 3; ++xyz) {
+                                double val;
+                                std::memcpy(&val, smpl_buf.data() + (frame * stride + joint * 3 + xyz) * sizeof(double), sizeof(double));
+                                if (needs_swap) val = byte_swap(val);
+                                decoded_smpl_joints[frame][joint][xyz] = val;
+                            }
+                        }
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded smpl_joints: " << num_frames 
+                              << " frames, " << num_smpl_joints << " joints" << std::endl;
+                }
+            }
+        }
+        
+        // Decode SMPL poses if present
+        // Expected shape: [N, num_poses, 3] or [N, 3] for single pose
+        std::vector<std::vector<std::array<double, 3>>> decoded_smpl_pose; // [frame][pose][xyz]
+        int num_smpl_poses = 0;
+        bool has_smpl_pose = (smpl_pose_idx >= 0);
+        
+        if (has_smpl_pose) {
+            const auto& smpl_pose_field = buffered_header_.fields[smpl_pose_idx];
+            const auto& smpl_pose_buf = buffered_buffers_[smpl_pose_idx];
+            
+            // Determine shape: [N, num_poses, 3] or [N, 3]
+            if (smpl_pose_field.shape.size() == 3) {
+                num_smpl_poses = static_cast<int>(smpl_pose_field.shape[1]);
+            } else if (smpl_pose_field.shape.size() == 2) {
+                num_smpl_poses = 1;
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid smpl_pose shape dimensions: " 
+                          << smpl_pose_field.shape.size() << std::endl;
+                has_smpl_pose = false; // Invalid shape, skip decoding
+            }
+            
+            if (has_smpl_pose && num_smpl_poses > 0) {
+                decoded_smpl_pose.resize(num_frames);
+                
+                int stride = num_smpl_poses * 3;
+                
+                if (smpl_pose_field.dtype == "f32") {
+                    for (int frame = 0; frame < num_frames; ++frame) {
+                        decoded_smpl_pose[frame].resize(num_smpl_poses);
+                        for (int pose = 0; pose < num_smpl_poses; ++pose) {
+                            for (int xyz = 0; xyz < 3; ++xyz) {
+                                float val;
+                                std::memcpy(&val, smpl_pose_buf.data() + (frame * stride + pose * 3 + xyz) * sizeof(float), sizeof(float));
+                                if (needs_swap) val = byte_swap(val);
+                                decoded_smpl_pose[frame][pose][xyz] = static_cast<double>(val);
+                            }
+                        }
+                    }
+                } else if (smpl_pose_field.dtype == "f64") {
+                    for (int frame = 0; frame < num_frames; ++frame) {
+                        decoded_smpl_pose[frame].resize(num_smpl_poses);
+                        for (int pose = 0; pose < num_smpl_poses; ++pose) {
+                            for (int xyz = 0; xyz < 3; ++xyz) {
+                                double val;
+                                std::memcpy(&val, smpl_pose_buf.data() + (frame * stride + pose * 3 + xyz) * sizeof(double), sizeof(double));
+                                if (needs_swap) val = byte_swap(val);
+                                decoded_smpl_pose[frame][pose][xyz] = val;
+                            }
+                        }
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded smpl_pose: " << num_frames 
+                              << " frames, " << num_smpl_poses << " poses" << std::endl;
+                }
+            }
+        }
+        
+        // Decode hand joint positions if present (7 DOF joint values)
+        bool has_left_hand_joints = (left_hand_joints_idx >= 0);
+        bool has_right_hand_joints = (right_hand_joints_idx >= 0);
+        auto [has_left_hand, left_hand_joint_values] = GetHandPose(true);
+        auto [has_right_hand, right_hand_joint_values] = GetHandPose(false);
+        
+        if (has_left_hand_joints) {
+            const auto& left_hand_field = buffered_header_.fields[left_hand_joints_idx];
+            const auto& left_hand_buf = buffered_buffers_[left_hand_joints_idx];
+            
+            // Validate shape: expect [7] or [1, 7]
+            int num_hand_joints = 0;
+            if (left_hand_field.shape.size() == 1 && left_hand_field.shape[0] == 7) {
+                num_hand_joints = 7;
+            } else if (left_hand_field.shape.size() == 2 && left_hand_field.shape[1] == 7) {
+                num_hand_joints = 7;
+            }
+            
+            if (num_hand_joints == 7) {
+                // Decode 7 joint values
+                if (left_hand_field.dtype == "f32") {
+                    for (int j = 0; j < 7; ++j) {
+                        float val;
+                        std::memcpy(&val, left_hand_buf.data() + j * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        left_hand_joint_values[j] = static_cast<double>(val);
+                    }
+                } else if (left_hand_field.dtype == "f64") {
+                    for (int j = 0; j < 7; ++j) {
+                        double val;
+                        std::memcpy(&val, left_hand_buf.data() + j * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        left_hand_joint_values[j] = val;
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded left_hand_joints: [";
+                    for (int j = 0; j < 7; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(4) << left_hand_joint_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid left_hand_joints shape" << std::endl;
+                has_left_hand_joints = false;
+            }
+        }
+        
+        if (has_right_hand_joints) {
+            const auto& right_hand_field = buffered_header_.fields[right_hand_joints_idx];
+            const auto& right_hand_buf = buffered_buffers_[right_hand_joints_idx];
+            
+            // Validate shape: expect [7] or [1, 7]
+            int num_hand_joints = 0;
+            if (right_hand_field.shape.size() == 1 && right_hand_field.shape[0] == 7) {
+                num_hand_joints = 7;
+            } else if (right_hand_field.shape.size() == 2 && right_hand_field.shape[1] == 7) {
+                num_hand_joints = 7;
+            }
+            
+            if (num_hand_joints == 7) {
+                // Decode 7 joint values
+                if (right_hand_field.dtype == "f32") {
+                    for (int j = 0; j < 7; ++j) {
+                        float val;
+                        std::memcpy(&val, right_hand_buf.data() + j * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        right_hand_joint_values[j] = static_cast<double>(val);
+                    }
+                } else if (right_hand_field.dtype == "f64") {
+                    for (int j = 0; j < 7; ++j) {
+                        double val;
+                        std::memcpy(&val, right_hand_buf.data() + j * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        right_hand_joint_values[j] = val;
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded right_hand_joints: [";
+                    for (int j = 0; j < 7; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(4) << right_hand_joint_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid right_hand_joints shape" << std::endl;
+                has_right_hand_joints = false;
+            }
+        }
+        
+        // ===== Decode VR 3-point tracking data if present =====
+        // VR 3-point format:
+        //   vr_position: 9 doubles (left wrist xyz, right wrist xyz, head xyz) - REQUIRED for VR mode
+        //   vr_orientation: 12 doubles (left quat wxyz, right quat wxyz, head quat wxyz) - optional
+        //   vr_compliance: 3 doubles (left_arm, right_arm, head compliance) - optional
+        bool has_vr_position = (vr_position_idx >= 0);
+        bool has_vr_orientation = (vr_orientation_idx >= 0);
+        bool has_vr_compliance = (vr_compliance_idx >= 0);
+        
+        // Default values for VR 3-point (from InputInterface defaults)
+        std::array<double, 9> vr_position_values = {
+            0.0903,  0.1615, -0.2411,   // left wrist xyz
+            0.1280, -0.1522, -0.2461,   // right wrist xyz
+            0.0241, -0.0081,  0.4028    // head xyz
+        };
+        std::array<double, 12> vr_orientation_values = {
+            0.7295,  0.3145,  0.5533, -0.2506,   // left quat (w,x,y,z)
+            0.7320, -0.2639,  0.5395,  0.3217,   // right quat (w,x,y,z)
+            0.9991,  0.011,   0.0402, -0.0002    // head quat (w,x,y,z)
+        };
+        std::array<double, 3> vr_compliance_values = GetVR3PointCompliance();  // Use keyboard-controlled compliance
+        
+        if (has_vr_position) {
+            const auto& vr_pos_field = buffered_header_.fields[vr_position_idx];
+            const auto& vr_pos_buf = buffered_buffers_[vr_position_idx];
+            
+            // Validate shape: expect [9] or [1, 9] or [3, 3]
+            size_t total_elements = 1;
+            for (auto dim : vr_pos_field.shape) total_elements *= dim;
+            
+            if (total_elements == 9) {
+                if (vr_pos_field.dtype == "f32") {
+                    for (int j = 0; j < 9; ++j) {
+                        float val;
+                        std::memcpy(&val, vr_pos_buf.data() + j * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        vr_position_values[j] = static_cast<double>(val);
+                    }
+                } else if (vr_pos_field.dtype == "f64") {
+                    for (int j = 0; j < 9; ++j) {
+                        double val;
+                        std::memcpy(&val, vr_pos_buf.data() + j * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        vr_position_values[j] = val;
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded vr_position: [";
+                    for (int j = 0; j < 9; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        if (j == 3 || j == 6) std::cout << " | ";
+                        std::cout << std::fixed << std::setprecision(4) << vr_position_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid vr_position shape (expected 9 elements, got " 
+                          << total_elements << ")" << std::endl;
+                has_vr_position = false;
+            }
+        }
+        
+        if (has_vr_orientation) {
+            const auto& vr_orient_field = buffered_header_.fields[vr_orientation_idx];
+            const auto& vr_orient_buf = buffered_buffers_[vr_orientation_idx];
+            
+            // Validate shape: expect [12] or [1, 12] or [3, 4]
+            size_t total_elements = 1;
+            for (auto dim : vr_orient_field.shape) total_elements *= dim;
+            
+            if (total_elements == 12) {
+                if (vr_orient_field.dtype == "f32") {
+                    for (int j = 0; j < 12; ++j) {
+                        float val;
+                        std::memcpy(&val, vr_orient_buf.data() + j * sizeof(float), sizeof(float));
+                        if (needs_swap) val = byte_swap(val);
+                        vr_orientation_values[j] = static_cast<double>(val);
+                    }
+                } else if (vr_orient_field.dtype == "f64") {
+                    for (int j = 0; j < 12; ++j) {
+                        double val;
+                        std::memcpy(&val, vr_orient_buf.data() + j * sizeof(double), sizeof(double));
+                        if (needs_swap) val = byte_swap(val);
+                        vr_orientation_values[j] = val;
+                    }
+                }
+                
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Decoded vr_orientation: [";
+                    for (int j = 0; j < 12; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        if (j == 4 || j == 8) std::cout << " | ";
+                        std::cout << std::fixed << std::setprecision(4) << vr_orientation_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            } else {
+                std::cerr << "[ZMQEndpointInterface] Invalid vr_orientation shape (expected 12 elements, got " 
+                          << total_elements << ")" << std::endl;
+                has_vr_orientation = false;
+            }
+        }
+        
+        // Note: vr_compliance from ZMQ is intentionally IGNORED
+        // We always use the keyboard-controlled compliance values (g/h/b/v keys)
+        // This keeps compliance control consistent across all input modes
+        if (has_vr_compliance) {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] vr_compliance field present but IGNORED (using keyboard-controlled values instead)" << std::endl;
+            }
+        }
+        
+        // ===== STEP 2: Decode frame indices =====
+        // Note: The merger will calculate frame_step and incoming_frame_start internally
+        std::vector<int64_t> frame_indices;
+        
+        if (frame_index_idx >= 0) {
+            const auto& frame_idx_field = buffered_header_.fields[frame_index_idx];
+            const auto& frame_idx_buf = buffered_buffers_[frame_index_idx];
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] Raw message field '" << frame_idx_field.name 
+                          << "' (dtype=" << frame_idx_field.dtype << ", size=" << frame_idx_buf.size() << " bytes)" << std::endl;
+            }
+            
+            if (frame_idx_field.dtype == "i32") {
+                int num_indices = frame_idx_buf.size() / sizeof(int32_t);
+                frame_indices.resize(num_indices);
+                
+                for (int i = 0; i < num_indices; ++i) {
+                    int32_t val;
+                    std::memcpy(&val, frame_idx_buf.data() + i * sizeof(int32_t), sizeof(int32_t));
+                    if (needs_swap) val = byte_swap(val);
+                    frame_indices[i] = val;
+                }
+            } else if (frame_idx_field.dtype == "i64") {
+                int num_indices = frame_idx_buf.size() / sizeof(int64_t);
+                frame_indices.resize(num_indices);
+                
+                for (int i = 0; i < num_indices; ++i) {
+                    int64_t val;
+                    std::memcpy(&val, frame_idx_buf.data() + i * sizeof(int64_t), sizeof(int64_t));
+                    if (needs_swap) val = byte_swap(val);
+                    frame_indices[i] = val;
+                }
+            }
+            
+            // Print frame indices for protocol v3 (SMPL actions)
+            if (protocol_version == 3 && !frame_indices.empty()) {
+                if (frame_indices.size() == 1) {
+                    std::cout << "[ZMQEndpointInterface] Protocol v3: Received SMPL action (single) - frame_index: " 
+                              << frame_indices[0] << std::endl;
+                } else {
+                    std::cout << "[ZMQEndpointInterface] Protocol v3: Received SMPL action (chunk) - frames: " 
+                              << frame_indices[0] << " to " << frame_indices.back() 
+                              << ", chunk_size: " << frame_indices.size() << std::endl;
+                }
+            }
+        }
+
+        // Optional: decode heading_increment (single scalar, f32 or f64)
+        if (heading_increment_idx >= 0) {
+          double heading_increment = 0.0;
+          const auto& dh_buf = buffered_buffers_[heading_increment_idx];
+          const auto& dh_field = buffered_header_.fields[heading_increment_idx];
+          if (dh_field.dtype == "f32") {
+            float val = 0.0f;
+            if (dh_buf.size() >= sizeof(float)) {
+              std::memcpy(&val, dh_buf.data(), sizeof(float));
+              if (needs_swap) val = byte_swap(val);
+              heading_increment = static_cast<double>(val);
+            }
+          } else { // f64 or default
+            double val = 0.0;
+            if (dh_buf.size() >= sizeof(double)) {
+              std::memcpy(&val, dh_buf.data(), sizeof(double));
+              if (needs_swap) val = byte_swap(val);
+              heading_increment = val;
+            }
+          }
+
+          auto current_heading_state = heading_state_buffer.GetDataWithTime().data;
+          HeadingState current_state =
+            current_heading_state ? *current_heading_state : HeadingState();
+
+          // Add increment to current heading
+          heading_state_buffer.SetData(
+              HeadingState(
+                current_state.init_base_quat,
+                current_state.delta_heading + heading_increment));
+        }
+
+        // Optional: decode monotonic timestamp (single scalar, f64)
+        if (timestamp_monotonic_idx >= 0) {
+          double timestamp_monotonic = 0.0;
+          const auto& ts_buf = buffered_buffers_[timestamp_monotonic_idx];
+          const auto& ts_field = buffered_header_.fields[timestamp_monotonic_idx];
+          if (ts_field.dtype == "f64") {
+            double val = 0.0;
+            if (ts_buf.size() >= sizeof(double)) {
+              std::memcpy(&val, ts_buf.data(), sizeof(double));
+              if (needs_swap) val = byte_swap(val);
+              timestamp_monotonic = val;
+            }
+          }
+          if (is_localhost_)
+          {
+            auto duration_monotonic = std::chrono::duration<double>(timestamp_monotonic);
+            auto time_point_monotonic = std::chrono::steady_clock::time_point(
+                std::chrono::duration_cast<std::chrono::steady_clock::duration>(duration_monotonic));
+            data_timestamp_ = time_point_monotonic;
+          }
+        }
+
+        // ===== Decode catch_up field if present =====
+        // Default: catch_up = true (use MAX_GAP_FRAMES)
+        // If catch_up = false: allow infinite delays (set max_gap_frames to very large value)
+        bool catch_up_enabled = true; // Default to true if field not present
+        if (catch_up_idx >= 0) {
+            const auto& catch_up_field = buffered_header_.fields[catch_up_idx];
+            const auto& catch_up_buf = buffered_buffers_[catch_up_idx];
+            
+            // Decode boolean value (support bool, i32, i64, u8)
+            if (catch_up_field.dtype == "bool" || catch_up_field.dtype == "u8") {
+                uint8_t val = 0;
+                if (catch_up_buf.size() >= sizeof(uint8_t)) {
+                    std::memcpy(&val, catch_up_buf.data(), sizeof(uint8_t));
+                    catch_up_enabled = (val != 0);
+                }
+            } else if (catch_up_field.dtype == "i32") {
+                int32_t val = 0;
+                if (catch_up_buf.size() >= sizeof(int32_t)) {
+                    std::memcpy(&val, catch_up_buf.data(), sizeof(int32_t));
+                    if (needs_swap) val = byte_swap(val);
+                    catch_up_enabled = (val != 0);
+                }
+            } else if (catch_up_field.dtype == "i64") {
+                int64_t val = 0;
+                if (catch_up_buf.size() >= sizeof(int64_t)) {
+                    std::memcpy(&val, catch_up_buf.data(), sizeof(int64_t));
+                    if (needs_swap) val = byte_swap(val);
+                    catch_up_enabled = (val != 0);
+                }
+            }
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] catch_up field: " << (catch_up_enabled ? "true" : "false") << std::endl;
+            }
+        } else {
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] catch_up field not present, using default: true" << std::endl;
+            }
+        }
+        
+        // ===== DEBUG: Print merged frame indices and decoded data =====
+        if constexpr (DEBUG_LOGGING) {
+            // Print first 20 frames (or all frames if fewer than 20)
+            int print_frames = std::min(20, num_frames);
+            
+            std::cout << "[ZMQEndpointInterface] Decoded data (Version " << protocol_version << ", first " << print_frames << " frames";
+            if (has_joint_data) std::cout << ", " << num_joints << " joints";
+            if (has_smpl_joints) std::cout << ", " << num_smpl_joints << " smpl_joints";
+            if (has_smpl_pose) std::cout << ", " << num_smpl_poses << " smpl_pose";
+            std::cout << "):" << std::endl;
+            
+            for (int frame = 0; frame < print_frames; ++frame) {
+                // Print frame index if available
+                std::cout << "  Frame[" << frame << "]";
+                if (frame < static_cast<int>(frame_indices.size())) {
+                    std::cout << " (idx=" << frame_indices[frame] << ")";
+                }
+                
+                // Print joint_pos and joint_vel if present
+                if (has_joint_data && !decoded_joint_pos.empty() && !decoded_joint_vel.empty()) {
+                    int print_joints = std::min(2, num_joints);
+                    std::cout << " joint_pos: [";
+                    for (int j = 0; j < print_joints; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(6) << decoded_joint_pos[frame][j];
+                    }
+                    std::cout << "], joint_vel: [";
+                    for (int j = 0; j < print_joints; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(6) << decoded_joint_vel[frame][j];
+                    }
+                    std::cout << "]";
+                }
+                
+                // Print body_quat (always present)
+                std::cout << ", body_quat: [";
+                int print_quat_bodies = std::min(2, static_cast<int>(decoded_body_quat[frame].size()));
+                for (int b = 0; b < print_quat_bodies; ++b) {
+                    if (b > 0) std::cout << "; ";
+                    std::cout << "(";
+                    for (int q = 0; q < 4; ++q) {
+                        if (q > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(6) << decoded_body_quat[frame][b][q];
+                    }
+                    std::cout << ")";
+                }
+                std::cout << "]";
+                
+                // Print smpl_joints if present
+                if (has_smpl_joints && frame < static_cast<int>(decoded_smpl_joints.size())) {
+                    std::cout << ", smpl_joints: [";
+                    int print_bodies = std::min(1, static_cast<int>(decoded_smpl_joints[frame].size()));
+                    for (int b = 0; b < print_bodies; ++b) {
+                        if (b > 0) std::cout << "; ";
+                        std::cout << "(";
+                        for (int xyz = 0; xyz < 3; ++xyz) {
+                            if (xyz > 0) std::cout << ", ";
+                            std::cout << std::fixed << std::setprecision(6) << decoded_smpl_joints[frame][b][xyz];
+                        }
+                        std::cout << ")";
+                    }
+                    std::cout << "]";
+                }
+                
+                // Print smpl_pose if present
+                if (has_smpl_pose && frame < static_cast<int>(decoded_smpl_pose.size())) {
+                    std::cout << ", smpl_pose: [";
+                    int print_poses = std::min(1, static_cast<int>(decoded_smpl_pose[frame].size()));
+                    for (int p = 0; p < print_poses; ++p) {
+                        if (p > 0) std::cout << "; ";
+                        std::cout << "(";
+                        for (int xyz = 0; xyz < 3; ++xyz) {
+                            if (xyz > 0) std::cout << ", ";
+                            std::cout << std::fixed << std::setprecision(6) << decoded_smpl_pose[frame][p][xyz];
+                        }
+                        std::cout << ")";
+                    }
+                    std::cout << "]";
+                }
+                
+                std::cout << std::endl;
+            }
+        }
+        
+        // ===== STEP 3: Validate protocol version (application-specific) =====
+        
+        // Check protocol version before merging
+        if (active_protocol_version_ == -1) {
+            // First message - establish protocol version
+            active_protocol_version_ = protocol_version;
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] Protocol version " << active_protocol_version_ << " established" << std::endl;
+            }
+        } else if (active_protocol_version_ != protocol_version) {
+            // Protocol version changed - this is an error
+            std::cerr << "[ZMQEndpointInterface] ERROR: Protocol version changed from " 
+                      << active_protocol_version_ << " to " << protocol_version << std::endl;
+            result.protocol_version = protocol_version;  // Signal the change to caller
+            return result;
+        }
+        
+        // ===== STEP 4: Package decoded data and call StreamedMotionMerger =====
+        
+        // Prepare IncomingData structure for the merger
+        StreamedMotionMerger::IncomingData incoming_data;
+        incoming_data.joint_pos = std::move(decoded_joint_pos);
+        incoming_data.joint_vel = std::move(decoded_joint_vel);
+        incoming_data.body_quat = std::move(decoded_body_quat);
+        incoming_data.smpl_joints = std::move(decoded_smpl_joints);
+        incoming_data.smpl_pose = std::move(decoded_smpl_pose);
+        incoming_data.frame_indices = std::move(frame_indices);
+        incoming_data.protocol_version = protocol_version;
+        incoming_data.catch_up_enabled = catch_up_enabled;
+        incoming_data.num_frames = num_frames;
+        incoming_data.num_joints = num_joints;
+        incoming_data.num_quat_bodies = num_quat_bodies;
+        incoming_data.num_smpl_joints = num_smpl_joints;
+        incoming_data.num_smpl_poses = num_smpl_poses;
+        
+        // Call the reusable merger to handle sliding window logic
+        auto merge_result = motion_merger_.MergeIncomingData(incoming_data, current_playback_frame);
+        
+        // Check for merge failure
+        if (!merge_result.motion) {
+            std::cerr << "[ZMQEndpointInterface] Failed to merge incoming data" << std::endl;
+            return result;
+        }
+        
+        // Convert MergeResult to DecodeResult
+        if (active_protocol_version_ == 1) {
+            merge_result.motion->SetEncodeMode(0);  // Protocol 1: joint-based
+        } else if (active_protocol_version_ == 2 || active_protocol_version_ == 3) {
+            // Protocol versions 2 and 3 both use encoder mode 2 (SMPL-based)
+            merge_result.motion->SetEncodeMode(2);
+        }
+        result.motion = merge_result.motion;
+        result.window_start = merge_result.window_start;
+        result.frame_offset_adjustment = merge_result.frame_offset_adjustment;
+        result.did_catchup_reset = merge_result.did_catchup_reset;
+        result.frame_step = merge_result.frame_step;
+        result.protocol_version = merge_result.protocol_version;
+        
+        // Handle hand joints: set hand joint values directly from decoded data
+        if (has_left_hand_joints || has_right_hand_joints) {
+            has_hand_joints_ = true;
+            
+            if (has_left_hand_joints) {
+                left_hand_joint_.SetData(left_hand_joint_values);
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Left hand joints set: [";
+                    for (int j = 0; j < 7; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(4) << left_hand_joint_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            }
+            
+            if (has_right_hand_joints) {
+                right_hand_joint_.SetData(right_hand_joint_values);
+                if constexpr (DEBUG_LOGGING) {
+                    std::cout << "[ZMQEndpointInterface] Right hand joints set: [";
+                    for (int j = 0; j < 7; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(4) << right_hand_joint_values[j];
+                    }
+                    std::cout << "]" << std::endl;
+                }
+            }
+        }
+        
+        // Handle VR 3-point tracking: set buffers when vr_position is present
+        // vr_position is required to enable VR mode; orientation uses default if not provided
+        // compliance is ALWAYS from keyboard-controlled values (ignoring ZMQ data)
+        if (has_vr_position) {
+            vr_3point_position_.SetData(vr_position_values);
+            vr_3point_orientation_.SetData(vr_orientation_values);
+            if (has_vr_compliance) SetVR3PointCompliance(vr_compliance_values);
+            has_vr_3point_control_ = true;
+            
+            if constexpr (DEBUG_LOGGING) {
+                std::cout << "[ZMQEndpointInterface] VR 3-point tracking ENABLED:" << std::endl;
+                std::cout << "  Position [L|R|H]: [";
+                for (int j = 0; j < 9; ++j) {
+                    if (j > 0) std::cout << ", ";
+                    if (j == 3 || j == 6) std::cout << "| ";
+                    std::cout << std::fixed << std::setprecision(4) << vr_position_values[j];
+                }
+                std::cout << "]" << std::endl;
+                std::cout << "  Orientation [L|R|H]: [";
+                for (int j = 0; j < 12; ++j) {
+                    if (j > 0) std::cout << ", ";
+                    if (j == 4 || j == 8) std::cout << "| ";
+                    std::cout << std::fixed << std::setprecision(4) << vr_orientation_values[j];
+                }
+                std::cout << "]" << (has_vr_orientation ? "" : " (default)") << std::endl;
+                if (has_vr_compliance) {
+                    std::cout << "  Compliance [L,R,H]: [";
+                    for (int j = 0; j < 3; ++j) {
+                        if (j > 0) std::cout << ", ";
+                        std::cout << std::fixed << std::setprecision(2) << vr_compliance_values[j];
+                    }
+                    std::cout << "] (keyboard-controlled)" << std::endl;
+                }
+            }
+        }
+
+        // log the decode interval and decode time
+        uint64_t decode_end_time = std::chrono::steady_clock::now().time_since_epoch().count() / 1000000; // milliseconds
+        if constexpr (DEBUG_LOGGING) {
+            if (last_decode_time_ > 0) {
+                uint64_t decode_time = decode_end_time - decode_start_time;
+                uint64_t time_delta = decode_end_time - last_decode_time_;
+                std::cout << "[ZMQEndpointInterface] Decode interval: " << time_delta << " ms, decode time: " << decode_time << " ms" << std::endl;
+            }
+        }
+        last_decode_time_ = decode_end_time;
+
+        return result;
+    }
+    
+    /**
+     * @brief ZMQ subscriber callback – invoked on the **background thread**.
+     *
+     * Copies the received header and buffer data into `buffered_header_` /
+     * `buffered_buffers_` under `data_mutex_` and sets `has_new_data_` = true.
+     * The actual decoding happens later on the main thread in handle_input().
+     */
+    void OnPoseDataReceived(
+        const std::string& topic,
+        const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+        const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+        
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        
+        // Print message received info
+        std::cout << "[ZMQEndpointInterface] Received ZMQ message - topic: '" << topic 
+                  << "', protocol_version: " << hdr.version 
+                  << ", num_fields: " << hdr.fields.size() 
+                  << ", total_size: " << bufs.size() << " buffers" << std::endl;
+        
+        // Buffer the received data for processing in handle_input (main thread)
+        buffered_header_ = hdr;
+        buffered_buffers_.clear();
+        for (const auto& buf : bufs) {
+            // Copy buffer data (BufferView is only valid during callback)
+            std::vector<uint8_t> copied(static_cast<const uint8_t*>(buf.data),
+                                       static_cast<const uint8_t*>(buf.data) + buf.size);
+            buffered_buffers_.push_back(std::move(copied));
+        }
+        
+        has_new_data_ = true;
+        last_receive_time_ = std::chrono::steady_clock::now();
+        receive_count_++;
+    }
+    
+    // ------------------------------------------------------------------
+    // Configuration
+    // ------------------------------------------------------------------
+    std::string host_;    ///< ZMQ server hostname.
+    int port_;            ///< ZMQ server port.
+    std::string topic_;   ///< ZMQ subscription topic.
+    bool verbose_;        ///< Verbose logging flag.
+    
+    /// Background subscriber for the pose / motion topic.
+    std::unique_ptr<ZMQPackedMessageSubscriber> subscriber_;
+    
+    struct termios old_termios_;  ///< Saved terminal state for restoration on destruction.
+    
+    // ------------------------------------------------------------------
+    // Thread-safe data buffering (written by ZMQ subscriber thread, read by input thread)
+    // ------------------------------------------------------------------
+    mutable std::mutex data_mutex_;           ///< Guards the fields below.
+    bool has_new_data_ = false;               ///< True when a new message is waiting to be decoded.
+    ZMQPackedMessageSubscriber::DecodedHeader buffered_header_;  ///< Latest JSON header.
+    std::vector<std::vector<uint8_t>> buffered_buffers_;         ///< Copied binary field data.
+    
+    // ------------------------------------------------------------------
+    // Timing / diagnostics
+    // ------------------------------------------------------------------
+    bool is_localhost_ = true;         ///< True if host_ is localhost (for directly comparing timestamps)
+    std::optional<std::chrono::steady_clock::time_point> data_timestamp_{};  ///< Timestamp of last received message from XR source
+    std::optional<std::chrono::steady_clock::time_point> last_receive_time_{}; ///< Timestamp of last OnPoseDataReceived (ms, monotonic).
+    uint64_t receive_count_ = 0;       ///< Total number of messages received.
+    uint64_t last_decode_time_ = 0;    ///< Timestamp of last DecodeIntoMotionSequence call (ms).
+    
+};
+
+#endif // ZMQ_ENDPOINT_INTERFACE_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/zmq_manager.hpp

@@ -0,0 +1,1257 @@
+/**
+ * @file zmq_manager.hpp
+ * @brief Network-only input manager that switches between planner mode and
+ *        streamed-motion mode, both driven by ZMQ topics.
+ *
+ * ZMQManager subscribes to **three** ZMQ topics on the same host:port:
+ *
+ *   Topic      | Purpose
+ *   -----------|--------
+ *   command    | High-level control (start / stop / mode switch).
+ *              | Wire format: `{ start: bool, stop: bool, planner: bool, delta_heading?: f32 }`
+ *   planner    | Per-frame locomotion commands (mode, movement, facing, speed, height,
+ *              | optional upper-body / hand / VR data).  Active in PLANNER mode.
+ *   pose       | Streamed motion frames (joint_pos, joint_vel, body_quat, …).
+ *              | Active in STREAMED_MOTION mode, handled by an internal ZMQEndpointInterface.
+ *
+ * ## Mode Switching
+ *
+ * The `planner` field in the command message selects the mode:
+ *   - `planner = true`  → PLANNER mode (movement commands from the planner topic).
+ *   - `planner = false` → STREAMED_MOTION mode (pose data from the pose topic).
+ *
+ * On each mode switch, safety resets are triggered and the planner buffer is
+ * cleared to prevent stale commands from leaking across modes.
+ *
+ * ## Planner Timeout
+ *
+ * If no planner message arrives within 1 second (PLANNER_TIMEOUT), the manager
+ * automatically resets the locomotion to IDLE and clears upper-body / hand-joint
+ * control flags.
+ *
+ * ## Keyboard Shortcuts (via stdin)
+ *
+ *   Key  | Action
+ *   -----|-------
+ *   O/o  | Emergency stop
+ *   g/G, h/H | Left-hand compliance ±0.1
+ *   b/B, v/V | Right-hand compliance ±0.1
+ *   x/X, c/C | Hand max-close ratio ±0.1
+ */
+
+#ifndef ZMQ_MANAGER_HPP
+#define ZMQ_MANAGER_HPP
+
+#include <memory>
+#include <vector>
+#include <iostream>
+#include <cstring>
+#include <cmath>
+#include <array>
+#include <thread>
+#include <chrono>
+#include <mutex>
+
+#include "input_interface.hpp"
+#include "input_command.hpp"
+#include "zmq_endpoint_interface.hpp"
+#include "zmq_packed_message_subscriber.hpp"
+#include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
+#include "../math_utils.hpp"  // For normalize_vector
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @class ZMQManager
+ * @brief InputInterface that manages two ZMQ-driven modes:
+ *        PLANNER (locomotion commands) and STREAMED_MOTION (pose data).
+ *
+ * Internally owns a ZMQEndpointInterface for streamed-motion mode and two
+ * ZMQPackedMessageSubscriber instances for the command and planner topics.
+ */
+class ZMQManager : public InputInterface {
+  public:
+    static constexpr bool DEBUG_LOGGING = false;
+
+    enum class ManagedMode {
+      PLANNER = 0,         // Planner-only mode (self-managed planner topic)
+      STREAMED_MOTION = 1  // ZMQ streamed motion mode (pose topic via ZMQEndpointInterface)
+    };
+
+    ZMQManager(
+      const std::string& zmq_host,
+      int zmq_port,
+      const std::string& pose_topic = "pose",
+      const std::string& command_topic = "command",
+      const std::string& planner_topic = "planner",
+      bool zmq_conflate = false,
+      bool zmq_verbose = false
+    ) : InputInterface(), 
+        zmq_host_(zmq_host), 
+        zmq_port_(zmq_port), 
+        pose_topic_(pose_topic),
+        command_topic_(command_topic),
+        planner_topic_(planner_topic),
+        zmq_conflate_(zmq_conflate), 
+        zmq_verbose_(zmq_verbose) {
+      
+      type_ = InputType::NETWORK;
+      active_mode_ = ManagedMode::PLANNER;  // Default to planner mode
+      
+      // Create pose interface (for streamed motion mode)
+      pose_interface_ = std::make_unique<ZMQEndpointInterface>(
+        zmq_host_, zmq_port_, pose_topic_, zmq_conflate_, zmq_verbose_
+      );
+      
+      // Create command subscriber
+      command_subscriber_ = std::make_unique<ZMQPackedMessageSubscriber>(
+        zmq_host_, zmq_port_, command_topic_,
+        /*timeout_ms=*/100,
+        zmq_verbose_,
+        /*use_conflate=*/false,
+        /*rcv_hwm=*/3
+      );
+      
+      command_subscriber_->SetOnDecodedMessage(
+        [this](const std::string& topic,
+               const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+               const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+          this->OnCommandReceived(topic, hdr, bufs);
+        }
+      );
+      
+      command_subscriber_->Start();
+      
+      // Create planner subscriber
+      planner_subscriber_ = std::make_unique<ZMQPackedMessageSubscriber>(
+        zmq_host_, zmq_port_, planner_topic_,
+        /*timeout_ms=*/100,
+        zmq_verbose_,
+        /*use_conflate=*/false, 
+        /*rcv_hwm=*/3
+      );
+      
+      planner_subscriber_->SetOnDecodedMessage(
+        [this](const std::string& topic,
+               const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+               const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+          this->OnPlannerReceived(topic, hdr, bufs);
+        }
+      );
+      
+      planner_subscriber_->Start();
+      
+      std::cout << "[ZMQManager] Initialized (default: PLANNER mode)" << std::endl;
+      std::cout << "  - Host: " << zmq_host_ << ":" << zmq_port_ << std::endl;
+      std::cout << "  - Command topic: '" << command_topic_ << "' (start/stop/mode)" << std::endl;
+      std::cout << "    Format: { start: bool, stop: bool, planner: bool }" << std::endl;
+      std::cout << "  - Planner topic: '" << planner_topic_ << "' (movement)" << std::endl;
+      std::cout << "  - Pose topic: '" << pose_topic_ << "' (streamed motion)" << std::endl;
+    }
+    
+    ~ZMQManager() {
+      if (command_subscriber_) command_subscriber_->Stop();
+      if (planner_subscriber_) planner_subscriber_->Stop();
+    }
+
+    void update() override {
+      // Reset per-frame flags
+      emergency_stop_ = false;
+      report_temperature_flag_ = false;
+      start_control_ = false;
+      stop_control_ = false;
+      
+      // Handle stdin shortcuts
+      char ch;
+      while (ReadStdinChar(ch)) {
+        bool is_manager_key = false;
+        switch (ch) {
+          case 'o':
+          case 'O':
+            emergency_stop_ = true;
+            is_manager_key = true;
+            std::cout << "[ZMQManager] EMERGENCY STOP (O/o)" << std::endl;
+            break;
+          case 'f':
+          case 'F':
+            report_temperature_flag_ = true;
+            is_manager_key = true;
+            break;
+          // Global compliance controls - work across ALL modes
+          case 'g':
+          case 'G':
+            // Increase left hand compliance by 0.1
+            AdjustLeftHandCompliance(0.1);
+            is_manager_key = true;
+            break;
+          case 'h':
+          case 'H':
+            // Decrease left hand compliance by 0.1
+            AdjustLeftHandCompliance(-0.1);
+            is_manager_key = true;
+            break;
+          case 'b':
+          case 'B':
+            // Increase right hand compliance by 0.1
+            AdjustRightHandCompliance(0.1);
+            is_manager_key = true;
+            break;
+          case 'v':
+          case 'V':
+            // Decrease right hand compliance by 0.1
+            AdjustRightHandCompliance(-0.1);
+            is_manager_key = true;
+            break;
+          // Global hand max close ratio controls (x/c keys)
+          case 'x':
+          case 'X':
+            // Increase max close ratio by 0.1 (allow hands to close more)
+            AdjustMaxCloseRatio(0.1);
+            is_manager_key = true;
+            break;
+          case 'c':
+          case 'C':
+            // Decrease max close ratio by 0.1 (keep hands more open)
+            AdjustMaxCloseRatio(-0.1);
+            is_manager_key = true;
+            break;
+        }
+
+        // Pass other keys to pose interface (only in streamed motion mode)
+        if (!is_manager_key && active_mode_ == ManagedMode::STREAMED_MOTION && pose_interface_) {
+          pose_interface_->PushStdinChar(ch);
+        }
+      }
+
+      // Translate received command to control flags and handle mode switching
+      bool trigger_zmq_toggle = false;
+      {
+        std::lock_guard<std::mutex> lock(command_mutex_);
+        if (latest_command_.valid) {
+          // Set control flags (already accumulated in callback)
+          if (latest_command_.start) {
+            start_control_ = true;
+          }
+          if (latest_command_.stop) {
+            stop_control_ = true;
+          }
+
+          // Handle mode switching
+          ManagedMode new_mode = latest_command_.planner ? ManagedMode::PLANNER : ManagedMode::STREAMED_MOTION;
+          
+          if (new_mode != active_mode_) {
+            // Trigger safety reset on mode switch
+            TriggerSafetyReset();
+            if (pose_interface_) {
+              pose_interface_->TriggerSafetyReset();
+            }
+
+            if (new_mode == ManagedMode::PLANNER) {
+              std::cout << "[ZMQManager] Switched to: PLANNER mode (safety reset)" << std::endl;
+              if (latest_planner_message_.valid) {
+                constexpr auto PLANNER_MESSAGE_TIMEOUT = std::chrono::milliseconds(100);
+                auto time_since_last_planner = std::chrono::steady_clock::now() - latest_planner_message_.timestamp;
+                if (time_since_last_planner < PLANNER_MESSAGE_TIMEOUT) {
+                  // Valid planner message within timeout - use it
+                  // Update upper body control state based on this message
+                  has_upper_body_control_ = latest_planner_message_.upper_body_position.has_value();
+
+                  // Update hand joints control state based on this message
+                  has_hand_joints_ = latest_planner_message_.left_hand_joints.has_value() || 
+                                     latest_planner_message_.right_hand_joints.has_value();
+                }
+              }
+            } else if (new_mode == ManagedMode::STREAMED_MOTION) {
+              std::cout << "[ZMQManager] Switched to: STREAMED MOTION mode (safety reset)" << std::endl;
+              trigger_zmq_toggle = true;
+
+              // Clear planner buffer when switching away from planner mode
+              {
+                std::lock_guard<std::mutex> lock(planner_mutex_);
+                latest_planner_message_.valid = false;
+                latest_planner_message_.timestamp = {};
+                is_planner_ready_ = false;
+                switch_from_teleop_to_planner_ = true;
+              }
+              std::cout << "[ZMQManager] Cleared planner buffer" << std::endl;
+            }
+          }
+
+          // Clear valid flag - next callback will start fresh accumulation
+          active_mode_ = new_mode;
+          latest_command_.valid = false;
+        }
+      }
+
+      // Update active interface based on mode
+      if (active_mode_ == ManagedMode::STREAMED_MOTION && pose_interface_) {
+        // In streamed motion mode: update pose interface
+        pose_interface_->update();
+        if (trigger_zmq_toggle) {
+          pose_interface_->TriggerZMQToggle();
+          std::cout << "[ZMQManager] ZMQ streaming enabled" << std::endl;
+        }
+      }
+    }
+
+    void handle_input(MotionDataReader& motion_reader,
+                      std::shared_ptr<const MotionSequence>& current_motion,
+                      int& current_frame,
+                      OperatorState& operator_state,
+                      bool& reinitialize_heading,
+                      DataBuffer<HeadingState>& heading_state_buffer,
+                      bool has_planner,
+                      PlannerState& planner_state,
+                      DataBuffer<MovementState>& movement_state_buffer,
+                      std::mutex& current_motion_mutex,
+                      bool& report_temperature) override {
+      if (!has_planner) {
+        std::cerr << "[ZMQCommandManager ERROR] Planner not available in planner mode" << std::endl;
+        operator_state.stop = true;
+        return;
+      }
+      // Emergency stop
+      if (report_temperature_flag_) {
+        report_temperature = true;
+        report_temperature_flag_ = false;
+      }
+      if (emergency_stop_) {
+        operator_state.stop = true;
+        if (planner_state.enabled) {
+          planner_state.enabled = false;
+          planner_state.initialized = false;
+        }
+        
+        // Clear planner buffer on emergency stop
+        {
+          std::lock_guard<std::mutex> lock(planner_mutex_);
+          latest_planner_message_.valid = false;
+          latest_planner_message_.timestamp = {};
+        }
+        // Clear upper body control state
+        has_upper_body_control_ = false;
+        
+        // Clear hand joints control state
+        has_hand_joints_ = false;
+        
+        return;
+      }
+
+      // Handle stop control
+      if (stop_control_) {
+        operator_state.stop = true;
+        if (planner_state.enabled) {
+          planner_state.enabled = false;
+          planner_state.initialized = false;
+        }
+        
+        // Clear planner buffer on stop
+        {
+          std::lock_guard<std::mutex> lock(planner_mutex_);
+          latest_planner_message_.valid = false;
+          latest_planner_message_.timestamp = {};
+        }
+        // Clear upper body control state
+        has_upper_body_control_ = false;
+        
+        // Clear hand joints control state
+        has_hand_joints_ = false;
+      }
+
+      // Delegate based on current mode
+      if (active_mode_ == ManagedMode::PLANNER) {
+        // Planner mode: handle planner input ourselves
+        handlePlannerInput(motion_reader, current_motion, current_frame,
+                          operator_state, reinitialize_heading,
+                          heading_state_buffer,
+                          has_planner, planner_state, movement_state_buffer,
+                          current_motion_mutex);
+      } else {
+        // Streamed motion mode: delegate to pose interface
+        if (pose_interface_) {
+          pose_interface_->handle_input(motion_reader, current_motion, current_frame,
+                                       operator_state, reinitialize_heading,
+                                       heading_state_buffer,
+                                       has_planner, planner_state, movement_state_buffer,
+                                       current_motion_mutex, report_temperature);
+        }
+      }
+    }
+
+    // Forward getters to pose interface when in streamed motion mode
+    bool HasVR3PointControl() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->HasVR3PointControl();
+      }
+      return has_vr_3point_control_;
+    }
+
+    bool HasHandJoints() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->HasHandJoints();
+      }
+      return has_hand_joints_;
+    }
+
+    bool HasExternalTokenState() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->HasExternalTokenState();
+      }
+      return has_external_token_state_;
+    }
+
+    std::pair<bool, std::array<double, 9>> GetVR3PointPosition() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->GetVR3PointPosition();
+      }
+      return InputInterface::GetVR3PointPosition();
+    }
+
+    std::pair<bool, std::array<double, 12>> GetVR3PointOrientation() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->GetVR3PointOrientation();
+      }
+      return InputInterface::GetVR3PointOrientation();
+    }
+
+    std::array<double, 3> GetVR3PointCompliance() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->GetVR3PointCompliance();
+      }
+      return InputInterface::GetVR3PointCompliance();
+    }
+
+    std::pair<bool, std::array<double, 7>> GetHandPose(bool is_left) const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->GetHandPose(is_left);
+      }
+      return InputInterface::GetHandPose(is_left);
+    }
+
+    std::pair<bool, std::vector<double>> GetExternalTokenState() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION || (!is_planner_ready_ && switch_from_teleop_to_planner_)) && pose_interface_) {
+        return pose_interface_->GetExternalTokenState();
+      }
+      return InputInterface::GetExternalTokenState();
+    }
+
+    std::optional<std::chrono::steady_clock::time_point> GetLastUpdateTime() const override {
+      if ((active_mode_ == ManagedMode::STREAMED_MOTION) && pose_interface_) {
+        return pose_interface_->GetLastUpdateTime();
+      }
+      return InputInterface::GetLastUpdateTime();
+    }
+
+  private:
+    // Handle planner mode input (similar to GamepadManager::handleGamepadPlannerInput)
+    void handlePlannerInput(MotionDataReader& motion_reader,
+                           std::shared_ptr<const MotionSequence>& current_motion,
+                           int& current_frame,
+                           OperatorState& operator_state,
+                           bool& reinitialize_heading,
+                           DataBuffer<HeadingState>& heading_state_buffer,
+                           bool has_planner,
+                           PlannerState& planner_state,
+                           DataBuffer<MovementState>& movement_state_buffer,
+                           std::mutex& current_motion_mutex) {
+      
+      // Handle safety reset from interface manager (same as GamepadManager)
+      if (CheckAndClearSafetyReset()) {
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+        }
+        if (operator_state.start) {
+          if (planner_state.enabled && planner_state.initialized) {
+            // Planner is already on, keep it as is (don't touch initialized flag)
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              if (current_motion->GetEncodeMode() == 1) {
+                current_motion->SetEncodeMode(0);
+              }
+              operator_state.play = true;
+            }
+            auto current_facing = movement_state_buffer.GetDataWithTime().data->facing_direction;
+            std::cout << "[ZMQManager] Safety reset: Planner kept enabled with current state" << std::endl;
+          } else {
+            // Planner was disabled, set initial movement state
+            movement_state_buffer.SetData(MovementState(static_cast<int>(LocomotionMode::IDLE), 
+                                                        {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, -1.0f, -1.0f));
+
+            // Now enable planner
+            planner_state.enabled = true;
+            std::cout << "[ZMQManager] Planner enabled" << std::endl;
+
+            // Wait for planner to be initialized with timeout (5 seconds)
+            auto wait_start = std::chrono::steady_clock::now();
+            constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+            while (planner_state.enabled) {
+              {
+                std::lock_guard<std::mutex> lock(current_motion_mutex);
+                if (current_motion->name == "planner_motion") {
+                  break;
+                }
+              }
+              std::this_thread::sleep_for(std::chrono::milliseconds(100));
+              auto elapsed = std::chrono::steady_clock::now() - wait_start;
+              if (elapsed > PLANNER_INIT_TIMEOUT) {
+                std::cerr << "[ZMQCommandManager ERROR] Planner initialization timeout after 5 seconds" << std::endl;
+                operator_state.stop = true;
+                return;
+              }
+              std::cout << "[ZMQManager] Waiting for planner to be initialized" << std::endl;
+            }
+
+            // Check if planner is enabled and initialized
+            if (!planner_state.enabled || !planner_state.initialized) {
+              std::cerr << "[ZMQCommandManager ERROR] Planner failed to initialize. Stopping control." << std::endl;
+              operator_state.stop = true;
+              return;
+            }
+
+            is_planner_ready_ = true;
+
+            // Play motion
+            {
+              std::lock_guard<std::mutex> lock(current_motion_mutex);
+              operator_state.play = true;
+            }
+          }
+        }
+        return;
+      }
+
+      // Handle start control
+      if (start_control_ && !operator_state.start) {
+        operator_state.start = true;
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = false;
+          reinitialize_heading = true;
+        }
+
+        // Ensure planner is enabled
+        if (!planner_state.enabled) {
+          planner_state.enabled = true;
+          std::cout << "[ZMQManager] Planner enabled" << std::endl;
+        }
+        
+        // Wait for initialization
+        auto wait_start = std::chrono::steady_clock::now();
+        constexpr auto PLANNER_INIT_TIMEOUT = std::chrono::seconds(5);
+        while (planner_state.enabled) {
+          {
+            std::lock_guard<std::mutex> lock(current_motion_mutex);
+            if (current_motion->name == "planner_motion") {
+              std::cout << "[ZMQManager] motion name is planner_motion" << std::endl;
+              break;
+            }
+          }
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));
+          auto elapsed = std::chrono::steady_clock::now() - wait_start;
+          if (elapsed > PLANNER_INIT_TIMEOUT) {
+            std::cerr << "[ZMQCommandManager ERROR] Planner initialization timeout" << std::endl;
+            operator_state.stop = true;
+            return;
+          }
+          std::cout << "[ZMQManager] Waiting for planner to be initialized" << std::endl;
+        }
+        
+        // Check if planner is enabled and initialized
+        if (!planner_state.enabled || !planner_state.initialized) {
+          std::cerr << "[ZMQCommandManager ERROR] Planner failed to initialize. Stopping control." << std::endl;
+          operator_state.stop = true;
+          return;
+        }
+        
+        is_planner_ready_ = true;
+
+        {
+          std::lock_guard<std::mutex> lock(current_motion_mutex);
+          operator_state.play = true;
+        }
+      }
+
+      // Apply planner commands if planner is ready
+      if (planner_state.enabled && planner_state.initialized) {
+        std::lock_guard<std::mutex> lock(planner_mutex_);
+        
+        // Check for planner timeout (1 second)
+        constexpr auto PLANNER_TIMEOUT = std::chrono::milliseconds(1000);
+        auto time_since_last_planner = std::chrono::steady_clock::now() - latest_planner_message_.timestamp;
+        
+        if (latest_planner_message_.valid) {
+          // Valid planner message within timeout - use it
+          // Update upper body control state based on this message
+          has_upper_body_control_ = latest_planner_message_.upper_body_position.has_value();
+
+          // Update hand joints control state based on this message
+          has_hand_joints_ = latest_planner_message_.left_hand_joints.has_value() || 
+                             latest_planner_message_.right_hand_joints.has_value();
+
+          MovementState mode_state(
+            latest_planner_message_.mode,
+            latest_planner_message_.movement,
+            latest_planner_message_.facing,
+            latest_planner_message_.speed,
+            latest_planner_message_.height
+          );
+
+          if (is_squat_motion_mode(static_cast<LocomotionMode>(mode_state.locomotion_mode))) {
+            if (mode_state.height < 0.2) mode_state.height = 0.2;
+          }
+          if (is_static_motion_mode(static_cast<LocomotionMode>(mode_state.locomotion_mode))) {
+            mode_state.movement_speed = -1.0f;
+          }
+
+          // normalize facing direction and movement direction
+          mode_state.facing_direction = normalize_vector_d(mode_state.facing_direction);
+          mode_state.movement_direction = normalize_vector_d(mode_state.movement_direction);
+
+          movement_state_buffer.SetData(mode_state);
+          
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ZMQManager] Planner command: mode=" << latest_planner_message_.mode 
+                      << ", speed=" << latest_planner_message_.speed << std::endl;
+          }
+
+          // Clear planner buffer to avoid using stale data
+          latest_planner_message_.valid = false;
+
+        } else if (!latest_planner_message_.valid && time_since_last_planner >= PLANNER_TIMEOUT) {
+          // Planner timeout - reset to IDLE and clear buffer
+          has_upper_body_control_ = false;
+
+          has_hand_joints_ = false;
+
+          auto current_facing = movement_state_buffer.GetDataWithTime().data->facing_direction;
+          MovementState idle_state(
+            static_cast<int>(LocomotionMode::IDLE),
+            {0.0f, 0.0f, 0.0f},
+            current_facing,
+            -1.0f,
+            -1.0f
+          );
+          movement_state_buffer.SetData(idle_state);
+          
+          if (latest_planner_message_.timestamp != std::chrono::steady_clock::time_point{}) {
+            std::cout << "[ZMQManager] Planner timeout (" 
+                      << std::chrono::duration_cast<std::chrono::milliseconds>(time_since_last_planner).count()
+                      << "ms) - reset to IDLE and cleared buffer" << std::endl;
+
+            // Clear planner buffer to avoid using stale data
+            latest_planner_message_.valid = false;
+            latest_planner_message_.timestamp = {};
+          }
+          
+        }
+      }
+
+      if (has_vr_3point_control_ && !last_has_vr_3point_control_) {
+        std::cout << "[ZMQManager] VR 3-point control enabled" << std::endl;
+        std::lock_guard<std::mutex> lock(current_motion_mutex);
+        if (current_motion->GetEncodeMode() >= 0) {
+              current_motion->SetEncodeMode(1);
+        }
+      }
+      else if (!has_vr_3point_control_ && last_has_vr_3point_control_) {
+        std::cout << "[ZMQManager] VR 3-point control disabled" << std::endl;
+        std::lock_guard<std::mutex> lock(current_motion_mutex);
+        if (current_motion->GetEncodeMode() >= 0) {
+              current_motion->SetEncodeMode(0);
+        }
+      }
+      last_has_vr_3point_control_ = has_vr_3point_control_;
+    }
+
+    // Callback handlers - just update buffer, no queue
+    void OnCommandReceived(
+        const std::string& topic,
+        const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+        const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+      
+      if (hdr.fields.empty() || bufs.empty()) return;
+      
+      int start_idx = -1, stop_idx = -1, planner_idx = -1;
+      for (size_t i = 0; i < hdr.fields.size(); ++i) {
+        if (hdr.fields[i].name == "start") start_idx = static_cast<int>(i);
+        else if (hdr.fields[i].name == "stop") stop_idx = static_cast<int>(i);
+        else if (hdr.fields[i].name == "planner") planner_idx = static_cast<int>(i);
+      }
+      
+      if (start_idx < 0 || stop_idx < 0 || planner_idx < 0) {
+        std::cerr << "[ZMQManager] Command missing fields (need: start, stop, planner)" << std::endl;
+        return;
+      }
+      
+      CommandMessage cmd;
+      cmd.valid = true;
+      
+      bool needs_swap = hdr.NeedsByteSwap();
+      
+      // Decode start
+      const auto& start_buf = bufs[start_idx];
+      const auto& start_field = hdr.fields[start_idx];
+      if (start_field.dtype == "bool" || start_field.dtype == "u8") {
+        uint8_t val = 0;
+        if (start_buf.size >= sizeof(uint8_t)) {
+          std::memcpy(&val, start_buf.data, sizeof(uint8_t));
+          cmd.start = (val != 0);
+        }
+      } else if (start_field.dtype == "i32") {
+        int32_t val = 0;
+        if (start_buf.size >= sizeof(int32_t)) {
+          std::memcpy(&val, start_buf.data, sizeof(int32_t));
+          if (needs_swap) val = byte_swap(val);
+          cmd.start = (val != 0);
+        }
+      }
+      
+      // Decode stop
+      const auto& stop_buf = bufs[stop_idx];
+      const auto& stop_field = hdr.fields[stop_idx];
+      if (stop_field.dtype == "bool" || stop_field.dtype == "u8") {
+        uint8_t val = 0;
+        if (stop_buf.size >= sizeof(uint8_t)) {
+          std::memcpy(&val, stop_buf.data, sizeof(uint8_t));
+          cmd.stop = (val != 0);
+        }
+      } else if (stop_field.dtype == "i32") {
+        int32_t val = 0;
+        if (stop_buf.size >= sizeof(int32_t)) {
+          std::memcpy(&val, stop_buf.data, sizeof(int32_t));
+          if (needs_swap) val = byte_swap(val);
+          cmd.stop = (val != 0);
+        }
+      }
+      
+      // Decode planner
+      const auto& planner_buf = bufs[planner_idx];
+      const auto& planner_field = hdr.fields[planner_idx];
+      if (planner_field.dtype == "bool" || planner_field.dtype == "u8") {
+        uint8_t val = 0;
+        if (planner_buf.size >= sizeof(uint8_t)) {
+          std::memcpy(&val, planner_buf.data, sizeof(uint8_t));
+          cmd.planner = (val != 0);
+        }
+      } else if (planner_field.dtype == "i32") {
+        int32_t val = 0;
+        if (planner_buf.size >= sizeof(int32_t)) {
+          std::memcpy(&val, planner_buf.data, sizeof(int32_t));
+          if (needs_swap) val = byte_swap(val);
+          cmd.planner = (val != 0);
+        }
+      }
+      
+      // Update buffer with OR logic to accumulate start/stop signals
+      std::lock_guard<std::mutex> lock(command_mutex_);
+      
+      // If starting new accumulation cycle, reset start/stop
+      if (!latest_command_.valid) {
+        latest_command_.start = false;
+        latest_command_.stop = false;
+      }
+      
+      // Accumulate start/stop with OR logic
+      latest_command_.start = latest_command_.start || cmd.start;
+      latest_command_.stop = latest_command_.stop || cmd.stop;
+      latest_command_.planner = cmd.planner;  // Overwrite (mode should be latest)
+      latest_command_.valid = true;
+      
+      if constexpr (DEBUG_LOGGING) {
+        std::cout << "[ZMQManager] Command received: start=" << cmd.start 
+                  << ", stop=" << cmd.stop << ", planner=" << cmd.planner << std::endl;
+      }
+    }
+    
+    void OnPlannerReceived(
+        const std::string& topic,
+        const ZMQPackedMessageSubscriber::DecodedHeader& hdr,
+        const std::vector<ZMQPackedMessageSubscriber::BufferView>& bufs) {
+      
+      int mode_idx = -1, movement_idx = -1, facing_idx = -1;
+      int speed_idx = -1, height_idx = -1;
+      int upper_body_position_idx = -1, upper_body_velocity_idx = -1;
+      int left_hand_joints_idx = -1, right_hand_joints_idx = -1;
+      int vr_position_idx = -1, vr_orientation_idx = -1, vr_compliance_idx = -1;
+
+      for (size_t i = 0; i < hdr.fields.size(); ++i) {
+        const auto& f = hdr.fields[i];
+        if (f.name == "mode") mode_idx = static_cast<int>(i);
+        else if (f.name == "movement") movement_idx = static_cast<int>(i);
+        else if (f.name == "facing") facing_idx = static_cast<int>(i);
+        else if (f.name == "speed") speed_idx = static_cast<int>(i);
+        else if (f.name == "height") height_idx = static_cast<int>(i);
+        else if (f.name == "upper_body_position") upper_body_position_idx = static_cast<int>(i);
+        else if (f.name == "upper_body_velocity") upper_body_velocity_idx = static_cast<int>(i);
+        else if (f.name == "left_hand_joints") left_hand_joints_idx = static_cast<int>(i);
+        else if (f.name == "right_hand_joints") right_hand_joints_idx = static_cast<int>(i);
+        else if (f.name == "vr_position") vr_position_idx = static_cast<int>(i);
+        else if (f.name == "vr_orientation") vr_orientation_idx = static_cast<int>(i);
+        else if (f.name == "vr_compliance") vr_compliance_idx = static_cast<int>(i);
+      }
+      
+      if (mode_idx < 0 || movement_idx < 0 || facing_idx < 0) {
+        std::cerr << "[ZMQManager] Planner missing required fields" << std::endl;
+        return;
+      }
+      
+      PlannerMessage msg;
+      msg.valid = true;
+      
+      bool needs_swap = hdr.NeedsByteSwap();
+      
+      // Decode mode
+      const auto& mode_buf = bufs[mode_idx];
+      int32_t mode_val;
+      std::memcpy(&mode_val, mode_buf.data, sizeof(int32_t));
+      if (needs_swap) mode_val = byte_swap(mode_val);
+      msg.mode = static_cast<int>(mode_val);
+      
+      // Decode movement based on dtype
+      const auto& movement_buf = bufs[movement_idx];
+      const auto& movement_field = hdr.fields[movement_idx];
+      if (movement_field.dtype == "f32") {
+        for (int i = 0; i < 3; ++i) {
+          float val;
+          std::memcpy(&val, static_cast<const uint8_t*>(movement_buf.data) + i * sizeof(float), sizeof(float));
+          if (needs_swap) val = byte_swap(val);
+          msg.movement[i] = static_cast<double>(val);
+        }
+      } else { // f64 or default
+        for (int i = 0; i < 3; ++i) {
+          double val;
+          std::memcpy(&val, static_cast<const uint8_t*>(movement_buf.data) + i * sizeof(double), sizeof(double));
+          if (needs_swap) val = byte_swap(val);
+          msg.movement[i] = val;
+        }
+      }
+      
+      // Decode facing based on dtype
+      const auto& facing_buf = bufs[facing_idx];
+      const auto& facing_field = hdr.fields[facing_idx];
+      if (facing_field.dtype == "f32") {
+        for (int i = 0; i < 3; ++i) {
+          float val;
+          std::memcpy(&val, static_cast<const uint8_t*>(facing_buf.data) + i * sizeof(float), sizeof(float));
+          if (needs_swap) val = byte_swap(val);
+          msg.facing[i] = static_cast<double>(val);
+        }
+      } else { // f64 or default
+        for (int i = 0; i < 3; ++i) {
+          double val;
+          std::memcpy(&val, static_cast<const uint8_t*>(facing_buf.data) + i * sizeof(double), sizeof(double));
+          if (needs_swap) val = byte_swap(val);
+          msg.facing[i] = val;
+        }
+      }
+      
+      // Optional: speed (decode based on dtype)
+      if (speed_idx >= 0) {
+        const auto& speed_buf = bufs[speed_idx];
+        const auto& speed_field = hdr.fields[speed_idx];
+        if (speed_field.dtype == "f32") {
+          float val;
+          std::memcpy(&val, speed_buf.data, sizeof(float));
+          if (needs_swap) val = byte_swap(val);
+          msg.speed = static_cast<double>(val);
+        } else { // f64 or default
+          double val;
+          std::memcpy(&val, speed_buf.data, sizeof(double));
+          if (needs_swap) val = byte_swap(val);
+          msg.speed = val;
+        }
+      }
+      
+      // Optional: height (decode based on dtype)
+      if (height_idx >= 0) {
+        const auto& height_buf = bufs[height_idx];
+        const auto& height_field = hdr.fields[height_idx];
+        if (height_field.dtype == "f32") {
+          float val;
+          std::memcpy(&val, height_buf.data, sizeof(float));
+          if (needs_swap) val = byte_swap(val);
+          msg.height = static_cast<double>(val);
+        } else { // f64 or default
+          double val;
+          std::memcpy(&val, height_buf.data, sizeof(double));
+          if (needs_swap) val = byte_swap(val);
+          msg.height = val;
+        }
+      }
+
+      // Optional: upper_body_position (17 DOF, decode based on dtype)
+      if (upper_body_position_idx >= 0) {
+        const auto& ub_pos_buf = bufs[upper_body_position_idx];
+        const auto& ub_pos_field = hdr.fields[upper_body_position_idx];
+
+        std::array<double, 17> upper_body_position_data{};
+        if (ub_pos_field.dtype == "f32") {
+          for (int i = 0; i < 17; ++i) {
+            float val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(ub_pos_buf.data) + i * sizeof(float),
+                        sizeof(float));
+            if (needs_swap) val = byte_swap(val);
+            upper_body_position_data[i] = static_cast<double>(val);
+          }
+        } else { // f64 or default
+          for (int i = 0; i < 17; ++i) {
+            double val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(ub_pos_buf.data) + i * sizeof(double),
+                        sizeof(double));
+            if (needs_swap) val = byte_swap(val);
+            upper_body_position_data[i] = val;
+          }
+        }
+        msg.upper_body_position = upper_body_position_data;
+
+        // Push into upper-body position buffer
+        upper_body_joint_positions_.SetData(upper_body_position_data);
+      }
+
+      // Optional: upper_body_velocity (17 DOF, decode based on dtype)
+      if (upper_body_velocity_idx >= 0) {
+        const auto& ub_vel_buf = bufs[upper_body_velocity_idx];
+        const auto& ub_vel_field = hdr.fields[upper_body_velocity_idx];
+
+        std::array<double, 17> upper_body_velocity_data{};
+        if (ub_vel_field.dtype == "f32") {
+          for (int i = 0; i < 17; ++i) {
+            float val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(ub_vel_buf.data) + i * sizeof(float),
+                        sizeof(float));
+            if (needs_swap) val = byte_swap(val);
+            upper_body_velocity_data[i] = static_cast<double>(val);
+          }
+        } else { // f64 or default
+          for (int i = 0; i < 17; ++i) {
+            double val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(ub_vel_buf.data) + i * sizeof(double),
+                        sizeof(double));
+            if (needs_swap) val = byte_swap(val);
+            upper_body_velocity_data[i] = val;
+          }
+        }
+        msg.upper_body_velocity = upper_body_velocity_data;
+
+        // Push into upper-body velocity buffer
+        upper_body_joint_velocities_.SetData(upper_body_velocity_data);
+      }
+      
+      // Optional: left_hand_joints (7 DOF, decode based on dtype)
+      if (left_hand_joints_idx >= 0) {
+        const auto& lh_buf = bufs[left_hand_joints_idx];
+        const auto& lh_field = hdr.fields[left_hand_joints_idx];
+
+        std::array<double, 7> left_hand_joints_data{};
+        if (lh_field.dtype == "f32") {
+          for (int i = 0; i < 7; ++i) {
+            float val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(lh_buf.data) + i * sizeof(float),
+                        sizeof(float));
+            if (needs_swap) val = byte_swap(val);
+            left_hand_joints_data[i] = static_cast<double>(val);
+          }
+        } else { // f64 or default
+          for (int i = 0; i < 7; ++i) {
+            double val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(lh_buf.data) + i * sizeof(double),
+                        sizeof(double));
+            if (needs_swap) val = byte_swap(val);
+            left_hand_joints_data[i] = val;
+          }
+        }
+        msg.left_hand_joints = left_hand_joints_data;
+
+        // Push into left hand joint buffer
+        left_hand_joint_.SetData(left_hand_joints_data);
+      }
+
+      // Optional: right_hand_joints (7 DOF, decode based on dtype)
+      if (right_hand_joints_idx >= 0) {
+        const auto& rh_buf = bufs[right_hand_joints_idx];
+        const auto& rh_field = hdr.fields[right_hand_joints_idx];
+
+        std::array<double, 7> right_hand_joints_data{};
+        if (rh_field.dtype == "f32") {
+          for (int i = 0; i < 7; ++i) {
+            float val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(rh_buf.data) + i * sizeof(float),
+                        sizeof(float));
+            if (needs_swap) val = byte_swap(val);
+            right_hand_joints_data[i] = static_cast<double>(val);
+          }
+        } else { // f64 or default
+          for (int i = 0; i < 7; ++i) {
+            double val;
+            std::memcpy(&val,
+                        static_cast<const uint8_t*>(rh_buf.data) + i * sizeof(double),
+                        sizeof(double));
+            if (needs_swap) val = byte_swap(val);
+            right_hand_joints_data[i] = val;
+          }
+        }
+        msg.right_hand_joints = right_hand_joints_data;
+
+        // Push into right hand joint buffer
+        right_hand_joint_.SetData(right_hand_joints_data);
+      }
+
+      // Decode VR 3-point tracking data if present (9 doubles for position, 12 doubles for orientation, 3 doubles for compliance)
+      // Use default values from InputInterface as fallback
+      bool has_vr_position = (vr_position_idx >= 0);
+      bool has_vr_orientation = (vr_orientation_idx >= 0);
+      bool has_vr_compliance = (vr_compliance_idx >= 0);
+      // Default values from input_interface.hpp
+      std::array<double, 9> vr_position_values = GetVR3PointPosition().second;
+      std::array<double, 12> vr_orientation_values = GetVR3PointOrientation().second;
+      std::array<double, 3> vr_compliance_values = GetVR3PointCompliance();
+      
+      if (has_vr_position) {
+          const auto& vr_pos_field = hdr.fields[vr_position_idx];
+          const auto& vr_pos_buf = bufs[vr_position_idx];
+          
+          // Validate shape: expect [9] or [1, 9]
+          int num_vr_pos_values = 0;
+          if (vr_pos_field.shape.size() == 1 && vr_pos_field.shape[0] == 9) {
+              num_vr_pos_values = 9;
+          } else if (vr_pos_field.shape.size() == 2 && vr_pos_field.shape[1] == 9) {
+              num_vr_pos_values = 9;
+          }
+          
+          if (num_vr_pos_values == 9) {
+              // Decode 9 position values
+              if (vr_pos_field.dtype == "f32") {
+                  for (int j = 0; j < 9; ++j) {
+                      float val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_pos_buf.data) + j * sizeof(float), sizeof(float));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_position_values[j] = static_cast<double>(val);
+                  }
+              } else { // f64 or default
+                  for (int j = 0; j < 9; ++j) {
+                      double val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_pos_buf.data) + j * sizeof(double), sizeof(double));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_position_values[j] = val;
+                  }
+              }
+
+              if constexpr (DEBUG_LOGGING) {
+                  std::cout << "[ZMQManager] Decoded vr_position: [";
+                  for (int j = 0; j < 9; ++j) {
+                      if (j > 0) std::cout << ", ";
+                      std::cout << std::fixed << std::setprecision(4) << vr_position_values[j];
+                  }
+                  std::cout << "]" << std::endl;
+              }
+          } else {
+              std::cerr << "[ZMQManager] Invalid vr_position shape" << std::endl;
+              has_vr_position = false;
+          }
+      }
+      
+      if (has_vr_orientation) {
+          const auto& vr_orient_field = hdr.fields[vr_orientation_idx];
+          const auto& vr_orient_buf = bufs[vr_orientation_idx];
+          
+          // Validate shape: expect [12] or [1, 12]
+          int num_vr_orient_values = 0;
+          if (vr_orient_field.shape.size() == 1 && vr_orient_field.shape[0] == 12) {
+              num_vr_orient_values = 12;
+          } else if (vr_orient_field.shape.size() == 2 && vr_orient_field.shape[1] == 12) {
+              num_vr_orient_values = 12;
+          }
+          
+          if (num_vr_orient_values == 12) {
+              // Decode 12 orientation values (quaternions)
+              if (vr_orient_field.dtype == "f32") {
+                  for (int j = 0; j < 12; ++j) {
+                      float val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_orient_buf.data) + j * sizeof(float), sizeof(float));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_orientation_values[j] = static_cast<double>(val);
+                  }
+              } else { // f64 or default
+                  for (int j = 0; j < 12; ++j) {
+                      double val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_orient_buf.data) + j * sizeof(double), sizeof(double));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_orientation_values[j] = val;
+                  }
+              }
+              
+              if constexpr (DEBUG_LOGGING) {
+                  std::cout << "[ZMQManager] Decoded vr_orientation: [";
+                  for (int j = 0; j < 12; ++j) {
+                      if (j > 0) std::cout << ", ";
+                      std::cout << std::fixed << std::setprecision(4) << vr_orientation_values[j];
+                  }
+                  std::cout << "]" << std::endl;
+              }
+          } else {
+              std::cerr << "[ZMQManager] Invalid vr_orientation shape" << std::endl;
+              has_vr_orientation = false;
+          }
+      }
+      
+      if (has_vr_compliance) {
+          const auto& vr_compl_field = hdr.fields[vr_compliance_idx];
+          const auto& vr_compl_buf = bufs[vr_compliance_idx];
+          
+          // Validate shape: expect [3] or [1, 3]
+          int num_vr_compl_values = 0;
+          if (vr_compl_field.shape.size() == 1 && vr_compl_field.shape[0] == 3) {
+              num_vr_compl_values = 3;
+          } else if (vr_compl_field.shape.size() == 2 && vr_compl_field.shape[1] == 3) {
+              num_vr_compl_values = 3;
+          }
+          
+          if (num_vr_compl_values == 3) {
+              // Decode 3 compliance values
+              if (vr_compl_field.dtype == "f32") {
+                  for (int j = 0; j < 3; ++j) {
+                      float val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_compl_buf.data) + j * sizeof(float), sizeof(float));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_compliance_values[j] = static_cast<double>(val);
+                  }
+              } else { // f64 or default
+                  for (int j = 0; j < 3; ++j) {
+                      double val;
+                      std::memcpy(&val, static_cast<const uint8_t*>(vr_compl_buf.data) + j * sizeof(double), sizeof(double));
+                      if (needs_swap) val = byte_swap(val);
+                      vr_compliance_values[j] = val;
+                  }
+              }
+              
+              if constexpr (DEBUG_LOGGING) {
+                  std::cout << "[ZMQManager] Decoded vr_compliance: [";
+                  for (int j = 0; j < 3; ++j) {
+                      if (j > 0) std::cout << ", ";
+                      std::cout << std::fixed << std::setprecision(4) << vr_compliance_values[j];
+                  }
+                  std::cout << "]" << std::endl;
+              }
+          } else {
+              std::cerr << "[ZMQManager] Invalid vr_compliance shape" << std::endl;
+              has_vr_compliance = false;
+          }
+      }
+
+      // Handle VR 3-point tracking: vr_position is required, orientation and compliance are optional
+      // If vr_position is present, set has_vr_3point_control_ = true and update all buffers
+      if (has_vr_position) {
+        // Always update all three buffers when VR position is present
+        // (orientation and compliance will use defaults if not provided)
+        vr_3point_position_.SetData(vr_position_values);
+        vr_3point_orientation_.SetData(vr_orientation_values);
+        if (has_vr_compliance) {
+          SetVR3PointCompliance(vr_compliance_values);
+        }
+        has_vr_3point_control_ = true;
+
+        pose_interface_->SetVR3PointPosition(vr_position_values);
+        pose_interface_->SetVR3PointOrientation(vr_orientation_values);
+        pose_interface_->SetVR3PointCompliance(vr_compliance_values);
+        
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ZMQManager] VR 3-point data updated:" << std::endl;
+            std::cout << "  Position (left, right, head): [";
+            for (int j = 0; j < 9; ++j) {
+                if (j > 0) std::cout << ", ";
+                std::cout << std::fixed << std::setprecision(4) << vr_position_values[j];
+            }
+            std::cout << "]" << (has_vr_position ? " (from message)" : " (default)") << std::endl;
+            
+            std::cout << "  Orientation (left quat, right quat, head quat): [";
+            for (int j = 0; j < 12; ++j) {
+                if (j > 0) std::cout << ", ";
+                std::cout << std::fixed << std::setprecision(4) << vr_orientation_values[j];
+            }
+            std::cout << "]" << (has_vr_orientation ? " (from message)" : " (default)") << std::endl;
+            if (has_vr_compliance) {
+              std::cout << "  Compliance (left, right, head): [";
+              for (int j = 0; j < 3; ++j) {
+                  if (j > 0) std::cout << ", ";
+                  std::cout << std::fixed << std::setprecision(4) << vr_compliance_values[j];
+              }
+              std::cout << "]" << (has_vr_compliance ? " (from message)" : " (default)") << std::endl;
+            }
+        }
+      }
+      else {
+        // No VR position provided - disable VR 3-point control
+        has_vr_3point_control_ = false;
+      }
+
+      // Update buffer directly (no queue) and set timestamp
+      std::lock_guard<std::mutex> lock(planner_mutex_);
+      latest_planner_message_ = msg;
+      latest_planner_message_.timestamp = std::chrono::steady_clock::now();
+    }
+    
+
+  private:
+    // ------------------------------------------------------------------
+    // Configuration (set once in constructor)
+    // ------------------------------------------------------------------
+    std::string zmq_host_;           ///< ZMQ server hostname.
+    int zmq_port_;                   ///< ZMQ server port.
+    std::string pose_topic_;         ///< Topic for streamed motion data.
+    std::string command_topic_;      ///< Topic for start / stop / mode commands.
+    std::string planner_topic_;      ///< Topic for planner movement commands.
+    bool zmq_conflate_;              ///< ZMQ conflate option for pose topic.
+    bool zmq_verbose_;               ///< Verbose logging flag.
+    
+    // ------------------------------------------------------------------
+    // Owned sub-components
+    // ------------------------------------------------------------------
+    /// Pose-streaming interface (handles STREAMED_MOTION mode internally).
+    std::unique_ptr<ZMQEndpointInterface> pose_interface_;
+    
+    /// Background subscriber for the command topic.
+    std::unique_ptr<ZMQPackedMessageSubscriber> command_subscriber_;
+    /// Background subscriber for the planner topic.
+    std::unique_ptr<ZMQPackedMessageSubscriber> planner_subscriber_;
+    
+    // ------------------------------------------------------------------
+    // Mode / message state
+    // ------------------------------------------------------------------
+    ManagedMode active_mode_;           ///< Current operational mode (PLANNER or STREAMED_MOTION).
+    
+    std::mutex command_mutex_;          ///< Guards access to latest_command_.
+    CommandMessage latest_command_;     ///< Most recent (or accumulated) command message.
+    
+    std::mutex planner_mutex_;          ///< Guards access to latest_planner_message_.
+    PlannerMessage latest_planner_message_;  ///< Most recent planner movement message.
+    
+    // ------------------------------------------------------------------
+    // Per-frame control flags (reset at start of update())
+    // ------------------------------------------------------------------
+    bool emergency_stop_ = false;  ///< Set by 'O'/'o' keyboard shortcut.
+    bool report_temperature_flag_ = false;  ///< Set by 'F'/'f' keyboard shortcut.
+    bool start_control_ = false;   ///< Start request from command message.
+    bool stop_control_ = false;    ///< Stop request from command message.
+
+    /// True once the planner has been initialised and is generating motions.
+    bool is_planner_ready_ = false;
+    /// True when transitioning from streamed-motion (teleop) back to planner mode;
+    /// used to keep forwarding VR/hand data from the pose interface until planner is ready.
+    bool switch_from_teleop_to_planner_ = false;
+
+    /// Tracks the previous frame's VR-3-point state to detect enable/disable transitions
+    /// and automatically toggle encoder mode accordingly.
+    bool last_has_vr_3point_control_ = false;
+};
+
+#endif // ZMQ_MANAGER_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/zmq_packed_message_subscriber.hpp

@@ -0,0 +1,439 @@
+/**
+ * @file zmq_packed_message_subscriber.hpp
+ * @brief ZeroMQ SUB client for receiving "packed" binary messages.
+ *
+ * ## Wire Format
+ *
+ * Each ZMQ message is a **single-part message** with the following layout:
+ *
+ *   [topic_prefix (optional)] [1280-byte JSON header] [concatenated binary fields]
+ *
+ * - **Topic prefix**: If a non-empty topic is configured, the subscriber
+ *   filters on this prefix and strips it before processing.
+ * - **JSON header** (exactly `HEADER_SIZE` = 1280 bytes, null-padded):
+ *   Describes the binary payload – version, endianness, field names, dtypes,
+ *   and shapes.  Example:
+ *   ```json
+ *   { "v": 1, "endian": "le", "count": 100,
+ *     "fields": [
+ *       { "name": "joint_pos", "dtype": "f32", "shape": [100, 29] },
+ *       { "name": "body_quat", "dtype": "f64", "shape": [100, 1, 4] }
+ *     ]
+ *   }
+ *   ```
+ * - **Binary payload**: Fields concatenated in the order listed in the header.
+ *
+ * Because each message is a single ZMQ part, ZMQ's `conflate` option can safely
+ * be used to drop older messages and always process the latest one.
+ *
+ * ## Threading Model
+ *
+ * Start() spawns a background thread that calls PollOnce() in a tight loop.
+ * Decoded messages are dispatched to the user-supplied callback, which runs
+ * **on the background thread** – the callback must therefore be thread-safe
+ * or buffer data for later processing on the main thread.
+ *
+ * ## Endianness
+ *
+ * The header's `endian` field ("le" or "be") is compared against the native
+ * endianness at runtime.  Helper functions `is_little_endian()` and
+ * `byte_swap()` are provided for callers that need to swap multi-byte values.
+ */
+
+#ifndef ZMQ_PACKED_MESSAGE_SUBSCRIBER_HPP
+#define ZMQ_PACKED_MESSAGE_SUBSCRIBER_HPP
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <thread>
+#include <functional>
+#include <vector>
+#include <algorithm>
+
+#include <zmq.hpp>
+#include <nlohmann/json.hpp>
+
+/// @brief Detect whether the host CPU is little-endian.
+inline bool is_little_endian() {
+  uint32_t test = 1;
+  return *reinterpret_cast<uint8_t*>(&test) == 1;
+}
+
+/**
+ * @brief Reverse the byte order of a value (up to 64 bits).
+ * @tparam T  An arithmetic type of at most 8 bytes.
+ */
+template<typename T>
+inline T byte_swap(T value) {
+  static_assert(sizeof(T) <= 8, "byte_swap only supports up to 64-bit types");
+  union {
+    T val;
+    uint8_t bytes[sizeof(T)];
+  } src, dst;
+  src.val = value;
+  for (size_t i = 0; i < sizeof(T); ++i) {
+    dst.bytes[i] = src.bytes[sizeof(T) - 1 - i];
+  }
+  return dst.val;
+}
+
+/**
+ * @class ZMQPackedMessageSubscriber
+ * @brief ZeroMQ SUB client that receives, parses, and dispatches packed
+ *        binary messages described by a JSON header.
+ *
+ * Usage:
+ *  1. Construct with host / port / topic.
+ *  2. Call SetOnDecodedMessage() to register a callback.
+ *  3. Call Start() to begin background receiving.
+ *  4. Call Stop() (or let the destructor do it) to shut down.
+ */
+class ZMQPackedMessageSubscriber {
+  public:
+    /// Fixed size (in bytes) of the JSON header block at the start of each packed message.
+    static constexpr size_t HEADER_SIZE = 1280;
+
+    /**
+     * @brief Construct a subscriber (does NOT connect or start yet).
+     * @param host        ZMQ server hostname or IP.
+     * @param port        ZMQ server port.
+     * @param topic       Subscription topic prefix (empty = receive all).
+     * @param timeout_ms  Receive timeout in milliseconds.
+     * @param verbose     Enable verbose logging.
+     * @param conflate    Enable ZMQ conflate (keep only latest message).
+     * @param rcv_hwm     Receive high-water mark (−1 = ZMQ default).
+     */
+    ZMQPackedMessageSubscriber(
+      const std::string &host = "localhost",
+      int port = 5556,
+      const std::string &topic = "pose",
+      int timeout_ms = 1000,
+      bool verbose = true,
+      bool conflate = false,
+      int rcv_hwm = -1
+    )
+      : host_(host), port_(port), topic_(topic), timeout_ms_(timeout_ms), verbose_(verbose),
+        conflate_(conflate), rcv_hwm_(rcv_hwm),
+        context_(1), running_(false) {}
+
+    // Non-copyable, non-movable
+    ZMQPackedMessageSubscriber(const ZMQPackedMessageSubscriber&) = delete;
+    ZMQPackedMessageSubscriber& operator=(const ZMQPackedMessageSubscriber&) = delete;
+
+    ~ZMQPackedMessageSubscriber() { Stop(); }
+
+    /// Zero-copy view into the received message buffer.
+    /// **Only valid during the callback invocation** – do not store.
+    struct BufferView {
+      const void* data;    ///< Pointer to the raw field bytes inside the ZMQ message.
+      std::size_t size;    ///< Number of bytes.
+    };
+
+    /// Describes one binary field as declared in the JSON header.
+    struct FieldInfo {
+      std::string name;              ///< Human-readable field name (e.g. "joint_pos").
+      std::string dtype;             ///< Data-type string: "f32", "f64", "i32", "i64", "bool", etc.
+      std::vector<size_t> shape;     ///< N-D shape (e.g. [100, 29] for 100 frames × 29 joints).
+      bool optional = false;         ///< Whether the field may be absent from the payload.
+      
+      /// @brief Return the byte size of a single element for this dtype.
+      size_t GetElementSize() const {
+        if (dtype == "f64" || dtype == "i64") return 8;
+        if (dtype == "f32" || dtype == "i32") return 4;
+        if (dtype == "i16" || dtype == "f16") return 2;
+        if (dtype == "i8" || dtype == "u8" || dtype == "bool") return 1;
+        return 4; // default
+      }
+      
+      // Compute total byte size for this field
+      size_t ComputeByteSize() const {
+        if (shape.empty()) return 0;
+        size_t total_elements = 1;
+        for (auto dim : shape) total_elements *= dim;
+        return total_elements * GetElementSize();
+      }
+    };
+
+    /// Parsed representation of the 1280-byte JSON header.
+    struct DecodedHeader {
+      int version = 0;               ///< Protocol version (e.g. 1, 2, 3).
+      std::string endian;            ///< "le" or "be" (empty defaults to "le").
+      int count = -1;                ///< Optional frame/element count hint.
+      std::vector<FieldInfo> fields; ///< Ordered list of binary field descriptors.
+      
+      /// @return True if the payload byte order differs from the native CPU order.
+      bool NeedsByteSwap() const {
+        bool native_le = is_little_endian();
+        bool data_le = (endian == "le" || endian.empty()); // default to le
+        return native_le != data_le;
+      }
+    };
+
+    /// Register a callback to be invoked (on the background thread) for each
+    /// successfully decoded packed message.  Must be called before Start().
+    void SetOnDecodedMessage(
+      std::function<void(const std::string&, const DecodedHeader&, const std::vector<BufferView>&)> cb) {
+      on_decoded_ = std::move(cb);
+    }
+
+    /// Create the ZMQ SUB socket, set options, and connect to the endpoint.
+    /// @return True on success, false on error (logged to stderr).
+    bool Connect() {
+      if (socket_) return true;
+
+      try {
+        socket_ = std::make_unique<zmq::socket_t>(context_, zmq::socket_type::sub);
+
+        socket_->set(zmq::sockopt::rcvtimeo, timeout_ms_);
+        socket_->set(zmq::sockopt::linger, 0);
+        if (rcv_hwm_ > 0) {
+          socket_->set(zmq::sockopt::rcvhwm, rcv_hwm_);
+        }
+        if (conflate_) {
+          socket_->set(zmq::sockopt::conflate, 1);
+        }
+
+        const std::string endpoint = "tcp://" + host_ + ":" + std::to_string(port_);
+        socket_->connect(endpoint);
+        
+        // Subscribe: if topic is empty, receive all; otherwise filter by topic prefix
+        socket_->set(zmq::sockopt::subscribe, topic_);
+
+        if (verbose_) {
+          std::cout << "[ZMQPackedMessageSubscriber] Subscribed to '" << topic_ << "' at "
+                    << endpoint << std::endl;
+        }
+        return true;
+      } catch (const zmq::error_t &e) {
+        std::cerr << "[ZMQPackedMessageSubscriber] Connect error: " << e.what() << std::endl;
+        socket_.reset();
+        return false;
+      }
+    }
+
+    /// Spawn the background receive thread.  Requires a callback and a connection.
+    void Start() {
+      if (running_) {
+        std::cerr << "[ZMQPackedMessageSubscriber] Already running" << std::endl;
+        return;
+      }
+      if (!on_decoded_) {
+        std::cerr << "[ZMQPackedMessageSubscriber] Error: callback not set" << std::endl;
+        return;
+      }
+      if (!Connect()) {
+        std::cerr << "[ZMQPackedMessageSubscriber] Error: Connect() failed" << std::endl;
+        return;
+      }
+      running_ = true;
+      recv_thread_ = std::thread([this]() { this->RunLoop(); });
+      if (verbose_) {
+        std::cout << "[ZMQPackedMessageSubscriber] Background thread started" << std::endl;
+      }
+    }
+
+    /// Stop the background thread and close the socket.
+    void Stop() {
+      if (!running_) return;
+      running_ = false;
+      if (recv_thread_.joinable()) {
+        recv_thread_.join();
+      }
+      if (socket_) {
+        try { socket_->close(); } catch (...) {}
+        socket_.reset();
+      }
+    }
+
+    /**
+     * @brief Receive and decode one packed message (blocking up to timeout_ms).
+     * @return True if a message was successfully received and decoded.
+     *
+     * Can be called manually for single-threaded use, or is called in a loop
+     * by the background thread spawned by Start().
+     */
+    bool PollOnce() {
+      if (!socket_ && !Connect()) return false;
+
+      try {
+        // Receive single packed message
+        zmq::message_t packed_frame;
+        zmq::recv_result_t r1 = socket_->recv(packed_frame, zmq::recv_flags::none);
+        if (!r1) return false;
+
+        size_t packed_size = packed_frame.size();
+        const unsigned char* packed_data = static_cast<const unsigned char*>(packed_frame.data());
+        
+        // Strip topic prefix if configured
+        if (!topic_.empty()) {
+          if (packed_size < topic_.size()) return false; // too small
+          if (std::memcmp(packed_data, topic_.data(), topic_.size()) != 0) {
+            return false; // topic mismatch
+          }
+          packed_data += topic_.size();
+          packed_size -= topic_.size();
+        }
+
+        if (packed_size < HEADER_SIZE) {
+          if (verbose_) {
+            std::cerr << "[ZMQPackedMessageSubscriber] Packed frame too small: " << packed_size 
+                      << " < " << HEADER_SIZE << std::endl;
+          }
+          return false;
+        }
+
+        // Extract JSON header (first HEADER_SIZE bytes, null-terminated)
+        std::string header_json;
+        size_t json_len = strnlen(reinterpret_cast<const char*>(packed_data), HEADER_SIZE);
+        header_json.assign(reinterpret_cast<const char*>(packed_data), json_len);
+
+        // Decode header
+        DecodedHeader decoded;
+        if (!DecodeHeaderJSON(header_json, decoded)) {
+          if (verbose_) {
+            std::cerr << "[ZMQPackedMessageSubscriber] JSON parse failed" << std::endl;
+            std::cerr << "  Header content: " << header_json << std::endl;
+          }
+          return false;
+        }
+        
+        bool needs_swap = decoded.NeedsByteSwap();
+        
+        if (verbose_) {
+          std::cout << "[ZMQPackedMessageSubscriber] Parsed header: v=" << decoded.version 
+                    << " endian=" << decoded.endian << " count=" << decoded.count 
+                    << " fields=" << decoded.fields.size() 
+                    << " needs_byteswap=" << (needs_swap ? "yes" : "no") << std::endl;
+          for (size_t i = 0; i < decoded.fields.size(); ++i) {
+            const auto& f = decoded.fields[i];
+            std::cout << "  Field[" << i << "]: name=" << f.name << " dtype=" << f.dtype 
+                      << " shape=[";
+            for (size_t j = 0; j < f.shape.size(); ++j) {
+              std::cout << f.shape[j];
+              if (j < f.shape.size() - 1) std::cout << ",";
+            }
+            std::cout << "] bytes=" << f.ComputeByteSize() << std::endl;
+          }
+        }
+        
+        // Note: BufferView provides raw pointers. If needs_swap=true, user must byte-swap
+        // when reading multi-byte values. We don't modify the received data in-place.
+
+        // Build BufferViews for each field from the data section
+        const unsigned char* data_start = packed_data + HEADER_SIZE;
+        const size_t data_size = packed_size - HEADER_SIZE;
+        
+        std::vector<BufferView> buffers;
+        buffers.reserve(decoded.fields.size());
+        
+        size_t offset = 0;
+        for (const auto& field : decoded.fields) {
+          size_t field_bytes = field.ComputeByteSize();
+          if (offset + field_bytes > data_size) {
+            if (verbose_) {
+              std::cerr << "[ZMQPackedMessageSubscriber] Field " << field.name 
+                        << " exceeds data bounds" << std::endl;
+            }
+            return false;
+          }
+          buffers.push_back(BufferView{data_start + offset, field_bytes});
+          offset += field_bytes;
+        }
+
+        if (verbose_) {
+          std::cout << "[ZMQPackedMessageSubscriber] Received packed message: header_bytes=" << json_len 
+                    << " data_bytes=" << offset << " fields=" << buffers.size() << std::endl;
+        }
+
+        if (on_decoded_) {
+          on_decoded_(topic_, decoded, buffers); // Use configured topic, not extracted
+        }
+
+        return true;
+      } catch (const zmq::error_t &e) {
+        if (verbose_) {
+          std::cerr << "[ZMQPackedMessageSubscriber] Receive error: " << e.what() << std::endl;
+        }
+        Reconnect();
+        return false;
+      }
+    }
+
+  private:
+    void RunLoop() {
+      if (verbose_) {
+        std::cout << "[ZMQPackedMessageSubscriber] RunLoop started" << std::endl;
+      }
+      int poll_count = 0;
+      while (running_) {
+        PollOnce();
+        poll_count++;
+      }
+      if (verbose_) {
+        std::cout << "[ZMQPackedMessageSubscriber] RunLoop finished after " << poll_count << " polls" << std::endl;
+      }
+    }
+
+    void Reconnect() {
+      try {
+        if (socket_) {
+          try { socket_->close(); } catch (...) {}
+          socket_.reset();
+        }
+      } catch (...) {}
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      Connect();
+    }
+
+    bool DecodeHeaderJSON(const std::string& header_json, DecodedHeader& out) const {
+      try {
+        auto j = nlohmann::json::parse(header_json);
+        if (j.contains("v")) out.version = j["v"].get<int>();
+        if (j.contains("endian")) out.endian = j["endian"].get<std::string>();
+        if (j.contains("count")) out.count = j["count"].get<int>();
+        out.fields.clear();
+        if (j.contains("fields") && j["fields"].is_array()) {
+          for (const auto& f : j["fields"]) {
+            FieldInfo fi;
+            if (f.contains("name")) fi.name = f["name"].get<std::string>();
+            if (f.contains("dtype")) fi.dtype = f["dtype"].get<std::string>();
+            if (f.contains("optional")) fi.optional = f["optional"].get<bool>();
+            fi.shape.clear();
+            if (f.contains("shape") && f["shape"].is_array()) {
+              for (const auto& dim : f["shape"]) {
+                fi.shape.push_back(dim.get<size_t>());
+              }
+            }
+            out.fields.push_back(std::move(fi));
+          }
+        }
+        return true;
+      } catch (...) {
+        return false;
+      }
+    }
+
+    std::string host_;
+    int port_;
+    std::string topic_;
+    int timeout_ms_;
+    bool verbose_;
+    bool conflate_;
+    int rcv_hwm_;
+
+    zmq::context_t context_;
+    std::unique_ptr<zmq::socket_t> socket_;
+
+    std::atomic<bool> running_;
+    std::thread recv_thread_;
+
+    std::function<void(const std::string&, const DecodedHeader&, const std::vector<BufferView>&)> on_decoded_;
+};
+
+#endif // ZMQ_PACKED_MESSAGE_SUBSCRIBER_HPP
+

GR00T-WholeBodyControl/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/output_interface/zmq_output_handler.hpp

@@ -0,0 +1,423 @@
+/**
+ * @file zmq_output_handler.hpp
+ * @brief ZMQ PUB output handler for publishing robot state and configuration.
+ *
+ * Publishes two topic-prefixed streams over a single ZMQ PUB socket:
+ *
+ *   Topic Prefix    | Frequency       | Description
+ *   ----------------|-----------------|-------------------------------------------
+ *   {user_topic}    | Every tick      | Combined state + visualisation (msgpack).
+ *   robot_config    | Every ~2 s      | Robot configuration (msgpack, always re-published).
+ *
+ * Wire format (single-part ZMQ message):
+ *
+ *   [topic_prefix][msgpack payload]
+ *
+ * The topic prefix is a plain string (e.g. "g1_debug") prepended to the
+ * message so that subscribers using `zmq::sockopt::subscribe` can filter by
+ * topic.
+ *
+ * ---------------------------------------------------------------------------
+ * ## `{user_topic}` (e.g. `g1_debug`) — published every tick
+ * ---------------------------------------------------------------------------
+ *
+ * A single msgpack map with up to 30 keys (28 always-present + 2 conditional).
+ * All joints are in **MuJoCo order** (remapped from IsaacLab via
+ * `isaaclab_to_mujoco`).
+ *
+ *   #  | Key                    | Type         | Description
+ *   ---|------------------------|--------------|---------------------------------------------
+ *      | **Metadata**           |              |
+ *   1  | control_loop_type      | string       | Always "cpp".
+ *   2  | index                  | int          | Monotonic state-logger entry index.
+ *   3  | ros_timestamp          | double       | ROS 2 wall-clock (s); 0.0 if no ROS 2.
+ *      |                        |              |
+ *      | **Base IMU**           |              |
+ *   4  | base_quat              | double[4]    | Base IMU quaternion (w,x,y,z).
+ *   5  | base_ang_vel           | double[3]    | Base angular velocity.
+ *   6  | body_torso_quat        | double[4]    | Torso IMU quaternion.
+ *   7  | body_torso_ang_vel     | double[3]    | Torso angular velocity.
+ *      |                        |              |
+ *      | **Body joints**        |              |
+ *   8  | body_q                 | double[29]   | Joint positions (+ default offsets).
+ *   9  | body_dq                | double[29]   | Joint velocities.
+ *      |                        |              |
+ *      | **Hand joints**        |              |
+ *  10  | left_hand_q            | double[7]    | Left-hand joint positions (from state logger).
+ *  11  | left_hand_dq           | double[7]    | Left-hand joint velocities.
+ *  12  | right_hand_q           | double[7]    | Right-hand joint positions (from state logger).
+ *  13  | right_hand_dq          | double[7]    | Right-hand joint velocities.
+ *      |                        |              |
+ *      | **Policy actions**     |              |
+ *  14  | last_action            | double[29]   | Last body action (scaled + default offsets).
+ *  15  | last_left_hand_action  | double[7]    | Last left-hand action.
+ *  16  | last_right_hand_action | double[7]    | Last right-hand action.
+ *      |                        |              |
+ *      | **Encoder**            |              |
+ *  17  | token_state            | double[N]    | Encoder token state (empty array if N/A).
+ *      |                        |              |
+ *      | **Heading** *(conditional — only when heading state is available)* |
+ *  18  | init_base_quat         | double[4]    | Initial base quaternion at heading init.
+ *  19  | delta_heading          | double       | Accumulated heading delta (rad).
+ *      |                        |              |
+ *      | **Viz: targets** *(from current motion frame + heading correction)* |
+ *  20  | base_trans_target      | double[3]    | Target base translation.
+ *  21  | base_quat_target       | double[4]    | Target base quaternion.
+ *  22  | body_q_target          | double[29]   | Target joint positions.
+ *      |                        |              |
+ *      | **Viz: measured**      |              |
+ *  23  | base_trans_measured    | double[3]    | Measured base translation (fixed default).
+ *  24  | base_quat_measured     | double[4]    | Measured base quaternion (= base_quat).
+ *  25  | body_q_measured        | double[29]   | Measured joint positions (= body_q).
+ *  26  | left_hand_q_measured   | double[7]    | Measured left-hand Dex3 positions.
+ *  27  | right_hand_q_measured  | double[7]    | Measured right-hand Dex3 positions.
+ *      |                        |              |
+ *      | **Viz: VR 3-point**    |              |
+ *  28  | vr_3point_position     | double[9]    | VR positions (3×xyz, target body frame).
+ *  29  | vr_3point_orientation  | double[12]   | VR orientations (3×quat wxyz).
+ *  30  | vr_3point_compliance   | double[3]    | VR compliance (left arm, right arm, head).
+ *
+ * ---------------------------------------------------------------------------
+ * ## `robot_config` — re-published every ~2 s
+ * ---------------------------------------------------------------------------
+ *
+ * StateLogger configuration map (policy parameters, joint mappings, etc.)
+ * as a msgpack map of string → string | int | double | bool.
+ * Re-published on every `publish()` tick (throttled to ~2 s intervals) so
+ * that late-joining subscribers always receive it (ZMQ PUB has no persistence).
+ *
+ * ---------------------------------------------------------------------------
+ * ## Socket Options
+ * ---------------------------------------------------------------------------
+ *
+ * - Send HWM = 10 (old messages dropped rather than queued).
+ * - Send buffer = 32 KB.
+ * - Linger = 0 (immediate close, no pending-send wait).
+ * - Non-blocking send (dontwait) to avoid stalling the control loop.
+ */
+
+#ifndef ZMQ_OUTPUT_HANDLER_HPP
+#define ZMQ_OUTPUT_HANDLER_HPP
+
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <cstring>
+#include <map>
+#include <vector>
+#include <variant>
+#include <stdexcept>
+#include <zmq.hpp>
+#include <msgpack.hpp>
+
+#include "output_interface.hpp"
+#include "../policy_parameters.hpp"  // For isaaclab_to_mujoco, default_angles, g1_action_scale
+#include "../robot_parameters.hpp"   // For HeadingState
+#include "../utils.hpp"              // For DataBuffer
+
+/**
+ * @class ZMQOutputHandler
+ * @brief OutputInterface that publishes state data over a ZMQ PUB socket.
+ */
+class ZMQOutputHandler : public OutputInterface {
+public:
+    static constexpr bool DEBUG_LOGGING = true;
+
+    /**
+     * @brief Construct the handler: create a ZMQ PUB socket and bind to the given port.
+     * @param logger  Reference to the shared StateLogger.
+     * @param port    TCP port to bind the PUB socket to (e.g. 5557).
+     * @param topic   Topic prefix prepended to each published message.
+     */
+    explicit ZMQOutputHandler(StateLogger& logger, int port, const std::string& topic) 
+        : OutputInterface(logger), realtime_debug_context_(1), topic_(topic),
+          robot_config_topic_("robot_config") {
+
+        std::cout << "Initializing realtime debug socket" << std::endl;
+        std::cout << "Binding to port: " << port << " and topic: " << topic_ << std::endl;
+        realtime_debug_socket_ = std::make_unique<zmq::socket_t>(realtime_debug_context_, ZMQ_PUB);
+
+        realtime_debug_socket_->set(zmq::sockopt::sndhwm, 10);     // Drop old messages quickly
+        realtime_debug_socket_->set(zmq::sockopt::sndbuf, 32768);   // 32 KB send buffer
+        realtime_debug_socket_->set(zmq::sockopt::linger, 0);       // No lingering on close
+        realtime_debug_socket_->bind("tcp://*:" + std::to_string(port));
+
+        std::cout << "[INFO] Realtime debug socket bound to port: " << port << std::endl;
+
+        if constexpr (DEBUG_LOGGING) {
+            std::cout << "[ZMQ Output DEBUG] ZMQOutputHandler initialized with topics: "
+                      << "'" << topic_ << "' (combined state+viz), "
+                      << "'" << robot_config_topic_ << "' (config)" << std::endl;
+        }
+        
+        type_ = OutputType::ZMQ;
+    }
+
+    /**
+     * @brief Send combined visualisation + state-logger data in a single ZMQ message (non-blocking, called each tick).
+     *
+     * Calls `create_output_data_map()` to compute visualisation targets, then
+     * `pack_combined_state()` to merge state-logger + viz fields into one msgpack
+     * buffer, and sends it on the user topic with `zmq::send_flags::dontwait`.
+     */
+    void publish(
+        const std::array<double, 9>& vr_3point_position,
+        const std::array<double, 12>& vr_3point_orientation,
+        const std::array<double, 3>& vr_3point_compliance,
+        const std::array<double, 7>& left_hand_joint,
+        const std::array<double, 7>& right_hand_joint,
+        const std::array<double, 4>& init_ref_data_root_rot_array,
+        DataBuffer<HeadingState>& heading_state_buffer,
+        std::shared_ptr<const MotionSequence> current_motion,
+        int current_frame
+    ) override
+    {
+        // 1. Compute visualisation data (populates output_data_map_)
+        create_output_data_map(
+            vr_3point_position,
+            vr_3point_orientation,
+            vr_3point_compliance,
+            left_hand_joint,
+            right_hand_joint,
+            init_ref_data_root_rot_array,
+            heading_state_buffer,
+            current_motion,
+            current_frame
+        );
+
+        // 2. Build a single combined message with state-logger + visualisation fields
+        pack_combined_state(heading_state_buffer);
+
+        // 3. Send once on user topic (e.g. "g1_debug")
+        if (state_data_sbuf_.size() > 0) {
+            send_zmq_message(topic_, state_data_sbuf_);
+        }
+
+        // 4. Re-publish robot_config periodically (ZMQ has no persistence)
+        publish_config();
+    }
+
+    /**
+     * @brief Publish robot_config if enough time has elapsed since the last send.
+     *
+     * On the first call the config is serialised from StateLogger and cached.
+     * Subsequent calls simply re-send the cached buffer every
+     * `CONFIG_REPUBLISH_INTERVAL_SEC` seconds.  This gives ZMQ PUB the same
+     * "late-subscriber" semantics as ROS 2's `transient_local` QoS.
+     *
+     * Called once during init (from g1_deploy_onnx_ref.cpp) and then on every
+     * control-loop tick from `publish()`.
+     */
+    void publish_config() override {
+        // Lazy-init: serialise once, reuse forever.
+        if (config_sbuf_cache_.size() == 0) {
+            auto config_opt = state_logger_.GetConfig();
+            if (!config_opt.has_value() || config_opt->empty()) {
+                throw std::runtime_error("[ZMQ Output ERROR] Cannot publish config: StateLogger config is empty");
+            }
+            pack_robot_config(config_sbuf_cache_, *config_opt);
+            std::cout << "[ZMQ Output] Robot config cached ("
+                      << config_opt->size() << " fields, "
+                      << config_sbuf_cache_.size() << " bytes)" << std::endl;
+        }
+
+        auto now = std::chrono::steady_clock::now();
+        double elapsed = std::chrono::duration<double>(now - config_last_publish_time_).count();
+        if (elapsed >= CONFIG_REPUBLISH_INTERVAL_SEC) {
+            send_zmq_message(robot_config_topic_, config_sbuf_cache_);
+            config_last_publish_time_ = now;
+        }
+    }
+
+private:
+    zmq::context_t realtime_debug_context_;                ///< ZMQ context (1 I/O thread).
+    std::unique_ptr<zmq::socket_t> realtime_debug_socket_; ///< ZMQ PUB socket.
+
+    std::string topic_;              ///< User-provided topic name (e.g. "g1_debug") for combined state+viz.
+    std::string robot_config_topic_; ///< Topic for robot config messages.
+
+    msgpack::sbuffer state_data_sbuf_;  ///< Reused each tick; cleared in pack_combined_state().
+
+    // -- Config re-publish (ZMQ equivalent of ROS 2 transient_local) --
+    static constexpr double CONFIG_REPUBLISH_INTERVAL_SEC = 2.0;
+    msgpack::sbuffer config_sbuf_cache_;  ///< Serialised config (populated on first publish_config()).
+    std::chrono::steady_clock::time_point config_last_publish_time_;
+
+    /// Non-blocking send of [topic][msgpack payload] over the PUB socket.
+    void send_zmq_message(const std::string& topic, const msgpack::sbuffer& sbuf) {
+        zmq::message_t msg(topic.size() + sbuf.size());
+        memcpy(msg.data(), topic.c_str(), topic.size());
+        memcpy(static_cast<char*>(msg.data()) + topic.size(), sbuf.data(), sbuf.size());
+        realtime_debug_socket_->send(msg, zmq::send_flags::dontwait);
+    }
+
+    /**
+     * @brief Serialise both state-logger and visualisation data into state_data_sbuf_.
+     *
+     * Combines into a single msgpack map:
+     *   - State-logger fields (body_q, body_dq, last_action, etc. in MuJoCo order)
+     *   - Visualisation fields from output_data_map_ (targets, VR data, measured poses)
+     *
+     * This avoids sending two overlapping messages per tick.
+     */
+    void pack_combined_state(const DataBuffer<HeadingState>& heading_state_buffer) {
+        state_data_sbuf_.clear();
+
+        if (state_logger_.size() == 0) {
+            return;
+        }
+
+        std::vector<StateLogger::Entry> entries = state_logger_.GetLatest(1);
+        const StateLogger::Entry& state = entries[0];
+        msgpack::packer<msgpack::sbuffer> pk(&state_data_sbuf_);
+
+        HeadingState heading_state;
+        bool has_heading_state = false;
+        auto heading_state_data = heading_state_buffer.GetDataWithTime().data;
+        if (heading_state_data) {
+            heading_state = *heading_state_data;
+            has_heading_state = true;
+        }
+
+        // State-logger fields: 18 base + 2 optional heading
+        // Visualisation fields: output_data_map_.size() (typically 11)
+        int num_state_fields = has_heading_state ? 20 : 18;
+        int num_viz_fields = static_cast<int>(output_data_map_.size());
+        pk.pack_map(num_state_fields + num_viz_fields);
+
+        // ---- State-logger fields ----
+
+        pk.pack("control_loop_type");
+        pk.pack("cpp");
+
+        pk.pack("index");
+        pk.pack(state.index);
+
+        pk.pack("ros_timestamp");
+        pk.pack(state.ros_timestamp);
+
+        pk.pack("base_quat");
+        pk.pack_array(4);
+        for (const auto& val : state.base_quat) pk.pack(val);
+
+        pk.pack("base_ang_vel");
+        pk.pack_array(3);
+        for (const auto& val : state.base_ang_vel) pk.pack(val);
+
+        pk.pack("body_torso_quat");
+        pk.pack_array(4);
+        for (const auto& val : state.body_torso_quat) pk.pack(val);
+
+        pk.pack("body_torso_ang_vel");
+        pk.pack_array(3);
+        for (const auto& val : state.body_torso_ang_vel) pk.pack(val);
+
+        // body_q: IsaacLab -> MuJoCo order, add default-angle offset
+        pk.pack("body_q");
+        pk.pack_array(state.body_q.size());
+        if (state.body_q.size() == 29) {
+            std::array<double, 29> body_q_mujoco;
+            for (size_t i = 0; i < 29; ++i)
+                body_q_mujoco[i] = state.body_q[isaaclab_to_mujoco[i]] + default_angles[i];
+            for (const auto& val : body_q_mujoco) pk.pack(val);
+        } else {
+            for (const auto& val : state.body_q) pk.pack(val);
+        }
+
+        // body_dq: IsaacLab -> MuJoCo order (no offset for velocities)
+        pk.pack("body_dq");
+        pk.pack_array(state.body_dq.size());
+        if (state.body_dq.size() == 29) {
+            std::array<double, 29> body_dq_mujoco;
+            for (size_t i = 0; i < 29; ++i)
+                body_dq_mujoco[i] = state.body_dq[isaaclab_to_mujoco[i]];
+            for (const auto& val : body_dq_mujoco) pk.pack(val);
+        } else {
+            for (const auto& val : state.body_dq) pk.pack(val);
+        }
+
+        // last_action: IsaacLab -> MuJoCo order, scale + default-angle offset
+        pk.pack("last_action");
+        pk.pack_array(state.last_action.size());
+        if (state.last_action.size() == 29) {
+            std::array<double, 29> last_action_mujoco;
+            for (size_t i = 0; i < 29; ++i)
+                last_action_mujoco[i] = state.last_action[isaaclab_to_mujoco[i]] * g1_action_scale[i] + default_angles[i];
+            for (const auto& val : last_action_mujoco) pk.pack(val);
+        } else {
+            for (const auto& val : state.last_action) pk.pack(val);
+        }
+
+        pk.pack("left_hand_q");
+        pk.pack_array(state.left_hand_q.size());
+        for (const auto& val : state.left_hand_q) pk.pack(val);
+
+        pk.pack("left_hand_dq");
+        pk.pack_array(state.left_hand_dq.size());
+        for (const auto& val : state.left_hand_dq) pk.pack(val);
+
+        pk.pack("right_hand_q");
+        pk.pack_array(state.right_hand_q.size());
+        for (const auto& val : state.right_hand_q) pk.pack(val);
+
+        pk.pack("right_hand_dq");
+        pk.pack_array(state.right_hand_dq.size());
+        for (const auto& val : state.right_hand_dq) pk.pack(val);
+
+        pk.pack("last_left_hand_action");
+        pk.pack_array(state.last_left_hand_action.size());
+        for (const auto& val : state.last_left_hand_action) pk.pack(val);
+
+        pk.pack("last_right_hand_action");
+        pk.pack_array(state.last_right_hand_action.size());
+        for (const auto& val : state.last_right_hand_action) pk.pack(val);
+
+        pk.pack("token_state");
+        if (state.has_post_state_data && !state.token_state.empty()) {
+            pk.pack_array(state.token_state.size());
+            for (const auto& val : state.token_state) pk.pack(val);
+        } else {
+            pk.pack_array(0);
+        }
+
+        // Motor temperature: hardware order, 2 values per motor (winding, driver)
+        pk.pack("motor_temperature");
+        pk.pack_array(state.motor_temperature.size());
+        for (const auto& val : state.motor_temperature) pk.pack(val);
+
+        if (has_heading_state) {
+            pk.pack("init_base_quat");
+            pk.pack_array(4);
+            for (const auto& val : heading_state.init_base_quat) pk.pack(val);
+
+            pk.pack("delta_heading");
+            pk.pack(heading_state.delta_heading);
+        }
+
+        // ---- Visualisation fields (from output_data_map_) ----
+        // Adds: base_trans_target, base_quat_target, body_q_target,
+        //       base_trans_measured, base_quat_measured, body_q_measured,
+        //       left_hand_q_measured, right_hand_q_measured,
+        //       vr_3point_position, vr_3point_orientation, vr_3point_compliance
+        for (const auto& [key, values] : output_data_map_) {
+            pk.pack(key);
+            pk.pack_array(values.size());
+            for (const auto& val : values) pk.pack(val);
+        }
+    }
+
+    /// Serialise a config map into the given sbuffer.
+    static void pack_robot_config(msgpack::sbuffer& sbuf,
+                                  const std::map<std::string, std::variant<std::string, int, double, bool>>& config) {
+        sbuf.clear();
+        msgpack::packer<msgpack::sbuffer> pk(&sbuf);
+        pk.pack_map(config.size());
+        for (const auto& [key, value] : config) {
+            pk.pack(key);
+            std::visit([&pk](auto&& arg) { pk.pack(arg); }, value);
+        }
+    }
+
+};
+
+#endif // ZMQ_OUTPUT_HANDLER_HPP

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/.devcontainer/Dockerfile.devcontainer

@@ -0,0 +1,35 @@
+FROM ubuntu:20.04
+
+ENV TZ=Asia/Shanghai
+RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone 
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+USER root
+
+# install zsh and oh-my-zsh
+RUN apt update && apt-get install -y git wget zsh && \
+    wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh && chmod +x install.sh
+RUN ./install.sh
+RUN git clone https://github.com/zsh-users/zsh-autosuggestions.git ${ZSH_CUSTOM:-~/.oh-my-zsh/custom}/plugins/zsh-autosuggestions && \
+    apt install zsh-syntax-highlighting
+# configure zsh
+RUN echo 'export ZSH=$HOME/.oh-my-zsh' >> ~/.zshrc && \
+    echo 'ZSH_THEME="ys"' >> ~/.zshrc && \
+    echo 'plugins=(git zsh-autosuggestions)' >> ~/.zshrc && \
+    echo 'source $ZSH/oh-my-zsh.sh' >> ~/.zshrc && \
+    echo 'source /usr/share/zsh-syntax-highlighting/zsh-syntax-highlighting.zsh' >> ~/.zshrc
+RUN chsh -s /bin/zsh 
+
+# install build tools
+RUN apt-get install -y build-essential gcc-9  g++-9 gdb  cmake  ninja-build
+
+RUN update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 100 \
+    && update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-9 100
+
+# install libraries
+RUN apt-get install -y libyaml-cpp-dev libeigen3-dev
+
+WORKDIR /workspace
+
+CMD ["zsh"]

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/.devcontainer/devcontainer.json

@@ -0,0 +1,29 @@
+{
+	"name": "Existing Docker Compose (Extend)",
+	"dockerComposeFile": [
+		"docker-compose.yml"
+	],
+	"service": "devcontainer",
+	"workspaceFolder": "/workspace",
+	"customizations": {
+		"vscode": {
+			"extensions": [
+				"gitlab.gitlab-workflow",
+				"llvm-vs-code-extensions.vscode-clangd",
+				"xaver.clang-format",
+				"ms-vscode.cmake-tools",
+				"zxh404.vscode-proto3",
+				"donjayamanne.githistory",
+				"mhutchie.git-graph",
+				"matepek.vscode-catch2-test-adapter",
+				"ryanluker.vscode-coverage-gutters",
+				"donjayamanne.python-extension-pack",
+				"adam-bender.commit-message-editor",
+				"axetroy.vscode-changelog-generator",
+				"vsliveshare.vsliveshare",
+				"bierner.markdown-mermaid",
+				"vadimcn.vscode-lldb"
+			]
+		}
+	}
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/.devcontainer/docker-compose.yml

@@ -0,0 +1,25 @@
+version: '3'
+services: 
+  devcontainer: 
+    volumes: 
+      - ..:/workspace:cached
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - /mnt/wslg:/mnt/wslg
+      - /root/.gazebo/models/:/root/.gazebo/models/
+      - /var/run/docker.sock:/var/run/docker.sock 
+      - $HOME/.Xauthority:/tmp/.Xauthority:rw
+    environment:
+      - DISPLAY=${DISPLAY}
+      - XAUTHORITY=/tmp/.Xauthority
+
+    cap_add:
+      - SYS_PTRACE  
+    command: /bin/sh -c "while sleep 1000; do :; done"
+ 
+    build:
+      context: .
+      dockerfile: Dockerfile.devcontainer 
+
+    privileged: true
+    network_mode: "host"
+ 

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/.gitignore

@@ -0,0 +1,8 @@
+# IDE
+.vscode
+
+# cpp
+build
+
+#cland
+.cache

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.5)
+project(unitree_sdk2 VERSION 2.0.0)
+
+## Project Options
+option(BUILD_EXAMPLES "Build examples" ON)
+
+## Set compiler to use c++ 17 features
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## Chosse build type
+set(default_build_type "Release")
+if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+    message(STATUS "Setting build type to '${default_build_type}' as none was specified.")
+    set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE
+            STRING "Choose the type of build." FORCE)
+    # Set the possible values of build type for cmake-gui
+    set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS
+            "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+endif ()
+
+## Use GNUInstallDirs to install libraries into correct locations on all platforms.
+include(GNUInstallDirs)
+
+## Put all binary files into /bin and libraries into /lib
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${CMAKE_INSTALL_BINDIR})
+
+## Check system architecture
+message(STATUS "Current system architecture: ${CMAKE_SYSTEM_PROCESSOR}")
+
+## Import thirdparty libraries
+add_subdirectory(thirdparty)
+
+## Import Unitree SDK2 library
+set(UNITREE_SDK_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/${CMAKE_SYSTEM_PROCESSOR})
+find_library(UNITREE_SDK_LIB unitree_sdk2 PATHS ${UNITREE_SDK_PATH} NO_DEFAULT_PATH)
+
+if (NOT UNITREE_SDK_LIB)
+    message(FATAL_ERROR "Unitree SDK library for the architecture is not found")
+else ()
+    message(STATUS "Unitree SDK library found at: ${UNITREE_SDK_LIB}")
+endif ()
+
+message(STATUS "Importing: ${UNITREE_SDK_LIB}")
+
+find_package(Threads REQUIRED)
+
+add_library(unitree_sdk2 STATIC IMPORTED GLOBAL)
+set_target_properties(unitree_sdk2 PROPERTIES
+        IMPORTED_LOCATION ${UNITREE_SDK_LIB})
+target_link_libraries(unitree_sdk2 INTERFACE ddsc ddscxx Threads::Threads)
+target_include_directories(unitree_sdk2 INTERFACE
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+if (BUILD_EXAMPLES)
+    add_subdirectory(example)
+endif ()
+
+## Install the library
+install(DIRECTORY include/
+    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+install(FILES ${UNITREE_SDK_LIB}
+    DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+install(FILES cmake/unitree_sdk2Targets.cmake
+    DESTINATION lib/cmake/unitree_sdk2)
+
+include(CMakePackageConfigHelpers)
+write_basic_package_version_file(
+    unitree_sdk2ConfigVersion.cmake
+    VERSION "${${PROJECT_NAME}_VERSION_MAJOR}.${${PROJECT_NAME}_VERSION_MINOR}.${${PROJECT_NAME}_VERSION_PATCH}"
+    COMPATIBILITY ExactVersion)
+
+configure_file(cmake/unitree_sdk2Config.cmake.in unitree_sdk2Config.cmake @ONLY)
+install(FILES "${CMAKE_CURRENT_BINARY_DIR}/unitree_sdk2Config.cmake"
+    "${CMAKE_CURRENT_BINARY_DIR}/unitree_sdk2ConfigVersion.cmake"
+    DESTINATION lib/cmake/unitree_sdk2)

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/README.md

@@ -0,0 +1,60 @@
+# unitree_sdk2
+Unitree robot sdk version 2.
+
+### Prebuild environment
+* OS  (Ubuntu 20.04 LTS)  
+* CPU  (aarch64 and x86_64)   
+* Compiler  (gcc version 9.4.0) 
+
+### Environment Setup
+
+Before building or running the SDK, ensure the following dependencies are installed:
+
+- CMake (version 3.10 or higher)
+- GCC (version 9.4.0)
+- Make
+
+You can install the required packages on Ubuntu 20.04 with:
+
+```bash
+apt-get update
+apt-get install -y cmake g++ build-essential libyaml-cpp-dev libeigen3-dev libboost-all-dev libspdlog-dev libfmt-dev
+```
+
+### Build examples
+
+To build the examples inside this repository:
+
+```bash
+mkdir build
+cd build
+cmake ..
+make
+```
+
+### Installation
+
+To build your own application with the SDK, you can install the unitree_sdk2 to your system directory:
+
+```bash
+mkdir build
+cd build
+cmake ..
+sudo make install
+```
+
+Or install unitree_sdk2 to a specified directory:
+
+```bash
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=/opt/unitree_robotics
+sudo make install
+```
+
+You can refer to `example/cmake_sample` on how to import the unitree_sdk2 into your CMake project. 
+
+Note that if you install the library to other places other than `/opt/unitree_robotics`, you need to make sure the path is added to "${CMAKE_PREFIX_PATH}" so that cmake can find it with "find_package()".
+
+### Notice
+For more reference information, please go to [Unitree Document Center](https://support.unitree.com/home/zh/developer).

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/cmake/unitree_sdk2Config.cmake.in

@@ -0,0 +1,7 @@
+include(CMakeFindDependencyMacro)
+
+# Same syntax as find_package
+find_dependency(Threads REQUIRED)
+
+# Add the targets file
+include("${CMAKE_CURRENT_LIST_DIR}/unitree_sdk2Targets.cmake")

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/cmake/unitree_sdk2Targets.cmake

@@ -0,0 +1,96 @@
+if(CMAKE_VERSION VERSION_LESS 3.5.0)
+  message(FATAL_ERROR "This file relies on consumers using CMake 3.5.0 or greater.")
+endif()
+cmake_policy(PUSH)
+cmake_policy(VERSION 2.6)
+
+# Commands may need to know the format version.
+set(CMAKE_IMPORT_FILE_VERSION 1)
+
+# Protect against multiple inclusion, which would fail when already imported targets are added once more.
+set(_targetsDefined)
+set(_targetsNotDefined)
+set(_expectedTargets)
+foreach(_expectedTarget unitree_sdk2)
+  list(APPEND _expectedTargets ${_expectedTarget})
+  if(NOT TARGET ${_expectedTarget})
+    list(APPEND _targetsNotDefined ${_expectedTarget})
+  endif()
+  if(TARGET ${_expectedTarget})
+    list(APPEND _targetsDefined ${_expectedTarget})
+  endif()
+endforeach()
+if("${_targetsDefined}" STREQUAL "${_expectedTargets}")
+  unset(_targetsDefined)
+  unset(_targetsNotDefined)
+  unset(_expectedTargets)
+  set(CMAKE_IMPORT_FILE_VERSION)
+  cmake_policy(POP)
+  return()
+endif()
+if(NOT "${_targetsDefined}" STREQUAL "")
+  message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_targetsDefined}\nTargets not yet defined: ${_targetsNotDefined}\n")
+endif()
+unset(_targetsDefined)
+unset(_targetsNotDefined)
+unset(_expectedTargets)
+
+
+# Compute the installation prefix relative to this file.
+get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+if(_IMPORT_PREFIX STREQUAL "/")
+  set(_IMPORT_PREFIX "")
+endif()
+
+# Create imported target ddsc and ddscxx
+add_library(ddsc SHARED IMPORTED GLOBAL)
+set_target_properties(ddsc PROPERTIES
+    IMPORTED_LOCATION ${_IMPORT_PREFIX}/lib/libddsc.so
+    INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include;${_IMPORT_PREFIX}/include"
+    INTERFACE_LINK_LIBRARIES "Threads::Threads"
+    IMPORTED_NO_SONAME TRUE)
+
+add_library(ddscxx SHARED IMPORTED GLOBAL)
+set_target_properties(ddscxx PROPERTIES
+    IMPORTED_LOCATION ${_IMPORT_PREFIX}/lib/libddscxx.so
+    INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include;${_IMPORT_PREFIX}/include/ddscxx"
+    INTERFACE_LINK_LIBRARIES "Threads::Threads"
+    IMPORTED_NO_SONAME TRUE)
+
+# Create imported target unitree_sdk2
+add_library(unitree_sdk2 STATIC IMPORTED GLOBAL)
+set_target_properties(unitree_sdk2 PROPERTIES
+    IMPORTED_LOCATION ${_IMPORT_PREFIX}/lib/libunitree_sdk2.a
+    INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include;${_IMPORT_PREFIX}/include"
+    INTERFACE_LINK_LIBRARIES "ddsc;ddscxx;Threads::Threads"
+    LINKER_LANGUAGE CXX
+)
+
+# Cleanup temporary variables.
+set(_IMPORT_PREFIX)
+
+# Loop over all imported files and verify that they actually exist
+foreach(target ${_IMPORT_CHECK_TARGETS} )
+  foreach(file ${_IMPORT_CHECK_FILES_FOR_${target}} )
+    if(NOT EXISTS "${file}" )
+      message(FATAL_ERROR "The imported target \"${target}\" references the file
+   \"${file}\"
+but this file does not exist.  Possible reasons include:
+* The file was deleted, renamed, or moved to another location.
+* An install or uninstall procedure did not complete successfully.
+* The installation package was faulty and contained
+   \"${CMAKE_CURRENT_LIST_FILE}\"
+but not all the files it references.
+")
+    endif()
+  endforeach()
+  unset(_IMPORT_CHECK_FILES_FOR_${target})
+endforeach()
+unset(_IMPORT_CHECK_TARGETS)
+
+# Commands beyond this point should not need to know the version.
+set(CMAKE_IMPORT_FILE_VERSION)
+cmake_policy(POP)

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_subdirectory(helloworld)
+add_subdirectory(wireless_controller)
+add_subdirectory(jsonize)
+add_subdirectory(state_machine)
+
+
+add_subdirectory(go2)
+add_subdirectory(b2)
+add_subdirectory(h1)
+add_subdirectory(g1)
+add_subdirectory(go2w)
+add_subdirectory(b2w)

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(b2_sport_client  b2_sport_client.cpp)
+target_link_libraries(b2_sport_client unitree_sdk2)
+
+add_executable(b2_stand_example  b2_stand_example.cpp)
+target_link_libraries(b2_stand_example unitree_sdk2)

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2/b2_sport_client.cpp

@@ -0,0 +1,24 @@
+#include <unitree/robot/go2/sport/sport_client.hpp>
+
+int main()
+{
+    /*
+     * Initilaize ChannelFactory
+     */
+    unitree::robot::ChannelFactory::Instance()->Init(0);
+    unitree::robot::go2::SportClient sc;
+
+    sc.SetTimeout(5.0f);
+    sc.Init();
+
+    //Test Api
+    while (true)
+    {
+        int32_t ret = sc.Move(0.5, 0.0, 0.0);
+        std::cout << "Call Move ret:" << ret << std::endl;
+
+        usleep(1000);
+    }
+
+    return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2/b2_stand_example.cpp

@@ -0,0 +1,350 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree::robot::b2;
+
+#define TOPIC_LOWCMD "rt/lowcmd"
+#define TOPIC_LOWSTATE "rt/lowstate"
+
+constexpr double PosStopF = (2.146E+9f);
+constexpr double VelStopF = (16000.0f);
+
+class Custom
+{
+public:
+    explicit Custom(){}
+    ~Custom(){}
+
+    void Init();
+    void Start();
+
+private:
+    void InitLowCmd();
+    void LowStateMessageHandler(const void* messages);
+    void LowCmdWrite();
+    int queryMotionStatus();
+    std::string queryServiceName(std::string form,std::string name);
+
+private:
+    float Kp = 1000.0;
+    float Kd = 10.0;
+    double time_consume = 0;
+    int rate_count = 0;
+    int sin_count = 0;
+    int motiontime = 0;
+    float dt = 0.002; // 0.001~0.01
+
+    MotionSwitcherClient msc;
+
+    unitree_go::msg::dds_::LowCmd_ low_cmd{};      // default init
+    unitree_go::msg::dds_::LowState_ low_state{};  // default init
+
+    /*publisher*/
+    ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
+    /*subscriber*/
+    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
+
+    /*LowCmd write thread*/
+    ThreadPtr lowCmdWriteThreadPtr;
+
+    float _targetPos_1[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                              0.2, 1.36, -2.65, 0.2, 1.36, -2.65};
+
+    float _targetPos_2[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                              0.0, 0.67, -1.3, 0.0, 0.67, -1.3};
+
+    float _targetPos_3[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                              0.0, 1.36, -2.65, 0.0, 1.36, -2.65};
+
+    float _targetPos_4[12] = {-0.5, 1.36, -2.65, 0.5, 1.36, -2.65,
+                              -0.5, 1.36, -2.65, 0.5, 1.36, -2.65};
+
+    float _startPos[12];
+    float _duration_1 = 500;   
+    float _duration_2 = 900; 
+    float _duration_3 = 1000;   
+    float _duration_4 = 1100;   
+    float _duration_5 = 500;   
+    float _percent_1 = 0;    
+    float _percent_2 = 0;    
+    float _percent_3 = 0;    
+    float _percent_4 = 0;    
+    float _percent_5 = 0;    
+
+    bool firstRun = true;
+    bool done = false;
+};
+
+uint32_t crc32_core(uint32_t* ptr, uint32_t len)
+{
+    unsigned int xbit = 0;
+    unsigned int data = 0;
+    unsigned int CRC32 = 0xFFFFFFFF;
+    const unsigned int dwPolynomial = 0x04c11db7;
+
+    for (unsigned int i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (unsigned int bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+            {
+                CRC32 <<= 1;
+            }
+
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+            xbit >>= 1;
+        }
+    }
+
+    return CRC32;
+}
+
+void Custom::Init()
+{
+    InitLowCmd();
+
+    /*create publisher*/
+    lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
+    lowcmd_publisher->InitChannel();
+
+    /*create subscriber*/
+    lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
+    lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1);
+
+    /*init MotionSwitcherClient*/
+    msc.SetTimeout(10.0f); 
+    msc.Init();
+
+    /*Shut down  motion control-related service*/
+    while(queryMotionStatus())
+    {
+        std::cout << "Try to deactivate the motion control-related service." << std::endl;
+        int32_t ret = msc.ReleaseMode(); 
+        if (ret == 0) {
+            std::cout << "ReleaseMode succeeded." << std::endl;
+        } else {
+            std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
+        }
+        sleep(5);
+    }
+}
+
+void Custom::InitLowCmd()
+{
+    low_cmd.head()[0] = 0xFE;
+    low_cmd.head()[1] = 0xEF;
+    low_cmd.level_flag() = 0xFF;
+    low_cmd.gpio() = 0;
+
+    for(int i=0; i<20; i++)
+    {
+        low_cmd.motor_cmd()[i].mode() = (0x0A);   // motor switch to servo (PMSM) mode
+        low_cmd.motor_cmd()[i].q() = (PosStopF);
+        low_cmd.motor_cmd()[i].kp() = (0);
+        low_cmd.motor_cmd()[i].dq() = (VelStopF);
+        low_cmd.motor_cmd()[i].kd() = (0);
+        low_cmd.motor_cmd()[i].tau() = (0);
+    }
+}
+
+int Custom::queryMotionStatus()
+{
+    std::string robotForm,motionName;
+    int motionStatus;
+    int32_t ret = msc.CheckMode(robotForm,motionName);
+    if (ret == 0) {
+        std::cout << "CheckMode succeeded." << std::endl;
+    } else {
+        std::cout << "CheckMode failed. Error code: " << ret << std::endl;
+    }
+    if(motionName.empty())
+    {
+        std::cout << "The motion control-related service is deactivated." << std::endl;
+        motionStatus = 0;
+    }
+    else
+    {
+        std::string serviceName = queryServiceName(robotForm,motionName);
+        std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
+        motionStatus = 1;
+    }
+    return motionStatus;
+}
+
+std::string Custom::queryServiceName(std::string form,std::string name)
+{
+    if(form == "0")
+    {
+        if(name == "normal" ) return "sport_mode"; 
+        if(name == "ai" ) return "ai_sport"; 
+        if(name == "advanced" ) return "advanced_sport"; 
+    }
+    else
+    {
+        if(name == "ai-w" ) return "wheeled_sport(go2W)"; 
+        if(name == "normal-w" ) return "wheeled_sport(b2W)";
+    }
+    return "";
+}
+
+void Custom::Start()
+{
+    /*loop publishing thread*/
+    lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Custom::LowCmdWrite, this);
+}
+
+void Custom::LowStateMessageHandler(const void* message)
+{
+    low_state = *(unitree_go::msg::dds_::LowState_*)message;
+}
+
+void Custom::LowCmdWrite()
+{
+    if(_percent_5<1)
+    {
+        std::cout<<"Read sensor data example: "<<std::endl;
+        std::cout<<"Joint 0 pos: "<<low_state.motor_state()[0].q()<<std::endl;
+        std::cout<<"Imu accelerometer : "<<"x: "<<low_state.imu_state().accelerometer()[0]<<" y: "<<low_state.imu_state().accelerometer()[1]<<" z: "<<low_state.imu_state().accelerometer()[2]<<std::endl;
+        std::cout<<"Foot force "<<low_state.foot_force()[0]<<std::endl;
+        std::cout<<std::endl;
+    }
+    if((_percent_5 == 1) && ( done == false))
+    {
+        std::cout<<"The example is done! "<<std::endl;
+        std::cout<<std::endl;
+        done = true;
+    }
+
+    motiontime++;
+    if(motiontime>=500)
+    {
+        if(firstRun)
+        {
+            for(int i = 0; i < 12; i++)
+            {
+                _startPos[i] = low_state.motor_state()[i].q();
+            }
+            firstRun = false;
+        }
+
+        _percent_1 += (float)1 / _duration_1;
+        _percent_1 = _percent_1 > 1 ? 1 : _percent_1;
+        if (_percent_1 < 1)
+        {
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_1) * _startPos[j] + _percent_1 * _targetPos_1[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        
+        }
+        if ((_percent_1 == 1)&&(_percent_2 < 1))
+        {
+            _percent_2 += (float)1 / _duration_2;
+            _percent_2 = _percent_2 > 1 ? 1 : _percent_2;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_2) * _targetPos_1[j] + _percent_2 * _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3<1))
+        {
+            _percent_3 += (float)1 / _duration_3;
+            _percent_3 = _percent_3 > 1 ? 1 : _percent_3;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() =  _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&(_percent_4<1))
+        {
+            _percent_4 += (float)1 / _duration_4;
+            _percent_4 = _percent_4 > 1 ? 1 : _percent_4;
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_4) * _targetPos_2[j] + _percent_4 * _targetPos_3[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&(_percent_4==1)&&((_percent_5<=1)))
+        {
+            _percent_5 += (float)1 / _duration_5;
+            _percent_5 = _percent_5 > 1 ? 1 : _percent_5;
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_5) * _targetPos_3[j] + _percent_5 * _targetPos_4[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+        low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
+    
+        lowcmd_publisher->Write(low_cmd);
+    }
+   
+}
+
+int main(int argc, const char** argv)
+{
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1); 
+    }
+
+    std::cout << "WARNING: Make sure the robot is lying on the ground." << std::endl
+            << "Press Enter to continue..." << std::endl;
+    std::cin.ignore();
+
+    ChannelFactory::Instance()->Init(0, argv[1]);
+
+    Custom custom;
+    custom.Init();
+    custom.Start();
+  
+    while (1)
+    {
+        sleep(10);
+    }
+
+    return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2w/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(b2w_sport_client b2w_sport_client.cpp)
+target_link_libraries(b2w_sport_client unitree_sdk2)
+
+add_executable(b2w_stand_example b2w_stand_example.cpp)
+target_link_libraries(b2w_stand_example unitree_sdk2)

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2w/b2w_sport_client.cpp

@@ -0,0 +1,170 @@
+#include <iostream>
+#include <string>
+#include <chrono>
+#include <mutex>
+#include <thread>
+#include <pthread.h>
+#include <stdexcept>
+#include <unitree/robot/b2/sport/sport_client.hpp>
+
+using namespace std;
+
+struct TestOption
+{
+    std::string name; 
+    int id;           
+};
+
+const vector<TestOption> option_list =
+    {{"damp", 0},        
+     {"stand_up", 1},    
+     {"stand_down", 2},  
+     {"move forward", 3},
+     {"move lateral", 4},         
+     {"move rotate", 5},        
+     {"stop_move", 6},   
+     {"switch_gait", 7}, 
+     {"switch_gait", 8},
+     {"recovery", 9},   
+    }; 
+
+int ConvertToInt(const std::string &str)
+{
+    try
+    {
+        std::stoi(str);
+        return std::stoi(str);
+    }
+    catch (const std::invalid_argument &)
+    {
+        return -1;
+    }
+    catch (const std::out_of_range &)
+    {
+        return -1; 
+    }
+}
+
+class UserInterface
+{
+public:
+    UserInterface(){};
+    ~UserInterface(){};
+
+    void terminalHandle()
+    {
+        std::string input;
+        std::getline(std::cin, input);
+
+        if (input.compare("list") == 0)
+        {
+            for (TestOption option : option_list)
+            {
+                std::cout << option.name << ", id: " << option.id << std::endl;
+            }
+        }
+
+        for (TestOption option : option_list)
+        {
+            if (input.compare(option.name) == 0 || ConvertToInt(input) == option.id)
+            {
+                test_option_->id = option.id;
+                test_option_->name = option.name;
+                std::cout << "Test: " << test_option_->name << ", test_id: " << test_option_->id << std::endl;
+            }
+        }
+    };
+
+    TestOption *test_option_;
+};
+
+int main(int argc, char **argv)
+{
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1);
+    }
+    unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+    TestOption test_option;
+    test_option.id = 1;
+
+    unitree::robot::b2::SportClient sport_client;
+    sport_client.SetTimeout(25.0f);
+    sport_client.Init();
+
+    UserInterface user_interface;
+    user_interface.test_option_ = &test_option;
+
+    std::cout << "Input \"list \" to list all test option ..." << std::endl;
+    long res_count = 0;
+    while (1)
+    {
+        auto time_start_trick = std::chrono::high_resolution_clock::now();
+        static const constexpr auto dt = std::chrono::microseconds(20000); // 50Hz
+
+        user_interface.terminalHandle();
+
+        int res = 1;
+        if (test_option.id == 0)
+        {
+            res = sport_client.Damp();
+        }
+        else if (test_option.id == 1)
+        {
+            res = sport_client.StandUp();
+        }
+        else if (test_option.id == 2)
+        {
+            res = sport_client.StandDown();
+        }
+        else if (test_option.id == 3)
+        {
+            res = sport_client.Move(0.3, 0, 0);// the robot will move for 0.5 seconds before stopping.
+                                               // If the Move command is called in a loop, the robot will continue moving. 
+                                               // If no Move request is received within 0.5 seconds, the robot will automatically stop.
+        }
+        else if (test_option.id == 4)
+        {
+            res = sport_client.Move(0, 0.3, 0);// the robot will move for 0.5 seconds before stopping.
+                                               // If the Move command is called in a loop, the robot will continue moving. 
+                                               // If no Move request is received within 0.5 seconds, the robot will automatically stop.
+        }
+        else if (test_option.id == 5)
+        {
+            res = sport_client.Move(0, 0, 0.5);// the robot will move for 0.5 seconds before stopping.
+                                               // If the Move command is called in a loop, the robot will continue moving. 
+                                               // If no Move request is received within 0.5 seconds, the robot will automatically stop.
+        }
+        else if (test_option.id == 6)
+        {
+            res = sport_client.StopMove();
+        }
+        else if (test_option.id == 7)
+        {
+            res = sport_client.SwitchGait(0);
+        }
+        else if (test_option.id == 8)
+        {
+            res = sport_client.SwitchGait(1);
+        }
+        else if (test_option.id == 9)
+        {
+            res = sport_client.RecoveryStand();
+        }
+
+        if (res < 0)
+        {
+            res_count += 1;
+            std::cout << "Request error for: " << option_list[test_option.id].name << ", code: " << res << ", count: " << res_count << std::endl;
+        }
+        else
+        {
+            res_count = 0;
+            std::cout << "Request successed: " << option_list[test_option.id].name << ", code: " << res << std::endl;
+        }
+        std::this_thread::sleep_until(time_start_trick + dt);
+    }
+    return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/b2w/b2w_stand_example.cpp

@@ -0,0 +1,372 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+#include <unitree/robot/b2/sport/sport_client.hpp>
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree::robot::b2;
+
+#define TOPIC_LOWCMD "rt/lowcmd"
+#define TOPIC_LOWSTATE "rt/lowstate"
+
+constexpr double PosStopF = (2.146E+9f);
+constexpr double VelStopF = (16000.0f);
+
+class Custom
+{
+public:
+    explicit Custom(){}
+    ~Custom(){}
+
+    void Init();
+    void Start();
+
+private:
+    void InitLowCmd();
+    void LowStateMessageHandler(const void* messages);
+    void LowCmdWrite();
+    int queryMotionStatus();
+    std::string queryServiceName(std::string form,std::string name);
+ 
+private:
+    float Kp = 1000.0;
+    float Kd = 10.0;
+    double time_consume = 0;
+    int rate_count = 0;
+    int sin_count = 0;
+    int motiontime = 0;
+    float dt = 0.002; // 0.001~0.01
+
+    MotionSwitcherClient msc;
+    SportClient sc;
+
+    unitree_go::msg::dds_::LowCmd_ low_cmd{};      // default init
+    unitree_go::msg::dds_::LowState_ low_state{};  // default init
+
+    /*publisher*/
+    ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
+    /*subscriber*/
+    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
+
+    /*LowCmd write thread*/
+    ThreadPtr lowCmdWriteThreadPtr;
+
+    float _targetPos_1[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                              -0.2, 1.36, -2.65, 0.2, 1.36, -2.65};
+
+    float _targetPos_2[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                              0.0, 0.67, -1.3, 0.0, 0.67, -1.3};
+
+    float _targetPos_3[12] = {-0.65, 1.36, -2.65, 0.65, 1.36, -2.65,
+                              -0.65, 1.36, -2.65, 0.65, 1.36, -2.65};
+
+    float _startPos[12];
+    float _duration_1 = 800;   
+    float _duration_2 = 800; 
+    float _duration_3 = 2000;   
+    float _duration_4 = 1500;   
+    float _percent_1 = 0;    
+    float _percent_2 = 0;    
+    float _percent_3 = 0;    
+    float _percent_4 = 0;    
+
+    bool firstRun = true;
+    bool done = false;
+};
+
+uint32_t crc32_core(uint32_t* ptr, uint32_t len)
+{
+    unsigned int xbit = 0;
+    unsigned int data = 0;
+    unsigned int CRC32 = 0xFFFFFFFF;
+    const unsigned int dwPolynomial = 0x04c11db7;
+
+    for (unsigned int i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (unsigned int bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+            {
+                CRC32 <<= 1;
+            }
+
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+            xbit >>= 1;
+        }
+    }
+
+    return CRC32;
+}
+
+void Custom::Init()
+{
+    InitLowCmd();
+
+    /*create publisher*/
+    lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
+    lowcmd_publisher->InitChannel();
+
+    /*create subscriber*/
+    lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
+    lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1);
+
+    /*init MotionSwitcherClient*/
+    msc.SetTimeout(10.0f); 
+    msc.Init();
+
+    /*Shut down motion control-related service*/
+    while(queryMotionStatus())
+    {
+        std::cout << "Try to deactivate the motion control-related service." << std::endl;
+        int32_t ret = msc.ReleaseMode(); 
+        if (ret == 0) {
+            std::cout << "ReleaseMode succeeded." << std::endl;
+        } else {
+            std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
+        }
+        sleep(5);
+    }
+}
+
+void Custom::InitLowCmd()
+{
+    low_cmd.head()[0] = 0xFE;
+    low_cmd.head()[1] = 0xEF;
+    low_cmd.level_flag() = 0xFF;
+    low_cmd.gpio() = 0;
+
+    for(int i=0; i<20; i++)
+    {
+        low_cmd.motor_cmd()[i].mode() = (0x01);   // motor switch to servo (PMSM) mode
+        low_cmd.motor_cmd()[i].q() = (PosStopF);
+        low_cmd.motor_cmd()[i].kp() = (0);
+        low_cmd.motor_cmd()[i].dq() = (VelStopF);
+        low_cmd.motor_cmd()[i].kd() = (0);
+        low_cmd.motor_cmd()[i].tau() = (0);
+    }
+}
+
+int Custom::queryMotionStatus()
+{
+    std::string robotForm,motionName;
+    int motionStatus;
+    int32_t ret = msc.CheckMode(robotForm,motionName);
+    if (ret == 0) {
+        std::cout << "CheckMode succeeded." << std::endl;
+    } else {
+        std::cout << "CheckMode failed. Error code: " << ret << std::endl;
+    }
+    if(motionName.empty())
+    {
+        std::cout << "The motion control-related service is deactivated." << std::endl;
+        motionStatus = 0;
+    }
+    else
+    {
+        std::string serviceName = queryServiceName(robotForm,motionName);
+        std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
+        motionStatus = 1;
+    }
+    return motionStatus;
+}
+
+std::string Custom::queryServiceName(std::string form,std::string name)
+{
+    if(form == "0")
+    {
+        if(name == "normal" ) return "sport_mode"; 
+        if(name == "ai" ) return "ai_sport"; 
+        if(name == "advanced" ) return "advanced_sport"; 
+    }
+    else
+    {
+        if(name == "ai-w" ) return "wheeled_sport(go2W)"; 
+        if(name == "normal-w" ) return "wheeled_sport(b2W)";
+    }
+    return "";
+}
+
+void Custom::Start()
+{
+    /*loop publishing thread*/
+    lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Custom::LowCmdWrite, this);
+}
+
+void Custom::LowStateMessageHandler(const void* message)
+{
+    low_state = *(unitree_go::msg::dds_::LowState_*)message;
+}
+
+void Custom::LowCmdWrite()
+{
+    if(_percent_4<1)
+    {
+        std::cout<<"Read sensor data example: "<<std::endl;
+        std::cout<<"Joint 0 pos: "<<low_state.motor_state()[0].q()<<std::endl;
+        std::cout<<"Imu accelerometer : "<<"x: "<<low_state.imu_state().accelerometer()[0]<<" y: "<<low_state.imu_state().accelerometer()[1]<<" z: "<<low_state.imu_state().accelerometer()[2]<<std::endl;
+        std::cout<<"Foot force "<<low_state.foot_force()[0]<<std::endl;
+        std::cout<<std::endl;
+    }
+    if((_percent_4 == 1) && ( done == false))
+    {
+        std::cout<<"The example is done! "<<std::endl;
+        std::cout<<std::endl;
+        done = true;
+    }
+
+    motiontime++;
+    if(motiontime>=500)
+    {
+        if(firstRun)
+        {
+            for(int i = 0; i < 12; i++)
+            {
+                _startPos[i] = low_state.motor_state()[i].q();
+            }
+            firstRun = false;
+        }
+
+        _percent_1 += (float)1 / _duration_1;
+        _percent_1 = _percent_1 > 1 ? 1 : _percent_1;
+        if (_percent_1 < 1)
+        {
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_1) * _startPos[j] + _percent_1 * _targetPos_1[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        
+        }
+        if ((_percent_1 == 1)&&(_percent_2 < 1))
+        {
+            _percent_2 += (float)1 / _duration_2;
+            _percent_2 = _percent_2 > 1 ? 1 : _percent_2;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_2) * _targetPos_1[j] + _percent_2 * _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3<1))
+        {
+            _percent_3 += (float)1 / _duration_3;
+            _percent_3 = _percent_3 > 1 ? 1 : _percent_3;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() =  _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+            if(_percent_3<0.4)
+            {
+                for (int j = 12; j < 16; j++)
+                {
+                    low_cmd.motor_cmd()[j].q() =  0;
+                    low_cmd.motor_cmd()[j].kp() = 0;
+                    low_cmd.motor_cmd()[j].dq() = 3;
+
+                    low_cmd.motor_cmd()[j].kd() = Kd;
+                    low_cmd.motor_cmd()[j].tau() = 0;
+                }
+                            
+            }
+            else if((_percent_3>=0.4)&&(_percent_3<0.8))
+            {
+                for (int j = 12; j < 16; j++)
+                {
+                    low_cmd.motor_cmd()[j].q() =  0;
+                    low_cmd.motor_cmd()[j].kp() = 0;
+                    low_cmd.motor_cmd()[j].dq() = -3;
+
+                    low_cmd.motor_cmd()[j].kd() = Kd;
+                    low_cmd.motor_cmd()[j].tau() = 0;
+                }
+                            
+            }
+            else if(_percent_3>=0.8)
+            {
+                for (int j = 12; j < 16; j++)
+                {
+                    low_cmd.motor_cmd()[j].q() =  0;
+                    low_cmd.motor_cmd()[j].kp() = 0;
+                    low_cmd.motor_cmd()[j].dq() = 0;
+
+                    low_cmd.motor_cmd()[j].kd() = Kd;
+                    low_cmd.motor_cmd()[j].tau() = 0;
+                } 
+            }
+            
+        }
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&((_percent_4<=1)))
+        {
+            _percent_4 += (float)1 / _duration_4;
+            _percent_4 = _percent_4 > 1 ? 1 : _percent_4;
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_4) * _targetPos_2[j] + _percent_4 * _targetPos_3[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+        low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
+    
+        lowcmd_publisher->Write(low_cmd);
+    }
+   
+}
+
+int main(int argc, const char** argv)
+{
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1); 
+    }
+
+    std::cout << "WARNING: Make sure the robot is lying on the ground." << std::endl
+            << "Press Enter to continue..." << std::endl;
+    std::cin.ignore();
+
+    ChannelFactory::Instance()->Init(0, argv[1]);
+
+    Custom custom;
+    custom.Init();
+    custom.Start();
+  
+    while (1)
+    {
+        sleep(10);
+    }
+
+    return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/CMakeLists.txt

@@ -0,0 +1,38 @@
+add_executable(g1_loco_client high_level/g1_loco_client_example.cpp)
+target_link_libraries(g1_loco_client unitree_sdk2)
+
+add_executable(g1_arm5_sdk_dds_example high_level/g1_arm5_sdk_dds_example.cpp)
+target_link_libraries(g1_arm5_sdk_dds_example unitree_sdk2)
+
+add_executable(g1_arm7_sdk_dds_example high_level/g1_arm7_sdk_dds_example.cpp)
+target_link_libraries(g1_arm7_sdk_dds_example unitree_sdk2)
+
+add_executable(g1_arm_action_example high_level/g1_arm_action_example.cpp)
+target_link_libraries(g1_arm_action_example unitree_sdk2)
+
+add_executable(g1_ankle_swing_example low_level/g1_ankle_swing_example.cpp)
+target_link_libraries(g1_ankle_swing_example unitree_sdk2)
+
+add_executable(g1_audio_client_example audio/g1_audio_client_example.cpp)
+target_link_libraries(g1_audio_client_example unitree_sdk2)
+
+add_executable(g1_dex3_example dex3/g1_dex3_example.cpp)
+target_link_libraries(g1_dex3_example unitree_sdk2)
+
+find_package(Boost COMPONENTS program_options)
+if(Boost_FOUND)
+    add_executable(g1_termination low_level/terminations.cpp)
+    target_link_libraries(g1_termination unitree_sdk2 Boost::program_options)
+endif()
+
+find_package(yaml-cpp QUIET)
+if(yaml-cpp_FOUND)
+    if (${yaml-cpp_VERSION} VERSION_GREATER_EQUAL "0.6")
+        message(STATUS "Found yaml-cpp version ${yaml-cpp_VERSION}")
+        add_executable(g1_dual_arm_example low_level/g1_dual_arm_example.cpp)
+        target_link_libraries(g1_dual_arm_example PRIVATE unitree_sdk2 yaml-cpp)
+        target_compile_definitions(g1_dual_arm_example PUBLIC BLIB_DIR="${CMAKE_CURRENT_SOURCE_DIR}/low_level/behavior_lib/")
+    else()
+        message(STATUS "yaml-cpp version ${yaml-cpp_VERSION} is too old, skipping build of g1_dual_arm_example.")
+    endif()
+endif()

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/audio/g1_audio_client_example.cpp

@@ -0,0 +1,174 @@
+#include <fstream>
+#include <iostream>
+#include <thread>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/idl/ros2/String_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/g1/audio/g1_audio_client.hpp>
+
+#include "wav.hpp"
+
+#define AUDIO_FILE_PATH "../example/g1/audio/test.wav"
+#define AUDIO_SUBSCRIBE_TOPIC "rt/audio_msg"
+#define GROUP_IP "239.168.123.161"
+#define PORT 5555
+
+#define WAV_SECOND 5  // record seconds
+#define WAV_LEN (16000 * 2 * WAV_SECOND)
+#define CHUNK_SIZE 96000  // 3 seconds
+int sock;
+
+void asr_handler(const void *msg) {
+  std_msgs::msg::dds_::String_ *resMsg = (std_msgs::msg::dds_::String_ *)msg;
+  std::cout << "Topic:\"rt/audio_msg\" recv: " << resMsg->data() << std::endl;
+}
+
+std::string get_local_ip_for_multicast() {
+  struct ifaddrs *ifaddr, *ifa;
+  char host[NI_MAXHOST];
+  std::string result = "";
+
+  getifaddrs(&ifaddr);
+  for (ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) {
+    if (!ifa->ifa_addr || ifa->ifa_addr->sa_family != AF_INET) continue;
+    getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST,
+                NULL, 0, NI_NUMERICHOST);
+    std::string ip(host);
+    if (ip.find("192.168.123.") == 0) {
+      result = ip;
+      break;
+    }
+  }
+  freeifaddrs(ifaddr);
+  return result;
+}
+
+void thread_mic(void) {
+  sock = socket(AF_INET, SOCK_DGRAM, 0);
+  sockaddr_in local_addr{};
+  local_addr.sin_family = AF_INET;
+  local_addr.sin_port = htons(PORT);
+  local_addr.sin_addr.s_addr = INADDR_ANY;
+  bind(sock, (sockaddr *)&local_addr, sizeof(local_addr));
+
+  ip_mreq mreq{};
+  inet_pton(AF_INET, GROUP_IP, &mreq.imr_multiaddr);
+  std::string local_ip = get_local_ip_for_multicast();
+  std::cout << "local ip: " << local_ip << std::endl;
+  mreq.imr_interface.s_addr = inet_addr(local_ip.c_str());
+  setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
+
+  int total_bytes = 0;
+  std::vector<int16_t> pcm_data;
+  pcm_data.reserve(WAV_LEN / 2);
+  std::cout << "start record!" << std::endl;
+  while (total_bytes < WAV_LEN) {
+    char buffer[2048];
+    ssize_t len = recvfrom(sock, buffer, sizeof(buffer), 0, nullptr, nullptr);
+    if (len > 0) {
+      size_t sample_count = len / 2;
+      const int16_t *samples = reinterpret_cast<const int16_t *>(buffer);
+      pcm_data.insert(pcm_data.end(), samples, samples + sample_count);
+      total_bytes += len;
+    }
+  }
+
+  WriteWave("record.wav", 16000, pcm_data.data(), pcm_data.size(), 1);
+  std::cout << "record finish! save to record.wav " << std::endl;
+}
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: audio_client_example [NetWorkInterface(eth0)]"
+              << std::endl;
+    exit(0);
+  }
+  int32_t ret;
+  /*
+   * Initilaize ChannelFactory
+   */
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+  unitree::robot::g1::AudioClient client;
+  client.Init();
+  client.SetTimeout(10.0f);
+
+  /*ASR message Example*/
+  unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_> subscriber(
+      AUDIO_SUBSCRIBE_TOPIC);
+  subscriber.InitChannel(asr_handler);
+
+  /*Volume Example*/
+  uint8_t volume;
+  ret = client.GetVolume(volume);
+  std::cout << "GetVolume API ret:" << ret
+            << "  volume = " << std::to_string(volume) << std::endl;
+  ret = client.SetVolume(100);
+  std::cout << "SetVolume to 100% , API ret:" << ret << std::endl;
+
+  /*TTS Example*/
+  ret = client.TtsMaker("你好。我是宇树科技的机器人。例程启动成功",
+                        0);  // Auto play
+  std::cout << "TtsMaker API ret:" << ret << std::endl;
+  unitree::common::Sleep(5);
+
+  ret = client.TtsMaker(
+      "Hello. I'm a robot from Unitree Robotics. The example has started "
+      "successfully. ",
+      1);  // Engilsh TTS
+  std::cout << "TtsMaker API ret:" << ret << std::endl;
+  unitree::common::Sleep(8);
+
+  /*Audio Play Example*/
+  int32_t sample_rate = -1;
+  int8_t num_channels = 0;
+  bool filestate = false;
+  std::vector<uint8_t> pcm =
+      ReadWave(AUDIO_FILE_PATH, &sample_rate, &num_channels, &filestate);
+
+  std::cout << "wav file sample_rate = " << sample_rate
+            << " num_channels =  " << std::to_string(num_channels)
+            << " filestate =" << filestate << "filesize = " << pcm.size()
+            << std::endl;
+
+  if (filestate && sample_rate == 16000 && num_channels == 1) {
+    size_t total_size = pcm.size();
+    size_t offset = 0;
+    int chunk_index = 0;
+    std::string stream_id =
+        std::to_string(unitree::common::GetCurrentTimeMillisecond());
+
+    while (offset < total_size) {
+      size_t remaining = total_size - offset;
+      size_t current_chunk_size =
+          std::min(static_cast<size_t>(CHUNK_SIZE), remaining);
+      std::vector<uint8_t> chunk(pcm.begin() + offset,
+                                 pcm.begin() + offset + current_chunk_size);
+      client.PlayStream("example", stream_id, chunk);
+      unitree::common::Sleep(1);
+      std::cout << "Playing size: " << offset << std::endl;
+      offset += current_chunk_size;
+    }
+
+    ret = client.PlayStop(stream_id);  // stop playback after transmission ends
+
+  } else {
+    std::cout << "audio file format error, please check!" << std::endl;
+  }
+
+  /*LED Control Example*/
+  client.LedControl(0, 255, 0);
+  unitree::common::Sleep(1);
+  client.LedControl(0, 0, 0);
+  unitree::common::Sleep(1);
+  client.LedControl(0, 0, 255);
+
+  std::cout << "AudioClient api test finish , asr start..." << std::endl;
+
+  std::thread mic_t(thread_mic);
+
+  while (1) {
+    sleep(1);  // wait for asr message
+  }
+  mic_t.join();
+  return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/audio/wav.hpp

@@ -0,0 +1,232 @@
+#ifndef __UT_ROBOT_WAV_READER_HPP__
+#define __UT_ROBOT_WAV_READER_HPP__
+
+struct WaveHeader {
+  void SeekToDataChunk(std::istream &is) {
+    while (is && subchunk2_id != 0x61746164) {
+      is.seekg(subchunk2_size, std::istream::cur);
+      is.read(reinterpret_cast<char *>(&subchunk2_id), sizeof(int32_t));
+      is.read(reinterpret_cast<char *>(&subchunk2_size), sizeof(int32_t));
+    }
+  }
+
+  int32_t chunk_id;
+  int32_t chunk_size;
+  int32_t format;
+  int32_t subchunk1_id;
+  int32_t subchunk1_size;
+  int16_t audio_format;
+  int16_t num_channels;
+  int32_t sample_rate;
+  int32_t byte_rate;
+  int16_t block_align;
+  int16_t bits_per_sample;
+  int32_t subchunk2_id;    // a tag of this chunk
+  int32_t subchunk2_size;  // size of subchunk2
+};
+
+static_assert(sizeof(WaveHeader) == 44);
+
+std::vector<uint8_t> ReadWaveImpl(std::istream &is, int32_t *sampling_rate,
+                                  int8_t *channelCount, bool *is_ok) {
+  WaveHeader header{};
+  is.read(reinterpret_cast<char *>(&header.chunk_id), sizeof(header.chunk_id));
+
+  //                        F F I R
+  if (header.chunk_id != 0x46464952) {
+    printf("Expected chunk_id RIFF. Given: 0x%08x\n", header.chunk_id);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.chunk_size),
+          sizeof(header.chunk_size));
+
+  is.read(reinterpret_cast<char *>(&header.format), sizeof(header.format));
+
+  //                      E V A W
+  if (header.format != 0x45564157) {
+    printf("Expected format WAVE. Given: 0x%08x\n", header.format);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.subchunk1_id),
+          sizeof(header.subchunk1_id));
+
+  is.read(reinterpret_cast<char *>(&header.subchunk1_size),
+          sizeof(header.subchunk1_size));
+
+  if (header.subchunk1_id == 0x4b4e554a) {
+    // skip junk padding
+    is.seekg(header.subchunk1_size, std::istream::cur);
+
+    is.read(reinterpret_cast<char *>(&header.subchunk1_id),
+            sizeof(header.subchunk1_id));
+
+    is.read(reinterpret_cast<char *>(&header.subchunk1_size),
+            sizeof(header.subchunk1_size));
+  }
+
+  if (header.subchunk1_id != 0x20746d66) {
+    printf("Expected subchunk1_id 0x20746d66. Given: 0x%08x\n",
+           header.subchunk1_id);
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.subchunk1_size != 16 &&
+      header.subchunk1_size != 18) {  // 16 for PCM
+    printf("Expected subchunk1_size 16. Given: %d\n", header.subchunk1_size);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.audio_format),
+          sizeof(header.audio_format));
+
+  if (header.audio_format != 1) {  // 1 for PCM
+    printf("Expected audio_format 1. Given: %d\n", header.audio_format);
+    *is_ok = false;
+    return {};
+  }
+
+  is.read(reinterpret_cast<char *>(&header.num_channels),
+          sizeof(header.num_channels));
+
+  *channelCount = (int8_t)header.num_channels;
+
+  is.read(reinterpret_cast<char *>(&header.sample_rate),
+          sizeof(header.sample_rate));
+
+  is.read(reinterpret_cast<char *>(&header.byte_rate),
+          sizeof(header.byte_rate));
+
+  is.read(reinterpret_cast<char *>(&header.block_align),
+          sizeof(header.block_align));
+
+  is.read(reinterpret_cast<char *>(&header.bits_per_sample),
+          sizeof(header.bits_per_sample));
+
+  if (header.byte_rate !=
+      (header.sample_rate * header.num_channels * header.bits_per_sample / 8)) {
+    printf("Incorrect byte rate: %d. Expected: %d", header.byte_rate,
+           (header.sample_rate * header.num_channels * header.bits_per_sample /
+            8));
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.block_align !=
+      (header.num_channels * header.bits_per_sample / 8)) {
+    printf("Incorrect block align: %d. Expected: %d\n", header.block_align,
+           (header.num_channels * header.bits_per_sample / 8));
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.bits_per_sample != 16) {  // we support only 16 bits per sample
+    printf("Expected bits_per_sample 16. Given: %d\n", header.bits_per_sample);
+    *is_ok = false;
+    return {};
+  }
+
+  if (header.subchunk1_size == 18) {
+    int16_t extra_size = -1;
+    is.read(reinterpret_cast<char *>(&extra_size), sizeof(int16_t));
+    if (extra_size != 0) {
+      printf(
+          "Extra size should be 0 for wave from NAudio. Current extra size "
+          "%d\n",
+          extra_size);
+      *is_ok = false;
+      return {};
+    }
+  }
+
+  is.read(reinterpret_cast<char *>(&header.subchunk2_id),
+          sizeof(header.subchunk2_id));
+
+  is.read(reinterpret_cast<char *>(&header.subchunk2_size),
+          sizeof(header.subchunk2_size));
+
+  header.SeekToDataChunk(is);
+  if (!is) {
+    *is_ok = false;
+    return {};
+  }
+
+  *sampling_rate = header.sample_rate;
+
+  // header.subchunk2_size contains the number of bytes in the data.
+  // As we assume each sample contains two bytes, so it is divided by 2 here
+  std::vector<int16_t> samples(header.subchunk2_size / 2);
+
+  is.read(reinterpret_cast<char *>(samples.data()), header.subchunk2_size);
+  if (!is) {
+    *is_ok = false;
+    return {};
+  }
+
+  std::vector<uint8_t> ans(samples.size() * 2);
+  for (int32_t i = 0; i != static_cast<int32_t>(samples.size()); ++i) {
+    ans[i * 2] = samples[i] & 0xFF;
+    ans[i * 2 + 1] = (samples[i] >> 8) & 0xFF;
+  }
+
+  *is_ok = true;
+  return ans;
+}
+
+std::vector<uint8_t> ReadWave(const std::string &filename,
+                              int32_t *sampling_rate, int8_t *channelCount,
+                              bool *is_ok) {
+  std::ifstream is(filename, std::ifstream::binary);
+  auto samples = ReadWaveImpl(is, sampling_rate, channelCount, is_ok);
+  return samples;
+}
+
+bool WriteWave(const std::string &filename, int32_t sampling_rate,
+               const int16_t *samples, int32_t n, uint8_t num_channels) {
+  WaveHeader header{};
+  header.chunk_id = 0x46464952;      // FFIR
+  header.format = 0x45564157;        // EVAW
+  header.subchunk1_id = 0x20746d66;  // "fmt "
+  header.subchunk1_size = 16;        // 16 for PCM
+  header.audio_format = 1;           // PCM =1
+
+  int32_t bits_per_sample = 16;  // int16_t
+  header.num_channels = num_channels;
+  header.sample_rate = sampling_rate;
+  header.byte_rate = sampling_rate * num_channels * bits_per_sample / 8;
+  header.block_align = num_channels * bits_per_sample / 8;
+  header.bits_per_sample = bits_per_sample;
+  header.subchunk2_id = 0x61746164;  // atad
+  header.subchunk2_size = n * num_channels * bits_per_sample / 8;
+
+  header.chunk_size = 36 + header.subchunk2_size;
+
+  std::vector<int16_t> samples_int16(n * num_channels);
+  for (int32_t i = 0; i != n * num_channels; ++i) {
+    samples_int16[i] = samples[i];
+  }
+
+  std::ofstream os(filename, std::ios::binary);
+  if (!os) {
+    printf("Failed to create %s", filename.c_str());
+    return false;
+  }
+
+  os.write(reinterpret_cast<const char *>(&header), sizeof(header));
+  os.write(reinterpret_cast<const char *>(samples_int16.data()),
+           samples_int16.size() * sizeof(int16_t));
+
+  if (!os) {
+    printf("Write %s failed", filename.c_str());
+    return false;
+  }
+
+  return true;
+}
+
+#endif

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/dex3/g1_dex3_example.cpp

@@ -0,0 +1,326 @@
+#include <chrono>
+#include <thread>
+#include <unitree/idl/hg/HandState_.hpp> //replace your sdk path
+#include <unitree/idl/hg/HandCmd_.hpp> //replace your sdk path
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <iostream>
+#include <unistd.h>
+#include <atomic>
+#include <mutex>
+#include <cmath>
+#include <termios.h>
+#include <unistd.h>
+#include <eigen3/Eigen/Dense>
+
+
+enum State {
+    INIT,
+    ROTATE,
+    GRIP,
+    STOP,
+    PRINT
+};
+
+// set URDF Limits
+const float maxLimits_left[7]=  {  1.05 ,  1.05  , 1.75 ,   0   ,  0    , 0     , 0   }; // set max motor value
+const float minLimits_left[7]=  { -1.05 , -0.724 ,   0  , -1.57 , -1.75 , -1.57  ,-1.75}; 
+const float maxLimits_right[7]= {  1.05 , 0.742  ,   0  ,  1.57 , 1.75  , 1.57  , 1.75}; 
+const float minLimits_right[7]= { -1.05 , -1.05  , -1.75,    0  ,  0    ,   0   ,0    }; 
+
+// Initing the dds configuration
+std::string dds_namespace = "rt/dex3/left";
+std::string sub_namespace = "rt/dex3/left/state";
+unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::HandCmd_> handcmd_publisher;
+unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::HandState_> handstate_subscriber;
+unitree_hg::msg::dds_::HandCmd_ msg;
+unitree_hg::msg::dds_::HandState_ state;
+std::atomic<State> currentState(INIT);
+std::mutex stateMutex;
+
+#define MOTOR_MAX 7
+#define SENSOR_MAX 9
+uint8_t hand_id = 0;
+
+typedef struct {
+    uint8_t id     : 4;
+    uint8_t status : 3;
+    uint8_t timeout: 1;
+} RIS_Mode_t;
+
+// stateToString Method
+const char* stateToString(State state) {
+    switch (state) {
+        case INIT: return "INIT";
+        case ROTATE: return "ROTATE";
+        case GRIP: return "GRIP";
+        case STOP: return "STOP";
+        case PRINT: return "PRINT";
+        default: return "UNKNOWN";
+    }
+}
+
+// Monitor user's input
+char getNonBlockingInput() {
+    struct termios oldt, newt;
+    char ch;
+    int oldf;
+
+    tcgetattr(STDIN_FILENO, &oldt); 
+    newt = oldt;
+    newt.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
+    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
+
+    ch = getchar(); 
+
+    tcsetattr(STDIN_FILENO, TCSANOW, &oldt); 
+    fcntl(STDIN_FILENO, F_SETFL, oldf);
+
+    return ch;
+}
+
+void userInputThread() {
+    while (true) {
+        char ch = getNonBlockingInput();
+        if (ch == 'q') {
+            std::cout << "Exiting..." << std::endl;
+                currentState = STOP;
+                break;
+        } else if (ch == 'r') {
+            currentState = ROTATE;
+        } else if (ch == 'g') {
+            currentState = GRIP;
+        } else if (ch == 'p') {
+            currentState = PRINT;
+        } else if (ch == 's') {
+            currentState = STOP;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(100)); 
+    }
+}
+
+// this method can send kp and kd to motors
+void rotateMotors(bool isLeftHand) {
+    static int _count = 1; 
+    static int dir = 1;    
+    const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
+    const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
+
+    for (int i = 0; i < MOTOR_MAX; i++) {
+        RIS_Mode_t ris_mode;
+        ris_mode.id = i;        
+        ris_mode.status = 0x01; 
+   
+        
+        uint8_t mode = 0;
+        mode |= (ris_mode.id & 0x0F);           
+        mode |= (ris_mode.status & 0x07) << 4;    
+        mode |= (ris_mode.timeout & 0x01) << 7;  
+        msg.motor_cmd()[i].mode(mode);
+        msg.motor_cmd()[i].tau(0);
+        msg.motor_cmd()[i].kp(0.5);    
+        msg.motor_cmd()[i].kd(0.1);    
+
+
+        float range = maxLimits[i] - minLimits[i];
+        float mid = (maxLimits[i] + minLimits[i]) / 2.0; 
+        float amplitude = range / 2.0; 
+        float q = mid + amplitude * sin(_count / 20000.0 * M_PI); 
+
+        msg.motor_cmd()[i].q(q);
+    }
+
+    handcmd_publisher->Write(msg);
+    _count += dir;
+
+
+    if (_count >= 10000) {
+        dir = -1;
+    }
+    if (_count <= -10000) {
+        dir = 1;
+    }
+
+    usleep(100); 
+}
+
+// this method can send static position to motors
+void gripHand(bool isLeftHand) {
+
+    const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
+    const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
+
+    for (int i = 0; i < MOTOR_MAX; i++) {
+        RIS_Mode_t ris_mode;
+        ris_mode.id = i;        
+        ris_mode.status = 0x01; 
+    
+        
+        uint8_t mode = 0;
+        mode |= (ris_mode.id & 0x0F);            
+        mode |= (ris_mode.status & 0x07) << 4;    
+        mode |= (ris_mode.timeout & 0x01) << 7;   
+        msg.motor_cmd()[i].mode(mode);
+        msg.motor_cmd()[i].tau(0);
+
+      
+        float mid = (maxLimits[i] + minLimits[i]) / 2.0;
+
+
+        msg.motor_cmd()[i].q(mid); 
+        msg.motor_cmd()[i].dq(0);  
+        msg.motor_cmd()[i].kp(1.5);      
+        msg.motor_cmd()[i].kd(0.1);   
+    }
+
+
+    handcmd_publisher->Write(msg);
+    usleep(1000000);
+}
+
+// this method can send dynamic position to motors
+void stopMotors() {
+    for (int i = 0; i < MOTOR_MAX; i++) {
+        RIS_Mode_t ris_mode;
+        ris_mode.id = i;       
+        ris_mode.status = 0x01; 
+        ris_mode.timeout = 0x01; 
+        
+        uint8_t mode = 0;
+        mode |= (ris_mode.id & 0x0F);            
+        mode |= (ris_mode.status & 0x07) << 4;  
+        mode |= (ris_mode.timeout & 0x01) << 7;   
+        msg.motor_cmd()[i].mode(mode);
+        msg.motor_cmd()[i].tau(0);
+        msg.motor_cmd()[i].dq(0); 
+        msg.motor_cmd()[i].kp(0);
+        msg.motor_cmd()[i].kd(0);
+        msg.motor_cmd()[i].q(0); 
+
+    }
+    handcmd_publisher->Write(msg);
+    usleep(1000000); 
+}
+
+// this method can subscribe dds and show the position for now
+void printState(bool isLeftHand){
+    Eigen::Matrix<float, 7, 1> q;
+
+    const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
+    const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
+    for(int i = 0; i < 7; i++) 
+    {
+        q(i) = state.motor_state()[i].q();
+      
+        q(i) = (q(i) - minLimits[i] ) / (maxLimits[i] - minLimits[i]);
+        q(i) = std::clamp(q(i), 0.0f, 1.0f);
+    }
+    std::cout << "\033[2J\033[H"; 
+    std::cout << "-- Hand State --\n";
+    std::cout << "--- Current State: " << "Test" << " ---\n";
+    std::cout << "Commands:\n";
+    std::cout << "  r - Rotate\n";
+    std::cout << "  g - Grip\n";
+    std::cout << "  t - Test\n";
+    std::cout << "  q - Quit\n";
+    if(isLeftHand){
+        std::cout << " L: " << q.transpose() << std::endl;
+    }else std::cout << " R: " << q.transpose() << std::endl;
+    usleep(0.1 * 1e6);
+
+}
+
+void StateHandler(const void *message) {
+  state = *(unitree_hg::msg::dds_::HandState_ *)message;
+}
+
+
+
+
+int main(int argc, const char** argv)
+{
+    std::cout << " --- Unitree Robotics --- \n";
+    std::cout << "     Dex3 Hand Example      \n\n";
+    std::string input;
+    std::cout << "Please input the hand id (L for left hand, R for right hand): ";
+    std::cin >> input;
+
+    if (input == "L") {
+        hand_id = 0;
+        dds_namespace = "rt/dex3/left";
+        sub_namespace = "rt/lf/dex3/left/state";
+    } else if (input == "R") {
+        hand_id = 1;
+        dds_namespace = "rt/dex3/right";
+        sub_namespace = "rt/lf/dex3/right/state";
+    } else {
+        std::cout << "Invalid hand id. Please input 'L' or 'R'." << std::endl;
+        return -1;
+    }
+
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1); 
+    }
+    unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+    handcmd_publisher.reset(new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::HandCmd_>(dds_namespace + "/cmd"));
+    handstate_subscriber.reset(new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::HandState_>(sub_namespace));
+    handcmd_publisher->InitChannel();
+    handstate_subscriber->InitChannel(
+      std::bind(&StateHandler, std::placeholders::_1), 1);
+    state.motor_state().resize(MOTOR_MAX);
+    state.press_sensor_state().resize(SENSOR_MAX);
+    msg.motor_cmd().resize(MOTOR_MAX);
+    
+    // handcmd_publisher->msg_.motor_cmd().resize(MOTOR_MAX);
+
+   
+    std::thread inputThread(userInputThread);
+    State lastState = INIT; 
+    while (true) {
+        State state;
+        {
+            std::lock_guard<std::mutex> lock(stateMutex);
+            state = currentState.load();
+        }
+                
+        if (state != lastState) {
+            std::cout << "\n--- Current State: " << stateToString(state) << " ---\n";
+            std::cout << "Commands:\n";
+            std::cout << "  r - Rotate\n";
+            std::cout << "  g - Grip\n";
+            std::cout << "  p - Print_state\n";
+            std::cout << "  q - Quit\n";
+            std::cout << "  s - Stop\n";
+            lastState = state; 
+        }
+
+        switch (state) {
+            case INIT:
+                std::cout << "Initializing..." << std::endl;
+                currentState = ROTATE;
+                break;
+            case ROTATE:
+                rotateMotors(input == "L");
+                break;
+            case GRIP:
+                gripHand(input == "L");
+                break;
+            case STOP:
+                stopMotors();
+                break;
+            case PRINT:
+                printState(input == "L");
+                break;
+            default:
+                std::cout << "Invalid state!" << std::endl;
+                inputThread.join();  
+                break;
+        }
+    }
+
+    return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/high_level/g1_arm5_sdk_dds_example.cpp

@@ -0,0 +1,256 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+    // Left leg
+    kLeftHipPitch = 0,
+    kLeftHipRoll = 1,
+    kLeftHipYaw = 2,
+    kLeftKnee = 3,
+    kLeftAnkle  = 4,
+    kLeftAnkleRoll = 5,
+
+    // Right leg
+    kRightHipPitch = 6,
+    kRightHipRoll = 7,
+    kRightHipYaw = 8,
+    kRightKnee = 9,
+    kRightAnkle = 10,
+    kRightAnkleRoll = 11,
+
+    kWaistYaw = 12,
+    kWaistRoll = 13,
+    kWaistPitch = 14,
+
+    // Left arm
+    kLeftShoulderPitch = 15,
+    kLeftShoulderRoll = 16,
+    kLeftShoulderYaw = 17,
+    kLeftElbowPitch = 18,
+    kLeftElbowRoll = 19,
+    
+    // Right arm
+    kRightShoulderPitch = 22,
+    kRightShoulderRoll = 23,
+    kRightShoulderYaw = 24,
+    kRightElbowPitch = 25,
+    kRightElbowRoll = 26,
+
+    kNotUsedJoint = 29,
+    kNotUsedJoint1 = 30,
+    kNotUsedJoint2 = 31,
+    kNotUsedJoint3 = 32,
+    kNotUsedJoint4 = 33,
+    kNotUsedJoint5 = 34
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 13> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbowPitch,
+      JointIndex::kLeftElbowRoll,
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbowPitch,
+      JointIndex::kRightElbowRoll,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 13> init_pos{0, 0, 0, 0, 0,
+                                 0, 0, 0, 0, 0,
+                                 0, 0, 0};
+
+  std::array<float, 13> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 
+                                     0.f, 0.f, 0.f};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+  // get current joint position
+  std::array<float, 13> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // set weight
+	weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+	float phase = 1.0 * i / init_time_steps;
+	std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 13> current_jpos_des{};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // set weight
+  msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
+  // send dds msg
+  arm_sdk_publisher->Write(msg);
+  
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/high_level/g1_arm7_sdk_dds_example.cpp

@@ -0,0 +1,262 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+    // Left leg
+    kLeftHipPitch,
+    kLeftHipRoll,
+    kLeftHipYaw,
+    kLeftKnee,
+    kLeftAnkle,
+    kLeftAnkleRoll,
+
+    // Right leg
+    kRightHipPitch,
+    kRightHipRoll,
+    kRightHipYaw,
+    kRightKnee,
+    kRightAnkle,
+    kRightAnkleRoll,
+
+    kWaistYaw,
+    kWaistRoll,
+    kWaistPitch,
+
+    // Left arm
+    kLeftShoulderPitch,
+    kLeftShoulderRoll,
+    kLeftShoulderYaw,
+    kLeftElbow,
+    kLeftWristRoll,
+    kLeftWristPitch,
+    kLeftWristYaw,
+    // Right arm
+    kRightShoulderPitch,
+    kRightShoulderRoll,
+    kRightShoulderYaw,
+    kRightElbow,
+    kRightWristRoll,
+    kRightWristPitch,
+    kRightWristYaw,
+
+    kNotUsedJoint,
+    kNotUsedJoint1,
+    kNotUsedJoint2,
+    kNotUsedJoint3,
+    kNotUsedJoint4,
+    kNotUsedJoint5
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 17> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbow,
+      JointIndex::kLeftWristRoll,       JointIndex::kLeftWristPitch,
+      JointIndex::kLeftWristYaw,
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbow,
+      JointIndex::kRightWristRoll,      JointIndex::kRightWristPitch,
+      JointIndex::kRightWristYaw,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 17> init_pos{0, 0, 0, 0, 0, 0, 0,
+                                  0, 0, 0, 0, 0, 0, 0,
+                                  0, 0, 0};
+
+  std::array<float, 17> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f, 0.f, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f, 
+                                     0, 0, 0};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+  // get current joint position
+  std::array<float, 17> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // increase weight
+    weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+    float phase = 1.0 * i / init_time_steps;
+    std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 17> current_jpos_des{};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+
+  // set weight
+  msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
+  // send dds msg
+  arm_sdk_publisher->Write(msg);
+
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/high_level/g1_arm_action_example.cpp

@@ -0,0 +1,80 @@
+/**
+ * @file g1_arm_action_example.cpp
+ * @brief This example demonstrates how to use the G1 Arm Action Client to execute predefined arm actions.
+ */
+#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
+#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
+
+using namespace unitree::robot::g1;
+
+int main(int argc, const char** argv) 
+{
+    std::cout << " --- Unitree Robotics --- \n";
+    std::cout << "     G1 Arm Action Example      \n\n";
+
+    // Unitree DDS Initialization; arg[1] is the network interface
+    unitree::robot::ChannelFactory::Instance()->Init(0, argc > 1 ? argv[1] : "");
+
+    auto client = std::make_shared<unitree::robot::g1::G1ArmActionClient>();
+    client->Init();
+    client->SetTimeout(10.f); // All actions will last less than 10 seconds.
+
+    std::cout << "Usage: \n";
+    std::cout << "  - 0: print supported actions.\n";
+    std::cout << "  - an id: execute an action.\n";
+    std::cout << "Attention: \n";
+    std::cout << "  Some actions will not be displayed on the APP, \n"; 
+    std::cout << "  but can be executed by the program.\n";
+    std::cout << "  These actions may cause the robot to fall,\n";
+    std::cout << "  so please execute them with caution.\n";
+
+    int32_t action_id = 0;
+    std::string line;
+    while (true) {
+        std::cout << "\nEnter action ID: .\n";
+        std::getline(std::cin, line);
+        try {
+            action_id = std::stoi(line);
+        } catch (const std::exception&) {
+            std::cout << "Invalid input. Please enter an integer.\n";
+            continue;
+        }
+
+        if (action_id == 0) {
+            std::string action_list_data;
+            int32_t ret = client->GetActionList(action_list_data);
+            if (ret != 0) {
+                std::cerr << "Failed to get action list, error code: " << ret << "\n";
+                continue;
+            }
+            std::cout << "Available actions:\n" << action_list_data << std::endl;
+        } else {
+            int32_t ret = client->ExecuteAction(action_id);
+            if(ret != 0) {
+                switch (ret)
+                {
+                case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
+                    std::cout << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << std::endl;
+                    break;
+                case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
+                    std::cout << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
+                    break;
+                case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
+                    std::cout << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
+                    break;
+                case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
+                    std::cout << "The actions are only supported in fsm id {500, 501, 801}" << std::endl;
+                    std::cout << "You can subscribe the topic rt/sportmodestate to check the fsm id." << std::endl;
+                    std::cout << "And in the state 801, the actions are only supported in the fsm mode {0, 3}." << std::endl;
+                    std::cout << "If an error is still returned at this point, ignore this action.";
+                    break;
+                default:
+                    std::cerr << "Execute action failed, error code: " << ret << std::endl;
+                    break;
+                }
+            }
+        }
+    }
+
+    return 0;
+};

GR00T-WholeBodyControl/gear_sonic_deploy/thirdparty/unitree_sdk2/example/g1/high_level/g1_loco_client_example.cpp

@@ -0,0 +1,267 @@
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/robot/g1/loco/g1_loco_api.hpp>
+#include <unitree/robot/g1/loco/g1_loco_client.hpp>
+
+std::vector<float> stringToFloatVector(const std::string &str) {
+  std::vector<float> result;
+  std::stringstream ss(str);
+  float num;
+  while (ss >> num) {
+    result.push_back(num);
+    // ignore any trailing whitespace
+    ss.ignore();
+  }
+  return result;
+}
+
+int main(int argc, char const *argv[]) {
+  std::map<std::string, std::string> args = {{"network_interface", "lo"}};
+
+  std::map<std::string, std::string> values;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg.substr(0, 2) == "--") {
+      size_t pos = arg.find("=");
+      std::string key, value;
+      if (pos != std::string::npos) {
+        key = arg.substr(2, pos - 2);
+        value = arg.substr(pos + 1);
+
+        if (value.front() == '"' && value.back() == '"') {
+          value = value.substr(1, value.length() - 2);
+        }
+      } else {
+        key = arg.substr(2);
+        value = "";
+      }
+      if (args.find(key) != args.end()) {
+        args[key] = value;
+      } else {
+        args.insert({{key, value}});
+      }
+    }
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0,
+                                                   args["network_interface"]);
+
+  unitree::robot::g1::LocoClient client;
+
+  client.Init();
+  client.SetTimeout(10.f);
+
+  for (const auto &arg_pair : args) {
+    std::cout << "Processing command: [" << arg_pair.first << "] with param: ["
+              << arg_pair.second << "] ..." << std::endl;
+    if (arg_pair.first == "network_interface") {
+      continue;
+    }
+
+    if (arg_pair.first == "get_fsm_id") {
+      int fsm_id;
+      client.GetFsmId(fsm_id);
+      std::cout << "current fsm_id: " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "get_fsm_mode") {
+      int fsm_mode;
+      client.GetFsmMode(fsm_mode);
+      std::cout << "current fsm_mode: " << fsm_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_balance_mode") {
+      int balance_mode;
+      client.GetBalanceMode(balance_mode);
+      std::cout << "current balance_mode: " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_swing_height") {
+      float swing_height;
+      client.GetSwingHeight(swing_height);
+      std::cout << "current swing_height: " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_stand_height") {
+      float stand_height;
+      client.GetStandHeight(stand_height);
+      std::cout << "current stand_height: " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_phase") {
+      std::vector<float> phase;
+      client.GetPhase(phase);
+      std::cout << "current phase: (";
+      for (const auto &p : phase) {
+        std::cout << p << ", ";
+      }
+      std::cout << ")" << std::endl;
+    }
+
+    if (arg_pair.first == "set_fsm_id") {
+      int fsm_id = std::stoi(arg_pair.second);
+      client.SetFsmId(fsm_id);
+      std::cout << "set fsm_id to " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "set_balance_mode") {
+      int balance_mode = std::stoi(arg_pair.second);
+      client.SetBalanceMode(balance_mode);
+      std::cout << "set balance_mode to " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "set_swing_height") {
+      float swing_height = std::stof(arg_pair.second);
+      client.SetSwingHeight(swing_height);
+      std::cout << "set swing_height to " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_stand_height") {
+      float stand_height = std::stof(arg_pair.second);
+      client.SetStandHeight(stand_height);
+      std::cout << "set stand_height to " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_velocity") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega, duration;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = 1.f;
+      } else if (param_size == 4) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = param.at(3);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size
+                  << std::endl;
+        return 1;
+      }
+
+      client.SetVelocity(vx, vy, omega, duration);
+      std::cout << "set velocity to " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "damp") {
+      client.Damp();
+    }
+
+    if (arg_pair.first == "start") {
+      client.Start();
+    }
+
+    if (arg_pair.first == "squat") {
+      client.Squat();
+    }
+
+    if (arg_pair.first == "sit") {
+      client.Sit();
+    }
+
+    if (arg_pair.first == "stand_up") {
+      client.StandUp();
+    }
+
+    if (arg_pair.first == "zero_torque") {
+      client.ZeroTorque();
+    }
+
+    if (arg_pair.first == "stop_move") {
+      client.StopMove();
+    }
+
+    if (arg_pair.first == "high_stand") {
+      client.HighStand();
+    }
+
+    if (arg_pair.first == "low_stand") {
+      client.LowStand();
+    }
+
+    if (arg_pair.first == "balance_stand") {
+      client.BalanceStand();
+    }
+
+    if (arg_pair.first == "continous_gait") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.ContinuousGait(flag);
+    }
+
+    if (arg_pair.first == "switch_move_mode") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.SwitchMoveMode(flag);
+    }
+
+    if (arg_pair.first == "move") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size
+                  << std::endl;
+        return 1;
+      }
+      client.Move(vx, vy, omega);
+    }
+
+    if (arg_pair.first == "set_task_id") {
+      int task_id = std::stoi(arg_pair.second);
+      client.SetTaskId(task_id);
+      std::cout << "set task_id to " << task_id << std::endl;
+    }
+
+    if (arg_pair.first == "shake_hand") {
+      client.ShakeHand(0);
+      std::cout << "Shake hand starts! Waiting for 10 s for ending"
+                << std::endl;
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+      std::cout << "Shake hand ends!" << std::endl;
+      client.ShakeHand(1);
+    }
+
+    if (arg_pair.first == "wave_hand") {
+      client.WaveHand();
+      std::cout << "wave hand" << std::endl;
+    }
+
+    if (arg_pair.first == "wave_hand_with_turn") {
+      client.WaveHand(true);
+      std::cout << "wave hand with turn" << std::endl;
+    }
+
+    if (arg_pair.first == "set_speed_mode") {
+      client.SetSpeedMode(std::stoi(arg_pair.second));
+      std::cout << "set speed mode" << std::endl;
+    }
+
+    std::cout << "Done!" << std::endl;
+  }
+
+  return 0;
+}