fix(vision_grasp): 修正 tcp_to_base 旋转矩阵 Y 坐标符号

问题：
- 相机坐标 (10, -5, 100) → 基坐标 (299.6, 10, 15)
- 期望：(299.6, -10, 15)
- Y 坐标符号相反

根因：
- 旋转矩阵第二行：TCP X → 基坐标 Y
- 应该是：TCP X → 基坐标 -Y

修复：
- 第二行从 [cos(phi), 0, sin(phi)]
- 改为 [-cos(phi), 0, -sin(phi)]

验证（phi=0）：
- TCP (10, -5, 100) + TCP位置(199.6, 0, 10)
- = 基坐标 (299.6, -10, 15) ✓

ros2/README.md

@@ -23,6 +23,9 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 **功能**：
 - 关节空间和笛卡尔空间运动控制
 - 完整的逆运动学和正运动学
+- **自动 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）
 
@@ -31,9 +34,14 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 
 **功能**：
 - 相机坐标到基坐标系的自动转换
+- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换
 - 抓取流程：释放 → 移动 → 抓取 → 回收
 - 释放流程：移动 → 释放 → 回收
 
+**相机旋转说明**：
+- J5 < 0°（闭合/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)`
+- J5 > 0°（张开/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)`
+
 ## 🚀 快速开始
 
 ### 编译

ros2/src/arm_control_msgs/srv/MovePose.srv

@@ -4,12 +4,14 @@ float64 y           # 目标 Y 坐标 (mm)
 float64 z           # 目标 Z 坐标 (mm)
 float64 phi         # 目标偏航角 (度)
 bool elbow_up       # 是否使用肘部向上解
-uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
+bool up             # 夹爪朝上（z4=-100）
-bool grip           # 是否抓取
+bool down           # 夹爪朝下（z4=55）
-bool release        # 是否释放
+bool grip           # 是否抓取（J6=-5）
+bool release        # 是否释放（J6=80）
 float64 duration    # 运动时长 (秒，0 表示使用默认值)
 
-# 夹爪状态常量
+# 已废弃：使用 up/down 代替
+uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
 uint8 GRIPPER_KEEP = 0
 uint8 GRIPPER_OPEN = 1
 uint8 GRIPPER_CLOSED = 2

ros2/src/udp_teleop/README.md

