diff --git a/tools/.udp_control_state.json b/tools/.udp_control_state.json index 4be2ddd..c42a9cb 100644 --- a/tools/.udp_control_state.json +++ b/tools/.udp_control_state.json @@ -1,8 +1,8 @@ { - "height": 0, - "j2": -90, - "j3": 140, - "j4": 25, - "j5": 81, + "height": -280, + "j2": -6, + "j3": 122, + "j4": -80, + "j5": -100, "j6": 0 } \ No newline at end of file diff --git a/tools/udp_control.py b/tools/udp_control.py index f91a7b4..6188f62 100644 --- a/tools/udp_control.py +++ b/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.joint_max, + limits.j2_min, + limits.j2_max, "J2(cmd)", ), j3=clamp_int( math_state.theta3_deg + zero_offsets.j3, - limits.joint_min, - limits.joint_max, + limits.j3_min, + limits.j3_max, "J3(cmd)", ), j4=clamp_int( math_state.theta4_deg + zero_offsets.j4, - limits.joint_min, - limits.joint_max, + limits.j4_min, + 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"), - j3=clamp_int(payload["j3"], limits.joint_min, limits.joint_max, "J3"), - j4=clamp_int(payload["j4"], limits.joint_min, limits.joint_max, "J4"), + j2=clamp_int(payload["j2"], limits.j2_min, limits.j2_max, "J2"), + j3=clamp_int(payload["j3"], limits.j3_min, limits.j3_max, "J3"), + 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"), - j3=clamp_int(args.j3, limits.joint_min, limits.joint_max, "J3"), - j4=clamp_int(args.j4, limits.joint_min, limits.joint_max, "J4"), - j5=clamp_int(resolve_j5(args.up), limits.joint_min, limits.joint_max, "J5"), - j6=clamp_int(args.j6, limits.joint_min, limits.joint_max, "J6"), + j2=clamp_int(args.j2, limits.j2_min, limits.j2_max, "J2"), + j3=clamp_int(args.j3, limits.j3_min, limits.j3_max, "J3"), + j4=clamp_int(args.j4, limits.j4_min, limits.j4_max, "J4"), + j5=clamp_int(resolve_j5(up), limits.joint_min, limits.joint_max, "J5"), + 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, - ) - 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, + j6=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)