FallenSigh
83b32542ef
- 创建 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
320 lines
6.4 KiB
Markdown
320 lines
6.4 KiB
Markdown
# 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`
|