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,
|
"height": 0,
|
||||||
"j2": -66,
|
"j2": -90,
|
||||||
"j3": 145,
|
"j3": 140,
|
||||||
"j4": -44,
|
"j4": 25,
|
||||||
"j5": -100,
|
"j5": 81,
|
||||||
"j6": 0
|
"j6": 0
|
||||||
}
|
}
|
||||||
@@ -571,7 +571,6 @@ def build_parser() -> argparse.ArgumentParser:
|
|||||||
action="store_true",
|
action="store_true",
|
||||||
help="使用肘部向上分支,默认使用肘部向下分支",
|
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")
|
pose.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="附加发送的 J6 命令值,默认固定 0")
|
||||||
|
|
||||||
return parser
|
return parser
|
||||||
@@ -607,6 +606,16 @@ def resolve_z4(up: bool) -> float:
|
|||||||
return Z4_CLOSED if up else Z4_OPEN
|
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:
|
def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJointState:
|
||||||
return ArmJointState(
|
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)"),
|
||||||
@@ -711,9 +720,10 @@ def main() -> int:
|
|||||||
print_pose_summary(pose)
|
print_pose_summary(pose)
|
||||||
|
|
||||||
elif args.mode == "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)
|
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_command_state = resolve_start_command_state(limits, use_state_cache)
|
||||||
start_math_state = command_to_math_state(start_command_state, zero_offsets)
|
start_math_state = command_to_math_state(start_command_state, zero_offsets)
|
||||||
start_pose = forward_kinematics(geometry, start_math_state)
|
start_pose = forward_kinematics(geometry, start_math_state)
|
||||||
|
|||||||
Reference in New Issue
Block a user