Compare commits
2 Commits
83b32542ef
...
d716ed5e67
| Author | SHA1 | Date | |
|---|---|---|---|
| d716ed5e67 | |||
| f00537ebb9 |
208
README.md
208
README.md
@@ -2,9 +2,9 @@
|
|||||||
|
|
||||||
> 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。
|
> 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。
|
||||||
|
|
||||||
ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机器人控制系统。
|
ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机器人系统。
|
||||||
|
|
||||||
## 赛事
|
## 赛事信息
|
||||||
|
|
||||||
**中国机器人及人工智能大赛**（**C**hina **R**obot and **A**rtificial **I**ntelligence **C**ompetition，简称 **CRAIC**）是由 [中国人工智能学会](https://www.caai.cn)（CAAI）主办的全国性学科竞赛，始于 1999 年，已列入**全国普通高等学校学科竞赛排行榜**及**教育部 A 类竞赛名单**。
|
**中国机器人及人工智能大赛**（**C**hina **R**obot and **A**rtificial **I**ntelligence **C**ompetition，简称 **CRAIC**）是由 [中国人工智能学会](https://www.caai.cn)（CAAI）主办的全国性学科竞赛，始于 1999 年，已列入**全国普通高等学校学科竞赛排行榜**及**教育部 A 类竞赛名单**。
|
||||||
|
|
||||||
@@ -23,68 +23,59 @@ ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机
|
|||||||
|
|
||||||
```
|
```
|
||||||
craic/
|
craic/
|
||||||
├── jxbeye/ # ESP32-S3 摄像头固件 (PlatformIO)
|
├── jxbeye/ # ESP32-S3 固件 (PlatformIO)
|
||||||
│ ├── src/main.cpp # 双核并行推流 + AsyncUDP + ESP-NOW
|
│ ├── src/main.cpp # 双核摄像头推流 + UDP 控制
|
||||||
│ ├── platformio.ini # 构建配置 (ESP32-S3-WROOM-1-N16R8)
|
│ └── platformio.ini # ESP32-S3-WROOM-1-N16R8 配置
|
||||||
│ ├── boards/ # 自定义板级定义
|
├── ros2/ # ROS 2 控制系统
|
||||||
│ ├── lib/FTServo/ # 飞特舵机控制库
|
│ ├── src/arm_control_msgs/ # 消息和服务定义
|
||||||
│ └── include/ / test/
|
│ └── src/udp_teleop/ # 机械臂控制和视觉抓取节点
|
||||||
├── ros2/ # ROS 2 Humble 键盘遥控
|
├── tools/ # 独立工具脚本
|
||||||
│ └── src/udp_teleop/ # 底盘 + 6轴机械臂 UDP 遥控节点
|
│ ├── udp_control.py # 命令行机械臂控制（带逆运动学）
|
||||||
├── tools/ # 调试工具
|
│ ├── camera_to_base.py # 相机坐标到基坐标变换
|
||||||
│ └── udp_server.py # UDP 回显测试服务器
|
│ └── camera_capture.py # MJPEG 流采集
|
||||||
└── .gitignore
|
└── docs/ # 技术文档
|
||||||
|
└── arm.md # 机械臂运动学推导
|
||||||
```
|
```
|
||||||
|
|
||||||
## 硬件
|
## 硬件
|
||||||
|
|
||||||
| 组件 | 型号 |
|
| 组件 | 型号 |
|
||||||
|------|------|
|
|------|------|
|
||||||
| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM Octal) |
|
| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM) |
|
||||||
| 摄像头 | OV2640 (XGA 1024×768, JPEG) |
|
| 摄像头 | OV2640 (XGA 1024×768, JPEG) |
|
||||||
| 舵机 | 飞特 SCS/STS 系列串行舵机 |
|
| 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 |
|
||||||
|
|
||||||
## 功能
|
## 核心功能
|
||||||
|
|
||||||
### 1. ESP32-S3 固件 (`jxbeye/`)
|
### 1. ESP32-S3 固件 (`jxbeye/`)
|
||||||
|
|
||||||
- **双核并行架构**：Core 0 负责摄像头采集，Core 1 负责 WiFi 推流
|
- **双核架构**：Core 0 采集，Core 1 推流
|
||||||
- **MJPEG 实时推流**：Web 浏览器直接访问 `http://<IP>` 查看实时画面
|
- **MJPEG 推流**：`http://<IP>` 实时查看
|
||||||
- **AsyncUDP 非阻塞通讯**：UDP 指令由中断回调处理，不干扰推流
|
- **UDP 控制**：端口 8888，非阻塞异步处理
|
||||||
- **ESP-NOW**：低延迟点对点通信
|
- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置
|
||||||
- **在线 WiFi 配置**：通过 Web 页面或串口修改 SSID/密码，自动保存到 NVS
|
|
||||||
|
|
||||||
### 2. ROS 2 键盘遥控 (`ros2/src/udp_teleop/`)
|
### 2. ROS 2 机械臂控制 (`ros2/`)
|
||||||
|
|
||||||
- **底盘遥控**：W/S 前进后退、A/D 左右平移、Q/E 旋转（差速驱动）
|
**arm_control 节点**：
|
||||||
- **机械臂遥控**：6 轴关节角度独立控制，高度升降
|
- 关节空间和笛卡尔空间运动控制
|
||||||
- **多平台键盘后端**：
|
- 完整逆运动学和正运动学
|
||||||
- `stdin` — Linux/macOS 终端原生输入，零依赖
|
- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper`
|
||||||
- `pynput` — 跨平台按键监听
|
- 状态发布（10Hz）
|
||||||
- `win_poll` — Windows Win32 API 轮询
|
|
||||||
- **可配置参数**：速度、步长、目标 IP/端口均通过 YAML 配置
|
|
||||||
|
|
||||||
### 3. UDP 通讯协议
|
**vision_grasp 节点**：
|
||||||
|
- 相机坐标到基坐标系自动转换
|
||||||
|
- 自动抓取：释放 → 移动 → 抓取 → 回收
|
||||||
|
- 自动释放：移动 → 释放 → 回收
|
||||||
|
|
||||||
所有指令通过 UDP 端口 `8888` 发送，格式：
|
### 3. 工具脚本 (`tools/`)
|
||||||
|
|
||||||
```
|
- `udp_control.py` - 命令行机械臂控制（支持逆运动学）
|
||||||
# 底盘控制
|
- `camera_to_base.py` - 坐标变换工具
|
||||||
XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
|
- `camera_capture.py` - 相机帧采集（支持自动扫描）
|
||||||
|
|
||||||
# 机械臂控制
|
|
||||||
JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
|
|
||||||
|
|
||||||
# 激光控制
|
|
||||||
LASERON / LASEROFF
|
|
||||||
|
|
||||||
# 串口透传
|
|
||||||
<any payload ending with ZHY or \n>
|
|
||||||
```
|
|
||||||
|
|
||||||
## 快速开始
|
## 快速开始
|
||||||
|
|
||||||
### 编译 & 烧录 ESP32 固件
|
### ESP32 固件
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd jxbeye
|
cd jxbeye
|
||||||
@@ -92,55 +83,134 @@ pio run -t upload
|
|||||||
pio device monitor
|
pio device monitor
|
||||||
```
|
```
|
||||||
|
|
||||||
首次启动后 ESP32 创建热点 `ESP32-S3-Camera`（密码 `12345678`），浏览器访问 `http://192.168.4.1`。
|
首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），访问 `http://192.168.4.1`。
|
||||||
|
|
||||||
### 运行 ROS 2 键盘遥控
|
### ROS 2 控制系统
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# 创建环境（首次）
|
# 环境准备（首次）
|
||||||
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
|
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
|
||||||
conda install -n ros2_humble -c robostack-staging -c conda-forge colcon-common-extensions
|
|
||||||
conda run -n ros2_humble pip install pynput
|
|
||||||
|
|
||||||
# 构建 & 运行
|
|
||||||
conda activate ros2_humble
|
conda activate ros2_humble
|
||||||
|
|
||||||
|
# 编译
|
||||||
cd ros2
|
cd ros2
|
||||||
colcon build --symlink-install --packages-select udp_teleop
|
export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
|
||||||
|
export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
|
||||||
|
export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
|
||||||
|
|
||||||
|
colcon build --packages-select arm_control_msgs \
|
||||||
|
--cmake-args \
|
||||||
|
-DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
|
||||||
|
-DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
|
||||||
|
-DPython_LIBRARY=$PYTHON_LIBRARY
|
||||||
|
|
||||||
|
colcon build --packages-select udp_teleop
|
||||||
source install/setup.bash
|
source install/setup.bash
|
||||||
|
|
||||||
# 启动键盘遥控（默认目标 192.168.233.67:8888）
|
# 运行机械臂控制
|
||||||
ros2 run udp_teleop keyboard_control \
|
ros2 run udp_teleop arm_control \
|
||||||
--ros-args --params-file src/udp_teleop/config/params.yaml
|
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||||||
|
|
||||||
|
# 运行视觉抓取（新终端）
|
||||||
|
ros2 run udp_teleop vision_grasp \
|
||||||
|
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
||||||
```
|
```
|
||||||
|
|
||||||
### 测试 UDP 通讯
|
### 命令行工具
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# 启动回显服务器
|
# 机械臂控制（关节空间）
|
||||||
python tools/udp_server.py
|
python tools/udp_control.py joints \
|
||||||
|
--height -100 --j2 10 --j3 20 --j4 30 --duration 2.0
|
||||||
|
|
||||||
# 发送测试指令
|
# 机械臂控制（笛卡尔空间）
|
||||||
echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
|
python tools/udp_control.py pose \
|
||||||
|
--x 200 --y 100 --z -100 --phi 45 --duration 2.0
|
||||||
|
|
||||||
|
# 相机采集
|
||||||
|
python tools/camera_capture.py --ip 192.168.4.1
|
||||||
|
python tools/camera_capture.py --scan # 自动扫描
|
||||||
```
|
```
|
||||||
|
|
||||||
### 串口配置 WiFi
|
## UDP 协议
|
||||||
|
|
||||||
在 ESP32 串口监视器中发送：
|
所有指令通过 UDP 端口 `8888` 发送：
|
||||||
|
|
||||||
```
|
```bash
|
||||||
WIFI:热点名称:密码
|
# 底盘控制（麦克纳姆轮）
|
||||||
|
XYW:<X>:<Y>:<W>:XZHY\n
|
||||||
|
|
||||||
|
# 机械臂控制（6 轴）
|
||||||
|
JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
|
||||||
|
|
||||||
|
# 激光控制
|
||||||
|
LASERON / LASEROFF
|
||||||
```
|
```
|
||||||
|
|
||||||
设备自动保存配置并重启。
|
## 使用示例
|
||||||
|
|
||||||
|
### ROS 服务调用
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 查询位姿
|
||||||
|
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
||||||
|
|
||||||
|
# 移动到目标位置
|
||||||
|
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
||||||
|
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
||||||
|
|
||||||
|
# 视觉抓取（发布相机坐标）
|
||||||
|
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
|
||||||
|
"{x: 10.0, y: 5.0, z: 250.0}"
|
||||||
|
```
|
||||||
|
|
||||||
|
### Python 集成
|
||||||
|
|
||||||
|
```python
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import Point
|
||||||
|
|
||||||
|
class VisionDetector:
|
||||||
|
def __init__(self):
|
||||||
|
self.grasp_pub = self.create_publisher(Point, 'vision_grasp/grasp_target', 10)
|
||||||
|
|
||||||
|
def on_detection(self, camera_x, camera_y, camera_z):
|
||||||
|
msg = Point()
|
||||||
|
msg.x = camera_x
|
||||||
|
msg.y = camera_y
|
||||||
|
msg.z = camera_z
|
||||||
|
self.grasp_pub.publish(msg)
|
||||||
|
```
|
||||||
|
|
||||||
|
## 文档
|
||||||
|
|
||||||
|
- **ROS 2 系统**：[ros2/README.md](ros2/README.md) - 完整的 ROS 节点文档
|
||||||
|
- **运动学推导**：[docs/arm.md](docs/arm.md) - 机械臂逆运动学数学推导
|
||||||
|
- **视觉标定**：[docs/vision_calibration_horizontal.md](docs/vision_calibration_horizontal.md) - 相机标定指南
|
||||||
|
|
||||||
|
## 坐标系说明
|
||||||
|
|
||||||
|
**机械臂基坐标系**（Z 轴朝上）：
|
||||||
|
- 原点：J1 线性滑轨底部
|
||||||
|
- X 轴：基座正前方
|
||||||
|
- Y 轴：基座左侧
|
||||||
|
- Z 轴：竖直向上（高度）
|
||||||
|
|
||||||
|
**相机坐标系**（水平安装）：
|
||||||
|
- X 轴：右侧
|
||||||
|
- Y 轴：向下
|
||||||
|
- Z 轴：正前方（光轴）
|
||||||
|
|
||||||
|
变换由 `camera_to_base.py` 和 `vision_grasp` 节点自动处理。
|
||||||
|
|
||||||
## 依赖
|
## 依赖
|
||||||
|
|
||||||
| 环境 | 依赖 |
|
| 环境 | 依赖 |
|
||||||
|------|------|
|
|------|------|
|
||||||
| ESP32 | PlatformIO, Arduino framework, esp32-camera |
|
| ESP32 | PlatformIO, Arduino framework, esp32-camera |
|
||||||
| ROS 2 | ROS 2 Humble (Conda robostack), pynput |
|
| ROS 2 | ROS 2 Humble (robostack), Python 3.12 |
|
||||||
| 测试 | Python 3, Unix netcat |
|
| 工具 | NumPy, OpenCV (可选) |
|
||||||
|
|
||||||
## 许可
|
## 许可
|
||||||
|
|
||||||
待定。
|
MIT License
|
||||||
|
|||||||
@@ -1,319 +0,0 @@
|
|||||||
# arm_control ROS 节点使用指南
|
|
||||||
|
|
||||||
## 概述
|
|
||||||
|
|
||||||
`arm_control` 是一个封装了机械臂控制功能的 ROS 2 节点，基于 `udp_control.py` 改造，提供服务接口进行机械臂控制。
|
|
||||||
|
|
||||||
## 功能特性
|
|
||||||
|
|
||||||
- ✅ 关节空间运动控制（带插值）
|
|
||||||
- ✅ 笛卡尔空间运动控制（带逆运动学）
|
|
||||||
- ✅ 正运动学查询
|
|
||||||
- ✅ 夹爪控制
|
|
||||||
- ✅ 状态发布（关节状态 + TCP 位姿）
|
|
||||||
- ✅ 状态缓存（平滑运动）
|
|
||||||
|
|
||||||
## 编译
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
|
|
||||||
# 1. 编译消息包
|
|
||||||
colcon build --packages-select arm_control_msgs
|
|
||||||
|
|
||||||
# 2. Source 消息包
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
# 3. 编译控制节点
|
|
||||||
colcon build --packages-select udp_teleop
|
|
||||||
|
|
||||||
# 4. Source 控制节点
|
|
||||||
source install/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
## 运行
|
|
||||||
|
|
||||||
### 启动控制节点
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 使用默认参数
|
|
||||||
ros2 run udp_teleop arm_control
|
|
||||||
|
|
||||||
# 使用配置文件
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
|
|
||||||
# 覆盖特定参数
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args -p udp_ip:=192.168.233.67 -p udp_port:=8888
|
|
||||||
```
|
|
||||||
|
|
||||||
## 服务接口
|
|
||||||
|
|
||||||
### 1. 关节空间运动
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \
|
|
||||||
"{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. 笛卡尔空间运动
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 基本运动
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
|
||||||
|
|
||||||
# 带夹爪控制
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, grip: true, duration: 2.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 查询当前位姿
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
```
|
|
||||||
|
|
||||||
输出示例：
|
|
||||||
```
|
|
||||||
success: true
|
|
||||||
message: ''
|
|
||||||
x: 150.234
|
|
||||||
y: 75.123
|
|
||||||
z: -100.0
|
|
||||||
phi: 45.678
|
|
||||||
height: -100
|
|
||||||
j2: 13
|
|
||||||
j3: 27
|
|
||||||
j4: 55
|
|
||||||
j5: 81
|
|
||||||
j6: 30
|
|
||||||
```
|
|
||||||
|
|
||||||
### 4. 夹爪控制
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 抓取
|
|
||||||
ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
|
|
||||||
"{grip: true}"
|
|
||||||
|
|
||||||
# 释放
|
|
||||||
ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
|
|
||||||
"{release: true}"
|
|
||||||
```
|
|
||||||
|
|
||||||
## 话题订阅
|
|
||||||
|
|
||||||
### 1. 关节状态
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 topic echo /arm_control/joint_states
|
|
||||||
```
|
|
||||||
|
|
||||||
输出：
|
|
||||||
```yaml
|
|
||||||
header:
|
|
||||||
stamp:
|
|
||||||
sec: 1234567890
|
|
||||||
nanosec: 123456789
|
|
||||||
frame_id: ''
|
|
||||||
height: -100
|
|
||||||
j2: 13
|
|
||||||
j3: 27
|
|
||||||
j4: 55
|
|
||||||
j5: 81
|
|
||||||
j6: 30
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. TCP 位姿
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 topic echo /arm_control/tcp_pose
|
|
||||||
```
|
|
||||||
|
|
||||||
输出：
|
|
||||||
```yaml
|
|
||||||
header:
|
|
||||||
stamp:
|
|
||||||
sec: 1234567890
|
|
||||||
nanosec: 123456789
|
|
||||||
frame_id: ''
|
|
||||||
x: 150.234
|
|
||||||
y: 75.123
|
|
||||||
z: -100.0
|
|
||||||
phi: 45.678
|
|
||||||
```
|
|
||||||
|
|
||||||
## Python 客户端示例
|
|
||||||
|
|
||||||
```python
|
|
||||||
#!/usr/bin/env python3
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from arm_control_msgs.srv import MovePose
|
|
||||||
|
|
||||||
class MyArmController(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('my_controller')
|
|
||||||
self.cli = self.create_client(MovePose, 'arm_control/move_pose')
|
|
||||||
self.cli.wait_for_service()
|
|
||||||
|
|
||||||
def move_to(self, x, y, z, phi):
|
|
||||||
req = MovePose.Request()
|
|
||||||
req.x = x
|
|
||||||
req.y = y
|
|
||||||
req.z = z
|
|
||||||
req.phi = phi
|
|
||||||
req.duration = 2.0
|
|
||||||
|
|
||||||
future = self.cli.call_async(req)
|
|
||||||
rclpy.spin_until_future_complete(self, future)
|
|
||||||
return future.result().success
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
controller = MyArmController()
|
|
||||||
|
|
||||||
# 移动到目标位置
|
|
||||||
controller.move_to(200.0, 100.0, -100.0, 45.0)
|
|
||||||
|
|
||||||
controller.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
```
|
|
||||||
|
|
||||||
## 完整抓取流程示例
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 运行示例客户端（包含完整抓取流程）
|
|
||||||
ros2 run udp_teleop arm_control_client
|
|
||||||
```
|
|
||||||
|
|
||||||
或手动调用：
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 1. 查询当前位姿
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
|
|
||||||
# 2. 移动到物体上方
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, release: true, duration: 2.0}"
|
|
||||||
|
|
||||||
# 3. 下降到抓取位置
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -150.0, phi: 45.0, release: true, duration: 1.0}"
|
|
||||||
|
|
||||||
# 4. 抓取
|
|
||||||
ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
|
|
||||||
"{grip: true}"
|
|
||||||
|
|
||||||
# 5. 提升
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, grip: true, duration: 1.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
## 参数配置
|
|
||||||
|
|
||||||
编辑 `config/arm_control.yaml`：
|
|
||||||
|
|
||||||
```yaml
|
|
||||||
arm_control:
|
|
||||||
ros__parameters:
|
|
||||||
# UDP 配置
|
|
||||||
udp_ip: '192.168.4.1'
|
|
||||||
udp_port: 8888
|
|
||||||
|
|
||||||
# 机械臂几何参数
|
|
||||||
l1: 125.0
|
|
||||||
l2: 125.0
|
|
||||||
x4: 110.0
|
|
||||||
z4: 80.0
|
|
||||||
|
|
||||||
# 关节限位
|
|
||||||
height_min: -290
|
|
||||||
height_max: 0
|
|
||||||
j2_min: -110
|
|
||||||
j2_max: 115
|
|
||||||
# ... (更多参数见配置文件)
|
|
||||||
```
|
|
||||||
|
|
||||||
## 调试
|
|
||||||
|
|
||||||
### 查看服务列表
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 service list | grep arm_control
|
|
||||||
```
|
|
||||||
|
|
||||||
### 查看话题列表
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 topic list | grep arm_control
|
|
||||||
```
|
|
||||||
|
|
||||||
### 查看服务接口定义
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 interface show arm_control_msgs/srv/MovePose
|
|
||||||
```
|
|
||||||
|
|
||||||
### 实时监控状态
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 终端 1: 查看关节状态
|
|
||||||
ros2 topic echo /arm_control/joint_states
|
|
||||||
|
|
||||||
# 终端 2: 查看 TCP 位姿
|
|
||||||
ros2 topic echo /arm_control/tcp_pose
|
|
||||||
|
|
||||||
# 终端 3: 发送控制命令
|
|
||||||
ros2 service call /arm_control/move_pose ...
|
|
||||||
```
|
|
||||||
|
|
||||||
## 常见问题
|
|
||||||
|
|
||||||
### Q1: 服务调用失败
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
1. 节点是否正在运行？`ros2 node list`
|
|
||||||
2. UDP 连接是否正常？检查 `udp_ip` 参数
|
|
||||||
3. 关节限位是否合理？查看错误消息
|
|
||||||
|
|
||||||
### Q2: 运动不平滑
|
|
||||||
|
|
||||||
**调整参数**：
|
|
||||||
- 增加 `duration`（运动时长）
|
|
||||||
- 增加 `default_rate`（插值频率）
|
|
||||||
|
|
||||||
### Q3: 状态不更新
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
- `use_state_cache` 是否启用？
|
|
||||||
- `tools/.udp_control_state.json` 是否可写？
|
|
||||||
|
|
||||||
## 与原始 udp_control.py 对比
|
|
||||||
|
|
||||||
| 功能 | udp_control.py | arm_control 节点 |
|
|
||||||
|------|---------------|-----------------|
|
|
||||||
| 接口 | 命令行 | ROS 服务 + 话题 |
|
|
||||||
| 集成 | 独立脚本 | ROS 生态系统 |
|
|
||||||
| 状态查询 | 文件缓存 | 服务调用 |
|
|
||||||
| 多客户端 | 不支持 | 支持 |
|
|
||||||
| 实时监控 | 不支持 | 话题订阅 |
|
|
||||||
|
|
||||||
## 下一步
|
|
||||||
|
|
||||||
- 集成视觉系统：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务
|
|
||||||
- 添加轨迹规划：创建轨迹规划器，生成平滑路径
|
|
||||||
- 碰撞检测：添加工作空间限制和碰撞检测
|
|
||||||
|
|
||||||
## 相关文件
|
|
||||||
|
|
||||||
- 节点实现：`udp_teleop/arm_control.py`
|
|
||||||
- 消息定义：`arm_control_msgs/msg/`
|
|
||||||
- 服务定义：`arm_control_msgs/srv/`
|
|
||||||
- 配置文件：`udp_teleop/config/arm_control.yaml`
|
|
||||||
- 示例客户端：`udp_teleop/arm_control_client.py`
|
|
||||||
@@ -1,122 +0,0 @@
|
|||||||
# 编译成功！🎉
|
|
||||||
|
|
||||||
## ✅ 已完成
|
|
||||||
|
|
||||||
1. **消息包编译** - arm_control_msgs ✓
|
|
||||||
2. **控制节点编译** - udp_teleop ✓
|
|
||||||
|
|
||||||
## 🚀 快速测试
|
|
||||||
|
|
||||||
### 1. 启动控制节点
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 激活环境
|
|
||||||
conda activate ros2_humble
|
|
||||||
|
|
||||||
# Source 工作空间
|
|
||||||
cd ros2
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
# 启动节点（修改 IP 为你的 ESP32 IP）
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. 测试服务（新终端）
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 激活环境
|
|
||||||
conda activate ros2_humble
|
|
||||||
cd ros2
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
# 查询当前位姿
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
|
|
||||||
# 移动到指定位置
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 查看状态
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查看关节状态
|
|
||||||
ros2 topic echo /arm_control/joint_states
|
|
||||||
|
|
||||||
# 查看 TCP 位姿
|
|
||||||
ros2 topic echo /arm_control/tcp_pose
|
|
||||||
|
|
||||||
# 查看所有服务
|
|
||||||
ros2 service list | grep arm_control
|
|
||||||
```
|
|
||||||
|
|
||||||
## ⚠️ 重要提示
|
|
||||||
|
|
||||||
### 编译说明
|
|
||||||
|
|
||||||
由于 robostack 的 Python 配置问题，编译时需要显式指定 Python 路径：
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 已在 build_arm_control.sh 中自动处理
|
|
||||||
export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
|
|
||||||
export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
|
|
||||||
export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
|
|
||||||
```
|
|
||||||
|
|
||||||
### 修改配置
|
|
||||||
|
|
||||||
编辑 `src/udp_teleop/config/arm_control.yaml` 修改参数：
|
|
||||||
|
|
||||||
```yaml
|
|
||||||
arm_control:
|
|
||||||
ros__parameters:
|
|
||||||
udp_ip: '192.168.4.1' # 修改为你的 ESP32 IP
|
|
||||||
udp_port: 8888
|
|
||||||
```
|
|
||||||
|
|
||||||
修改后直接重启节点即可，无需重新编译。
|
|
||||||
|
|
||||||
## 📝 下一步
|
|
||||||
|
|
||||||
1. **修改 ESP32 IP**: 编辑 `config/arm_control.yaml`
|
|
||||||
2. **测试连接**: 启动节点，查看是否有错误
|
|
||||||
3. **调用服务**: 使用上面的命令测试
|
|
||||||
4. **运行示例**: `ros2 run udp_teleop arm_control_client`
|
|
||||||
|
|
||||||
## 🐛 故障排查
|
|
||||||
|
|
||||||
### 问题：找不到服务
|
|
||||||
|
|
||||||
**解决**：
|
|
||||||
```bash
|
|
||||||
# 检查节点是否运行
|
|
||||||
ros2 node list
|
|
||||||
|
|
||||||
# 重新 source 环境
|
|
||||||
source install/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
### 问题：UDP 发送失败
|
|
||||||
|
|
||||||
**解决**：
|
|
||||||
1. 检查 ESP32 IP 是否正确
|
|
||||||
2. 测试网络连接：`ping 192.168.4.1`
|
|
||||||
3. 测试 UDP：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888`
|
|
||||||
|
|
||||||
### 问题：重新编译
|
|
||||||
|
|
||||||
**解决**：
|
|
||||||
```bash
|
|
||||||
# 清理后重新编译
|
|
||||||
rm -rf build/ install/ log/
|
|
||||||
./build_arm_control.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📚 文档
|
|
||||||
|
|
||||||
- 完整文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md)
|
|
||||||
- 快速指南：[QUICKSTART.md](QUICKSTART.md)
|
|
||||||
- 实现总结：[IMPLEMENTATION_SUMMARY.md](IMPLEMENTATION_SUMMARY.md)
|
|
||||||
|
|
||||||
祝使用愉快！🎉
|
|
||||||
@@ -1,252 +0,0 @@
|
|||||||
# arm_control ROS 节点封装总结
|
|
||||||
|
|
||||||
## ✅ 完成的工作
|
|
||||||
|
|
||||||
### 1. 创建了消息和服务定义包 (`arm_control_msgs`)
|
|
||||||
|
|
||||||
**消息类型**：
|
|
||||||
- `TCPPose.msg` - TCP 位姿（x, y, z, phi）
|
|
||||||
- `JointState.msg` - 关节状态（height, j2-j6）
|
|
||||||
|
|
||||||
**服务类型**：
|
|
||||||
- `MoveJoints.srv` - 关节空间运动控制
|
|
||||||
- `MovePose.srv` - 笛卡尔空间运动控制（带逆运动学）
|
|
||||||
- `GetPose.srv` - 查询当前位姿（正运动学）
|
|
||||||
- `SetGripper.srv` - 夹爪控制
|
|
||||||
|
|
||||||
### 2. 封装了控制节点 (`arm_control.py`)
|
|
||||||
|
|
||||||
**核心功能**：
|
|
||||||
- ✅ 关节空间插值运动
|
|
||||||
- ✅ 笛卡尔空间逆运动学求解
|
|
||||||
- ✅ 正运动学位姿计算
|
|
||||||
- ✅ UDP 命令发送（与 ESP32 通信）
|
|
||||||
- ✅ 状态缓存（平滑运动）
|
|
||||||
- ✅ 参数化配置
|
|
||||||
- ✅ 状态发布（10Hz）
|
|
||||||
|
|
||||||
**服务接口**：
|
|
||||||
- `/arm_control/move_joints` - 关节运动
|
|
||||||
- `/arm_control/move_pose` - 位姿运动
|
|
||||||
- `/arm_control/get_pose` - 查询位姿
|
|
||||||
- `/arm_control/set_gripper` - 夹爪控制
|
|
||||||
|
|
||||||
**话题发布**：
|
|
||||||
- `/arm_control/joint_states` - 关节状态（10Hz）
|
|
||||||
- `/arm_control/tcp_pose` - TCP 位姿（10Hz）
|
|
||||||
|
|
||||||
### 3. 创建了示例客户端 (`arm_control_client.py`)
|
|
||||||
|
|
||||||
**演示功能**：
|
|
||||||
- 查询当前位姿
|
|
||||||
- 完整抓取流程：
|
|
||||||
1. 移动到物体上方
|
|
||||||
2. 下降
|
|
||||||
3. 抓取
|
|
||||||
4. 提升
|
|
||||||
5. 移动到目标位置
|
|
||||||
6. 下降
|
|
||||||
7. 释放
|
|
||||||
8. 提升
|
|
||||||
|
|
||||||
### 4. 配置和文档
|
|
||||||
|
|
||||||
**配置文件**：
|
|
||||||
- `config/arm_control.yaml` - 完整参数配置
|
|
||||||
|
|
||||||
**文档**：
|
|
||||||
- `ARM_CONTROL_README.md` - 完整使用文档
|
|
||||||
- `QUICKSTART.md` - 快速开始指南
|
|
||||||
|
|
||||||
**脚本**：
|
|
||||||
- `build_arm_control.sh` - 一键编译脚本
|
|
||||||
|
|
||||||
## 📁 文件清单
|
|
||||||
|
|
||||||
```
|
|
||||||
ros2/
|
|
||||||
├── build_arm_control.sh # 编译脚本 ✨
|
|
||||||
├── ARM_CONTROL_README.md # 完整文档 ✨
|
|
||||||
├── QUICKSTART.md # 快速指南 ✨
|
|
||||||
└── src/
|
|
||||||
├── arm_control_msgs/ # 消息包 ✨
|
|
||||||
│ ├── CMakeLists.txt
|
|
||||||
│ ├── package.xml
|
|
||||||
│ ├── msg/
|
|
||||||
│ │ ├── TCPPose.msg
|
|
||||||
│ │ └── JointState.msg
|
|
||||||
│ └── srv/
|
|
||||||
│ ├── MoveJoints.srv
|
|
||||||
│ ├── MovePose.srv
|
|
||||||
│ ├── GetPose.srv
|
|
||||||
│ └── SetGripper.srv
|
|
||||||
└── udp_teleop/
|
|
||||||
├── setup.py # 已更新 ✨
|
|
||||||
├── package.xml # 已更新 ✨
|
|
||||||
├── udp_teleop/
|
|
||||||
│ ├── keyboard_control.py # 原有
|
|
||||||
│ ├── arm_control.py # 新增 ✨
|
|
||||||
│ └── arm_control_client.py # 新增 ✨
|
|
||||||
└── config/
|
|
||||||
├── params.yaml # 原有
|
|
||||||
└── arm_control.yaml # 新增 ✨
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🚀 快速使用
|
|
||||||
|
|
||||||
### 编译
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
./build_arm_control.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
### 运行节点
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
### 测试服务
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查询位姿
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
|
|
||||||
# 移动
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 运行示例
|
|
||||||
|
|
||||||
```bash
|
|
||||||
ros2 run udp_teleop arm_control_client
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🎯 与原始 udp_control.py 对比
|
|
||||||
|
|
||||||
| 特性 | udp_control.py | arm_control 节点 |
|
|
||||||
|------|---------------|-----------------|
|
|
||||||
| **接口方式** | 命令行参数 | ROS 服务调用 |
|
|
||||||
| **状态查询** | 读取 JSON 文件 | 服务调用 + 话题订阅 |
|
|
||||||
| **多客户端** | ❌ 不支持 | ✅ 支持 |
|
|
||||||
| **实时监控** | ❌ 无 | ✅ 10Hz 状态发布 |
|
|
||||||
| **参数配置** | 命令行参数 | YAML 配置文件 |
|
|
||||||
| **集成度** | 独立工具 | ROS 生态集成 |
|
|
||||||
| **可编程性** | Shell 脚本 | Python/C++ 客户端 |
|
|
||||||
|
|
||||||
## 💡 优势
|
|
||||||
|
|
||||||
### 1. **标准化接口**
|
|
||||||
- 使用 ROS 服务和话题，符合 ROS 生态标准
|
|
||||||
- 易于与其他 ROS 节点集成（如视觉、规划器）
|
|
||||||
|
|
||||||
### 2. **多客户端支持**
|
|
||||||
- 多个客户端可同时连接
|
|
||||||
- 适合复杂系统（如视觉 + 手动控制）
|
|
||||||
|
|
||||||
### 3. **实时状态监控**
|
|
||||||
- 10Hz 状态发布
|
|
||||||
- 可用于可视化、日志记录、故障诊断
|
|
||||||
|
|
||||||
### 4. **灵活配置**
|
|
||||||
- YAML 参数文件
|
|
||||||
- 运行时参数覆盖
|
|
||||||
- 无需重新编译
|
|
||||||
|
|
||||||
### 5. **易于扩展**
|
|
||||||
- 添加新服务：只需定义 .srv 文件
|
|
||||||
- 添加新话题：只需定义 .msg 文件
|
|
||||||
- 集成其他功能：订阅/发布话题即可
|
|
||||||
|
|
||||||
## 🔧 使用场景
|
|
||||||
|
|
||||||
### 场景 1：视觉抓取
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 视觉节点订阅相机话题，检测物体
|
|
||||||
# 调用 arm_control 服务控制机械臂
|
|
||||||
class VisionGraspNode(Node):
|
|
||||||
def __init__(self):
|
|
||||||
self.arm_cli = self.create_client(MovePose, 'arm_control/move_pose')
|
|
||||||
self.sub = self.create_subscription(Image, '/camera/image', self.on_image, 10)
|
|
||||||
|
|
||||||
def on_image(self, msg):
|
|
||||||
# 检测物体
|
|
||||||
x, y, z = detect_object(msg)
|
|
||||||
|
|
||||||
# 控制机械臂抓取
|
|
||||||
self.move_to(x, y, z, phi=45.0)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 场景 2：示教编程
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 记录示教点位
|
|
||||||
class TeachPendant(Node):
|
|
||||||
def __init__(self):
|
|
||||||
self.get_cli = self.create_client(GetPose, 'arm_control/get_pose')
|
|
||||||
self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
|
|
||||||
self.waypoints = []
|
|
||||||
|
|
||||||
def record_waypoint(self):
|
|
||||||
# 记录当前位置
|
|
||||||
pose = self.get_current_pose()
|
|
||||||
self.waypoints.append(pose)
|
|
||||||
|
|
||||||
def replay(self):
|
|
||||||
# 重放示教轨迹
|
|
||||||
for pose in self.waypoints:
|
|
||||||
self.move_to(pose.x, pose.y, pose.z, pose.phi)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 场景 3：轨迹规划
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 使用规划器生成轨迹
|
|
||||||
class TrajectoryPlanner(Node):
|
|
||||||
def __init__(self):
|
|
||||||
self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
|
|
||||||
|
|
||||||
def execute_trajectory(self, waypoints):
|
|
||||||
# 执行轨迹点序列
|
|
||||||
for wp in waypoints:
|
|
||||||
self.move_to(wp.x, wp.y, wp.z, wp.phi, duration=0.5)
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📚 下一步建议
|
|
||||||
|
|
||||||
### 1. **视觉集成**
|
|
||||||
创建视觉抓取节点，结合 `camera_to_base.py` 实现自动抓取
|
|
||||||
|
|
||||||
### 2. **GUI 控制面板**
|
|
||||||
使用 RQt 创建图形界面，实时显示状态和控制
|
|
||||||
|
|
||||||
### 3. **轨迹记录与回放**
|
|
||||||
实现示教编程功能
|
|
||||||
|
|
||||||
### 4. **碰撞检测**
|
|
||||||
添加工作空间限制和简单碰撞检测
|
|
||||||
|
|
||||||
### 5. **MoveIt 集成**
|
|
||||||
创建 URDF 和 MoveIt 配置，使用高级运动规划
|
|
||||||
|
|
||||||
## 🎓 学习资源
|
|
||||||
|
|
||||||
- ROS 2 服务教程：https://docs.ros.org/en/humble/Tutorials/Services.html
|
|
||||||
- ROS 2 话题教程：https://docs.ros.org/en/humble/Tutorials/Topics.html
|
|
||||||
- 自定义消息：https://docs.ros.org/en/humble/Tutorials/Custom-ROS2-Interfaces.html
|
|
||||||
|
|
||||||
## ✨ 总结
|
|
||||||
|
|
||||||
现在你有了一个完整的 ROS 节点化的机械臂控制系统：
|
|
||||||
|
|
||||||
1. ✅ **功能完整** - 保留了 udp_control.py 的所有功能
|
|
||||||
2. ✅ **接口标准** - 使用 ROS 服务和话题
|
|
||||||
3. ✅ **易于集成** - 可与其他 ROS 节点无缝配合
|
|
||||||
4. ✅ **文档齐全** - 提供了完整的文档和示例
|
|
||||||
5. ✅ **开箱即用** - 一键编译，快速上手
|
|
||||||
|
|
||||||
祝你使用愉快！🎉
|
|
||||||
@@ -1,244 +0,0 @@
|
|||||||
# 机械臂控制 ROS 节点 - 快速开始
|
|
||||||
|
|
||||||
## 🚀 快速编译和运行
|
|
||||||
|
|
||||||
### 1. 一键编译
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
./build_arm_control.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. 启动节点
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 方法 A: 使用配置文件（推荐）
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
|
|
||||||
# 方法 B: 使用默认参数
|
|
||||||
ros2 run udp_teleop arm_control
|
|
||||||
|
|
||||||
# 方法 C: 覆盖特定参数
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args -p udp_ip:=192.168.233.67
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 测试服务
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查询当前位姿
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
|
|
||||||
# 移动到指定位置
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 4. 运行完整示例
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 在新终端运行示例客户端（包含完整抓取流程）
|
|
||||||
ros2 run udp_teleop arm_control_client
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📁 文件结构
|
|
||||||
|
|
||||||
```
|
|
||||||
ros2/
|
|
||||||
├── build_arm_control.sh # 一键编译脚本
|
|
||||||
├── ARM_CONTROL_README.md # 完整使用文档
|
|
||||||
├── QUICKSTART.md # 本文件
|
|
||||||
└── src/
|
|
||||||
├── arm_control_msgs/ # 消息和服务定义
|
|
||||||
│ ├── msg/
|
|
||||||
│ │ ├── TCPPose.msg # TCP 位姿消息
|
|
||||||
│ │ └── JointState.msg # 关节状态消息
|
|
||||||
│ └── srv/
|
|
||||||
│ ├── MoveJoints.srv # 关节运动服务
|
|
||||||
│ ├── MovePose.srv # 位姿运动服务
|
|
||||||
│ ├── GetPose.srv # 查询位姿服务
|
|
||||||
│ └── SetGripper.srv # 夹爪控制服务
|
|
||||||
└── udp_teleop/
|
|
||||||
├── udp_teleop/
|
|
||||||
│ ├── arm_control.py # 控制节点实现
|
|
||||||
│ └── arm_control_client.py # 示例客户端
|
|
||||||
└── config/
|
|
||||||
└── arm_control.yaml # 参数配置
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🎯 常用命令
|
|
||||||
|
|
||||||
### 服务调用
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 1. 关节空间运动
|
|
||||||
ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \
|
|
||||||
"{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}"
|
|
||||||
|
|
||||||
# 2. 笛卡尔空间运动
|
|
||||||
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
|
||||||
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
|
||||||
|
|
||||||
# 3. 查询当前位姿
|
|
||||||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
|
||||||
|
|
||||||
# 4. 夹爪控制
|
|
||||||
ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
|
|
||||||
"{grip: true}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 话题订阅
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查看关节状态（10Hz 发布）
|
|
||||||
ros2 topic echo /arm_control/joint_states
|
|
||||||
|
|
||||||
# 查看 TCP 位姿（10Hz 发布）
|
|
||||||
ros2 topic echo /arm_control/tcp_pose
|
|
||||||
```
|
|
||||||
|
|
||||||
### 调试命令
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查看所有服务
|
|
||||||
ros2 service list | grep arm_control
|
|
||||||
|
|
||||||
# 查看所有话题
|
|
||||||
ros2 topic list | grep arm_control
|
|
||||||
|
|
||||||
# 查看节点信息
|
|
||||||
ros2 node info /arm_control
|
|
||||||
|
|
||||||
# 查看服务接口定义
|
|
||||||
ros2 interface show arm_control_msgs/srv/MovePose
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📝 Python 客户端模板
|
|
||||||
|
|
||||||
```python
|
|
||||||
#!/usr/bin/env python3
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from arm_control_msgs.srv import MovePose, GetPose
|
|
||||||
|
|
||||||
class MyController(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('my_controller')
|
|
||||||
|
|
||||||
# 创建服务客户端
|
|
||||||
self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
|
|
||||||
self.get_cli = self.create_client(GetPose, 'arm_control/get_pose')
|
|
||||||
|
|
||||||
# 等待服务
|
|
||||||
self.move_cli.wait_for_service()
|
|
||||||
self.get_cli.wait_for_service()
|
|
||||||
|
|
||||||
def move_to(self, x, y, z, phi, duration=2.0):
|
|
||||||
"""移动到指定位置"""
|
|
||||||
req = MovePose.Request()
|
|
||||||
req.x = x
|
|
||||||
req.y = y
|
|
||||||
req.z = z
|
|
||||||
req.phi = phi
|
|
||||||
req.duration = duration
|
|
||||||
|
|
||||||
future = self.move_cli.call_async(req)
|
|
||||||
rclpy.spin_until_future_complete(self, future)
|
|
||||||
return future.result().success
|
|
||||||
|
|
||||||
def get_pose(self):
|
|
||||||
"""查询当前位姿"""
|
|
||||||
req = GetPose.Request()
|
|
||||||
future = self.get_cli.call_async(req)
|
|
||||||
rclpy.spin_until_future_complete(self, future)
|
|
||||||
return future.result()
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
controller = MyController()
|
|
||||||
|
|
||||||
# 查询位姿
|
|
||||||
pose = controller.get_pose()
|
|
||||||
print(f"当前位置: ({pose.x}, {pose.y}, {pose.z})")
|
|
||||||
|
|
||||||
# 移动
|
|
||||||
controller.move_to(200.0, 100.0, -100.0, 45.0)
|
|
||||||
|
|
||||||
controller.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🔧 配置修改
|
|
||||||
|
|
||||||
编辑 `src/udp_teleop/config/arm_control.yaml`：
|
|
||||||
|
|
||||||
```yaml
|
|
||||||
arm_control:
|
|
||||||
ros__parameters:
|
|
||||||
# 修改 ESP32 IP
|
|
||||||
udp_ip: '192.168.4.1'
|
|
||||||
|
|
||||||
# 修改运动速度
|
|
||||||
default_duration: 1.0 # 更快：0.5，更慢：2.0
|
|
||||||
|
|
||||||
# 修改关节限位
|
|
||||||
height_min: -290
|
|
||||||
height_max: 0
|
|
||||||
```
|
|
||||||
|
|
||||||
修改后重新运行节点即可（无需重新编译）。
|
|
||||||
|
|
||||||
## ⚠️ 常见问题
|
|
||||||
|
|
||||||
### 编译失败
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 确保环境激活
|
|
||||||
conda activate ros2_humble
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
# 清理后重新编译
|
|
||||||
rm -rf build/ install/ log/
|
|
||||||
./build_arm_control.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
### 服务不可用
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 检查节点是否运行
|
|
||||||
ros2 node list
|
|
||||||
|
|
||||||
# 检查服务是否存在
|
|
||||||
ros2 service list | grep arm_control
|
|
||||||
|
|
||||||
# 查看节点日志
|
|
||||||
ros2 run udp_teleop arm_control --ros-args --log-level debug
|
|
||||||
```
|
|
||||||
|
|
||||||
### UDP 连接失败
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 测试 UDP 连接
|
|
||||||
echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888
|
|
||||||
|
|
||||||
# 修改 IP 配置
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args -p udp_ip:=<你的IP> -p udp_port:=8888
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📚 更多文档
|
|
||||||
|
|
||||||
- 完整使用文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md)
|
|
||||||
- 原始工具文档：[../tools/README.md](../tools/README.md)
|
|
||||||
- ROS 2 包文档：[src/udp_teleop/README.md](src/udp_teleop/README.md)
|
|
||||||
|
|
||||||
## 🎓 下一步
|
|
||||||
|
|
||||||
1. **集成视觉**：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务
|
|
||||||
2. **添加规划**：使用 MoveIt 或自定义轨迹规划器
|
|
||||||
3. **多机械臂**：启动多个 arm_control 节点控制多个机械臂
|
|
||||||
4. **远程控制**：通过 ROS 2 的 DDS 实现跨机器控制
|
|
||||||
@@ -1,359 +0,0 @@
|
|||||||
# 视觉抓取节点使用指南
|
|
||||||
|
|
||||||
## 概述
|
|
||||||
|
|
||||||
`vision_grasp` 节点基于 `camera_to_base.py` 实现自动抓取和释放功能，将相机坐标系的检测结果转换为机械臂基坐标系，并自动执行抓取流程。
|
|
||||||
|
|
||||||
## 功能
|
|
||||||
|
|
||||||
### 1. 抓取功能
|
|
||||||
|
|
||||||
**输入**：相机坐标系 `(x, y, z)`
|
|
||||||
|
|
||||||
**流程**：
|
|
||||||
1. 坐标转换：`(xc, yc, zc) = (x, -y, z)`（图像坐标到相机坐标）
|
|
||||||
2. 转换到基坐标系
|
|
||||||
3. 释放夹爪（duration=0）
|
|
||||||
4. 移动到目标位置（duration=3s）
|
|
||||||
5. 抓取（duration=1s）
|
|
||||||
6. 回收到 (200, 0, 当前z)
|
|
||||||
|
|
||||||
### 2. 释放功能
|
|
||||||
|
|
||||||
**输入**：基坐标系 `(x, y, z)`
|
|
||||||
|
|
||||||
**流程**：
|
|
||||||
1. 移动到释放位置
|
|
||||||
2. 释放夹爪（duration=0）
|
|
||||||
3. 回收到 (200, 0, 当前z)
|
|
||||||
|
|
||||||
## 编译
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
colcon build --packages-select udp_teleop
|
|
||||||
source install/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
## 运行
|
|
||||||
|
|
||||||
### 启动节点
|
|
||||||
|
|
||||||
**终端 1**：启动机械臂控制节点
|
|
||||||
```bash
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
**终端 2**：启动视觉抓取节点
|
|
||||||
```bash
|
|
||||||
ros2 run udp_teleop vision_grasp \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
## 使用
|
|
||||||
|
|
||||||
### 方法 1：发布话题触发抓取
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 抓取：输入相机坐标
|
|
||||||
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
|
|
||||||
"{x: 10.0, y: 5.0, z: 250.0}"
|
|
||||||
|
|
||||||
# 释放：输入基坐标
|
|
||||||
ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \
|
|
||||||
"{x: 100.0, y: 150.0, z: -100.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
### 方法 2：Python 脚本集成
|
|
||||||
|
|
||||||
```python
|
|
||||||
#!/usr/bin/env python3
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import Point
|
|
||||||
|
|
||||||
class VisionDetector(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('vision_detector')
|
|
||||||
|
|
||||||
# 创建发布者
|
|
||||||
self.grasp_pub = self.create_publisher(
|
|
||||||
Point,
|
|
||||||
'vision_grasp/grasp_target',
|
|
||||||
10
|
|
||||||
)
|
|
||||||
|
|
||||||
def detect_and_grasp(self):
|
|
||||||
# 模拟检测结果（相机坐标系）
|
|
||||||
camera_x = 10.0 # 相机右侧 10mm
|
|
||||||
camera_y = 5.0 # 相机下方 5mm
|
|
||||||
camera_z = 250.0 # 前方 250mm
|
|
||||||
|
|
||||||
# 发布抓取目标
|
|
||||||
msg = Point()
|
|
||||||
msg.x = camera_x
|
|
||||||
msg.y = camera_y
|
|
||||||
msg.z = camera_z
|
|
||||||
|
|
||||||
self.grasp_pub.publish(msg)
|
|
||||||
self.get_logger().info(f'发送抓取目标: ({camera_x}, {camera_y}, {camera_z})')
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
node = VisionDetector()
|
|
||||||
|
|
||||||
# 检测并抓取
|
|
||||||
node.detect_and_grasp()
|
|
||||||
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
```
|
|
||||||
|
|
||||||
### 方法 3：与检测节点集成
|
|
||||||
|
|
||||||
```python
|
|
||||||
#!/usr/bin/env python3
|
|
||||||
"""完整的视觉检测+抓取示例"""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
from geometry_msgs.msg import Point
|
|
||||||
import cv2
|
|
||||||
from cv_bridge import CvBridge
|
|
||||||
|
|
||||||
class VisionPipeline(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('vision_pipeline')
|
|
||||||
|
|
||||||
# 订阅相机图像
|
|
||||||
self.image_sub = self.create_subscription(
|
|
||||||
Image,
|
|
||||||
'/camera/image_raw',
|
|
||||||
self.on_image,
|
|
||||||
10
|
|
||||||
)
|
|
||||||
|
|
||||||
# 发布抓取目标
|
|
||||||
self.grasp_pub = self.create_publisher(
|
|
||||||
Point,
|
|
||||||
'vision_grasp/grasp_target',
|
|
||||||
10
|
|
||||||
)
|
|
||||||
|
|
||||||
self.bridge = CvBridge()
|
|
||||||
|
|
||||||
def on_image(self, msg):
|
|
||||||
# 转换 ROS 图像到 OpenCV
|
|
||||||
image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
|
||||||
|
|
||||||
# 检测物体（示例：使用轮廓检测）
|
|
||||||
detected = self.detect_object(image)
|
|
||||||
|
|
||||||
if detected:
|
|
||||||
camera_x, camera_y, camera_z = detected
|
|
||||||
|
|
||||||
# 发布抓取目标
|
|
||||||
target = Point()
|
|
||||||
target.x = camera_x
|
|
||||||
target.y = camera_y
|
|
||||||
target.z = camera_z
|
|
||||||
|
|
||||||
self.grasp_pub.publish(target)
|
|
||||||
self.get_logger().info(f'检测到物体，发送抓取指令')
|
|
||||||
|
|
||||||
def detect_object(self, image):
|
|
||||||
"""检测物体并返回相机坐标"""
|
|
||||||
# TODO: 实现你的检测算法
|
|
||||||
# 1. 图像处理（阈值、轮廓等）
|
|
||||||
# 2. 获取像素坐标 (u, v) 和像素宽度
|
|
||||||
# 3. 使用相似三角形计算深度
|
|
||||||
# 4. 转换到相机坐标系
|
|
||||||
|
|
||||||
# 示例返回值
|
|
||||||
return (10.0, 5.0, 250.0) # (xc, yc, zc)
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
node = VisionPipeline()
|
|
||||||
rclpy.spin(node)
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
```
|
|
||||||
|
|
||||||
## 参数配置
|
|
||||||
|
|
||||||
编辑 `config/vision_grasp.yaml`：
|
|
||||||
|
|
||||||
```yaml
|
|
||||||
vision_grasp:
|
|
||||||
ros__parameters:
|
|
||||||
# 相机到 TCP 的变换（如果相机不在 TCP 中心）
|
|
||||||
cam_tx: 0.0 # X 偏移
|
|
||||||
cam_ty: 0.0 # Y 偏移（高度）
|
|
||||||
cam_tz: 0.0 # Z 偏移（前后）
|
|
||||||
|
|
||||||
# 回收位置
|
|
||||||
retract_position_x: 200.0
|
|
||||||
retract_position_y: 0.0
|
|
||||||
|
|
||||||
# 运动时长
|
|
||||||
grasp_duration: 3.0 # 抓取移动时长
|
|
||||||
release_duration: 2.0 # 释放移动时长
|
|
||||||
```
|
|
||||||
|
|
||||||
## 坐标系说明
|
|
||||||
|
|
||||||
### 相机坐标系
|
|
||||||
|
|
||||||
```
|
|
||||||
Yc (下)
|
|
||||||
|
|
|
||||||
|
|
|
||||||
o-----> Zc (前，水平)
|
|
||||||
/
|
|
||||||
/
|
|
||||||
Xc (右)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 坐标转换
|
|
||||||
|
|
||||||
检测结果 `(x, y, z)` 表示：
|
|
||||||
- `x`: 图像列方向（右为正）
|
|
||||||
- `y`: 图像行方向（下为正）
|
|
||||||
- `z`: 深度方向（前为正）
|
|
||||||
|
|
||||||
节点会自动转换：
|
|
||||||
```
|
|
||||||
(xc, yc, zc) = (x, -y, z)
|
|
||||||
```
|
|
||||||
|
|
||||||
这是因为：
|
|
||||||
- 图像 Y 向下 → 相机 Y 向下（负号修正方向）
|
|
||||||
- 然后再转换到基坐标系
|
|
||||||
|
|
||||||
## 调试
|
|
||||||
|
|
||||||
### 查看节点状态
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 查看节点列表
|
|
||||||
ros2 node list
|
|
||||||
|
|
||||||
# 查看话题列表
|
|
||||||
ros2 topic list | grep vision_grasp
|
|
||||||
|
|
||||||
# 监听抓取目标
|
|
||||||
ros2 topic echo /vision_grasp/grasp_target
|
|
||||||
```
|
|
||||||
|
|
||||||
### 测试流程
|
|
||||||
|
|
||||||
1. **启动节点**
|
|
||||||
```bash
|
|
||||||
# 终端 1
|
|
||||||
ros2 run udp_teleop arm_control --ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
|
|
||||||
# 终端 2
|
|
||||||
ros2 run udp_teleop vision_grasp --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
2. **发送测试抓取**
|
|
||||||
```bash
|
|
||||||
# 终端 3
|
|
||||||
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
|
|
||||||
"{x: 0.0, y: 0.0, z: 300.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
3. **观察日志**
|
|
||||||
- 终端 2 会显示详细的抓取流程日志
|
|
||||||
- 确认坐标转换和每一步动作
|
|
||||||
|
|
||||||
## 常见问题
|
|
||||||
|
|
||||||
### Q1: 坐标转换不正确
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
1. 相机内参是否准确标定
|
|
||||||
2. 相机到 TCP 的变换参数是否正确
|
|
||||||
3. 当前 TCP 位姿是否正确
|
|
||||||
|
|
||||||
### Q2: 抓取位置偏移
|
|
||||||
|
|
||||||
**可能原因**：
|
|
||||||
1. 深度计算不准确
|
|
||||||
2. 相机安装角度有偏差
|
|
||||||
3. 坐标系定义理解错误
|
|
||||||
|
|
||||||
**解决**：
|
|
||||||
1. 调整 `cam_pitch` 参数（如果相机有俯仰角）
|
|
||||||
2. 校准相机内参
|
|
||||||
3. 使用已知位置物体验证
|
|
||||||
|
|
||||||
### Q3: 夹爪动作失败
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
1. arm_control 节点是否正常运行
|
|
||||||
2. UDP 连接是否正常
|
|
||||||
3. 关节限位是否合理
|
|
||||||
|
|
||||||
## 扩展功能
|
|
||||||
|
|
||||||
### 添加安全检查
|
|
||||||
|
|
||||||
```python
|
|
||||||
def execute_grasp(self, x: float, y: float, z: float, phi: float):
|
|
||||||
# 检查目标是否在工作空间内
|
|
||||||
if not self.is_in_workspace(x, y, z):
|
|
||||||
self.get_logger().warn(f'目标超出工作空间: ({x}, {y}, {z})')
|
|
||||||
return
|
|
||||||
|
|
||||||
# 执行抓取...
|
|
||||||
```
|
|
||||||
|
|
||||||
### 添加碰撞检测
|
|
||||||
|
|
||||||
```python
|
|
||||||
def is_path_safe(self, start, end):
|
|
||||||
# 检查路径是否安全
|
|
||||||
# TODO: 实现碰撞检测逻辑
|
|
||||||
return True
|
|
||||||
```
|
|
||||||
|
|
||||||
### 多物体抓取
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 订阅物体列表
|
|
||||||
self.objects_sub = self.create_subscription(
|
|
||||||
PointArray, # 自定义消息类型
|
|
||||||
'vision_grasp/object_list',
|
|
||||||
self.handle_objects,
|
|
||||||
10
|
|
||||||
)
|
|
||||||
|
|
||||||
def handle_objects(self, msg):
|
|
||||||
for obj in msg.points:
|
|
||||||
self.execute_grasp(obj.x, obj.y, obj.z, self.current_phi)
|
|
||||||
# 等待完成...
|
|
||||||
```
|
|
||||||
|
|
||||||
## 相关文件
|
|
||||||
|
|
||||||
- 节点实现：`udp_teleop/vision_grasp.py`
|
|
||||||
- 配置文件：`udp_teleop/config/vision_grasp.yaml`
|
|
||||||
- 坐标变换工具：`tools/camera_to_base.py`
|
|
||||||
- 机械臂控制：`udp_teleop/arm_control.py`
|
|
||||||
|
|
||||||
## 下一步
|
|
||||||
|
|
||||||
1. 集成物体检测算法（YOLO、轮廓检测等）
|
|
||||||
2. 添加深度估计（相似三角形、双目视觉等）
|
|
||||||
3. 优化抓取策略（多物体排序、路径规划等）
|
|
||||||
4. 添加可视化（RViz 显示检测结果和机械臂状态）
|
|
||||||
@@ -1,286 +0,0 @@
|
|||||||
# 视觉抓取节点 - 完成总结
|
|
||||||
|
|
||||||
## ✅ 完成的工作
|
|
||||||
|
|
||||||
### 1. 创建了视觉抓取 ROS 节点 (`vision_grasp.py`)
|
|
||||||
|
|
||||||
**功能**：
|
|
||||||
- ✅ 抓取功能：输入相机坐标 → 自动转换 → 执行抓取流程
|
|
||||||
- ✅ 释放功能：输入基坐标 → 移动 → 释放物体
|
|
||||||
- ✅ 坐标变换：集成 `camera_to_base.py` 的完整变换逻辑
|
|
||||||
- ✅ 自动化流程：释放夹爪 → 移动 → 抓取 → 回收
|
|
||||||
|
|
||||||
### 2. 抓取流程
|
|
||||||
|
|
||||||
```
|
|
||||||
输入相机坐标 (x, y, z)
|
|
||||||
↓
|
|
||||||
转换: (xc, yc, zc) = (x, -y, z)
|
|
||||||
↓
|
|
||||||
变换到基坐标系
|
|
||||||
↓
|
|
||||||
1. Release 夹爪 (duration=0)
|
|
||||||
↓
|
|
||||||
2. 移动到目标 (duration=3s)
|
|
||||||
↓
|
|
||||||
3. Grip 夹爪 (duration=1s)
|
|
||||||
↓
|
|
||||||
4. 回收到 (200, 0, 当前z)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 释放流程
|
|
||||||
|
|
||||||
```
|
|
||||||
输入基坐标 (x, y, z)
|
|
||||||
↓
|
|
||||||
1. 移动到释放位置
|
|
||||||
↓
|
|
||||||
2. Release 夹爪 (duration=0)
|
|
||||||
↓
|
|
||||||
3. 回收到 (200, 0, 当前z)
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📁 创建的文件
|
|
||||||
|
|
||||||
```
|
|
||||||
ros2/
|
|
||||||
├── src/udp_teleop/
|
|
||||||
│ ├── udp_teleop/
|
|
||||||
│ │ └── vision_grasp.py ✨ 视觉抓取节点
|
|
||||||
│ └── config/
|
|
||||||
│ └── vision_grasp.yaml ✨ 参数配置
|
|
||||||
├── test_vision_grasp.py ✨ 测试脚本
|
|
||||||
└── VISION_GRASP_README.md ✨ 完整文档
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🚀 快速使用
|
|
||||||
|
|
||||||
### 启动节点
|
|
||||||
|
|
||||||
**终端 1**：arm_control 节点
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
ros2 run udp_teleop arm_control \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
**终端 2**：vision_grasp 节点
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
ros2 run udp_teleop vision_grasp \
|
|
||||||
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
|
||||||
```
|
|
||||||
|
|
||||||
**终端 3**：测试
|
|
||||||
```bash
|
|
||||||
cd ros2
|
|
||||||
source install/setup.bash
|
|
||||||
|
|
||||||
# 测试抓取（相机正前方 300mm）
|
|
||||||
python test_vision_grasp.py grasp 0 0 300
|
|
||||||
|
|
||||||
# 测试抓取（相机右侧 50mm，前方 300mm）
|
|
||||||
python test_vision_grasp.py grasp 50 0 300
|
|
||||||
|
|
||||||
# 测试释放（基坐标）
|
|
||||||
python test_vision_grasp.py release 100 150 -100
|
|
||||||
```
|
|
||||||
|
|
||||||
### 或使用话题发布
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 抓取
|
|
||||||
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
|
|
||||||
"{x: 0.0, y: 0.0, z: 300.0}"
|
|
||||||
|
|
||||||
# 释放
|
|
||||||
ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \
|
|
||||||
"{x: 100.0, y: 150.0, z: -100.0}"
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🎯 关键特性
|
|
||||||
|
|
||||||
### 1. 自动坐标转换
|
|
||||||
|
|
||||||
- **输入**：相机坐标系 `(x, y, z)`
|
|
||||||
- **自动转换**：`(xc, yc, zc) = (x, -y, z)`（图像坐标修正）
|
|
||||||
- **变换到基坐标系**：使用当前 TCP 位姿进行完整变换
|
|
||||||
|
|
||||||
### 2. 参数化配置
|
|
||||||
|
|
||||||
```yaml
|
|
||||||
vision_grasp:
|
|
||||||
ros__parameters:
|
|
||||||
# 相机到 TCP 的变换
|
|
||||||
cam_tx: 0.0
|
|
||||||
cam_ty: 0.0
|
|
||||||
cam_tz: 0.0
|
|
||||||
|
|
||||||
# 回收位置
|
|
||||||
retract_position_x: 200.0
|
|
||||||
retract_position_y: 0.0
|
|
||||||
|
|
||||||
# 运动时长
|
|
||||||
grasp_duration: 3.0
|
|
||||||
release_duration: 2.0
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 完整日志
|
|
||||||
|
|
||||||
节点会输出详细的流程日志：
|
|
||||||
```
|
|
||||||
============================================================
|
|
||||||
开始抓取流程
|
|
||||||
============================================================
|
|
||||||
1. 释放夹爪
|
|
||||||
2. 移动到目标位置: (323.5, 229.6, -108.6)
|
|
||||||
3. 抓取物体
|
|
||||||
4. 移动到回收位置: (200.0, 0.0, -108.6)
|
|
||||||
============================================================
|
|
||||||
✓ 抓取完成！
|
|
||||||
============================================================
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🔗 集成示例
|
|
||||||
|
|
||||||
### Python 脚本集成
|
|
||||||
|
|
||||||
```python
|
|
||||||
#!/usr/bin/env python3
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import Point
|
|
||||||
|
|
||||||
class MyDetector(Node):
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('my_detector')
|
|
||||||
self.grasp_pub = self.create_publisher(
|
|
||||||
Point, 'vision_grasp/grasp_target', 10)
|
|
||||||
|
|
||||||
def on_detection(self, camera_x, camera_y, camera_z):
|
|
||||||
"""检测到物体后触发抓取"""
|
|
||||||
msg = Point()
|
|
||||||
msg.x = camera_x
|
|
||||||
msg.y = camera_y
|
|
||||||
msg.z = camera_z
|
|
||||||
self.grasp_pub.publish(msg)
|
|
||||||
|
|
||||||
def main():
|
|
||||||
rclpy.init()
|
|
||||||
node = MyDetector()
|
|
||||||
|
|
||||||
# 模拟检测结果
|
|
||||||
node.on_detection(10.0, 5.0, 250.0)
|
|
||||||
|
|
||||||
rclpy.spin(node)
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
```
|
|
||||||
|
|
||||||
## 📊 话题接口
|
|
||||||
|
|
||||||
| 话题 | 类型 | 说明 |
|
|
||||||
|------|------|------|
|
|
||||||
| `/vision_grasp/grasp_target` | geometry_msgs/Point | 抓取目标（相机坐标） |
|
|
||||||
| `/vision_grasp/release_target` | geometry_msgs/Point | 释放目标（基坐标） |
|
|
||||||
|
|
||||||
## 🎓 下一步
|
|
||||||
|
|
||||||
### 1. 集成物体检测
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 订阅相机图像
|
|
||||||
self.image_sub = self.create_subscription(
|
|
||||||
Image, '/camera/image_raw', self.on_image, 10)
|
|
||||||
|
|
||||||
def on_image(self, msg):
|
|
||||||
# 检测物体
|
|
||||||
camera_x, camera_y, camera_z = detect_object(msg)
|
|
||||||
|
|
||||||
# 触发抓取
|
|
||||||
self.publish_grasp_target(camera_x, camera_y, camera_z)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 2. 添加深度估计
|
|
||||||
|
|
||||||
使用 `tools/vision_transform.py` 中的相似三角形方法：
|
|
||||||
|
|
||||||
```python
|
|
||||||
from tools.vision_transform import compute_depth_from_size
|
|
||||||
|
|
||||||
# 从检测获得像素宽度
|
|
||||||
pixel_width = 100 # px
|
|
||||||
real_width = 50 # mm
|
|
||||||
focal_length = 500 # px
|
|
||||||
|
|
||||||
depth = compute_depth_from_size(pixel_width, real_width, focal_length)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. 多物体抓取
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 创建队列
|
|
||||||
self.grasp_queue = []
|
|
||||||
|
|
||||||
def on_multiple_detections(self, detections):
|
|
||||||
for det in detections:
|
|
||||||
self.grasp_queue.append(det)
|
|
||||||
|
|
||||||
# 逐个抓取
|
|
||||||
while self.grasp_queue:
|
|
||||||
target = self.grasp_queue.pop(0)
|
|
||||||
self.publish_grasp_target(target.x, target.y, target.z)
|
|
||||||
# 等待完成...
|
|
||||||
```
|
|
||||||
|
|
||||||
## 🐛 故障排查
|
|
||||||
|
|
||||||
### Q1: 坐标转换不正确
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
1. TCP 位姿是否正确（`ros2 service call /arm_control/get_pose`）
|
|
||||||
2. 相机到 TCP 的变换参数（`cam_tx/ty/tz`, `cam_roll/pitch/yaw`）
|
|
||||||
3. 坐标系方向理解是否正确
|
|
||||||
|
|
||||||
### Q2: 抓取位置偏移
|
|
||||||
|
|
||||||
**解决**：
|
|
||||||
1. 校准相机内参
|
|
||||||
2. 验证深度计算准确性
|
|
||||||
3. 调整 `cam_pitch`（如果相机有俯仰角）
|
|
||||||
|
|
||||||
### Q3: 服务调用超时
|
|
||||||
|
|
||||||
**检查**：
|
|
||||||
1. arm_control 节点是否运行
|
|
||||||
2. UDP 连接是否正常
|
|
||||||
3. 机械臂是否在合理位置
|
|
||||||
|
|
||||||
## 📚 相关文档
|
|
||||||
|
|
||||||
- **完整文档**：`VISION_GRASP_README.md`
|
|
||||||
- **坐标变换**：`tools/camera_to_base.py`
|
|
||||||
- **机械臂控制**：`ARM_CONTROL_README.md`
|
|
||||||
- **视觉变换**：`docs/vision_calibration_horizontal.md`
|
|
||||||
|
|
||||||
## 🎉 总结
|
|
||||||
|
|
||||||
现在你有了一个完整的视觉抓取系统：
|
|
||||||
|
|
||||||
1. ✅ **独立的机械臂控制节点** - `arm_control`
|
|
||||||
2. ✅ **自动化抓取节点** - `vision_grasp`
|
|
||||||
3. ✅ **完整的坐标变换** - 相机 → 基坐标系
|
|
||||||
4. ✅ **参数化配置** - 灵活调整参数
|
|
||||||
5. ✅ **测试工具** - 快速验证功能
|
|
||||||
6. ✅ **完整文档** - 使用指南和示例
|
|
||||||
|
|
||||||
只需要：
|
|
||||||
1. 添加物体检测算法
|
|
||||||
2. 连接相机获取图像
|
|
||||||
3. 发布检测结果到 `/vision_grasp/grasp_target`
|
|
||||||
|
|
||||||
系统就会自动完成抓取！
|
|
||||||
423
docs/arm.md
423
docs/arm.md
@@ -1,115 +1,424 @@
|
|||||||
这是一个非常漂亮且实用的自定义机械臂！从你的描述和图片来看，这属于一个**类 SCARA（水平多关节）构型**的机械臂，带有一个直线 Z 轴和三个平面的旋转关节。
|
# 机械臂运动学推导
|
||||||
|
|
||||||
为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。
|
6-DOF 机械臂逆运动学和正运动学完整推导。
|
||||||
|
|
||||||
除角度外，所有线性长度和坐标统一使用 **mm**。
|
## 机械臂结构
|
||||||
|
|
||||||
### 变量与坐标系定义
|
### 关节配置
|
||||||
|
|
||||||
我们将基坐标系原点设在红色滑轨的零点位置。
|
```
|
||||||
|
J1: 线性滑轨（垂直运动，高度 d1）
|
||||||
|
J2: 基座旋转（绕 Z 轴，XY 平面）
|
||||||
|
J3: 肘关节（绕 Z 轴，XY 平面）
|
||||||
|
J4: 腕关节（绕 Z 轴，XY 平面）
|
||||||
|
J5: 末端俯仰（0° = 水平）
|
||||||
|
J6: 夹爪旋转
|
||||||
|
```
|
||||||
|
|
||||||
* **$d_1$**: 关节 1（高度滑块）的位置变量。
|
### 几何参数
|
||||||
* **$\theta_2$**: 关节 2 的旋转角度（相对于基坐标系 X 轴）。
|
|
||||||
* **$\theta_3$**: 关节 3 的旋转角度（相对于上一连杆 $L_1$）。
|
|
||||||
* **$\theta_4$**: 关节 4 的旋转角度（相对于上一连杆 $L_2$）。
|
|
||||||
* **$(X, Y, Z, \Phi)$**: 夹爪末端（TCP - Tool Center Point）在基坐标系下的位姿。其中 $(X, Y, Z)$ 是空间坐标，$\Phi$ 是夹爪在水平面上的总朝向角（偏航角）。
|
|
||||||
* **已知常量**: $L_1$（二三关节距离）, $L_2$（三四关节距离）, $x_4$（夹爪相对关节四的 X 偏移）, $z_4$（夹爪相对关节四的高度偏移）。
|
|
||||||
|
|
||||||
---
|
| 参数 | 值 (mm) | 说明 |
|
||||||
|
|------|---------|------|
|
||||||
|
| L1 | 125 | J2-J3 连杆长度 |
|
||||||
|
| L2 | 125 | J3-J4 连杆长度 |
|
||||||
|
| x4 | 110 | J4-TCP 水平偏移 |
|
||||||
|
| z4 | 80 | J4-TCP 垂直偏移（夹爪状态相关：张开 55mm，闭合 -100mm） |
|
||||||
|
|
||||||
### 1. 运动学正解 (Forward Kinematics)
|
### 坐标系
|
||||||
|
|
||||||
正解的目的是：**已知各个电机的角度和滑块高度 $(d_1, \theta_2, \theta_3, \theta_4)$，求夹爪末端的位置 $(X, Y, Z, \Phi)$。**
|
**基坐标系**（右手系，Z 轴朝上）：
|
||||||
|
```
|
||||||
|
Z↑ (高度)
|
||||||
|
|
|
||||||
|
|
|
||||||
|
o----→ X (前)
|
||||||
|
/
|
||||||
|
/
|
||||||
|
Y (左)
|
||||||
|
```
|
||||||
|
|
||||||
因为 Z 轴的直线运动与 XY 平面的旋转运动是完全解耦的，我们可以分别计算：
|
- 原点：J1 滑轨底部
|
||||||
|
- d1 范围：[-290, 0] mm（负值表示在基准面以下）
|
||||||
|
|
||||||
**高度 (Z 轴):**
|
## 正运动学（FK）
|
||||||
|
|
||||||
|
已知关节状态 → 计算 TCP 位姿
|
||||||
|
|
||||||
$$Z = d_1 + z_4$$
|
### 输入
|
||||||
|
|
||||||
**平面朝向角 (偏航角 $\Phi$):**
|
- `d1`: 高度（J1 线性位移）
|
||||||
|
- `θ2, θ3, θ4`: J2/J3/J4 角度（度）
|
||||||
|
|
||||||
|
### 输出
|
||||||
|
|
||||||
$$\Phi = \theta_2 + \theta_3 + \theta_4$$
|
- `(x, y, z)`: TCP 位置（mm）
|
||||||
|
- `φ`: TCP 偏航角（度）
|
||||||
|
|
||||||
**平面坐标 (X, Y):**
|
### 推导
|
||||||
|
|
||||||
|
**步骤 1**：计算 J4 中心位置（XY 平面）
|
||||||
|
|
||||||
$$X = L_1 \cos(\theta_2) + L_2 \cos(\theta_2 + \theta_3) + x_4 \cos(\theta_2 + \theta_3 + \theta_4)$$
|
J2 和 J3 形成二连杆机构：
|
||||||
|
|
||||||
$$Y = L_1 \sin(\theta_2) + L_2 \sin(\theta_2 + \theta_3) + x_4 \sin(\theta_2 + \theta_3 + \theta_4)$$
|
$$
|
||||||
|
x_{j4} = L_1 \cos\theta_2 + L_2 \cos(\theta_2 + \theta_3)
|
||||||
|
$$
|
||||||
|
|
||||||
*(注意：在实际编程中，如果你的电机零点不是一条直线，需要在角度上加上相应的初始偏置)*
|
$$
|
||||||
|
y_{j4} = L_1 \sin\theta_2 + L_2 \sin(\theta_2 + \theta_3)
|
||||||
|
$$
|
||||||
|
|
||||||
---
|
**步骤 2**：计算 TCP 偏航角
|
||||||
|
|
||||||
### 2. 运动学逆解 (Inverse Kinematics)
|
$$
|
||||||
|
\phi = \theta_2 + \theta_3 + \theta_4
|
||||||
|
$$
|
||||||
|
|
||||||
逆解的目的是：**给出夹爪期望到达的目标位置和朝向 $(X, Y, Z, \Phi)$，求出各关节需要运动到的目标值 $(d_1, \theta_2, \theta_3, \theta_4)$。**
|
**步骤 3**：计算 TCP 位置
|
||||||
|
|
||||||
> **注意：** 你的机械臂在 XY 平面上有 3 个旋转自由度，但平面位置只需要 2 个自由度 $(X,Y)$。这意味着如果只给定目标坐标，机械臂有无数种姿态可以到达（冗余）。因此，**为了得到唯一解，必须同时指定夹爪的最终期望朝向角 $\Phi$**。
|
从 J4 中心沿 φ 方向偏移 x4，垂直偏移 z4：
|
||||||
|
|
||||||
下面是逆解的推导步骤，非常适合直接转化为固件中的控制代码：
|
$$
|
||||||
|
x = x_{j4} + x_4 \cos\phi
|
||||||
|
$$
|
||||||
|
|
||||||
#### 第一步：求解滑块高度 $d_1$
|
$$
|
||||||
|
y = y_{j4} + x_4 \sin\phi
|
||||||
|
$$
|
||||||
|
|
||||||
高度依然是解耦的，直接通过目标 $Z$ 坐标和常量偏移计算：
|
$$
|
||||||
|
z = d_1 - z_4
|
||||||
|
$$
|
||||||
|
|
||||||
|
### Python 实现
|
||||||
|
|
||||||
$$d_1 = Z - z_4$$
|
```python
|
||||||
|
def forward_kinematics(d1, theta2_deg, theta3_deg, theta4_deg, L1=125, L2=125, x4=110, z4=80):
|
||||||
|
"""正运动学"""
|
||||||
|
theta2 = math.radians(theta2_deg)
|
||||||
|
theta3 = math.radians(theta3_deg)
|
||||||
|
theta4 = math.radians(theta4_deg)
|
||||||
|
phi = theta2 + theta3 + theta4
|
||||||
|
|
||||||
#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$
|
# J4 中心
|
||||||
|
x_j4 = L1 * math.cos(theta2) + L2 * math.cos(theta2 + theta3)
|
||||||
|
y_j4 = L1 * math.sin(theta2) + L2 * math.sin(theta2 + theta3)
|
||||||
|
|
||||||
既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置：
|
# TCP 位置
|
||||||
|
x = x_j4 + x4 * math.cos(phi)
|
||||||
|
y = y_j4 + x4 * math.sin(phi)
|
||||||
|
z = d1 - z4
|
||||||
|
|
||||||
|
return x, y, z, math.degrees(phi)
|
||||||
|
```
|
||||||
|
|
||||||
$$X_4 = X - x_4 \cos(\Phi)$$
|
## 逆运动学（IK）
|
||||||
|
|
||||||
$$Y_4 = Y - x_4 \sin(\Phi)$$
|
已知 TCP 目标位姿 → 计算关节角度
|
||||||
|
|
||||||
#### 第三步：求解关节 3 的角度 $\theta_3$
|
### 输入
|
||||||
|
|
||||||
现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。
|
- `(x, y, z)`: 目标位置（mm）
|
||||||
根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有：
|
- `φ`: 目标偏航角（度）
|
||||||
|
|
||||||
|
### 输出
|
||||||
|
|
||||||
$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
|
- `d1`: 高度
|
||||||
|
- `θ2, θ3, θ4`: 关节角度（度）
|
||||||
|
|
||||||
设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。
|
### 推导
|
||||||
如果合法，则 $\theta_3$ 的正弦值为：
|
|
||||||
|
|
||||||
|
**步骤 1**：计算 J4 中心目标位置
|
||||||
|
|
||||||
$$S_3 = \pm \sqrt{1 - C_3^2}$$
|
从 TCP 位置反向计算 J4 位置：
|
||||||
|
|
||||||
|
$$
|
||||||
|
x_{j4} = x - x_4 \cos\phi
|
||||||
|
$$
|
||||||
|
|
||||||
*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)*
|
$$
|
||||||
|
y_{j4} = y - x_4 \sin\phi
|
||||||
|
$$
|
||||||
|
|
||||||
最终使用反正切函数求解 $\theta_3$：
|
$$
|
||||||
|
z_{j4} = z + z_4
|
||||||
|
$$
|
||||||
|
|
||||||
|
$$
|
||||||
|
d_1 = z_{j4}
|
||||||
|
$$
|
||||||
|
|
||||||
$$\theta_3 = \text{atan2}(S_3, C_3)$$
|
**步骤 2**：计算平面距离
|
||||||
|
|
||||||
#### 第四步：求解关节 2 的角度 $\theta_2$
|
$$
|
||||||
|
r = \sqrt{x_{j4}^2 + y_{j4}^2}
|
||||||
|
$$
|
||||||
|
|
||||||
利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$：
|
**步骤 3**：求解 θ3（余弦定理）
|
||||||
|
|
||||||
|
二连杆机构的标准解法：
|
||||||
|
|
||||||
$$\theta_2 = \text{atan2}(Y_4, X_4) - \text{atan2}(L_2 S_3, L_1 + L_2 C_3)$$
|
$$
|
||||||
|
\cos\theta_3 = \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}
|
||||||
|
$$
|
||||||
|
|
||||||
#### 第五步：求解关节 4 的角度 $\theta_4$
|
检查工作空间：
|
||||||
|
|
||||||
因为总朝向 $\Phi = \theta_2 + \theta_3 + \theta_4$，所以：
|
$$
|
||||||
|
-1 \leq \cos\theta_3 \leq 1 \quad \Rightarrow \quad |L_1 - L_2| \leq r \leq L_1 + L_2
|
||||||
|
$$
|
||||||
|
|
||||||
|
否则目标超出工作空间。
|
||||||
|
|
||||||
$$\theta_4 = \Phi - \theta_2 - \theta_3$$
|
有两个解（肘部朝上 / 朝下）：
|
||||||
|
|
||||||
---
|
$$
|
||||||
|
\theta_3 = \pm \arccos\left(\frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)
|
||||||
|
$$
|
||||||
|
|
||||||
### 💡 嵌入式固件实现建议
|
**步骤 4**：求解 θ2
|
||||||
|
|
||||||
由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议：
|
$$
|
||||||
|
\theta_2 = \arctan2(y_{j4}, x_{j4}) - \arctan2\left(L_2\sin\theta_3, L_1 + L_2\cos\theta_3\right)
|
||||||
|
$$
|
||||||
|
|
||||||
1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。
|
**步骤 5**：求解 θ4
|
||||||
2. **死区与奇异点保护**：当 $X_4^2 + Y_4^2 \approx 0$ 时（关节 4 缩回到了原点正上方），此时 $\theta_2$ 会失去意义（奇异点）。在代码中应当加入对 $X_4^2 + Y_4^2 < \epsilon$（一个极小值）的判断，防止产生 NaN。
|
|
||||||
3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。
|
$$
|
||||||
|
\theta_4 = \phi - \theta_2 - \theta_3
|
||||||
|
$$
|
||||||
|
|
||||||
|
### Python 实现
|
||||||
|
|
||||||
|
```python
|
||||||
|
def inverse_kinematics(x, y, z, phi_deg, elbow_up=False, L1=125, L2=125, x4=110, z4=80):
|
||||||
|
"""逆运动学"""
|
||||||
|
phi = math.radians(phi_deg)
|
||||||
|
|
||||||
|
# 步骤 1: 计算 J4 中心
|
||||||
|
x_j4 = x - x4 * math.cos(phi)
|
||||||
|
y_j4 = y - x4 * math.sin(phi)
|
||||||
|
z_j4 = z + z4
|
||||||
|
d1 = z_j4
|
||||||
|
|
||||||
|
# 步骤 2: 平面距离
|
||||||
|
r2 = x_j4**2 + y_j4**2
|
||||||
|
r = math.sqrt(r2)
|
||||||
|
|
||||||
|
# 步骤 3: 求解 theta3
|
||||||
|
cos_theta3 = (r2 - L1**2 - L2**2) / (2 * L1 * L2)
|
||||||
|
|
||||||
|
if cos_theta3 < -1 or cos_theta3 > 1:
|
||||||
|
raise ValueError(f"目标超出工作空间，r={r:.1f}mm")
|
||||||
|
|
||||||
|
# 肘部朝上：负角度，肘部朝下：正角度
|
||||||
|
sin_theta3 = -math.sqrt(1 - cos_theta3**2) if elbow_up else math.sqrt(1 - cos_theta3**2)
|
||||||
|
theta3 = math.atan2(sin_theta3, cos_theta3)
|
||||||
|
|
||||||
|
# 步骤 4: 求解 theta2
|
||||||
|
theta2 = math.atan2(y_j4, x_j4) - math.atan2(
|
||||||
|
L2 * sin_theta3,
|
||||||
|
L1 + L2 * cos_theta3
|
||||||
|
)
|
||||||
|
|
||||||
|
# 步骤 5: 求解 theta4
|
||||||
|
theta4 = phi - theta2 - theta3
|
||||||
|
|
||||||
|
# 角度归一化到 [-180, 180)
|
||||||
|
def normalize(angle):
|
||||||
|
a = (angle + math.pi) % (2 * math.pi) - math.pi
|
||||||
|
return a if a != -math.pi or angle <= 0 else math.pi
|
||||||
|
|
||||||
|
return (
|
||||||
|
d1,
|
||||||
|
math.degrees(normalize(theta2)),
|
||||||
|
math.degrees(normalize(theta3)),
|
||||||
|
math.degrees(normalize(theta4))
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
## 零点偏移
|
||||||
|
|
||||||
|
物理零点（机械对齐）与数学零点（直线构型）存在偏差：
|
||||||
|
|
||||||
|
| 关节 | 零点偏移 |
|
||||||
|
|------|----------|
|
||||||
|
| J2 | +3° |
|
||||||
|
| J3 | +7° |
|
||||||
|
| J4 | +25° |
|
||||||
|
|
||||||
|
### 转换关系
|
||||||
|
|
||||||
|
**命令角度 ↔ 数学角度**：
|
||||||
|
|
||||||
|
$$
|
||||||
|
\theta_{\text{command}} = \theta_{\text{math}} + \text{offset}
|
||||||
|
$$
|
||||||
|
|
||||||
|
$$
|
||||||
|
\theta_{\text{math}} = \theta_{\text{command}} - \text{offset}
|
||||||
|
$$
|
||||||
|
|
||||||
|
**使用**：
|
||||||
|
- 正运动学：先减去偏移（命令 → 数学），再计算
|
||||||
|
- 逆运动学：先计算（数学），再加上偏移（数学 → 命令）
|
||||||
|
|
||||||
|
## 工作空间
|
||||||
|
|
||||||
|
### 高度范围
|
||||||
|
|
||||||
|
$$
|
||||||
|
z \in [-290 - z_4, 0 - z_4] = [-370, -80] \text{ mm}
|
||||||
|
$$
|
||||||
|
|
||||||
|
### 水平范围
|
||||||
|
|
||||||
|
$$
|
||||||
|
r_{\min} = |L_1 - L_2| + x_4 = 0 + 110 = 110 \text{ mm}
|
||||||
|
$$
|
||||||
|
|
||||||
|
$$
|
||||||
|
r_{\max} = L_1 + L_2 + x_4 = 125 + 125 + 110 = 360 \text{ mm}
|
||||||
|
$$
|
||||||
|
|
||||||
|
**可达圆环**：半径 110mm 到 360mm
|
||||||
|
|
||||||
|
### 关节限位
|
||||||
|
|
||||||
|
| 关节 | 最小值 | 最大值 | 单位 |
|
||||||
|
|------|--------|--------|------|
|
||||||
|
| height | -290 | 0 | mm |
|
||||||
|
| J2 | -110 | 115 | ° |
|
||||||
|
| J3 | -120 | 145 | ° |
|
||||||
|
| J4 | -90 | 130 | ° |
|
||||||
|
| J5 | -180 | 180 | ° |
|
||||||
|
| J6 | -180 | 180 | ° |
|
||||||
|
|
||||||
|
## 奇异点
|
||||||
|
|
||||||
|
### 1. 肩部奇异点
|
||||||
|
|
||||||
|
当 J4 中心在原点正上方：
|
||||||
|
|
||||||
|
$$
|
||||||
|
x_{j4} = y_{j4} = 0 \quad \Rightarrow \quad r = 0
|
||||||
|
$$
|
||||||
|
|
||||||
|
此时 θ2 无定义（可取任意值）。
|
||||||
|
|
||||||
|
**避免**：保持 `r > 10mm`
|
||||||
|
|
||||||
|
### 2. 肘部奇异点
|
||||||
|
|
||||||
|
当机械臂完全伸直或完全折叠：
|
||||||
|
|
||||||
|
$$
|
||||||
|
\theta_3 = 0° \text{ 或 } \pm 180°
|
||||||
|
$$
|
||||||
|
|
||||||
|
此时运动控制不稳定。
|
||||||
|
|
||||||
|
**避免**：保持 `|θ3| > 5°`
|
||||||
|
|
||||||
|
## 运动插值
|
||||||
|
|
||||||
|
关节空间线性插值，避免笛卡尔空间的复杂路径规划。
|
||||||
|
|
||||||
|
### 算法
|
||||||
|
|
||||||
|
给定起点 `q_start` 和终点 `q_end`，生成 N 个中间点：
|
||||||
|
|
||||||
|
$$
|
||||||
|
q_i = q_{\text{start}} + \frac{i}{N}(q_{\text{end}} - q_{\text{start}}), \quad i = 1, 2, \ldots, N
|
||||||
|
$$
|
||||||
|
|
||||||
|
### 参数
|
||||||
|
|
||||||
|
- `duration`: 运动时长（秒）
|
||||||
|
- `rate`: 插值频率（Hz）
|
||||||
|
- `steps = ceil(duration × rate)`: 插值点数
|
||||||
|
|
||||||
|
**示例**：`duration=2.0s`, `rate=20Hz` → `steps=40`
|
||||||
|
|
||||||
|
### Python 实现
|
||||||
|
|
||||||
|
```python
|
||||||
|
def interpolate_joints(start, end, duration=2.0, rate=20.0):
|
||||||
|
"""关节空间插值"""
|
||||||
|
steps = max(1, int(math.ceil(duration * rate)))
|
||||||
|
trajectory = []
|
||||||
|
|
||||||
|
for i in range(1, steps + 1):
|
||||||
|
t = i / steps
|
||||||
|
state = {
|
||||||
|
key: int(round(start[key] + t * (end[key] - start[key])))
|
||||||
|
for key in start.keys()
|
||||||
|
}
|
||||||
|
trajectory.append(state)
|
||||||
|
|
||||||
|
return trajectory
|
||||||
|
```
|
||||||
|
|
||||||
|
## 完整示例
|
||||||
|
|
||||||
|
### 示例 1：前方抓取
|
||||||
|
|
||||||
|
**目标**：抓取前方 250mm，右侧 50mm，高度 -150mm 的物体
|
||||||
|
|
||||||
|
```python
|
||||||
|
# 逆运动学
|
||||||
|
d1, theta2, theta3, theta4 = inverse_kinematics(
|
||||||
|
x=250, y=50, z=-150, phi_deg=0
|
||||||
|
)
|
||||||
|
# 输出: d1=-70, theta2=11.3°, theta3=-48.6°, theta4=37.3°
|
||||||
|
|
||||||
|
# 验证：正运动学
|
||||||
|
x, y, z, phi = forward_kinematics(d1, theta2, theta3, theta4)
|
||||||
|
# 输出: (250.0, 50.0, -150.0, 0.0°) ✓
|
||||||
|
```
|
||||||
|
|
||||||
|
### 示例 2：多段轨迹
|
||||||
|
|
||||||
|
```python
|
||||||
|
# 初始位置
|
||||||
|
start = {'height': 0, 'j2': 0, 'j3': 0, 'j4': 0, 'j5': 81, 'j6': 30}
|
||||||
|
|
||||||
|
# 目标 1: 上方
|
||||||
|
_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -50, 45)
|
||||||
|
waypoint1 = {'height': -50, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': 81, 'j6': 30}
|
||||||
|
|
||||||
|
# 目标 2: 抓取位置
|
||||||
|
_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -150, 45)
|
||||||
|
waypoint2 = {'height': -150, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': -100, 'j6': -5}
|
||||||
|
|
||||||
|
# 生成轨迹
|
||||||
|
traj1 = interpolate_joints(start, waypoint1, duration=2.0) # 2秒到上方
|
||||||
|
traj2 = interpolate_joints(waypoint1, waypoint2, duration=1.0) # 1秒下降抓取
|
||||||
|
```
|
||||||
|
|
||||||
|
## 参考
|
||||||
|
|
||||||
|
### UDP 命令格式
|
||||||
|
|
||||||
|
```
|
||||||
|
JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
|
||||||
|
```
|
||||||
|
|
||||||
|
- 所有角度值已包含零点偏移
|
||||||
|
- 直接发送到 ESP32 的 UDP 端口 8888
|
||||||
|
|
||||||
|
### 相关工具
|
||||||
|
|
||||||
|
- `tools/udp_control.py` - 命令行控制（支持 `joints` 和 `pose` 模式）
|
||||||
|
- `ros2/src/udp_teleop/udp_teleop/arm_control.py` - ROS 节点
|
||||||
|
- `docs/vision_calibration_horizontal.md` - 相机坐标变换
|
||||||
|
|
||||||
|
### 测试
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 测试逆运动学
|
||||||
|
python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --dry-run
|
||||||
|
|
||||||
|
# 发送命令
|
||||||
|
python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0
|
||||||
|
```
|
||||||
|
|||||||
308
ros2/README.md
308
ros2/README.md
@@ -1,98 +1,260 @@
|
|||||||
# ros2 — ROS 2 工作空间
|
# ROS 2 机械臂控制系统
|
||||||
|
|
||||||
## 环境搭建
|
CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
|
||||||
|
|
||||||
支持以下三种安装方式，任选其一。
|
## 📦 包含组件
|
||||||
|
|
||||||
### 方式一：Conda (robostack，跨平台)
|
### 1. arm_control_msgs
|
||||||
|
消息和服务定义包。
|
||||||
|
|
||||||
适用于 Linux / macOS / Windows，无需 root 权限。
|
**消息类型**：
|
||||||
|
- `JointState` - 关节状态
|
||||||
|
- `TCPPose` - TCP 位姿
|
||||||
|
|
||||||
```bash
|
**服务类型**：
|
||||||
# 安装 Miniconda
|
- `MoveJoints` - 关节空间运动
|
||||||
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
|
- `MovePose` - 笛卡尔空间运动
|
||||||
bash Miniconda3-latest-Linux-x86_64.sh
|
- `GetPose` - 查询当前位姿
|
||||||
|
- `SetGripper` - 夹爪控制
|
||||||
|
|
||||||
# 创建 ROS 2 Humble 环境
|
### 2. arm_control 节点
|
||||||
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
|
独立的机械臂控制节点（不依赖 tools/udp_control.py）。
|
||||||
conda activate ros2_humble
|
|
||||||
|
|
||||||
# 安装构建工具
|
**功能**：
|
||||||
conda install -c robostack-staging -c conda-forge \
|
- 关节空间和笛卡尔空间运动控制
|
||||||
colcon-common-extensions \
|
- 完整的逆运动学和正运动学
|
||||||
ros-humble-ament-cmake \
|
- UDP 通信（与 ESP32）
|
||||||
python3-pip
|
- 状态发布（10Hz）
|
||||||
|
|
||||||
# Python 依赖
|
### 3. vision_grasp 节点
|
||||||
pip install pynput
|
自动化视觉抓取节点。
|
||||||
```
|
|
||||||
|
|
||||||
### 方式二：apt 原生安装 (Ubuntu 22.04)
|
**功能**：
|
||||||
|
- 相机坐标到基坐标系的自动转换
|
||||||
|
- 抓取流程：释放 → 移动 → 抓取 → 回收
|
||||||
|
- 释放流程：移动 → 释放 → 回收
|
||||||
|
|
||||||
官方推荐的 Ubuntu 安装方式，系统级集成。
|
## 🚀 快速开始
|
||||||
|
|
||||||
```bash
|
### 编译
|
||||||
# 添加 ROS 2 源
|
|
||||||
sudo apt update && sudo apt install curl gnupg lsb-release
|
|
||||||
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
|
|
||||||
-o /usr/share/keyrings/ros-archive-keyring.gpg
|
|
||||||
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
|
|
||||||
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
|
|
||||||
|
|
||||||
# 安装 ROS 2 Humble
|
|
||||||
sudo apt update
|
|
||||||
sudo apt install ros-humble-desktop python3-colcon-common-extensions
|
|
||||||
|
|
||||||
# Python 依赖
|
|
||||||
pip install pynput
|
|
||||||
|
|
||||||
# 环境配置（或写入 ~/.bashrc）
|
|
||||||
source /opt/ros/humble/setup.bash
|
|
||||||
```
|
|
||||||
|
|
||||||
### 方式三：Docker
|
|
||||||
|
|
||||||
推荐用于 CI/CD 或快速体验，无需污染宿主机环境。
|
|
||||||
|
|
||||||
```bash
|
|
||||||
# 拉取镜像
|
|
||||||
docker pull osrf/ros:humble-desktop
|
|
||||||
|
|
||||||
# 启动容器（挂载工作空间）
|
|
||||||
docker run -it --rm \
|
|
||||||
-v $(pwd)/ros2:/ws \
|
|
||||||
osrf/ros:humble-desktop \
|
|
||||||
bash
|
|
||||||
|
|
||||||
# 容器内安装依赖
|
|
||||||
apt update && apt install -y python3-colcon-common-extensions python3-pip
|
|
||||||
pip install pynput
|
|
||||||
```
|
|
||||||
|
|
||||||
## 构建
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd ros2
|
cd ros2
|
||||||
colcon build --symlink-install --packages-select udp_teleop
|
|
||||||
|
# 设置 Python 环境变量（robostack 需要）
|
||||||
|
export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
|
||||||
|
export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
|
||||||
|
export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
|
||||||
|
|
||||||
|
# 编译
|
||||||
|
colcon build --packages-select arm_control_msgs \
|
||||||
|
--cmake-args \
|
||||||
|
-DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
|
||||||
|
-DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
|
||||||
|
-DPython_LIBRARY=$PYTHON_LIBRARY
|
||||||
|
|
||||||
|
colcon build --packages-select udp_teleop
|
||||||
|
|
||||||
|
# Source 环境
|
||||||
source install/setup.bash
|
source install/setup.bash
|
||||||
```
|
```
|
||||||
|
|
||||||
> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
|
### 运行
|
||||||
|
|
||||||
## 运行
|
|
||||||
|
|
||||||
|
**机械臂控制**：
|
||||||
```bash
|
```bash
|
||||||
ros2 run udp_teleop keyboard_control \
|
ros2 run udp_teleop arm_control \
|
||||||
--ros-args --params-file src/udp_teleop/config/params.yaml
|
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||||||
```
|
```
|
||||||
|
|
||||||
也可以通过命令行覆盖参数：
|
**视觉抓取**：
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run udp_teleop keyboard_control \
|
# 终端 1: 启动 arm_control
|
||||||
--ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
|
ros2 run udp_teleop arm_control \
|
||||||
|
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||||||
|
|
||||||
|
# 终端 2: 启动 vision_grasp
|
||||||
|
ros2 run udp_teleop vision_grasp \
|
||||||
|
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
||||||
```
|
```
|
||||||
|
|
||||||
## 包文档
|
## 📚 使用示例
|
||||||
|
|
||||||
详见 [src/udp_teleop/README.md](src/udp_teleop/README.md)，包含按键映射、UDP 协议、参数配置等。
|
### 1. 控制机械臂
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 查询位姿
|
||||||
|
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
||||||
|
|
||||||
|
# 关节运动
|
||||||
|
ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \
|
||||||
|
"{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}"
|
||||||
|
|
||||||
|
# 笛卡尔运动
|
||||||
|
ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
|
||||||
|
"{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2. 视觉抓取
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 发布抓取目标（相机坐标）
|
||||||
|
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
|
||||||
|
"{x: 10.0, y: 5.0, z: 250.0}"
|
||||||
|
|
||||||
|
# 发布释放目标（基坐标）
|
||||||
|
ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \
|
||||||
|
"{x: 100.0, y: 150.0, z: -100.0}"
|
||||||
|
```
|
||||||
|
|
||||||
|
### 3. Python 集成
|
||||||
|
|
||||||
|
```python
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from geometry_msgs.msg import Point
|
||||||
|
|
||||||
|
class VisionDetector(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('vision_detector')
|
||||||
|
self.grasp_pub = self.create_publisher(
|
||||||
|
Point, 'vision_grasp/grasp_target', 10)
|
||||||
|
|
||||||
|
def detect_and_grasp(self, camera_x, camera_y, camera_z):
|
||||||
|
msg = Point()
|
||||||
|
msg.x = camera_x
|
||||||
|
msg.y = camera_y
|
||||||
|
msg.z = camera_z
|
||||||
|
self.grasp_pub.publish(msg)
|
||||||
|
|
||||||
|
def main():
|
||||||
|
rclpy.init()
|
||||||
|
node = VisionDetector()
|
||||||
|
node.detect_and_grasp(10.0, 5.0, 250.0)
|
||||||
|
rclpy.spin(node)
|
||||||
|
```
|
||||||
|
|
||||||
|
## ⚙️ 配置
|
||||||
|
|
||||||
|
### arm_control 参数
|
||||||
|
|
||||||
|
编辑 `src/udp_teleop/config/arm_control.yaml`：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
arm_control:
|
||||||
|
ros__parameters:
|
||||||
|
udp_ip: '192.168.4.1' # ESP32 IP
|
||||||
|
udp_port: 8888
|
||||||
|
|
||||||
|
# 几何参数 (mm)
|
||||||
|
l1: 125.0
|
||||||
|
l2: 125.0
|
||||||
|
x4: 110.0
|
||||||
|
z4: 80.0
|
||||||
|
|
||||||
|
# 关节限位 (mm 或度)
|
||||||
|
height_min: -290
|
||||||
|
height_max: 0
|
||||||
|
j2_min: -110
|
||||||
|
j2_max: 115
|
||||||
|
|
||||||
|
# 运动参数
|
||||||
|
default_duration: 1.0 # 默认运动时长 (秒)
|
||||||
|
default_rate: 20.0 # 插值频率 (Hz)
|
||||||
|
```
|
||||||
|
|
||||||
|
### vision_grasp 参数
|
||||||
|
|
||||||
|
编辑 `src/udp_teleop/config/vision_grasp.yaml`：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
vision_grasp:
|
||||||
|
ros__parameters:
|
||||||
|
# 相机到 TCP 的变换
|
||||||
|
cam_tx: 0.0
|
||||||
|
cam_ty: 0.0
|
||||||
|
cam_tz: 0.0
|
||||||
|
cam_pitch: 0.0 # 如果相机有俯仰角
|
||||||
|
|
||||||
|
# 回收位置
|
||||||
|
retract_position_x: 200.0
|
||||||
|
retract_position_y: 0.0
|
||||||
|
|
||||||
|
# 运动时长
|
||||||
|
grasp_duration: 3.0
|
||||||
|
release_duration: 2.0
|
||||||
|
```
|
||||||
|
|
||||||
|
## 📡 话题和服务
|
||||||
|
|
||||||
|
### arm_control
|
||||||
|
|
||||||
|
**服务**：
|
||||||
|
- `/arm_control/move_joints` - 关节运动
|
||||||
|
- `/arm_control/move_pose` - 位姿运动
|
||||||
|
- `/arm_control/get_pose` - 查询位姿
|
||||||
|
- `/arm_control/set_gripper` - 夹爪控制
|
||||||
|
|
||||||
|
**话题**（发布）：
|
||||||
|
- `/arm_control/joint_states` - 关节状态 (10Hz)
|
||||||
|
- `/arm_control/tcp_pose` - TCP 位姿 (10Hz)
|
||||||
|
|
||||||
|
### vision_grasp
|
||||||
|
|
||||||
|
**话题**（订阅）：
|
||||||
|
- `/vision_grasp/grasp_target` - 抓取目标（相机坐标）
|
||||||
|
- `/vision_grasp/release_target` - 释放目标（基坐标）
|
||||||
|
|
||||||
|
## 🐛 故障排查
|
||||||
|
|
||||||
|
### 编译失败
|
||||||
|
|
||||||
|
**问题**：找不到 Python 开发文件
|
||||||
|
|
||||||
|
**解决**：设置环境变量
|
||||||
|
```bash
|
||||||
|
export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
|
||||||
|
export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
|
||||||
|
export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
|
||||||
|
```
|
||||||
|
|
||||||
|
### 服务调用超时
|
||||||
|
|
||||||
|
**问题**：vision_grasp 节点服务调用超时
|
||||||
|
|
||||||
|
**原因**：在回调中使用 `time.sleep()` 阻塞了执行器
|
||||||
|
|
||||||
|
**解决**：已使用多线程执行器和独立线程处理
|
||||||
|
|
||||||
|
### 移动失败
|
||||||
|
|
||||||
|
**检查**：
|
||||||
|
1. ESP32 是否在线：`ping 192.168.4.1`
|
||||||
|
2. UDP 是否可达：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888`
|
||||||
|
3. 目标是否在工作空间内
|
||||||
|
4. 关节限位是否合理
|
||||||
|
|
||||||
|
## 📖 相关文档
|
||||||
|
|
||||||
|
- **机械臂运动学**：`docs/arm.md` - 完整的运动学推导
|
||||||
|
- **视觉标定**：`docs/vision_calibration_horizontal.md` - 相机标定指南
|
||||||
|
- **原始工具**：`tools/README.md` - 命令行工具文档
|
||||||
|
|
||||||
|
## 🔗 依赖关系
|
||||||
|
|
||||||
|
```
|
||||||
|
vision_grasp
|
||||||
|
↓ (依赖)
|
||||||
|
arm_control
|
||||||
|
↓ (依赖)
|
||||||
|
arm_control_msgs
|
||||||
|
```
|
||||||
|
|
||||||
|
所有节点都独立运行，通过 ROS 服务通信。
|
||||||
|
|
||||||
|
## 📝 下一步
|
||||||
|
|
||||||
|
1. **集成物体检测**：订阅相机图像，检测物体，发布到 `/vision_grasp/grasp_target`
|
||||||
|
2. **添加轨迹规划**：避障和路径优化
|
||||||
|
3. **可视化**：RViz 显示机械臂状态和检测结果
|
||||||
|
4. **多物体处理**：队列管理和优先级排序
|
||||||
|
|||||||
Reference in New Issue
Block a user