From 9c4f48a225149ae0be8129b284df8cefed13ea03 Mon Sep 17 00:00:00 2001
From: FallenSigh <fallensigh@gmail.com>
Date: Mon, 15 Jun 2026 21:51:10 +0800
Subject: [PATCH] Auto-select gripper state from z coordinate in pose mode
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Add resolve_gripper_from_z(): z>=55 → down (open), z<55 → up (closed),
  overlap range prefers down
- Remove --up flag from pose subparser (fully auto-detected from --z)
- joints mode retains manual --up flag
- Update cached state to match latest run
---
 tools/.udp_control_state.json | 10 +++++-----
 tools/udp_control.py          | 16 +++++++++++++---
 2 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/tools/.udp_control_state.json b/tools/.udp_control_state.json
index c18083c..4be2ddd 100644
--- a/tools/.udp_control_state.json
+++ b/tools/.udp_control_state.json
@@ -1,8 +1,8 @@
 {
-  "height": -290,
-  "j2": -66,
-  "j3": 145,
-  "j4": -44,
-  "j5": -100,
+  "height": 0,
+  "j2": -90,
+  "j3": 140,
+  "j4": 25,
+  "j5": 81,
   "j6": 0
 }
\ No newline at end of file
diff --git a/tools/udp_control.py b/tools/udp_control.py
index 6665c09..f91a7b4 100644
--- a/tools/udp_control.py
+++ b/tools/udp_control.py
@@ -571,7 +571,6 @@ def build_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="使用肘部向上分支，默认使用肘部向下分支",
     )
-    pose.add_argument("--up", action="store_true", help="夹爪抬起 (J5=-100°)，默认放下 (J5=81°)")
     pose.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="附加发送的 J6 命令值，默认固定 0")
 
     return parser
@@ -607,6 +606,16 @@ def resolve_z4(up: bool) -> float:
     return Z4_CLOSED if up else Z4_OPEN
 
 
+def resolve_gripper_from_z(z: float) -> bool:
+    """Auto-select gripper state based on TCP z coordinate (user mm).
+
+    - z in [55, 345] -> down  (gripper open,  J5=81,  Z4=-55)
+    - z in [-100, 190] -> up   (gripper closed, J5=-100, Z4=100)
+    - Overlap [55, 190] -> down (prefer down in intersection)
+    """
+    return z < 55   # True = up, False = down
+
+
 def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJointState:
     return ArmJointState(
         height=-clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
@@ -711,9 +720,10 @@ def main() -> int:
                 print_pose_summary(pose)
 
         elif args.mode == "pose":
-            z4 = resolve_z4(args.up)
+            auto_up = resolve_gripper_from_z(args.z)
+            z4 = resolve_z4(auto_up)
             geometry = geometry_from_args(args, z4=z4)
-            j5 = resolve_j5(args.up)
+            j5 = resolve_j5(auto_up)
             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)