xbox_udp链路

.gitignore

@@ -0,0 +1,3 @@
+/TienKung_URDF
+/TienKung-Lab
+/xGMR

Deploy_Tienkung/common/xbox_control.py

@@ -69,7 +69,6 @@ class XBOXController:
         # smoothing
         self.height_step = 0.05
 
-        self._load_config()
         # default button map indices (can be overridden in config)
         self.button_map = {
             'a': 0, 'b': 1, 'x': 2, 'y': 3,
@@ -84,6 +83,8 @@ class XBOXController:
             'dpad_h': 6, 'dpad_v': 7
         }
 
+        self._load_config()
+
     def _load_config(self):
         try:
             config_path = os.path.join('.', 'config', 'dex_config.yaml')
@@ -91,22 +92,24 @@ class XBOXController:
                 cfg = yaml.safe_load(f) or {}
             xbox_cfg = cfg.get('xbox', {})
             # override button_map if provided
-            # bm = xbox_cfg.get('button_map')
+            bm = xbox_cfg.get('button_map')
-            # if isinstance(bm, dict):
+            if isinstance(bm, dict):
-            #     for k, v in bm.items():
+                for k, v in bm.items():
-            #         try:
+                    if k in self.button_map:
-            #             self.button_map[k] = int(v)
+                        try:
-            #         except Exception:
+                            self.button_map[k] = int(v)
-            #             pass
+                        except Exception:
-                        
+                            pass
-            # # override axis_map if provided
+
-            # am = xbox_cfg.get('axis_map')
+            # override axis_map if provided
-            # if isinstance(am, dict):
+            am = xbox_cfg.get('axis_map')
-            #     for k, v in am.items():
+            if isinstance(am, dict):
-            #         try:
+                for k, v in am.items():
-            #             self.axis_map[k] = int(v)
+                    if k in self.axis_map:
-            #         except Exception:
+                        try:
-            #             pass
+                            self.axis_map[k] = int(v)
+                        except Exception:
+                            pass
             
             self.initial_height = xbox_cfg.get('initial_height', 0.89)
             self.forward_command_offset = xbox_cfg.get('forward_command_offset', 0.0)
@@ -155,6 +158,9 @@ class XBOXController:
             # c -> gotoSTOP
             if self.map.y == 1:
                 self.flag.fsm_state_command = 'gotoSTOP'
+            # a -> gotoWALKAMP
+            elif self.map.a == 1:
+                self.flag.fsm_state_command = 'gotoWALKAMP'
             # h -> gotoDH (Left trigger + A)
             # v -> gotoBEYONDMIMIC (Left trigger + home)
             elif self.map.l_trigger < -0.5 and self.map.home == 1:

Deploy_Tienkung/policy/stop/fsm_stop.py

@@ -80,6 +80,8 @@ class FSMStateStop(FSMState):
             return FSMStateName.STOP
         elif flag.fsm_state_command == "gotoZERO":
             return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
         elif flag.fsm_state_command == "gotoMYPOLICY":
             return FSMStateName.MYPOLICY
         elif flag.fsm_state_command == "gotoXSIMRUN":

Deploy_Tienkung/policy/xsim_run/config/xsim_run.yaml

