feat: 添加机械臂 ROS 2 控制节点和视觉抓取系统
- 创建 arm_control_msgs 包：定义机械臂控制的消息和服务接口 - 消息：JointState, TCPPose - 服务：MoveJoints, MovePose, GetPose, SetGripper - 实现 arm_control 节点：独立的机械臂控制 ROS 节点 - 完整的逆运动学和正运动学 - 关节空间和笛卡尔空间运动控制 - UDP 通信与 ESP32 - 状态发布（10Hz） - 实现 vision_grasp 节点：自动化视觉抓取 - 相机坐标系到基坐标系的完整变换 - 自动抓取流程：释放→移动→抓取→回收 - 自动释放流程：移动→释放→回收 - 多线程执行器支持 - 添加完整文档 - ARM_CONTROL_README.md: 机械臂控制节点使用指南 - VISION_GRASP_README.md: 视觉抓取节点使用指南 - QUICKSTART.md: 快速开始指南 - 文档重命名：docs/craic.md → docs/arm.md
This commit is contained in:
319
docs/ARM_CONTROL_README.md
Normal file
319
docs/ARM_CONTROL_README.md
Normal file
@@ -0,0 +1,319 @@
|
||||
# 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`
|
||||
Reference in New Issue
Block a user