Add per-joint limits, joint-space interpolation, grip/release, --up/--down

- Add per-joint angle limits (J2=-110..115, J3=-120..145, J4=-90..130)
  with CLI overrides and validation
- Replace Cartesian interpolation with joint-space interpolation in
  pose mode to guarantee intermediate steps stay within joint limits
- Add --up/--down mutually exclusive groups to both joints and pose
  modes; pose mode auto-detects from z when neither is specified
- Add --grip/--release for J6 control (GRIP/RELEASE_ANGLE placeholders)
- Update cached state to match latest run

tools/.udp_control_state.json

@@ -1,8 +1,8 @@
 {
-  "height": 0,
+  "height": -280,
-  "j2": -90,
+  "j2": -6,
-  "j3": 140,
+  "j3": 122,
-  "j4": 25,
+  "j4": -80,
-  "j5": 81,
+  "j5": -100,
   "j6": 0
 }

tools/udp_control.py

@@ -31,12 +31,22 @@ DEFAULT_HEIGHT_MIN = 0      # 用户坐标系：顶部，单位 mm
 DEFAULT_HEIGHT_MAX = 290    # 用户坐标系：底部，单位 mm
 DEFAULT_JOINT_MIN = -180
 DEFAULT_JOINT_MAX = 180
+DEFAULT_J2_MIN = -110
+DEFAULT_J2_MAX = 115
+DEFAULT_J3_MIN = -120
+DEFAULT_J3_MAX = 145
+DEFAULT_J4_MIN = -90
+DEFAULT_J4_MAX = 130
 J5_OPEN = 81
 J5_CLOSED = -100
 Z4_OPEN = -55
 Z4_CLOSED = 100
 DEFAULT_FIXED_J5 = J5_OPEN
 DEFAULT_FIXED_J6 = 0
+# ==== 抓取/释放（由 J6 控制）====
+# 请填写实际角度值：
+GRIP_ANGLE = -5      # TODO: 填写抓取时 J6 的角度
+RELEASE_ANGLE = 30   # TODO: 填写释放时 J6 的角度
 DEFAULT_ZERO_J2 = 3
 DEFAULT_ZERO_J3 = 7
 DEFAULT_ZERO_J4 = 25
@@ -67,6 +77,12 @@ class ArmLimits:
     height_max: int = DEFAULT_HEIGHT_MAX
     joint_min: int = DEFAULT_JOINT_MIN
     joint_max: int = DEFAULT_JOINT_MAX
+    j2_min: int = DEFAULT_J2_MIN
+    j2_max: int = DEFAULT_J2_MAX
+    j3_min: int = DEFAULT_J3_MIN
+    j3_max: int = DEFAULT_J3_MAX
+    j4_min: int = DEFAULT_J4_MIN
+    j4_max: int = DEFAULT_J4_MAX
 
 
 @dataclass(frozen=True)
@@ -258,20 +274,20 @@ def math_to_command_state(
         ),
         j2=clamp_int(
             math_state.theta2_deg + zero_offsets.j2,
-            limits.joint_min,
+            limits.j2_min,
-            limits.joint_max,
+            limits.j2_max,
             "J2(cmd)",
         ),
         j3=clamp_int(
             math_state.theta3_deg + zero_offsets.j3,
-            limits.joint_min,
+            limits.j3_min,
-            limits.joint_max,
+            limits.j3_max,
             "J3(cmd)",
         ),
         j4=clamp_int(
             math_state.theta4_deg + zero_offsets.j4,
-            limits.joint_min,
+            limits.j4_min,
-            limits.joint_max,
+            limits.j4_max,
             "J4(cmd)",
         ),
         j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"),
@@ -287,9 +303,9 @@ def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None:
         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)"),
-            j2=clamp_int(payload["j2"], limits.joint_min, limits.joint_max, "J2"),
+            j2=clamp_int(payload["j2"], limits.j2_min, limits.j2_max, "J2"),
-            j3=clamp_int(payload["j3"], limits.joint_min, limits.joint_max, "J3"),
+            j3=clamp_int(payload["j3"], limits.j3_min, limits.j3_max, "J3"),
-            j4=clamp_int(payload["j4"], limits.joint_min, limits.joint_max, "J4"),
+            j4=clamp_int(payload["j4"], limits.j4_min, limits.j4_max, "J4"),
             j5=clamp_int(payload.get("j5", DEFAULT_FIXED_J5), limits.joint_min, limits.joint_max, "J5"),
             j6=clamp_int(payload.get("j6", DEFAULT_FIXED_J6), limits.joint_min, limits.joint_max, "J6"),
         )