@@ -0,0 +1,82 @@
+model_path: "../mypolicy/model/policy.onnx"
+motor_num: 29
+actions_size: 23
+dt: 0.01
+warm_start_time: 0.0
+command_clip: 1.0
+
+sim:
+  mujoco_timestep: 0.005
+
+joint_names: [
+  hip_pitch_l_joint, hip_pitch_r_joint, waist_yaw_joint,
+  hip_roll_l_joint, hip_roll_r_joint, waist_roll_joint,
+  hip_yaw_l_joint, hip_yaw_r_joint, waist_pitch_joint,
+  knee_pitch_l_joint, knee_pitch_r_joint,
+  shoulder_pitch_l_joint, shoulder_pitch_r_joint,
+  ankle_pitch_l_joint, ankle_pitch_r_joint,
+  shoulder_roll_l_joint, shoulder_roll_r_joint,
+  ankle_roll_l_joint, ankle_roll_r_joint,
+  shoulder_yaw_l_joint, shoulder_yaw_r_joint,
+  elbow_pitch_l_joint, elbow_pitch_r_joint
+]
+
+control:
+  action_scale: 0.25
+  decimation: 2
+
+gait:
+  gait_air_ratio_l: 0.6
+  gait_air_ratio_r: 0.6
+  gait_phase_offset_l: 0.6
+  gait_phase_offset_r: 0.1
+  gait_cycle: 0.64
+
+normalization:
+  clip_scales:
+    clip_observations: 100.0
+    clip_actions: 100.0
+
+size:
+  num_hist: 10
+  observations_size: 84
+
+gains:
+  kp: [
+    300.0, 300.0, 400.0,
+    300.0, 300.0, 400.0,
+    150.0, 150.0, 400.0,
+    350.0, 350.0,
+    150.0, 150.0,
+    30.0, 30.0,
+    50.0, 50.0,
+    16.8, 16.8,
+    50.0, 50.0,
+    150.0, 150.0
+  ]
+  kd: [
+    10.0, 10.0, 5.0,
+    10.0, 10.0, 10.0,
+    5.0, 5.0, 10.0,
+    10.0, 10.0,
+    7.5, 7.5,
+    2.5, 2.5,
+    2.5, 2.5,
+    1.4, 1.4,
+    2.5, 2.5,
+    5.0, 5.0
+  ]
+
+init_state:
+  default_joint_angles: [
+    0.0, 0.0, 0.0,
+    -0.5, -0.5, 0.0,
+    0.0, 0.0, 0.0,
+    1.0, 1.0,
+    0.0, 0.0,
+    -0.5, -0.5,
+    0.2, -0.2,
+    0.0, 0.0,
+    0.0, 0.0,
+    -0.3, -0.3
+  ]

Deploy_Tienkung/policy/xsim_run/fsm_xsim_run.py

