First Commit

2026-03-27 16:10:51 +08:00
commit c45245038f
103 changed files with 10994 additions and 0 deletions
--- a/Deploy_Tienkung/README.md
+++ b/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. 提交前运行所有测试确保无误
+
+## 许可证
+
+本项目仅供内部使用。
+
+## 项目状态
+
+项目正在积极开发中。