feat: 将机械臂坐标系改为 z 轴朝上

主要修改:
- 坐标系统: z=0 为顶部改为底部，z 轴朝上
- 高度范围: d1 ∈ [-290, 0] mm (-290=底部, 0=顶部)
- Z4 偏移: 80.0 (原 -80.0), Z4_OPEN=55, Z4_CLOSED=-100
- 运动学: 更新 tcp_to_joint4_center 和 forward_kinematics 的 z 坐标变换
- 夹爪逻辑: 更新 resolve_gripper_from_z 的高度判断范围
- 插值优化: 先完成末端运动，再处理 grip/release
- 移除所有坐标转换中的负号，统一为正向 z 轴朝上系统

tools/udp_control.py

@@ -27,8 +27,8 @@ from pathlib import Path
 DEFAULT_UDP_IP = "192.168.4.1"
 DEFAULT_UDP_PORT = 8888
 
-DEFAULT_HEIGHT_MIN = 0      # 用户坐标系：顶部，单位 mm
+DEFAULT_HEIGHT_MIN = -290    # 内部 d1 坐标：底部，单位 mm（z 轴朝上）
-DEFAULT_HEIGHT_MAX = 290    # 用户坐标系：底部，单位 mm
+DEFAULT_HEIGHT_MAX = 0       # 内部 d1 坐标：顶部，单位 mm（z 轴朝上）
 DEFAULT_JOINT_MIN = -180
 DEFAULT_JOINT_MAX = 180
 DEFAULT_J2_MIN = -110
@@ -39,21 +39,21 @@ DEFAULT_J4_MIN = -90
 DEFAULT_J4_MAX = 130
 J5_OPEN = 81
 J5_CLOSED = -100
-Z4_OPEN = -55
+Z4_OPEN = 55
-Z4_CLOSED = 100
+Z4_CLOSED = -100
 DEFAULT_FIXED_J5 = J5_OPEN
-DEFAULT_FIXED_J6 = 0
 # ==== 抓取/释放（由 J6 控制）====
 # 请填写实际角度值：
 GRIP_ANGLE = -5      # TODO: 填写抓取时 J6 的角度
 RELEASE_ANGLE = 30   # TODO: 填写释放时 J6 的角度
+DEFAULT_FIXED_J6 = RELEASE_ANGLE
 DEFAULT_ZERO_J2 = 3
 DEFAULT_ZERO_J3 = 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
 DEFAULT_INTERP_DURATION = 1.0
 DEFAULT_INTERP_RATE = 20.0
 STATE_FILE = Path(__file__).with_name(".udp_control_state.json")
@@ -133,7 +133,7 @@ class Joint4Center:
 
 def default_command_state() -> ArmJointState:
     return ArmJointState(
-        height=-DEFAULT_HEIGHT_MIN,
+        height=-DEFAULT_HEIGHT_MAX,
         j2=DEFAULT_ZERO_J2,
         j3=DEFAULT_ZERO_J3,
         j4=DEFAULT_ZERO_J4,
@@ -177,7 +177,7 @@ def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center:
     return Joint4Center(
         x=pose.x - geometry.x4 * math.cos(phi),
         y=pose.y - geometry.x4 * math.sin(phi),
-        z=pose.z - geometry.z4,
+        z=pose.z + geometry.z4,
     )
 
 
@@ -196,7 +196,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 - geometry.z4
     return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
 
 
@@ -266,8 +266,8 @@ def math_to_command_state(
     j6: int,
 ) -> ArmJointState:
     return ArmJointState(
-        height=-clamp_int(
+        height=clamp_int(
-            round(-math_state.d1),
+            round(math_state.d1),
             limits.height_min,
             limits.height_max,
             "height(cmd)",
@@ -302,7 +302,7 @@ def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None:
     try:
         payload = json.loads(STATE_FILE.read_text(encoding="utf-8"))
         return ArmJointState(
-            height=-clamp_int(-payload["height"], limits.height_min, limits.height_max, "height(cmd)"),
+            height=clamp_int(payload["height"], limits.height_min, limits.height_max, "height(cmd)"),
             j2=clamp_int(payload["j2"], limits.j2_min, limits.j2_max, "J2"),
             j3=clamp_int(payload["j3"], limits.j3_min, limits.j3_max, "J3"),
             j4=clamp_int(payload["j4"], limits.j4_min, limits.j4_max, "J4"),
@@ -401,7 +401,11 @@ def build_pose_command_path(
         )
         return [math_to_command_state(math_state, zero_offsets, limits, j5, j6)]
 
+    # Interpolate pose first (movement)
     path: list[ArmJointState] = []
+    start_command_j5 = None
+    start_command_j6 = None
+
     for step_index in range(1, steps + 1):
         t = step_index / steps
         pose = interpolate_pose(start_pose, target_pose, t)
@@ -413,7 +417,35 @@ def build_pose_command_path(
             j5=j5,
             j6=j6,
         )
-        path.append(math_to_command_state(math_state, zero_offsets, limits, j5, j6))
+        command = math_to_command_state(math_state, zero_offsets, limits, j5, j6)
+
+        # Store the first command's j5/j6 values
+        if start_command_j5 is None:
+            start_command_j5 = command.j5
+            start_command_j6 = command.j6
+
+        # For all movement steps, use the starting j5/j6 values
+        path.append(ArmJointState(
+            height=command.height,
+            j2=command.j2,
+            j3=command.j3,
+            j4=command.j4,
+            j5=start_command_j5,
+            j6=start_command_j6,
+        ))
+
+    # Add final grip/release adjustment if j5 or j6 changed
+    final = path[-1]
+    if final.j5 != j5 or final.j6 != j6:
+        path.append(ArmJointState(
+            height=final.height,
+            j2=final.j2,
+            j3=final.j3,
+            j4=final.j4,
+            j5=j5,
+            j6=j6,
+        ))
+
     return path
 
 
@@ -484,13 +516,13 @@ def build_parser() -> argparse.ArgumentParser:
         "--height-min",
         type=int,
         default=DEFAULT_HEIGHT_MIN,
-        help=f"高度下限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}",
+        help=f"高度下限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}",
     )
     parser.add_argument(
         "--height-max",
         type=int,
         default=DEFAULT_HEIGHT_MAX,
-        help=f"高度上限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}",
+        help=f"高度上限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}",
     )
     parser.add_argument(
         "--joint-min",
@@ -571,7 +603,7 @@ def build_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="不读取或更新上次发送的关节命令缓存",
     )
-    joints.add_argument("--height", type=int, required=True, help="升降高度 mm (0=顶部, 290=底部)")
+    joints.add_argument("--height", type=int, required=True, help="内部 d1 坐标 mm (-290=底部, 0=顶部，z 轴朝上)")
     joints.add_argument("--j2", type=int, required=True, help="UDP 指令里的 J2 命令值")
     joints.add_argument("--j3", type=int, required=True, help="UDP 指令里的 J3 命令值")
     joints.add_argument("--j4", type=int, required=True, help="UDP 指令里的 J4 命令值")
@@ -617,7 +649,7 @@ def build_parser() -> argparse.ArgumentParser:
     )
     pose.add_argument("--x", type=float, required=True, help="TCP X 坐标 mm")
     pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标 mm")
-    pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (0=顶部, 正值向下)")
+    pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (-290=底部, 0=顶部, z 轴朝上)")
     pose.add_argument("--phi", type=float, required=True, help="TCP 偏航角，单位度；等于 J2+J3+J4")
     pose.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})")
     pose.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})")