@@ -0,0 +1,303 @@
+"""
+FSM state implementation for an xSIM MuJoCo run policy that follows the
+TienKung-Lab sim2sim observation/action flow more closely.
+"""
+
+import os
+
+import numpy as np
+import onnxruntime as ort
+import yaml
+from scipy.spatial.transform import Rotation
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateXSIMRUN(FSMState):
+    """Direct-position run policy for xSIM MuJoCo."""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.XSIMRUN
+        self.log_prefix = "FSMStateXSIMRUN"
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "xsim_run.yaml")
+        with open(config_path, "r", encoding="utf-8") as f:
+            policy_config = yaml.safe_load(f)
+
+        self.action_num_ = int(policy_config["actions_size"])
+        self.motor_num_ = int(policy_config["motor_num"])
+        self.dt_ = float(policy_config["dt"])
+        self.command_clip_ = float(policy_config.get("command_clip", 1.0))
+
+        size_config = policy_config.get("size", {})
+        self.num_hist_ = int(size_config["num_hist"])
+        self.obs_size_ = int(size_config["observations_size"])
+
+        control_config = policy_config.get("control", {})
+        self.action_scale_ = float(control_config["action_scale"])
+        self.decimation_ = int(control_config["decimation"])
+        self.warm_start_time_ = float(
+            control_config.get(
+                "warm_start_time",
+                policy_config.get("warm_start_time", 0.0),
+            )
+        )
+
+        sim_config = policy_config.get("sim", {})
+        self.mujoco_timestep_ = float(sim_config.get("mujoco_timestep", 0.005))
+        self.policy_period_ = self.dt_ * self.decimation_
+
+        gait_config = policy_config.get("gait", {})
+        self.gait_cycle_ = float(gait_config["gait_cycle"])
+        self.phase_ratio_ = np.array(
+            [
+                gait_config["gait_air_ratio_l"],
+                gait_config["gait_air_ratio_r"],
+            ],
+            dtype=np.float32,
+        )
+        self.phase_offset_ = np.array(
+            [
+                gait_config["gait_phase_offset_l"],
+                gait_config["gait_phase_offset_r"],
+            ],
+            dtype=np.float32,
+        )
+
+        norm_config = policy_config.get("normalization", {})
+        clip_config = norm_config.get("clip_scales", {})
+        self.clip_obs_ = float(clip_config.get("clip_observations", 100.0))
+        self.clip_act_ = float(clip_config.get("clip_actions", 100.0))
+
+        self.default_angles_lab_ = np.array(
+            policy_config["init_state"]["default_joint_angles"],
+            dtype=np.float32,
+        )
+        self.stiffness_lab_ = np.array(policy_config["gains"]["kp"], dtype=np.float32)
+        self.damping_lab_ = np.array(policy_config["gains"]["kd"], dtype=np.float32)
+
+        model_rel_path = policy_config["model_path"]
+        self.model_path_ = os.path.normpath(os.path.join(current_dir, model_rel_path))
+        self._init_onnx_session()
+
+        # sim2sim.py uses policy output in Isaac order and then maps to MuJoCo order.
+        self.mujoco_to_policy_idx_ = np.array(
+            [0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 15, 19, 4, 10, 16, 20, 5, 11, 17, 21, 18, 22],
+            dtype=int,
+        )
+        self.policy_to_mujoco_idx_ = np.array(
+            [0, 3, 6, 9, 13, 17, 1, 4, 7, 10, 14, 18, 2, 5, 8, 11, 15, 19, 21, 12, 16, 20, 22],
+            dtype=int,
+        )
+
+        # RobotData stores 29 joints in leg -> waist -> arm order.
+        self.mujoco_control_indices_ = np.array(
+            [
+                0, 1, 2, 3, 4, 5,
+                6, 7, 8, 9, 10, 11,
+                12, 13, 14,
+                15, 16, 17, 18,
+                22, 23, 24, 25,
+            ],
+            dtype=int,
+        )
+
+        self.default_angles_mujoco23_ = self.default_angles_lab_[self.policy_to_mujoco_idx_]
+        self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.obs_history_ = np.zeros_like(self.observations_)
+        self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.current_gait_ = np.zeros(6, dtype=np.float32)
+        self.hold_pose_29_ = np.zeros(self.motor_num_, dtype=np.float32)
+        self._warm_start_pose_29_ = np.zeros(self.motor_num_, dtype=np.float32)
+        self._first_obs = True
+        self._policy_step_counter = 0
+        self.waiting_for_motion_ = True
+        self.motion_threshold_ = 1e-3
+
+        if self.warm_start_time_ > 0 and self.policy_period_ > 0:
+            self._warm_start_steps = max(1, int(self.warm_start_time_ / self.policy_period_))
+        else:
+            self._warm_start_steps = 0
+        self._warmup_inference_counter = 0
+
+        self.kp_29_ = np.zeros(self.motor_num_, dtype=np.float32)
+        self.kd_29_ = np.zeros(self.motor_num_, dtype=np.float32)
+        for lab_idx, mj_idx in enumerate(self.mujoco_control_indices_[self.policy_to_mujoco_idx_]):
+            self.kp_29_[mj_idx] = self.stiffness_lab_[lab_idx]
+            self.kd_29_[mj_idx] = self.damping_lab_[lab_idx]
+
+    def _init_onnx_session(self) -> None:
+        options = ort.SessionOptions()
+        options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+        options.intra_op_num_threads = 1
+        options.inter_op_num_threads = 1
+        self.ort_session_ = ort.InferenceSession(
+            self.model_path_,
+            options,
+            providers=["CPUExecutionProvider"],
+        )
+        print(f"[{self.log_prefix}] ONNX model loaded: {self.model_path_}")
+
+    def on_enter(self):
+        self.observations_.fill(0.0)
+        self.obs_history_.fill(0.0)
+        self.actions_.fill(0.0)
+        self.last_actions_.fill(0.0)
+        self.current_gait_.fill(0.0)
+        self._first_obs = True
+        self._policy_step_counter = 0
+        self._warmup_inference_counter = 0
+        self.waiting_for_motion_ = True
+
+        current_q = self.robot_data_.get_joint_pos().copy()
+        self.hold_pose_29_ = current_q
+        self._warm_start_pose_29_ = current_q
+
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+        self.robot_data_.q_d_[base:base + self.motor_num_] = current_q
+        self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
+        self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
+        self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
+        self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
+        print(f"[{self.log_prefix}] enter")
+
+    def run(self, flag: ControlFlag):
+        walk_cmd = np.clip(
+            np.array(self.robot_data_.get_walk_cmd(), dtype=np.float32),
+            -self.command_clip_,
+            self.command_clip_,
+        )
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+
+        if self.waiting_for_motion_:
+            if np.max(np.abs(walk_cmd)) <= self.motion_threshold_:
+                self.robot_data_.q_d_[base:base + self.motor_num_] = self.hold_pose_29_
+                self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
+                self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
+                self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
+                self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
+                return
+            self.waiting_for_motion_ = False
+            self._warm_start_pose_29_ = self.robot_data_.get_joint_pos().copy()
+            print(f"[{self.log_prefix}] motion command detected: {walk_cmd}")
+
+        if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
+            self.current_gait_ = self._compute_gait_features()
+            self.compute_observation(walk_cmd)
+            self.compute_actions()
+
+            target_mujoco23 = (
+                self.actions_[self.policy_to_mujoco_idx_] * self.action_scale_
+                + self.default_angles_mujoco23_
+            )
+            target_q_29 = self.hold_pose_29_.copy()
+            target_q_29[self.mujoco_control_indices_] = target_mujoco23
+
+            commanded_q_29 = target_q_29
+            if self._warm_start_steps > 0 and self._warmup_inference_counter < self._warm_start_steps:
+                self._warmup_inference_counter += 1
+                blend = self._warmup_inference_counter / float(self._warm_start_steps)
+                commanded_q_29 = (1.0 - blend) * self._warm_start_pose_29_ + blend * target_q_29
+
+            self.robot_data_.q_d_[base:base + self.motor_num_] = commanded_q_29
+            self.robot_data_.q_dot_d_[base:base + self.motor_num_] = 0.0
+            self.robot_data_.tau_d_[base:base + self.motor_num_] = 0.0
+            self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_29_
+            self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_29_
+            self.last_actions_[:] = self.actions_
+
+    def _compute_gait_features(self) -> np.ndarray:
+        t = self._policy_step_counter * self.policy_period_ / self.gait_cycle_
+        gait_phase = (t + self.phase_offset_) % 1.0
+        self._policy_step_counter += 1
+        return np.concatenate(
+            [
+                np.sin(2.0 * np.pi * gait_phase),
+                np.cos(2.0 * np.pi * gait_phase),
+                self.phase_ratio_,
+            ],
+            axis=0,
+        ).astype(np.float32)
+
+    def compute_observation(self, walk_cmd: np.ndarray):
+        if np.linalg.norm(self.robot_data_.imu_quat_) > 0.0:
+            q_wxyz = self.robot_data_.imu_quat_.astype(np.float32)
+            q_xyzw = np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]], dtype=np.float32)
+        else:
+            roll = float(self.robot_data_.imu_data_[2])
+            pitch = float(self.robot_data_.imu_data_[1])
+            yaw = float(self.robot_data_.imu_data_[0])
+            q_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
+            q_xyzw = np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]], dtype=np.float32)
+
+        gravity = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.0, 0.0, -1.0], dtype=np.float32))
+        q_29 = self.robot_data_.get_joint_pos()
+        dq_29 = self.robot_data_.get_joint_vel()
+        q_23 = q_29[self.mujoco_control_indices_]
+        dq_23 = dq_29[self.mujoco_control_indices_]
+
+        proprio = np.concatenate(
+            [
+                self.robot_data_.get_angular_velocity(),
+                gravity,
+                walk_cmd,
+                (q_23 - self.default_angles_mujoco23_)[self.mujoco_to_policy_idx_],
+                dq_23[self.mujoco_to_policy_idx_],
+                np.clip(self.last_actions_, -self.clip_act_, self.clip_act_),
+                self.current_gait_,
+            ],
+            axis=0,
+        ).astype(np.float32)
+
+        if self._first_obs:
+            for i in range(self.num_hist_):
+                start = i * self.obs_size_
+                self.obs_history_[start:start + self.obs_size_] = proprio
+            self._first_obs = False
+        else:
+            self.obs_history_ = np.roll(self.obs_history_, -self.obs_size_)
+            self.obs_history_[-self.obs_size_:] = proprio
+
+        self.observations_ = np.clip(self.obs_history_, -self.clip_obs_, self.clip_obs_)
+
+    def compute_actions(self):
+        input_name = self.ort_session_.get_inputs()[0].name
+        input_data = self.observations_.reshape(1, -1).astype(np.float32)
+        outputs = self.ort_session_.run(None, {input_name: input_data})
+        self.actions_[:] = np.clip(outputs[0][0][: self.action_num_], -self.clip_act_, self.clip_act_)
+
+    def on_exit(self):
+        print(f"[{self.log_prefix}] exit")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        if flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        if flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        if flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        if flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        return None
+
+    @staticmethod
+    def euler_to_quaternion_scipy(roll, pitch, yaw, degrees=False):
+        r = Rotation.from_euler("xyz", [roll, pitch, yaw], degrees=degrees)
+        q_xyzw = r.as_quat()
+        return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]], dtype=np.float32)
+
+    @staticmethod
+    def quat_rotate_inverse_numpy(q_xyzw, v):
+        q_w = q_xyzw[3]
+        q_v = q_xyzw[:3]
+        a = v * (2.0 * q_w * q_w - 1.0)
+        b = np.cross(q_v, v) * (2.0 * q_w)
+        c = q_v * (2.0 * np.dot(q_v, v))
+        return a - b + c

