From aa7cd8715133f7a32a30f17bee6c020e46ff04f4 Mon Sep 17 00:00:00 2001
From: FallenSigh <fallensigh@gmail.com>
Date: Tue, 16 Jun 2026 19:44:12 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=E7=B1=BB=E5=9E=8B?=
 =?UTF-8?q?=E9=94=99=E8=AF=AF=E5=B9=B6=E6=B7=BB=E5=8A=A0=20up/down=20?=
 =?UTF-8?q?=E6=98=BE=E5=BC=8F=E6=8E=A7=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

问题 1：发布状态失败 - 类型错误
- 修复：在发布 TCP 位姿时显式转换为 float
- 修复：在 GetPose 响应中显式转换类型

问题 2：缺少显式 z4 控制
- 添加 up/down 参数到 MovePose 服务
- up: 强制使用 z4=-100（夹爪朝上）
- down: 强制使用 z4=55（夹爪朝下）
- 默认：根据目标 z 坐标自动选择

控制逻辑：
- z4 选择优先级：up/down > 自动选择
- up/down 与 grip/release 完全独立
- up/down 控制夹爪高度（z4）
- grip/release 控制夹爪开合（J6）

与 udp_control.py 行为完全一致！
---
 ros2/src/arm_control_msgs/srv/MovePose.srv    | 10 ++--
 ros2/src/udp_teleop/udp_teleop/arm_control.py | 49 ++++++++++---------
 2 files changed, 31 insertions(+), 28 deletions(-)

diff --git a/ros2/src/arm_control_msgs/srv/MovePose.srv b/ros2/src/arm_control_msgs/srv/MovePose.srv
index 4e0ec31..03bd8a5 100644
--- a/ros2/src/arm_control_msgs/srv/MovePose.srv
+++ b/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 grip           # 是否抓取
-bool release        # 是否释放
+bool up             # 夹爪朝上（z4=-100）
+bool down           # 夹爪朝下（z4=55）
+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
diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control.py b/ros2/src/udp_teleop/udp_teleop/arm_control.py
index 02f1eb7..1edeb09 100644
--- a/ros2/src/udp_teleop/udp_teleop/arm_control.py
+++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py
@@ -604,14 +604,17 @@ class ArmControlNode(Node):
                 phi_deg=request.phi
             )
 
-            # 解析夹爪状态
-            if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
-                j5 = J5_OPEN
-            elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
-                j5 = J5_CLOSED
+            # 解析 z4：优先级 up/down > 自动选择
+            if request.up:
+                z4 = Z4_CLOSED  # 夹爪朝上，z4=-100
+            elif request.down:
+                z4 = Z4_OPEN    # 夹爪朝下，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:
@@ -619,10 +622,8 @@ class ArmControlNode(Node):
             else:
                 j6 = self.current_state.j6
 
-            # 根据目标 z 坐标自动选择 J5 和 z4
-            if request.gripper_state == MovePose.Request.GRIPPER_KEEP:
-                j5 = resolve_j5_from_z(target_pose.z)
-            z4 = resolve_z4_from_j5(j5)
+            # 根据 z4 反推 J5（用于 UDP 命令）
+            j5 = J5_CLOSED if z4 == Z4_CLOSED else J5_OPEN
 
             # 逆运动学
             math_state = inverse_kinematics(
@@ -679,16 +680,16 @@ class ArmControlNode(Node):
             pose = forward_kinematics(self.geometry, math_state, z4)
 
             response.success = True
-            response.x = pose.x
-            response.y = pose.y
-            response.z = pose.z
-            response.phi = pose.phi_deg
-            response.height = self.current_state.height
-            response.j2 = self.current_state.j2
-            response.j3 = self.current_state.j3
-            response.j4 = self.current_state.j4
-            response.j5 = self.current_state.j5
-            response.j6 = self.current_state.j6
+            response.x = float(pose.x)
+            response.y = float(pose.y)
+            response.z = float(pose.z)
+            response.phi = float(pose.phi_deg)
+            response.height = int(self.current_state.height)
+            response.j2 = int(self.current_state.j2)
+            response.j3 = int(self.current_state.j3)
+            response.j4 = int(self.current_state.j4)
+            response.j5 = int(self.current_state.j5)
+            response.j6 = int(self.current_state.j6)
 
         except Exception as e:
             response.success = False
@@ -760,10 +761,10 @@ class ArmControlNode(Node):
 
             pose_msg = TCPPose()
             pose_msg.header.stamp = self.get_clock().now().to_msg()
-            pose_msg.x = pose.x
-            pose_msg.y = pose.y
-            pose_msg.z = pose.z
-            pose_msg.phi = pose.phi_deg
+            pose_msg.x = float(pose.x)
+            pose_msg.y = float(pose.y)
+            pose_msg.z = float(pose.z)
+            pose_msg.phi = float(pose.phi_deg)
             self.pub_tcp_pose.publish(pose_msg)
 
         except Exception as e: