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
parent aa7cd87151
commit 197de7908a
4 changed files with 23 additions and 184 deletions
--- a/ros2/README.md
+++ b/ros2/README.md
@@ -24,8 +24,8 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 - 关节空间和笛卡尔空间运动控制
 - 完整的逆运动学和正运动学
 - **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态
-  - `z > -55mm`: J5=-100（闭合），z4=-100mm，工作范围 z ∈ [-190, 110]mm
-  - `z ≤ -55mm`: J5=81（张开），z4=55mm，工作范围 z ∈ [-345, -55]mm
+  - `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）

@@ -39,8 +39,8 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 - 释放流程：移动 → 释放 → 回收

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

 ## 🚀 快速开始

--- a/ros2/src/udp_teleop/README.md
+++ b/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` 在终端直接运行。
--- a/ros2/src/udp_teleop/udp_teleop/arm_control.py
+++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py
@@ -51,8 +51,10 @@ J5_CLOSED = -100
 DEFAULT_FIXED_J5 = J5_OPEN

 # Z4 值根据夹爪状态变化
-Z4_OPEN = 55      # 夹爪张开（J5=81）
-Z4_CLOSED = -100  # 夹爪闭合（J5=-100）
+# 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
@@ -172,17 +174,17 @@ def normalize_angle_deg(angle_deg: float) -> float:
 def resolve_z4_from_j5(j5: int) -> float:
    """根据 J5 状态确定 z4 值

-    - J5 > 0 (张开): z4 = 55mm
-    - J5 < 0 (闭合): z4 = -100mm
+    - J5 = -100 (闭合): 夹爪朝上，z4 = -100mm
+    - J5 = 81 (张开): 夹爪朝下，z4 = 55mm
    """
-    return Z4_OPEN if j5 > 0 else Z4_CLOSED
+    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)
+    - z > -55: 使用朝上状态 (J5=-100, z4=-100)
+    - z <= -55: 使用朝下状态 (J5=81, z4=55)
    """
    return J5_CLOSED if z > -55 else J5_OPEN

@@ -606,9 +608,9 @@ class ArmControlNode(Node):

            # 解析 z4：优先级 up/down > 自动选择
            if request.up:
-                z4 = Z4_CLOSED  # 夹爪朝上，z4=-100
+                z4 = Z4_UP      # 夹爪朝上，z4=-100
            elif request.down:
-                z4 = Z4_OPEN    # 夹爪朝下，z4=55
+                z4 = Z4_DOWN    # 夹爪朝下，z4=55
            else:
                # 自动选择：根据目标 z 坐标
                j5_auto = resolve_j5_from_z(target_pose.z)
@@ -623,7 +625,7 @@ class ArmControlNode(Node):
                j6 = self.current_state.j6

            # 根据 z4 反推 J5（用于 UDP 命令）
-            j5 = J5_CLOSED if z4 == Z4_CLOSED else J5_OPEN
+            j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN

            # 逆运动学
            math_state = inverse_kinematics(
--- a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py
+++ b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py
@@ -286,20 +286,22 @@ class VisionGraspNode(Node):

            # 图像坐标到相机坐标系转换
            # 相机水平安装，但会随 J5 状态旋转 180°
-            if j5 > 0:
-                # J5 张开（UP）：相机正向
+            # J5 = -100 (闭合) → 夹爪朝上 (UP)
+            # J5 = 81 (张开) → 夹爪朝下 (DOWN)
+            if j5 < 0:
+                # J5 闭合（UP）：相机正向
                # 图像 Y 向下 → 相机 -Y
                xc = x
                yc = -y
                zc = z
-                self.get_logger().info(f'相机正向（UP），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+                self.get_logger().info(f'J5={j5}° (UP)，相机正向，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
            else:
-                # J5 闭合（DOWN）：相机旋转 180°
+                # J5 张开（DOWN）：相机旋转 180°
                # 图像 X,Y 都翻转（相对于 UP 状态）
                xc = -x
                yc = y
                zc = z
-                self.get_logger().info(f'相机旋转 180°（DOWN），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
+                self.get_logger().info(f'J5={j5}° (DOWN)，相机旋转 180°，相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')

            # 坐标变换：相机 → 基坐标系
            target_x, target_y, target_z = camera_to_base(