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）**机器人任务挑战赛（小型桌面级）** 参赛代码。
|
||||
|
||||
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 类竞赛名单**。
|
||||
|
||||
@@ -23,68 +23,59 @@ ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机
|
||||
|
||||
```
|
||||
craic/
|
||||
├── jxbeye/ # ESP32-S3 摄像头固件 (PlatformIO)
|
||||
│ ├── src/main.cpp # 双核并行推流 + AsyncUDP + ESP-NOW
|
||||
│ ├── platformio.ini # 构建配置 (ESP32-S3-WROOM-1-N16R8)
|
||||
│ ├── boards/ # 自定义板级定义
|
||||
│ ├── lib/FTServo/ # 飞特舵机控制库
|
||||
│ └── include/ / test/
|
||||
├── ros2/ # ROS 2 Humble 键盘遥控
|
||||
│ └── src/udp_teleop/ # 底盘 + 6轴机械臂 UDP 遥控节点
|
||||
├── tools/ # 调试工具
|
||||
│ └── udp_server.py # UDP 回显测试服务器
|
||||
└── .gitignore
|
||||
├── jxbeye/ # ESP32-S3 固件 (PlatformIO)
|
||||
│ ├── src/main.cpp # 双核摄像头推流 + UDP 控制
|
||||
│ └── platformio.ini # ESP32-S3-WROOM-1-N16R8 配置
|
||||
├── ros2/ # ROS 2 控制系统
|
||||
│ ├── src/arm_control_msgs/ # 消息和服务定义
|
||||
│ └── src/udp_teleop/ # 机械臂控制和视觉抓取节点
|
||||
├── tools/ # 独立工具脚本
|
||||
│ ├── udp_control.py # 命令行机械臂控制（带逆运动学）
|
||||
│ ├── camera_to_base.py # 相机坐标到基坐标变换
|
||||
│ └── camera_capture.py # MJPEG 流采集
|
||||
└── 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) |
|
||||
| 舵机 | 飞特 SCS/STS 系列串行舵机 |
|
||||
| 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 |
|
||||
|
||||
## 功能
|
||||
## 核心功能
|
||||
|
||||
### 1. ESP32-S3 固件 (`jxbeye/`)
|
||||
|
||||
- **双核并行架构**：Core 0 负责摄像头采集，Core 1 负责 WiFi 推流
|
||||
- **MJPEG 实时推流**：Web 浏览器直接访问 `http://<IP>` 查看实时画面
|
||||
- **AsyncUDP 非阻塞通讯**：UDP 指令由中断回调处理，不干扰推流
|
||||
- **ESP-NOW**：低延迟点对点通信
|
||||
- **在线 WiFi 配置**：通过 Web 页面或串口修改 SSID/密码，自动保存到 NVS
|
||||
- **双核架构**：Core 0 采集，Core 1 推流
|
||||
- **MJPEG 推流**：`http://<IP>` 实时查看
|
||||
- **UDP 控制**：端口 8888，非阻塞异步处理
|
||||
- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置
|
||||
|
||||
### 2. ROS 2 键盘遥控 (`ros2/src/udp_teleop/`)
|
||||
### 2. ROS 2 机械臂控制 (`ros2/`)
|
||||
|
||||
- **底盘遥控**：W/S 前进后退、A/D 左右平移、Q/E 旋转（差速驱动）
|
||||
- **机械臂遥控**：6 轴关节角度独立控制，高度升降
|
||||
- **多平台键盘后端**：
|
||||
- `stdin` — Linux/macOS 终端原生输入，零依赖
|
||||
- `pynput` — 跨平台按键监听
|
||||
- `win_poll` — Windows Win32 API 轮询
|
||||
- **可配置参数**：速度、步长、目标 IP/端口均通过 YAML 配置
|
||||
**arm_control 节点**：
|
||||
- 关节空间和笛卡尔空间运动控制
|
||||
- 完整逆运动学和正运动学
|
||||
- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper`
|
||||
- 状态发布（10Hz）
|
||||
|
||||
### 3. UDP 通讯协议
|
||||
**vision_grasp 节点**：
|
||||
- 相机坐标到基坐标系自动转换
|
||||
- 自动抓取：释放 → 移动 → 抓取 → 回收
|
||||
- 自动释放：移动 → 释放 → 回收
|
||||
|
||||
所有指令通过 UDP 端口 `8888` 发送，格式：
|
||||
### 3. 工具脚本 (`tools/`)
|
||||
|
||||
```
|
||||
# 底盘控制
|
||||
XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
|
||||
|
||||
# 机械臂控制
|
||||
JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
|
||||
|
||||
# 激光控制
|
||||
LASERON / LASEROFF
|
||||
|
||||
# 串口透传
|
||||
<any payload ending with ZHY or \n>
|
||||
```
|
||||
- `udp_control.py` - 命令行机械臂控制（支持逆运动学）
|
||||
- `camera_to_base.py` - 坐标变换工具
|
||||
- `camera_capture.py` - 相机帧采集（支持自动扫描）
|
||||
|
||||
## 快速开始
|
||||
|
||||
### 编译 & 烧录 ESP32 固件
|
||||
### ESP32 固件
|
||||
|
||||
```bash
|
||||
cd jxbeye
|
||||
@@ -92,55 +83,134 @@ pio run -t upload
|
||||
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
|
||||
# 创建环境（首次）
|
||||
# 环境准备（首次）
|
||||
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
|
||||
|
||||
# 编译
|
||||
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
|
||||
|
||||
# 启动键盘遥控（默认目标 192.168.233.67:8888）
|
||||
ros2 run udp_teleop keyboard_control \
|
||||
--ros-args --params-file src/udp_teleop/config/params.yaml
|
||||
# 运行机械臂控制
|
||||
ros2 run udp_teleop arm_control \
|
||||
--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
|
||||
# 启动回显服务器
|
||||
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` 发送：
|
||||
|
||||
```
|
||||
WIFI:热点名称:密码
|
||||
```bash
|
||||
# 底盘控制（麦克纳姆轮）
|
||||
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 |
|
||||
| ROS 2 | ROS 2 Humble (Conda robostack), pynput |
|
||||
| 测试 | Python 3, Unix netcat |
|
||||
| ROS 2 | ROS 2 Humble (robostack), Python 3.12 |
|
||||
| 工具 | 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
|
||||
|
||||
# J4 中心
|
||||
x_j4 = L1 * math.cos(theta2) + L2 * math.cos(theta2 + theta3)
|
||||
y_j4 = L1 * math.sin(theta2) + L2 * math.sin(theta2 + theta3)
|
||||
|
||||
# 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)
|
||||
```
|
||||
|
||||
#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$
|
||||
## 逆运动学（IK）
|
||||
|
||||
既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置：
|
||||
已知 TCP 目标位姿 → 计算关节角度
|
||||
|
||||
### 输入
|
||||
|
||||
$$X_4 = X - x_4 \cos(\Phi)$$
|
||||
- `(x, y, z)`: 目标位置（mm）
|
||||
- `φ`: 目标偏航角（度）
|
||||
|
||||
$$Y_4 = Y - x_4 \sin(\Phi)$$
|
||||
### 输出
|
||||
|
||||
#### 第三步：求解关节 3 的角度 $\theta_3$
|
||||
- `d1`: 高度
|
||||
- `θ2, θ3, θ4`: 关节角度（度）
|
||||
|
||||
现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。
|
||||
根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有：
|
||||
### 推导
|
||||
|
||||
**步骤 1**：计算 J4 中心目标位置
|
||||
|
||||
$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
|
||||
从 TCP 位置反向计算 J4 位置：
|
||||
|
||||
设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。
|
||||
如果合法，则 $\theta_3$ 的正弦值为：
|
||||
$$
|
||||
x_{j4} = x - x_4 \cos\phi
|
||||
$$
|
||||
|
||||
$$
|
||||
y_{j4} = y - x_4 \sin\phi
|
||||
$$
|
||||
|
||||
$$S_3 = \pm \sqrt{1 - C_3^2}$$
|
||||
$$
|
||||
z_{j4} = z + z_4
|
||||
$$
|
||||
|
||||
$$
|
||||
d_1 = z_{j4}
|
||||
$$
|
||||
|
||||
*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)*
|
||||
**步骤 2**：计算平面距离
|
||||
|
||||
最终使用反正切函数求解 $\theta_3$：
|
||||
$$
|
||||
r = \sqrt{x_{j4}^2 + y_{j4}^2}
|
||||
$$
|
||||
|
||||
**步骤 3**：求解 θ3（余弦定理）
|
||||
|
||||
$$\theta_3 = \text{atan2}(S_3, C_3)$$
|
||||
二连杆机构的标准解法：
|
||||
|
||||
#### 第四步：求解关节 2 的角度 $\theta_2$
|
||||
$$
|
||||
\cos\theta_3 = \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}
|
||||
$$
|
||||
|
||||
利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$：
|
||||
检查工作空间：
|
||||
|
||||
$$
|
||||
-1 \leq \cos\theta_3 \leq 1 \quad \Rightarrow \quad |L_1 - L_2| \leq r \leq L_1 + L_2
|
||||
$$
|
||||
|
||||
$$\theta_2 = \text{atan2}(Y_4, X_4) - \text{atan2}(L_2 S_3, L_1 + L_2 C_3)$$
|
||||
否则目标超出工作空间。
|
||||
|
||||
#### 第五步：求解关节 4 的角度 $\theta_4$
|
||||
有两个解（肘部朝上 / 朝下）：
|
||||
|
||||
因为总朝向 $\Phi = \theta_2 + \theta_3 + \theta_4$，所以：
|
||||
$$
|
||||
\theta_3 = \pm \arccos\left(\frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)
|
||||
$$
|
||||
|
||||
**步骤 4**：求解 θ2
|
||||
|
||||
$$\theta_4 = \Phi - \theta_2 - \theta_3$$
|
||||
$$
|
||||
\theta_2 = \arctan2(y_{j4}, x_{j4}) - \arctan2\left(L_2\sin\theta_3, L_1 + L_2\cos\theta_3\right)
|
||||
$$
|
||||
|
||||
---
|
||||
**步骤 5**：求解 θ4
|
||||
|
||||
### 💡 嵌入式固件实现建议
|
||||
$$
|
||||
\theta_4 = \phi - \theta_2 - \theta_3
|
||||
$$
|
||||
|
||||
由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议：
|
||||
### Python 实现
|
||||
|
||||
1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。
|
||||
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$ 进行归一化和软限位拦截。
|
||||
```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
|
||||
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
|
||||
bash Miniconda3-latest-Linux-x86_64.sh
|
||||
**服务类型**：
|
||||
- `MoveJoints` - 关节空间运动
|
||||
- `MovePose` - 笛卡尔空间运动
|
||||
- `GetPose` - 查询当前位姿
|
||||
- `SetGripper` - 夹爪控制
|
||||
|
||||
# 创建 ROS 2 Humble 环境
|
||||
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
|
||||
conda activate ros2_humble
|
||||
### 2. arm_control 节点
|
||||
独立的机械臂控制节点（不依赖 tools/udp_control.py）。
|
||||
|
||||
# 安装构建工具
|
||||
conda install -c robostack-staging -c conda-forge \
|
||||
colcon-common-extensions \
|
||||
ros-humble-ament-cmake \
|
||||
python3-pip
|
||||
**功能**：
|
||||
- 关节空间和笛卡尔空间运动控制
|
||||
- 完整的逆运动学和正运动学
|
||||
- UDP 通信（与 ESP32）
|
||||
- 状态发布（10Hz）
|
||||
|
||||
# Python 依赖
|
||||
pip install pynput
|
||||
```
|
||||
### 3. vision_grasp 节点
|
||||
自动化视觉抓取节点。
|
||||
|
||||
### 方式二：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
|
||||
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
|
||||
```
|
||||
|
||||
> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
|
||||
|
||||
## 运行
|
||||
### 运行
|
||||
|
||||
**机械臂控制**：
|
||||
```bash
|
||||
ros2 run udp_teleop keyboard_control \
|
||||
--ros-args --params-file src/udp_teleop/config/params.yaml
|
||||
ros2 run udp_teleop arm_control \
|
||||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||||
```
|
||||
|
||||
也可以通过命令行覆盖参数：
|
||||
|
||||
**视觉抓取**：
|
||||
```bash
|
||||
ros2 run udp_teleop keyboard_control \
|
||||
--ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
|
||||
# 终端 1: 启动 arm_control
|
||||
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