craic/docs/ARM_CONTROL_README.md

arm_control ROS 节点使用指南

概述

arm_control 是一个封装了机械臂控制功能的 ROS 2 节点，基于 udp_control.py 改造，提供服务接口进行机械臂控制。

功能特性

• ✅ 关节空间运动控制（带插值）
• ✅ 笛卡尔空间运动控制（带逆运动学）
• ✅ 正运动学查询
• ✅ 夹爪控制
• ✅ 状态发布（关节状态 + TCP 位姿）
• ✅ 状态缓存（平滑运动）

编译

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

运行

启动控制节点

# 使用默认参数
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. 关节空间运动

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}"

# 带夹爪控制
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. 查询当前位姿

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. 夹爪控制

# 抓取
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. 关节状态

ros2 topic echo /arm_control/joint_states

输出：

header:
  stamp:
    sec: 1234567890
    nanosec: 123456789
  frame_id: ''
height: -100
j2: 13
j3: 27
j4: 55
j5: 81
j6: 30

2. TCP 位姿

ros2 topic echo /arm_control/tcp_pose

输出：

header:
  stamp:
    sec: 1234567890
    nanosec: 123456789
  frame_id: ''
x: 150.234
y: 75.123
z: -100.0
phi: 45.678

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()

完整抓取流程示例

# 运行示例客户端（包含完整抓取流程）
ros2 run udp_teleop arm_control_client

或手动调用：

# 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：

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
    # ... (更多参数见配置文件)

调试

查看服务列表

ros2 service list | grep arm_control

查看话题列表

ros2 topic list | grep arm_control

查看服务接口定义

ros2 interface show arm_control_msgs/srv/MovePose

实时监控状态

# 终端 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