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:
@@ -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
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user