Deploy_Tienkung/udp_loopback/README.md

@@ -5,6 +5,7 @@
 包含：
 
 - `udp_keyboard_sender.py`：从终端读取按键，编码 UDP 报文并发送
+- `udp_xbox_sender.py`：订阅 `/xbox_data`，把 Xbox 摇杆/按键转成 UDP 报文
 - `udp_loopback_node.py`：接收 UDP 报文，解码事件并计算目标值
 - `protocol.py`：自定义协议和状态结构
 - `config/udp_loopback.yaml`：本地测试配置
@@ -40,6 +41,56 @@ cd /home/meiqi/tienkung/Deploy_Tienkung
 python3 udp_loopback/udp_keyboard_sender.py
 ```
 
+如果改成 Xbox 经 UDP 转发，则启动方式是：
+
+1. 把 [dex_config.yaml](/home/meiqi/tienkung/Deploy_Tienkung/config/dex_config.yaml) 里的 `control_tool` 改成 `udp_loopback`
+2. 启动控制节点：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=10
+python3 rl_control_node_sim.py
+```
+
+3. 启动 MuJoCo：
+
+```bash
+cd /home/meiqi/tienkung/xSIM_MUJOCO
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=10
+python3 scripts/simulator_view_asyn.py -m evt2
+```
+
+4. 启动手柄节点：
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=10
+ros2 run joy joy_node --ros-args -r joy:=/xbox_data
+```
+
+5. 启动 UDP Xbox 转发：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=10
+python3 udp_loopback/udp_xbox_sender.py
+```
+
+默认按键映射：
+
+- `A -> mode_stride -> gotoWALKAMP`
+- `X -> pose_home -> gotoZERO`
+- `Y -> pose_hold -> gotoSTOP`
+- `B -> mode_dash -> gotoMYPOLICY`
+- `START -> trim_reset`
+- 左摇杆 Y -> 连续前后速度
+- 左摇杆 X -> 连续横移速度
+- 右摇杆 X -> 连续转向速度
+- 十字键左右 -> 高度增减
+
 此时 UDP 接收结果会在接收侧被映射回现有 FSM 命令：
 
 - `pose_home -> gotoZERO`