@@ -682,13 +714,13 @@ def resolve_z4(up: bool) -> float:
 
 
 def resolve_gripper_from_z(z: float) -> bool:
-    """Auto-select gripper state based on TCP z coordinate (user mm).
+    """Auto-select gripper state based on TCP z coordinate (user mm, z-up).
 
-    - z in [55, 345] -> down  (gripper open,  J5=81,  Z4=-55)
+    - z in [-345, -55] -> down  (gripper open,  J5=81,  Z4=55)
-    - z in [-100, 190] -> up   (gripper closed, J5=-100, Z4=100)
+    - z in [-190, 110] -> up   (gripper closed, J5=-100, Z4=-100)
-    - Overlap [55, 190] -> down (prefer down in intersection)
+    - Overlap [-190, -55] -> down (prefer down in intersection)
     """
-    return z < 55   # True = up, False = down
+    return z > -55   # True = up, False = down
 
 
 def resolve_j6(grip: bool, release: bool, fallback: int) -> int:
@@ -702,7 +734,7 @@ def resolve_j6(grip: bool, release: bool, fallback: int) -> int:
 def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJointState:
     up = args.up if args.up is not None else False
     return ArmJointState(
-        height=-clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
+        height=clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
         j2=clamp_int(args.j2, limits.j2_min, limits.j2_max, "J2"),
         j3=clamp_int(args.j3, limits.j3_min, limits.j3_max, "J3"),
         j4=clamp_int(args.j4, limits.j4_min, limits.j4_max, "J4"),
@@ -714,7 +746,7 @@ def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJoi
 def print_joint_summary(state: ArmJointState) -> None:
     print(
         "UDP command joints:",
-        f"height={-state.height}mm",
+        f"height={state.height}mm",
         f"J2={state.j2}",
         f"J3={state.j3}",
         f"J4={state.j4}",
@@ -728,7 +760,7 @@ def print_pose_summary(pose: ArmPose) -> None:
         "TCP pose:",
         f"x={pose.x:.3f}mm",
         f"y={pose.y:.3f}mm",
-        f"z={-pose.z:.3f}mm",
+        f"z={pose.z:.3f}mm",
         f"phi={pose.phi_deg:.3f}deg",
     )
 
@@ -738,14 +770,14 @@ def print_joint4_center_summary(center: Joint4Center) -> None:
         "J4 center:",
         f"x={center.x:.3f}mm",
         f"y={center.y:.3f}mm",
-        f"z={-center.z:.3f}mm",
+        f"z={center.z:.3f}mm",
     )
 
 
 def print_math_summary(state: ArmMathState) -> None:
     print(
         "Math joints:",
-        f"d1={-state.d1:.3f}mm",
+        f"d1={state.d1:.3f}mm",
         f"J2={state.theta2_deg:.3f}",
         f"J3={state.theta3_deg:.3f}",
         f"J4={state.theta4_deg:.3f}",
@@ -819,7 +851,7 @@ def main() -> int:
             target_pose = ArmPose(
                 x=args.x,
                 y=args.y,
-                z=-args.z,
+                z=args.z,
                 phi_deg=args.phi,
             )
             joint4_center = tcp_to_joint4_center(geometry, target_pose)