# webots_controller

**Repository Path**: csejoseph/webots_controller

## Basic Information

- **Project Name**: webots_controller
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-03-12
- **Last Updated**: 2026-04-14

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 快速开始

## 1. 项目简介

本项目用于在 Webots 中运行 TIAGo 右臂与右手仿真，并通过图形界面和 UDP JSON 指令控制机械臂运动与手部动作。

本指南覆盖以下内容：
- 首次运行所需环境配置
- world 与 controller 的启动流程
- 使用键盘 UDP 工具进行联调
- 外部程序接入 UDP 控制协议的方法

更详细的架构和实现说明请参考 `README_tiago.md`（总览文档请参考项目内 README 文档）。

---

## 2. 环境配置

### 2.1 安装 Webots

- 安装与项目匹配版本的 Webots（项目文件头部显示 `#VRML_SIM R2025a utf8`）。
- 建议先手动打开 world，确认场景能正常加载。

### 2.2 配置 Webots 使用的 Python 环境

本项目 controller 为 Python 脚本：
- `controllers/tiago_right_arm_ui_controller/tiago_right_arm_ui_controller.py`

需要确保 Webots 使用的 Python 解释器可导入 `numpy`，并由 Webots 提供 `controller` 模块运行时。

### 2.3 安装 Python 依赖

在目标 Python 环境中至少安装：

```bash
pip install numpy
```

说明：
- `tkinter` 通常随 Python 自带。
- `controller` 模块由 Webots 运行 controller 时提供，不需要单独 `pip install`。

### 2.4 配套关系说明

以下内容必须配套使用：
- world: `worlds/tiago_right_arm_table_cup.wbt`
- robot/proto: `worlds/projects/robots/pal_robotics/tiago_extensions/protos/TiagoRightArm.proto`、`TiagoRightHey5.proto`
- controller: `controllers/tiago_right_arm_ui_controller/tiago_right_arm_ui_controller.py`

### 2.5 最基本环境检查

1. 能在 Webots 打开 `worlds/tiago_right_arm_table_cup.wbt`。
2. 点击运行后，控制台不出现 controller 启动异常（尤其是 Python 解释器和模块导入错误）。
3. UI 窗口正常弹出，且第一个 tab 可见“UDP 控制”面板。

### 2.6 EXE Controller 方案（目标机不安装 Python）

如果你希望目标机不安装/不配置 Python，可以将 controller 打包为 `.exe`。

核心行为：
- Webots 会优先启动同名 `.exe`，其次才是同名 `.py`。
- `.py` 源码会保留，不会被改名，便于后续继续开发与重复打包。
- 目标机仍需安装 Webots（`controller` API 来自 Webots 运行时）。

打包脚本：
- `tools/build_webots_controller_exe.ps1`

### 2.6.1 打包前准备（构建机）

```powershell
# 一次性安装打包工具
pip install pyinstaller
```

### 2.6.2 常用打包命令

```powershell
# 仅打包 TIAGo
powershell -ExecutionPolicy Bypass -File .\tools\build_webots_controller_exe.ps1 -t tiago_right_arm_ui_controller

# 仅打包 UR5e
powershell -ExecutionPolicy Bypass -File .\tools\build_webots_controller_exe.ps1 -t ur5e_controller

# 打包默认全部控制器（等价于 -t all）
powershell -ExecutionPolicy Bypass -File .\tools\build_webots_controller_exe.ps1
```

### 2.6.3 参数与路径约定

- `-t <controller_name>`：按固定规则查找源码：
  `controllers/<controller_name>/<controller_name>.py`
- `-t all`（默认）：打包脚本内置的默认控制器集合。

### 2.6.4 WEBOTS_HOME 解析规则

脚本按以下顺序解析 Webots 安装目录：
1. 优先使用已设置且有效的环境变量 `WEBOTS_HOME`。
2. 若未设置，自动探测常见路径：
   - `C:\Program Files\Webots`
   - `C:\Program Files\Webots\msys64\mingw64`
   - `D:\Program Files\Webots`
   - `D:\Webots`
3. 若仍未找到，则报错并提示手动设置 `WEBOTS_HOME`。

### 2.6.5 打包产物

每个控制器打包后会生成：
- `controllers/<controller_name>/<controller_name>.exe`

源码文件保持不变：
- `controllers/<controller_name>/<controller_name>.py`

### 2.6.6 部署与回滚

部署到目标机：
1. 复制项目目录（至少包含对应 controller 目录下的 `.exe`）。
2. 安装与项目匹配版本的 Webots。
3. 打开 world 后直接运行。

回滚到 Python 运行方式：
1. 删除同名 `.exe`。
2. 保留的同名 `.py` 会自动成为 Webots 的下一优先入口。

### 2.6.7 常见问题

- 报错 `pyinstaller not found`：先执行 `pip install pyinstaller`。
- 报错 `Script not found`：检查 `-t` 参数是否与目录名、文件名完全一致。
- 目标机运行失败且提示 `controller` 模块相关问题：检查 Webots 安装是否完整，以及 `WEBOTS_HOME` 是否正确。
- `tools/keyboard_cartesian_teleop.py` 是独立调试工具，不影响 controller 的 exe 打包与运行。

---

## 3. 如何运行

### 3.1 打开 world 并启动仿真

