From aa1bc2bf75d2f254ce2c37d7cfbcfc0bd9c7fb46 Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Tue, 16 Jun 2026 20:15:56 +0800 Subject: [PATCH] =?UTF-8?q?fix(vision=5Fgrasp):=20=E4=BF=AE=E6=AD=A3=20tcp?= =?UTF-8?q?=5Fto=5Fbase=20=E6=97=8B=E8=BD=AC=E7=9F=A9=E9=98=B5=20Y=20?= =?UTF-8?q?=E5=9D=90=E6=A0=87=E7=AC=A6=E5=8F=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 问题： - 相机坐标 (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/src/udp_teleop/udp_teleop/vision_grasp.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py index 6651209..2026943 100644 --- a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py +++ b/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.cos(phi), 0, math.sin(phi)], - [0, -1, 0] + [-math.sin(phi), 0, math.cos(phi)], # X_base + [-math.cos(phi), 0, -math.sin(phi)], # Y_base (修正：添加负号) + [0, -1, 0] # Z_base ]) P_tcp = np.array([xt, yt, zt])