@@ -60,6 +111,10 @@ python3 udp_loopback/udp_keyboard_sender.py
 - `sway_right`
 - `spin_left`
 - `spin_right`
+- `set_surge`
+- `set_sway`
+- `set_spin`
+- `set_lift`
 - `lift_up`
 - `lift_down`
 - `trim_reset`

Deploy_Tienkung/udp_loopback/config/udp_loopback.yaml

@@ -5,6 +5,14 @@ sender:
   target_port: 31000
   source_tag: local_keys
 
+xbox_sender:
+  joy_topic: /xbox_data
+  source_tag: xbox_udp
+  deadzone: 0.10
+  analog_epsilon: 0.01
+  dpad_threshold: 0.50
+  trigger_pressed_threshold: -0.50
+
 receiver:
   listen_host: 127.0.0.1
   listen_port: 31000

Deploy_Tienkung/udp_loopback/protocol.py

@@ -10,7 +10,7 @@ from typing import Any, Dict
 
 @dataclass
 class InputEnvelope:
-    """Small UDP payload carrying one encoded keyboard event."""
+    """Small UDP payload carrying one encoded input event."""
 
     seq_id: int
     event_code: str

Deploy_Tienkung/udp_loopback/udp_fsm_controller.py

@@ -177,6 +177,22 @@ class UDPFSMController:
                 self.motion_frame.spin_goal = min(
                     self.max_spin, self.motion_frame.spin_goal + self.spin_step
                 )