1. 在 Webots 打开 `worlds/tiago_right_arm_table_cup.wbt`。
2. 确认 `TIAGO_RIG` 节点使用 controller `tiago_right_arm_ui_controller`。
3. 点击播放按钮启动仿真。

### 3.2 controller UI 主要区域

第一个 tab（末端位移控制）
- 左侧：末端位移步进按钮、手部开闭按钮、调试信息。
- 右侧：UDP 控制面板（监听配置、启用控制输入、状态显示）。

其他 tab
- 手臂关节控制（含 J6/J7 手动）
- 手部整体控制
- 手指关节控制

### 3.3 本地按钮控制

- 末端按钮以增量方式更新目标。
- IK 只求解 J1-J5。
- J6/J7 保持手动控制。
- Home/启动后，目标会同步到实测腕基点（`IK_WRIST_BASE_REF`）。

### 3.4 启动 UDP 监听与启用控制输入

在第一个 tab 右侧 UDP 面板按顺序操作：
1. 设置监听 IP/端口（默认 `0.0.0.0:9998`）。
2. 点击“启动监听”（网络层：绑定 socket 并接收 UDP）。
3. 勾选“启用 UDP 控制输入”（控制层：允许将已接收命令注入控制链）。

说明：
- “启动监听/停止监听”与“启用 UDP 控制输入”职责分离。
- 未启用控制输入时，仍会接收并解析命令，但不会驱动机械臂动作。

---

## 4. UDP 调试方法

### 4.1 调试脚本用途

`tools/keyboard_cartesian_teleop.py` 是本项目自带的 UDP 键盘发包工具，用于联调接收端。

### 4.2 运行方式

Windows PowerShell：

```powershell
python .\tools\keyboard_cartesian_teleop.py
```

可选参数：

```powershell
python .\tools\keyboard_cartesian_teleop.py --host 127.0.0.1 --port 9998 --step 5
```

参数说明：
- `--host` 默认 `127.0.0.1`
- `--port` 默认 `9998`
- `--step` 单步位移（厘米），默认 `5`

### 4.3 键位与命令

- `w/s/a/d/q/e`：发送 `move` 增量
- `o`：发送 `hand_open`
- `c`：发送 `hand_close`
- `h`：发送 `go_home`

说明：该脚本是调试工具，只负责发包，不负责状态闭环。

---

## 5. 如何接入 UDP 指令

### 5.1 协议与格式

- 协议：UDP
- 数据格式：JSON
- 默认监听：`0.0.0.0:9998`

### 5.2 接入前提

接收端需要先：
1. 点击“启动监听”
2. 再勾选“启用 UDP 控制输入”

### 5.3 命令语义

- `move`：`dx/dy/dz` 为机械臂基坐标系（arm-base）下的增量，单位米。
- `move` 不是世界坐标，不是绝对位置。
- `hand_open` / `hand_close` / `go_home` 是离散命令。

### 5.4 JSON 示例

```json
{"cmd": "move", "dx": 0.0, "dy": 0.0, "dz": 0.01}
```

```json
{"cmd": "hand_open"}
```

```json
{"cmd": "hand_close"}
```

```json
{"cmd": "go_home"}
```

---

## 6. 接收端限制与行为说明

### 6.1 UDP move 接收侧限速

接收端对 UDP `move` 执行接收侧限速，避免发送端异常高频导致目标无限制推进。

限速规则：
- 每周期最大允许注入位移 = `最大线速度 × timestep`
- 其中 `timestep` 来自仿真步长（毫秒转秒）

实现行为：
- 同一轮轮询中多个 `move` 先累加为一个总增量。
- 再对总增量做向量范数限幅。
- 最终将限幅后的增量注入控制链。

### 6.2 其他接收行为

- UDP 使用非阻塞 socket，在 `_sim_step()` 中轮询。
- 单条 `move` 先做分量限幅（防异常值）。
- 离散命令（`hand_open` / `hand_close` / `go_home`）按接收顺序处理。
- 当“启用 UDP 控制输入”关闭时：
  - 仍接收、解析、计数并更新状态
  - 不执行 move 注入和离散命令动作

---

## 7. 常见问题

### 7.1 已经收到 UDP 命令但机械臂不动

按顺序检查：
1. 是否已点击“启动监听”。
2. 是否已勾选“启用 UDP 控制输入”。
3. 是否仍处于启动阶段（startup 未完成时 move 会被忽略）。
4. UDP 面板“最近错误”是否有 JSON 解析或字段错误。
5. 外部发送地址端口是否与面板监听配置一致。

### 7.2 为什么 move 发得很大但机械臂实际移动较慢

原因通常是接收侧限速生效：
- 每周期允许注入的位移受“最大线速度 (m/s)”与 `timestep` 约束。
- 同时 IK 本身也有平滑步进过程，因此视觉上会更平滑而非瞬移。

可在 UDP 面板查看：
- “最大线速度”
- “每周期最大位移”
- “最近 move 限幅”

### 7.3 为什么 marker 的世界坐标变化方向和 move 的 dx/dy/dz 看起来不一致

这是坐标系差异导致：
- `move` 的 `dx/dy/dz` 定义在机械臂基坐标系。
- Marker 仅用于世界坐标显示。

因此在某些相机视角或机器人姿态下，世界系中的视觉方向与 arm-base 分量直觉不完全一致，这是当前设计的预期行为。