@@ -1,165 +0,0 @@
-# udp_teleop — ROS 2 底盘 + 机械臂键盘 UDP 遥控
-
-通过键盘实时控制底盘（差速驱动）和机械臂（6 电机），指令通过 **单一 UDP socket** 发送到设备端。
-
-## 项目结构
-
-```
-ros2/
-├── build/                     # colcon 构建产物（自动生成）
-├── install/                   # colcon 安装产物（自动生成）
-├── log/                       # 构建日志（自动生成）
-└── src/
-    └── udp_teleop/            # ROS 2 包
-        ├── config/
-        │   └── params.yaml        # 可配置参数
-        ├── launch/                # launch 文件（预留）
-        ├── udp_teleop/
-        │   ├── __init__.py
-        │   └── keyboard_control.py  # 键盘遥控节点
-        ├── resource/
-        ├── test/
-        ├── package.xml
-        ├── setup.cfg
-        └── setup.py
-```
-
-## 环境搭建
-
-### 1. 安装 Conda
-
-使用 Miniconda 或 Anaconda。推荐 Miniconda：
-
-```bash
-# 下载并安装 Miniconda
-wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
-bash Miniconda3-latest-Linux-x86_64.sh
-```
-
-### 2. 创建 ROS 2 Humble 环境
-
-使用 robostack 频道安装 ROS 2 Humble Desktop 完整版：
-
-```bash
-conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
-```
-
-### 3. 安装构建工具
-
-```bash
-conda activate ros2_humble
-conda install -c robostack-staging -c conda-forge \
-    colcon-common-extensions \
-    ros-humble-ament-cmake \
-    python3-pip
-```
-
-### 4. 安装 Python 依赖
-
-```bash
-pip install pynput
-```
-
-### 5. 激活环境
-
-每次使用前：
-
-```bash
-conda activate ros2_humble
-source /path/to/ros2/install/setup.bash   # 首次构建后执行
-```
-
-## 构建
-
-```bash
-cd ros2
-colcon build --symlink-install --packages-select udp_teleop
-source install/setup.bash
-```
-
-> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
-
-## 运行
-
-### 使用参数文件（推荐）
-
-```bash
-ros2 run udp_teleop keyboard_control \
-    --ros-args --params-file src/udp_teleop/config/params.yaml
-```
-
-### 命令行覆盖参数
-
-```bash
-ros2 run udp_teleop keyboard_control \
-    --ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
-```
-
-## 按键映射
-
-### 底盘控制
-
-| 按键 | 功能 |
-|------|------|
-| `W` / `S` | 前进 / 后退 |
-| `A` / `D` | 左移 / 右移 |
-| `Q` / `E` | 左转 / 右转 |
-
-### 机械臂控制
-
-| 按键 | 功能 |
-|------|------|
-| `↑` / `↓` | 升降高度（↑ 升高，↓ 降低） |
-| `2` ~ `6` | 选择关节 J2 ~ J6 |
-| `←` / `→` | 减小 / 增大当前关节角度 |
-
-> 底盘和机械臂可以**同时操控**。机械臂指令仅在按下机械臂相关按键后发送。
-
-### 其他
-
-| 按键 | 功能 |
-|------|------|
-| `Ctrl+C` | 退出并发送底盘停止指令 |
-
-## UDP 协议
-
-### 底盘指令
-
-```
-XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
-```
-
-### 机械臂指令
-
-```
-JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
-```
-
-- 6 个电机：电机 1 控制高度，电机 2~6 对应关节 J2~J6
-- 末尾补零至 8 个值
-
-## 参数配置
-
-| 参数 | 默认值 | 说明 |
-|------|--------|------|
-| `udp_ip` | `127.0.0.1` | UDP 目标 IP 地址 |
-| `udp_port` | `8888` | UDP 目标端口 |
-| `chassis_linear_speed` | `100` | 底盘线速度 |
-| `chassis_angular_speed` | `45` | 底盘角速度 |
-| `arm_height_step` | `5` | 高度每步变化量 |
-| `arm_joint_step` | `5` | 关节角度每步变化量 |
-| `update_rate` | `0.05` | 控制循环周期（秒） |
-| `stdin_hold_time` | `0.04` | 按键持续时间（秒），修复箭头键时序问题 |
-| `debug_keys` | `false` | 是否在状态行显示当前按键 |
-| `keyboard_backend` | `auto` | 键盘后端：`auto` / `stdin` / `pynput` / `win_poll` |
-
-## 键盘后端
-
-| 后端 | 说明 |
-|------|------|
-| `auto` | 自动选择：Linux/macOS 用 `stdin`，Windows 用 `win_poll` |
-| `stdin` | 基于终端原始输入，无需额外依赖，**需要交互终端** |
-| `pynput` | 基于 pynput 库，跨平台，需要 `pip install pynput` |
-| `win_poll` | Windows 专用，通过 Win32 API 轮询按键状态 |
-
-> `ros2 launch` 启动的子进程**没有交互终端**，使用 `stdin` 后端会报错。必须通过 `ros2 run` 在终端直接运行。

ros2/src/udp_teleop/udp_teleop/arm_control.py

@@ -50,6 +50,12 @@ J5_OPEN = 81
 J5_CLOSED = -100
 DEFAULT_FIXED_J5 = J5_OPEN
 