+            elif event_code == "set_surge":
+                self.motion_frame.surge_goal = max(
+                    -self.max_surge, min(self.max_surge, packet.drive_value)
+                )
+            elif event_code == "set_sway":
+                self.motion_frame.sway_goal = max(
+                    -self.max_sway, min(self.max_sway, packet.drive_value)
+                )
+            elif event_code == "set_spin":
+                self.motion_frame.spin_goal = max(
+                    -self.max_spin, min(self.max_spin, packet.drive_value)
+                )
+            elif event_code == "set_lift":
+                self.motion_frame.lift_goal = max(
+                    self.min_lift, min(self.max_lift, packet.drive_value)
+                )
             elif event_code == "lift_up":
                 self.motion_frame.lift_goal = min(
                     self.max_lift, self.motion_frame.lift_goal + self.lift_step

Deploy_Tienkung/udp_loopback/udp_xbox_sender.py

@@ -0,0 +1,253 @@
+"""ROS2 Joy -> localhost UDP bridge for Xbox control."""
+
+from __future__ import annotations
+
+import socket
+from pathlib import Path
+from typing import Dict
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import Joy
+import yaml
+
+try:
+    from .protocol import InputEnvelope
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope
+
+
+class UDPXboxSender(Node):
+    """Subscribe to Joy messages and forward them as UDP loopback events."""
+
+    def __init__(self) -> None:
+        super().__init__("udp_xbox_sender")
+        self.config: Dict[str, object] = {}
+        self.seq_id = 0
+        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.last_buttons: Dict[str, int] = {}
+        self.last_dpad_h = 0.0
+
+        self._load_config()
+
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+        self.subscription = self.create_subscription(
+            Joy, self.joy_topic, self._joy_callback, qos_profile
+        )
+
+        self.get_logger().info(
+            f"Forwarding {self.joy_topic} -> udp://{self.target_host}:{self.target_port}"
+        )
+        self.get_logger().info(
+            "Buttons: A=WALKAMP X=ZERO Y=STOP B=MYPOLICY START=reset"
+        )
+
+    def destroy_node(self) -> bool:
+        if self.socket is not None:
+            self.socket.close()
+        return super().destroy_node()
+
+    def _load_config(self) -> None:
+        udp_config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        main_config_path = Path(__file__).resolve().parents[1] / "config" / "dex_config.yaml"
+
+        with udp_config_path.open("r", encoding="utf-8") as file:
+            udp_config = yaml.safe_load(file) or {}
+        with main_config_path.open("r", encoding="utf-8") as file:
+            main_config = yaml.safe_load(file) or {}
+
+        sender_cfg = udp_config.get("sender", {})
+        xbox_sender_cfg = udp_config.get("xbox_sender", {})
+        xbox_cfg = main_config.get("xbox", {})
+
+        self.target_host = sender_cfg.get("target_host", "127.0.0.1")
+        self.target_port = int(sender_cfg.get("target_port", 31000))
+        self.source_tag = xbox_sender_cfg.get("source_tag", "xbox_udp")
+        self.joy_topic = xbox_sender_cfg.get("joy_topic", "/xbox_data")
+
+        self.deadzone = float(xbox_sender_cfg.get("deadzone", 0.10))
+        self.analog_epsilon = float(xbox_sender_cfg.get("analog_epsilon", 0.01))
+        self.dpad_threshold = float(xbox_sender_cfg.get("dpad_threshold", 0.50))
+        self.trigger_pressed_threshold = float(
+            xbox_sender_cfg.get("trigger_pressed_threshold", -0.50)
+        )
+
+        self.forward_command_offset = float(
+            xbox_cfg.get("forward_command_offset", 0.0)
+        )
+        self.lateral_command_offset = float(
+            xbox_cfg.get("lateral_command_offset", 0.0)
+        )
+        self.rotation_command_offset = float(
+            xbox_cfg.get("rotation_command_offset", 0.0)
+        )
+
+        self.button_map = {
+            "a": 0,
+            "b": 1,
+            "x": 2,
+            "y": 3,
+            "lb": 4,
+            "rb": 5,
+            "select": 6,
+            "start": 7,
+            "home": 8,
+        }
+        self.axis_map = {
+            "lx": 0,
+            "ly": 1,
+            "l_trigger": 2,
+            "rx": 3,
+            "ry": 4,
+            "r_trigger": 5,
+            "dpad_h": 6,
+            "dpad_v": 7,
+        }
+
+        self._merge_mapping(self.button_map, xbox_cfg.get("button_map"))
+        self._merge_mapping(self.axis_map, xbox_cfg.get("axis_map"))
+        self._merge_mapping(self.button_map, xbox_sender_cfg.get("button_map"))
+        self._merge_mapping(self.axis_map, xbox_sender_cfg.get("axis_map"))
+
+    def _merge_mapping(self, target: Dict[str, int], override: object) -> None:
+        if not isinstance(override, dict):
+            return
+        for name, index in override.items():
+            if name in target:
+                try:
+                    target[name] = int(index)
+                except (TypeError, ValueError):
+                    pass
+
+    def _joy_callback(self, msg: Joy) -> None:
+        axes = list(msg.axes) + [0.0] * 16
+        buttons = list(msg.buttons) + [0] * 32
+
+        state = {
+            "a": self._button_value(buttons, "a"),
+            "b": self._button_value(buttons, "b"),
+            "x": self._button_value(buttons, "x"),
+            "y": self._button_value(buttons, "y"),
+            "start": self._button_value(buttons, "start"),
+            "home": self._button_value(buttons, "home"),
+            "lx": self._axis_value(axes, "lx"),
+            "ly": self._axis_value(axes, "ly"),
+            "rx": self._axis_value(axes, "rx"),
+            "l_trigger": self._axis_value(axes, "l_trigger"),
+            "dpad_h": self._axis_value(axes, "dpad_h"),
+        }
+
+        self._send_mode_events(state)
+        self._send_trim_event(state)
+        self._send_lift_events(state)
+        self._send_analog_events(state)
+
+        self.last_buttons = {
+            name: int(state[name]) for name in ("a", "b", "x", "y", "start", "home")
+        }
+        self.last_dpad_h = float(state["dpad_h"])
+
+    def _button_value(self, buttons: list[int], name: str) -> int:
+        index = self.button_map[name]
+        return int(buttons[index]) if index < len(buttons) else 0
+
+    def _axis_value(self, axes: list[float], name: str) -> float:
+        index = self.axis_map[name]
+        return float(axes[index]) if index < len(axes) else 0.0
+
+    def _send_mode_events(self, state: Dict[str, float]) -> None:
+        if self._rising_edge(state, "y"):
+            self._send_event("pose_hold", "y")
+        elif self._rising_edge(state, "x"):
+            self._send_event("pose_home", "x")
+        elif self._rising_edge(state, "a"):
+            self._send_event("mode_stride", "a")
+        elif self._rising_edge(state, "b"):
+            self._send_event("mode_dash", "b")
+        elif (
+            self._rising_edge(state, "home")
+            and state["l_trigger"] < self.trigger_pressed_threshold
+        ):
+            self._send_event("mode_xrun", "home")
+
+    def _send_trim_event(self, state: Dict[str, float]) -> None:
+        if self._rising_edge(state, "start"):
+            self._send_event("trim_reset", "start")
+
+    def _send_lift_events(self, state: Dict[str, float]) -> None:
+        dpad_h = float(state["dpad_h"])
+        if dpad_h <= -self.dpad_threshold and self.last_dpad_h > -self.dpad_threshold:
+            self._send_event("lift_up", "dpad_left")
+        elif dpad_h >= self.dpad_threshold and self.last_dpad_h < self.dpad_threshold:
+            self._send_event("lift_down", "dpad_right")
+
+    def _send_analog_events(self, state: Dict[str, float]) -> None:
+        surge = self._compute_surge(state["ly"])
+        sway = self._cleanup_command(
+            self._apply_deadzone(state["lx"]) * -0.4 + self.lateral_command_offset
+        )
+        spin = self._cleanup_command(
+            self._apply_deadzone(state["rx"]) * -0.4 + self.rotation_command_offset
+        )
+
+        self._send_event("set_surge", "left_stick_y", surge)
+        self._send_event("set_sway", "left_stick_x", sway)
+        self._send_event("set_spin", "right_stick_x", spin)
+
+    def _compute_surge(self, ly: float) -> float:
+        ly = self._apply_deadzone(ly)
+        if ly >= 0.0:
+            value = ly * 0.8 + self.forward_command_offset
+        else:
+            value = ly * 0.5
+        return self._cleanup_command(value)
+
+    def _apply_deadzone(self, value: float) -> float:
+        if abs(value) < self.deadzone:
+            return 0.0
+        return float(value)
+
+    def _cleanup_command(self, value: float) -> float:
+        if abs(value) < self.analog_epsilon:
+            return 0.0
+        return float(value)
+
+    def _rising_edge(self, state: Dict[str, float], name: str) -> bool:
+        previous = int(self.last_buttons.get(name, 0))
+        return int(state[name]) == 1 and previous == 0
+
+    def _send_event(
+        self, event_code: str, key_name: str, drive_value: float = 1.0
+    ) -> None:
+        envelope = InputEnvelope(
+            seq_id=self.seq_id,
+            event_code=event_code,
+            key_name=key_name,
+            drive_value=drive_value,
+            source_tag=self.source_tag,
+        )
+        self.seq_id += 1
+        self.socket.sendto(
+            envelope.encode(), (self.target_host, self.target_port)
+        )
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node = UDPXboxSender()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()