First Commit

.vscode/settings.json

@@ -0,0 +1,4 @@
+{
+    "python-envs.defaultEnvManager": "ms-python.python:conda",
+    "python-envs.defaultPackageManager": "ms-python.python:conda"
+}

Deploy_Tienkung/.gitignore

@@ -0,0 +1,8 @@
+*.pyc
+.vscode
+.venv
+__pycache__/
+.history
+bag/
+*build/
+*egg-info/

Deploy_Tienkung/FSM/fsm_base.py

@@ -0,0 +1,61 @@
+"""
+Finite State Machine (FSM) Module
+Python equivalent of the C++ FSM system
+"""
+from abc import ABC, abstractmethod
+from enum import Enum
+
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateName(Enum):
+    """FSM状态枚举"""
+    STOP = 0      # 停止状态
+    ZERO = 1      # 零位状态
+    WALKAMP = 2   # WALKAMP策略状态
+    MYPOLICY = 3  # 自定义策略状态
+    XSIMRUN = 4   # 更贴近sim2sim的xSIM run状态
+
+class FSMState(ABC):
+    """FSM状态抽象基类"""
+
+    def __init__(self, robot_data: RobotData):
+        self.robot_data_ = robot_data
+
+    @abstractmethod
+    def on_enter(self):
+        """进入状态时的行为"""
+        pass
+
+    @abstractmethod
+    def run(self, flag: ControlFlag):
+        """运行状态的正常行为"""
+        pass
+
+    @abstractmethod
+    def on_exit(self):
+        """退出状态时的行为"""
+        pass
+
+    @abstractmethod
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        pass
+
+class RobotFSM(ABC):
+    """机器人FSM抽象基类"""
+
+    def __init__(self, robot_data: RobotData):
+        self.robot_data_ = robot_data
+        # self.disable_joints_ = False
+
+    @abstractmethod
+    def run_fsm(self, flag: ControlFlag):
+        """运行FSM"""
+        pass
+
+    @abstractmethod
+    def get_current_state(self) -> FSMStateName:
+        """获取当前FSM状态"""
+        pass

Deploy_Tienkung/FSM/robot_fsm.py

@@ -0,0 +1,90 @@
+"""
+FSM Implementation
+Complete FSM implementation with state management
+"""
+from typing import Dict
+from .fsm_base import RobotFSM, FSMStateName
+from policy.walk_amp.fsm_walkamp import FSMStateWALKAMP
+from policy.mypolicy.fsm_mypolicy import FSMStateMYPOLICY
+from policy.xsim_run.fsm_xsim_run import FSMStateXSIMRUN
+from policy.zero.fsm_zero import FSMStateZero
+from policy.stop.fsm_stop import FSMStateStop
+from policy.beyond_mimic.fsm_beyond_mimic import FSMStateBeyondMimic
+from policy.beyondzero.fsm_beyondzero import FSMStateBeyondZero
+from common.robot_data import RobotData
+from common.joystick import ControlFlag
+import functools
+import time
+
+def timing_decorator(func):
+    """
+    装饰器：记录函数执行时间
+    """
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+        start_time = time.perf_counter()
+        result = func(*args, **kwargs)
+        end_time = time.perf_counter()
+        execution_time = end_time - start_time
+        print(f"[TIMING] {func.__name__} executed in {execution_time:.6f} seconds")
+        return result
+    return wrapper
+
+class RobotFSMImpl(RobotFSM):
+    """机器人FSM具体实现"""
+
+    def __init__(self, robot_data: RobotData, config: Dict):
+        super().__init__(robot_data)
+        self.config = config
+
+        # 当前状态
+        self.current_state = FSMStateName.STOP
+        self.state_objects = {}
+
+        # 初始化所有状态对象
+        self._init_states()
+
+        # 进入初始状态
+        self.state_objects[self.current_state].on_enter()
+
+    def _init_states(self):
+        """初始化所有状态对象"""
+        self.state_objects[FSMStateName.STOP] = FSMStateStop(self.robot_data_)
+        self.state_objects[FSMStateName.ZERO] = FSMStateZero(self.robot_data_)
+        self.state_objects[FSMStateName.WALKAMP] = FSMStateWALKAMP(self.robot_data_)
+        self.state_objects[FSMStateName.MYPOLICY] = FSMStateMYPOLICY(self.robot_data_)
+        self.state_objects[FSMStateName.XSIMRUN] = FSMStateXSIMRUN(self.robot_data_)
+
+        # TODO: 添加其他状态对象
+    @timing_decorator
+    def run_fsm(self, flag: ControlFlag):
+        """运行FSM"""
+        # 检查状态转换
+        current_state_obj = self.state_objects[self.current_state]
+        next_state = current_state_obj.check_transition(flag)
+
+        # 如果需要状态转换
+        if next_state is not None and next_state != self.current_state:
+            if next_state in self.state_objects:
+                print(f"FSM transition: {self.current_state.name} -> {next_state.name}")
+
+                # 退出当前状态
+                current_state_obj.on_exit()
+
+                # 切换到新状态
+                self.current_state = next_state
+                self.state_objects[self.current_state].on_enter()
+            else:
+                print(f"Warning: State {next_state.name} not implemented")
+
+        # 运行当前状态
+        self.state_objects[self.current_state].run(flag)
+
+    def get_current_state(self) -> FSMStateName:
+        """获取当前FSM状态"""
+        return self.current_state
+
+
+def get_robot_fsm(robot_data: RobotData, config: Dict) -> RobotFSM:
+    """工厂函数，返回机器人FSM实例"""
+    return RobotFSMImpl(robot_data, config)

Deploy_Tienkung/LICENSE.txt

@@ -0,0 +1,39 @@
+BSD 3-Clause License
+
+Copyright (c) 2025, Open X-Humanoid
+
+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.
+
+
+Some of Deploy_Tienkung code is derived from broccoli, which is subject to the following copyright notice:
+
+Copyright 2021 Chair of Applied Mechanics, Technical University of Munich, Germany
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Deploy_Tienkung/README.md

@@ -0,0 +1,227 @@
+# xMIGCS
+
+xMIGCS是一个用于机器人控制的软件系统，专注于通过有限状态机（FSM）和策略模块实现对机器人的灵活控制。项目服务于运动智能领域的研究与开发，支持多种控制模式和外部输入方式（如键盘、手柄等）。
+
+## 功能特性
+
+- **状态机管理**: 基于 FSM 模块实现机器人行为的状态流转控制
+- **多策略支持**: 提供多种控制策略
+- **人机交互控制**: 支持键盘、手柄等外设进行机器人实时操控
+- **配置驱动**: 使用 YAML 文件进行参数配置，支持不同场景下的快速部署
+- **模块化设计**: 各策略独立封装，便于扩展与维护
+
+## 安装与运行
+
+### 环境要求
+
+- Git (用于版本控制)
+- bodyctrl_msgs
+
+### 安装步骤
+
+```bash
+cd your_project_folder
+unzip xmigcs.zip
+cd xmigcs/
+pip install -r requirements.txt
+pip install lib/sptlib_python-0.1.0-cp312-cp312-linux_x86_64.whl
+# 仅仿真中需要安装
+sudo dpkg i lib/ros-jazzy-bodyctrl-msgs_0.0.0-0noble_amd64.deb 
+```
+
+### 运行项目
+
+```bash
+# body节点, 只有真机才需要，仿真启动xsim_mujoco就行
+# 真机上启动body_control
+sudo su
+source /home/ubuntu/xos/setup.bash
+ros2 launch body_control body_control.launch.py
+# 启动手柄
+sudo su
+source /home/ubuntu/xos/setup.bash
+ros2 launch joystick joystick.launch.py
+
+# 启动主控制节点
+# 实机
+source /home/ubuntu/xos/setup.bash
+cd xmigcs
+python3 rl_control_node.py
+# 仿真
+# 设置domain_id防止局域网络与其他机器人冲突
+export ROS_DOMAIN_ID=YOUR_DOMAIN_ID
+source /opt/ros/jazzy/setup.bash
+cd xmigcs
+python3 rl_control_node_sim.py
+```
+
+## 控制器使用说明
+
+### XBOX手柄键位映射
+```bash
+# 仿真中启动xbox手柄
+export ROS_DOMAIN_ID=YOUR_DOMAIN_ID
+source /opt/ros/jazzy/setup.bash
+ros2 run joy joy_node --ros-args --remap joy:=xbox_data
+```
+xMIGCS支持标准XBOX手柄控制，以下是详细键位映射关系：
+
+#### 状态映射关系
+
+##### 单按钮状态切换
+
+| 按钮 | 对应状态 | 功能说明 |
+|------|----------|----------|
+| X | gotoZERO | 回到零位状态 |
+| Y | gotoSTOP | 停止状态 |
+
+
+
+### 云卓手柄键位映射
+
+xMIGCS支持标准云卓手柄控制，开始使用前先确保所有键都回中，以下是详细键位映射关系：
+
+#### 状态映射关系
+
+##### 单按钮状态切换
+
+| 按钮 | 对应状态 | 功能说明 |
+|------|----------|----------|
+| C | gotoSTOP | 停止状态 |
+
+##### 组合按钮状态切换
+
+| 切入策略按钮组合| 策略内使用按键 | 对应状态 | 功能说明 |
+|------------|----------------|--------------|-------------|
+|   所有键(拨中)   |         D      | gotoZERO     | 回到零位状态  |
+|   所有键(拨中)   |         A      | gotoWALKAMP      | WALKAMP策略状态  |
+|   E(上拨)   |         A      | gotoBEYONDMIMIC   | BEYONDMIMIC策略状态   |
+|   E(上拨)   |         D      | gotoBEYONDZERO   | BEYONDMIMIC零位状态   |
+|   F(上拨)   |         无      | 手柄控制失能，只有停止键可用      | |
+
+##### 基础运动控制
+
+| 控制方式 | 功能 |
+|----------|------|
+| 左摇杆Y1轴 | 前后移动控制（正向为前进） |
+| 左摇杆X1轴 | 左右移动控制 |
+| 右摇杆X2轴 | 机身旋转控制 |
+
+## 项目结构
+
+```
+.
+├── FSM                 # 有限状态机模块
+├── common              # 通用功能模块
+├── config              # 配置文件
+├── policy              # 控制策略模块
+├── test                # 测试文件
+└── rl_control_node.py  # 真机控制节点
+└── rl_control_node_sim.py # 仿真控制节点
+```
+
+## 如何添加新的控制策略
+1. 在 policy 目录下创建新的策略文件夹，例如 my_new_policy
+
+2. 在新文件夹中创建以下文件：
+   - fsm_mypolicy.py - 实现具体的FSM状态类
+   - config/mypolicy.yaml - 策略配置文件（可选）
+3. 在 fsm_mypolicy.py 中实现 FSMState 类：
+```python
+    from FSM.fsm_base import FSMState, FSMStateName, ControlFlag
+    from common.robot_data import RobotData
+
+    class FSMStateMyPolicy(FSMState):
+        def __init__(self, robot_data: RobotData):
+            super().__init__(robot_data)
+            # 初始化策略特定变量
+        
+        def on_enter(self):
+            # 进入状态时的初始化操作
+            pass
+        
+        def run(self, flag: ControlFlag):
+            # 策略的主要运行逻辑
+            pass
+        
+        def on_exit(self):
+            # 退出状态时的清理操作
+            pass
+        
+        def check_transition(self, flag: ControlFlag) -> FSMStateName:
+            # 检查是否需要转换到其他状态
+            pass
+```
+1. 在 FSM/robot_fsm.py 中注册新状态：
+   - 导入新策略类
+   - 在 _init_states() 方法中初始化状态对象
+   - 在 FSMStateName 枚举中添加新状态
+2. 控制器设置：云卓12手柄(默认)、键盘(需自定义实现)、XBOX手柄(自定义实现)
+   - 以云卓12手柄为例，需要在common/joystick.py中添加对应的按键映射
+  ```python
+      def joy_flag_update(self):
+        """根据手柄输入更新控制标志"""
+        with self.data_mutex:
+            # 更新手柄启动标志
+            if self.joy_map.f == -1.0:
+                self.joy_flag.enable = False
+            else:
+                self.joy_flag.enable = True
+            # FSM状态切换命令
+            if self.joy_map.c == 1.0:
+                self.joy_flag.fsm_state_command = "gotoSTOP"
+            else:
+                button_pressed_nums = self.check_button_pressed_nums(
+                    self.joy_map)
+                if button_pressed_nums == 0:
+                    if self.joy_map.d == 1.0:
+                        self.joy_flag.fsm_state_command = "gotoZERO"
+                    elif self.joy_map.a == 1.0:
+                        self.joy_flag.fsm_state_command = "gotoWALKAMP"
+                    # 获取walk速度命令
+                    self.get_x_y_yaw_speed_command()
+                    # 获取高度命令
+                    self.get_walk_height_command()
+                if button_pressed_nums == 1:
+                    if self.joy_map.e == -1.0:
+                        #e上拨
+                        if self.joy_map.a == 1.0:
+                            self.joy_flag.fsm_state_command = "gotoBEYONDMIMIC"
+                        elif self.joy_map.d == 1.0:
+                            self.joy_flag.fsm_state_command = "gotoBEYONDZERO"
+  ```
+3. robot_interface.py 中添加新策略的控制映射
+  ```python
+    def _load_control_status(self, config: Dict[str, Any]):
+        # 字符串命令到枚举值的映射
+        state_to_FSMState = {
+            "STOP": FSMStateName.STOP,
+            "ZERO": FSMStateName.ZERO,
+            "WALKAMP": FSMStateName.WALKAMP,
+            "BEYONDMIMIC": FSMStateName.BEYONDMIMIC,
+            "BEYONDZERO": FSMStateName.BEYONDZERO,
+            "MYPOLICY": FSMStateName.MYPOLICY,
+        }
+  ```
+4. 更新配置文件dex_config.yaml 以支持新策略的相关参数
+   ```yaml
+    control_tool: joystick # joystick, xbox, keyboard
+    waist_control_status: ["ZERO", "STOP", "BEYONDMIMIC", "BEYONDZERO", "WALKAMP"] # 
+    legs_control_status: [] #空代表都允许控制，仅腿部是这个逻辑
+    arms_control_status: ["ZERO", "STOP",  "BEYONDMIMIC", "BEYONDZERO", "WALKAMP"] #
+   ```
+## 开发与贡献
+
+欢迎对项目进行贡献，开发前请确保：
+
+1. 遵循项目代码规范
+2. 添加适当的测试用例
+3. 提交前运行所有测试确保无误
+
+## 许可证
+
+本项目仅供内部使用。
+
+## 项目状态
+
+项目正在积极开发中。

Deploy_Tienkung/__init__.py

@@ -0,0 +1,7 @@
+"""
+xMIGCS Package
+Humanoid robot control system package
+"""
+
+# 确保__init__.py是空的或包含适当的包初始化代码
+# 这里可以导入需要在包级别可用的模块

Deploy_Tienkung/common/BasicFunction.py

@@ -0,0 +1,178 @@
+import numpy as np
+from dataclasses import dataclass, field
+
+
+def rot_x(x: float) -> np.ndarray:
+    c, s = np.cos(x), np.sin(x)
+    return np.array([
+        [1.0, 0.0, 0.0],
+        [0.0, c,   -s ],
+        [0.0, s,    c ],
+    ], dtype=float)
+
+
+def rot_y(y: float) -> np.ndarray:
+    c, s = np.cos(y), np.sin(y)
+    return np.array([
+        [ c, 0.0, s],
+        [0.0, 1.0, 0.0],
+        [-s, 0.0, c],
+    ], dtype=float)
+
+
+def rot_z(z: float) -> np.ndarray:
+    c, s = np.cos(z), np.sin(z)
+    return np.array([
+        [c, -s, 0.0],
+        [s,  c, 0.0],
+        [0.0, 0.0, 1.0],
+    ], dtype=float)
+
+
+def euler_xyz_to_matrix(euler_a: np.ndarray) -> np.ndarray:
+    # 对应 C++: RotX * RotY * RotZ
+    return rot_x(euler_a[0]) @ rot_y(euler_a[1]) @ rot_z(euler_a[2])
+
+
+def clip_vector(v: np.ndarray, lb: float, ub: float) -> np.ndarray:
+    # 返回裁剪后的新向量（如果你希望原地修改，可用 out=v）
+    return np.clip(v, lb, ub)
+
+
+def clip_scalar(a: float, lb: float, ub: float) -> float:
+    return float(min(max(a, lb), ub))
+
+
+def gait_phase(timer: float,
+               gait_cycle: float,
+               left_theta_offset: float,
+               right_theta_offset: float,
+               left_phase_ratio: float,
+               right_phase_ratio: float) -> np.ndarray:
+    res = np.zeros(6, dtype=float)
+
+    left_phase = (timer / gait_cycle + left_theta_offset) - np.floor(timer / gait_cycle + left_theta_offset)
+    right_phase = (timer / gait_cycle + right_theta_offset) - np.floor(timer / gait_cycle + right_theta_offset)
+
+    res[0] = np.sin(2.0 * np.pi * left_phase)
+    res[1] = np.sin(2.0 * np.pi * right_phase)
+    res[2] = np.cos(2.0 * np.pi * left_phase)
+    res[3] = np.cos(2.0 * np.pi * right_phase)
+    res[4] = left_phase_ratio
+    res[5] = right_phase_ratio
+    return res
+
+
+def fifth_poly(p0: np.ndarray, p0_dot: np.ndarray, p0_dotdot: np.ndarray,
+               p1: np.ndarray, p1_dot: np.ndarray, p1_dotdot: np.ndarray,
+               total_time: float, current_time: float):
+    """
+    返回: pd, pd_dot, pd_dotdot
+    """
+    p0 = np.asarray(p0, dtype=float)
+    p0_dot = np.asarray(p0_dot, dtype=float)
+    p0_dotdot = np.asarray(p0_dotdot, dtype=float)
+    p1 = np.asarray(p1, dtype=float)
+    p1_dot = np.asarray(p1_dot, dtype=float)
+    p1_dotdot = np.asarray(p1_dotdot, dtype=float)
+
+    n = p0.shape[0]
+    pd = np.zeros(n, dtype=float)
+    pd_dot = np.zeros(n, dtype=float)
+    pd_dotdot = np.zeros(n, dtype=float)
+
+    t = current_time
+    time = total_time
+
+    if t < total_time:
+        A = np.array([
+            [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0 / 2.0, 0.0, 0.0, 0.0],
+            [-10.0 / time**3, -6.0 / time**2, -3.0 / (2.0 * time), 10.0 / time**3, -4.0 / time**2, 1.0 / (2.0 * time)],
+            [15.0 / time**4, 8.0 / time**3, 3.0 / (2.0 * time**2), -15.0 / time**4, 7.0 / time**3, -1.0 / time**2],
+            [-6.0 / time**5, -3.0 / time**4, -1.0 / (2.0 * time**3), 6.0 / time**5, -3.0 / time**4, 1.0 / (2.0 * time**3)],
+        ], dtype=float)
+
+        for i in range(n):
+            x0 = np.array([
+                p0[i], p0_dot[i], p0_dotdot[i],
+                p1[i], p1_dot[i], p1_dotdot[i]
+            ], dtype=float)
+            a = A @ x0
+
+            pd[i] = a[0] + a[1] * t + a[2] * t**2 + a[3] * t**3 + a[4] * t**4 + a[5] * t**5
+            pd_dot[i] = a[1] + 2.0 * a[2] * t + 3.0 * a[3] * t**2 + 4.0 * a[4] * t**3 + 5.0 * a[5] * t**4
+            pd_dotdot[i] = 2.0 * a[2] + 6.0 * a[3] * t + 12.0 * a[4] * t**2 + 20.0 * a[5] * t**3
+    else:
+        pd = p1.copy()
+        pd_dot = p1_dot.copy()
+        pd_dotdot = p1_dotdot.copy()
+
+    return pd, pd_dot, pd_dotdot
+
+
+@dataclass
+class LowPassFilter:
+    cut_off_freq: float
+    damp_ratio: float
+    d_time: float
+    n_filter: int
+
+    dT: float = field(init=False)
+    sigIn_1: np.ndarray = field(init=False)
+    sigIn_2: np.ndarray = field(init=False)
+    sigOut_1: np.ndarray = field(init=False)
+    sigOut_2: np.ndarray = field(init=False)
+
+    a2: float = field(init=False)
+    a1: float = field(init=False)
+    a0: float = field(init=False)
+    b2: float = field(init=False)
+    b1: float = field(init=False)
+    b0: float = field(init=False)
+
+    def __post_init__(self):
+        self.dT = self.d_time
+        self.sigIn_1 = np.zeros(self.n_filter, dtype=float)
+        self.sigIn_2 = np.zeros(self.n_filter, dtype=float)
+        self.sigOut_1 = np.zeros(self.n_filter, dtype=float)
+        self.sigOut_2 = np.zeros(self.n_filter, dtype=float)
+
+        freq_in_rad = 2.0 * np.pi * self.cut_off_freq
+        c = 2.0 / self.dT
+        sqr_c = c * c
+        sqr_w = freq_in_rad * freq_in_rad
+
+        self.b2 = sqr_c + 2.0 * self.damp_ratio * freq_in_rad * c + sqr_w
+        self.b1 = -2.0 * (sqr_c - sqr_w)
+        self.b0 = sqr_c - 2.0 * self.damp_ratio * freq_in_rad * c + sqr_w
+
+        self.a2 = sqr_w
+        self.a1 = 2.0 * sqr_w
+        self.a0 = sqr_w
+
+        self.a2 /= self.b2
+        self.a1 /= self.b2
+        self.a0 /= self.b2
+
+        self.b1 /= self.b2
+        self.b0 /= self.b2
+        self.b2 = 1.0
+
+    def m_filter(self, sig_in: np.ndarray) -> np.ndarray:
+        sig_in = np.asarray(sig_in, dtype=float)
+
+        sig_out = (
+            self.a2 * sig_in
+            + self.a1 * self.sigIn_1
+            + self.a0 * self.sigIn_2
+            - self.b1 * self.sigOut_1
+            - self.b0 * self.sigOut_2
+        )
+
+        self.sigIn_2 = self.sigIn_1
+        self.sigIn_1 = sig_in
+        self.sigOut_2 = self.sigOut_1
+        self.sigOut_1 = sig_out
+        return sig_out

Deploy_Tienkung/common/body_id_map.py

@@ -0,0 +1,74 @@
+"""
+Body ID Mapping
+Python equivalent of the C++ bodyIdMap functionality
+"""
+from typing import Dict, List
+
+
+class BodyServoIdMap:
+    """身体伺服电机ID映射"""
+
+    def __init__(self):
+        self.id_to_index_map: Dict[int, int] = {}
+        self.index_to_id_map: Dict[int, int] = {}
+        self.name_to_index_map: Dict[str, int] = {}
+        self.index_to_name_map: Dict[int, str] = {}
+        self.leg_motor_nums = 0
+        self.waist_motor_nums = 0
+        self.arm_motor_nums = 0
+        self.whole_motor_nums = 0
+
+    def body_can_id_map_init(self):
+        """初始化身体CAN ID映射"""
+        # 腿部关节映射 (0-11)
+        leg_ids = [51, 52, 53, 54, 55, 56,  # 左腿
+                   61, 62, 63, 64, 65, 66]  # 右腿
+        leg_names = [
+            "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
+            "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"
+        ]
+        self.leg_motor_nums = len(leg_ids)
+ 
+        # 腰部关节映射 (12-14)
+        waist_ids = [33, 32, 31]
+        waist_names = ["waist_yaw",  "waist_roll", "waist_pitch" ]
+        self.waist_motor_nums = len(waist_ids)
+
+        # 手臂关节映射 (15-28)
+        arm_ids = [11, 12, 13, 14, 15, 16, 17,  # 左臂
+                   21, 22, 23, 24, 25, 26, 27]  # 右臂
+        arm_names = [
+            "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
+            "l_wrist_yaw", "l_wrist_pitch", "l_wrist_roll",
+            "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
+            "r_wrist_yaw", "r_wrist_pitch", "r_wrist_roll"
+        ]
+        self.arm_motor_nums = len(arm_ids)
+
+        # 合并所有映射
+        all_ids = leg_ids + waist_ids + arm_ids
+        all_names = leg_names + waist_names + arm_names
+        self.whole_motor_nums = len(all_ids)
+
+        # 创建双向映射
+        for index, (can_id, name) in enumerate(zip(all_ids, all_names)):
+            self.id_to_index_map[can_id] = index
+            self.index_to_id_map[index] = can_id
+            self.name_to_index_map[name] = index
+            self.index_to_name_map[index] = name
+
+    def get_index_by_id(self, can_id: int) -> int:
+        """根据CAN ID获取索引"""
+        return self.id_to_index_map.get(can_id, -1)
+
+    def get_id_by_index(self, index: int) -> int:
+        """根据索引获取CAN ID"""
+        return self.index_to_id_map.get(index, -1)
+
+    def get_index_by_name(self, name: str) -> int:
+        """根据关节名称获取索引"""
+        return self.name_to_index_map.get(name, -1)
+
+    def get_name_by_index(self, index: int) -> str:
+        """根据索引获取关节名称"""
+        return self.index_to_name_map.get(index, "")

Deploy_Tienkung/common/joystick.py

@@ -0,0 +1,236 @@
+"""
+Joystick Control Module
+Python equivalent of the C++ Joystick functionality for ROS Joy messages
+"""
+import os
+import yaml
+import threading
+from dataclasses import dataclass
+from typing import Optional
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Joy
+import numpy as np
+
+@dataclass
+class ControlFlag:
+    """手柄控制标志"""
+    fsm_state_command: str = "gotoZERO"
+    # 禁用、启用手柄控制标志
+    enable: bool = True
+
+@dataclass
+class YUNZHUOMap:
+    """云卓T12手柄按键映射 (对应ROS Joy消息)"""
+    a: float = 0.0   # axes[8] #a,b,c,d手柄轴初始值为-1
+    b: float = 0.0   # axes[9]
+    c: float = 0.0   # axes[10]
+    d: float = 0.0   # axes[11]
+    e: float = 0.0   # axes[4]  e,f,g,h手柄轴初始值为0.0
+    f: float = 0.0   # axes[7]
+    g: float = 0.0   # axes[5]
+    h: float = 0.0   # axes[6]
+    x1: float = 0.0  # axes[3]
+    x2: float = 0.0  # axes[0]
+    y1: float = 0.0  # axes[2]
+    y2: float = 0.0  # axes[1]
+
+
+class YUNZHUOFlag(ControlFlag):  # 继承ControlFlag
+    def __init__(self):
+        super().__init__()  # 调用父类初始化
+        # walk command
+        self.x_speed_command: float = 0.0
+        self.y_speed_command: float = 0.0
+        self.yaw_speed_command: float = 0.0
+        self.walk_height_command: float = 0.0
+        # floating base command
+        self.waist_roll_command: float = 0.0
+        self.waist_pitch_command: float = 0.0
+        self.waist_yaw_command: float = 0.0
+        self.waist_height_command: float = 0.0
+
+
+class JoystickHumanoid:
+    """人形机器人手柄控制器 (ROS Joy版本)"""
+
+    def __init__(self):
+        print("Joystick Start")
+
+        # 初始化成员变量
+        self.joy_map = YUNZHUOMap()
+        self.joy_flag = YUNZHUOFlag()
+        self.data_mutex = threading.Lock()
+
+        # 配置参数
+        self.initial_height = 0.0
+        self.current_height = 0.0
+        self.max_height = 0.0
+        self.min_height = 0.0
+        self.x_command_offset = 0.0
+        self.y_command_offset = 0.0
+        self.yaw_command_offset = 0.0
+        self.max_x_plus_speed = 0.0
+        self.max_x_minus_speed = 0.0
+        self.max_y_speed = 0.0
+        self.max_yaw_speed = 0.0
+        # 高度平滑控制
+        self.target_height = 0.0
+
+        # 加载配置文件
+        self._load_config()
+
+    def _load_config(self):
+        """加载YAML配置文件"""
+        config_path = os.path.join('.', "config", "dex_config.yaml")
+
+        with open(config_path, 'r') as file:
+            config = yaml.safe_load(file)
+
+        if not config:
+            print("[Joystick_humanoid] Failed to load config file")
+            return
+
+        joystick_cfg = config.get("joystick", {})
+
+        # 加载配置参数
+        self.initial_height = joystick_cfg.get("initial_height")
+        self.x_command_offset = joystick_cfg.get("x_command_offset")
+        self.y_command_offset = joystick_cfg.get("y_command_offset")
+        self.yaw_command_offset = joystick_cfg.get("yaw_command_offset")
+        self.max_x_plus_speed = joystick_cfg.get("max_x_plus_speed")
+        self.max_x_minus_speed = joystick_cfg.get("max_x_minus_speed")
+        self.max_y_speed = joystick_cfg.get("max_y_speed")
+        self.max_yaw_speed = joystick_cfg.get("max_yaw_speed")
+        self.max_height = joystick_cfg.get("max_height")
+        self.min_height = joystick_cfg.get("min_height")
+
+        print(f"Loaded initial_height: {self.initial_height}, "
+                f"x_command_offset: {self.x_command_offset}, "
+                f"y_command_offset: {self.y_command_offset}, "
+                f"yaw_command_offset: {self.yaw_command_offset},"
+                f"max_x_plus_speed: {self.max_x_plus_speed}, "
+                f"max_x_minus_speed: {self.max_x_minus_speed}, "
+                f"max_y_speed: {self.max_y_speed}, "
+                f"max_yaw_speed: {self.max_yaw_speed}, "
+                f"max_height: {self.max_height},"
+                f"min_height: {self.min_height}")
+
+        self.current_height = self.initial_height
+        self.target_height = self.initial_height
+        self.joy_flag.waist_height_command = self.current_height
+        self.joy_flag.walk_height_command = self.current_height
+
+
+    def joy_map_read(self, msg: Joy):
+        """处理ROS Joy消息，更新手柄映射"""
+        with self.data_mutex:
+            if len(msg.axes) >= 12:  # 确保有足够的轴数据
+                yunzhuo_map = YUNZHUOMap(
+                    a=msg.axes[8] if len(msg.axes) > 8 else 0.0,
+                    b=msg.axes[9] if len(msg.axes) > 9 else 0.0,
+                    c=msg.axes[10] if len(msg.axes) > 10 else 0.0,
+                    d=msg.axes[11] if len(msg.axes) > 11 else 0.0,
+                    e=msg.axes[4] if len(msg.axes) > 4 else 0.0,
+                    f=msg.axes[7] if len(msg.axes) > 7 else 0.0,
+                    g=msg.axes[5] if len(msg.axes) > 5 else 0.0,
+                    h=msg.axes[6] if len(msg.axes) > 6 else 0.0,
+                    x1=msg.axes[3] if len(msg.axes) > 3 else 0.0,
+                    x2=msg.axes[0] if len(msg.axes) > 1 else 0.0,
+                    y1=msg.axes[2] if len(msg.axes) > 2 else 0.0,
+                    y2=msg.axes[1] if len(msg.axes) > 0 else 0.0)
+                self.joy_map = yunzhuo_map
+
+    def joy_flag_update(self):
+        """根据手柄输入更新控制标志"""
+        with self.data_mutex:
+            # 更新手柄启动标志
+            if self.joy_map.f == -1.0:
+                self.joy_flag.enable = False
+            else:
+                self.joy_flag.enable = True
+            # FSM状态切换命令
+            if self.joy_map.c == 1.0:
+                self.joy_flag.fsm_state_command = "gotoSTOP"
+            else:
+                button_pressed_nums = self.check_button_pressed_nums(
+                    self.joy_map)
+                if button_pressed_nums == 0:
+                    if self.joy_map.d == 1.0:
+                        self.joy_flag.fsm_state_command = "gotoZERO"
+                    elif self.joy_map.a == 1.0:
+                        self.joy_flag.fsm_state_command = "gotoWALKAMP"
+                    # 获取walk速度命令
+                    self.get_x_y_yaw_speed_command()
+                    # 获取高度命令
+                    self.get_walk_height_command()
+                if button_pressed_nums == 1:
+                    if self.joy_map.e == -1.0:
+                        #e上拨
+                        if self.joy_map.a == 1.0:
+                            self.joy_flag.fsm_state_command = "gotoBEYONDMIMIC"
+                        elif self.joy_map.d == 1.0:
+                            self.joy_flag.fsm_state_command = "gotoBEYONDZERO"
+
+
+
+    def get_joy_flag(self) -> ControlFlag:
+        """获取当前手柄标志"""
+        with self.data_mutex:
+            return self.joy_flag
+
+    def init(self) -> int:
+        """初始化手柄控制器"""
+        print("Joystick controller initialized")
+        return 0
+
+    def check_button_pressed_nums(self, joy_map: YUNZHUOMap) -> int:
+        """检查按下的按钮数量"""
+        count = 0
+        if joy_map.e != 0.0:
+            count += 1
+        if joy_map.f != 0.0:
+            count += 1
+        if joy_map.g != 0.0:
+            count += 1
+        if joy_map.h != 0.0:
+            count += 1
+        return count
+
+    def get_x_y_yaw_speed_command(self):
+        """获取当前速度命令"""
+        # 速度命令计算
+        self.joy_flag.y_speed_command = (self.joy_map.x1 * -self.max_y_speed +
+                                         self.y_command_offset)
+
+        # X速度 (前进/后退)
+        if self.joy_map.y1 >= 0:
+            self.joy_flag.x_speed_command = (
+                self.joy_map.y1 * self.max_x_plus_speed + self.x_command_offset
+            )  # 前进快一点
+        else:
+            self.joy_flag.x_speed_command = self.joy_map.y1 * self.max_x_minus_speed  # 后退慢一点
+
+        # 偏航速度
+        self.joy_flag.yaw_speed_command = (
+            self.joy_map.x2 * -self.max_yaw_speed + self.yaw_command_offset)
+
+    def get_walk_height_command(self):
+        """获取当前高度命令"""
+        current_height_command = self.joy_flag.walk_height_command
+        deadzone_height = 0.5
+        # 高度命令计算
+        if self.joy_map.x2 >= deadzone_height:
+            # x2 下拨
+            self.joy_flag.walk_height_command += -self.joy_map.x2 * (
+                self.joy_flag.walk_height_command - self.min_height)
+        if self.joy_map.x2 <= -deadzone_height:
+            # x2 上拨
+            self.joy_flag.walk_height_command += -self.joy_map.x2 * (
+                self.max_height - self.joy_flag.walk_height_command)
+
+        # 1s中高度变化3cm, step= 0.03 / 100 hz = 0.0003
+        step = 0.03 / 100
+        self.joy_flag.walk_height_command = np.clip(
+            self.joy_flag.walk_height_command, current_height_command - step,
+            current_height_command + step)

Deploy_Tienkung/common/peekqueue.py

@@ -0,0 +1,11 @@
+import queue
+
+class PeekableQueue(queue.Queue):
+    def peek(self):
+        """取出再放回"""
+        item = self.get_nowait()
+        try:
+            self.put_nowait(item)
+        except queue.Full:
+            pass
+        return item