+# Z4 值根据夹爪状态变化
+# J5 = -100 (闭合) → 夹爪朝上 (UP) → z4 = -100
+# J5 = 81 (张开) → 夹爪朝下 (DOWN) → z4 = 55
+Z4_UP = -100      # 夹爪朝上（J5=-100，闭合）
+Z4_DOWN = 55      # 夹爪朝下（J5=81，张开）
+
 GRIP_ANGLE = -5
 RELEASE_ANGLE = 80
 DEFAULT_FIXED_J6 = RELEASE_ANGLE
@@ -61,7 +67,7 @@ DEFAULT_ZERO_J4 = 25
 DEFAULT_L1 = 125.0
 DEFAULT_L2 = 125.0
 DEFAULT_X4 = 110.0
-DEFAULT_Z4 = 80.0
+DEFAULT_Z4 = 80.0  # 仅用于配置默认值，实际使用动态 z4
 
 DEFAULT_INTERP_DURATION = 1.0
 DEFAULT_INTERP_RATE = 20.0
@@ -165,13 +171,37 @@ def normalize_angle_deg(angle_deg: float) -> float:
     return normalized
 
 
-def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
+def resolve_z4_from_j5(j5: int) -> float:
-    """正运动学：关节角度 → TCP 位姿"""
+    """根据 J5 状态确定 z4 值
+
+    - J5 = -100 (闭合): 夹爪朝上，z4 = -100mm
+    - J5 = 81 (张开): 夹爪朝下，z4 = 55mm
+    """
+    return Z4_UP if j5 < 0 else Z4_DOWN
+
+
+def resolve_j5_from_z(z: float) -> int:
+    """根据目标 z 坐标自动选择夹爪状态
+
+    - z > -55: 使用朝上状态 (J5=-100, z4=-100)
+    - z <= -55: 使用朝下状态 (J5=81, z4=55)
+    """
+    return J5_CLOSED if z > -55 else J5_OPEN
+
+
+def forward_kinematics(geometry: ArmGeometry, state: ArmMathState, z4: float) -> ArmPose:
+    """正运动学：关节角度 → TCP 位姿
+
+    Args:
+        geometry: 机械臂几何参数
+        state: 数学坐标系的关节状态
+        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
+    """
     theta2 = math.radians(state.theta2_deg)
     theta3 = math.radians(state.theta3_deg)
     theta4 = math.radians(state.theta4_deg)
     phi = theta2 + theta3 + theta4
-    
+
     j4_center_x = (
         geometry.l1 * math.cos(theta2) +
         geometry.l2 * math.cos(theta2 + theta3)
@@ -180,11 +210,11 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
         geometry.l1 * math.sin(theta2) +
         geometry.l2 * math.sin(theta2 + theta3)
     )
-    
+
     x = j4_center_x + geometry.x4 * math.cos(phi)
     y = j4_center_y + geometry.x4 * math.sin(phi)
-    z = state.d1 - geometry.z4
+    z = state.d1 - z4  # 使用动态 z4
-    
+
     return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
 
 
@@ -195,14 +225,25 @@ def inverse_kinematics(
     elbow_up: bool,
     j5: int,
     j6: int,
+    z4: float,
 ) -> ArmMathState:
-    """逆运动学：TCP 位姿 → 关节角度"""
+    """逆运动学：TCP 位姿 → 关节角度
+
+    Args:
+        geometry: 机械臂几何参数
+        pose: 目标 TCP 位姿
+        limits: 关节限位
+        elbow_up: 肘部朝上/朝下
+        j5: J5 角度
+        j6: J6 角度
+        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
+    """
     # 计算 J4 中心位置
     phi = math.radians(pose.phi_deg)
     j4_x = pose.x - geometry.x4 * math.cos(phi)
     j4_y = pose.y - geometry.x4 * math.sin(phi)
-    j4_z = pose.z + geometry.z4
+    j4_z = pose.z + z4  # 使用动态 z4
-    
+
     d1 = j4_z
     
     # 计算平面距离
@@ -565,14 +606,17 @@ class ArmControlNode(Node):
                 phi_deg=request.phi
             )
 
