From 7144005be2d82300fb136ce015fe5bf04acefa60 Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Tue, 16 Jun 2026 15:45:53 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=B0=86=E6=9C=BA=E6=A2=B0=E8=87=82?= =?UTF-8?q?=E5=9D=90=E6=A0=87=E7=B3=BB=E6=94=B9=E4=B8=BA=20z=20=E8=BD=B4?= =?UTF-8?q?=E6=9C=9D=E4=B8=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 主要修改: - 坐标系统: z=0 为顶部改为底部，z 轴朝上 - 高度范围: d1 ∈ [-290, 0] mm (-290=底部, 0=顶部) - Z4 偏移: 80.0 (原 -80.0), Z4_OPEN=55, Z4_CLOSED=-100 - 运动学: 更新 tcp_to_joint4_center 和 forward_kinematics 的 z 坐标变换 - 夹爪逻辑: 更新 resolve_gripper_from_z 的高度判断范围 - 插值优化: 先完成末端运动，再处理 grip/release - 移除所有坐标转换中的负号，统一为正向 z 轴朝上系统 --- tools/udp_control.py | 88 ++++++++++++++++++++++++++++++-------------- 1 file changed, 60 insertions(+), 28 deletions(-) diff --git a/tools/udp_control.py b/tools/udp_control.py index 6188f62..972de9e 100644 --- a/tools/udp_control.py +++ b/tools/udp_control.py @@ -27,8 +27,8 @@ from pathlib import Path DEFAULT_UDP_IP = "192.168.4.1" DEFAULT_UDP_PORT = 8888 -DEFAULT_HEIGHT_MIN = 0 # 用户坐标系：顶部，单位 mm -DEFAULT_HEIGHT_MAX = 290 # 用户坐标系：底部，单位 mm +DEFAULT_HEIGHT_MIN = -290 # 内部 d1 坐标：底部，单位 mm（z 轴朝上） +DEFAULT_HEIGHT_MAX = 0 # 内部 d1 坐标：顶部，单位 mm（z 轴朝上） DEFAULT_JOINT_MIN = -180 DEFAULT_JOINT_MAX = 180 DEFAULT_J2_MIN = -110 @@ -39,21 +39,21 @@ DEFAULT_J4_MIN = -90 DEFAULT_J4_MAX = 130 J5_OPEN = 81 J5_CLOSED = -100 -Z4_OPEN = -55 -Z4_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_FIXED_J6 = RELEASE_ANGLE DEFAULT_ZERO_J2 = 3 DEFAULT_ZERO_J3 = 7 DEFAULT_ZERO_J4 = 25 DEFAULT_L1 = 125.0 DEFAULT_L2 = 125.0 DEFAULT_X4 = 110.0 -DEFAULT_Z4 = -80.0 +DEFAULT_Z4 = 80.0 DEFAULT_INTERP_DURATION = 1.0 DEFAULT_INTERP_RATE = 20.0 STATE_FILE = Path(__file__).with_name(".udp_control_state.json") @@ -133,7 +133,7 @@ class Joint4Center: def default_command_state() -> ArmJointState: return ArmJointState( - height=-DEFAULT_HEIGHT_MIN, + height=-DEFAULT_HEIGHT_MAX, j2=DEFAULT_ZERO_J2, j3=DEFAULT_ZERO_J3, j4=DEFAULT_ZERO_J4, @@ -177,7 +177,7 @@ def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center: return Joint4Center( x=pose.x - geometry.x4 * math.cos(phi), y=pose.y - geometry.x4 * math.sin(phi), - z=pose.z - geometry.z4, + z=pose.z + geometry.z4, ) @@ -196,7 +196,7 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose: ) x = j4_center_x + geometry.x4 * math.cos(phi) y = j4_center_y + geometry.x4 * math.sin(phi) - z = state.d1 + geometry.z4 + z = state.d1 - geometry.z4 return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi)) @@ -266,8 +266,8 @@ def math_to_command_state( j6: int, ) -> ArmJointState: return ArmJointState( - height=-clamp_int( - round(-math_state.d1), + height=clamp_int( + round(math_state.d1), limits.height_min, limits.height_max, "height(cmd)", @@ -302,7 +302,7 @@ def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None: try: 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)"), + height=clamp_int(payload["height"], limits.height_min, limits.height_max, "height(cmd)"), 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"), @@ -401,7 +401,11 @@ def build_pose_command_path( ) return [math_to_command_state(math_state, zero_offsets, limits, j5, j6)] + # Interpolate pose first (movement) path: list[ArmJointState] = [] + start_command_j5 = None + start_command_j6 = None + for step_index in range(1, steps + 1): t = step_index / steps pose = interpolate_pose(start_pose, target_pose, t) @@ -413,7 +417,35 @@ def build_pose_command_path( j5=j5, j6=j6, ) - path.append(math_to_command_state(math_state, zero_offsets, limits, j5, j6)) + command = math_to_command_state(math_state, zero_offsets, limits, j5, j6) + + # Store the first command's j5/j6 values + if start_command_j5 is None: + start_command_j5 = command.j5 + start_command_j6 = command.j6 + + # For all movement steps, use the starting j5/j6 values + path.append(ArmJointState( + height=command.height, + j2=command.j2, + j3=command.j3, + j4=command.j4, + j5=start_command_j5, + j6=start_command_j6, + )) + + # Add final grip/release adjustment if j5 or j6 changed + final = path[-1] + if final.j5 != j5 or final.j6 != j6: + path.append(ArmJointState( + height=final.height, + j2=final.j2, + j3=final.j3, + j4=final.j4, + j5=j5, + j6=j6, + )) + return path @@ -484,13 +516,13 @@ def build_parser() -> argparse.ArgumentParser: "--height-min", type=int, default=DEFAULT_HEIGHT_MIN, - help=f"高度下限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}", + help=f"高度下限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}", ) parser.add_argument( "--height-max", type=int, default=DEFAULT_HEIGHT_MAX, - help=f"高度上限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}", + help=f"高度上限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}", ) parser.add_argument( "--joint-min", @@ -571,7 +603,7 @@ def build_parser() -> argparse.ArgumentParser: action="store_true", help="不读取或更新上次发送的关节命令缓存", ) - joints.add_argument("--height", type=int, required=True, help="升降高度 mm (0=顶部, 290=底部)") + joints.add_argument("--height", type=int, required=True, help="内部 d1 坐标 mm (-290=底部, 0=顶部，z 轴朝上)") 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 命令值") @@ -617,7 +649,7 @@ def build_parser() -> argparse.ArgumentParser: ) pose.add_argument("--x", type=float, required=True, help="TCP X 坐标 mm") pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标 mm") - pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (0=顶部, 正值向下)") + pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (-290=底部, 0=顶部, z 轴朝上)") pose.add_argument("--phi", type=float, required=True, help="TCP 偏航角，单位度；等于 J2+J3+J4") pose.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})") pose.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})") @@ -682,13 +714,13 @@ def resolve_z4(up: bool) -> float: def resolve_gripper_from_z(z: float) -> bool: - """Auto-select gripper state based on TCP z coordinate (user mm). + """Auto-select gripper state based on TCP z coordinate (user mm, z-up). - - 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) + - z in [-345, -55] -> down (gripper open, J5=81, Z4=55) + - z in [-190, 110] -> up (gripper closed, J5=-100, Z4=-100) + - Overlap [-190, -55] -> down (prefer down in intersection) """ - return z < 55 # True = up, False = down + return z > -55 # True = up, False = down def resolve_j6(grip: bool, release: bool, fallback: int) -> int: @@ -702,7 +734,7 @@ def resolve_j6(grip: bool, release: bool, fallback: int) -> int: 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)"), + height=clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"), 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"), @@ -714,7 +746,7 @@ def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJoi def print_joint_summary(state: ArmJointState) -> None: print( "UDP command joints:", - f"height={-state.height}mm", + f"height={state.height}mm", f"J2={state.j2}", f"J3={state.j3}", f"J4={state.j4}", @@ -728,7 +760,7 @@ def print_pose_summary(pose: ArmPose) -> None: "TCP pose:", f"x={pose.x:.3f}mm", f"y={pose.y:.3f}mm", - f"z={-pose.z:.3f}mm", + f"z={pose.z:.3f}mm", f"phi={pose.phi_deg:.3f}deg", ) @@ -738,14 +770,14 @@ def print_joint4_center_summary(center: Joint4Center) -> None: "J4 center:", f"x={center.x:.3f}mm", f"y={center.y:.3f}mm", - f"z={-center.z:.3f}mm", + f"z={center.z:.3f}mm", ) def print_math_summary(state: ArmMathState) -> None: print( "Math joints:", - f"d1={-state.d1:.3f}mm", + f"d1={state.d1:.3f}mm", f"J2={state.theta2_deg:.3f}", f"J3={state.theta3_deg:.3f}", f"J4={state.theta4_deg:.3f}", @@ -819,7 +851,7 @@ def main() -> int: target_pose = ArmPose( x=args.x, y=args.y, - z=-args.z, + z=args.z, phi_deg=args.phi, ) joint4_center = tcp_to_joint4_center(geometry, target_pose)