Deploy_Tienkung/common/robot_data.py

@@ -0,0 +1,163 @@
+"""
+Robot Data Structure
+Python equivalent of the C++ RobotData class
+"""
+import numpy as np
+from scipy.spatial.transform import Rotation
+from common.joystick import ControlFlag
+import copy
+
+class RobotData:
+    """机器人状态数据结构"""
+
+    def __init__(self, motor_num: int = 29, whole_joint_num: int = 35):
+        self.motor_num = motor_num
+        self.whole_joint_num = whole_joint_num
+
+        # Joint states (actual)
+        self.q_a_ = np.zeros(whole_joint_num)  # Joint positions
+        self.q_dot_a_ = np.zeros(whole_joint_num)  # Joint velocities
+        self.tau_a_ = np.zeros(whole_joint_num)  # Joint torques
+        self.temperature_a = np.zeros(motor_num)  # Joint temperatures
+        self.q_a_last = np.zeros(whole_joint_num) # 上一时刻关节位置
+        self.qdot_a_last = np.zeros(whole_joint_num) # 上一时刻关节速度
+        self.tor_a_last = np.zeros(whole_joint_num) # 上一时刻关节力矩
+
+        # Joint commands (desired)
+        self.q_d_ = np.zeros(whole_joint_num)  # Desired joint positions
+        self.q_d_s_ = np.zeros(whole_joint_num) # Desired serial joint positions 
+        self.q_dot_d_ = np.zeros(whole_joint_num)  # Desired joint velocities
+        self.tau_d_ = np.zeros(whole_joint_num)  # Desired joint torques
+
+        # Control gains
+        self.joint_kp_p_ = np.zeros(motor_num)  # Proportional gains
+        self.joint_kd_p_ = np.zeros(motor_num)  # Derivative gains
+
+        # IMU data: [yaw, pitch, roll, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z]
+        self.imu_data_ = np.zeros(13)
+        self.imu_quat_ = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)  # w, x, y, z
+
+        # Timing
+        self.time_now_ = 0.0
+
+        # Configuration
+        self.config_file_ = ""
+
+        # control cmd
+        # walk command
+        self.walk_cmd_ = np.zeros(3) # x_speed, y_speed, yaw_speed
+        # 控制符
+        self.control_flag = ControlFlag()
+
+
+    def copy_from(self, other: 'RobotData'):
+        """从另一个RobotData对象复制数据"""
+        self.q_a_[:] = other.q_a_[:]
+        self.q_dot_a_[:] = other.q_dot_a_[:]
+        self.tau_a_[:] = other.tau_a_[:]
+        self.q_d_[:] = other.q_d_[:]
+        self.q_dot_d_[:] = other.q_dot_d_[:]
+        self.tau_d_[:] = other.tau_d_[:]
+        self.joint_kp_p_[:] = other.joint_kp_p_[:]
+        self.joint_kd_p_[:] = other.joint_kd_p_[:]
+        self.imu_data_[:] = other.imu_data_[:]
+        self.imu_quat_[:] = other.imu_quat_[:]
+        self.time_now_ = other.time_now_
+        self.config_file_ = other.config_file_
+        self.walk_cmd_[:] = other.walk_cmd_[:]
+        self.control_flag = copy.deepcopy(other.control_flag)
+
+
+    def get_joint_pos(self) -> np.ndarray:
+        joint_start_idx = 35 - self.motor_num
+        joint_pos = self.q_a_[joint_start_idx:].astype(np.float32) 
+        return joint_pos
+    
+    def get_serial_joint_pos_desired(self) -> np.ndarray:
+        joint_start_idx = 35 - self.motor_num
+        joint_pos_desired = self.q_d_s_[joint_start_idx:].astype(np.float32)
+        return joint_pos_desired
+
+    def get_joint_vel(self)-> np.ndarray:
+        joint_start_idx = 35 - self.motor_num
+        joint_vel = self.q_dot_a_[joint_start_idx:].astype(np.float32)
+        return joint_vel
+
+    def get_angular_velocity(self) -> np.ndarray:
+        omega_xyz = np.array([
+            self.imu_data_[3],
+            self.imu_data_[4],
+            self.imu_data_[5]
+        ], dtype=np.float32)
+        return omega_xyz
+    
+    def get_robot_quat(self):
+        if np.linalg.norm(self.imu_quat_) > 0.0:
+            return self.imu_quat_.astype(np.float32)
+        rpy = np.array([
+            self.imu_data_[2],  # roll
+            self.imu_data_[1],  # pitch
+            self.imu_data_[0]   # yaw
+        ], dtype=np.float32) * 1.0    
+        robot_quat_wxyz = self.euler_to_quaternion_scipy(rpy[0], rpy[1], rpy[2])
+        return robot_quat_wxyz
+
+    def euler_to_quaternion_scipy(self, roll, pitch, yaw, degrees=False):
+        """
+        使用SciPy进行欧拉角转四元数
+        参数:
+            roll: 绕x轴的旋转角度
+            pitch: 绕y轴的旋转角度  
+            yaw: 绕z轴的旋转角度
+            degrees: 输入角度是否为度，默认为弧度
+        返回:
+            [w, x, y, z]: 四元数分量 (w为实部)
+        """
+        # 创建旋转对象 (顺序: 'xyz' 对应 roll, pitch, yaw)
+        rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=degrees)
+        
+        # 转换为四元数 (顺序: [x, y, z, w])
+        quaternion = rotation.as_quat()
+    
+        return [quaternion[3], quaternion[0], quaternion[1], quaternion[2]]  # 返回 w, x, y, z
+    
+    def get_waist_yrp(self) -> np.ndarray:
+        joint_pos  = self.get_joint_pos()
+        waist_yaw, waist_roll, waist_pitch = joint_pos[12], joint_pos[13], joint_pos[14]
+        return np.array([waist_yaw, waist_roll, waist_pitch], dtype=np.float32)
+    
+    def get_base_linear_acceleration(self) -> np.ndarray:
+        lin_acc = np.array([
+            self.imu_data_[6],
+            self.imu_data_[7],
+            self.imu_data_[8]
+        ], dtype=np.float32)
+        return lin_acc
+    
+    def get_project_gravity(self) -> np.ndarray:
+        """根据机器人姿态重力投影(待完善)
+
+        Args:
+            None
+        """        
+        robot_quat_wxyz = self.get_robot_quat()
+        robot_quat_xyzw = np.array([robot_quat_wxyz[1], robot_quat_wxyz[2], robot_quat_wxyz[3], robot_quat_wxyz[0]])
+        g = np.array([0., 0., -1.])
+        projected_gravity = self.quat_rotate_inverse_numpy(robot_quat_xyzw, g)
+        return projected_gravity
+    
+    def quat_rotate_inverse_numpy(self, q, v):
+        """
+        q: [x, y, z, w], shape=(4)\\
+        v: [x, y, z], shape=(3)
+        """
+        q_w = q[3]
+        q_vec = q[:3]
+        a = v * (2.0 * q_w ** 2 - 1.0)
+        b = np.cross(q_vec, v) * q_w * 2.0
+        c = q_vec * np.dot(q_vec, v) * 2.0
+        return a - b + c
+
+    def get_walk_cmd(self) -> np.ndarray:
+        """获取行走命令: [x_speed, y_speed, yaw_speed]"""
+        return self.walk_cmd_

Deploy_Tienkung/common/robot_interface.py

@@ -0,0 +1,814 @@
+"""
+Robot Interface
+Python equivalent of the C++ RobotInterface class
+"""
+from __future__ import annotations
+import queue
+from common.peekqueue import PeekableQueue
+import yaml
+import os
+from abc import ABC, abstractmethod
+from typing import Dict, Any
+
+import numpy as np
+# ROS messages
+from bodyctrl_msgs.msg import MotorStatusMsg, CmdMotorCtrl, MotorCtrl, Imu, MotorStatus
+import rclpy
+from std_msgs.msg import String
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+import transforms3d as t3d
+from .body_id_map import BodyServoIdMap
+from .robot_data import RobotData
+import functools
+import time
+import math
+from sptlib_python import funcSPTrans as FuncSPTrans
+
+from common.joystick import ControlFlag
+from geometry_msgs.msg import TwistStamped
+from std_msgs.msg import Float64
+from FSM.fsm_base import FSMStateName
+
+def timing_decorator(func):
+    """
+    装饰器：记录函数执行时间
+    """
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+        start_time = time.perf_counter()
+        result = func(*args, **kwargs)
+        end_time = time.perf_counter()
+        execution_time = end_time - start_time
+        # print(f"[TIMING] {func.__name__} executed in {execution_time:.6f} seconds")
+        return result
+    return wrapper
+
+
+class RobotInterface(ABC):
+    """机器人接口抽象基类"""
+
+    def __init__(self, robot_data: RobotData):
+        self.robot_data_ = robot_data
+
+    @abstractmethod
+    def init(self, node: Node):
+        """初始化接口"""
+        pass
+
+    @abstractmethod
+    def update_robot_data(self, flag:ControlFlag, time_passed: float):
+        """更新机器人状态"""
+        pass
+
+
+    @abstractmethod
+    def send_motor_commands(self, flag: ControlFlag):
+        """发布电机控制命令"""
+        pass
+
+
+
+class RobotInterfaceImpl(RobotInterface):
+    """机器人接口具体实现"""
+
+    def __init__(self, robot_data: RobotData, config_path: str = ''):
+        super().__init__(robot_data)
+        self.initialized = False
+        self.node = None
+        self.config_path = config_path
+
+        # ID映射
+        self.id_map = BodyServoIdMap()
+        self.id_map.body_can_id_map_init()
+
+        # 消息队列
+        self.queue_leg_motor_state = PeekableQueue(maxsize=1)
+        self.queue_arm_motor_state = PeekableQueue(maxsize=1)
+        self.queue_waist_motor_state = PeekableQueue(maxsize=1)
+        self.queue_imu_xsens = PeekableQueue(maxsize=1)
+        self.queue_walk_cmd = PeekableQueue(maxsize=1)
+
+
+        # 关节维度
+        self.floating_base_dof = 6
+        self.whole_joint_nums = self.id_map.whole_motor_nums + self.floating_base_dof
+
+        # 临时变量用于优化计算
+        self.temp_q = np.empty(self.id_map.whole_motor_nums)
+        # 预分配另一个用于存储中间计算的临时数组
+        self._temp_zero_cnt = np.empty(self.id_map.whole_motor_nums)
+
+        # 电机控制参数
+        self.motor_dir = np.ones(self.id_map.whole_motor_nums)
+        self.zero_cnt = np.zeros(self.id_map.whole_motor_nums)
+        self.zero_offset = np.zeros(self.id_map.whole_motor_nums)
+
+        # 添加标志位，用于跟踪是否是首次接收数据
+        self.first_leg_data_received = False
+        self.first_arm_data_received = False
+        self.first_waist_data_received = False
+
+        # 关节限位
+        self.joint_limits = {}
+        self.joint_pos_threshold = math.pi
+
+        # 串并联转换器
+        self.fun_s2p = FuncSPTrans()
+
+        # 串并联转换相关变量
+        self.left_ankle_indices = np.array([4, 5]) + self.floating_base_dof
+        self.right_ankle_indices = np.array([10, 11]) + self.floating_base_dof
+        self.q_a_p = np.zeros(4)  # 并联关节位置
+        self.qdot_a_p = np.zeros(4)  # 并联关节速度
+        self.tor_a_p = np.zeros(4)  # 并联关节力矩
+        self.ankle_kp_p = np.zeros(4)  # 并联关节刚度
+        self.ankle_kd_p = np.zeros(4)  # 并联关节阻尼
+
+        # TF相关属性
+        self.tf_buffer = None
+        self.tf_listener = None
+        
+        # ROS publishers and subscribers
+        self.pub_leg_motor_cmd = None
+        self.pub_arm_motor_cmd = None
+        self.pub_waist_motor_cmd = None
+        self.pub_fsm_state = None
+        self.sub_leg_state = None
+        self.sub_arm_state = None
+        self.sub_waist_state = None
+
+        # 当前机器人所处状态
+        self.current_state: FSMStateName = FSMStateName.STOP
+
+    def update_param(self, current_state: FSMStateName = None):
+        """更新机器人接口"""
+        if current_state is not None:
+            self.current_state = current_state
+
+    def load_config(self):
+        """从配置文件加载关键参数"""
+        config_path = self.config_path
+        if not os.path.exists(config_path):
+            self.node.get_logger().error(
+                f"Joint limits config file not found: {config_path}")
+            raise FileNotFoundError(
+                f"Joint limits config file not found: {config_path}")
+
+        try:
+            with open(config_path, 'r') as f:
+                config = yaml.safe_load(f)
+        except Exception as e:
+            self.node.get_logger().error(
+                f"Failed to load joint limits config from {config_path}: {e}")
+            raise RuntimeError(
+                f"Failed to load joint limits config from {config_path}: {e}")
+
+        # 读取运行模式
+        self.sim = config.get('sim')
+        self.debug = config.get('debug')
+        # 机器人接口配置
+        interface_config = config.get('robot_interface')
+        # 是否限位
+        self.clip_actions = interface_config.get('clip_actions')
+        # 加载关节限位值
+        self._load_joint_limits(interface_config)
+        # 加载控制状态
+        self._load_control_status(interface_config)
+        # 零位
+        self.zero_pos = np.array(interface_config.get('zero_pos'))
+        # 电流转换比例
+        self.ct_scale = np.array(interface_config.get('ct_scale'))
+        # IMU 数据偏移
+        self.xsense_roll_offset = interface_config.get(
+            'xsense_data_roll_offset')
+        # 禁用电机
+        self.disable_joints_ = interface_config.get('disable_joints', False)
+        # 脚踝Kp,Kd
+        self.ankle_kp_p = np.array(interface_config.get('ankle_kp_p'))
+        self.ankle_kd_p = np.array(interface_config.get('ankle_kd_p'))
+
+    def _load_control_status(self, config: Dict[str, Any]):
+        # 字符串命令到枚举值的映射
+        state_to_FSMState = {
+            "STOP": FSMStateName.STOP,
+            "ZERO": FSMStateName.ZERO,
+            "WALKAMP": FSMStateName.WALKAMP,
+            "MYPOLICY": FSMStateName.MYPOLICY,
+            "XSIMRUN": FSMStateName.XSIMRUN,
+        }
+        self.waist_control_status = [state_to_FSMState[state] for state in config.get('waist_control_status')]
+        self.legs_control_status = [state_to_FSMState[state] for state in config.get('legs_control_status')]
+        self.arms_control_status = [state_to_FSMState[state] for state in config.get('arms_control_status')]
+        self.left_arm_only_status = [state_to_FSMState[state] for state in config.get('left_arm_only_status')]
+        self.right_arm_only_status = [state_to_FSMState[state] for state in config.get('right_arm_only_status')]
+
+    def _load_joint_limits(self, config: Dict[str, Any]):
+        """从配置文件加载关节限位值"""
+        # 从配置中获取关节限位信息
+        joint_limits_config = config.get('joint_limits', None)
+
+        if joint_limits_config is None:
+            error_msg = "No joint_limits section in config"
+            self.node.get_logger().error(error_msg)
+            raise ValueError(error_msg)
+        else:
+            # 从配置中加载限位值
+            for joint_name, limits in joint_limits_config.items():
+                if 'min' in limits and 'max' in limits:
+                    self.joint_limits[joint_name] = {
+                        'min': float(limits['min']),
+                        'max': float(limits['max'])
+                    }
+                else:
+                    error_msg = f"Invalid limits for joint {joint_name}"
+                    self.node.get_logger().error(error_msg)
+                    raise ValueError(error_msg)
+
+            self.node.get_logger().info(f"Loaded joint limits from {config}")
+        # 预计算ID到限位的映射
+        self.id_to_limits = {}
+        for joint_name, limits in self.joint_limits.items():
+            index = self.id_map.get_index_by_name(joint_name)
+            if index >= 0:
+                motor_id = self.id_map.get_id_by_index(index)
+                self.id_to_limits[motor_id] = limits
+
+        # 记录加载的限位值
+        for joint_name, limits in self.joint_limits.items():
+            self.node.get_logger().debug(
+                f"Joint {joint_name}: [{limits['min']}, {limits['max']}]")
+        print("-" * 30 + '关节限位值' + '-' * 30)
+        print(self.joint_limits)
+
+    def init(self, node: Node):
+        """初始化接口"""
+        self.node = node
+        self.initialized = True
+
+        # 初始化ROS接口
+        self._init_ros_interfaces()
+        # 加载配置文件
+        self.load_config()
+        
+        node.get_logger().info("Robot interface initialized")
+
+    def _init_ros_interfaces(self):
+        """初始化ROS接口"""
+        qos_profile = QoSProfile(
+            # reliability=ReliabilityPolicy.RELIABLE,
+            # history=HistoryPolicy.KEEP_LAST,
+            depth=10)
+
+        # 发布者
+        self.pub_leg_motor_cmd = self.node.create_publisher(
+            CmdMotorCtrl, '/leg/cmd_ctrl', qos_profile)
+        self.pub_arm_motor_cmd = self.node.create_publisher(
+            CmdMotorCtrl, '/arm/cmd_ctrl', qos_profile)
+        self.pub_waist_motor_cmd = self.node.create_publisher(
+            CmdMotorCtrl, '/waist/cmd_ctrl', qos_profile)
+        self.pub_fsm_state = self.node.create_publisher(
+            String, '/control/fsm_state', qos_profile)
+
+        # 订阅者
+        self.sub_leg_state = self.node.create_subscription(
+            MotorStatusMsg, '/leg/status', self._leg_motor_status_callback,
+            qos_profile)
+        self.sub_arm_state = self.node.create_subscription(
+            MotorStatusMsg, '/arm/status', self._arm_motor_status_callback,
+            qos_profile)
+        self.sub_waist_state = self.node.create_subscription(
+            MotorStatusMsg, '/waist/status', self._waist_motor_status_callback,
+            qos_profile)
+
+        #（非电机相关）
+        self.sub_imu_xsens = self.node.create_subscription(
+            Imu, '/imu/status', self._imu_status_callback, qos_profile)
+
+
+    # @timing_decorator
+    def get_imu_data(self):
+        """处理传感器数据（非电机）"""
+        # 处理IMU数据
+        while True:
+            try:
+                msg = self.queue_imu_xsens.peek()
+                self.robot_data_.imu_data_[0] = msg.euler.yaw
+                self.robot_data_.imu_data_[1] = msg.euler.pitch
+                self.robot_data_.imu_data_[2] = msg.euler.roll
+                self.robot_data_.imu_data_[3] = msg.angular_velocity.x
+                self.robot_data_.imu_data_[4] = msg.angular_velocity.y
+                self.robot_data_.imu_data_[5] = msg.angular_velocity.z
+                self.robot_data_.imu_data_[6] = msg.linear_acceleration.x
+                self.robot_data_.imu_data_[7] = msg.linear_acceleration.y
+                self.robot_data_.imu_data_[8] = msg.linear_acceleration.z
+                self.robot_data_.imu_quat_[0] = msg.orientation.w
+                self.robot_data_.imu_quat_[1] = msg.orientation.x
+                self.robot_data_.imu_quat_[2] = msg.orientation.y
+                self.robot_data_.imu_quat_[3] = msg.orientation.z
+                break
+            except queue.Empty:
+                time.sleep(0.0001)
+
+    def update_robot_state(self):
+        """读取电机状态数据更新为机器人状态数据"""
+        # 处理腿部电机状态
+        while True:
+            try:
+                msg = self.queue_leg_motor_state.peek()
+                if self.debug:
+                    current_time = self.node.get_clock().now().to_msg()
+                    msg_time = msg.header.stamp
+                    time_diff = (current_time.sec -
+                                 msg_time.sec) * 1000000000 + (
+                                     current_time.nanosec - msg_time.nanosec)
+                    time_diff_ms = time_diff / 1000000.0
+                    print(f"Time difference: {time_diff_ms} ms")
+                for status in msg.status:
+                    self.motor_state_to_robot_state(
+                        status, self.first_leg_data_received)
+                break
+            except queue.Empty:
+                time.sleep(0.0001)
+
+        # 处理手臂电机状态
+        while True:
+            try:
+                msg = self.queue_arm_motor_state.peek()
+                if self.debug:
+                    current_time = self.node.get_clock().now().to_msg()
+                    msg_time = msg.header.stamp
+                    time_diff = (current_time.sec -
+                                 msg_time.sec) * 1000000000 + (
+                                     current_time.nanosec - msg_time.nanosec)
+                    time_diff_ms = time_diff / 1000000.0
+                    print(f"Time difference: {time_diff_ms} ms")
+                for status in msg.status:
+                    self.motor_state_to_robot_state(
+                        status, self.first_arm_data_received)
+
+                break
+            except queue.Empty:
+                time.sleep(0.0001)
+
+        # 处理腰部电机状态
+        while True:
+            try:
+                msg = self.queue_waist_motor_state.peek()
+                if self.debug:
+                    current_time = self.node.get_clock().now().to_msg()
+                    msg_time = msg.header.stamp
+                    time_diff = (current_time.sec -
+                                 msg_time.sec) * 1000000000 + (
+                                     current_time.nanosec - msg_time.nanosec)
+                    time_diff_ms = time_diff / 1000000.0
+                    print(f"Time difference: {time_diff_ms} ms")
+                for status in msg.status:
+                    self.motor_state_to_robot_state(
+                        status, self.first_waist_data_received)
+                break
+            except queue.Empty:
+                time.sleep(0.0001)
+
+    def motor_state_to_robot_state(self, status, received_flag: bool):
+        index = self.id_map.get_index_by_id(status.name)
+        if index >= 0:
+            robotdata_index = index + self.floating_base_dof  # 偏移到完整关节数组中
+            # 直接赋值
+            self.robot_data_.q_a_[robotdata_index] = status.pos
+            self.robot_data_.q_dot_a_[robotdata_index] = status.speed
+            self.robot_data_.tau_a_[
+                robotdata_index] = status.current * self.ct_scale[min(
+                    index,
+                    len(self.ct_scale) - 1)]
+            self.robot_data_.temperature_a[
+                robotdata_index - self.floating_base_dof] = status.temperature
+
+            self.robot_data_.q_a_[robotdata_index] = (
+                status.pos - self.zero_pos[index]
+            ) * self.motor_dir[index] + self.zero_offset[index]
+            if self.robot_data_.q_a_[robotdata_index] > math.pi:
+                self.zero_cnt[index] = -1.0
+            elif self.robot_data_.q_a_[robotdata_index] < -math.pi:
+                self.zero_cnt[index] = 1.0
+
+            self.robot_data_.q_a_[
+                robotdata_index] += self.zero_cnt[index] * 2.0 * math.pi
+            self.robot_data_.q_dot_a_[robotdata_index] *= self.motor_dir[index]
+            self.robot_data_.tau_a_[robotdata_index] *= self.motor_dir[index]
+
+            if not received_flag or abs(
+                    self.robot_data_.q_a_[robotdata_index] -
+                    self.robot_data_.q_a_last[robotdata_index]
+            ) > self.joint_pos_threshold:
+                if received_flag:
+                    self.node.get_logger().warn(
+                        f"Joint {index} error detected")
+                    self.robot_data_.q_a_[
+                        robotdata_index] = self.robot_data_.q_a_last[
+                            robotdata_index]
+                    self.robot_data_.q_dot_a_[
+                        robotdata_index] = self.robot_data_.qdot_a_last[
+                            robotdata_index]
+                    self.robot_data_.tau_a_[
+                        robotdata_index] = self.robot_data_.tor_a_last[
+                            robotdata_index]
+                else:
+                    # 首次接收数据，更新标志位
+                    received_flag = True
+            self.robot_data_.q_a_last[robotdata_index] = self.robot_data_.q_a_[
+                robotdata_index]
+            self.robot_data_.qdot_a_last[
+                robotdata_index] = self.robot_data_.q_dot_a_[robotdata_index]
+            self.robot_data_.tor_a_last[
+                robotdata_index] = self.robot_data_.tau_a_[robotdata_index]
+
+    def update_sensor_states(self):
+        # 获取Imu数据
+        self.get_imu_data()
+        # 添加IMU偏移
+        self.robot_data_.imu_data_[2] += self.xsense_roll_offset
+
+    def update_robot_data(self, flag: ControlFlag, time_passed: float):
+        # 更新传感器数据
+        self.update_sensor_states()
+        # 更新电机状态数据
+        self.update_robot_state()
+        # 更新机器人控制命令
+        self.update_robot_cmd(flag)
+        # 脚踝并联转串联
+        self.backup_serial_cmd()
+        if not self.sim:
+            #真机
+            self.ankle_parallel_to_serial()
+
+        # 更新robot_data 时间戳
+        self.robot_data_.time_now_ = time_passed
+
+    def backup_serial_cmd(self):
+        self.robot_data_.q_d_s_ = self.robot_data_.q_d_.copy()
+
+    def _check_and_clip_joint_limits_fast(
+            self, cmd_name: int, position: float) -> tuple[bool, float]:
+        """
+        快速检查并修正关节位置限位（避免重复查询）
+        """
+        if not self.sim and self.clip_actions:
+            limit = self.id_to_limits[cmd_name]
+            clipped_pos = np.clip(position, limit["min"], limit["max"])
+            return clipped_pos == position, clipped_pos
+        else:
+            return True, position
+
+    @timing_decorator
+    def convert_to_motor_commands(self):
+        """将机器人控制命令转换为电机控制命令"""
+        # 使用切片操作一次性复制数据，避免逐个元素赋值
+        q_d_reordered = self.robot_data_.q_d_[self.floating_base_dof:]
+        qdot_d_reordered = self.robot_data_.q_dot_d_[self.floating_base_dof:]
+        tor_d_reordered = self.robot_data_.tau_d_[self.floating_base_dof:]
+
+        # 计算 q_d_reordered - self.zero_offset
+        np.subtract(q_d_reordered, self.zero_offset, out=self.temp_q)
+        # 计算 self.zero_cnt * 2.0 * self.pi
+        np.multiply(self.zero_cnt, 2.0 * math.pi, out=self._temp_zero_cnt)
+        # 计算 q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi
+        np.subtract(self.temp_q, self._temp_zero_cnt, out=self.temp_q)
+        # 计算 (q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi) * self.motor_dir
+        np.multiply(self.temp_q, self.motor_dir, out=self.temp_q)
+        # 计算最终结果 (q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi) * self.motor_dir + self.zero_pos
+        np.add(self.temp_q,
+               self.zero_pos,
+               out=self.robot_data_.q_d_[self.floating_base_dof:])
+
+        # qdot_d和tor_d的计算也可以向量化
+        np.multiply(qdot_d_reordered,
+                    self.motor_dir,
+                    out=self.robot_data_.q_dot_d_[self.floating_base_dof:])
+        np.multiply(tor_d_reordered,
+                    self.motor_dir,
+                    out=self.robot_data_.tau_d_[self.floating_base_dof:])
+
+        # 如果关节被禁用
+        if self.disable_joints_:
+            self.robot_data_.joint_kp_p_.fill(0.0)
+            self.robot_data_.joint_kd_p_.fill(0.0)
+            self.robot_data_.tau_d_.fill(0.0)
+            self.node.get_logger().warn("Joints disabled!")
+
+    def publish_motor_commands(self, flag: ControlFlag):
+        # flag_fsm_command = flag.fsm_state_command
+        current_state = self.current_state
+        if self.pub_fsm_state is not None:
+            state_msg = String()
+            state_msg.data = current_state.name
+            self.pub_fsm_state.publish(state_msg)
+        # 发布腿部控制命令
+        if self.legs_control_status == [] or current_state in self.legs_control_status:
+            leg_msg = CmdMotorCtrl()
+            leg_msg.header.stamp = self.node.get_clock().now().to_msg()
+            for i in range(self.id_map.leg_motor_nums):
+                cmd = MotorCtrl()
+                cmd.name = self.id_map.get_id_by_index(i)
+                cmd.kp = float(self.robot_data_.joint_kp_p_[i])
+                cmd.kd = float(self.robot_data_.joint_kd_p_[i])
+                cmd.pos = float(self.robot_data_.q_d_[i +
+                                                      self.floating_base_dof])
+                cmd.spd = float(
+                    self.robot_data_.q_dot_d_[i + self.floating_base_dof])
+                cmd.tor = float(
+                    self.robot_data_.tau_d_[i + self.floating_base_dof])
+
+                # 检查关节位置限位
+                within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
+                    cmd.name, cmd.pos)
+                if not within_limit:
+                    print(
+                        f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
+                    )
+                leg_msg.cmds.append(cmd)
+            self.pub_leg_motor_cmd.publish(leg_msg)
+
+        # 只在特定模式下控制腰部
+        if current_state in self.waist_control_status:
+            # 腰部控制命令
+            waist_msg = CmdMotorCtrl()
+            waist_msg.header.stamp = self.node.get_clock().now().to_msg()
+            for i in range(self.id_map.waist_motor_nums):
+                cmd = MotorCtrl()
+                motor_idx = i + self.id_map.leg_motor_nums
+                cmd.name = self.id_map.get_id_by_index(
+                    motor_idx)  # 12 -> 33(yaw)
+                cmd.kp = float(self.robot_data_.joint_kp_p_[motor_idx])
+                cmd.kd = float(self.robot_data_.joint_kd_p_[motor_idx])
+                cmd.pos = float(self.robot_data_.q_d_[motor_idx +
+                                                      self.floating_base_dof])
+                cmd.spd = float(
+                    self.robot_data_.q_dot_d_[motor_idx +
+                                              self.floating_base_dof])
+                cmd.tor = float(
+                    self.robot_data_.tau_d_[motor_idx +
+                                            self.floating_base_dof])
+
+                # 检查关节位置限位
+                within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
+                    cmd.name, cmd.pos)
+                if not within_limit:
+                    print(
+                        f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
+                    )
+                waist_msg.cmds.append(cmd)
+            # print(f'waist_msg {waist_msg}')
+            self.pub_waist_motor_cmd.publish(waist_msg)
+
+        # 只在特定模式下控制手臂
+        if current_state in self.arms_control_status:
+            # 手臂控制命令
+            arm_msg = CmdMotorCtrl()
+            arm_msg.header.stamp = self.node.get_clock().now().to_msg()
+            if current_state in self.left_arm_only_status:
+                control_index = np.arange(0, 7)
+            elif current_state in self.right_arm_only_status:
+                control_index = np.arange(self.id_map.arm_motor_nums - 7, self.id_map.arm_motor_nums)
+            else:
+                control_index = np.arange(0, self.id_map.arm_motor_nums)
+            for i in control_index:
+                cmd = MotorCtrl()
+                motor_idx = i + self.id_map.leg_motor_nums + self.id_map.waist_motor_nums
+                cmd.name = self.id_map.get_id_by_index(motor_idx)
+                cmd.kp = float(self.robot_data_.joint_kp_p_[motor_idx])
+                cmd.kd = float(self.robot_data_.joint_kd_p_[motor_idx])
+                cmd.pos = float(self.robot_data_.q_d_[motor_idx +
+                                                      self.floating_base_dof])
+                cmd.spd = float(
+                    self.robot_data_.q_dot_d_[motor_idx +
+                                              self.floating_base_dof])
+                cmd.tor = float(
+                    self.robot_data_.tau_d_[motor_idx +
+                                            self.floating_base_dof])
+
+                # 检查关节位置限位
+                within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
+                    cmd.name, cmd.pos)
+                if not within_limit:
+                    print(
+                        f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
+                    )
+                arm_msg.cmds.append(cmd)
+            # print(f'arm_msg {arm_msg}')
+            self.pub_arm_motor_cmd.publish(arm_msg)
+
+    @timing_decorator
+    def send_motor_commands(self, flag: ControlFlag):
+        """发布电机控制命令"""
+        if not self.initialized:
+            return
+        if not self.sim:
+            #真机
+            self.ankle_serial_to_parallel()
+        self.convert_to_motor_commands()
+        self.publish_motor_commands(flag)
+
+    # ROS回调函数
+    def _leg_motor_status_callback(self, msg):
+        """腿部电机状态回调"""
+        try:
+            self.queue_leg_motor_state.put_nowait(msg)
+        except queue.Full:
+            # 队列满时移除旧数据，加入新数据
+            try:
+                self.queue_leg_motor_state.get_nowait()  # 移除旧数据
+                self.queue_leg_motor_state.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+    def _arm_motor_status_callback(self, msg):
+        """手臂电机状态回调"""
+        try:
+            self.queue_arm_motor_state.put_nowait(msg)
+        except queue.Full:
+            # 队列满时移除旧数据，加入新数据
+            try:
+                self.queue_arm_motor_state.get_nowait()  # 移除旧数据
+                self.queue_arm_motor_state.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+    def _waist_motor_status_callback(self, msg):
+        """腰部电机状态回调"""
+        try:
+            self.queue_waist_motor_state.put_nowait(msg)
+        except queue.Full:
+            # 队列满时移除旧数据，加入新数据
+            try:
+                self.queue_waist_motor_state.get_nowait()  # 移除旧数据
+                self.queue_waist_motor_state.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+    def _imu_status_callback(self, msg):
+        """IMU状态回调"""
+        try:
+            self.queue_imu_xsens.put_nowait(msg)
+        except queue.Full:
+            try:
+                self.queue_imu_xsens.get_nowait()  # 移除旧数据
+                self.queue_imu_xsens.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+
+    def ankle_parallel_to_serial(self):
+        # 串并联转换：并转串 (类似C++版本中的处理)
+        # 提取左右脚两个踝关节（并联关节）
+        q_a_p = np.zeros(4)  # 并联关节角度（实际）
+        qdot_a_p = np.zeros(4)  # 并联关节速度（实际）
+        tor_a_p = np.zeros(4)  # 并联关节力矩（实际）
+        q_a_s = np.zeros(4)  # 串联关节角度（实际）
+        qdot_a_s = np.zeros(4)  # 串联关节速度（实际）
+        tor_a_s = np.zeros(4)  # 串联关节力矩（实际）
+
+        q_a_p[:2] = self.robot_data_.q_a_[
+            self.left_ankle_indices]  # 左脚踝关节 (pitch, roll)
+        q_a_p[2:] = self.robot_data_.q_a_[
+            self.right_ankle_indices]  # 右脚踝关节 (pitch, roll)
+
+        qdot_a_p[:2] = self.robot_data_.q_dot_a_[self.left_ankle_indices]
+        qdot_a_p[2:] = self.robot_data_.q_dot_a_[self.right_ankle_indices]
+
+        tor_a_p[:2] = self.robot_data_.tau_a_[self.left_ankle_indices]
+        tor_a_p[2:] = self.robot_data_.tau_a_[self.right_ankle_indices]
+
+        self.q_a_p = q_a_p.copy()
+        self.qdot_a_p = qdot_a_p.copy()
+        self.tor_a_p = tor_a_p.copy()
+        if self.debug:
+            print("-" * 20 + "并转串联前" + "-" * 20)
+            print("q_a_p:", q_a_p)
+            print("qdot_a_p:", qdot_a_p)
+            print("tor_a_p:", tor_a_p)
+
+        # 计算并转串（正运动学）
+        self.fun_s2p.set_p_est(q_a_p, qdot_a_p, tor_a_p)
+        self.fun_s2p.calcFK()
+        self.fun_s2p.calcIK()
+
+        success, q_a_s, qdot_a_s, tor_a_s = self.fun_s2p.get_s_state()
+
+        if self.debug:
+            print("-" * 20 + "并转串联后" + "-" * 20)
+            print("q_a_s:", q_a_s)
+            print("qdot_a_s:", qdot_a_s)
+            print("tor_a_s:", tor_a_s)
+
+        # 用串联关节值替换原来的并联关节值
+        self.robot_data_.q_a_[self.left_ankle_indices] = q_a_s[:2]  # 左脚踝关节串联值
+        self.robot_data_.q_a_[self.right_ankle_indices] = q_a_s[2:]  # 右脚踝关节串联值
+
+        self.robot_data_.q_dot_a_[self.left_ankle_indices] = qdot_a_s[:2]
+        self.robot_data_.q_dot_a_[self.right_ankle_indices] = qdot_a_s[2:]
+
+        self.robot_data_.tau_a_[self.left_ankle_indices] = tor_a_s[:2]
+        self.robot_data_.tau_a_[self.right_ankle_indices] = tor_a_s[2:]
+
+    def ankle_serial_to_parallel(self):
+        # 串转并：将串联关节命令转换为并联关节命令（类似C++版本）
+        # 提取踝关节两关节的串联命令
+        q_d_p = np.zeros(4)  # 并联关节角度（期望）
+        qdot_d_p = np.zeros(4)  # 并联关节速度（期望）
+        tor_d_p = np.zeros(4)  # 并联关节力矩（期望）
+        q_d_s = np.zeros(4)  # 串联关节角度（期望）
+        qdot_d_s = np.zeros(4)  # 串联关节速度（期望）
+        tor_d_s = np.zeros(4)  # 串联关节力矩（期望）
+
+        q_d_s[:2] = self.robot_data_.q_d_[self.left_ankle_indices]  # 左脚踝关节串联命令
+        q_d_s[2:] = self.robot_data_.q_d_[
+            self.right_ankle_indices]  # 右脚踝关节串联命令
+
+        qdot_d_s[:2] = self.robot_data_.q_dot_d_[self.left_ankle_indices]
+        qdot_d_s[2:] = self.robot_data_.q_dot_d_[self.right_ankle_indices]
+
+        tor_d_s[:2] = self.robot_data_.tau_d_[self.left_ankle_indices]
+        tor_d_s[2:] = self.robot_data_.tau_d_[self.right_ankle_indices]
+
+        q_a_s = np.zeros(4)  # 串联关节角度（实际）
+        qdot_a_s = np.zeros(4)  # 串联关节速度（实际）
+        q_a_s[:2] = self.robot_data_.q_a_[self.left_ankle_indices]  # 左脚踝关节串联值
+        q_a_s[2:] = self.robot_data_.q_a_[self.right_ankle_indices]  # 右脚踝关节串联值
+        qdot_a_s[:2] = self.robot_data_.q_dot_a_[
+            self.left_ankle_indices]  # 左脚踝关节串联速度
+        qdot_a_s[2:] = self.robot_data_.q_dot_a_[
+            self.right_ankle_indices]  # 右脚踝关节串联速度
+
+        kp = np.zeros(4)  # 串联关节刚度
+        kd = np.zeros(4)  # 串联关节阻尼
+        kp[:2] = self.robot_data_.joint_kp_p_[self.left_ankle_indices -
+                                              self.floating_base_dof]
+        kp[2:] = self.robot_data_.joint_kp_p_[self.right_ankle_indices -
+                                              self.floating_base_dof]
+        kd[:2] = self.robot_data_.joint_kd_p_[self.left_ankle_indices -
+                                              self.floating_base_dof]
+        kd[2:] = self.robot_data_.joint_kd_p_[self.right_ankle_indices -
+                                              self.floating_base_dof]
+
+        tor_d_s = kp * (q_d_s - q_a_s) + kd * (qdot_d_s - qdot_a_s)
+
+        if self.debug:
+            print("-" * 20 + "串转并联前" + "-" * 20)
+            print("q_d_s:", q_d_s)
+            print("qdot_d_s:", qdot_d_s)
+            print("tor_d_s:", tor_d_s)
+
+        # 串转并计算
+        self.fun_s2p.set_s_des(q_d_s, qdot_d_s, tor_d_s)
+        self.fun_s2p.calc_joint_pos_ref()
+        self.fun_s2p.calc_joint_tor_des()
+        success, q_d_p, qdot_d_p, tor_d_p = self.fun_s2p.get_p_des()
+
+        q_d_p = (tor_d_p - self.ankle_kd_p *
+                 (qdot_d_p - self.qdot_a_p)) / self.ankle_kp_p + self.q_a_p
+
+        if self.debug:
+            print("-" * 20 + "串转并联后" + "-" * 20)
+            print("q_d_p:", q_d_p)
+            print("qdot_d_p:", qdot_d_p)
+            print("tor_d_p:", tor_d_p)
+
+        # 用并联关节命令覆盖原来的串联命令
+        self.robot_data_.q_d_[self.left_ankle_indices] = q_d_p[:2]  # 左脚踝关节并联命令
+        self.robot_data_.q_d_[self.right_ankle_indices] = q_d_p[2:]  # 右脚踝关节并联命令
+
+        self.robot_data_.q_dot_d_[self.left_ankle_indices] = qdot_d_p[:2]
+        self.robot_data_.q_dot_d_[self.right_ankle_indices] = qdot_d_p[2:]
+
+        # self.robot_data_.tau_d_[self.left_ankle_indices] = tor_d_p[:2]
+        # self.robot_data_.tau_d_[self.right_ankle_indices] = tor_d_p[2:]
+        self.robot_data_.tau_d_[self.left_ankle_indices] = 0.0
+        self.robot_data_.tau_d_[self.right_ankle_indices] = 0.0
+        # 更新脚踝Kp,Kd
+        self.robot_data_.joint_kp_p_[
+            self.left_ankle_indices -
+            self.floating_base_dof] = self.ankle_kp_p[:2]
+        self.robot_data_.joint_kp_p_[
+            self.right_ankle_indices -
+            self.floating_base_dof] = self.ankle_kp_p[2:]
+        self.robot_data_.joint_kd_p_[
+            self.left_ankle_indices -
+            self.floating_base_dof] = self.ankle_kd_p[:2]
+        self.robot_data_.joint_kd_p_[
+            self.right_ankle_indices -
+            self.floating_base_dof] = self.ankle_kd_p[2:]
+
+    def update_robot_cmd(self, flag: ControlFlag):
+        """更新机器人控制命令"""
+        if flag.enable:
+            # 使用getattr设置默认值，避免AttributeError
+            x_command = getattr(flag, 'x_speed_command')
+            y_command = getattr(flag, 'y_speed_command')
+            yaw_command = getattr(flag, 'yaw_speed_command')
+            self.robot_data_.walk_cmd_ = [x_command, y_command, yaw_command]
+        self.robot_data_.control_flag.fsm_state_command = flag.fsm_state_command
+
+def get_robot_interface(robot_data: RobotData, config_path: str) -> RobotInterface:
+    """工厂函数，返回机器人接口实例"""
+    return RobotInterfaceImpl(robot_data, config_path)

