feat(arm_control): 实现动态 z4 自动适配机制

问题根源：
- udp_control.py 使用动态 z4（根据目标 z 自动选择）
- arm_control.py 使用固定 z4=80
- 导致相同目标位姿在两个工具中行为不一致

解决方案：
- 添加 resolve_j5_from_z(): 根据目标 z 自动选择 J5
- 添加 resolve_z4_from_j5(): 根据 J5 确定 z4 值
- 更新正/逆运动学接受动态 z4 参数
- 自动适配规则：
  * z > -55mm: J5=-100, z4=-100 (闭合, 工作范围 [-190, 110])
  * z ≤ -55mm: J5=81, z4=55 (张开, 工作范围 [-345, -55])

现在 arm_control 与 udp_control.py 行为一致！

ros2/README.md

@@ -23,6 +23,9 @@ 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
 - UDP 通信（与 ESP32）
 - 状态发布（10Hz）
 

ros2/src/udp_teleop/udp_teleop/arm_control.py

@@ -50,6 +50,10 @@ J5_OPEN = 81
 J5_CLOSED = -100
 DEFAULT_FIXED_J5 = J5_OPEN
 
+# Z4 值根据夹爪状态变化
+Z4_OPEN = 55      # 夹爪张开（J5=81）
+Z4_CLOSED = -100  # 夹爪闭合（J5=-100）
+
 GRIP_ANGLE = -5
 RELEASE_ANGLE = 80
 DEFAULT_FIXED_J6 = RELEASE_ANGLE
@@ -61,7 +65,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,8 +169,32 @@ def normalize_angle_deg(angle_deg: float) -> float:
     return normalized
 
 
-def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
-    """正运动学：关节角度 → TCP 位姿"""
+def resolve_z4_from_j5(j5: int) -> float:
+    """根据 J5 状态确定 z4 值
+
+    - J5 > 0 (张开): z4 = 55mm
+    - J5 < 0 (闭合): z4 = -100mm
+    """
+    return Z4_OPEN if j5 > 0 else Z4_CLOSED
+
+
+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)
@@ -183,7 +211,7 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
 
     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,13 +223,24 @@ 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
     
@@ -580,6 +619,11 @@ 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)
+
             # 逆运动学
             math_state = inverse_kinematics(
                 geometry=self.geometry,
@@ -588,6 +632,7 @@ class ArmControlNode(Node):
                 elbow_up=request.elbow_up,
                 j5=j5,
                 j6=j6,
+                z4=z4,
             )
 
             # 转换为命令状态
@@ -628,7 +673,10 @@ 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
@@ -707,7 +755,8 @@ 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()