From f343a01372cda053076182512b0049f6b85faf25 Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Fri, 12 Jun 2026 19:01:48 +0800 Subject: [PATCH] Add arm UDP controller with IK and interpolation --- docs/craic.md | 113 +++ ros2/src/udp_teleop/config/params.yaml | 2 +- .../udp_teleop/udp_teleop/keyboard_control.py | 4 +- tools/.udp_control_state.json | 8 + tools/udp_control.py | 771 ++++++++++++++++++ 5 files changed, 895 insertions(+), 3 deletions(-) create mode 100644 docs/craic.md create mode 100644 tools/.udp_control_state.json create mode 100644 tools/udp_control.py diff --git a/docs/craic.md b/docs/craic.md new file mode 100644 index 0000000..b9febc1 --- /dev/null +++ b/docs/craic.md @@ -0,0 +1,113 @@ +这是一个非常漂亮且实用的自定义机械臂！从你的描述和图片来看，这属于一个**类 SCARA（水平多关节）构型**的机械臂，带有一个直线 Z 轴和三个平面的旋转关节。 + +为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。 + +### 变量与坐标系定义 + +我们将基坐标系原点设在红色滑轨的零点位置。 + +* **$d_1$**: 关节 1（高度滑块）的位置变量。 +* **$\theta_2$**: 关节 2 的旋转角度（相对于基坐标系 X 轴）。 +* **$\theta_3$**: 关节 3 的旋转角度（相对于上一连杆 $L_1$）。 +* **$\theta_4$**: 关节 4 的旋转角度（相对于上一连杆 $L_2$）。 +* **$(X, Y, Z, \Phi)$**: 夹爪末端（TCP - Tool Center Point）在基坐标系下的位姿。其中 $(X, Y, Z)$ 是空间坐标，$\Phi$ 是夹爪在水平面上的总朝向角（偏航角）。 +* **已知常量**: $L_1$（二三关节距离）, $L_2$（三四关节距离）, $x_4$（夹爪相对关节四的 X 偏移）, $z_4$（夹爪相对关节四的高度偏移）。 + +--- + +### 1. 运动学正解 (Forward Kinematics) + +正解的目的是：**已知各个电机的角度和滑块高度 $(d_1, \theta_2, \theta_3, \theta_4)$，求夹爪末端的位置 $(X, Y, Z, \Phi)$。** + +因为 Z 轴的直线运动与 XY 平面的旋转运动是完全解耦的，我们可以分别计算： + +**高度 (Z 轴):** + + +$$Z = d_1 + z_4$$ + +**平面朝向角 (偏航角 $\Phi$):** + + +$$\Phi = \theta_2 + \theta_3 + \theta_4$$ + +**平面坐标 (X, Y):** + + +$$X = L_1 \cos(\theta_2) + L_2 \cos(\theta_2 + \theta_3) + x_4 \cos(\theta_2 + \theta_3 + \theta_4)$$ + +$$Y = L_1 \sin(\theta_2) + L_2 \sin(\theta_2 + \theta_3) + x_4 \sin(\theta_2 + \theta_3 + \theta_4)$$ + +*(注意：在实际编程中，如果你的电机零点不是一条直线，需要在角度上加上相应的初始偏置)* + +--- + +### 2. 运动学逆解 (Inverse Kinematics) + +逆解的目的是：**给出夹爪期望到达的目标位置和朝向 $(X, Y, Z, \Phi)$，求出各关节需要运动到的目标值 $(d_1, \theta_2, \theta_3, \theta_4)$。** + +> **注意：** 你的机械臂在 XY 平面上有 3 个旋转自由度，但平面位置只需要 2 个自由度 $(X,Y)$。这意味着如果只给定目标坐标，机械臂有无数种姿态可以到达（冗余）。因此，**为了得到唯一解，必须同时指定夹爪的最终期望朝向角 $\Phi$**。 + +下面是逆解的推导步骤，非常适合直接转化为固件中的控制代码： + +#### 第一步：求解滑块高度 $d_1$ + +高度依然是解耦的，直接通过目标 $Z$ 坐标和常量偏移计算： + + +$$d_1 = Z - z_4$$ + +#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$ + +既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置： + + +$$X_4 = X - x_4 \cos(\Phi)$$ + +$$Y_4 = Y - x_4 \sin(\Phi)$$ + +#### 第三步：求解关节 3 的角度 $\theta_3$ + +现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。 +根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有： + + +$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$ + +设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。 +如果合法，则 $\theta_3$ 的正弦值为： + + +$$S_3 = \pm \sqrt{1 - C_3^2}$$ + + +*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)* + +最终使用反正切函数求解 $\theta_3$： + + +$$\theta_3 = \text{atan2}(S_3, C_3)$$ + +#### 第四步：求解关节 2 的角度 $\theta_2$ + +利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$： + + +$$\theta_2 = \text{atan2}(Y_4, X_4) - \text{atan2}(L_2 S_3, L_1 + L_2 C_3)$$ + +#### 第五步：求解关节 4 的角度 $\theta_4$ + +因为总朝向 $\Phi = \theta_2 + \theta_3 + \theta_4$，所以： + + +$$\theta_4 = \Phi - \theta_2 - \theta_3$$ + +--- + +### 💡 嵌入式固件实现建议 + +由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议： + +1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。 +2. **死区与奇异点保护**：当 $X_4^2 + Y_4^2 \approx 0$ 时（关节 4 缩回到了原点正上方），此时 $\theta_2$ 会失去意义（奇异点）。在代码中应当加入对 $X_4^2 + Y_4^2 < \epsilon$（一个极小值）的判断，防止产生 NaN。 +3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。 \ No newline at end of file diff --git a/ros2/src/udp_teleop/config/params.yaml b/ros2/src/udp_teleop/config/params.yaml index ddd43ec..a5cf652 100644 --- a/ros2/src/udp_teleop/config/params.yaml +++ b/ros2/src/udp_teleop/config/params.yaml @@ -1,6 +1,6 @@ keyboard_udp_control: ros__parameters: - udp_ip: "127.0.0.1" + udp_ip: "192.168.4.1" udp_port: 8888 chassis_linear_speed: 100 chassis_angular_speed: 45 diff --git a/ros2/src/udp_teleop/udp_teleop/keyboard_control.py b/ros2/src/udp_teleop/udp_teleop/keyboard_control.py index 832cce2..a09b3ac 100644 --- a/ros2/src/udp_teleop/udp_teleop/keyboard_control.py +++ b/ros2/src/udp_teleop/udp_teleop/keyboard_control.py @@ -61,7 +61,7 @@ class KeyboardUdpControlNode(Node): def __init__(self): super().__init__("keyboard_udp_control") - self.declare_parameter("udp_ip", "192.168.233.67") + self.declare_parameter("udp_ip", "192.168.4.1") self.declare_parameter("udp_port", 8888) self.declare_parameter("chassis_linear_speed", 100) self.declare_parameter("chassis_angular_speed", 45) @@ -283,7 +283,7 @@ class KeyboardUdpControlNode(Node): self.arm_height = min(self.arm_height + self.arm_height_step, -10) arm_changed = True if KEY_DOWN in keys: - self.arm_height = max(self.arm_height - self.arm_height_step, -280) + self.arm_height = max(self.arm_height - self.arm_height_step, -285) arm_changed = True joint_index = self.arm_selected_joint diff --git a/tools/.udp_control_state.json b/tools/.udp_control_state.json new file mode 100644 index 0000000..e163950 --- /dev/null +++ b/tools/.udp_control_state.json @@ -0,0 +1,8 @@ +{ + "height": -252, + "j2": 40, + "j3": 64, + "j4": -69, + "j5": 81, + "j6": 0 +} \ No newline at end of file diff --git a/tools/udp_control.py b/tools/udp_control.py new file mode 100644 index 0000000..bb1a937 --- /dev/null +++ b/tools/udp_control.py @@ -0,0 +1,771 @@ +#!/usr/bin/env python3 +"""CRAIC mechanical arm UDP controller. + +Mechanical structure used here: +1. J2/J3/J4 rotate around the Z axis in the XY plane. +2. The gripper TCP has a fixed offset relative to the J4 frame: (x4, 0, z4). +3. ``phi`` is the TCP yaw in the XY plane and is always ``J2 + J3 + J4``. + +Supports two control modes: +1. Direct joint command mode. +2. Cartesian TCP pose mode using the inverse kinematics from ``docs/craic.md``. +""" + +from __future__ import annotations + +import argparse +import json +import math +import socket +import sys +import time +from dataclasses import dataclass +from pathlib import Path + + +DEFAULT_UDP_IP = "192.168.4.1" +DEFAULT_UDP_PORT = 8888 + +DEFAULT_HEIGHT_MIN = -285 +DEFAULT_HEIGHT_MAX = -10 +DEFAULT_JOINT_MIN = -180 +DEFAULT_JOINT_MAX = 180 +DEFAULT_FIXED_J5 = 81 +DEFAULT_FIXED_J6 = 0 +DEFAULT_ZERO_J2 = 3 +DEFAULT_ZERO_J3 = 7 +DEFAULT_ZERO_J4 = 25 +DEFAULT_INTERP_DURATION = 1.0 +DEFAULT_INTERP_RATE = 20.0 +STATE_FILE = Path(__file__).with_name(".udp_control_state.json") + + +class ArmControlError(ValueError): + """Raised when the requested arm pose is invalid.""" + + +@dataclass(frozen=True) +class ArmGeometry: + l1: float + l2: float + x4: float + z4: float + + +@dataclass(frozen=True) +class ArmLimits: + height_min: int = DEFAULT_HEIGHT_MIN + height_max: int = DEFAULT_HEIGHT_MAX + joint_min: int = DEFAULT_JOINT_MIN + joint_max: int = DEFAULT_JOINT_MAX + + +@dataclass(frozen=True) +class ArmZeroOffsets: + j2: int = DEFAULT_ZERO_J2 + j3: int = DEFAULT_ZERO_J3 + j4: int = DEFAULT_ZERO_J4 + + +@dataclass(frozen=True) +class ArmJointState: + height: int + j2: int + j3: int + j4: int + j5: int = DEFAULT_FIXED_J5 + j6: int = DEFAULT_FIXED_J6 + + def to_udp_message(self) -> bytes: + return ( + f"JXB:{self.height}:{self.j2}:{self.j3}:{self.j4}:" + f"{self.j5}:{self.j6}:0:0:EZHY\n" + ).encode("utf-8") + + +@dataclass(frozen=True) +class ArmPose: + x: float + y: float + z: float + phi_deg: float + + +@dataclass(frozen=True) +class ArmMathState: + d1: float + theta2_deg: float + theta3_deg: float + theta4_deg: float + + +@dataclass(frozen=True) +class Joint4Center: + x: float + y: float + z: float + + +def default_command_state() -> ArmJointState: + return ArmJointState( + height=DEFAULT_HEIGHT_MAX, + j2=DEFAULT_ZERO_J2, + j3=DEFAULT_ZERO_J3, + j4=DEFAULT_ZERO_J4, + j5=DEFAULT_FIXED_J5, + j6=DEFAULT_FIXED_J6, + ) + + +def clamp_int(value: float, lower: int, upper: int, name: str) -> int: + rounded = int(round(value)) + if rounded < lower or rounded > upper: + raise ArmControlError( + f"{name}={rounded} 超出范围 [{lower}, {upper}]" + ) + return rounded + + +def normalize_angle_deg(angle_deg: float) -> float: + normalized = (angle_deg + 180.0) % 360.0 - 180.0 + if normalized == -180.0 and angle_deg > 0: + return 180.0 + return normalized + + +def lerp(start: float, end: float, t: float) -> float: + return start + (end - start) * t + + +def lerp_angle_deg(start_deg: float, end_deg: float, t: float) -> float: + delta = normalize_angle_deg(end_deg - start_deg) + return normalize_angle_deg(start_deg + delta * t) + + +def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center: + """Project the TCP target back to the J4 rotation center. + + ``x4`` and ``z4`` only affect position conversion. They do not affect ``phi``. + """ + + phi = math.radians(pose.phi_deg) + return Joint4Center( + x=pose.x - geometry.x4 * math.cos(phi), + y=pose.y - geometry.x4 * math.sin(phi), + z=pose.z - geometry.z4, + ) + + +def forward_kinematics(geometry: ArmGeometry, state: ArmJointState) -> ArmPose: + theta2 = math.radians(state.theta2_deg) + theta3 = math.radians(state.theta3_deg) + theta4 = math.radians(state.theta4_deg) + phi = theta2 + theta3 + theta4 + j4_center_x = ( + geometry.l1 * math.cos(theta2) + + geometry.l2 * math.cos(theta2 + theta3) + ) + j4_center_y = ( + geometry.l1 * math.sin(theta2) + + geometry.l2 * math.sin(theta2 + theta3) + ) + x = j4_center_x + geometry.x4 * math.cos(phi) + y = j4_center_y + geometry.x4 * math.sin(phi) + z = state.d1 + geometry.z4 + return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi)) + + +def inverse_kinematics( + geometry: ArmGeometry, + pose: ArmPose, + limits: ArmLimits, + elbow_up: bool, + j5: int, + j6: int, +) -> ArmJointState: + joint4_center = tcp_to_joint4_center(geometry, pose) + d1 = joint4_center.z + + r2 = joint4_center.x * joint4_center.x + joint4_center.y * joint4_center.y + if r2 < 1e-9: + raise ArmControlError("目标点过于接近奇异点，无法稳定求解 J2。") + + denom = 2.0 * geometry.l1 * geometry.l2 + if abs(denom) < 1e-9: + raise ArmControlError("机械臂几何参数无效：L1 和 L2 不能为 0。") + + c3 = (r2 - geometry.l1 * geometry.l1 - geometry.l2 * geometry.l2) / denom + if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9: + reach = math.sqrt(r2) + raise ArmControlError( + f"目标超出工作空间，关节 4 投影距离为 {reach:.3f}。" + ) + c3 = max(-1.0, min(1.0, c3)) + + s3_abs = math.sqrt(max(0.0, 1.0 - c3 * c3)) + s3 = -s3_abs if elbow_up else s3_abs + + theta3 = math.atan2(s3, c3) + theta2 = math.atan2(joint4_center.y, joint4_center.x) - math.atan2( + geometry.l2 * s3, + geometry.l1 + geometry.l2 * c3, + ) + phi = math.radians(pose.phi_deg) + theta4 = phi - theta2 - theta3 + + return ArmMathState( + d1=d1, + theta2_deg=normalize_angle_deg(math.degrees(theta2)), + theta3_deg=normalize_angle_deg(math.degrees(theta3)), + theta4_deg=normalize_angle_deg(math.degrees(theta4)), + ) + + +def command_to_math_state( + command_state: ArmJointState, + zero_offsets: ArmZeroOffsets, +) -> ArmMathState: + return ArmMathState( + d1=command_state.height, + theta2_deg=command_state.j2 - zero_offsets.j2, + theta3_deg=command_state.j3 - zero_offsets.j3, + theta4_deg=command_state.j4 - zero_offsets.j4, + ) + + +def math_to_command_state( + math_state: ArmMathState, + zero_offsets: ArmZeroOffsets, + limits: ArmLimits, + j5: int, + j6: int, +) -> ArmJointState: + return ArmJointState( + height=clamp_int( + math_state.d1, + limits.height_min, + limits.height_max, + "height(cmd)", + ), + j2=clamp_int( + math_state.theta2_deg + zero_offsets.j2, + limits.joint_min, + limits.joint_max, + "J2(cmd)", + ), + j3=clamp_int( + math_state.theta3_deg + zero_offsets.j3, + limits.joint_min, + limits.joint_max, + "J3(cmd)", + ), + j4=clamp_int( + math_state.theta4_deg + zero_offsets.j4, + limits.joint_min, + limits.joint_max, + "J4(cmd)", + ), + j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"), + j6=clamp_int(j6, limits.joint_min, limits.joint_max, "J6"), + ) + + +def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None: + if not STATE_FILE.exists(): + return 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"), + 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"), + 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"), + ) + except (OSError, json.JSONDecodeError, KeyError, TypeError, ArmControlError): + return None + + +def save_cached_command_state(state: ArmJointState) -> None: + STATE_FILE.write_text( + json.dumps( + { + "height": state.height, + "j2": state.j2, + "j3": state.j3, + "j4": state.j4, + "j5": state.j5, + "j6": state.j6, + }, + ensure_ascii=True, + indent=2, + ), + encoding="utf-8", + ) + + +def resolve_start_command_state( + limits: ArmLimits, + use_state_cache: bool, +) -> ArmJointState: + if use_state_cache: + cached_state = load_cached_command_state(limits) + if cached_state is not None: + return cached_state + return default_command_state() + + +def interpolate_command_states( + start: ArmJointState, + end: ArmJointState, + steps: int, +) -> list[ArmJointState]: + if steps <= 1: + return [end] + + states: list[ArmJointState] = [] + for step_index in range(1, steps + 1): + t = step_index / steps + states.append( + ArmJointState( + height=int(round(lerp(start.height, end.height, t))), + j2=int(round(lerp(start.j2, end.j2, t))), + j3=int(round(lerp(start.j3, end.j3, t))), + j4=int(round(lerp(start.j4, end.j4, t))), + j5=int(round(lerp(start.j5, end.j5, t))), + j6=int(round(lerp(start.j6, end.j6, t))), + ) + ) + return states + + +def interpolate_pose( + start: ArmPose, + end: ArmPose, + t: float, +) -> ArmPose: + return ArmPose( + x=lerp(start.x, end.x, t), + y=lerp(start.y, end.y, t), + z=lerp(start.z, end.z, t), + phi_deg=lerp_angle_deg(start.phi_deg, end.phi_deg, t), + ) + + +def build_pose_command_path( + start_pose: ArmPose, + target_pose: ArmPose, + steps: int, + geometry: ArmGeometry, + limits: ArmLimits, + zero_offsets: ArmZeroOffsets, + elbow_up: bool, + j5: int, + j6: int, +) -> list[ArmJointState]: + if steps <= 1: + math_state = inverse_kinematics( + geometry=geometry, + pose=target_pose, + limits=limits, + elbow_up=elbow_up, + j5=j5, + j6=j6, + ) + return [math_to_command_state(math_state, zero_offsets, limits, j5, j6)] + + path: list[ArmJointState] = [] + for step_index in range(1, steps + 1): + t = step_index / steps + pose = interpolate_pose(start_pose, target_pose, t) + math_state = inverse_kinematics( + geometry=geometry, + pose=pose, + limits=limits, + elbow_up=elbow_up, + j5=j5, + j6=j6, + ) + path.append(math_to_command_state(math_state, zero_offsets, limits, j5, j6)) + return path + + +def compute_interpolation_steps(duration: float, rate: float) -> int: + if duration <= 0.0 or rate <= 0.0: + return 1 + return max(1, int(math.ceil(duration * rate))) + + +def send_udp_commands( + ip: str, + port: int, + states: list[ArmJointState], + dry_run: bool, + duration: float, +) -> None: + if not states: + return + + delay = duration / len(states) if len(states) > 1 and duration > 0.0 else 0.0 + + if dry_run: + for state in states: + print(state.to_udp_message().decode("utf-8").strip()) + return + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + for index, state in enumerate(states): + sock.sendto(state.to_udp_message(), (ip, port)) + if delay > 0.0 and index < len(states) - 1: + time.sleep(delay) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="CRAIC 机械臂 UDP 控制程序" + ) + parser.add_argument("--ip", default=DEFAULT_UDP_IP, help="目标 UDP IP") + parser.add_argument("--port", type=int, default=DEFAULT_UDP_PORT, help="目标 UDP 端口") + parser.add_argument( + "--dry-run", + action="store_true", + help="只打印指令，不实际发送 UDP", + ) + parser.add_argument( + "--show-fk", + action="store_true", + help="输出对应关节角的 TCP 正运动学结果", + ) + parser.add_argument( + "--duration", + type=float, + default=DEFAULT_INTERP_DURATION, + help="插值总时长（秒），默认 1.0；设为 0 则直接发送", + ) + parser.add_argument( + "--rate", + type=float, + default=DEFAULT_INTERP_RATE, + help="插值发送频率（Hz），默认 20", + ) + parser.add_argument( + "--no-state-cache", + action="store_true", + help="不读取或更新上次发送的关节命令缓存", + ) + parser.add_argument( + "--height-min", + type=int, + default=DEFAULT_HEIGHT_MIN, + help="高度下限，默认 -285", + ) + parser.add_argument( + "--height-max", + type=int, + default=DEFAULT_HEIGHT_MAX, + help="UDP 指令高度上限，默认 -10", + ) + parser.add_argument( + "--joint-min", + type=int, + default=DEFAULT_JOINT_MIN, + help="关节角下限，默认 -180", + ) + parser.add_argument( + "--joint-max", + type=int, + default=DEFAULT_JOINT_MAX, + help="关节角上限，默认 180", + ) + + subparsers = parser.add_subparsers(dest="mode", required=True) + + joints = subparsers.add_parser("joints", help="直接发送关节角") + joints.add_argument( + "--dry-run", + action="store_true", + help="只打印指令，不实际发送 UDP", + ) + joints.add_argument( + "--show-fk", + action="store_true", + help="输出对应关节角的 TCP 正运动学结果", + ) + joints.add_argument( + "--duration", + type=float, + default=DEFAULT_INTERP_DURATION, + help="插值总时长（秒），默认 1.0；设为 0 则直接发送", + ) + joints.add_argument( + "--rate", + type=float, + default=DEFAULT_INTERP_RATE, + help="插值发送频率（Hz），默认 20", + ) + joints.add_argument( + "--no-state-cache", + action="store_true", + help="不读取或更新上次发送的关节命令缓存", + ) + joints.add_argument("--height", type=int, required=True, help="UDP 指令里的升降高度命令值") + 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("--j5", type=int, default=DEFAULT_FIXED_J5, help="UDP 指令里的 J5 命令值，默认固定 81") + joints.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="UDP 指令里的 J6 命令值，默认固定 0") + joints.add_argument("--l1", type=float, help="用于 --show-fk 的 L1") + joints.add_argument("--l2", type=float, help="用于 --show-fk 的 L2") + joints.add_argument("--x4", type=float, help="J4 坐标系到 TCP 的 X 偏移，用于 --show-fk") + joints.add_argument("--z4", type=float, help="J4 坐标系到 TCP 的 Z 偏移，用于 --show-fk") + + pose = subparsers.add_parser("pose", help="根据末端位姿逆解后发送") + pose.add_argument( + "--dry-run", + action="store_true", + help="只打印指令，不实际发送 UDP", + ) + pose.add_argument( + "--show-fk", + action="store_true", + help="输出对应关节角的 TCP 正运动学结果", + ) + pose.add_argument( + "--duration", + type=float, + default=DEFAULT_INTERP_DURATION, + help="插值总时长（秒），默认 1.0；设为 0 则直接发送", + ) + pose.add_argument( + "--rate", + type=float, + default=DEFAULT_INTERP_RATE, + help="插值发送频率（Hz），默认 20", + ) + pose.add_argument( + "--no-state-cache", + action="store_true", + help="不读取或更新上次发送的关节命令缓存", + ) + pose.add_argument("--x", type=float, required=True, help="TCP X 坐标") + pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标") + pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标") + pose.add_argument("--phi", type=float, required=True, help="TCP 偏航角，单位度；等于 J2+J3+J4") + pose.add_argument("--l1", type=float, required=True, help="J2 到 J3 的连杆长度 L1") + pose.add_argument("--l2", type=float, required=True, help="J3 到 J4 的连杆长度 L2") + pose.add_argument("--x4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 X 偏移 x4") + pose.add_argument("--z4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 Z 偏移 z4") + pose.add_argument( + "--elbow-up", + action="store_true", + help="使用肘部向上分支，默认使用肘部向下分支", + ) + pose.add_argument("--j5", type=int, default=DEFAULT_FIXED_J5, help="附加发送的 J5 命令值，默认固定 81") + pose.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="附加发送的 J6 命令值，默认固定 0") + + return parser + + +def geometry_from_args(args: argparse.Namespace) -> ArmGeometry: + return ArmGeometry( + l1=float(args.l1), + l2=float(args.l2), + x4=float(args.x4), + z4=float(args.z4), + ) + + +def limits_from_args(args: argparse.Namespace) -> ArmLimits: + if args.height_min > args.height_max: + raise ArmControlError("height-min 不能大于 height-max。") + if args.joint_min > args.joint_max: + raise ArmControlError("joint-min 不能大于 joint-max。") + return ArmLimits( + height_min=args.height_min, + height_max=args.height_max, + joint_min=args.joint_min, + joint_max=args.joint_max, + ) + + +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"), + 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(args.j5, limits.joint_min, limits.joint_max, "J5"), + j6=clamp_int(args.j6, limits.joint_min, limits.joint_max, "J6"), + ) + + +def print_joint_summary(state: ArmJointState) -> None: + print( + "UDP command joints:", + f"height={state.height}", + f"J2={state.j2}", + f"J3={state.j3}", + f"J4={state.j4}", + f"J5={state.j5}", + f"J6={state.j6}", + ) + + +def print_pose_summary(pose: ArmPose) -> None: + print( + "TCP pose:", + f"x={pose.x:.3f}", + f"y={pose.y:.3f}", + f"z={pose.z:.3f}", + f"phi={pose.phi_deg:.3f}deg", + ) + + +def print_joint4_center_summary(center: Joint4Center) -> None: + print( + "J4 center:", + f"x={center.x:.3f}", + f"y={center.y:.3f}", + f"z={center.z:.3f}", + ) + + +def print_math_summary(state: ArmMathState) -> None: + print( + "Math joints:", + f"d1={state.d1:.3f}", + f"J2={state.theta2_deg:.3f}", + f"J3={state.theta3_deg:.3f}", + f"J4={state.theta4_deg:.3f}", + ) + + +def print_interpolation_summary( + duration: float, + rate: float, + steps: int, + use_state_cache: bool, +) -> None: + cache_mode = "on" if use_state_cache else "off" + print( + "Interpolation:", + f"duration={duration:.3f}s", + f"rate={rate:.3f}Hz", + f"steps={steps}", + f"state_cache={cache_mode}", + ) + + +def main() -> int: + parser = build_parser() + args = parser.parse_args() + + try: + limits = limits_from_args(args) + zero_offsets = ArmZeroOffsets() + use_state_cache = not args.no_state_cache + steps = compute_interpolation_steps(args.duration, args.rate) + print_interpolation_summary(args.duration, args.rate, steps, use_state_cache) + + if args.mode == "joints": + start_command_state = resolve_start_command_state(limits, use_state_cache) + command_state = state_from_joint_args(args, limits) + math_state = command_to_math_state(command_state, zero_offsets) + start_math_state = command_to_math_state(start_command_state, zero_offsets) + 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) + print_joint_summary(command_state) + print_math_summary(math_state) + + if args.show_fk: + missing = [ + name for name in ("l1", "l2", "x4", "z4") + if getattr(args, name) is None + ] + if missing: + raise ArmControlError( + "--show-fk 需要同时提供 " + + ", ".join(f"--{name}" for name in missing) + ) + start_pose = forward_kinematics(geometry_from_args(args), start_math_state) + print("Start FK:") + print_pose_summary(start_pose) + pose = forward_kinematics(geometry_from_args(args), math_state) + print_pose_summary(pose) + + elif args.mode == "pose": + geometry = geometry_from_args(args) + 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) + target_pose = ArmPose( + x=args.x, + y=args.y, + z=args.z, + phi_deg=args.phi, + ) + joint4_center = tcp_to_joint4_center(geometry, target_pose) + math_state = inverse_kinematics( + geometry=geometry, + pose=target_pose, + limits=limits, + elbow_up=args.elbow_up, + j5=args.j5, + j6=args.j6, + ) + command_state = math_to_command_state( + math_state, + zero_offsets, + limits, + j5=args.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=args.j5, + j6=args.j6, + ) + print("Start state source:", "cache/default") + print_joint_summary(start_command_state) + print_math_summary(start_math_state) + print("Start FK:") + print_pose_summary(start_pose) + print_pose_summary(target_pose) + print_joint4_center_summary(joint4_center) + print_math_summary(math_state) + print_joint_summary(command_state) + + if args.show_fk: + solved_pose = forward_kinematics(geometry, math_state) + print("Solved FK check:") + print_pose_summary(solved_pose) + + else: + raise ArmControlError(f"未知模式: {args.mode}") + + final_payload = command_state.to_udp_message() + print("Final UDP payload:", final_payload.decode("utf-8").strip()) + send_udp_commands(args.ip, args.port, command_path, args.dry_run, args.duration) + if use_state_cache and not args.dry_run: + save_cached_command_state(command_state) + if not args.dry_run: + print(f"Sent to {args.ip}:{args.port}") + return 0 + + except ArmControlError as exc: + print(f"错误: {exc}", file=sys.stderr) + return 2 + + +if __name__ == "__main__": + raise SystemExit(main())