@@ -486,7 +502,43 @@ def build_parser() -> argparse.ArgumentParser:
         "--joint-max",
         type=int,
         default=DEFAULT_JOINT_MAX,
-        help="关节角上限，默认 180",
+        help="关节角上限（J5/J6），默认 180",
+    )
+    parser.add_argument(
+        "--j2-min",
+        type=int,
+        default=DEFAULT_J2_MIN,
+        help=f"J2 下限，默认 {DEFAULT_J2_MIN}",
+    )
+    parser.add_argument(
+        "--j2-max",
+        type=int,
+        default=DEFAULT_J2_MAX,
+        help=f"J2 上限，默认 {DEFAULT_J2_MAX}",
+    )
+    parser.add_argument(
+        "--j3-min",
+        type=int,
+        default=DEFAULT_J3_MIN,
+        help=f"J3 下限，默认 {DEFAULT_J3_MIN}",
+    )
+    parser.add_argument(
+        "--j3-max",
+        type=int,
+        default=DEFAULT_J3_MAX,
+        help=f"J3 上限，默认 {DEFAULT_J3_MAX}",
+    )
+    parser.add_argument(
+        "--j4-min",
+        type=int,
+        default=DEFAULT_J4_MIN,
+        help=f"J4 下限，默认 {DEFAULT_J4_MIN}",
+    )
+    parser.add_argument(
+        "--j4-max",
+        type=int,
+        default=DEFAULT_J4_MAX,
+        help=f"J4 上限，默认 {DEFAULT_J4_MAX}",
     )
 
     subparsers = parser.add_subparsers(dest="mode", required=True)
@@ -523,7 +575,12 @@ def build_parser() -> argparse.ArgumentParser:
     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 命令值")
-    joints.add_argument("--up", action="store_true", help="夹爪抬起 (J5=-100°)，默认放下 (J5=81°)")
+    joints_grip = joints.add_mutually_exclusive_group()
+    joints_grip.add_argument("--up", action="store_true", dest="up", default=None, help="夹爪抬起 (J5=-100°)")
+    joints_grip.add_argument("--down", action="store_false", dest="up", default=None, help="夹爪放下 (J5=81°)")
+    grip_release = joints.add_mutually_exclusive_group()
+    grip_release.add_argument("--grip", action="store_true", help=f"抓取（J6={GRIP_ANGLE}°，待填写）")
+    grip_release.add_argument("--release", action="store_true", help=f"释放（J6={RELEASE_ANGLE}°，待填写）")
     joints.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="UDP 指令里的 J6 命令值，默认固定 0")
     joints.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})")
     joints.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})")
@@ -571,6 +628,12 @@ def build_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="使用肘部向上分支，默认使用肘部向下分支",
     )
+    pose_grip_release = pose.add_mutually_exclusive_group()
+    pose_grip_release.add_argument("--grip", action="store_true", help=f"抓取（J6={GRIP_ANGLE}°，待填写）")
+    pose_grip_release.add_argument("--release", action="store_true", help=f"释放（J6={RELEASE_ANGLE}°，待填写）")
+    pose_grip = pose.add_mutually_exclusive_group()
+    pose_grip.add_argument("--up", action="store_true", dest="up", default=None, help="夹爪抬起 (J5=-100°)，覆盖 z 自动判断")
+    pose_grip.add_argument("--down", action="store_false", dest="up", default=None, help="夹爪放下 (J5=81°)，覆盖 z 自动判断")
     pose.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="附加发送的 J6 命令值，默认固定 0")
 
     return parser
@@ -590,11 +653,23 @@ def limits_from_args(args: argparse.Namespace) -> ArmLimits:
         raise ArmControlError("height-min 不能大于 height-max。")
     if args.joint_min > args.joint_max:
         raise ArmControlError("joint-min 不能大于 joint-max。")