-            # 解析夹爪状态
+            # 解析 z4：优先级 up/down > 自动选择
-            if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
+            if request.up:
-                j5 = J5_OPEN
+                z4 = Z4_UP      # 夹爪朝上，z4=-100
-            elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
+            elif request.down:
-                j5 = J5_CLOSED
+                z4 = Z4_DOWN    # 夹爪朝下，z4=55
             else:
-                j5 = self.current_state.j5
+                # 自动选择：根据目标 z 坐标
+                j5_auto = resolve_j5_from_z(target_pose.z)
+                z4 = resolve_z4_from_j5(j5_auto)
 
+            # 解析夹爪开合（J6：grip/release）
             if request.grip:
                 j6 = GRIP_ANGLE
             elif request.release:
@@ -580,6 +624,9 @@ class ArmControlNode(Node):
             else:
                 j6 = self.current_state.j6
 
+            # 根据 z4 反推 J5（用于 UDP 命令）
+            j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN
+
             # 逆运动学
             math_state = inverse_kinematics(
                 geometry=self.geometry,
@@ -588,6 +635,7 @@ class ArmControlNode(Node):
                 elbow_up=request.elbow_up,
                 j5=j5,
                 j6=j6,
+                z4=z4,
             )
 
             # 转换为命令状态
@@ -628,19 +676,22 @@ class ArmControlNode(Node):
                 return response
 
             math_state = command_to_math_state(self.current_state, self.zero_offsets)
-            pose = forward_kinematics(self.geometry, math_state)
+
+            # 根据当前 J5 状态确定 z4
+            z4 = resolve_z4_from_j5(self.current_state.j5)
+            pose = forward_kinematics(self.geometry, math_state, z4)
 
             response.success = True
-            response.x = pose.x
+            response.x = float(pose.x)
-            response.y = pose.y
+            response.y = float(pose.y)
-            response.z = pose.z
+            response.z = float(pose.z)
-            response.phi = pose.phi_deg
+            response.phi = float(pose.phi_deg)
-            response.height = self.current_state.height
+            response.height = int(self.current_state.height)
-            response.j2 = self.current_state.j2
+            response.j2 = int(self.current_state.j2)
-            response.j3 = self.current_state.j3
+            response.j3 = int(self.current_state.j3)
-            response.j4 = self.current_state.j4
+            response.j4 = int(self.current_state.j4)
-            response.j5 = self.current_state.j5
+            response.j5 = int(self.current_state.j5)
-            response.j6 = self.current_state.j6
+            response.j6 = int(self.current_state.j6)
 
         except Exception as e:
             response.success = False
@@ -707,14 +758,15 @@ class ArmControlNode(Node):
 
             # 计算并发布 TCP 位姿
             math_state = command_to_math_state(self.current_state, self.zero_offsets)
-            pose = forward_kinematics(self.geometry, math_state)
+            z4 = resolve_z4_from_j5(self.current_state.j5)
+            pose = forward_kinematics(self.geometry, math_state, z4)
 
             pose_msg = TCPPose()
             pose_msg.header.stamp = self.get_clock().now().to_msg()
-            pose_msg.x = pose.x
+            pose_msg.x = float(pose.x)
-            pose_msg.y = pose.y
+            pose_msg.y = float(pose.y)
-            pose_msg.z = pose.z
+            pose_msg.z = float(pose.z)
-            pose_msg.phi = pose.phi_deg
+            pose_msg.phi = float(pose.phi_deg)
             self.pub_tcp_pose.publish(pose_msg)
 
         except Exception as e:

ros2/src/udp_teleop/udp_teleop/vision_grasp.py

