Files

FallenSigh 197de7908a fix: 修正 UP/DOWN 与 J5 角度的对应关系

问题：
- 之前错误地将 J5=81 (张开) 标记为 UP
- 实际上 J5=-100 (闭合) 才是 UP（夹爪朝上）

正确对应关系：
- UP: J5=-100 (闭合), z4=-100, 工作范围 z ∈ [-190, 110]mm
- DOWN: J5=81 (张开), z4=55, 工作范围 z ∈ [-345, -55]mm

修复内容：
- 重命名常量：Z4_OPEN/Z4_CLOSED → Z4_UP/Z4_DOWN
- 更新 resolve_z4_from_j5(): J5 < 0 → UP
- 更新 vision_grasp 相机旋转逻辑：J5 < 0 → 正向
- 更新文档说明

现在 UP/DOWN 语义正确且一致！

2026-06-16 20:01:24 +08:00

6.5 KiB

Raw Permalink Blame History

ROS 2 机械臂控制系统

CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。

📦 包含组件

1. arm_control_msgs

消息和服务定义包。

消息类型：

JointState - 关节状态
TCPPose - TCP 位姿

服务类型：

MoveJoints - 关节空间运动
MovePose - 笛卡尔空间运动
GetPose - 查询当前位姿
SetGripper - 夹爪控制

2. arm_control 节点

独立的机械臂控制节点（不依赖 tools/udp_control.py）。

功能：

关节空间和笛卡尔空间运动控制
完整的逆运动学和正运动学
自动 z4 适配：根据目标 z 坐标自动选择夹爪状态
- z > -55mm: UP（J5=-100，z4=-100），工作范围 z ∈ [-190, 110]mm
- z ≤ -55mm: DOWN（J5=81，z4=55），工作范围 z ∈ [-345, -55]mm
UDP 通信（与 ESP32）
状态发布（10Hz）

3. vision_grasp 节点

自动化视觉抓取节点。

功能：

相机坐标到基坐标系的自动转换
自动处理相机旋转：根据 J5 状态自动调整图像坐标转换
抓取流程：释放 → 移动 → 抓取 → 回收
释放流程：移动 → 释放 → 回收

相机旋转说明：

J5 < 0°（闭合/UP）：相机正向，(xc, yc, zc) = (x_img, -y_img, z)
J5 > 0°（张开/DOWN）：相机旋转 180°，(xc, yc, zc) = (-x_img, y_img, z)

🚀 快速开始

编译

cd ros2

# 设置 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

运行

机械臂控制：

ros2 run udp_teleop arm_control \
    --ros-args --params-file src/udp_teleop/config/arm_control.yaml

视觉抓取：

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

📚 使用示例

1. 控制机械臂

# 查询位姿
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. 视觉抓取

# 发布抓取目标（相机坐标）
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 集成

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：

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：

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 开发文件

解决：设置环境变量

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() 阻塞了执行器

解决：已使用多线程执行器和独立线程处理

移动失败

检查：

ESP32 是否在线：ping 192.168.4.1
UDP 是否可达：echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888
目标是否在工作空间内
关节限位是否合理

📖 相关文档

机械臂运动学：docs/arm.md - 完整的运动学推导
视觉标定：docs/vision_calibration_horizontal.md - 相机标定指南
原始工具：tools/README.md - 命令行工具文档

🔗 依赖关系

vision_grasp
    ↓ (依赖)
arm_control
    ↓ (依赖)
arm_control_msgs

所有节点都独立运行，通过 ROS 服务通信。

📝 下一步

集成物体检测：订阅相机图像，检测物体，发布到 /vision_grasp/grasp_target
添加轨迹规划：避障和路径优化
可视化：RViz 显示机械臂状态和检测结果
多物体处理：队列管理和优先级排序

6.5 KiB Raw Permalink Blame History Unescape Escape