Deploy_Tienkung/common/stdin_keyboard_control.py

@@ -0,0 +1,422 @@
+"""
+Keyboard Control Module for SSH and Local Environments
+Keyboard input handling for robot state management without external libraries
+"""
+import threading
+import sys
+import select
+import termios
+import tty
+import os
+import yaml
+from typing import Optional
+from .joystick import ControlFlag
+import signal
+
+class KeyboardFlag(ControlFlag):  # 继承ControlFlag
+    def __init__(self):
+        super().__init__()  # 调用父类初始化
+        self.x_speed_command: float = 0.0
+        self.y_speed_command: float = 0.0
+        self.yaw_speed_command: float = 0.0
+        self.height_cmd: float = 0.89
+
+
+class KeyboardController:
+    """键盘控制器，纯Python实现，不依赖外部库"""
+
+    def __init__(self):
+        print("Keyboard Control Started (Pure Python Implementation)")
+        
+        # 初始化成员变量
+        self.keyboard_flag = KeyboardFlag()
+        self.data_mutex = threading.Lock()
+        
+        # 状态追踪变量
+        self.current_height = 0.89
+        self.target_height = 0.89
+        self.height_step = 0.05
+        
+        # 配置参数
+        self.initial_height = 0.89
+        self.forward_command_offset = 0.0
+        self.lateral_command_offset = 0.0
+        self.rotation_command_offset = 0.0
+        self.max_forward_speed = 1.0
+        self.max_lateral_speed = 0.5
+        self.max_rotation_speed = 0.5
+        
+        # 控制标志
+        self.running = False
+        self.input_thread = None
+        self.original_terminal_settings = None
+        
+        # 加载配置文件
+        self._load_config()
+        
+        print("Available keyboard commands:")
+        print("  z - Goto ZERO state")
+        print("  c - Goto STOP state")
+        print("  m - Goto WALKAMP state")
+        print("  p - Goto MYPOLICY state")
+        print("  n - Goto XSIMRUN state")
+        print("  Left/Right arrows - Adjust height")
+        print("  w/a/s/d - Movement controls")
+        print("  q/e - Rotation controls (turn left/right)")
+        print("  r - Reset all movement commands to zero")
+        print("  x - Quit")
+        print("  Ctrl+C - Emergency stop")
+        
+    def _load_config(self):
+        """加载YAML配置文件"""
+        try:
+            config_path = os.path.join('.', "config", "dex_config.yaml")
+            
+            with open(config_path, 'r') as file:
+                config = yaml.safe_load(file)
+                
+            if not config:
+                print("[Keyboard_controller] Failed to load config file")
+                return
+                
+            keyboard_cfg = config.get("keyboard", {})
+            
+            # 加载配置参数
+            self.initial_height = keyboard_cfg.get("initial_height", 0.89)
+            self.forward_command_offset = keyboard_cfg.get("forward_command_offset", 0.0)
+            self.lateral_command_offset = keyboard_cfg.get("lateral_command_offset", 0.0)
+            self.rotation_command_offset = keyboard_cfg.get("rotation_command_offset", 0.0)
+            self.height_step = keyboard_cfg.get("height_step", 0.05)
+            self.max_forward_speed = keyboard_cfg.get("max_forward_speed", 1.0)
+            self.max_lateral_speed = keyboard_cfg.get("max_lateral_speed", 0.5)
+            self.max_rotation_speed = keyboard_cfg.get("max_rotation_speed", 0.5)
+            
+            print(f"Loaded keyboard config:")
+            print(f"  Initial height: {self.initial_height}")
+            print(f"  Height step: {self.height_step}")
+            print(f"  Max forward speed: {self.max_forward_speed}")
+            print(f"  Max lateral speed: {self.max_lateral_speed}")
+            print(f"  Max rotation speed: {self.max_rotation_speed}")
+            
+            self.current_height = self.initial_height
+            self.target_height = self.initial_height
+            self.keyboard_flag.height_cmd = self.current_height
+            
+        except Exception as e:
+            print(f"[Keyboard_controller] YAML load error: {e}")
+    
+    def start(self):
+        """启动键盘监听线程"""
+        self.running = True
+        self.input_thread = threading.Thread(target=self._input_loop, daemon=True)
+        self.input_thread.start()
+        print("Keyboard input thread started")
+        
+    def stop(self):
+        """停止键盘监听"""
+        self.running = False
+
+        # 等待线程结束（但不要无限等待）
+        if self.input_thread and self.input_thread.is_alive():
+            self.input_thread.join(timeout=1.0)
+            if self.input_thread.is_alive():
+                print("Warning: Keyboard thread did not exit cleanly")
+
+        # # 恢复终端设置（重要：这会让Ctrl+C重新工作）
+        if self.original_terminal_settings:
+            try:
+                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.original_terminal_settings)
+                self.original_terminal_settings = None
+            except Exception as e:
+                print(f"Error restoring terminal settings: {e}")
+            
+        print("Keyboard input thread stopped")
+        
+    def _input_loop(self):
+        """主输入循环"""
+        # 保存原始终端设置
+        self.original_terminal_settings = termios.tcgetattr(sys.stdin)
+        
+        try:
+            # 设置终端为原始模式，支持即时按键检测
+            tty.setraw(sys.stdin.fileno())
+            
+            print("Keyboard listener ready. Press keys to control.")
+            print("Press 'x' to quit or Ctrl+C for emergency stop.")
+            
+            while self.running:
+                # 检查输入，100ms超时避免占用太多CPU
+                if select.select([sys.stdin], [], [], 0.1)[0]:
+                    key = sys.stdin.read(1)
+                    self._process_key(key)
+                    
+        except KeyboardInterrupt:
+            print("\nEmergency stop detected! Stopping keyboard controller.")
+            self._emergency_stop()
+        except Exception as e:
+            print(f"Input loop error: {e}")
+        finally:
+            # 确保终端设置被恢复
+            if self.original_terminal_settings:
+                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.original_terminal_settings)
+                self.original_terminal_settings = None
+    
+    def _process_key(self, key):
+        """处理按键输入"""
+        if key == 'w':
+            self._on_w_key()
+        elif key == 's':
+            self._on_s_key()
+        elif key == 'a':
+            self._on_a_key()
+        elif key == 'd':
+            self._on_d_key()
+        elif key == 'q':
+            self._on_q_key()
+        elif key == 'e':
+            self._on_e_key()
+        elif key == 'z':
+            self._on_z_key()
+        elif key == 'c':
+            self._on_c_key()
+        elif key == 'm':
+            self._on_m_key()
+        elif key == 'h':
+            self._on_h_key()
+        elif key == 'r':
+            self._on_r_key()
+        elif key == 'x':
+            self._on_x_key()
+        elif key == 'g':
+            self._on_g_key()
+        elif key == 'p':
+            self._on_p_key()
+        elif key == 'n':
+            self._on_n_key()
+        elif key == 'o':
+            self._on_o_key()
+        elif key == 'v':
+            self._on_v_key()
+        elif key == '\x03':  # Ctrl+C
+            print("\nCtrl+C detected - sending interrupt signal")
+            self._handle_ctrl_c()
+        elif key == '\x1b':  # ESC键，可能是方向键
+            self._handle_arrow_key()
+        else:
+            # 忽略其他按键
+            pass
+    
+    def _handle_arrow_key(self):
+        """处理方向键序列"""
+        # 方向键序列: ESC + [ + A/B/C/D
+        if select.select([sys.stdin], [], [], 0.1)[0]:
+            key2 = sys.stdin.read(1)
+            if key2 == '[':
+                if select.select([sys.stdin], [], [], 0.1)[0]:
+                    key3 = sys.stdin.read(1)
+                    if key3 == 'D':  # 左箭头
+                        self._on_left_arrow()
+                    elif key3 == 'C':  # 右箭头
+                        self._on_right_arrow()
+                    elif key3 == 'A':  # 上箭头
+                        self._on_up_arrow()
+                    elif key3 == 'B':  # 下箭头
+                        self._on_down_arrow()
+        
+    def _on_left_arrow(self):
+        """处理左箭头键 - 增加高度"""
+        with self.data_mutex:
+            self._increase_height()
+                
+    def _on_right_arrow(self):
+        """处理右箭头键 - 降低高度"""
+        with self.data_mutex:
+            self._decrease_height()
+    
+    def _on_up_arrow(self):
+        """处理上箭头键（备用功能）"""
+        print("Up arrow pressed - available for additional functions")
+    
+    def _on_down_arrow(self):
+        """处理下箭头键（备用功能）"""
+        print("Down arrow pressed - available for additional functions")
+    
+    def _on_w_key(self):
+        """处理w键 - 前进"""
+        with self.data_mutex:
+            self.keyboard_flag.x_speed_command += 0.1
+            if self.keyboard_flag.x_speed_command > self.max_forward_speed:
+                self.keyboard_flag.x_speed_command = self.max_forward_speed
+            print(f"Moving forward (speed: {self.keyboard_flag.x_speed_command:.2f})")
+            
+    def _on_s_key(self):
+        """处理s键 - 后退"""
+        with self.data_mutex:
+            self.keyboard_flag.x_speed_command -= 0.1
+            if self.keyboard_flag.x_speed_command < -self.max_forward_speed:
+                self.keyboard_flag.x_speed_command = -self.max_forward_speed
+            print(f"Moving backward (speed: {self.keyboard_flag.x_speed_command:.2f})")
+            
+    def _on_a_key(self):
+        """处理a键 - 左移"""
+        with self.data_mutex:
+            self.keyboard_flag.y_speed_command -= 0.1
+            if self.keyboard_flag.y_speed_command < -self.max_lateral_speed:
+                self.keyboard_flag.y_speed_command = -self.max_lateral_speed
+            print(f"Moving left (speed: {self.keyboard_flag.y_speed_command:.2f})")
+            
+    def _on_d_key(self):
+        """处理d键 - 右移"""
+        with self.data_mutex:
+            self.keyboard_flag.y_speed_command += 0.1
+            if self.keyboard_flag.y_speed_command > self.max_lateral_speed:
+                self.keyboard_flag.y_speed_command = self.max_lateral_speed
+            print(f"Moving right (speed: {self.keyboard_flag.y_speed_command:.2f})")
+            
+    def _on_q_key(self):
+        """处理q键 - 左转"""
+        with self.data_mutex:
+            self.keyboard_flag.yaw_speed_command -= 0.1
+            if self.keyboard_flag.yaw_speed_command < -self.max_rotation_speed:
+                self.keyboard_flag.yaw_speed_command = -self.max_rotation_speed
+            print(f"Turning left (speed: {self.keyboard_flag.yaw_speed_command:.2f})")
+            
+    def _on_e_key(self):
+        """处理e键 - 右转"""
+        with self.data_mutex:
+            self.keyboard_flag.yaw_speed_command += 0.1
+            if self.keyboard_flag.yaw_speed_command > self.max_rotation_speed:
+                self.keyboard_flag.yaw_speed_command = self.max_rotation_speed
+            print(f"Turning right (speed: {self.keyboard_flag.yaw_speed_command:.2f})")
+            
+    def _on_z_key(self):
+        """处理z键 - 切换到ZERO状态"""
+        with self.data_mutex:
+            self.keyboard_flag.fsm_state_command = "gotoZERO"
+            print("Command: gotoZERO")
+    
+    def _on_v_key(self):
+        """处理v键 - 切换到BEYONGDMIMIC状态"""
+        with self.data_mutex:
+            self.keyboard_flag.fsm_state_command = "gotoBEYONDMIMIC"
+            print("Command: gotoBEYONDMIMIC")
+    def _on_c_key(self):
+        """处理c键 - 切换到STOP状态"""
+        with self.data_mutex:
+            self.keyboard_flag.fsm_state_command = "gotoSTOP"
+            print("Command: gotoSTOP")
+            
+            
+    def _on_r_key(self):
+        """处理r键 - 重置移动命令"""
+        with self.data_mutex:
+            self.keyboard_flag.x_speed_command = 0.0
+            self.keyboard_flag.y_speed_command = 0.0
+            self.keyboard_flag.yaw_speed_command = 0.0
+            print("All movement commands reset to zero")
+            
+    def _on_x_key(self):
+        """处理x键 - 退出"""
+        with self.data_mutex:
+            self.running = False
+            print("Quit command received")
+
+    def _on_m_key(self):
+        """处理m键 - 切换到WALKAMP状态"""
+        with self.data_mutex:
+            self.keyboard_flag.fsm_state_command = "gotoWALKAMP"
+            print("Command: gotoWALKAMP")
+
+    def _on_p_key(self):
+        """处理p键 - 切换到MYPOLICY状态"""
+        with self.data_mutex:
+            self.keyboard_flag.x_speed_command = 0.0
+            self.keyboard_flag.y_speed_command = 0.0
+            self.keyboard_flag.yaw_speed_command = 0.0
+            self.keyboard_flag.fsm_state_command = "gotoMYPOLICY"
+            print("Command: gotoMYPOLICY (movement commands reset to zero)")
+
+    def _on_n_key(self):
+        """处理n键 - 切换到XSIMRUN状态"""
+        with self.data_mutex:
+            self.keyboard_flag.x_speed_command = 0.0
+            self.keyboard_flag.y_speed_command = 0.0
+            self.keyboard_flag.yaw_speed_command = 0.0
+            self.keyboard_flag.fsm_state_command = "gotoXSIMRUN"
+            print("Command: gotoXSIMRUN (movement commands reset to zero)")
+    
+    def _handle_ctrl_c(self):
+        """处理Ctrl+C - 发送SIGINT信号给主进程"""
+        # 先停止键盘控制器
+        self.running = False
+        # 恢复终端设置，让信号处理正常工作
+        if self.original_terminal_settings:
+            try:
+                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.original_terminal_settings)
+                self.original_terminal_settings = None
+            except:
+                pass
+        # 发送SIGINT信号给当前进程
+        os.kill(os.getpid(), signal.SIGINT)
+    
+    def _increase_height(self):
+        """增加机器人高度"""
+        new_target = self.target_height + self.height_step
+        if new_target <= 0.90:
+            self.target_height = new_target
+            print(f"Height increased to {self.target_height:.2f}m")
+        else:
+            print("Maximum height reached (0.90m)")
+            
+    def _decrease_height(self):
+        """降低机器人高度"""
+        new_target = self.target_height - self.height_step
+        if new_target >= 0.65:
+            self.target_height = new_target
+            print(f"Height decreased to {self.target_height:.2f}m")
+        else:
+            print("Minimum height reached (0.65m)")
+            
+    def update_flag(self):
+        """更新控制标志"""
+        with self.data_mutex:
+            # 平滑高度调节
+            if abs(self.current_height - self.target_height) > 0.0001:
+                if self.current_height < self.target_height:
+                    self.current_height += 0.0001
+                else:
+                    self.current_height -= 0.0001
+            else:
+                self.current_height = self.target_height
+                
+            self.keyboard_flag.height_cmd = self.current_height
+            
+    def get_keyboard_flag(self) -> KeyboardFlag:
+        """获取当前键盘标志的副本"""
+        with self.data_mutex:
+            flag_copy = KeyboardFlag()
+            flag_copy.__dict__.update(self.keyboard_flag.__dict__)
+            return flag_copy
+            
+    def init(self) -> int:
+        """初始化键盘控制器"""
+        print("Keyboard controller initialized (Pure Python)")
+        return 0
+
+
+# 测试代码
+if __name__ == "__main__":
+    controller = KeyboardController()
+    controller.init()
+    controller.start()
+    
+    try:
+        # 主循环
+        while controller.running:
+            controller.update_flag()
+            import time
+            time.sleep(0.01)
+    except KeyboardInterrupt:
+        print("\nMain loop interrupted")
+    finally:
+        controller.stop()

Deploy_Tienkung/common/xbox_control.py

@@ -0,0 +1,250 @@
+"""
+XBOX Controller compatibility layer.
+Implements the same FSM modes and control flags as `stdin_keyboard_control.py` / `joystick.py`.
+"""
+import os
+import yaml
+import threading
+from typing import Optional
+from dataclasses import dataclass
+from sensor_msgs.msg import Joy
+from .joystick import ControlFlag
+
+
+class XBOXFlag(ControlFlag):
+    def __init__(self):
+        super().__init__()
+        self.x_speed_command: float = 0.0
+        self.y_speed_command: float = 0.0
+        self.yaw_speed_command: float = 0.0
+        self.motion_number: int = 0
+        self.height_cmd: float = 0.89
+
+
+@dataclass
+class XBOXMap:
+    # minimal axes/buttons mapping; populated from Joy message in xbox_map_read
+    a: float = 0.0
+    b: float = 0.0
+    x: float = 0.0
+    y: float = 0.0
+    lb: float = 0.0
+    rb: float = 0.0
+    select: float = 0.0
+    start: float = 0.0
+    l_trigger: float = 0.0
+    r_trigger: float = 0.0
+    lx: float = 0.0
+    ly: float = 0.0
+    rx: float = 0.0
+    ry: float = 0.0
+    # optional keys for devices with limited buttons
+    home: float = 0.0
+    # dpad placeholders (axes 6/7 or buttons depending on device)
+    dpad_h: float = 0.0
+    dpad_v: float = 0.0
+
+
+class XBOXController:
+    """XBOX controller that mirrors the joystick keyboard behavior."""
+
+    def __init__(self):
+        print("XBOX Controller Start")
+        self.map = XBOXMap()
+        self.flag = XBOXFlag()
+        self.data_mutex = threading.Lock()
+
+        # state tracking
+        self.last_select = 0
+        self.last_start = 0
+
+        # configuration
+        self.initial_height = 0.89
+        self.current_height = 0.89
+        self.target_height = 0.89
+        self.forward_command_offset = 0.0
+        self.lateral_command_offset = 0.0
+        self.rotation_command_offset = 0.0
+
+        # 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,
+            'lb': 4, 'rb': 5,  'start': 7,
+            'select': 6, 'home': 8
+        }
+        
+        # default axis map indices (can be overridden in config)
+        self.axis_map = {
+            'lx': 0, 'ly': 1, 'rx': 3, 'ry': 4,
+            'l_trigger': 2, 'r_trigger': 5,
+            'dpad_h': 6, 'dpad_v': 7
+        }
+
+    def _load_config(self):
+        try:
+            config_path = os.path.join('.', 'config', 'dex_config.yaml')
+            with open(config_path, 'r') as f:
+                cfg = yaml.safe_load(f) or {}
+            xbox_cfg = cfg.get('xbox', {})
+            # override button_map if provided
+            # bm = xbox_cfg.get('button_map')
+            # if isinstance(bm, dict):
+            #     for k, v in bm.items():
+            #         try:
+            #             self.button_map[k] = int(v)
+            #         except Exception:
+            #             pass
+                        
+            # # override axis_map if provided
+            # am = xbox_cfg.get('axis_map')
+            # if isinstance(am, dict):
+            #     for k, v in am.items():
+            #         try:
+            #             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)
+            self.lateral_command_offset = xbox_cfg.get('lateral_command_offset', 0.0)
+            self.rotation_command_offset = xbox_cfg.get('rotation_command_offset', 0.0)
+            self.height_step = xbox_cfg.get('height_step', 0.05)
+
+            self.current_height = self.initial_height
+            self.target_height = self.initial_height
+            self.flag.height_cmd = self.current_height
+            print(f"Loaded XBOX config: initial_height={self.initial_height}")
+        except Exception as e:
+            print(f"[XBOXController] YAML load error: {e}")
+
+    def xbox_map_read(self, msg: Joy):
+        """Populate internal map from a ROS Joy message."""
+        with self.data_mutex:
+            # axes layout may differ; try safe indexing
+            axes = list(msg.axes) + [0.0] * 16
+            buttons = list(msg.buttons) + [0] * 32
+            
+            # common mapping assumptions (best-effort)
+            self.map.lx = axes[self.axis_map['lx']]
+            self.map.ly = axes[self.axis_map['ly']]
+            self.map.rx = axes[self.axis_map['rx']]
+            self.map.ry = axes[self.axis_map['ry']]
+            # triggers sometimes on axes
+            self.map.l_trigger = axes[self.axis_map['l_trigger']]
+            self.map.r_trigger = axes[self.axis_map['r_trigger']]
+            # dpad may be on axes
+            self.map.dpad_h = axes[self.axis_map['dpad_h']]
+            self.map.dpad_v = axes[self.axis_map['dpad_v']]
+            
+            # buttons using button_map indices
+            for name, idx in self.button_map.items():
+                try:
+                    val = buttons[idx]
+                except Exception:
+                    val = 0
+                setattr(self.map, name, val)
+
+    def xbox_flag_update(self):
+        """Update ControlFlag from the xbox map, mirroring joystick logic."""
+        with self.data_mutex:
+            # FSM state mapping - cover keyboard commands z/c/m/h/g/p/o
+            # c -> gotoSTOP
+            if self.map.y == 1:
+                self.flag.fsm_state_command = 'gotoSTOP'
+            # h -> gotoDH (Left trigger + A)
+            # v -> gotoBEYONDMIMIC (Left trigger + home)
+            elif self.map.l_trigger < -0.5 and self.map.home == 1:
+                self.flag.fsm_state_command = 'gotoBEYONDMIMIC'
+            # z -> gotoZERO
+            elif self.map.x == 1:
+                self.flag.fsm_state_command = 'gotoZERO'
+
+            # detect state change
+            if not hasattr(self, '_last_state'):
+                self._last_state = self.flag.fsm_state_command
+            state_changed = (self.flag.fsm_state_command != self._last_state)
+            self._last_state = self.flag.fsm_state_command
+
+            if (state_changed and
+                (self.flag.fsm_state_command == 'gotoZERO' or self.flag.fsm_state_command == 'gotoSTOP')):
+                self.current_height = self.initial_height
+                self.target_height = self.initial_height
+                self.flag.height_cmd = self.current_height
+
+            # velocity mapping: continuous (left stick) and small discrete adjustments via buttons
+            ly = float(self.map.ly)
+            lx = float(self.map.lx)
+            rx = float(self.map.rx)
+
+            # continuous stick control (same scaling as joystick)
+            if ly >= 0:
+                self.flag.x_speed_command = (ly * 0.8 + self.forward_command_offset)
+            else:
+                self.flag.x_speed_command = ly * 0.5
+
+            self.flag.y_speed_command = (lx * -0.4 + self.lateral_command_offset)
+            self.flag.yaw_speed_command = (rx * -0.4 + self.rotation_command_offset)
+
+            # discrete movement adjustments (map buttons to keyboard-like increments)
+            # emulate w/s/a/d via shoulder buttons or dpad if needed
+            # D-pad (axes 6/7) is common: we'll read them in xbox_map_read if available
+            dpad_h = getattr(self.map, 'dpad_h', 0.0)
+            dpad_v = getattr(self.map, 'dpad_v', 0.0)
+            # left/right dpad emulate arrow keys for height adjust
+            motion_add_number = 0
+            if dpad_h == -1.0 and getattr(self, 'last_dpad_h', 0.0) == 0.0:
+                # left arrow -> increase height
+                motion_add_number = 1
+            elif dpad_h == 1.0 and getattr(self, 'last_dpad_h', 0.0) == 0.0:
+                # right arrow -> decrease height
+                motion_add_number = -1
+
+            # Remove select/start height adjustments to use only dpad_h for height control
+            # if self.map.select == 1 and self.last_select == 0 and motion_add_number == 0:
+            #     motion_add_number = 1
+            # elif self.map.start == 1 and self.last_start == 0 and motion_add_number == 0:
+            #     motion_add_number = -1
+
+            self.last_select = self.map.select
+            self.last_start = self.map.start
+            self.last_dpad_h = dpad_h
+
+            self.flag.motion_number = motion_add_number
+
+            if motion_add_number != 0:
+                new_target = self.target_height + (motion_add_number * 0.05)
+                if new_target > 0.90:
+                    new_target = 0.90
+                elif new_target < 0.65:
+                    new_target = 0.65
+                self.target_height = new_target
+
+            # smooth height
+            step = 0.01
+            if abs(self.current_height - self.target_height) > step:
+                if self.current_height < self.target_height:
+                    self.current_height += step
+                else:
+                    self.current_height -= step
+            else:
+                self.current_height = self.target_height
+
+            self.flag.height_cmd = self.current_height
+
+            # reset movement (r key) -> using START button
+            if self.map.start == 1:
+                self.flag.x_speed_command = 0.0
+                self.flag.y_speed_command = 0.0
+                self.flag.yaw_speed_command = 0.0
+
+    def get_xbox_flag(self) -> ControlFlag:
+        with self.data_mutex:
+            return self.flag
+
+    def init(self) -> int:
+        print("XBOX controller initialized")
+        return 0

Deploy_Tienkung/config/dex_config.yaml

@@ -0,0 +1,110 @@
+motor_num: 29        # 电机数量
+# actions_size: 12     # action的大小
+dt: 0.01
+
+sim: false 
+
+debug: false
+
+control_tool: udp_loopback # joystick, xbox, keyboard, udp_loopback
+
+joystick:
+  initial_height: 0.957
+  max_height: 0.991
+  min_height: 0.791
+  x_command_offset: 0.0
+  y_command_offset: 0.0
+  yaw_command_offset: 0.0
+  max_x_plus_speed: 0.8
+  max_x_minus_speed: 0.5
+  max_y_speed: 0.5
+  max_yaw_speed: 0.8
+
+keyboard:
+  initial_height: 0.90
+  forward_command_offset: 0.0
+  lateral_command_offset: 0.0
+  rotation_command_offset: 0
+  height_step: 0.05
+  max_forward_speed: 1.5
+  max_lateral_speed: 0.5
+  max_rotation_speed: 0.8
+
+xbox:
+  # 其他配置参数
+  initial_height: 0.89
+  forward_command_offset: 0.0
+  lateral_command_offset: 0.0
+  rotation_command_offset: 0.0
+  height_step: 0.05
+
+
+
+robot_interface:
+  clip_actions: false
+  waist_control_status: ["ZERO", "STOP", "WALKAMP", "MYPOLICY", "XSIMRUN"] #
+  legs_control_status: [] #空代表都允许控制，仅腿部是这个逻辑
+  arms_control_status: ["ZERO", "STOP", "WALKAMP", "MYPOLICY", "XSIMRUN"] #
+  left_arm_only_status: []
+  right_arm_only_status: []
+  xsense_data_roll_offset: 0.0
+  zero_pos: [
+        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, 0.0, 0.0, 0.0, 0.0, 0.0, 
+        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+      ]
+  # 电流转力矩系数
+  ct_scale: [
+            2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+            2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+            3.207, 3.207, 3.207, 
+            3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3, 
+            3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+      ]
+  disable_joints: false
+
+  # 关节限位配置
+  joint_limits:
+    # 腿部关节
+    l_hip_pitch:     {min: -3.14, max: 3.14}   # -180° to 180°
+    l_hip_roll:      {min: -0.47, max: 2.62}   # -27° to 150°
+    l_hip_yaw:       {min: -1.57, max: 4.54}   # -90° to 260°
+    l_knee:          {min: -0.09, max: 2.53}   # -5° to 145°
+    l_ankle_pitch:   {min: -1.22, max: 0.52}   # -70° to 30°
+    l_ankle_roll:    {min: -0.52, max: 0.52}   # -30° to 30°
+    
+    r_hip_pitch:     {min: -3.14, max: 3.14}   # -180° to 180°
+    r_hip_roll:      {min: -0.47, max: 2.62}   # -27° to 150°
+    r_hip_yaw:       {min: -1.57, max: 4.54}   # -90° to 260°
+    r_knee:          {min: -0.09, max: 2.53}   # -5° to 145°
+    r_ankle_pitch:   {min: -1.22, max: 0.52}   # -70° to 30°
+    r_ankle_roll:    {min: -0.52, max: 0.52}   # -30° to 30°
+    
+    # 腰部关节
+    waist_yaw:       {min: -2.97, max: 3.14}   # -170° to 180°
+    waist_roll:      {min: -0.52, max: 0.52}   # -30° to 30°
+    waist_pitch:     {min: -0.52, max: 1.05}   # -30° to 60°
+    
+    # 左臂关节
+    l_shoulder_pitch: {min: -2.97, max: 2.97}  # -170° to 170°
+    l_shoulder_roll:  {min: -0.26, max: 3.40}  # -15° to 195°
+    l_shoulder_yaw:   {min: -2.97, max: 2.97}  # -170° to 170°
+    l_elbow:          {min: -2.62 , max: 0.26}  # -150° to 15°
+    l_wrist_yaw:      {min: -2.97, max: 2.97}  # -170° to 170°
+    l_wrist_pitch:    {min: -1.31, max: 1.40}  # -75° to 80°
+    l_wrist_roll:     {min: -1.05, max: 0.79}  # -60° to 45°
+    
+    # 右臂关节
+    r_shoulder_pitch: {min: -2.97, max: 2.97}  # -170° to 170°
+    r_shoulder_roll:  {min: -3.40, max: 0.26}  # -195° to 15°
+    r_shoulder_yaw:   {min: -2.97, max: 2.97}  # -170° to 170°
+    r_elbow:          {min: -2.62, max: 0.26}  # -150° to 15°
+    r_wrist_yaw:      {min: -2.97, max: 2.97}  # -170° to 170°
+    r_wrist_pitch:    {min: -1.31, max: 1.40}  # -75° to 80°
+    r_wrist_roll:     {min: -0.79, max: 1.05}  # -45° to 60°
+
+  # 脚踝并联关节Kp,Kd
+  ankle_kp_p: [15.0, 15.0, 15.0, 15.0] # ankle_pitch_left, ankle_roll_left, ankle_pitch_right, ankle_roll_right
+  ankle_kd_p: [1.25, 1.25, 1.25, 1.25]