@@ -71,10 +71,14 @@ def tcp_to_base(
     """TCP 坐标系 → 机械臂基坐标系（水平相机版本）"""
     phi = math.radians(tcp_phi_deg)
 
+    # 旋转矩阵：TCP → 基坐标系
+    # TCP X → 基坐标 -Y
+    # TCP Y → 基坐标 -Z
+    # TCP Z → 基坐标 X
     R_tcp_to_base = np.array([
-        [-math.sin(phi), 0, math.cos(phi)],
+        [-math.sin(phi), 0,  math.cos(phi)],   # X_base
-        [ math.cos(phi), 0, math.sin(phi)],
+        [-math.cos(phi), 0, -math.sin(phi)],   # Y_base (修正：添加负号)
-        [0,             -1, 0]
+        [0,             -1,  0]                # Z_base
     ])
 
     P_tcp = np.array([xt, yt, zt])
@@ -183,8 +187,8 @@ class VisionGraspNode(Node):
         self.get_logger().info('  - /vision_grasp/grasp_target (geometry_msgs/Point)')
         self.get_logger().info('  - /vision_grasp/release_target (geometry_msgs/Point)')
 
-    def get_current_tcp_pose(self) -> Tuple[float, float, float, float]:
+    def get_current_tcp_pose(self) -> Tuple[float, float, float, float, int]:
-        """查询当前 TCP 位姿"""
+        """查询当前 TCP 位姿（包括 J5 状态）"""
         req = GetPose.Request()
         future = self.get_pose_cli.call_async(req)
 
@@ -202,7 +206,7 @@ class VisionGraspNode(Node):
         if not result.success:
             raise RuntimeError(f"获取 TCP 位姿失败: {result.message}")
 
-        return result.x, result.y, result.z, result.phi
+        return result.x, result.y, result.z, result.phi, result.j5
 
     def move_to(self, x: float, y: float, z: float, phi: float,
                 duration: float, grip: bool = False, release: bool = False) -> bool:
@@ -280,18 +284,31 @@ class VisionGraspNode(Node):
     def _execute_grasp_thread(self, x: float, y: float, z: float):
         """在独立线程中执行抓取流程"""
         try:
-            # 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z)
+            # 获取当前 TCP 位姿（包括 J5 状态）
-            xc = x
+            tcp_x, tcp_y, tcp_z, tcp_phi, j5 = self.get_current_tcp_pose()
-            yc = -y
+            self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°, j5={j5}°')
-            zc = z
 
-            self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+            # 图像坐标到相机坐标系转换
+            # 相机水平安装，但会随 J5 状态旋转 180°
+            # J5 = -100 (闭合) → 夹爪朝上 (UP) → 相机正向
+            # J5 = 81 (张开) → 夹爪朝下 (DOWN) → 相机旋转 180°
 
-            # 获取当前 TCP 位姿
+            if j5 < 0:
-            tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose()
+                # J5 闭合（UP）：相机正向
-            self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°')
+                # 图像 Y 向下 → 相机 -Y
+                xc = x
+                yc = -y
+                zc = z
+                self.get_logger().info(f'J5={j5}° (UP)，相机正向，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+            else:
+                # J5 张开（DOWN）：相机旋转 180°
+                # 图像 X,Y 都翻转（相对于 UP 状态）
+                xc = -x
+                yc = y
+                zc = z
+                self.get_logger().info(f'J5={j5}° (DOWN)，相机旋转 180°，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
 
-            # 坐标变换
+            # 坐标变换：相机 → 基坐标系
             target_x, target_y, target_z = camera_to_base(
                 xc, yc, zc,
                 tcp_x, tcp_y, tcp_z, tcp_phi,
@@ -323,7 +340,7 @@ class VisionGraspNode(Node):
         """在独立线程中执行释放流程"""
         try:
             # 获取当前 TCP 位姿（用于获取 phi）
-            _, _, _, tcp_phi = self.get_current_tcp_pose()
+            _, _, _, tcp_phi, _ = self.get_current_tcp_pose()
 
             # 执行释放流程
             self.execute_release(x, y, z, tcp_phi)