fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add per-joint limits, joint-space interpolation, grip/release, --up/--down

Browse Source 
- Add per-joint angle limits (J2=-110..115, J3=-120..145, J4=-90..130)
  with CLI overrides and validation
- Replace Cartesian interpolation with joint-space interpolation in
  pose mode to guarantee intermediate steps stay within joint limits
- Add --up/--down mutually exclusive groups to both joints and pose
  modes; pose mode auto-detects from z when neither is specified
- Add --grip/--release for J6 control (GRIP/RELEASE_ANGLE placeholders)
- Update cached state to match latest run
This commit is contained in:
FallenSigh
2026-06-15 22:18:02 +08:00
parent 9c4f48a225
commit 87c8b909e9
2 changed files with 115 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

141
tools/udp_control.py
View File

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