+    if args.j2_min > args.j2_max:
+        raise ArmControlError("j2-min 不能大于 j2-max。")
+    if args.j3_min > args.j3_max:
+        raise ArmControlError("j3-min 不能大于 j3-max。")
+    if args.j4_min > args.j4_max:
+        raise ArmControlError("j4-min 不能大于 j4-max。")
     return ArmLimits(
         height_min=args.height_min,
         height_max=args.height_max,
         joint_min=args.joint_min,
         joint_max=args.joint_max,
+        j2_min=args.j2_min,
+        j2_max=args.j2_max,
+        j3_min=args.j3_min,
+        j3_max=args.j3_max,
+        j4_min=args.j4_min,
+        j4_max=args.j4_max,
     )
 
 
@@ -616,14 +691,23 @@ def resolve_gripper_from_z(z: float) -> bool:
     return z < 55   # True = up, False = down
 
 
+def resolve_j6(grip: bool, release: bool, fallback: int) -> int:
+    if grip:
+        return GRIP_ANGLE
+    if release:
+        return RELEASE_ANGLE
+    return fallback
+
+
 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)"),
-        j2=clamp_int(args.j2, limits.joint_min, limits.joint_max, "J2"),
+        j2=clamp_int(args.j2, limits.j2_min, limits.j2_max, "J2"),
-        j3=clamp_int(args.j3, limits.joint_min, limits.joint_max, "J3"),
+        j3=clamp_int(args.j3, limits.j3_min, limits.j3_max, "J3"),
-        j4=clamp_int(args.j4, limits.joint_min, limits.joint_max, "J4"),
+        j4=clamp_int(args.j4, limits.j4_min, limits.j4_max, "J4"),
-        j5=clamp_int(resolve_j5(args.up), limits.joint_min, limits.joint_max, "J5"),
+        j5=clamp_int(resolve_j5(up), limits.joint_min, limits.joint_max, "J5"),
-        j6=clamp_int(args.j6, limits.joint_min, limits.joint_max, "J6"),
+        j6=clamp_int(resolve_j6(args.grip, args.release, args.j6), limits.joint_min, limits.joint_max, "J6"),
     )
 
 
@@ -708,7 +792,8 @@ def main() -> int:
             print_math_summary(math_state)
 
             if args.show_fk:
-                z4 = resolve_z4(args.up)
+                up = args.up if args.up is not None else False
+                z4 = resolve_z4(up)
                 start_pose = forward_kinematics(
                     geometry_from_args(args, z4=z4), start_math_state,
                 )
@@ -720,10 +805,14 @@ def main() -> int:
                 print_pose_summary(pose)
 
         elif args.mode == "pose":
-            auto_up = resolve_gripper_from_z(args.z)
+            if args.up is not None:
+                auto_up = args.up
+            else:
+                auto_up = resolve_gripper_from_z(args.z)
             z4 = resolve_z4(auto_up)
             geometry = geometry_from_args(args, z4=z4)
             j5 = resolve_j5(auto_up)
+            j6 = resolve_j6(args.grip, args.release, args.j6)
             start_command_state = resolve_start_command_state(limits, use_state_cache)
             start_math_state = command_to_math_state(start_command_state, zero_offsets)
             start_pose = forward_kinematics(geometry, start_math_state)
@@ -740,26 +829,16 @@ def main() -> int:
                 limits=limits,
                 elbow_up=args.elbow_up,
                 j5=j5,
-                j6=args.j6,
+                j6=j6,
             )
             command_state = math_to_command_state(
                 math_state,
                 zero_offsets,
                 limits,
                 j5=j5,
-                j6=args.j6,
+                j6=j6,
-            )
-            command_path = build_pose_command_path(
-                start_pose=start_pose,
-                target_pose=target_pose,
-                steps=steps,
-                geometry=geometry,
-                limits=limits,
-                zero_offsets=zero_offsets,
-                elbow_up=args.elbow_up,
-                j5=j5,
-                j6=args.j6,
             )
+            command_path = interpolate_command_states(start_command_state, command_state, steps)
             print("Start state source:", "cache/default")
             print_joint_summary(start_command_state)
             print_math_summary(start_math_state)