Auto-select gripper state from z coordinate in pose mode

- 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
This commit is contained in:
2026-06-15 21:51:10 +08:00
parent 7accfc617e
commit 9c4f48a225
2 changed files with 18 additions and 8 deletions

View File

@@ -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
}

View File

@@ -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)