Deploy_Tienkung/policy/beyond_mimic/config/BeyondMimic.yaml

@@ -0,0 +1,117 @@
+# onnx_path: "policy_fightAndSports1_s1.onnx"
+# onnx_path: "policy-dance.onnx"
+onnx_path: "lastdance_onlycom.onnx"
+warm_start_time: 0.0
+
+body_names : [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "elbow_pitch_r_link",
+]
+anchor_body : "pelvis"
+
+#without motion anchor pos && anchor base lin vel
+# command :23+23+3+6  55
+# motion_anchor_ori_b: 6
+# base_ang_vel: 3
+# joint_pos（相对轴角）: 23
+# joint_vel: 23
+# actions（上一时刻动作）: 23
+# total : 133
+num_obs : 133 
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 18, 
+ 22, 25 
+]
+
+kps: [
+        300, 300, 150, 350, 30, 16.8,
+        300, 300, 150, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,150,200,200,
+        150, 50, 50, 150,150,200,200,
+]
+kds: [
+        10, 10, 5, 10, 5.0, 2.0,
+        10, 10, 5, 10, 5.0, 2.0,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+# motion_length: 140 
+
+# tau_limit: [88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                   139.0, 139.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                                       25.0, 25.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0
+#            ]
+
+# kp_lab: [40.179, 40.179, 40.179, 99.098, 99.098, 28.501, 
+#           40.179, 40.179, 28.501, 99.098, 99.098, 14.251, 
+#           14.251, 28.501, 28.501, 14.251, 14.251, 28.501, 
+#           28.501, 14.251, 14.251, 14.251, 14.251, 14.251, 
+#           14.251, 16.778, 16.778, 16.778, 16.778]
+
+# kd_lab: [2.558, 2.558, 2.558, 6.309, 6.309, 1.814, 
+#           2.558, 2.558, 1.814, 6.309, 6.309, 0.907, 
+#           0.907, 1.814, 1.814, 0.907, 0.907, 1.814, 
+#           1.814, 0.907, 0.907, 0.907, 0.907, 0.907, 
+#           0.907, 1.068, 1.068, 1.068, 1.068]
+
+# default_angles_lab: [-0.312, -0.312, 0.0, 0.0, 0.0, 0.0, 
+#                     0.0, 0.0, 0.0, 0.669, 0.669, 0.2, 
+#                     0.2, -0.363, -0.363, 0.2, -0.2, 0.0, 
+#                     0.0, 0.0, 0.0, 0.6, 0.6, 0.0, 
+#                     0.0, 0.0, 0.0, 0.0, 0.0]
+
+# action_scale_lab: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 
+#                   0.548, 0.548, 0.439, 0.351, 0.351, 0.439, 
+#                   0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 
+#                   0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 
+#                   0.439, 0.075, 0.075, 0.075, 0.075]
+
+num_actions: 19
+motor_nums: 29
+# num_obs: 154
+hold_final_reference: false
+
+# mj2lab: [0, 6, 12,
+#                   1, 7, 13,
+#                   2, 8, 14,
+#                   3, 9    , 15, 22,
+#                   4, 10   , 16, 23,
+#                   5, 11   , 17, 24,
+#                             18, 25,
+#                             19, 26,
+#                             20, 27,
+#                             21, 28]
+# history_length: 4
+physical_dt: 0.01
+decimation: 1

Deploy_Tienkung/policy/beyond_mimic/fsm_beyond_mimic.py

@@ -0,0 +1,547 @@
+from FSM.fsm_base import FSMState, FSMStateName
+import numpy as np
+import yaml
+import os
+from types import SimpleNamespace
+from typing import Optional
+try:
+    import onnx
+except ImportError:  # pragma: no cover - runtime fallback when onnx isn't installed
+    onnx = None
+import onnxruntime
+try:
+    import torch
+except ImportError:  # pragma: no cover - torch is optional for warm start prep
+    torch = None
+from common.robot_data import RobotData
+from common.joystick import ControlFlag
+import time
+
+
+DEFAULT_BODY_NAMES = [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "shoulder_roll_l_link",
+    "shoulder_yaw_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "shoulder_roll_r_link",
+    "shoulder_yaw_r_link",
+    "elbow_pitch_r_link",
+]
+
+class FSMStateBeyondMimic(FSMState):
+    def __init__(self, robot_data: RobotData, config_path: Optional[str] = None, variant_name: str = "default"):
+        super().__init__(robot_data)
+        self.motion_phase = 0
+        self.counter_step = 0
+        self.ref_motion_phase = 0
+        self.variant_name = variant_name
+        
+        
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "BeyondMimic.yaml")
+        self.config_path = os.path.abspath(config_path)
+        with open(self.config_path, "r") as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+            # 兼容多策略：模型路径仍然默认指向 policy/beyond_mimic/model 下
+            self.onnx_path = config["onnx_path"]
+            if not os.path.isabs(self.onnx_path):
+                self.onnx_path = os.path.join(current_dir, "model", self.onnx_path)
+            # self.motion_length = config["motion_length"]
+            # self.history_length = config["history_length"]
+            self.physical_dt = config["physical_dt"]
+            self.decimation_ = config["decimation"]
+            self.num_actions = config["num_actions"]
+            self.motor_nums = config["motor_nums"]
+            self.warm_start_time = config["warm_start_time"]
+            self.kps = config["kps"]
+            self.kds = config["kds"]
+            self.hold_final_reference = config.get("hold_final_reference", False)
+            self.motion_length: Optional[int] = config.get("motion_length")
+            self.body_names = config.get("body_names", DEFAULT_BODY_NAMES)
+            self.locked_joint_map = config["locked_joint_map"]
+            self.anchor_body_name = config.get("anchor_body", "pelvis")
+            if self.anchor_body_name not in self.body_names:
+                raise ValueError(f"Anchor body {self.anchor_body_name} missing from body list.")
+            self.anchor_body_index = self.body_names.index(self.anchor_body_name)
+            self.num_bodies = len(self.body_names)
+            self._warm_start_from_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_to_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_prev_target = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.last_run_time = time.perf_counter()
+            
+            self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)  # 初始化为最大可能大小
+            self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+            self.num_obs = None  # set after loading onnx
+            self.obs = None
+            self.action = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.ref_joint_pos = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_joint_vel = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_body_pos_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_quat_w = np.zeros((1, self.num_bodies, 4), dtype=np.float32)
+            self.ref_body_lin_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_ang_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            # load policy
+            self.onnx_model = None
+            metadata_props = []
+            if onnx is not None and hasattr(onnx, "load"):
+                try:
+                    self.onnx_model = onnx.load(self.onnx_path)
+                    metadata_props = getattr(self.onnx_model, "metadata_props", [])
+                except Exception as exc:
+                    print(f"[FSMStateBeyondMimic] Failed to load ONNX model via onnx.load: {exc}")
+            else:
+                print("[FSMStateBeyondMimic] Python onnx package unavailable, falling back to onnxruntime metadata.")
+
+            self.ort_session = onnxruntime.InferenceSession(self.onnx_path)
+            if not metadata_props:
+                model_meta = self.ort_session.get_modelmeta()
+                custom_map = getattr(model_meta, "custom_metadata_map", {})
+                metadata_props = [SimpleNamespace(key=k, value=v) for k, v in custom_map.items()]
+
+            input = self.ort_session.get_inputs()
+            self.input_name = []
+            for i, inpt in enumerate(input):
+                self.input_name.append(inpt.name)
+            obs_input = self.ort_session.get_inputs()[0]
+            last_dim = obs_input.shape[-1]
+            if isinstance(last_dim, int):
+                self.num_obs = last_dim
+            else:
+                self.num_obs = config.get("num_obs", 154)
+            self.obs = np.zeros(self.num_obs, dtype=np.float32)
+
+            # 从ONNX模型中读取参数
+            self.joint_seq = None
+            self.joint_pos_array_seq = None
+            self.action_scale = None
+            # self.stiffness_array_seq = None
+            # self.damping_array_seq = None
+            
+            for prop in metadata_props:
+                if prop.key == "joint_names":
+                    self.joint_seq = prop.value.split(",")
+                if prop.key == "default_joint_pos":   
+                    self.joint_pos_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_stiffness":
+                #     self.stiffness_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_damping":
+                #     self.damping_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key == "action_scale":
+                    self.action_scale = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key in ("motion_length", "time_step_total"):
+                    try:
+                        self.motion_length = int(float(prop.value))
+                    except (TypeError, ValueError):
+                        print(f"[FSMStateBeyondMimic] Invalid motion_length metadata value: {prop.value}")
+
+            if self.motion_length is not None:
+                try:
+                    self.motion_length = int(self.motion_length)
+                except (TypeError, ValueError):
+                    print(f"[FSMStateBeyondMimic] Invalid motion_length config value: {self.motion_length}")
+                    self.motion_length = None
+            
+            # # 根据YAML配置设置关节映射
+            # self.mj2lab = np.array(config["mj2lab"], dtype=np.int32)
+            
+            # 设置从序列到实验室顺序的映射
+            self.joint_xml = [
+                "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+                "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+                "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+                "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+                "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+                "shoulder_pitch_l_joint",
+                "elbow_pitch_l_joint",
+                "shoulder_pitch_r_joint",
+                "elbow_pitch_r_joint", 
+            ]
+            # 从MjXUML顺序映射到实验室顺序
+            self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+
+            # 从实验室顺序映射到MjXUML顺序
+            self.joint_pos_array = np.array([self.joint_pos_array_seq[self.joint_seq.index(joint)] for joint in self.joint_xml])
+            self.default_angles_lab = self.joint_pos_array_seq
+            self.action_scale_lab = self.action_scale
+
+            print("BeyondMimic-like policy initializing ...")
+            self._warmup_inference_counter = 0
+            self.warm_start_steps = 0
+            # Cache for the last motion frame so we can keep sending it after motion ends.
+            self._final_ref_cached = False
+            self._final_ref_joint_pos = np.zeros_like(self.ref_joint_pos)
+            self._final_ref_joint_vel = np.zeros_like(self.ref_joint_vel)
+            self._final_ref_body_pos_w = np.zeros_like(self.ref_body_pos_w)
+            self._final_ref_body_quat_w = np.zeros_like(self.ref_body_quat_w)
+            self._final_ref_body_lin_vel_w = np.zeros_like(self.ref_body_lin_vel_w)
+            self._final_ref_body_ang_vel_w = np.zeros_like(self.ref_body_ang_vel_w)
+    
+    def on_enter(self):
+        self.ref_motion_phase = 0.
+        self.motion_time = 0
+        self.counter_step = 0
+        self._warmup_inference_counter = 0
+        print(f"[FSMStateBeyondMimic] enter variant={self.variant_name}, config={self.config_path}")
+        if self.warm_start_time > 0:
+            step = self.decimation_ * self.physical_dt
+            self.warm_start_steps = max(1, int(self.warm_start_time / step))
+        else:
+            self.warm_start_steps = 0
+
+        observation = {}
+        observation[self.input_name[0]] = np.zeros((1, self.num_obs), dtype=np.float32)
+        observation[self.input_name[1]] = np.zeros((1, 1), dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.obs = np.zeros(self.num_obs)
+        self._final_ref_cached = False
+        self._warm_start_from_lab = self._get_current_joint_pos_lab()
+        self._warm_start_to_lab = self._get_onnx_first_frame_lab()
+        self._warm_start_prev_target = np.array(self._warm_start_from_lab, copy=True)
+
+        # self.action = np.zeros(self.num_actions)
+
+        pass
+        
+    def quat_mul(self, q1, q2):
+        w1, x1, y1, z1 = q1[0], q1[1], q1[2], q1[3]
+        w2, x2, y2, z2 = q2[0], q2[1], q2[2], q2[3]
+        # perform multiplication
+        ww = (z1 + x1) * (x2 + y2)
+        yy = (w1 - y1) * (w2 + z2)
+        zz = (w1 + y1) * (w2 - z2)
+        xx = ww + yy + zz
+        qq = 0.5 * (xx + (z1 - x1) * (x2 - y2))
+        w = qq - ww + (z1 - y1) * (y2 - z2)
+        x = qq - xx + (x1 + w1) * (x2 + w2)
+        y = qq - yy + (w1 - x1) * (y2 + z2)
+        z = qq - zz + (z1 + y1) * (w2 - x2)
+        return np.array([w, x, y, z])
+        
+    def matrix_from_quat(self, q):
+        w, x, y, z = q
+        return np.array([
+            [1 - 2 * (y**2 + z**2), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x**2 + z**2), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x**2 + y**2)]
+        ])
+
+    def yaw_quat(self, q):
+        w, x, y, z = q
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
+        return np.array([np.cos(yaw / 2), 0, 0, np.sin(yaw / 2)])
+    
+    def euler_single_axis_to_quat(self, angle, axis, degrees=False):
+        """
+        将单个欧拉角转换为四元数
+        
+        参数:
+            angle: 旋转角度
+            axis: 旋转轴，可以是 'x', 'y', 'z' 或者单位向量 [x, y, z]
+            degrees: 如果为True，输入角度为度数；如果为False，输入角度为弧度
+        
+        返回:
+            四元数 (w, x, y, z)
+        """
+        # 转换角度为弧度
+        if degrees:
+            angle = np.radians(angle)
+        
+        # 计算半角
+        half_angle = angle * 0.5
+        cos_half = np.cos(half_angle)
+        sin_half = np.sin(half_angle)
+        
+        # 根据旋转轴确定四元数分量
+        if isinstance(axis, str):
+            if axis.lower() == 'x':
+                return np.array([cos_half, sin_half, 0.0, 0.0])
+            elif axis.lower() == 'y':
+                return np.array([cos_half, 0.0, sin_half, 0.0])
+            elif axis.lower() == 'z':
+                return np.array([cos_half, 0.0, 0.0, sin_half])
+            else:
+                raise ValueError("axis must be 'x', 'y', 'z' or a 3D unit vector")
+        else:
+            # 假设axis是一个3D向量 [x, y, z]
+            axis = np.array(axis, dtype=np.float32)
+            # 归一化轴向量
+            axis_norm = np.linalg.norm(axis)
+            if axis_norm == 0:
+                raise ValueError("axis vector cannot be zero")
+            axis = axis / axis_norm
+            
+            # 计算四元数分量
+            w = cos_half
+            x = sin_half * axis[0]
+            y = sin_half * axis[1]
+            z = sin_half * axis[2]
+            
+            return np.array([w, x, y, z])
+
+    def inner_run(self):
+        robot_quat = self.robot_data_.get_robot_quat()
+        qj = self.robot_data_.get_joint_pos()
+        # 将29dof自由度的数据映射回锁住腕部6关节，之后的逻辑和和之前没区别
+        qj = qj[self.locked_joint_map]
+
+        qj = qj[self.mj2lab]
+        qj = (qj - self.default_angles_lab)
+
+        # IMU mounted on pelvis, so directly use measured orientation.
+        ref_anchor_pos_w, ref_anchor_ori_w = self._get_ref_anchor_pose()
+
+        # 在第一帧提取当前机器人yaw方向，与参考动作yaw方向做差（与beyond mimic一致）
+        if(self.counter_step < 2):
+            init_to_anchor = self.matrix_from_quat(self.yaw_quat(ref_anchor_ori_w))
+            world_to_anchor = self.matrix_from_quat(self.yaw_quat(robot_quat))
+            self.init_to_world = world_to_anchor @ init_to_anchor.T
+            print("self.init_to_world: ", self.init_to_world)
+            self.counter_step += 1
+            return
+
+        robot_rot_mat = self.matrix_from_quat(robot_quat)
+        motion_anchor_ori_b = robot_rot_mat.T @ self.init_to_world @ self.matrix_from_quat(ref_anchor_ori_w)
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+
+        dqj = self.robot_data_.get_joint_vel()
+        #映射到23dof
+        dqj = dqj[self.locked_joint_map]
+
+        use_warmstart = (
+            self.warm_start_steps > 0
+            and self._warmup_inference_counter < self.warm_start_steps
+        )
+        blended_target = None
+        if use_warmstart:
+            blend = (self._warmup_inference_counter + 1) / self.warm_start_steps
+            blended_target = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+            blended_vel = (blended_target - self._warm_start_prev_target) / (
+                self.decimation_ * self.physical_dt
+            )
+            self._warm_start_prev_target = blended_target
+
+            command_joint_pos = blended_target.reshape(1, -1)
+            command_joint_vel = blended_vel.reshape(1, -1)
+            safe_scale = np.where(self.action_scale_lab == 0, 1.0, self.action_scale_lab)
+            action_for_history = (blended_target - self.default_angles_lab) / safe_scale
+        else:
+            command_joint_pos = self.ref_joint_pos
+            command_joint_vel = self.ref_joint_vel
+            action_for_history = self.action
+
+        command_root = self.matrix_from_quat(ref_anchor_ori_w)
+        command_vec = np.concatenate(
+            (
+                command_joint_pos.squeeze(0),
+                command_joint_vel.squeeze(0),
+                # ref_anchor_pos_w,
+                # command_root[:, :2].reshape(-1),
+            ),
+            dtype=np.float32,
+        )
+
+        mimic_obs_buf = np.concatenate(
+            (
+                command_vec,
+                motion_anchor_ori_b[:, :2].reshape(-1),
+                ang_vel,
+                qj,
+                dqj[self.mj2lab],
+                np.asarray(action_for_history, dtype=np.float32).reshape(-1),
+            ),
+            axis=-1,
+            dtype=np.float32,
+        )
+        if mimic_obs_buf.shape[0] != self.num_obs:
+            raise RuntimeError(f"Observation length mismatch. Expected {self.num_obs}, got {mimic_obs_buf.shape[0]}.")
+        
+        if torch is not None:
+            mimic_obs_tensor = torch.from_numpy(mimic_obs_buf).unsqueeze(0).cpu().numpy()
+        else:
+            mimic_obs_tensor = np.expand_dims(mimic_obs_buf, axis=0)
+        observation = {}
+
+        # obs0 是网络观测，obs1 是当前时间步，用于输出参考动作信息
+        observation[self.input_name[0]] = mimic_obs_tensor
+        time_index = max(self.counter_step - self.warm_start_steps, 0)
+        
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if self.motion_length is not None and self.motion_length > 0:
+                time_index = min(time_index, self.motion_length - 1)
+
+        observation[self.input_name[1]] = np.array([[time_index]], dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if time_index == self.motion_length - 1 and not self._final_ref_cached:
+                self._cache_final_ref()
+            elif self.counter_step >= self.motion_length and self._final_ref_cached:
+                self._apply_final_ref()
+        target_dof_pos_mj = np.zeros(29)
+        target_dof_pos_mj_19dof = np.zeros(19)
+        if use_warmstart and blended_target is not None:
+            target_dof_pos_lab = blended_target
+            # Keep action history aligned with the inserted warm trajectory.
+            self.action = np.asarray(action_for_history, dtype=np.float32).reshape(1, -1)
+        else:
+            target_dof_pos_lab = self.action * self.action_scale_lab + self.default_angles_lab
+            if target_dof_pos_lab.ndim > 1:
+                target_dof_pos_lab = np.squeeze(target_dof_pos_lab, axis=0)
+
+        if self.warm_start_steps > 0:
+            self._warmup_inference_counter += 1
+            if self._warmup_inference_counter <= self.warm_start_steps:
+                blend = self._warmup_inference_counter / self.warm_start_steps
+                if not use_warmstart:
+                    target_dof_pos_lab = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+
+        target_dof_pos_mj_19dof[self.mj2lab] = target_dof_pos_lab
+        target_dof_pos_mj[self.locked_joint_map] = target_dof_pos_mj_19dof
+
+        # Set joint commands exactly like C++
+        for i in range(self.motor_nums):
+            # C++: robot_data_->q_d_(35 - motor_num_ + i)
+            joint_idx = 35 - self.motor_nums + i
+            self.robot_data_.q_d_[joint_idx] = target_dof_pos_mj[i]
+            self.robot_data_.q_dot_d_[joint_idx] = 0.0
+            self.robot_data_.tau_d_[joint_idx] = 0.0
+
+        # update motion phase
+        self.counter_step += 1
+
+    def _cache_final_ref(self):
+        if not self.hold_final_reference:
+            return
+        self._final_ref_cached = True
+        self._final_ref_joint_pos = np.array(self.ref_joint_pos, copy=True)
+        self._final_ref_joint_vel = np.array(self.ref_joint_vel, copy=True)
+        self._final_ref_body_pos_w = np.array(self.ref_body_pos_w, copy=True)
+        self._final_ref_body_quat_w = np.array(self.ref_body_quat_w, copy=True)
+        self._final_ref_body_lin_vel_w = np.array(self.ref_body_lin_vel_w, copy=True)
+        self._final_ref_body_ang_vel_w = np.array(self.ref_body_ang_vel_w, copy=True)
+
+    def _apply_final_ref(self):
+        if not self.hold_final_reference or not self._final_ref_cached:
+            return
+        self.ref_joint_pos = np.array(self._final_ref_joint_pos, copy=True)
+        self.ref_joint_vel = np.array(self._final_ref_joint_vel, copy=True)
+        self.ref_body_pos_w = np.array(self._final_ref_body_pos_w, copy=True)
+        self.ref_body_quat_w = np.array(self._final_ref_body_quat_w, copy=True)
+        self.ref_body_lin_vel_w = np.array(self._final_ref_body_lin_vel_w, copy=True)
+        self.ref_body_ang_vel_w = np.array(self._final_ref_body_ang_vel_w, copy=True)
+
+    def run(self, flag: ControlFlag):
+        if int(self.robot_data_.time_now_ / self.physical_dt) % self.decimation_ == 0:
+            current_time = time.perf_counter()
+            print(f"Inference hz: {1/(current_time - self.last_run_time)}")
+            self.last_run_time = current_time
+            self.inner_run()
+        self.set_kp_kd()
+    def set_kp_kd(self):
+        # Set kp/kd gains
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+    def on_exit(self):
+        self.action = np.zeros(self.num_actions, dtype=np.float32)
+        # self.action_buf = np.zeros(23 * self.history_length, dtype=np.float32)
+        self.ref_motion_phase = 0.
+        # self.ref_motion_phase_buf = np.zeros(1 * self.history_length, dtype=np.float32)
+        self.motion_time = 0
+        self.counter_step = 0
+        self._final_ref_cached = False
+        
+        print("exited")
+
+    
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        else:
+            return None  # 无状态转换
+
+    def _get_ref_anchor_pose(self):
+        ref_pos = self.ref_body_pos_w[:, self.anchor_body_index].squeeze(0)
+        ref_quat = self.ref_body_quat_w[:, self.anchor_body_index].squeeze(0)
+        return ref_pos.astype(np.float32), ref_quat.astype(np.float32)
+
+    def _get_current_joint_pos_lab(self) -> np.ndarray:
+        try:
+            current_q = self.robot_data_.get_joint_pos()
+            current_q = current_q[self.locked_joint_map]
+            current_q_lab = current_q[self.mj2lab]
+            return current_q_lab.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read current joint pose: {exc}")
+            return np.array(self.default_angles_lab, copy=True)
+
+    def _get_onnx_first_frame_lab(self) -> np.ndarray:
+        try:
+            action = self.action
+            if action is None:
+                raise RuntimeError("ONNX action output is None")
+            if action.ndim > 1:
+                action = np.squeeze(action, axis=0)
+            first_frame = action * self.action_scale_lab + self.default_angles_lab
+            return first_frame.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read ONNX first frame: {exc}")
+            return np.array(self.default_angles_lab, copy=True)

Deploy_Tienkung/policy/beyondzero/__init__.py

@@ -0,0 +1,2 @@
+"""BeyondZero policy package."""
+

Deploy_Tienkung/policy/beyondzero/config/beyondzero.yaml

@@ -0,0 +1,30 @@
+onnx_path: "policy_gui6_1.onnx"
+
+motor_nums: 29
+interp_step: 0.01
+interp_max: 1.0
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 16, 17, 18,
+ 22, 23, 24, 25
+]
+
+kps: [
+        400, 400, 250, 350, 30, 16.8,
+        400, 400, 250, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,150,200,200,
+        150, 50, 50, 150,150,200,200,
+]
+
+kds: [
+        10, 10, 5, 10, 2.5, 1.4,
+        10, 10, 5, 10, 2.5, 1.4,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+

Deploy_Tienkung/policy/beyondzero/fsm_beyondzero.py

@@ -0,0 +1,121 @@
+"""
+BeyondZero FSM state
+Moves the robot to the BeyondMimic default pose using smooth interpolation.
+"""
+import os
+from typing import List
+
+import numpy as np
+import yaml
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateBeyondZero(FSMState):
+    """Zero pose specifically aligned with the BeyondMimic policy, with explicitly specified 29-joint target positions."""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.BEYONDZERO
+        self.q_factor = 0.0
+        self.motor_nums = 29
+        self.start_pose = np.zeros(self.motor_nums, dtype=np.float32)
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "beyondzero.yaml")
+        with open(config_path, "r") as f:
+            config = yaml.safe_load(f)
+
+        self.motor_nums: int = int(config["motor_nums"])
+        self.locked_joint_map: List[int] = config["locked_joint_map"]
+        self.kps = np.array(config["kps"], dtype=np.float32)
+        self.kds = np.array(config["kds"], dtype=np.float32)
+        self.interp_step = float(config.get("interp_step", 0.001))
+        self.interp_max = float(config.get("interp_max", 1.0))
+
+        # Explicitly define the 29-joint zero target positions
+        # These values are based on the default joint positions expanded to 29 elements.
+        # Adjust the array below as needed to specify your desired initial target positions for all 29 joints.
+        # For example, positions for controllable joints are set from defaults, others to 0.0.
+        self.zero_target = np.array([
+            -0.94777486,  # hip_pitch_l_joint
+            0.15228635,   # hip_roll_l_joint
+            -0.11064088,   # hip_yaw_l_joint
+            0.70493567,   # knee_pitch_l_joint
+            -0.07427792,  # ankle_pitch_l_joint
+            -0.08085743,   # ankle_roll_l_joint
+            -0.94510548,  # hip_pitch_r_joint
+            -0.19115149,   # hip_roll_r_joint
+            0.01340432,   # hip_yaw_r_joint
+            0.79190012,   # knee_pitch_r_joint
+            -0.19874191,  # ankle_pitch_r_joint
+            0.02820061,   # ankle_roll_r_joint
+            -0.0,   # waist_yaw_joint
+            -0.0,   # waist_roll_joint
+            0.17578462,   # waist_pitch_joint
+            -1.01426013,   # shoulder_pitch_l_joint
+            0.000,  # shoulder_roll_l_joint (adjusted example)
+            0.000,   # shoulder_yaw_l_joint
+            -1.78047789,   # elbow_pitch_l_joint
+            0.0,
+            0.0,
+            0.0,
+            -1.01426013,   # shoulder_pitch_r_joint
+            0.000,   # shoulder_roll_r_joint
+            0.000,   # shoulder_yaw_r_joint
+            -1.78047789,   # elbow_pitch_r_joint
+            0.000,   # Additional joints (e.g., wrists or others, set to 0.0)
+            0.000,
+            0.000    # Last joint
+        ], dtype=np.float32)
+
+        # If preferred, load from config instead of hardcoding:
+        # self.zero_target = np.array(config.get("zero_target", self.zero_target.tolist()), dtype=np.float32)
+
+        if len(self.zero_target) != self.motor_nums:
+            raise ValueError(f"Specified zero_target length ({len(self.zero_target)}) does not match motor_nums ({self.motor_nums}).")
+
+    def on_enter(self):
+        print("[FSMStateBeyondZero] Enter zero pose with specified 29-joint targets")
+        self.q_factor = 0.0
+        if self.robot_data_ is not None:
+            self.start_pose = self.robot_data_.get_joint_pos().copy()
+        else:
+            self.start_pose = np.zeros(self.motor_nums, dtype=np.float32)
+
+    def run(self, flag: ControlFlag):
+        if self.robot_data_ is None:
+            return
+
+        target = self.zero_target
+        if self.q_factor < self.interp_max:
+            pos_cmd = (1.0 - self.q_factor) * self.start_pose + self.q_factor * target
+            self.q_factor = min(self.q_factor + self.interp_step, self.interp_max)
+        else:
+            pos_cmd = target
+
+        joint_start_idx = 35 - self.motor_nums
+        self.robot_data_.q_d_[joint_start_idx:] = pos_cmd
+        self.robot_data_.q_dot_d_[joint_start_idx:] = 0.0
+        self.robot_data_.tau_d_[joint_start_idx:] = 0.0
+
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+
+    def on_exit(self):
+        print("[FSMStateBeyondZero] Exit BeyondZero state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """Allow transitions to other FSM states."""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None

Deploy_Tienkung/policy/mypolicy/config/mypolicy.yaml

@@ -0,0 +1,96 @@
+model_path: "policy.onnx"
+motor_num: 29
+actions_size: 23
+dt: 0.01
+warm_start_time: 0.0
+xsense_data_roll_offset: 0.0
+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
+]
+zero_pos_offset: [
+      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, 0.0, 0.0, 0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6,
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6,
+          3.207, 3.207, 3.207,
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3,
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+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
+  obs_scales:
+    lin_vel: 1.0
+    ang_vel: 1.0
+    dof_pos: 1.0
+    dof_vel: 1.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/mypolicy/fsm_mypolicy.py

@@ -0,0 +1,373 @@
+"""
+FSM state implementation for the standalone MYPOLICY controller.
+"""
+
+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.BasicFunction import gait_phase
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateMYPOLICY(FSMState):
+    """Standalone FSM implementation for the custom ONNX policy."""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.MYPOLICY
+        self.log_prefix = "FSMStateMYPOLICY"
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "mypolicy.yaml")
+        with open(config_path, "r") as f:
+            policy_config = yaml.safe_load(f)
+
+        self.action_num_ = policy_config.get("actions_size")
+        self.motor_num_ = policy_config.get("motor_num")
+        self.dt_ = policy_config.get("dt")
+
+        size_config = policy_config.get("size", {})
+        self.num_hist_ = size_config.get("num_hist")
+        self.obs_size_ = size_config.get("observations_size")
+
+        control_config = policy_config.get("control", {})
+        self.action_scale_ = control_config.get("action_scale")
+        self.decimation_ = control_config.get("decimation")
+        self.warm_start_time_ = control_config.get(
+            "warm_start_time",
+            policy_config.get("warm_start_time", 0.3),
+        )
+
+        norm_config = policy_config.get("normalization", {})
+        clip_config = norm_config.get("clip_scales", {})
+        obs_config = norm_config.get("obs_scales", {})
+        self.clip_obs_ = clip_config.get("clip_observations", 100.0)
+        self.clip_act_ = clip_config.get("clip_actions", 100.0)
+        self.lin_vel_scale_ = obs_config.get("lin_vel")
+        self.ang_vel_scale_ = obs_config.get("ang_vel")
+        self.dof_pos_scale_ = obs_config.get("dof_pos")
+        self.dof_vel_scale_ = obs_config.get("dof_vel")
+
+        self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.proprio_hist_buf_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self._warm_start_pose = np.zeros(self.motor_num_, dtype=np.float32)
+
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+        self.timer_gait_ = 0.0
+
+        gait_config = policy_config.get("gait", {})
+        self.gait_cycle = gait_config.get("gait_cycle", 0.64)
+        self.left_phase_ratio = gait_config.get("gait_air_ratio_l", 0.6)
+        self.right_phase_ratio = gait_config.get("gait_air_ratio_r", 0.6)
+        self.left_theta_offset = gait_config.get("gait_phase_offset_l", 0.6)
+        self.right_theta_offset = gait_config.get("gait_phase_offset_r", 0.1)
+
+        step = (self.decimation_ if self.decimation_ else 1) * self.dt_
+        if self.warm_start_time_ > 0 and step > 0:
+            self._warm_start_steps = max(1, int(self.warm_start_time_ / step))
+        else:
+            self._warm_start_steps = 0
+        self._warmup_inference_counter = 0
+
+        self.waiting_for_motion = True
+        self.motion_threshold = 1e-3
+        self.hold_pose = np.zeros(self.motor_num_, dtype=np.float32)
+        self.filtered_x_speed = 0.0
+
+        self.model_path = os.path.join(current_dir, "model", policy_config["model_path"])
+        self._init_onnx_session()
+
+        joint_names = policy_config.get("joint_names")
+        if joint_names is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'joint_names' in mypolicy.yaml")
+        self.joint_seq = list(joint_names)
+
+        if self.action_scale_ is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'control.action_scale' in mypolicy.yaml")
+        if np.isscalar(self.action_scale_):
+            self.action_scale = np.full(len(self.joint_seq), float(self.action_scale_), dtype=np.float32)
+        else:
+            self.action_scale = np.array(self.action_scale_, dtype=np.float32)
+        if len(self.action_scale) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] control.action_scale length {len(self.action_scale)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        init_state_config = policy_config.get("init_state", {})
+        default_joint_angles = init_state_config.get("default_joint_angles")
+        if default_joint_angles is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'init_state.default_joint_angles' in mypolicy.yaml")
+        self.joint_pos_array_seq = np.array(default_joint_angles, dtype=np.float32)
+        if len(self.joint_pos_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] init_state.default_joint_angles length {len(self.joint_pos_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        gains_config = policy_config.get("gains", {})
+        kp_values = gains_config.get("kp")
+        kd_values = gains_config.get("kd")
+        if kp_values is None or kd_values is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'gains.kp' or 'gains.kd' in mypolicy.yaml")
+        self.stiffness_array_seq = np.array(kp_values, dtype=np.float32)
+        self.damping_array_seq = np.array(kd_values, dtype=np.float32)
+        if len(self.stiffness_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] gains.kp length {len(self.stiffness_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        if len(self.damping_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] gains.kd length {len(self.damping_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        self.joint_xml = [
+            "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+            "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+            "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+            "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+            "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+            "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+            "elbow_pitch_l_joint", "elbow_yaw_l_joint", "wrist_pitch_l_joint", "wrist_roll_l_joint",
+            "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+            "elbow_pitch_r_joint", "elbow_yaw_r_joint", "wrist_pitch_r_joint", "wrist_roll_r_joint",
+        ]
+
+        self.lab2mj = []
+        for name in self.joint_seq:
+            if name not in self.joint_xml:
+                raise ValueError(f"[FSMStateMYPOLICY] joint '{name}' from mypolicy.yaml not found in joint_xml")
+            self.lab2mj.append(self.joint_xml.index(name))
+        self.lab2mj = np.array(self.lab2mj, dtype=int)
+
+        n_mj = len(self.joint_xml)
+        self.joint_pos_array = np.zeros(n_mj, dtype=np.float32)
+        self.stiffness_array = np.zeros(n_mj, dtype=np.float32)
+        self.damping_array = np.zeros(n_mj, dtype=np.float32)
+        for lab_idx, mj_idx in enumerate(self.lab2mj):
+            self.joint_pos_array[mj_idx] = self.joint_pos_array_seq[lab_idx]
+            self.stiffness_array[mj_idx] = self.stiffness_array_seq[lab_idx]
+            self.damping_array[mj_idx] = self.damping_array_seq[lab_idx]
+
+        self.kps_lab = self.stiffness_array_seq
+        self.kds_lab = self.damping_array_seq
+        self.default_angles_lab = self.joint_pos_array_seq
+        self.action_scale_lab = self.action_scale
+
+    def _init_onnx_session(self):
+        try:
+            options = ort.SessionOptions()
+            options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+            options.intra_op_num_threads = 1
+            options.inter_op_num_threads = 1
+            options.enable_mem_pattern = False
+            options.enable_mem_reuse = True
+            self.ort_session_ = ort.InferenceSession(
+                self.model_path,
+                options,
+                providers=["CPUExecutionProvider"],
+            )
+            print(f"[{self.log_prefix}-ONNX] ONNX model loaded successfully: {self.model_path}")
+        except Exception as e:
+            print(f"[{self.log_prefix}] Failed to load ONNX model: {e}")
+            self.ort_session_ = None
+
+    def _reset_internal_state(self):
+        self.observations_.fill(0.0)
+        self.proprio_hist_buf_.fill(0.0)
+        self.last_actions_.fill(0.0)
+        self.actions_.fill(0.0)
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+        self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.joint_pos_array
+        self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+        self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+
+    def on_enter(self):
+        self._reset_internal_state()
+        print(f"[{self.log_prefix}] enter")
+        self.timer_gait_ = 0.0
+        self.waiting_for_motion = True
+        self._warmup_inference_counter = 0
+        if self.robot_data_ is not None:
+            try:
+                current_pose = self.robot_data_.get_joint_pos().copy()
+                self._warm_start_pose = current_pose
+                self.hold_pose = current_pose
+            except Exception:
+                self._warm_start_pose.fill(0.0)
+                self.hold_pose.fill(0.0)
+        else:
+            self._warm_start_pose.fill(0.0)
+            self.hold_pose.fill(0.0)
+        print(f"[{self.log_prefix}] waiting for motion command before starting policy")
+
+    def run(self, flag: ControlFlag):
+        walk_cmd = np.array(self.robot_data_.get_walk_cmd(), dtype=np.float32)
+        if self.waiting_for_motion:
+            if np.max(np.abs(walk_cmd)) <= self.motion_threshold:
+                base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+                self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.hold_pose
+                self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+                self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+                self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+                self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+                return
+            self.waiting_for_motion = False
+            self._warm_start_pose = self.robot_data_.get_joint_pos().copy()
+            self._warmup_inference_counter = 0
+            print(f"[{self.log_prefix}] motion command detected: {walk_cmd}, policy activated")
+
+        print(f"[{self.log_prefix}] run")
+        gait = gait_phase(
+            self.timer_gait_,
+            self.gait_cycle,
+            self.left_theta_offset,
+            self.right_theta_offset,
+            self.left_phase_ratio,
+            self.right_phase_ratio,
+        ).astype(np.float32)
+
+        if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
+            self.compute_observation(flag, gait)
+            self.compute_actions()
+
+            target_dof_pos_lab = self.actions_ * self.action_scale_lab + self.default_angles_lab
+            target_dof_pos_mj = self.robot_data_.get_joint_pos().copy()
+            target_dof_pos_mj[self.lab2mj] = target_dof_pos_lab
+            commanded_pos = target_dof_pos_mj
+            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_pos = (1.0 - blend) * self._warm_start_pose + blend * target_dof_pos_mj
+
+            base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+            self.robot_data_.q_d_[base:base + len(self.joint_xml)] = commanded_pos
+            self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+            self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+            self.last_actions_[:] = self.actions_
+
+        self.timer_gait_ += self.dt_
+        self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+        self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+
+    def compute_observation(self, flag: ControlFlag, gait):
+        roll, pitch, yaw = (
+            float(self.robot_data_.imu_data_[2]),
+            float(self.robot_data_.imu_data_[1]),
+            float(self.robot_data_.imu_data_[0]),
+        )
+        quat_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
+        q_xyzw = np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]], dtype=np.float32)
+        gravity_init = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.0, 0.0, -1.0], dtype=np.float32))
+
+        x_speed_command, y_speed_command, yaw_speed_command = self.robot_data_.get_walk_cmd()
+        new_filtered_x_speed = x_speed_command
+        change = new_filtered_x_speed - self.filtered_x_speed
+        change = np.clip(change, -0.005, 0.005)
+        self.filtered_x_speed = self.filtered_x_speed + change
+        command = np.array(
+            [x_speed_command, y_speed_command, yaw_speed_command],
+            dtype=np.float32,
+        )
+        print(f"Input command: {command}")
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+        q_mj = self.robot_data_.get_joint_pos()
+        dq_mj = self.robot_data_.get_joint_vel()
+        qj = q_mj[self.lab2mj] - self.default_angles_lab
+        dqj = dq_mj[self.lab2mj]
+
+        proprio = np.concatenate([
+            ang_vel,
+            gravity_init,
+            command,
+            qj,
+            dqj,
+            self.last_actions_,
+            gait,
+        ])
+
+        if self.is_first_obs_:
+            for i in range(self.num_hist_):
+                start_idx = i * self.obs_size_
+                end_idx = start_idx + self.obs_size_
+                self.proprio_hist_buf_[start_idx:end_idx] = proprio
+            self.is_first_obs_ = False
+        else:
+            shift_size = (self.num_hist_ - 1) * self.obs_size_
+            self.proprio_hist_buf_[:shift_size] = self.proprio_hist_buf_[self.obs_size_:]
+            self.proprio_hist_buf_[shift_size:] = proprio
+
+        self.observations_ = np.clip(self.proprio_hist_buf_, -self.clip_obs_, self.clip_obs_)
+
+    def compute_actions(self):
+        if self.ort_session_ is None:
+            return
+
+        try:
+            input_data = self.observations_.reshape(1, -1).astype(np.float32)
+            input_name = self.ort_session_.get_inputs()[0].name
+            outputs = self.ort_session_.run(None, {input_name: input_data})
+            output_data = outputs[0][0]
+            for i in range(self.action_num_):
+                self.actions_[i] = np.clip(output_data[i], -self.clip_act_, self.clip_act_)
+
+            if self.is_first_action_:
+                print(f"[{self.log_prefix}-ONNX] First Observation:")
+                for i in range(self.obs_size_):
+                    print(f"{self.observations_[i]:.6f} ", end="")
+                print()
+                self.is_first_action_ = False
+        except Exception as e:
+            print(f"[{self.log_prefix}] ONNX Runtime inference error: {e}")
+
+    def on_exit(self):
+        print(f"[{self.log_prefix}] exit")
+        if getattr(self, "obs_log_file", None) is not None:
+            try:
+                self.obs_log_file.flush()
+                self.obs_log_file.close()
+                print(f"[{self.log_prefix}] obs log saved to {self.obs_log_path}")
+            except Exception as e:
+                print(f"[{self.log_prefix}] failed to close obs log: {e}")
+            self.obs_log_file = None
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        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
+        if flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        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/policy/niukua/config/niukua.yaml

@@ -0,0 +1,117 @@
+# onnx_path: "policy_fightAndSports1_s1.onnx"
+# onnx_path: "policy-dance.onnx"
+onnx_path: "niukua_evt1216.onnx"
+warm_start_time: 0.0
+
+body_names : [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "elbow_pitch_r_link",
+]
+anchor_body : "pelvis"
+
+#without motion anchor pos && anchor base lin vel
+# command :23+23+3+6  55
+# motion_anchor_ori_b: 6
+# base_ang_vel: 3
+# joint_pos（相对轴角）: 23
+# joint_vel: 23
+# actions（上一时刻动作）: 23
+# total : 133
+num_obs : 133 
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 16, 17, 18, 
+ 22, 23, 24, 25 
+]
+
+kps: [
+        300, 300, 150, 350, 30, 16.8,
+        300, 300, 150, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,500,200,200,
+        150, 50, 50, 150,500,200,200,
+]
+kds: [
+        10, 10, 5, 10, 5.0, 2.0,
+        10, 10, 5, 10, 5.0, 2.0,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+# motion_length: 140 
+
+# tau_limit: [88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                   139.0, 139.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                                       25.0, 25.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0
+#            ]
+
+# kp_lab: [40.179, 40.179, 40.179, 99.098, 99.098, 28.501, 
+#           40.179, 40.179, 28.501, 99.098, 99.098, 14.251, 
+#           14.251, 28.501, 28.501, 14.251, 14.251, 28.501, 
+#           28.501, 14.251, 14.251, 14.251, 14.251, 14.251, 
+#           14.251, 16.778, 16.778, 16.778, 16.778]
+
+# kd_lab: [2.558, 2.558, 2.558, 6.309, 6.309, 1.814, 
+#           2.558, 2.558, 1.814, 6.309, 6.309, 0.907, 
+#           0.907, 1.814, 1.814, 0.907, 0.907, 1.814, 
+#           1.814, 0.907, 0.907, 0.907, 0.907, 0.907, 
+#           0.907, 1.068, 1.068, 1.068, 1.068]
+
+# default_angles_lab: [-0.312, -0.312, 0.0, 0.0, 0.0, 0.0, 
+#                     0.0, 0.0, 0.0, 0.669, 0.669, 0.2, 
+#                     0.2, -0.363, -0.363, 0.2, -0.2, 0.0, 
+#                     0.0, 0.0, 0.0, 0.6, 0.6, 0.0, 
+#                     0.0, 0.0, 0.0, 0.0, 0.0]
+
+# action_scale_lab: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 
+#                   0.548, 0.548, 0.439, 0.351, 0.351, 0.439, 
+#                   0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 
+#                   0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 
+#                   0.439, 0.075, 0.075, 0.075, 0.075]
+
+num_actions: 23
+motor_nums: 29
+# num_obs: 154
+hold_final_reference: false
+
+# mj2lab: [0, 6, 12,
+#                   1, 7, 13,
+#                   2, 8, 14,
+#                   3, 9    , 15, 22,
+#                   4, 10   , 16, 23,
+#                   5, 11   , 17, 24,
+#                             18, 25,
+#                             19, 26,
+#                             20, 27,
+#                             21, 28]
+# history_length: 4
+physical_dt: 0.01
+decimation: 1

Deploy_Tienkung/policy/niukua/fsm_beyond_mimic.py

@@ -0,0 +1,548 @@
+from FSM.fsm_base import FSMState, FSMStateName
+import numpy as np
+import yaml
+import os
+from types import SimpleNamespace
+from typing import Optional
+try:
+    import onnx
+except ImportError:  # pragma: no cover - runtime fallback when onnx isn't installed
+    onnx = None
+import onnxruntime
+try:
+    import torch
+except ImportError:  # pragma: no cover - torch is optional for warm start prep
+    torch = None
+from common.robot_data import RobotData
+from common.joystick import ControlFlag
+import time
+
+
+DEFAULT_BODY_NAMES = [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "shoulder_roll_l_link",
+    "shoulder_yaw_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "shoulder_roll_r_link",
+    "shoulder_yaw_r_link",
+    "elbow_pitch_r_link",
+]
+
+class FSMStateBeyondMimic(FSMState):
+    def __init__(self, robot_data: RobotData, config_path: Optional[str] = None, variant_name: str = "default"):
+        super().__init__(robot_data)
+        self.motion_phase = 0
+        self.counter_step = 0
+        self.ref_motion_phase = 0
+        self.variant_name = variant_name
+        
+        
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "BeyondMimic.yaml")
+        self.config_path = os.path.abspath(config_path)
+        with open(self.config_path, "r") as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+            # 兼容多策略：模型路径仍然默认指向 policy/beyond_mimic/model 下
+            self.onnx_path = config["onnx_path"]
+            if not os.path.isabs(self.onnx_path):
+                self.onnx_path = os.path.join(current_dir, "model", self.onnx_path)
+            # self.motion_length = config["motion_length"]
+            # self.history_length = config["history_length"]
+            self.physical_dt = config["physical_dt"]
+            self.decimation_ = config["decimation"]
+            self.num_actions = config["num_actions"]
+            self.motor_nums = config["motor_nums"]
+            self.warm_start_time = config["warm_start_time"]
+            self.kps = config["kps"]
+            self.kds = config["kds"]
+            self.hold_final_reference = config.get("hold_final_reference", False)
+            self.motion_length: Optional[int] = config.get("motion_length")
+            self.body_names = config.get("body_names", DEFAULT_BODY_NAMES)
+            self.locked_joint_map = config["locked_joint_map"]
+            self.anchor_body_name = config.get("anchor_body", "pelvis")
+            if self.anchor_body_name not in self.body_names:
+                raise ValueError(f"Anchor body {self.anchor_body_name} missing from body list.")
+            self.anchor_body_index = self.body_names.index(self.anchor_body_name)
+            self.num_bodies = len(self.body_names)
+            self._warm_start_from_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_to_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_prev_target = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.last_run_time = time.perf_counter()
+            
+            self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)  # 初始化为最大可能大小
+            self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+            self.num_obs = None  # set after loading onnx
+            self.obs = None
+            self.action = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.ref_joint_pos = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_joint_vel = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_body_pos_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_quat_w = np.zeros((1, self.num_bodies, 4), dtype=np.float32)
+            self.ref_body_lin_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_ang_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            # load policy
+            self.onnx_model = None
+            metadata_props = []
+            if onnx is not None and hasattr(onnx, "load"):
+                try:
+                    self.onnx_model = onnx.load(self.onnx_path)
+                    metadata_props = getattr(self.onnx_model, "metadata_props", [])
+                except Exception as exc:
+                    print(f"[FSMStateBeyondMimic] Failed to load ONNX model via onnx.load: {exc}")
+            else:
+                print("[FSMStateBeyondMimic] Python onnx package unavailable, falling back to onnxruntime metadata.")
+
+            self.ort_session = onnxruntime.InferenceSession(self.onnx_path)
+            if not metadata_props:
+                model_meta = self.ort_session.get_modelmeta()
+                custom_map = getattr(model_meta, "custom_metadata_map", {})
+                metadata_props = [SimpleNamespace(key=k, value=v) for k, v in custom_map.items()]
+
+            input = self.ort_session.get_inputs()
+            self.input_name = []
+            for i, inpt in enumerate(input):
+                self.input_name.append(inpt.name)
+            obs_input = self.ort_session.get_inputs()[0]
+            last_dim = obs_input.shape[-1]
+            if isinstance(last_dim, int):
+                self.num_obs = last_dim
+            else:
+                self.num_obs = config.get("num_obs", 154)
+            self.obs = np.zeros(self.num_obs, dtype=np.float32)
+
+            # 从ONNX模型中读取参数
+            self.joint_seq = None
+            self.joint_pos_array_seq = None
+            self.action_scale = None
+            # self.stiffness_array_seq = None
+            # self.damping_array_seq = None
+            
+            for prop in metadata_props:
+                if prop.key == "joint_names":
+                    self.joint_seq = prop.value.split(",")
+                if prop.key == "default_joint_pos":   
+                    self.joint_pos_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_stiffness":
+                #     self.stiffness_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_damping":
+                #     self.damping_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key == "action_scale":
+                    self.action_scale = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key in ("motion_length", "time_step_total"):
+                    try:
+                        self.motion_length = int(float(prop.value))
+                    except (TypeError, ValueError):
+                        print(f"[FSMStateBeyondMimic] Invalid motion_length metadata value: {prop.value}")
+
+            if self.motion_length is not None:
+                try:
+                    self.motion_length = int(self.motion_length)
+                except (TypeError, ValueError):
+                    print(f"[FSMStateBeyondMimic] Invalid motion_length config value: {self.motion_length}")
+                    self.motion_length = None
+            
+            # # 根据YAML配置设置关节映射
+            # self.mj2lab = np.array(config["mj2lab"], dtype=np.int32)
+            
+            # 设置从序列到实验室顺序的映射
+            self.joint_xml = [
+                "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+                "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+                "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+                "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+                "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+                "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+                "elbow_pitch_l_joint",
+                "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+                "elbow_pitch_r_joint",
+            ]
+            # 从MjXUML顺序映射到实验室顺序
+            self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+
+            # 从实验室顺序映射到MjXUML顺序
+            self.joint_pos_array = np.array([self.joint_pos_array_seq[self.joint_seq.index(joint)] for joint in self.joint_xml])
+
+            self.default_angles_lab = self.joint_pos_array_seq
+            self.action_scale_lab = self.action_scale
+
+            print("BeyondMimic-like policy initializing ...")
+            self._warmup_inference_counter = 0
+            self.warm_start_steps = 0
+            # Cache for the last motion frame so we can keep sending it after motion ends.
+            self._final_ref_cached = False
+            self._final_ref_joint_pos = np.zeros_like(self.ref_joint_pos)
+            self._final_ref_joint_vel = np.zeros_like(self.ref_joint_vel)
+            self._final_ref_body_pos_w = np.zeros_like(self.ref_body_pos_w)
+            self._final_ref_body_quat_w = np.zeros_like(self.ref_body_quat_w)
+            self._final_ref_body_lin_vel_w = np.zeros_like(self.ref_body_lin_vel_w)
+            self._final_ref_body_ang_vel_w = np.zeros_like(self.ref_body_ang_vel_w)
+    
+    def on_enter(self):
+        self.ref_motion_phase = 0.
+        self.motion_time = 0
+        self.counter_step = 0
+        self._warmup_inference_counter = 0
+        print(f"[FSMStateBeyondMimic] enter variant={self.variant_name}, config={self.config_path}")
+        if self.warm_start_time > 0:
+            step = self.decimation_ * self.physical_dt
+            self.warm_start_steps = max(1, int(self.warm_start_time / step))
+        else:
+            self.warm_start_steps = 0
+
+        observation = {}
+        observation[self.input_name[0]] = np.zeros((1, self.num_obs), dtype=np.float32)
+        observation[self.input_name[1]] = np.zeros((1, 1), dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.obs = np.zeros(self.num_obs)
+        self._final_ref_cached = False
+        self._warm_start_from_lab = self._get_current_joint_pos_lab()
+        self._warm_start_to_lab = self._get_onnx_first_frame_lab()
+        self._warm_start_prev_target = np.array(self._warm_start_from_lab, copy=True)
+
+        # self.action = np.zeros(self.num_actions)
+
+        pass
+        
+    def quat_mul(self, q1, q2):
+        w1, x1, y1, z1 = q1[0], q1[1], q1[2], q1[3]
+        w2, x2, y2, z2 = q2[0], q2[1], q2[2], q2[3]
+        # perform multiplication
+        ww = (z1 + x1) * (x2 + y2)
+        yy = (w1 - y1) * (w2 + z2)
+        zz = (w1 + y1) * (w2 - z2)
+        xx = ww + yy + zz
+        qq = 0.5 * (xx + (z1 - x1) * (x2 - y2))
+        w = qq - ww + (z1 - y1) * (y2 - z2)
+        x = qq - xx + (x1 + w1) * (x2 + w2)
+        y = qq - yy + (w1 - x1) * (y2 + z2)
+        z = qq - zz + (z1 + y1) * (w2 - x2)
+        return np.array([w, x, y, z])
+        
+    def matrix_from_quat(self, q):
+        w, x, y, z = q
+        return np.array([
+            [1 - 2 * (y**2 + z**2), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x**2 + z**2), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x**2 + y**2)]
+        ])
+
+    def yaw_quat(self, q):
+        w, x, y, z = q
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
+        return np.array([np.cos(yaw / 2), 0, 0, np.sin(yaw / 2)])
+    
+    def euler_single_axis_to_quat(self, angle, axis, degrees=False):
+        """
+        将单个欧拉角转换为四元数
+        
+        参数:
+            angle: 旋转角度
+            axis: 旋转轴，可以是 'x', 'y', 'z' 或者单位向量 [x, y, z]
+            degrees: 如果为True，输入角度为度数；如果为False，输入角度为弧度
+        
+        返回:
+            四元数 (w, x, y, z)
+        """
+        # 转换角度为弧度
+        if degrees:
+            angle = np.radians(angle)
+        
+        # 计算半角
+        half_angle = angle * 0.5
+        cos_half = np.cos(half_angle)
+        sin_half = np.sin(half_angle)
+        
+        # 根据旋转轴确定四元数分量
+        if isinstance(axis, str):
+            if axis.lower() == 'x':
+                return np.array([cos_half, sin_half, 0.0, 0.0])
+            elif axis.lower() == 'y':
+                return np.array([cos_half, 0.0, sin_half, 0.0])
+            elif axis.lower() == 'z':
+                return np.array([cos_half, 0.0, 0.0, sin_half])
+            else:
+                raise ValueError("axis must be 'x', 'y', 'z' or a 3D unit vector")
+        else:
+            # 假设axis是一个3D向量 [x, y, z]
+            axis = np.array(axis, dtype=np.float32)
+            # 归一化轴向量
+            axis_norm = np.linalg.norm(axis)
+            if axis_norm == 0:
+                raise ValueError("axis vector cannot be zero")
+            axis = axis / axis_norm
+            
+            # 计算四元数分量
+            w = cos_half
+            x = sin_half * axis[0]
+            y = sin_half * axis[1]
+            z = sin_half * axis[2]
+            
+            return np.array([w, x, y, z])
+
+    def inner_run(self):
+        robot_quat = self.robot_data_.get_robot_quat()
+        qj = self.robot_data_.get_joint_pos()
+        # 将29dof自由度的数据映射回锁住腕部6关节，之后的逻辑和和之前没区别
+        qj = qj[self.locked_joint_map]
+
+        qj = qj[self.mj2lab]
+        qj = (qj - self.default_angles_lab)
+
+        # IMU mounted on pelvis, so directly use measured orientation.
+        ref_anchor_pos_w, ref_anchor_ori_w = self._get_ref_anchor_pose()
+
+        # 在第一帧提取当前机器人yaw方向，与参考动作yaw方向做差（与beyond mimic一致）
+        if(self.counter_step < 2):
+            init_to_anchor = self.matrix_from_quat(self.yaw_quat(ref_anchor_ori_w))
+            world_to_anchor = self.matrix_from_quat(self.yaw_quat(robot_quat))
+            self.init_to_world = world_to_anchor @ init_to_anchor.T
+            print("self.init_to_world: ", self.init_to_world)
+            self.counter_step += 1
+            return
+
+        robot_rot_mat = self.matrix_from_quat(robot_quat)
+        motion_anchor_ori_b = robot_rot_mat.T @ self.init_to_world @ self.matrix_from_quat(ref_anchor_ori_w)
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+
+        dqj = self.robot_data_.get_joint_vel()
+        #映射到23dof
+        dqj = dqj[self.locked_joint_map]
+
+        use_warmstart = (
+            self.warm_start_steps > 0
+            and self._warmup_inference_counter < self.warm_start_steps
+        )
+        blended_target = None
+        if use_warmstart:
+            blend = (self._warmup_inference_counter + 1) / self.warm_start_steps
+            blended_target = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+            blended_vel = (blended_target - self._warm_start_prev_target) / (
+                self.decimation_ * self.physical_dt
+            )
+            self._warm_start_prev_target = blended_target
+
+            command_joint_pos = blended_target.reshape(1, -1)
+            command_joint_vel = blended_vel.reshape(1, -1)
+            safe_scale = np.where(self.action_scale_lab == 0, 1.0, self.action_scale_lab)
+            action_for_history = (blended_target - self.default_angles_lab) / safe_scale
+        else:
+            command_joint_pos = self.ref_joint_pos
+            command_joint_vel = self.ref_joint_vel
+            action_for_history = self.action
+
+        command_root = self.matrix_from_quat(ref_anchor_ori_w)
+        command_vec = np.concatenate(
+            (
+                command_joint_pos.squeeze(0),
+                command_joint_vel.squeeze(0),
+                # ref_anchor_pos_w,
+                # command_root[:, :2].reshape(-1),
+            ),
+            dtype=np.float32,
+        )
+
+        mimic_obs_buf = np.concatenate(
+            (
+                command_vec,
+                motion_anchor_ori_b[:, :2].reshape(-1),
+                ang_vel,
+                qj,
+                dqj[self.mj2lab],
+                np.asarray(action_for_history, dtype=np.float32).reshape(-1),
+            ),
+            axis=-1,
+            dtype=np.float32,
+        )
+        if mimic_obs_buf.shape[0] != self.num_obs:
+            raise RuntimeError(f"Observation length mismatch. Expected {self.num_obs}, got {mimic_obs_buf.shape[0]}.")
+        
+        if torch is not None:
+            mimic_obs_tensor = torch.from_numpy(mimic_obs_buf).unsqueeze(0).cpu().numpy()
+        else:
+            mimic_obs_tensor = np.expand_dims(mimic_obs_buf, axis=0)
+        observation = {}
+
+        # obs0 是网络观测，obs1 是当前时间步，用于输出参考动作信息
+        observation[self.input_name[0]] = mimic_obs_tensor
+        time_index = max(self.counter_step - self.warm_start_steps, 0)
+        
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if self.motion_length is not None and self.motion_length > 0:
+                time_index = min(time_index, self.motion_length - 1)
+
+        observation[self.input_name[1]] = np.array([[time_index]], dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if time_index == self.motion_length - 1 and not self._final_ref_cached:
+                self._cache_final_ref()
+            elif self.counter_step >= self.motion_length and self._final_ref_cached:
+                self._apply_final_ref()
+        target_dof_pos_mj = np.zeros(29)
+        target_dof_pos_mj_23dof = np.zeros(23)
+        if use_warmstart and blended_target is not None:
+            target_dof_pos_lab = blended_target
+            # Keep action history aligned with the inserted warm trajectory.
+            self.action = np.asarray(action_for_history, dtype=np.float32).reshape(1, -1)
+        else:
+            target_dof_pos_lab = self.action * self.action_scale_lab + self.default_angles_lab
+            if target_dof_pos_lab.ndim > 1:
+                target_dof_pos_lab = np.squeeze(target_dof_pos_lab, axis=0)
+
+        if self.warm_start_steps > 0:
+            self._warmup_inference_counter += 1
+            if self._warmup_inference_counter <= self.warm_start_steps:
+                blend = self._warmup_inference_counter / self.warm_start_steps
+                if not use_warmstart:
+                    target_dof_pos_lab = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+
+        target_dof_pos_mj_23dof[self.mj2lab] = target_dof_pos_lab
+        target_dof_pos_mj[self.locked_joint_map] = target_dof_pos_mj_23dof
+
+        # Set joint commands exactly like C++
+        for i in range(self.motor_nums):
+            # C++: robot_data_->q_d_(35 - motor_num_ + i)
+            joint_idx = 35 - self.motor_nums + i
+            self.robot_data_.q_d_[joint_idx] = target_dof_pos_mj[i]
+            self.robot_data_.q_dot_d_[joint_idx] = 0.0
+            self.robot_data_.tau_d_[joint_idx] = 0.0
+
+        # update motion phase
+        self.counter_step += 1
+
+    def _cache_final_ref(self):
+        if not self.hold_final_reference:
+            return
+        self._final_ref_cached = True
+        self._final_ref_joint_pos = np.array(self.ref_joint_pos, copy=True)
+        self._final_ref_joint_vel = np.array(self.ref_joint_vel, copy=True)
+        self._final_ref_body_pos_w = np.array(self.ref_body_pos_w, copy=True)
+        self._final_ref_body_quat_w = np.array(self.ref_body_quat_w, copy=True)
+        self._final_ref_body_lin_vel_w = np.array(self.ref_body_lin_vel_w, copy=True)
+        self._final_ref_body_ang_vel_w = np.array(self.ref_body_ang_vel_w, copy=True)
+
+    def _apply_final_ref(self):
+        if not self.hold_final_reference or not self._final_ref_cached:
+            return
+        self.ref_joint_pos = np.array(self._final_ref_joint_pos, copy=True)
+        self.ref_joint_vel = np.array(self._final_ref_joint_vel, copy=True)
+        self.ref_body_pos_w = np.array(self._final_ref_body_pos_w, copy=True)
+        self.ref_body_quat_w = np.array(self._final_ref_body_quat_w, copy=True)
+        self.ref_body_lin_vel_w = np.array(self._final_ref_body_lin_vel_w, copy=True)
+        self.ref_body_ang_vel_w = np.array(self._final_ref_body_ang_vel_w, copy=True)
+
+    def run(self, flag: ControlFlag):
+        if int(self.robot_data_.time_now_ / self.physical_dt) % self.decimation_ == 0:
+            current_time = time.perf_counter()
+            print(f"Inference hz: {1/(current_time - self.last_run_time)}")
+            self.last_run_time = current_time
+            self.inner_run()
+        self.set_kp_kd()
+    def set_kp_kd(self):
+        # Set kp/kd gains
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+    def on_exit(self):
+        self.action = np.zeros(self.num_actions, dtype=np.float32)
+        # self.action_buf = np.zeros(23 * self.history_length, dtype=np.float32)
+        self.ref_motion_phase = 0.
+        # self.ref_motion_phase_buf = np.zeros(1 * self.history_length, dtype=np.float32)
+        self.motion_time = 0
+        self.counter_step = 0
+        self._final_ref_cached = False
+        
+        print("exited")
+
+    
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        else:
+            return None  # 无状态转换
+
+    def _get_ref_anchor_pose(self):
+        ref_pos = self.ref_body_pos_w[:, self.anchor_body_index].squeeze(0)
+        ref_quat = self.ref_body_quat_w[:, self.anchor_body_index].squeeze(0)
+        return ref_pos.astype(np.float32), ref_quat.astype(np.float32)
+
+    def _get_current_joint_pos_lab(self) -> np.ndarray:
+        try:
+            current_q = self.robot_data_.get_joint_pos()
+            current_q = current_q[self.locked_joint_map]
+            current_q_lab = current_q[self.mj2lab]
+            return current_q_lab.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read current joint pose: {exc}")
+            return np.array(self.default_angles_lab, copy=True)
+
+    def _get_onnx_first_frame_lab(self) -> np.ndarray:
+        try:
+            action = self.action
+            if action is None:
+                raise RuntimeError("ONNX action output is None")
+            if action.ndim > 1:
+                action = np.squeeze(action, axis=0)
+            first_frame = action * self.action_scale_lab + self.default_angles_lab
+            return first_frame.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read ONNX first frame: {exc}")
+            return np.array(self.default_angles_lab, copy=True)

Deploy_Tienkung/policy/stop/config/stop.yaml

@@ -0,0 +1,37 @@
+
+
+motor_num: 29        # 电机数量
+actions_size: 12     # action的大小
+dt: 0.002
+xsense_data_roll_offset: 0.0
+zero_pos_offset: [
+      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, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          3.207, 3.207, 3.207, #
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3, 
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+
+kp_pos: 
+  [1000, 2000, 2000, 1000, 50, 50, #left legs
+  1000, 2000, 2000, 1000, 50, 50,
+  1500.0, 1500.0, 1000.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0
+  ]  
+
+kd_pos: 
+  [30, 70, 70, 30, 3, 1, #left legs
+    30, 70, 70, 30, 3, 1, # right legs
+    70.0, 70.0, 50.0,  # 腰
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0,
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0
+  ]  
+

Deploy_Tienkung/policy/stop/fsm_stop.py

@@ -0,0 +1,90 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+
+import numpy as np
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+import yaml
+import os
+
+
+class FSMStateStop(FSMState):
+    """停止状态实现 - 与C++版本完全一致"""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.STOP
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "stop.yaml")
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+
+        try:
+            self.action_num_ = policy_config["actions_size"]
+            self.motor_num_ = policy_config["motor_num"]
+
+            # Initialize vectors
+            self.hold_position_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+
+            # Load kp and kd gains from config
+            for i in range(self.motor_num_):
+                self.kp_pos_[i] = policy_config["kp_pos"][i]
+                self.kd_pos_[i] = policy_config["kd_pos"][i]
+
+        except Exception as e:
+            print(f"[FSMStateStop] YAML load error: {e}")
+            # Set default values like C++
+            self.action_num_ = 12
+            self.motor_num_ = 29
+            self.hold_position_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+
+    def on_enter(self):
+        """进入停止状态 - 与C++版本完全一致"""
+        # Store the last motor positions as hold positions (equivalent to tail(motor_num_))
+        self.hold_position_ = self.robot_data_.q_a_[-self.motor_num_:].copy()
+        print("[FSMStateStop] Enter stop state")
+
+    def run(self, flag: ControlFlag):
+        """运行停止状态 - 与C++版本完全一致"""
+        if self.robot_data_ is None:
+            return
+        print(f"""[FSMStateStop] Holding position: {self.hold_position_}""")
+        # Enforce the hold position for every frame (equivalent to tail(motor_num_))
+        self.robot_data_.q_d_[-self.motor_num_:] = self.hold_position_
+        # Set desired joint velocities to zero
+        self.robot_data_.q_dot_d_[-self.motor_num_:] = 0.0
+        # Set desired torques to zero
+        self.robot_data_.tau_d_[-self.motor_num_:] = 0.0
+
+        # Set proportional and derivative gains
+        self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_pos_
+        self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_pos_
+
+    def on_exit(self):
+        """退出停止状态 - 与C++版本完全一致"""
+        self.hold_position_.fill(0.0)
+        print("[FSMStateStop] Exit stop position control state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None  # 无状态转换

Deploy_Tienkung/policy/walk_amp/config/walk_amp.yaml

@@ -0,0 +1,98 @@
+model_path: "policy.onnx" 
+motor_num: 29        # 电机数量
+actions_size: 23     # action的大小
+dt: 0.01
+warm_start_time: 0.0
+xsense_data_roll_offset: 0.0
+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
+]
+zero_pos_offset: [
+      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, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          3.207, 3.207, 3.207, #TODO:待更新 pry
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3, 
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+control:
+  # gait_cycle_period: 0.9
+  action_scale: 0.25               # 动作缩放比例，可写标量或 23 维数组
+  decimation: 1                   # 策略下发频率控制
+
+gait:
+  gait_air_ratio_l: 0.38
+  gait_air_ratio_r: 0.38
+  gait_phase_offset_l: 0.38
+  gait_phase_offset_r: 0.88
+  gait_cycle: 0.85
+
+normalization:
+  clip_scales:
+    clip_observations: 100. #18.0mlp
+    clip_actions: 100. #18.0
+  obs_scales:
+    lin_vel: 1.0                  # 线速度缩放因子
+    ang_vel: 1.0                  # 角速度缩放因子
+    dof_pos: 1.0                  # 关节位置缩放因子
+    dof_vel: 1.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.15, -0.15, 0.0,
+    0.0, 0.0, 0.0,
+    -0.0, -0.0, 0.0,
+    0.3, 0.3,
+    0.2, 0.2,
+    -0.15, -0.15,
+    0.1, -0.1,
+    0.0, 0.0,
+    0.0, 0.0,
+    -0.5, -0.5
+  ]

Deploy_Tienkung/policy/walk_amp/fsm_walkamp.py

@@ -0,0 +1,454 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+
+import numpy as np
+import onnxruntime as ort
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+from common.BasicFunction import clip_vector, gait_phase
+import os
+import yaml
+from scipy.spatial.transform import Rotation
+
+class FSMStateWALKAMP(FSMState):
+    """WALKAMP策略状态实现"""
+
+    def _reset_internal_state(self):
+        """把所有随时间变化的内部状态重置成初始值"""
+
+        # 1) 清空 obs / hist / actions
+        self.observations_.fill(0.0)
+        self.proprio_hist_buf_.fill(0.0)
+        self.last_actions_.fill(0.0)
+        self.actions_.fill(0.0)
+
+        # 2) 标志位重置
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+
+        # 3) 期望关节 / 期望速度 / 力矩重置为“初始姿态”
+        # 你已经有 self.joint_pos_array（mj 顺序，长度 len(self.joint_xml)）
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+        # 期望角 = 初始角
+        self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.joint_pos_array
+        # 期望速度 = 0
+        self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+        # 期望力矩 = 0（位置控制）
+        self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+    def __init__(self,
+                 robot_data: RobotData,
+                 config_path: str | None = None,
+                 base_dir: str | None = None,
+                 log_prefix: str = "FSMStateWALKAMP"):
+        super().__init__(robot_data)
+        self.log_prefix = log_prefix
+
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "walk_amp.yaml")
+        config_path = os.path.abspath(config_path)
+        if base_dir is None:
+            base_dir = os.path.dirname(os.path.dirname(config_path))
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+        # Load configuration exactly like C++
+        self.action_num_ = policy_config.get('actions_size')
+        self.motor_num_ = policy_config.get('motor_num')
+        self.dt_ = policy_config.get('dt')
+
+        # Size configuration
+        size_config = policy_config.get('size', {})
+        self.num_hist_ = size_config.get('num_hist')
+        self.obs_size_ = size_config.get('observations_size')
+
+        # Control configuration
+        control_config = policy_config.get('control', {})
+        self.action_scale_ = control_config.get('action_scale')
+        # self.gait_cycle_period_ = control_config.get('gait_cycle_period', 1.0)
+        self.decimation_ = control_config.get('decimation')
+        self.warm_start_time_ = control_config.get(
+            'warm_start_time',
+            policy_config.get('warm_start_time', 0.3),
+        )
+
+        # Normalization configuration
+        norm_config = policy_config.get('normalization', {})
+        clip_config = norm_config.get('clip_scales', {})
+        obs_config = norm_config.get('obs_scales', {})
+
+        self.clip_obs_ = clip_config.get('clip_observations', 100.0)
+        self.clip_act_ = clip_config.get('clip_actions', 100.0)
+        self.lin_vel_scale_ = obs_config.get('lin_vel')
+        self.ang_vel_scale_ = obs_config.get('ang_vel')
+        self.dof_pos_scale_ = obs_config.get('dof_pos')
+        self.dof_vel_scale_ = obs_config.get('dof_vel')
+
+
+        # Initialize buffers and actions
+        self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.proprio_hist_buf_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self._warm_start_pose = np.zeros(self.motor_num_, dtype=np.float32)
+
+
+        # Flags matching C++
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        # self.phase_locked = False
+        self.timer_gait_ = 0.0
+        gait_config = policy_config.get('gait', {})
+        self.gait_cycle = gait_config.get('gait_cycle', 0.85)
+        self.left_phase_ratio = gait_config.get('gait_air_ratio_l', 0.38)
+        self.right_phase_ratio = gait_config.get('gait_air_ratio_r', 0.38)
+        self.left_theta_offset = gait_config.get('gait_phase_offset_l', 0.38)
+        self.right_theta_offset = gait_config.get('gait_phase_offset_r', 0.88)
+        
+        self.is_first_step_ = True
+        step = (self.decimation_ if self.decimation_ else 1) * self.dt_
+        if self.warm_start_time_ > 0 and step > 0:
+            self._warm_start_steps = max(1, int(self.warm_start_time_ / step))
+        else:
+            self._warm_start_steps = 0
+        self._warmup_inference_counter = 0
+
+
+        # Initialize ONNX session
+        self.model_path = os.path.join(base_dir, "model", policy_config["model_path"])
+        self._init_onnx_session()
+
+        self.joint_seq = None
+        self.joint_pos_array_seq = None
+        self.action_scale = None
+        self.stiffness_array_seq = None
+        self.damping_array_seq = None
+        
+        joint_names = policy_config.get('joint_names')
+        if joint_names is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'joint_names' in walk_amp.yaml")
+
+        self.joint_seq = list(joint_names)
+
+        if self.action_scale_ is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'control.action_scale' in walk_amp.yaml")
+
+        if np.isscalar(self.action_scale_):
+            self.action_scale = np.full(len(self.joint_seq), float(self.action_scale_), dtype=np.float32)
+        else:
+            self.action_scale = np.array(self.action_scale_, dtype=np.float32)
+
+        if len(self.action_scale) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] control.action_scale length {len(self.action_scale)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        init_state_config = policy_config.get('init_state', {})
+        default_joint_angles = init_state_config.get('default_joint_angles')
+        if default_joint_angles is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'init_state.default_joint_angles' in walk_amp.yaml")
+
+        self.joint_pos_array_seq = np.array(default_joint_angles, dtype=np.float32)
+        if len(self.joint_pos_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] init_state.default_joint_angles length {len(self.joint_pos_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        gains_config = policy_config.get('gains', {})
+        kp_values = gains_config.get('kp')
+        kd_values = gains_config.get('kd')
+        if kp_values is None or kd_values is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'gains.kp' or 'gains.kd' in walk_amp.yaml")
+
+        self.stiffness_array_seq = np.array(kp_values, dtype=np.float32)
+        self.damping_array_seq = np.array(kd_values, dtype=np.float32)
+
+        if len(self.stiffness_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] gains.kp length {len(self.stiffness_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        if len(self.damping_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] gains.kd length {len(self.damping_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        # # 设置从序列到实验室顺序的映射
+        self.joint_xml = [
+            "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+            "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+            "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+            "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+            "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+            "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+            "elbow_pitch_l_joint", "elbow_yaw_l_joint", "wrist_pitch_l_joint", "wrist_roll_l_joint",
+            "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+            "elbow_pitch_r_joint", "elbow_yaw_r_joint", "wrist_pitch_r_joint", "wrist_roll_r_joint",
+        ]
+
+        # 从MjXUML顺序映射到实验室顺序
+        # self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+        self.lab2mj = []
+        for name in self.joint_seq:
+            if name not in self.joint_xml:
+                raise ValueError(f"[FSMStateWALKAMP] joint '{name}' from walk_amp.yaml not found in joint_xml!")
+            self.lab2mj.append(self.joint_xml.index(name))
+        self.lab2mj = np.array(self.lab2mj, dtype=int)
+
+        # 从实验室顺序映射到MjXUML顺序
+        # ====== 把 23 个 lab 关节 scatter 到 29 个 xml 里，多的 6 个保持默认 ======
+        n_mj = len(self.joint_xml)
+
+        # 29 长度，mujoco XML 顺序，先全 0 或者你想要的默认值
+        self.joint_pos_array = np.zeros(n_mj, dtype=np.float32)
+        self.stiffness_array = np.zeros(n_mj, dtype=np.float32)
+        self.damping_array = np.zeros(n_mj, dtype=np.float32)
+
+        # joint_pos_array_seq / stiffness_array_seq / damping_array_seq 是 23 长度，lab 顺序
+        for lab_idx, mj_idx in enumerate(self.lab2mj):
+            self.joint_pos_array[mj_idx] = self.joint_pos_array_seq[lab_idx]
+            self.stiffness_array[mj_idx] = self.stiffness_array_seq[lab_idx]
+            self.damping_array[mj_idx] = self.damping_array_seq[lab_idx]
+
+
+        # 设置其他参数
+        self.kps_lab = self.stiffness_array_seq
+        self.kds_lab = self.damping_array_seq
+        self.default_angles_lab = self.joint_pos_array_seq
+        self.action_scale_lab = self.action_scale
+
+
+        self.filtered_x_speed = 0
+
+    def _init_onnx_session(self):
+        """初始化ONNX推理会话"""
+        try:
+            # 配置SessionOptions
+            options = ort.SessionOptions()
+
+            # 启用图优化，使用所有可用的优化（包括算子融合等）
+            options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+
+            # 设置执行模式（可选，默认执行模式是顺序执行，但图优化会改变计算图）
+            # 设置线程数（根据CPU核心数调整）
+            # 建议设置为CPU物理核心数（非超线程数），因为超线程可能不会带来线性提升
+            options.intra_op_num_threads = 1  # 设置计算图中的运算符内部并行线程数
+            options.inter_op_num_threads = 1  # 设置多个运算符之间的并行线程数（如果模型有多个分支）
+
+            # 启用内存优化（避免重复分配内存）
+            options.enable_mem_pattern = False  # 对于固定输入大小，可以设为False以避免内存规划的开销
+            options.enable_mem_reuse = True # 启用内存重用机制
+
+            self.ort_session_ = ort.InferenceSession(self.model_path, options, providers=['CPUExecutionProvider'])
+            
+            print(f"[{self.log_prefix}-ONNX] ONNX model loaded successfully: {self.model_path}")
+        except Exception as e:
+            print(f"[{self.log_prefix}] Failed to load ONNX model: {e}")
+            self.ort_session_ = None
+
+    def on_enter(self):
+        """进入WALKAMP状态"""
+        self._reset_internal_state()
+        print(f"[{self.log_prefix}] enter")
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self._warmup_inference_counter = 0
+        self.timer_gait_ = 0.0
+        if self.robot_data_ is not None:
+            try:
+                self._warm_start_pose = self.robot_data_.get_joint_pos().copy()
+            except Exception:
+                self._warm_start_pose.fill(0.0)
+        else:
+            self._warm_start_pose.fill(0.0)
+
+    def run(self, flag: ControlFlag):
+        """运行WALKAMP状态 - 与C++版本完全一致"""
+        print(f"[{self.log_prefix}] run")
+        # Only run policy inference every decimation_ steps
+        gait = gait_phase(
+            self.timer_gait_,
+            self.gait_cycle,
+            self.left_theta_offset,
+            self.right_theta_offset,
+            self.left_phase_ratio,
+            self.right_phase_ratio,
+        ).astype(np.float32)
+
+        if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
+
+            # print(f"[FSMStateWALKAMP] Gait phase: {gait}")
+            self.compute_observation(flag,gait)
+            self.compute_actions()
+
+            # lab 顺序目标角 23 维
+            target_dof_pos_lab = self.actions_ * self.action_scale_lab + self.default_angles_lab
+
+            # 拿一份当前 mj 顺序的关节角（或你原来用的 default 也行）
+            target_dof_pos_mj = self.robot_data_.get_joint_pos().copy()
+
+            # 只更新 23 个受控 DOF
+            target_dof_pos_mj[self.lab2mj] = target_dof_pos_lab
+            commanded_pos = target_dof_pos_mj
+            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_pos = (1.0 - blend) * self._warm_start_pose + blend * target_dof_pos_mj
+
+            base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+            self.robot_data_.q_d_[base:base + len(self.joint_xml)] = commanded_pos
+
+            self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+            self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+
+            self.last_actions_[:] = self.actions_
+
+
+        self.timer_gait_ += self.dt_
+        self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+        self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+
+    def compute_observation(self, flag: ControlFlag, gait):
+        roll, pitch, yaw = (
+                        float(self.robot_data_.imu_data_[2]),
+                        float(self.robot_data_.imu_data_[1]),
+                        float(self.robot_data_.imu_data_[0]),
+                    )
+        quat_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
+        q_xyzw    = np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]], dtype=np.float32)
+        gravity_init   = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.,0.,-1.], dtype=np.float32))
+        
+
+        x_speed_command, y_speed_command, yaw_speed_command = self.robot_data_.get_walk_cmd()
+        new_filtered_x_speed = 1 * x_speed_command + (1 - 1) * self.filtered_x_speed
+        change = new_filtered_x_speed - self.filtered_x_speed
+        change = np.clip(change, -0.005, 0.005)
+        self.filtered_x_speed = self.filtered_x_speed + change
+        command = np.concatenate([
+            np.array([
+                x_speed_command,
+                y_speed_command,
+                yaw_speed_command,
+            ], dtype=np.float32),
+        ])
+        print(f'Input command: {command}')
+
+        gyro = np.array([
+            self.robot_data_.imu_data_[3],
+            self.robot_data_.imu_data_[4],
+            self.robot_data_.imu_data_[5]
+        ], dtype=np.float32) * self.ang_vel_scale_
+
+        q_mj = self.robot_data_.get_joint_pos()
+        qdot_mj = self.robot_data_.get_joint_vel()
+
+
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+        q_mj = self.robot_data_.get_joint_pos()   # mj 顺序，长度 29
+        dq_mj = self.robot_data_.get_joint_vel()
+
+        # 只取 23 个受控关节，变成 lab 顺序
+        qj = q_mj[self.lab2mj]
+        dqj = dq_mj[self.lab2mj]
+
+        qj = qj - self.default_angles_lab
+
+
+        # Observation = ang_vel(3) + gravity(3) + command(9) + q(23) + dq(23) + action(23) = 84
+        proprio = np.concatenate([
+            ang_vel ,              # 3 elements
+            gravity_init,
+            command,
+            qj,
+            dqj,
+            self.last_actions_,
+            gait
+        ])
+
+        # History buffer management exactly like C++
+        if self.is_first_obs_:
+            for i in range(self.num_hist_):
+                start_idx = i * self.obs_size_
+                end_idx = start_idx + self.obs_size_
+                self.proprio_hist_buf_[start_idx:end_idx] = proprio
+            self.is_first_obs_ = False
+        else:
+            # Shift history: head((num_hist-1)*obs_size) = tail((num_hist-1)*obs_size)
+            shift_size = (self.num_hist_ - 1) * self.obs_size_
+            self.proprio_hist_buf_[:shift_size] = self.proprio_hist_buf_[self.obs_size_:]
+            self.proprio_hist_buf_[shift_size:] = proprio
+
+        # Clip observations exactly like C++
+        self.observations_ = np.clip(self.proprio_hist_buf_, -self.clip_obs_, self.clip_obs_)
+
+
+    @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
+    def compute_actions(self):
+        if self.ort_session_ is None:
+            return
+
+        try:
+            # Prepare input tensor
+            input_data = self.observations_.reshape(1, -1).astype(np.float32)
+
+            # ONNX inference
+            input_name = self.ort_session_.get_inputs()[0].name
+            outputs = self.ort_session_.run(None, {input_name: input_data})
+
+            # Extract and clip actions exactly like C++
+            output_data = outputs[0][0]
+            for i in range(self.action_num_):
+                self.actions_[i] = np.clip(output_data[i], -self.clip_act_, self.clip_act_)
+
+            if self.is_first_action_:
+                print(f"[{self.log_prefix}-ONNX] First Observation:")
+                for i in range(self.obs_size_):
+                    print(f"{self.observations_[i]:.6f} ", end="")
+                print()
+                self.is_first_action_ = False
+
+        except Exception as e:
+            print(f"[{self.log_prefix}] ONNX Runtime inference error: {e}")
+
+    def on_exit(self):
+        """退出WALKAMP状态"""
+        print(f"[{self.log_prefix}] exit")
+        # 关掉 obs 日志文件
+        if getattr(self, "obs_log_file", None) is not None:
+            try:
+                self.obs_log_file.flush()
+                self.obs_log_file.close()
+                print(f"[{self.log_prefix}] obs log saved to {self.obs_log_path}")
+            except Exception as e:
+                print(f"[{self.log_prefix}] failed to close obs log: {e}")
+            self.obs_log_file = None
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        else:
+            return None  # 无状态转换

Deploy_Tienkung/policy/zero/config/zero.yaml

@@ -0,0 +1,39 @@
+motor_num: 29        # 电机数量
+actions_size: 12     # action的大小
+
+zero_positions: [
+    -0.25, 0, 0, 0.5, -0.25, 0.0,   # 左腿
+    -0.25, 0, 0, 0.5, -0.25, 0.0,   # 右腿
+    0.0, 0.0, 0.0,                   # 腰
+    0.0, 0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 左臂
+    0.0, -0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 右臂
+    ]  
+    
+zero_positions_height: [
+    -0.4, 0.0, 0, 0.8, -0.4, 0.0,   # 左腿
+    -0.4, 0.0, 0, 0.8, -0.4, 0.0,   # 右腿
+    0.05, 0.08, 0.11,                   # 腰
+    0.0, 0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 左臂
+    0.0, -0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 右臂
+    ]  
+
+kp_pos: 
+  [1000, 2000, 2000, 1000, 50, 50, #left legs
+  1000, 2000, 2000, 1000, 50, 50,
+  1500.0, 1500.0, 1000.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0
+  ]  
+
+kd_pos: 
+  [30, 70, 70, 30, 3, 1, #left legs
+    30, 70, 70, 30, 3, 1, # right legs
+    70.0, 70.0, 50.0,  # 腰
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0,
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0
+  ]  
+
+
+
+interp_step: 0.001 #  控制插值速度
+interp_max: 0.9

Deploy_Tienkung/policy/zero/fsm_zero.py

@@ -0,0 +1,87 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+import numpy as np
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+import os
+import yaml
+
+
+class FSMStateZero(FSMState):
+    """零位状态实现（完整C++逻辑移植）"""
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.ZERO
+        self.q_factor_ = 0.0
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "zero.yaml")
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+
+        try:
+            self.action_num_ = policy_config["actions_size"]
+            self.motor_num_ = policy_config["motor_num"]
+            self.zero_positions_ = np.array(policy_config["zero_positions"], dtype=float)
+            self.zero_positions_height_ = np.array(policy_config["zero_positions_height"], dtype=float)
+            self.kp_pos_ = np.array(policy_config["kp_pos"], dtype=float)
+            self.kd_pos_ = np.array(policy_config["kd_pos"], dtype=float)
+            self.interp_step_ = float(policy_config["interp_step"])
+            self.interp_max_ = float(policy_config["interp_max"])
+        except Exception as e:
+            print(f"[FSMStateZero] YAML load error: {e}")
+            self.action_num_ = 12
+            self.motor_num_ = 29
+            self.zero_positions_ = np.zeros(self.motor_num_)
+            self.zero_positions_height_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+            self.interp_step_ = 0.00002
+            self.interp_max_ = 0.9
+        self.zero_positions = np.zeros(self.motor_num_)
+
+    def on_enter(self):
+        print("[FSMStateZero] Enter zero state")
+        self.q_factor_ = 0.0
+
+    def run(self, flag: ControlFlag):
+        if self.robot_data_ is None:
+            return
+        q_est = self.robot_data_.q_a_[-self.motor_num_:]  # numpy数组切片
+        if getattr(flag, 'height_control', False):
+            self.zero_positions = self.zero_positions_height_
+        else:
+            self.zero_positions = self.zero_positions_
+        if self.q_factor_ < self.interp_max_:
+            pos_cmd = (1.0 - self.q_factor_) * q_est + self.q_factor_ * self.zero_positions
+            self.q_factor_ = min(self.q_factor_ + self.interp_step_, self.interp_max_)
+        else:
+            pos_cmd = self.zero_positions
+        self.robot_data_.q_d_[-self.motor_num_:] = pos_cmd
+        self.robot_data_.q_dot_d_[-self.motor_num_:] = 0
+        self.robot_data_.tau_d_[-self.motor_num_:] = 0
+        self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_pos_
+        self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_pos_
+
+    def on_exit(self):
+        print("[FSMStateZero] Exit zero position control state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        elif flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None  # 无状态转换

Deploy_Tienkung/requirements.txt

@@ -0,0 +1,8 @@
+# 机器人控制系统依赖
+numpy == 2.3.5
+onnxruntime==1.24.2
+onnx==1.20.1
+pandas==3.0.1
+pyyaml==6.0.3
+scipy==1.17.1
+transforms3d==0.4.2

Deploy_Tienkung/rl_control_node.py

@@ -0,0 +1,353 @@
+"""
+RL Control Plugin (Python Version)
+Main ROS2 node for humanoid robot RL control system
+"""
+import math
+import os
+import queue
+import threading
+import time
+import sys
+
+import rclpy
+import yaml
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+# ROS messages
+from sensor_msgs.msg import Joy
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+
+from common.joystick import JoystickHumanoid, ControlFlag
+from common.xbox_control import XBOXController
+# Local imports
+from common.robot_data import RobotData
+from FSM.robot_fsm import get_robot_fsm
+from FSM.fsm_base import FSMStateName
+from common.robot_interface import get_robot_interface
+from common.stdin_keyboard_control  import KeyboardController
+from udp_loopback.udp_fsm_controller import UDPFSMController
+import functools
+
+def timing_decorator(func):
+    """
+    装饰器：记录函数执行时间
+    """
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+        start_time = time.perf_counter()
+        result = func(*args, **kwargs)
+        end_time = time.perf_counter()
+        execution_time = end_time - start_time
+        print(f"[TIMING] {func.__name__} executed in {execution_time:.6f} seconds")
+        return result
+    return wrapper
+
+
+class XMIGCSControlNode(Node):
+    """xMIGCS控制节点Python版本"""
+
+    def __init__(self, debug=False):
+        super().__init__('xmigcs_control_node')
+
+        # 配置和参数
+        self.debug = debug
+        self.whole_joint_num = 35
+        self.pi = math.pi
+        self.rpm2rps = math.pi / 30.0
+
+        self.config = {}
+
+        # 加载配置
+        self._load_config()
+
+        # 初始化数据结构
+        self._init_data_structures()
+
+        # 机器人接口
+        self.robot_interface = get_robot_interface(self.robot_data,
+                                                   self.config_file)
+        self.robot_interface.init(self)  # 传入node实例
+
+        # 机器人FSM
+        self.robot_fsm = get_robot_fsm(
+            self.robot_data,
+            self.config,
+        )
+
+        # 初始化ROS接口
+        self._init_ros_interfaces()
+
+        # 初始化控制系统
+        self._init_control_system()
+
+        # 启动控制线程
+        self._start_control_thread()
+
+    def _load_config(self):
+        """加载配置文件"""
+        # 获取包路径
+        self.config_file = os.path.join('.', 'config', 'dex_config.yaml')
+
+        with open(self.config_file, 'r') as f:
+            self.config = yaml.safe_load(f)
+
+        print(self.config)
+
+        # 获取控制器类型
+        self.control_tool = self.config.get('control_tool', 'keyboard')
+        # 提取关键配置参数
+        self.motor_num = self.config.get('motor_num')
+        self.dt = self.config.get('dt')
+        self.sim = self.config.get('sim')
+
+        # 检查当前用户名，如果是ubuntu则抛出异常
+        import getpass
+        user_name = getpass.getuser().lower()
+        if self.sim and user_name == 'ubuntu':
+            raise RuntimeError("On ubuntu user, sim must be set to false")
+
+    def _init_data_structures(self):
+        """初始化数据结构"""
+        # 机器人数据
+        self.robot_data = RobotData(self.motor_num, self.whole_joint_num)
+        self.robot_data.config_file_ = getattr(self, 'config_file', '')
+
+        # joysticks 消息队列
+        self.queue_joy_cmd = queue.Queue(maxsize=1)
+        self.queue_xbox_cmd = queue.Queue(maxsize=1)
+        self.control_flag = ControlFlag()
+
+    def _init_ros_interfaces(self):
+        """初始化ROS接口（仅非电机相关）"""
+        qos_profile = QoSProfile(reliability=ReliabilityPolicy.RELIABLE,
+                                 history=HistoryPolicy.KEEP_LAST,
+                                 depth=5)
+
+        # 订阅者（非电机相关）
+        if self.control_tool == "joystick":
+            self.sub_joy_cmd = self.create_subscription(
+                Joy, '/sbus_data', self._joy_callback, qos_profile)
+        if self.control_tool == "xbox":
+            self.sub_xbox_cmd = self.create_subscription(
+                Joy, '/xbox_data', self._xbox_callback, qos_profile)
+
+    def _init_control_system(self):
+        """初始化控制系统"""
+
+        # 手柄控制器
+        if self.control_tool == "joystick":
+            self.joystick_humanoid = JoystickHumanoid()
+            self.joystick_humanoid.init()
+
+        # 键盘控制器
+        if self.control_tool == "keyboard":
+            self.keyboard_controller = KeyboardController()
+            self.keyboard_controller.init()
+            # 如果使用键盘控制，启动键盘监听
+            self.keyboard_controller.start()
+
+        # UDP控制器
+        if self.control_tool == "udp_loopback":
+            self.udp_fsm_controller = UDPFSMController()
+            self.udp_fsm_controller.init()
+            self.udp_fsm_controller.start()
+
+        # Xbox控制器
+        if self.control_tool == "xbox":
+            self.xbox_controller = XBOXController()
+            self.xbox_controller.init()
+
+        # 控制标志
+        self.control_running = False
+        self.control_thread = None
+
+    def _start_control_thread(self):
+        """启动控制线程"""
+        self.control_running = True
+        self.control_thread = threading.Thread(target=self._rl_control_loop,
+                                               daemon=True)
+        self.control_thread.start()
+        self.get_logger().info("Control thread started")
+
+    def _rl_control_loop(self):
+        """主控制循环"""
+        self.get_logger().info("RL control loop starting...")
+        
+        # 初始化时间戳
+        time_passed = 0.0
+
+        # 处理手柄数据
+        self._process_controller_data()
+        # 更新机器人数据
+        self._update_robot_data(self.control_flag, time_passed)
+
+        while self.control_running and rclpy.ok():
+
+            # 运行FSM
+            loop_start = time.perf_counter()
+            self.robot_fsm.run_fsm(self.robot_data.control_flag)
+
+            # 发布控制命令
+            self.robot_interface.update_param(current_state=self.robot_fsm.get_current_state())
+            self._send_control_commands(self.robot_data.control_flag)
+
+            # 更新时间戳
+            time_passed += self.dt
+
+            # 处理手柄数据
+            self._process_controller_data()
+
+            # 更新机器人数据
+            self._update_robot_data(self.control_flag, time_passed)
+
+            # 控制频率
+            self._precise_sleep_until(loop_start + self.dt)
+            # print(
+            #      f"current control freq: {1/(time.perf_counter() - loop_start):.2f} Hz"
+            # )
+
+
+        self.get_logger().info("RL control loop ended")
+
+    def _precise_sleep_until(self, target_time):
+        """精确睡眠到目标时间"""
+        current_time = time.perf_counter()
+        sleep_time = target_time - current_time
+        
+        if sleep_time <= 0:
+            return  # 已经超时，立即返回
+        
+        # 分级睡眠策略
+        if sleep_time > 0.003:  # 3ms以上使用混合睡眠
+            # 先睡眠大部分时间
+            time.sleep(sleep_time * 0.9)
+            # 剩余时间忙等待
+            while time.perf_counter() < target_time:
+                pass
+        else:  # 3ms以内纯忙等待
+            while time.perf_counter() < target_time:
+                pass
+
+    # def _wait_for_start_signal(self):
+    #     """等待启动信号"""
+    #     start_file = "/tmp/rl_start_signal"
+    #     self.get_logger().info("Waiting for start signal...")
+    #     self.get_logger().info("Run: touch /tmp/rl_start_signal")
+
+    #     # 删除可能存在的旧文件
+    #     if os.path.exists(start_file):
+    #         os.remove(start_file)
+
+    #     # 等待启动文件出现
+    #     while not os.path.exists(start_file) and rclpy.ok():
+    #         time.sleep(0.5)
+
+    #     self.get_logger().info("Start signal received, beginning RL control!")
+
+    # @timing_decorator
+    def _process_controller_data(self):
+        # 处理控制器输入
+        if self.control_tool == "joystick":
+            # 处理手柄输入
+            while not self.queue_joy_cmd.empty():
+                try:
+                    msg = self.queue_joy_cmd.get_nowait()
+                    self.joystick_humanoid.joy_map_read(msg)
+                    self.joystick_humanoid.joy_flag_update()
+                    break
+                except queue.Empty:
+                    break
+        if self.control_tool == "xbox":
+            while not self.queue_xbox_cmd.empty():
+                try:
+                    msg = self.queue_xbox_cmd.get_nowait()
+                    self.xbox_controller.xbox_map_read(msg)
+                    self.xbox_controller.xbox_flag_update()
+                    break
+                except queue.Empty:
+                    break
+
+        if self.control_tool == "keyboard":
+            self.keyboard_controller.update_flag()
+            flag = self.keyboard_controller.get_keyboard_flag()
+        elif self.control_tool == "udp_loopback":
+            self.udp_fsm_controller.update_flag()
+            flag = self.udp_fsm_controller.get_udp_flag()
+        elif self.control_tool == "joystick":
+            flag = self.joystick_humanoid.get_joy_flag()
+        elif self.control_tool == "xbox":
+            flag = self.xbox_controller.get_xbox_flag()
+        else:
+            print("[ERROR] No control tool specified")
+        print('*' * 30 + f"current flag: {flag}" + '*' * 30)
+        self.control_flag = flag
+
+    # @timing_decorator
+    def _update_robot_data(self, flag: ControlFlag, time_passed: float):
+        """更新机器人数据"""
+        self.robot_interface.update_robot_data(flag, time_passed)
+
+    # @timing_decorator
+    def _send_control_commands(self, flag: ControlFlag):
+        """发布控制命令"""
+        # 通过robot_interface发布控制命令
+        self.robot_interface.send_motor_commands(flag)
+
+    def _joy_callback(self, msg):
+        """云卓手柄输入回调"""
+        try:
+            self.queue_joy_cmd.put_nowait(msg)
+        except queue.Full:
+            try:
+                self.queue_joy_cmd.get_nowait()  # 移除旧数据
+                self.queue_joy_cmd.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+    def _xbox_callback(self, msg):
+        """xbox手柄输入回调"""
+        try:
+            self.queue_xbox_cmd.put_nowait(msg)
+        except queue.Full:
+            try:
+                self.queue_xbox_cmd.get_nowait()  # 移除旧数据
+                self.queue_xbox_cmd.put_nowait(msg)  # 加入新数据
+            except:
+                pass  # 如果仍然无法加入，忽略
+
+    def destroy_node(self):
+        """节点销毁"""
+        self.control_running = False
+        # # 先停止键盘控制器（重要！）
+        if hasattr(self,
+                   'keyboard_controller') and self.control_tool == "keyboard":
+            self.keyboard_controller.stop()
+        if hasattr(self,
+                   'udp_fsm_controller') and self.control_tool == "udp_loopback":
+            self.udp_fsm_controller.stop()
+
+        if self.control_thread and self.control_thread.is_alive():
+            self.control_thread.join(timeout=1.0)
+
+        super().destroy_node()
+
+
+def main(args=None):
+    """主函数"""
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = XMIGCSControlNode(debug=False)
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if 'node' in locals() and node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+
+if __name__ == '__main__':
+    main()

Deploy_Tienkung/rl_control_node_sim.py

@@ -0,0 +1,32 @@
+import os
+import sys
+
+import rclpy
+import yaml
+
+from rl_control_node import XMIGCSControlNode
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+
+
+class XMIGCSControlNode_sim(XMIGCSControlNode):
+    def __init__(self, debug=False):
+        super().__init__(debug)
+        print("rewrite sim")
+        self.robot_interface.sim = True
+
+def main(args=None):
+    """主函数"""
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = XMIGCSControlNode_sim(debug=False)
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if "node" in locals() and node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

Deploy_Tienkung/scripts/detect_xbox_buttons.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+"""
+Listen to /joy and print which button indices change to 1 (pressed).
+Run while pressing your controller buttons to discover indices for select/start/turbo/home.
+"""
+import sys
+import argparse
+
+try:
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import Joy
+except Exception:
+    # allow syntax check on systems without ROS present
+    rclpy = None
+    Joy = None
+
+
+class JoyWatcher(Node):
+    def __init__(self, topic: str = '/joy'):
+        super().__init__('joy_watcher')
+        self.subscription = self.create_subscription(Joy, topic, self.cb, 10)
+        self.prev_buttons = []
+        self.prev_axes = []
+        self.get_logger().info(f'Listening to {topic} - press controller buttons now...')
+
+    def cb(self, msg: Joy):
+        buttons = list(msg.buttons)
+        axes = list(msg.axes)
+        # init prev
+        if not self.prev_buttons:
+            self.prev_buttons = [0] * len(buttons)
+        if not self.prev_axes:
+            self.prev_axes = [0.0] * len(axes)
+        # detect button press events
+        pressed = [i for i, (p, n) in enumerate(zip(self.prev_buttons, buttons)) if p == 0 and n == 1]
+        if pressed:
+            self.get_logger().info(f'Buttons pressed (indices): {pressed}')
+            self.get_logger().info(f'Full buttons array: {buttons}')
+        # detect dpad-like axis changes (axes 6/7 common)
+        for i, (p, n) in enumerate(zip(self.prev_axes, axes)):
+            if p != n:
+                # only log significant changes
+                if abs(n - p) > 1e-3:
+                    self.get_logger().info(f'Axis {i} changed: {p} -> {n}')
+        self.prev_buttons = buttons[:]
+        self.prev_axes = axes[:]
+
+
+def main(argv=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--topic', default='/joy')
+    args = parser.parse_args(argv)
+
+    if rclpy is None:
+        print('rclpy not available in this environment. Run this script on a machine with ROS2 and a /joy publisher.')
+        return 1
+
+    rclpy.init()
+    node = JoyWatcher(topic=args.topic)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+    return 0
+
+
+if __name__ == '__main__':
+    raise SystemExit(main())

Deploy_Tienkung/udp_loopback/README.md

@@ -0,0 +1,66 @@
+# UDP Loopback
+
+独立于原 `rl_control_node.py` 的 localhost UDP 键盘测试目录。
+
+包含：
+
+- `udp_keyboard_sender.py`：从终端读取按键，编码 UDP 报文并发送
+- `udp_loopback_node.py`：接收 UDP 报文，解码事件并计算目标值
+- `protocol.py`：自定义协议和状态结构
+- `config/udp_loopback.yaml`：本地测试配置
+
+运行方式：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_loopback_node.py
+```
+
+另开一个终端：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_keyboard_sender.py
+```
+
+接回现有 FSM 的方式：
+
+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
+python3 rl_control_node_sim.py
+```
+
+3. 再启动发送端：
+
+```bash
+cd /home/meiqi/tienkung/Deploy_Tienkung
+python3 udp_loopback/udp_keyboard_sender.py
+```
+
+此时 UDP 接收结果会在接收侧被映射回现有 FSM 命令：
+
+- `pose_home -> gotoZERO`
+- `pose_hold -> gotoSTOP`
+- `mode_stride -> gotoWALKAMP`
+- `mode_dash -> gotoMYPOLICY`
+- `surge/sway/spin -> x/y/yaw speed command`
+
+当前事件码不是原工程里的 `gotoZERO` / `gotoSTOP` / `x_speed_command` 这一套，而是：
+
+- `pose_home`
+- `pose_hold`
+- `mode_stride`
+- `mode_dash`
+- `surge_up`
+- `surge_down`
+- `sway_left`
+- `sway_right`
+- `spin_left`
+- `spin_right`
+- `lift_up`
+- `lift_down`
+- `trim_reset`
+- `session_quit`

Deploy_Tienkung/udp_loopback/__init__.py

@@ -0,0 +1 @@
+"""Standalone localhost UDP keyboard loopback package."""

Deploy_Tienkung/udp_loopback/config/udp_loopback.yaml

@@ -0,0 +1,22 @@
+dt: 0.01
+
+sender:
+  target_host: 127.0.0.1
+  target_port: 31000
+  source_tag: local_keys
+
+receiver:
+  listen_host: 127.0.0.1
+  listen_port: 31000
+
+motion:
+  initial_lift: 0.89
+  min_lift: 0.65
+  max_lift: 0.90
+  lift_step: 0.05
+  surge_step: 0.10
+  sway_step: 0.10
+  spin_step: 0.10
+  max_surge: 1.00
+  max_sway: 0.50
+  max_spin: 0.50

Deploy_Tienkung/udp_loopback/protocol.py

@@ -0,0 +1,61 @@
+"""Protocol and state definitions for the standalone UDP loopback."""
+
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass, field
+import json
+import time
+from typing import Any, Dict
+
+
+@dataclass
+class InputEnvelope:
+    """Small UDP payload carrying one encoded keyboard event."""
+
+    seq_id: int
+    event_code: str
+    key_name: str
+    sent_at: float = field(default_factory=time.time)
+    drive_value: float = 1.0
+    source_tag: str = "local_keys"
+
+    def encode(self) -> bytes:
+        return json.dumps(asdict(self), separators=(",", ":")).encode("utf-8")
+
+    @classmethod
+    def decode(cls, payload: bytes) -> "InputEnvelope":
+        data = json.loads(payload.decode("utf-8"))
+        return cls(
+            seq_id=int(data["seq_id"]),
+            event_code=str(data["event_code"]),
+            key_name=str(data["key_name"]),
+            sent_at=float(data.get("sent_at", time.time())),
+            drive_value=float(data.get("drive_value", 1.0)),
+            source_tag=str(data.get("source_tag", "unknown")),
+        )
+
+
+@dataclass
+class MotionFrame:
+    """Receiver-side motion targets computed from decoded events."""
+
+    mode_tag: str = "pose_home"
+    relay_on: bool = True
+    surge_goal: float = 0.0
+    sway_goal: float = 0.0
+    spin_goal: float = 0.0
+    lift_goal: float = 0.89
+    last_event_code: str = "boot"
+    last_rx_time: float = 0.0
+
+    def as_dict(self) -> Dict[str, Any]:
+        return asdict(self)
+
+
+def format_motion_frame(frame: MotionFrame) -> str:
+    return (
+        f"mode={frame.mode_tag} relay_on={frame.relay_on} "
+        f"surge={frame.surge_goal:.2f} sway={frame.sway_goal:.2f} "
+        f"spin={frame.spin_goal:.2f} lift={frame.lift_goal:.2f} "
+        f"event={frame.last_event_code}"
+    )

Deploy_Tienkung/udp_loopback/udp_fsm_controller.py

@@ -0,0 +1,219 @@
+"""Adapter that maps UDP loopback events onto the existing FSM control flag."""
+
+from __future__ import annotations
+
+import queue
+import socket
+import threading
+import time
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+from common.joystick import ControlFlag
+
+try:
+    from .protocol import InputEnvelope, MotionFrame, format_motion_frame
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope, MotionFrame, format_motion_frame
+
+
+class UDPFSMFlag(ControlFlag):
+    """FSM-facing flag produced from decoded UDP events."""
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.x_speed_command: float = 0.0
+        self.y_speed_command: float = 0.0
+        self.yaw_speed_command: float = 0.0
+        self.height_cmd: float = 0.89
+
+
+class UDPFSMController:
+    """Receive UDP packets and expose them as a ControlFlag-like interface."""
+
+    def __init__(self) -> None:
+        self.config: Dict[str, object] = {}
+        self.data_mutex = threading.Lock()
+
+        self._load_config()
+        self._init_data_structures()
+
+        self.recv_running = False
+        self.recv_thread: Optional[threading.Thread] = None
+        self.rx_socket: Optional[socket.socket] = None
+
+    def _load_config(self) -> None:
+        config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with config_path.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        receiver_cfg = self.config.get("receiver", {})
+        motion_cfg = self.config.get("motion", {})
+
+        self.listen_host = receiver_cfg.get("listen_host", "127.0.0.1")
+        self.listen_port = int(receiver_cfg.get("listen_port", 31000))
+
+        self.initial_lift = float(motion_cfg.get("initial_lift", 0.89))
+        self.lift_step = float(motion_cfg.get("lift_step", 0.05))
+        self.max_surge = float(motion_cfg.get("max_surge", 1.0))
+        self.max_sway = float(motion_cfg.get("max_sway", 0.5))
+        self.max_spin = float(motion_cfg.get("max_spin", 0.5))
+        self.max_lift = float(motion_cfg.get("max_lift", 0.90))
+        self.min_lift = float(motion_cfg.get("min_lift", 0.65))
+        self.surge_step = float(motion_cfg.get("surge_step", 0.1))
+        self.sway_step = float(motion_cfg.get("sway_step", 0.1))
+        self.spin_step = float(motion_cfg.get("spin_step", 0.1))
+
+    def _init_data_structures(self) -> None:
+        self.packet_queue: queue.Queue[InputEnvelope] = queue.Queue(maxsize=128)
+        self.motion_frame = MotionFrame(lift_goal=self.initial_lift)
+        self.udp_flag = UDPFSMFlag()
+        self.udp_flag.height_cmd = self.initial_lift
+        self.last_seq_id = -1
+
+    def start(self) -> None:
+        self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.rx_socket.bind((self.listen_host, self.listen_port))
+        self.rx_socket.settimeout(0.2)
+
+        self.recv_running = True
+        self.recv_thread = threading.Thread(target=self._recv_loop, daemon=True)
+        self.recv_thread.start()
+        print(
+            f"UDP FSM controller listening on {self.listen_host}:{self.listen_port}"
+        )
+
+    def stop(self) -> None:
+        self.recv_running = False
+        if self.recv_thread and self.recv_thread.is_alive():
+            self.recv_thread.join(timeout=1.0)
+        if self.rx_socket is not None:
+            self.rx_socket.close()
+            self.rx_socket = None
+        print("UDP FSM controller stopped")
+
+    def _recv_loop(self) -> None:
+        while self.recv_running:
+            try:
+                payload, _ = self.rx_socket.recvfrom(4096)
+            except socket.timeout:
+                continue
+            except OSError:
+                break
+
+            try:
+                packet = InputEnvelope.decode(payload)
+            except (ValueError, KeyError, TypeError, UnicodeDecodeError) as exc:
+                print(f"[udp_fsm] drop invalid payload: {exc}")
+                continue
+
+            try:
+                self.packet_queue.put_nowait(packet)
+            except queue.Full:
+                try:
+                    self.packet_queue.get_nowait()
+                    self.packet_queue.put_nowait(packet)
+                except queue.Empty:
+                    pass
+
+    def update_flag(self) -> None:
+        while not self.packet_queue.empty():
+            try:
+                packet = self.packet_queue.get_nowait()
+            except queue.Empty:
+                break
+            self._apply_packet(packet)
+
+    def get_udp_flag(self) -> UDPFSMFlag:
+        with self.data_mutex:
+            flag_copy = UDPFSMFlag()
+            flag_copy.__dict__.update(self.udp_flag.__dict__)
+            return flag_copy
+
+    def init(self) -> int:
+        print("UDP FSM controller initialized")
+        return 0
+
+    def _apply_packet(self, packet: InputEnvelope) -> None:
+        with self.data_mutex:
+            self.last_seq_id = packet.seq_id
+            self.motion_frame.last_event_code = packet.event_code
+            self.motion_frame.last_rx_time = packet.sent_at
+
+            event_code = packet.event_code
+            if event_code == "pose_home":
+                self.motion_frame.mode_tag = "pose_home"
+            elif event_code == "pose_hold":
+                self.motion_frame.mode_tag = "pose_hold"
+            elif event_code == "mode_stride":
+                self.motion_frame.mode_tag = "mode_stride"
+            elif event_code == "mode_dash":
+                self.motion_frame.mode_tag = "mode_dash"
+            elif event_code == "mode_xrun":
+                self.motion_frame.mode_tag = "mode_xrun"
+            elif event_code == "surge_up":
+                self.motion_frame.surge_goal = min(
+                    self.max_surge, self.motion_frame.surge_goal + self.surge_step
+                )
+            elif event_code == "surge_down":
+                self.motion_frame.surge_goal = max(
+                    -self.max_surge, self.motion_frame.surge_goal - self.surge_step
+                )
+            elif event_code == "sway_left":
+                self.motion_frame.sway_goal = max(
+                    -self.max_sway, self.motion_frame.sway_goal - self.sway_step
+                )
+            elif event_code == "sway_right":
+                self.motion_frame.sway_goal = min(
+                    self.max_sway, self.motion_frame.sway_goal + self.sway_step
+                )
+            elif event_code == "spin_left":
+                self.motion_frame.spin_goal = max(
+                    -self.max_spin, self.motion_frame.spin_goal - self.spin_step
+                )
+            elif event_code == "spin_right":
+                self.motion_frame.spin_goal = min(
+                    self.max_spin, self.motion_frame.spin_goal + self.spin_step
+                )
+            elif event_code == "lift_up":
+                self.motion_frame.lift_goal = min(
+                    self.max_lift, self.motion_frame.lift_goal + self.lift_step
+                )
+            elif event_code == "lift_down":
+                self.motion_frame.lift_goal = max(
+                    self.min_lift, self.motion_frame.lift_goal - self.lift_step
+                )
+            elif event_code == "trim_reset":
+                self.motion_frame.surge_goal = 0.0
+                self.motion_frame.sway_goal = 0.0
+                self.motion_frame.spin_goal = 0.0
+            elif event_code == "session_quit":
+                self.motion_frame.relay_on = False
+                self.motion_frame.mode_tag = "pose_hold"
+                self.recv_running = False
+
+            self._sync_motion_frame_to_flag()
+            print(
+                f"[udp_fsm] seq={packet.seq_id} key={packet.key_name} "
+                f"{format_motion_frame(self.motion_frame)} "
+                f"fsm={self.udp_flag.fsm_state_command}"
+            )
+
+    def _sync_motion_frame_to_flag(self) -> None:
+        mode_to_fsm_command = {
+            "pose_home": "gotoZERO",
+            "pose_hold": "gotoSTOP",
+            "mode_stride": "gotoWALKAMP",
+            "mode_dash": "gotoMYPOLICY",
+            "mode_xrun": "gotoXSIMRUN",
+        }
+        self.udp_flag.enable = self.motion_frame.relay_on
+        self.udp_flag.fsm_state_command = mode_to_fsm_command.get(
+            self.motion_frame.mode_tag, self.udp_flag.fsm_state_command
+        )
+        self.udp_flag.x_speed_command = self.motion_frame.surge_goal
+        self.udp_flag.y_speed_command = self.motion_frame.sway_goal
+        self.udp_flag.yaw_speed_command = self.motion_frame.spin_goal
+        self.udp_flag.height_cmd = self.motion_frame.lift_goal

Deploy_Tienkung/udp_loopback/udp_keyboard_sender.py

@@ -0,0 +1,200 @@
+"""Keyboard sender that encodes key events and emits them over localhost UDP."""
+
+from __future__ import annotations
+
+import os
+import select
+import signal
+import socket
+import sys
+import termios
+import threading
+import tty
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+try:
+    from .protocol import InputEnvelope
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope
+
+
+class UDPKeyboardSender:
+    """Standalone keyboard sender for localhost UDP testing."""
+
+    def __init__(self) -> None:
+        self.config: Dict[str, object] = {}
+        self.seq_id = 0
+        self.running = False
+        self.socket: Optional[socket.socket] = None
+        self.input_thread: Optional[threading.Thread] = None
+        self.original_terminal_settings = None
+
+        self._load_config()
+        self._init_socket()
+        self._print_help()
+
+    def _load_config(self) -> None:
+        config_path = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with config_path.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        sender_cfg = self.config.get("sender", {})
+        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 = sender_cfg.get("source_tag", "local_keys")
+
+    def _init_socket(self) -> None:
+        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+    def _print_help(self) -> None:
+        print("UDP keyboard sender ready")
+        print(f"Target: {self.target_host}:{self.target_port}")
+        print("Keys:")
+        print("  z -> pose_home")
+        print("  c -> pose_hold")
+        print("  m -> mode_stride")
+        print("  p -> mode_dash")
+        print("  n -> mode_xrun")
+        print("  w/s -> surge +/-")
+        print("  a/d -> sway +/-")
+        print("  q/e -> spin +/-")
+        print("  Left/Right -> lift +/-")
+        print("  Up/Down -> surge +/-")
+        print("  r -> trim_reset")
+        print("  x -> session_quit")
+
+    def start(self) -> None:
+        self.running = True
+        self.input_thread = threading.Thread(target=self._input_loop, daemon=True)
+        self.input_thread.start()
+        print("UDP keyboard sender thread started")
+
+    def stop(self) -> None:
+        self.running = False
+        if self.input_thread and self.input_thread.is_alive():
+            self.input_thread.join(timeout=1.0)
+        if self.original_terminal_settings is not None:
+            try:
+                termios.tcsetattr(
+                    sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
+                )
+            except termios.error:
+                pass
+            self.original_terminal_settings = None
+        if self.socket is not None:
+            self.socket.close()
+            self.socket = None
+        print("UDP keyboard sender stopped")
+
+    def _input_loop(self) -> None:
+        self.original_terminal_settings = termios.tcgetattr(sys.stdin)
+        try:
+            tty.setraw(sys.stdin.fileno())
+            while self.running:
+                if select.select([sys.stdin], [], [], 0.1)[0]:
+                    key = sys.stdin.read(1)
+                    self._process_key(key)
+        except KeyboardInterrupt:
+            self._handle_ctrl_c()
+        finally:
+            if self.original_terminal_settings is not None:
+                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.original_terminal_settings)
+                self.original_terminal_settings = None
+
+    def _process_key(self, key: str) -> None:
+        event_map = {
+            "w": ("surge_up", "w"),
+            "s": ("surge_down", "s"),
+            "a": ("sway_left", "a"),
+            "d": ("sway_right", "d"),
+            "q": ("spin_left", "q"),
+            "e": ("spin_right", "e"),
+            "z": ("pose_home", "z"),
+            "c": ("pose_hold", "c"),
+            "m": ("mode_stride", "m"),
+            "p": ("mode_dash", "p"),
+            "n": ("mode_xrun", "n"),
+            "r": ("trim_reset", "r"),
+            "x": ("session_quit", "x"),
+        }
+
+        if key == "\x03":
+            self._handle_ctrl_c()
+            return
+
+        if key == "\x1b":
+            self._handle_arrow_key()
+            return
+
+        if key in event_map:
+            event_code, key_name = event_map[key]
+            self._send_event(event_code, key_name)
+
+    def _handle_arrow_key(self) -> None:
+        if not select.select([sys.stdin], [], [], 0.1)[0]:
+            return
+        key2 = sys.stdin.read(1)
+        if key2 != "[":
+            return
+        if not select.select([sys.stdin], [], [], 0.1)[0]:
+            return
+        key3 = sys.stdin.read(1)
+        arrow_map = {
+            "A": ("surge_up", "arrow_up"),
+            "B": ("surge_down", "arrow_down"),
+            "C": ("lift_down", "arrow_right"),
+            "D": ("lift_up", "arrow_left"),
+        }
+        if key3 in arrow_map:
+            event_code, key_name = arrow_map[key3]
+            self._send_event(event_code, key_name)
+
+    def _send_event(self, event_code: str, key_name: str) -> None:
+        if self.socket is None:
+            return
+        envelope = InputEnvelope(
+            seq_id=self.seq_id,
+            event_code=event_code,
+            key_name=key_name,
+            source_tag=self.source_tag,
+        )
+        self.seq_id += 1
+        self.socket.sendto(envelope.encode(), (self.target_host, self.target_port))
+        print(
+            f"sent seq={envelope.seq_id} event={envelope.event_code} "
+            f"key={envelope.key_name}"
+        )
+        if event_code == "session_quit":
+            self.running = False
+
+    def _handle_ctrl_c(self) -> None:
+        self.running = False
+        if self.original_terminal_settings is not None:
+            try:
+                termios.tcsetattr(
+                    sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
+                )
+            except termios.error:
+                pass
+            self.original_terminal_settings = None
+        os.kill(os.getpid(), signal.SIGINT)
+
+
+def main() -> None:
+    sender = UDPKeyboardSender()
+    sender.start()
+    try:
+        while sender.running:
+            if sender.input_thread is not None:
+                sender.input_thread.join(timeout=0.2)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        sender.stop()
+
+
+if __name__ == "__main__":
+    main()

Deploy_Tienkung/udp_loopback/udp_loopback_node.py

@@ -0,0 +1,227 @@
+"""Receiver-side localhost UDP loopback node with a control loop layout."""
+
+from __future__ import annotations
+
+import queue
+import socket
+import threading
+import time
+from pathlib import Path
+from typing import Dict, Optional
+
+import yaml
+
+try:
+    from .protocol import InputEnvelope, MotionFrame, format_motion_frame
+except ImportError:  # pragma: no cover - direct script execution fallback
+    from protocol import InputEnvelope, MotionFrame, format_motion_frame
+
+
+class UDPLoopbackNode:
+    """Standalone UDP receiver that mirrors rl_control_node.py structure."""
+
+    def __init__(self, debug: bool = False) -> None:
+        self.debug = debug
+        self.config: Dict[str, object] = {}
+
+        self._load_config()
+        self._init_data_structures()
+        self._init_udp_interfaces()
+        self._init_control_system()
+        self._start_threads()
+
+    def _load_config(self) -> None:
+        self.config_file = Path(__file__).resolve().parent / "config" / "udp_loopback.yaml"
+        with self.config_file.open("r", encoding="utf-8") as file:
+            self.config = yaml.safe_load(file) or {}
+
+        self.dt = float(self.config.get("dt", 0.01))
+        self.receiver_cfg = self.config.get("receiver", {})
+        self.motion_cfg = self.config.get("motion", {})
+
+        self.listen_host = self.receiver_cfg.get("listen_host", "127.0.0.1")
+        self.listen_port = int(self.receiver_cfg.get("listen_port", 31000))
+
+        self.initial_lift = float(self.motion_cfg.get("initial_lift", 0.89))
+        self.lift_step = float(self.motion_cfg.get("lift_step", 0.05))
+        self.max_surge = float(self.motion_cfg.get("max_surge", 1.0))
+        self.max_sway = float(self.motion_cfg.get("max_sway", 0.5))
+        self.max_spin = float(self.motion_cfg.get("max_spin", 0.5))
+        self.max_lift = float(self.motion_cfg.get("max_lift", 0.90))
+        self.min_lift = float(self.motion_cfg.get("min_lift", 0.65))
+        self.surge_step = float(self.motion_cfg.get("surge_step", 0.1))
+        self.sway_step = float(self.motion_cfg.get("sway_step", 0.1))
+        self.spin_step = float(self.motion_cfg.get("spin_step", 0.1))
+
+    def _init_data_structures(self) -> None:
+        self.packet_queue: queue.Queue[InputEnvelope] = queue.Queue(maxsize=128)
+        self.motion_frame = MotionFrame(lift_goal=self.initial_lift)
+        self.frame_lock = threading.Lock()
+        self.last_seq_id = -1
+
+    def _init_udp_interfaces(self) -> None:
+        self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.rx_socket.bind((self.listen_host, self.listen_port))
+        self.rx_socket.settimeout(0.2)
+
+    def _init_control_system(self) -> None:
+        self.control_running = False
+        self.recv_running = False
+        self.control_thread: Optional[threading.Thread] = None
+        self.recv_thread: Optional[threading.Thread] = None
+
+    def _start_threads(self) -> None:
+        self.recv_running = True
+        self.control_running = True
+
+        self.recv_thread = threading.Thread(target=self._udp_recv_loop, daemon=True)
+        self.recv_thread.start()
+
+        self.control_thread = threading.Thread(target=self._control_loop, daemon=True)
+        self.control_thread.start()
+
+        print(
+            f"UDP loopback node listening on {self.listen_host}:{self.listen_port} "
+            f"with dt={self.dt:.3f}s"
+        )
+
+    def _udp_recv_loop(self) -> None:
+        while self.recv_running:
+            try:
+                payload, _ = self.rx_socket.recvfrom(4096)
+            except socket.timeout:
+                continue
+            except OSError:
+                break
+
+            try:
+                packet = InputEnvelope.decode(payload)
+            except (ValueError, KeyError, TypeError, UnicodeDecodeError, yaml.YAMLError) as exc:
+                print(f"[udp_loopback] drop invalid payload: {exc}")
+                continue
+
+            try:
+                self.packet_queue.put_nowait(packet)
+            except queue.Full:
+                try:
+                    self.packet_queue.get_nowait()
+                    self.packet_queue.put_nowait(packet)
+                except queue.Empty:
+                    pass
+
+    def _control_loop(self) -> None:
+        print("UDP loopback control loop started")
+        while self.control_running:
+            loop_start = time.perf_counter()
+            self._process_udp_packets()
+            self._precise_sleep_until(loop_start + self.dt)
+        print("UDP loopback control loop ended")
+
+    def _process_udp_packets(self) -> None:
+        while not self.packet_queue.empty():
+            try:
+                packet = self.packet_queue.get_nowait()
+            except queue.Empty:
+                break
+            self._apply_packet(packet)
+
+    def _apply_packet(self, packet: InputEnvelope) -> None:
+        with self.frame_lock:
+            self.last_seq_id = packet.seq_id
+            self.motion_frame.last_event_code = packet.event_code
+            self.motion_frame.last_rx_time = packet.sent_at
+
+            event_code = packet.event_code
+            if event_code == "pose_home":
+                self.motion_frame.mode_tag = "pose_home"
+            elif event_code == "pose_hold":
+                self.motion_frame.mode_tag = "pose_hold"
+            elif event_code == "mode_stride":
+                self.motion_frame.mode_tag = "mode_stride"
+            elif event_code == "mode_dash":
+                self.motion_frame.mode_tag = "mode_dash"
+            elif event_code == "mode_xrun":
+                self.motion_frame.mode_tag = "mode_xrun"
+            elif event_code == "surge_up":
+                self.motion_frame.surge_goal = min(
+                    self.max_surge, self.motion_frame.surge_goal + self.surge_step
+                )
+            elif event_code == "surge_down":
+                self.motion_frame.surge_goal = max(
+                    -self.max_surge, self.motion_frame.surge_goal - self.surge_step
+                )
+            elif event_code == "sway_left":
+                self.motion_frame.sway_goal = max(
+                    -self.max_sway, self.motion_frame.sway_goal - self.sway_step
+                )
+            elif event_code == "sway_right":
+                self.motion_frame.sway_goal = min(
+                    self.max_sway, self.motion_frame.sway_goal + self.sway_step
+                )
+            elif event_code == "spin_left":
+                self.motion_frame.spin_goal = max(
+                    -self.max_spin, self.motion_frame.spin_goal - self.spin_step
+                )
+            elif event_code == "spin_right":
+                self.motion_frame.spin_goal = min(
+                    self.max_spin, self.motion_frame.spin_goal + self.spin_step
+                )
+            elif event_code == "lift_up":
+                self.motion_frame.lift_goal = min(
+                    self.max_lift, self.motion_frame.lift_goal + self.lift_step
+                )
+            elif event_code == "lift_down":
+                self.motion_frame.lift_goal = max(
+                    self.min_lift, self.motion_frame.lift_goal - self.lift_step
+                )
+            elif event_code == "trim_reset":
+                self.motion_frame.surge_goal = 0.0
+                self.motion_frame.sway_goal = 0.0
+                self.motion_frame.spin_goal = 0.0
+            elif event_code == "session_quit":
+                self.motion_frame.relay_on = False
+                self.motion_frame.mode_tag = "session_quit"
+                self.control_running = False
+                self.recv_running = False
+
+            print(
+                f"[udp_loopback] seq={packet.seq_id} key={packet.key_name} "
+                f"{format_motion_frame(self.motion_frame)}"
+            )
+
+    def _precise_sleep_until(self, target_time: float) -> None:
+        current_time = time.perf_counter()
+        sleep_time = target_time - current_time
+        if sleep_time <= 0:
+            return
+        if sleep_time > 0.003:
+            time.sleep(sleep_time * 0.9)
+            while time.perf_counter() < target_time:
+                pass
+        else:
+            while time.perf_counter() < target_time:
+                pass
+
+    def destroy(self) -> None:
+        self.control_running = False
+        self.recv_running = False
+        if self.control_thread and self.control_thread.is_alive():
+            self.control_thread.join(timeout=1.0)
+        if self.recv_thread and self.recv_thread.is_alive():
+            self.recv_thread.join(timeout=1.0)
+        self.rx_socket.close()
+
+
+def main() -> None:
+    node = UDPLoopbackNode(debug=False)
+    try:
+        while node.control_running:
+            time.sleep(0.2)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy()
+
+
+if __name__ == "__main__":
+    main()

xSIM_MUJOCO/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

xSIM_MUJOCO/README.md

@@ -0,0 +1,109 @@
+# MuJoCo DEX 机器人仿真项目
+
+本项目是基于 MuJoCo 物理引擎的 DEX 机器人仿真环境，用于开发和测试机器人控制算法。
+
+## 项目结构
+
+```
+.
+├── README.md                # 项目说明文档
+├── resources                # 机器人模型文件
+│   ├── evt2
+└── scripts                      # Python 脚本目录
+    ├── convert_xml.py           # URDF 到 XML 转换工具
+    ├── elastic_band.py          # 弹性带控制器
+    └── simulator_view_asyn.py   # 异步仿真器（支持 ROS2）
+```
+
+## 主要功能
+
+### 1. 机器人模型
+- 包含完整的 DEX 机器人模型，具有多个自由度
+- 支持腿部、腰部和手臂关节控制
+- 集成 IMU 传感器和关节传感器
+
+### 2. 控制系统
+- 支持位置控制和力矩控制模式
+- 实现 PVD（位置-速度-力矩）混合控制器
+- 提供传感器数据读取接口
+
+### 3. 仿真功能
+- 实时物理仿真
+- 3D 可视化显示
+- 数据记录和绘图功能
+- ROS2 集成支持（通过 simulator_view_asyn.py）
+
+## 安装依赖
+
+```bash
+# 建议使用虚拟环境
+pip install mujoco numpy matplotlib pynput
+```
+bodyctrl_msg 位于xmigcs/lib/下
+```bash
+sudo dpkg -i ros-jazzy*.deb
+```
+## 使用方法
+
+### 异步仿真（支持 ROS2）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=your_domain_id
+python scripts/simulator_view_asyn.py -m evt2(机器人名称)
+```
+
+## 主要脚本说明
+
+### simulator_view_asyn.py
+异步版本的机器人仿真器，支持：
+- 多线程运行
+- ROS2 集成
+- 键盘控制
+- 实时数据发布
+
+### elastic_band.py
+弹性带控制器
+
+## 机器人关节结构
+
+DEX 机器人包含以下关节组：
+
+1. **腿部关节**：
+   - hip_pitch_l_joint, hip_roll_l_joint, hip_yaw_l_joint
+   - knee_pitch_l_joint, ankle_pitch_l_joint, ankle_roll_l_joint
+   - 右腿对应关节
+
+2. **腰部关节**：
+   - waist_yaw_joint, waist_roll_joint, waist_pitch_joint
+
+3. **手臂关节**：
+   - shoulder_pitch_l_joint, shoulder_roll_l_joint, shoulder_yaw_l_joint
+   - elbow_pitch_l_joint, elbow_yaw_l_joint, wrist_pitch_l_joint, wrist_roll_l_joint
+   - 右臂对应关节
+
+## 控制接口
+
+通过修改 `data.ctrl[]` 数组来控制关节力矩，通过读取 `data.sensordata[]` 获取传感器数据。
+
+## 键盘控制
+
+在仿真运行时：
+- end：切换力控、位控
+- numlock: 暂停、继续
+- up: 升起机器人
+- down: 降下机器人
+- ESC: 释放机器人，可以自由运动
+
+
+## 数据记录
+
+仿真器会自动记录以下数据用于后续分析和可视化：
+- 关节位置、速度、力矩
+- IMU 方向、加速度、位置数据
+
+## 注意事项
+
+1. 确保 MuJoCo 模型文件路径正确
+2. 如需 ROS2 支持，请先启动 ROS2 环境
+3. 仿真速度受系统性能影响，可通过调整 timestep 参数优化

xSIM_MUJOCO/resources/evt2/urdf/evt2.xml

@@ -0,0 +1,499 @@
+<mujoco model="evt2">
+  <compiler angle="radian" meshdir="../meshes_new/"/>
+  <option integrator="implicitfast" timestep='0.001' iterations='100' solver='CG' gravity='0 0 -9.81'>
+  <flag contact="enable" energy="enable"/>
+  </option>
+  <!-- 禁用位置传感器 -->
+  <option actuatorgroupdisable="0 1 3"/>
+
+
+
+
+  <default>
+    <default class="large_actuator">
+      <position kp="2000" kv="100" forcerange="-105 105"/>
+    </default>
+    <default class="small_actuator">
+      <position kp="500" kv="50" forcerange="-52 52"/>
+    </default>
+  </default>
+
+  <default>
+    <default class="arm_motor">
+      <joint damping="1" armature="0.1" frictionloss="0.1"/>
+    </default>
+    <default class="wrist_motor">
+      <joint damping="1" armature="0.0236" frictionloss="0.1"/>
+    </default>
+
+    <default class="waist_motor">
+      <joint damping="1" armature="0.17" frictionloss="0.1"/>
+    </default>
+
+    <default class="hip_pitch_motor">
+      <joint damping="1" armature="0.24" frictionloss="0.1"/>
+    </default>
+  
+    <default class="hip_roll_motor">
+      <joint damping="1" armature="0.24" frictionloss="0.1"/>
+    </default>
+  
+    <default class="hip_yaw_motor">
+      <joint damping="1" armature="0.18" frictionloss="0.1"/>
+    </default>
+  
+    <default class="knee_motor">
+      <joint damping="1" armature="0.37" frictionloss="0.1"/>
+    </default>
+  
+    <default class="ankle_motor">
+      <joint damping="1" armature="0.032" frictionloss="0.1"/>
+    </default>
+
+  </default>
+
+  <actuator>
+    <!-- 位置控制器（pos_前缀） -->
+    <!-- 左腿关节驱动器 -->
+    <position class="large_actuator" name="pos_hip_pitch_l_joint" joint="hip_pitch_l_joint" ctrlrange="-3.14159 3.14159" group="0"/>
+    <position class="large_actuator" name="pos_hip_roll_l_joint" joint="hip_roll_l_joint" ctrlrange="-0.5236 2.618" group="0"/>
+    <position class="large_actuator" name="pos_hip_yaw_l_joint" joint="hip_yaw_l_joint" ctrlrange="-1.5708 4.5379" group="0"/>
+    <position class="large_actuator" name="pos_knee_pitch_l_joint" joint="knee_pitch_l_joint" ctrlrange="-0.0873 2.4435" group="0"/>
+    <position class="large_actuator" name="pos_ankle_pitch_l_joint" joint="ankle_pitch_l_joint" ctrlrange="-1.2217 0.5236" group="0"/>
+    <position class="small_actuator" name="pos_ankle_roll_l_joint" joint="ankle_roll_l_joint" ctrlrange="-0.5236 0.5236" group="0"/>
+    
+    <!-- 右腿关节驱动器 -->
+    <position class="large_actuator" name="pos_hip_pitch_r_joint" joint="hip_pitch_r_joint" ctrlrange="-3.14159 3.14159" group="0"/>
+    <position class="large_actuator" name="pos_hip_roll_r_joint" joint="hip_roll_r_joint" ctrlrange="-2.618 0.5236" group="0"/>
+    <position class="large_actuator" name="pos_hip_yaw_r_joint" joint="hip_yaw_r_joint" ctrlrange="-4.5379 1.5708" group="0"/>
+    <position class="large_actuator" name="pos_knee_pitch_r_joint" joint="knee_pitch_r_joint" ctrlrange="-0.0873 2.4435" group="0"/>
+    <position class="large_actuator" name="pos_ankle_pitch_r_joint" joint="ankle_pitch_r_joint" ctrlrange="-1.2217 0.5236" group="0"/>
+    <position class="small_actuator" name="pos_ankle_roll_r_joint" joint="ankle_roll_r_joint" ctrlrange="-0.5236 0.5236" group="0"/>
+    
+    <!-- 腰部关节驱动器 -->
+    <position class="large_actuator" name="pos_waist_yaw_joint" joint="waist_yaw_joint" ctrlrange="-2.9671 3.1416" group="1"/>
+    <position class="large_actuator" name="pos_waist_roll_joint" joint="waist_roll_joint" ctrlrange="-0.5236 0.5236" group="1"/>
+    <position class="large_actuator" name="pos_waist_pitch_joint" joint="waist_pitch_joint" ctrlrange="-0.5236 1.0472" group="1"/>
+
+    <!-- 上肢关节驱动器 -->
+    <position class="large_actuator" name="pos_shoulder_pitch_l_joint" joint="shoulder_pitch_l_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_roll_l_joint" joint="shoulder_roll_l_joint" ctrlrange="-0.2618 3.4034" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_yaw_l_joint" joint="shoulder_yaw_l_joint" ctrlrange="-2.9671 2.9671" group="3" />
+    <position class="large_actuator" name="pos_elbow_pitch_l_joint" joint="elbow_pitch_l_joint" ctrlrange="-2.618 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_elbow_yaw_l_joint" joint="elbow_yaw_l_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_wrist_pitch_l_joint" joint="wrist_pitch_l_joint" ctrlrange="-1.309 1.65806" group="3"/>
+    <position class="small_actuator" name="pos_wrist_roll_l_joint" joint="wrist_roll_l_joint" ctrlrange="-1.0472 0.7854" group="3"/>
+    
+    <position class="large_actuator" name="pos_shoulder_pitch_r_joint" joint="shoulder_pitch_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_roll_r_joint" joint="shoulder_roll_r_joint" ctrlrange="-3.4034 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_yaw_r_joint" joint="shoulder_yaw_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_elbow_pitch_r_joint" joint="elbow_pitch_r_joint" ctrlrange="-2.618 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_elbow_yaw_r_joint" joint="elbow_yaw_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_wrist_pitch_r_joint" joint="wrist_pitch_r_joint" ctrlrange="-1.309 1.65806" group="3"/>
+    <position class="small_actuator" name="pos_wrist_roll_r_joint" joint="wrist_roll_r_joint" ctrlrange="-0.7854 1.0472" group="3"/>
+
+    <!-- 力控制器（motor_前缀） -->
+    <!-- 左腿关节驱动器 -->
+    <motor name="motor_hip_pitch_l_joint" joint="hip_pitch_l_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_roll_l_joint" joint="hip_roll_l_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_yaw_l_joint" joint="hip_yaw_l_joint" ctrlrange="-142 142" group="2"/>
+    <motor name="motor_knee_pitch_l_joint" joint="knee_pitch_l_joint" ctrlrange="-330 330" group="2"/>
+    <motor name="motor_ankle_pitch_l_joint" joint="ankle_pitch_l_joint" ctrlrange="-100 100" group="2"/>
+    <motor name="motor_ankle_roll_l_joint" joint="ankle_roll_l_joint" ctrlrange="-50 50" group="2"/>
+    
+    <!-- 右腿关节驱动器 -->
+    <motor name="motor_hip_pitch_r_joint" joint="hip_pitch_r_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_roll_r_joint" joint="hip_roll_r_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_yaw_r_joint" joint="hip_yaw_r_joint" ctrlrange="-142 142" group="2"/>
+    <motor name="motor_knee_pitch_r_joint" joint="knee_pitch_r_joint" ctrlrange="-330 330" group="2"/>
+    <motor name="motor_ankle_pitch_r_joint" joint="ankle_pitch_r_joint" ctrlrange="-100 100" group="2"/>
+    <motor name="motor_ankle_roll_r_joint" joint="ankle_roll_r_joint" ctrlrange="-50 50" group="2"/>
+
+    <!-- 腰部关节驱动器 -->
+    <motor name="motor_waist_yaw_joint" joint="waist_yaw_joint" ctrlrange="-91 91" group="2"/>
+    <motor name="motor_waist_roll_joint" joint="waist_roll_joint" ctrlrange="-91 91" group="2"/>
+    <motor name="motor_waist_pitch_joint" joint="waist_pitch_joint" ctrlrange="-91 91" group="2"/>
+
+    <!-- 上肢关节驱动器 -->
+    <motor name="motor_shoulder_pitch_l_joint" joint="shoulder_pitch_l_joint" ctrlrange="-90 90" group="2"/>
+    <motor name="motor_shoulder_roll_l_joint" joint="shoulder_roll_l_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_shoulder_yaw_l_joint" joint="shoulder_yaw_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_elbow_pitch_l_joint" joint="elbow_pitch_l_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_elbow_yaw_l_joint" joint="elbow_yaw_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_pitch_l_joint" joint="wrist_pitch_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_roll_l_joint" joint="wrist_roll_l_joint" ctrlrange="-36 36" group="2"/>
+    
+    <motor name="motor_shoulder_pitch_r_joint" joint="shoulder_pitch_r_joint" ctrlrange="-90 90" group="2"/>
+    <motor name="motor_shoulder_roll_r_joint" joint="shoulder_roll_r_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_shoulder_yaw_r_joint" joint="shoulder_yaw_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_elbow_pitch_r_joint" joint="elbow_pitch_r_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_elbow_yaw_r_joint" joint="elbow_yaw_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_pitch_r_joint" joint="wrist_pitch_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_roll_r_joint" joint="wrist_roll_r_joint" ctrlrange="-36 36" group="2"/>
+  </actuator>
+
+  <sensor>
+    <!-- 左腿关节传感器 -->
+    <jointpos name="hip_pitch_l_joint_pos" joint="hip_pitch_l_joint"/>
+    <jointpos name="hip_roll_l_joint_pos" joint="hip_roll_l_joint"/>
+    <jointpos name="hip_yaw_l_joint_pos" joint="hip_yaw_l_joint"/>
+    <jointpos name="knee_pitch_l_joint_pos" joint="knee_pitch_l_joint"/>
+    <jointpos name="ankle_pitch_l_joint_pos" joint="ankle_pitch_l_joint"/>
+    <jointpos name="ankle_roll_l_joint_pos" joint="ankle_roll_l_joint"/>
+    
+    <jointvel name="hip_pitch_l_joint_vel" joint="hip_pitch_l_joint"/>
+    <jointvel name="hip_roll_l_joint_vel" joint="hip_roll_l_joint"/>
+    <jointvel name="hip_yaw_l_joint_vel" joint="hip_yaw_l_joint"/>
+    <jointvel name="knee_pitch_l_joint_vel" joint="knee_pitch_l_joint"/>
+    <jointvel name="ankle_pitch_l_joint_vel" joint="ankle_pitch_l_joint"/>
+    <jointvel name="ankle_roll_l_joint_vel" joint="ankle_roll_l_joint"/>
+    
+    <jointactuatorfrc name="hip_pitch_l_joint_torque" joint="hip_pitch_l_joint"/>
+    <jointactuatorfrc name="hip_roll_l_joint_torque" joint="hip_roll_l_joint"/>
+    <jointactuatorfrc name="hip_yaw_l_joint_torque" joint="hip_yaw_l_joint"/>
+    <jointactuatorfrc name="knee_pitch_l_joint_torque" joint="knee_pitch_l_joint"/>
+    <jointactuatorfrc name="ankle_pitch_l_joint_torque" joint="ankle_pitch_l_joint"/>
+    <jointactuatorfrc name="ankle_roll_l_joint_torque" joint="ankle_roll_l_joint"/>
+    
+    <!-- 右腿关节传感器 -->
+    <jointpos name="hip_pitch_r_joint_pos" joint="hip_pitch_r_joint"/>
+    <jointpos name="hip_roll_r_joint_pos" joint="hip_roll_r_joint"/>
+    <jointpos name="hip_yaw_r_joint_pos" joint="hip_yaw_r_joint"/>
+    <jointpos name="knee_pitch_r_joint_pos" joint="knee_pitch_r_joint"/>
+    <jointpos name="ankle_pitch_r_joint_pos" joint="ankle_pitch_r_joint"/>
+    <jointpos name="ankle_roll_r_joint_pos" joint="ankle_roll_r_joint"/>
+    
+    <jointvel name="hip_pitch_r_joint_vel" joint="hip_pitch_r_joint"/>
+    <jointvel name="hip_roll_r_joint_vel" joint="hip_roll_r_joint"/>
+    <jointvel name="hip_yaw_r_joint_vel" joint="hip_yaw_r_joint"/>
+    <jointvel name="knee_pitch_r_joint_vel" joint="knee_pitch_r_joint"/>
+    <jointvel name="ankle_pitch_r_joint_vel" joint="ankle_pitch_r_joint"/>
+    <jointvel name="ankle_roll_r_joint_vel" joint="ankle_roll_r_joint"/>
+    
+    <jointactuatorfrc name="hip_pitch_r_joint_torque" joint="hip_pitch_r_joint"/>
+    <jointactuatorfrc name="hip_roll_r_joint_torque" joint="hip_roll_r_joint"/>
+    <jointactuatorfrc name="hip_yaw_r_joint_torque" joint="hip_yaw_r_joint"/>
+    <jointactuatorfrc name="knee_pitch_r_joint_torque" joint="knee_pitch_r_joint"/>
+    <jointactuatorfrc name="ankle_pitch_r_joint_torque" joint="ankle_pitch_r_joint"/>
+    <jointactuatorfrc name="ankle_roll_r_joint_torque" joint="ankle_roll_r_joint"/>
+    
+    <!-- 腰部关节传感器 -->
+    <jointpos name="waist_yaw_joint_pos" joint="waist_yaw_joint"/>
+    <jointpos name="waist_roll_joint_pos" joint="waist_roll_joint"/>
+    <jointpos name="waist_pitch_joint_pos" joint="waist_pitch_joint"/>
+    
+    <jointvel name="waist_yaw_joint_vel" joint="waist_yaw_joint"/>
+    <jointvel name="waist_roll_joint_vel" joint="waist_roll_joint"/>
+    <jointvel name="waist_pitch_joint_vel" joint="waist_pitch_joint"/>
+    
+    <jointactuatorfrc name="waist_yaw_joint_torque" joint="waist_yaw_joint"/>
+    <jointactuatorfrc name="waist_roll_joint_torque" joint="waist_roll_joint"/>
+    <jointactuatorfrc name="waist_pitch_joint_torque" joint="waist_pitch_joint"/>
+    
+    <!-- 左臂关节传感器 -->
+    <jointpos name="shoulder_pitch_l_joint_pos" joint="shoulder_pitch_l_joint"/>
+    <jointpos name="shoulder_roll_l_joint_pos" joint="shoulder_roll_l_joint"/>
+    <jointpos name="shoulder_yaw_l_joint_pos" joint="shoulder_yaw_l_joint"/>
+    <jointpos name="elbow_pitch_l_joint_pos" joint="elbow_pitch_l_joint"/>
+    <jointpos name="elbow_yaw_l_joint_pos" joint="elbow_yaw_l_joint"/>
+    <jointpos name="wrist_pitch_l_joint_pos" joint="wrist_pitch_l_joint"/>
+    <jointpos name="wrist_roll_l_joint_pos" joint="wrist_roll_l_joint"/>
+    
+    <jointvel name="shoulder_pitch_l_joint_vel" joint="shoulder_pitch_l_joint"/>
+    <jointvel name="shoulder_roll_l_joint_vel" joint="shoulder_roll_l_joint"/>
+    <jointvel name="shoulder_yaw_l_joint_vel" joint="shoulder_yaw_l_joint"/>
+    <jointvel name="elbow_pitch_l_joint_vel" joint="elbow_pitch_l_joint"/>
+    <jointvel name="elbow_yaw_l_joint_vel" joint="elbow_yaw_l_joint"/>
+    <jointvel name="wrist_pitch_l_joint_vel" joint="wrist_pitch_l_joint"/>
+    <jointvel name="wrist_roll_l_joint_vel" joint="wrist_roll_l_joint"/>
+    
+    <jointactuatorfrc name="shoulder_pitch_l_joint_torque" joint="shoulder_pitch_l_joint"/>
+    <jointactuatorfrc name="shoulder_roll_l_joint_torque" joint="shoulder_roll_l_joint"/>
+    <jointactuatorfrc name="shoulder_yaw_l_joint_torque" joint="shoulder_yaw_l_joint"/>
+    <jointactuatorfrc name="elbow_pitch_l_joint_torque" joint="elbow_pitch_l_joint"/>
+    <jointactuatorfrc name="elbow_yaw_l_joint_torque" joint="elbow_yaw_l_joint"/>
+    <jointactuatorfrc name="wrist_pitch_l_joint_torque" joint="wrist_pitch_l_joint"/>
+    <jointactuatorfrc name="wrist_roll_l_joint_torque" joint="wrist_roll_l_joint"/>
+    
+    <!-- 右臂关节传感器 -->
+    <jointpos name="shoulder_pitch_r_joint_pos" joint="shoulder_pitch_r_joint"/>
+    <jointpos name="shoulder_roll_r_joint_pos" joint="shoulder_roll_r_joint"/>
+    <jointpos name="shoulder_yaw_r_joint_pos" joint="shoulder_yaw_r_joint"/>
+    <jointpos name="elbow_pitch_r_joint_pos" joint="elbow_pitch_r_joint"/>
+    <jointpos name="elbow_yaw_r_joint_pos" joint="elbow_yaw_r_joint"/>
+    <jointpos name="wrist_pitch_r_joint_pos" joint="wrist_pitch_r_joint"/>
+    <jointpos name="wrist_roll_r_joint_pos" joint="wrist_roll_r_joint"/>
+    
+    <jointvel name="shoulder_pitch_r_joint_vel" joint="shoulder_pitch_r_joint"/>
+    <jointvel name="shoulder_roll_r_joint_vel" joint="shoulder_roll_r_joint"/>
+    <jointvel name="shoulder_yaw_r_joint_vel" joint="shoulder_yaw_r_joint"/>
+    <jointvel name="elbow_pitch_r_joint_vel" joint="elbow_pitch_r_joint"/>
+    <jointvel name="elbow_yaw_r_joint_vel" joint="elbow_yaw_r_joint"/>
+    <jointvel name="wrist_pitch_r_joint_vel" joint="wrist_pitch_r_joint"/>
+    <jointvel name="wrist_roll_r_joint_vel" joint="wrist_roll_r_joint"/>
+    
+    <jointactuatorfrc name="shoulder_pitch_r_joint_torque" joint="shoulder_pitch_r_joint"/>
+    <jointactuatorfrc name="shoulder_roll_r_joint_torque" joint="shoulder_roll_r_joint"/>
+    <jointactuatorfrc name="shoulder_yaw_r_joint_torque" joint="shoulder_yaw_r_joint"/>
+    <jointactuatorfrc name="elbow_pitch_r_joint_torque" joint="elbow_pitch_r_joint"/>
+    <jointactuatorfrc name="elbow_yaw_r_joint_torque" joint="elbow_yaw_r_joint"/>
+    <jointactuatorfrc name="wrist_pitch_r_joint_torque" joint="wrist_pitch_r_joint"/>
+    <jointactuatorfrc name="wrist_roll_r_joint_torque" joint="wrist_roll_r_joint"/>
+
+    <!-- imu -->
+    <framequat name="orientation" objtype="site" objname="imu" />
+    <gyro name="angular-velocity" site="imu" />
+    <accelerometer name="linear-acceleration" site="imu" />
+
+    <framepos name="position" objtype="site" objname="imu" />
+    <framelinvel name="linear-velocity" objtype="site" objname="imu" />
+    <magnetometer  name='magnetometer' site='imu'/> 
+
+  </sensor>
+
+  <asset>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
+        width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <texture name="texplane2" type="2d" builtin="checker" rgb1="1 0.3137 0.1843" rgb2="0.0 0.30196 0.38039"
+        width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true"/>
+    <material name="matplane2" reflectance="0.1" texture="texplane2" texrepeat="1 1" texuniform="true"/>
+    
+    <mesh name="pelvis" file="pelvis.STL"/>
+    <mesh name="hip_pitch_l_link" file="hip_pitch_l_link.STL"/>
+    <mesh name="hip_roll_l_link" file="hip_roll_l_link.STL"/>
+    <mesh name="hip_yaw_l_link" file="hip_yaw_l_link.STL"/>
+    <mesh name="knee_pitch_l_link" file="knee_pitch_l_link.STL"/>
+    <mesh name="ankle_pitch_l_link" file="ankle_pitch_l_link.STL"/>
+    <mesh name="ankle_roll_l_link" file="ankle_roll_l_link.STL"/>
+    <mesh name="hip_pitch_r_link" file="hip_pitch_r_link.STL"/>
+    <mesh name="hip_roll_r_link" file="hip_roll_r_link.STL"/>
+    <mesh name="hip_yaw_r_link" file="hip_yaw_r_link.STL"/>
+    <mesh name="knee_pitch_r_link" file="knee_pitch_r_link.STL"/>
+    <mesh name="ankle_pitch_r_link" file="ankle_pitch_r_link.STL"/>
+    <mesh name="ankle_roll_r_link" file="ankle_roll_r_link.STL"/>
+    <mesh name="waist_yaw_link" file="waist_yaw_link.STL"/>
+    <mesh name="waist_roll_link" file="waist_roll_link.STL"/>
+    <mesh name="waist_pitch_link" file="waist_pitch_link.STL"/>
+    <mesh name="head_yaw_link" file="head_yaw_link.STL"/>
+    <mesh name="head_pitch_link" file="head_pitch_link.STL"/>
+    <mesh name="camera_head_link" file="camera_head_link.STL"/>
+    <mesh name="radar_head_link" file="radar_head_link.STL"/>
+    <mesh name="imu_head_link" file="imu_head_link.STL"/>
+    <mesh name="shoulder_pitch_l_link" file="shoulder_pitch_l_link.STL"/>
+    <mesh name="shoulder_roll_l_link" file="shoulder_roll_l_link.STL"/>
+    <mesh name="shoulder_yaw_l_link" file="shoulder_yaw_l_link.STL"/>
+    <mesh name="elbow_pitch_l_link" file="elbow_pitch_l_link.STL"/>
+    <mesh name="elbow_yaw_l_link" file="elbow_yaw_l_link.STL"/>
+    <mesh name="wrist_pitch_l_link" file="wrist_pitch_l_link.STL"/>
+    <mesh name="wrist_roll_l_link" file="wrist_roll_l_link.STL"/>
+    <mesh name="left_tcp_link" file="left_tcp_link.STL"/>
+    <mesh name="shoulder_pitch_r_link" file="shoulder_pitch_r_link.STL"/>
+    <mesh name="shoulder_roll_r_link" file="shoulder_roll_r_link.STL"/>
+    <mesh name="shoulder_yaw_r_link" file="shoulder_yaw_r_link.STL"/>
+    <mesh name="elbow_pitch_r_link" file="elbow_pitch_r_link.STL"/>
+    <mesh name="elbow_yaw_r_link" file="elbow_yaw_r_link.STL"/>
+    <mesh name="wrist_pitch_r_link" file="wrist_pitch_r_link.STL"/>
+    <mesh name="wrist_roll_r_link" file="wrist_roll_r_link.STL"/>
+    <mesh name="right_tcp_link" file="right_tcp_link.STL"/>
+    <mesh name="camera_body_front_link" file="camera_body_front_link.STL"/>
+    <mesh name="imu_waist_link" file="imu_waist_link.STL"/>
+  </asset>
+
+  <worldbody>
+    <light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
+    <light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
+    <geom name="ground" type="plane" size="0 0 1" pos="0.001 0 0" quat="1 0 0 0" material="matplane" condim="1" conaffinity='15' friction="1.5 0.005 0.0001"/>
+    
+    <body name="pelvis" pos="0 0 1">
+      <inertial pos="0.000708938 -0.000476035 -0.0603041" quat="0.995372 0.0929922 0.0239168 -0.00377104" mass="10.5873" diaginertia="0.0891022 0.0725256 0.0697952"/>
+      <joint name="pelvis_to_world_joint" type="free" limited="false" actuatorfrclimited="false"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="pelvis"/>
+      <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="pelvis"/>
+      <geom pos="-0.081641 -0.00010952 -0.063057" quat="-2.59903e-06 -0.707565 0.706648 2.59566e-06" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="imu_waist_link"/>
+      <geom pos="-0.081641 -0.00010952 -0.063057" quat="-2.59903e-06 -0.707565 0.706648 2.59566e-06" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="imu_waist_link"/>
+      <site name='imu' size='0.01' pos='0.0 0.0 0.0'/>
+      <body name="hip_pitch_l_link" pos="-0.00010305 0.076815 -0.11847" quat="0.996195 -0.0871543 -5.84587e-05 0.000668198">
+        <inertial pos="0.009589 0.069327 -0.024358" quat="0.868301 0.493294 -0.0452228 -0.0258879" mass="2.22138" diaginertia="0.00485603 0.0037213 0.00328367"/>
+        <joint name="hip_pitch_l_joint" class="hip_pitch_motor" pos="0 0 0" axis="0 1 0" range="-3.14159 2.87979" actuatorfrcrange="-235 235"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_pitch_l_link"/>
+        <body name="hip_roll_l_link" pos="9.9595e-05 0.073992 -0.026966" quat="0.996195 0.0871543 0 0">
+          <inertial pos="-0.000767 -4.3e-05 -0.03918" quat="0.478756 0.521129 0.520225 0.478104" mass="0.560034" diaginertia="0.001743 0.00147771 0.00109729"/>
+          <joint name="hip_roll_l_joint" class="hip_roll_motor" pos="0 0 0" axis="1 0 0" range="-0.418879 2.61799" actuatorfrcrange="-235 235"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_roll_l_link"/>
+          <!-- <geom size="0.07 0.05" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/> -->
+          <body name="hip_yaw_l_link" pos="0 0 -0.0785">
+            <inertial pos="0.001014 -0.022912 -0.125892" quat="0.997124 -0.028488 0.00651602 -0.0699284" mass="5.65816" diaginertia="0.0456692 0.0428675 0.0143123"/>
+            <joint name="hip_yaw_l_joint" class="hip_yaw_motor" pos="0 0 0" axis="0 0 1" range="-1.39626 4.53786" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_l_link"/>
+            <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_l_link"/>
+            <body name="knee_pitch_l_link" pos="-0.012927 9.9662e-05 -0.30599">
+              <inertial pos="0.02218 -0.000444 -0.151985" quat="0.634112 0.0235196 0.0364207 0.772025" mass="3.15858" diaginertia="0.0348599 0.0346942 0.00377596"/>
+              <joint name="knee_pitch_l_joint" class="knee_motor" pos="0 0 0" axis="0 1 0" range="-0.0872665 2.53073" actuatorfrcrange="-400 400"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_l_link"/>
+              <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_l_link"/>
+              <body name="ankle_pitch_l_link" pos="0 0 -0.4">
+                <inertial pos="0 0 0" quat="0 0.707107 0 0.707107" mass="0.142658" diaginertia="4.9e-05 3.2e-05 2.3e-05"/>
+                <joint name="ankle_pitch_l_joint" class="ankle_motor" pos="0 0 0" axis="0 1 0" range="-1.22173 0.523599" actuatorfrcrange="-55 55"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_pitch_l_link"/>
+                <body name="ankle_roll_l_link">
+                  <inertial pos="0.024536 -1.7e-05 -0.033593" quat="0.000973792 0.727447 0.00122134 0.686162" mass="0.888892" diaginertia="0.00514091 0.004839 0.000764092"/>
+                  <joint name="ankle_roll_l_joint" class="ankle_motor" pos="0 0 0" axis="1 0 0" range="-0.523599 0.523599" actuatorfrcrange="-55 55"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_roll_l_link"/>
+                  <geom name="foot_left_front_outer" size="0.012 0.03" pos="0.085 -0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_front_inner" size="0.012 0.03" pos="0.085 0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_mid_outer" size="0.01 0.1" pos="0.045 -0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_mid_inner" size="0.01 0.1" pos="0.045 0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_outer" size="0.012 0.11" pos="0.045 -0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_center" size="0.012 0.12" pos="0.045 0 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_inner" size="0.012 0.11" pos="0.045 0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="hip_pitch_r_link" pos="0.00010305 -0.076815 -0.11847" quat="0.996195 0.0871543 5.84587e-05 0.000668198">
+        <inertial pos="0.009589 -0.069327 -0.024358" quat="0.493294 0.868301 0.0258879 0.0452228" mass="2.22138" diaginertia="0.00485603 0.0037213 0.00328367"/>
+        <joint name="hip_pitch_r_joint" class="hip_pitch_motor" pos="0 0 0" axis="0 1 0" range="-3.14159 2.87979" actuatorfrcrange="-235 235"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_pitch_r_link"/>
+        <body name="hip_roll_r_link" pos="9.9595e-05 -0.073992 -0.026966" quat="0.996195 -0.0871543 0 0">
+          <inertial pos="-0.000767 4.3e-05 -0.03918" quat="0.478104 0.520225 0.521129 0.478756" mass="0.560034" diaginertia="0.001743 0.00147771 0.00109729"/>
+          <joint name="hip_roll_r_joint" class="hip_roll_motor" pos="0 0 0" axis="1 0 0" range="-2.61799 0.418879" actuatorfrcrange="-235 235"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_roll_r_link"/>
+          <!-- <geom size="0.07 0.05" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/> -->
+          <body name="hip_yaw_r_link" pos="0 0 -0.0785">
+            <inertial pos="0.001014 0.022912 -0.125892" quat="0.997124 0.028488 0.00651602 0.0699284" mass="5.65816" diaginertia="0.0456692 0.0428675 0.0143123"/>
+            <joint name="hip_yaw_r_joint" class="hip_yaw_motor" pos="0 0 0" axis="0 0 1" range="-4.53786 1.39626" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_r_link"/>
+            <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_r_link"/>
+            <body name="knee_pitch_r_link" pos="-0.012927 -9.9662e-05 -0.30599">
+              <inertial pos="0.02218 0.000444 -0.151985" quat="0.772025 0.0364207 0.0235196 0.634112" mass="3.15858" diaginertia="0.0348599 0.0346942 0.00377596"/>
+              <joint name="knee_pitch_r_joint" class="knee_motor" pos="0 0 0" axis="0 1 0" range="-0.0872665 2.53073" actuatorfrcrange="-400 400"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_r_link"/>
+              <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_r_link"/>
+              <body name="ankle_pitch_r_link" pos="0 0 -0.4">
+                <inertial pos="0 0 0" quat="0 0.707107 0 0.707107" mass="0.142658" diaginertia="4.9e-05 3.2e-05 2.3e-05"/>
+                <joint name="ankle_pitch_r_joint" class="ankle_motor" pos="0 0 0" axis="0 1 0" range="-1.22173 0.523599" actuatorfrcrange="-55 55"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_pitch_r_link"/>
+                <body name="ankle_roll_r_link">
+                  <inertial pos="0.024536 1.7e-05 -0.033593" quat="-0.000973792 0.727447 -0.00122134 0.686162" mass="0.888892" diaginertia="0.00514091 0.004839 0.000764092"/>
+                  <joint name="ankle_roll_r_joint" class="ankle_motor" pos="0 0 0" axis="1 0 0" range="-0.523599 0.523599" actuatorfrcrange="-55 55"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_roll_r_link"/>
+                  <geom name="foot_right_front_outer" size="0.012 0.03" pos="0.085 -0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_front_inner" size="0.012 0.03" pos="0.085 0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_mid_outer" size="0.01 0.1" pos="0.045 -0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_mid_inner" size="0.01 0.1" pos="0.045 0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_outer" size="0.012 0.11" pos="0.045 -0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_center" size="0.012 0.12" pos="0.045 0 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_inner" size="0.012 0.11" pos="0.045 0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link" quat="1 0 0 0.00067075">
+        <inertial pos="-0.001947 -0.000934 0.044217" quat="0.642825 -0.123511 -0.167177 0.737273" mass="1.68837" diaginertia="0.00247429 0.00229232 0.0018544"/>
+        <joint name="waist_yaw_joint" class="waist_motor" pos="0 0 0" axis="0 0 1" range="-2.74017 3.22886" actuatorfrcrange="-91 91"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_yaw_link"/>
+        <body name="waist_roll_link" pos="0 0 0.0838">
+          <inertial pos="-0.006103 -0.006401 0.0721" quat="0.638305 0.66286 -0.234695 0.313212" mass="3.59519" diaginertia="0.0149204 0.0144797 0.00701787"/>
+          <joint name="waist_roll_joint" class="waist_motor" pos="0 0 0" axis="1 0 0" range="-0.436332 0.436332" actuatorfrcrange="-150 150"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_roll_link"/>
+          <body name="waist_pitch_link" pos="0 0 0.078424">
+            <inertial pos="0.00047912 0.00141024 0.212505" quat="0.999945 -0.00130091 0.0101935 -0.00226087" mass="11.8972" diaginertia="0.25491 0.225892 0.0979488"/>
+            <joint name="waist_pitch_joint" class="waist_motor" pos="0 0 0" axis="0 1 0" range="-0.523599 0.959931" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_pitch_link"/>
+            <!-- <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="waist_pitch_link"/> -->
+            <geom pos="0 0 0.43627" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="head_yaw_link"/>
+            <geom pos="0.0689 -7.7578e-05 0.43567" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="head_pitch_link"/>
+            <geom pos="0.08445 0.0111314 0.435546" quat="0.501222 -0.498774 0.498775 -0.501224" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="camera_head_link"/>
+            <geom pos="0 0 0.38177" quat="-3.67321e-06 -1 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="radar_head_link"/>
+            <geom pos="-0.057774 0 0.42677" quat="-2.59734e-06 -0.707105 0.707108 2.59735e-06" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="imu_head_link"/>
+            <geom pos="0.092709 -0.02375 0.039976" quat="0.21264 0.674376 0.674378 0.212641" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="camera_body_front_link"/>
+            <geom pos="0.092709 -0.02375 0.039976" quat="0.21264 0.674376 0.674378 0.212641" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="camera_body_front_link"/>
+            <body name="shoulder_pitch_l_link" pos="0 0.14552 0.18664" quat="0.999048 0.0436192 0 0">
+              <inertial pos="0.006861 0.060647 -2.7e-05" quat="-0.102177 0.698772 0.102786 0.700509" mass="0.914808" diaginertia="0.001164 0.000926669 0.000748328"/>
+              <joint name="shoulder_pitch_l_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.96706 2.96706" actuatorfrcrange="-90 90"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_pitch_l_link"/>
+              <body name="shoulder_roll_l_link" pos="0 0.0678 0">
+                <inertial pos="-0.000672 0 -0.043313" quat="0.707078 0.00642532 0.00642532 0.707078" mass="0.319153" diaginertia="0.000936 0.000764073 0.000543927"/>
+                <joint name="shoulder_roll_l_joint" class="arm_motor" pos="0 0 0" axis="1 0 0" range="-0.261799 3.40339" actuatorfrcrange="-90 90"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_roll_l_link"/>
+                <body name="shoulder_yaw_l_link" pos="0 0 -0.1004">
+                  <inertial pos="0.009141 0.003692 -0.094058" quat="0.996924 0.0324636 -0.0701011 -0.0132387" mass="1.50222" diaginertia="0.00793691 0.00755988 0.00117522"/>
+                  <joint name="shoulder_yaw_l_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_l_link"/>
+                  <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_l_link"/>
+                  <body name="elbow_pitch_l_link" pos="0.02 0 -0.1596">
+                    <inertial pos="-0.017311 0.000579 -0.066888" quat="0.996392 -0.023316 0.0810929 -0.00914165" mass="0.647211" diaginertia="0.00137312 0.00123605 0.000683824"/>
+                    <joint name="elbow_pitch_l_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.61799 0.261799" actuatorfrcrange="-50 50"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_pitch_l_link"/>
+                    <body name="elbow_yaw_l_link" pos="-0.02 0 -0.107">
+                      <inertial pos="0.001113 0.005481 -0.05106" quat="0.998888 -0.0470326 0.00308357 0.000463667" mass="0.229689" diaginertia="0.000368006 0.000290755 0.000205239"/>
+                      <joint name="elbow_yaw_l_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-25 25"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_yaw_l_link"/>
+                      <body name="wrist_pitch_l_link" pos="0 0 -0.091">
+                        <inertial pos="0.000815 0.001368 -0.031332" quat="0.703924 -0.0255071 0.00529546 0.709798" mass="0.931823" diaginertia="0.00155774 0.00154075 0.000635512"/>
+                        <joint name="wrist_pitch_l_joint" class="wrist_motor" pos="0 0 0" axis="0 1 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_l_link"/>
+                        <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_l_link"/>
+                        <body name="wrist_roll_l_link" pos="0 0 -0.062">
+                          <inertial pos="0.00513346 -0.00203116 -0.0838367" quat="0.780783 0.043744 0.102158 0.61484" mass="0.577436" diaginertia="0.0012863 0.00114426 0.000499739"/>
+                          <joint name="wrist_roll_l_joint" class="wrist_motor" pos="0 0 0" axis="1 0 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_roll_l_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="left_tcp_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="left_tcp_link"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="shoulder_pitch_r_link" pos="0 -0.14552 0.18664" quat="0.999048 -0.0436192 0 0">
+              <inertial pos="0.006861 -0.060647 -2.7e-05" quat="0.102177 0.698772 -0.102786 0.700509" mass="0.914808" diaginertia="0.001164 0.000926669 0.000748328"/>
+              <joint name="shoulder_pitch_r_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.96706 2.96706" actuatorfrcrange="-90 90"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_pitch_r_link"/>
+              <body name="shoulder_roll_r_link" pos="0 -0.0678 0">
+                <inertial pos="-0.000672 0 -0.043313" quat="0.707078 0.00642532 0.00642532 0.707078" mass="0.319153" diaginertia="0.000936 0.000764073 0.000543927"/>
+                <joint name="shoulder_roll_r_joint" class="arm_motor" pos="0 0 0" axis="1 0 0" range="-3.40339 0.261799" actuatorfrcrange="-90 90"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_roll_r_link"/>
+                <body name="shoulder_yaw_r_link" pos="0 0 -0.1004">
+                  <inertial pos="0.009141 -0.003692 -0.094058" quat="0.996924 -0.0324636 -0.0701011 0.0132387" mass="1.50222" diaginertia="0.00793691 0.00755988 0.00117522"/>
+                  <joint name="shoulder_yaw_r_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_r_link"/>
+                  <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_r_link"/>
+                  <body name="elbow_pitch_r_link" pos="0.02 0 -0.1596">
+                    <inertial pos="-0.017311 -0.000579 -0.066888" quat="0.996392 0.023316 0.0810929 0.00914165" mass="0.647211" diaginertia="0.00137312 0.00123605 0.000683824"/>
+                    <joint name="elbow_pitch_r_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.61799 0.261799" actuatorfrcrange="-50 50"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_pitch_r_link"/>
+                    <body name="elbow_yaw_r_link" pos="-0.02 0 -0.107">
+                      <inertial pos="0.001113 -0.005481 -0.05106" quat="0.998888 0.0470326 0.00308357 -0.000463667" mass="0.229689" diaginertia="0.000368006 0.000290755 0.000205239"/>
+                      <joint name="elbow_yaw_r_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-25 25"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_yaw_r_link"/>
+                      <body name="wrist_pitch_r_link" pos="0 0 -0.091">
+                        <inertial pos="0.000815 -0.001368 -0.031332" quat="0.709798 0.00529546 -0.0255071 0.703924" mass="0.931823" diaginertia="0.00155774 0.00154075 0.000635512"/>
+                        <joint name="wrist_pitch_r_joint" class="wrist_motor" pos="0 0 0" axis="0 1 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_r_link"/>
+                        <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_r_link"/>
+                        <body name="wrist_roll_r_link" pos="0 0 -0.062">
+                          <inertial pos="0.00513346 0.00203116 -0.0838367" quat="0.61484 0.102158 0.043744 0.780783" mass="0.577436" diaginertia="0.0012863 0.00114426 0.000499739"/>
+                          <joint name="wrist_roll_r_joint" class="wrist_motor" pos="0 0 0" axis="1 0 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_roll_r_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="right_tcp_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="right_tcp_link"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>