diff --git a/README.md b/README.md index d01579e..8b54bdc 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,25 @@ # litearm-python Python SDK for the LiteArm robotic arm: drive the arm from a PC over one USB serial cable, -straight to its firmware — no server or middleware in between. Trajectory planning, kinematics -and dynamics are all carried by the firmware. +straight to its firmware — no server or middleware in between. The firmware owns real-time +control and S-curves; the optional SDK model computes local FK/IK/Jacobians and guarded PTPs. > Full interface reference: [docs/DEVELOPER_GUIDE.md](https://github.com/nexform-tech/litearm-python/blob/main/docs/DEVELOPER_GUIDE.md). ## Features - **Pure Python**: Python 3.9+, poses are plain lists / tuples, no numpy needed -- **A single dependency**: `pyserial`, nothing else +- **Lightweight by default**: the normal serial install depends only on `pyserial` - **Direct over USB**: one cable to the firmware, no server in between -- **The firmware does the planning**: kinematics and dynamics live there; the PC sends points and decides arrival +- **Optional local kinematics**: Pinocchio uses the packaged URDF for FK/IK/Jacobians +- **Execution stays in firmware**: STM32 enforces velocity, acceleration, jerk and synchronized S-curves - **Full motion API**: joint moves, Cartesian lines / arcs / waypoints, hand-guiding - **Tool point (TCP) offset**: a host-side xyz + rpy layer, so `move_*` and `ik()` take tool poses while `get_tcp()` keeps returning the flange - **Payload identification**: measure the mass and centre of mass of the end-of-arm load from the arm's own torque, then hand the result to `set_payload()` +- **Optional self-collision checks**: Pinocchio/hpp-fcl checks the packaged URDF meshes before + a predictable joint path is sent; the normal serial SDK stays dependency-light - **Safety built in**: fail-closed in child processes, a dedicated emergency stop, and every irreversible command flagged ## Install @@ -24,7 +27,7 @@ and dynamics are all carried by the firmware. | Item | Requirement | | --- | --- | | Python | 3.9 or later | -| Dependencies | `pyserial >= 3.4` (the only one) | +| Default dependency | `pyserial >= 3.4` (`robotics` extras are optional) | | Firmware | `Litearm1.5.0` or later | | Connection | USB CDC serial, VID:PID `1d50:606f` | @@ -35,6 +38,47 @@ pip install litearm-python python3 -c "import litearm; print(litearm.__version__)" # verify ``` +Local kinematics and self-collision checking are an optional, heavier install: + +```bash +pip install "litearm-python[robotics]" +``` + +If the shell has sourced ROS, its `PYTHONPATH` and `LD_LIBRARY_PATH` can inject +a Pinocchio build for another Python ABI. On `pinocchio_pywrap_default` or +`undefined symbol` errors, run with +`env -u PYTHONPATH -u LD_LIBRARY_PATH .venv/bin/python your_script.py`. + +`CollisionModel()` automatically loads the wheel's URDF and nine collision meshes. Its +`in_collision(q)` method returns a boolean; `colliding_pairs(q)` returns all colliding geometry +name pairs; `minimum_distance(q)` returns the nearest non-adjacent-link clearance. All require +exactly seven finite joint angles. Each checker owns its mutable +Pinocchio data and serializes calls on that instance, so it can be shared across threads. + +Use it for planning/preflight work, not a real-time loop. For path checking, sample every +configuration along the path. In particular, `movej_sync` follows the joint-space line and can +be checked that way; ordinary `movej` drives independent per-axis S-curves, so checking only its +endpoints does not prove the unknown intermediate path collision-free. + +```text +collision = pa.CollisionModel() +collision.in_collision([0.0] * 7) +collision.colliding_pairs(q) +``` + +For VR/servo control, the exact differential-IK backend from `lite-vr` is also +available without changing the firmware pose solver `Arm.ik()`: + +```bash +pip install "litearm-python[ik]" +``` + +Call `litearm.exact_ik.exact_task_velocity(J, twist, lower, upper)` with a +Jacobian and velocity/range box. It either delivers the complete twist or +returns zero velocity with a structured failure reason. Use +`bounded_task_velocity(...)` when partial, damped motion is preferable to +holding position. The Jacobian and twist must use the same reference frame. + Or install from a checkout: ```bash @@ -219,9 +263,29 @@ state = arm.get_state().value print(state.q, state.dq, state.tau) # joint angles / velocity / torque print(state.enabled, state.faulted) # enabled? faulted? print(arm.get_tcp().value) # flange pose (firmware forward kinematics) -print(arm.ik((0.30, 0.0, 0.35, 3.1416, 0.0, 0.0))) # pose → joint angles +print(arm.ik((0.30, 0.0, 0.35, 3.1416, 0.0, 0.0))) # firmware remote IK + +q = state.q +print(arm.kinematics.fk(q, frame="flange")) # SDK-local FK, no command sent +print(arm.kinematics.jacobian(q).sigma_min) # SDK-local Jacobian +q_seed = [0.0, -0.5, 0.0, -1.0, 0.0, 0.5, 0.0] +target_pose = arm.kinematics.fk(q_seed, frame="flange") +solution = arm.kinematics.ik(target_pose, q_seed=q_seed, frame="flange") +# arm.kinematics.move_pose(target_pose, speed=0.2) # preflight, then one 0x07 ``` +`arm.kinematics.move_pose()` seeds from fresh state, reads the STM32 runtime soft limits, and +samples the joint line at no more than 0.02 rad to check limit margin, self-collision/5 mm +clearance and `sigma_min >= 0.05`. If the start moves during preflight it replans once, then +fails closed on a second drift. A passing move sends exactly one `movej_sync`; STM32 still owns +velocity, acceleration and jerk limits. This checks **self-collision only**—not benches, people, +or external obstacles. + +The IK residual is numerical convergence, not guaranteed physical TCP accuracy. `move_pose()` +waits with the existing joint-space `q_tol` / `dq_tol` criteria, so tracking, encoder and calibration +errors still apply. Evaluate `arm.kinematics.fk(returned_state.q)` when the measured endpoint matters; +hardware review saw about 0.7-1.1 mm error on 2-10 mm test moves (informative, not a specification). + ### Tool point (TCP) offset The firmware's TCP *is* the flange. To control a tool point instead, declare the offset once — @@ -488,6 +552,7 @@ See [examples/README.md](https://github.com/nexform-tech/litearm-python/blob/mai - `02_movej.py` — joint motion - `03_move_p.py` — Cartesian point to point - `04_ik_tcp.py` — inverse kinematics and the current pose +- `10_local_kinematics.py` — SDK-local FK/IK/Jacobian and guarded pose PTP - `06_cartesian.py` — Cartesian lines / arcs / waypoints - `07_vel_jitter_trace.py` — 300 Hz per-tick capture diff --git a/README.zh-CN.md b/README.zh-CN.md index b69128b..8da393b 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -1,18 +1,22 @@ # litearm-python LiteArm 机械臂的 Python SDK:从 PC 经一根 USB 串口线直连固件驱动机械臂 —— -不需要服务端或中间件。轨迹规划、运动学、动力学全部由固件承担。 +不需要服务端或中间件。固件负责实时控制和 S 曲线;SDK 可选在本地计算 +FK/IK/Jacobian,并在安全预检后下发同步关节 PTP。 > 完整接口说明见 [docs/DEVELOPER_GUIDE.zh-CN.md](docs/DEVELOPER_GUIDE.zh-CN.md)。 ## 特性 - **纯 Python**:Python 3.9+,位姿就是普通的 list / tuple,不需要 numpy -- **单一依赖**:只有 `pyserial` +- **默认轻依赖**:普通串口安装只有 `pyserial` - **USB 直连**:一根线接到固件,不经过任何服务端 -- **规划在固件里**:运动学、动力学都在固件里,PC 侧只发点、判到位 +- **本地运动学可选**:Pinocchio 使用随包 URDF 算 FK/IK/Jacobian;默认串口安装不变 +- **执行仍在固件里**:速度、加速度、jerk 限幅和同步 S 曲线由 STM32 完成 - **完整运动接口**:关节运动、笛卡尔直线 / 圆弧 / 多路点、拖动示教 - **负载辨识**:从臂自身的力矩里量出末端负载的质量与质心,结果直接交给 `set_payload()` +- **可选自碰检测**:Pinocchio/hpp-fcl 用随包 URDF 网格做规划前检查;默认串口 SDK + 仍保持轻依赖 - **安全兜底**:子进程 fail-closed,急停独立入口,不可逆命令逐个标注 ## 安装 @@ -20,7 +24,7 @@ LiteArm 机械臂的 Python SDK:从 PC 经一根 USB 串口线直连固件驱 | 项目 | 要求 | | --- | --- | | Python | 3.9 及以上 | -| 依赖 | `pyserial >= 3.4`(唯一依赖) | +| 默认依赖 | `pyserial >= 3.4`(`robotics` 为可选重依赖) | | 固件 | `Litearm1.5.0` 及以上 | | 连接 | USB CDC 串口,VID:PID `1d50:606f` | @@ -31,6 +35,44 @@ pip install -e . python3 -c "import litearm; print(litearm.__version__)" # 验证 ``` +本地运动学与碰撞检测是可选的重依赖能力,按需安装: + +```bash +pip install -e ".[robotics]" +``` + +若当前终端 source 过 ROS,ROS 的 `PYTHONPATH` / `LD_LIBRARY_PATH` 可能把另一 Python +版本的 Pinocchio 动态库注入虚拟环境。遇到 `pinocchio_pywrap_default` 缺失或 +`undefined symbol` 时,用 +`env -u PYTHONPATH -u LD_LIBRARY_PATH .venv/bin/python your_script.py` 运行。 + +`CollisionModel()` 会自动加载 wheel 内的 URDF 与 9 个碰撞网格。`in_collision(q)` 返回 +是否自碰,`colliding_pairs(q)` 返回全部碰撞几何名称对,`minimum_distance(q)` 返回非相邻 +连杆的最小间隙;三者都严格要求 7 个有限关节角。 +每个 checker 独占 Pinocchio 的可变模型数据,同一实例的查询会串行化,因此可跨线程共享。 + +它只用于规划/下发前检查,不应进入实时控制环。路径检测必须沿路径采样每个构型: +`movej_sync` 的中间轨迹是关节空间直线,可以据此检查;普通 `movej` 各轴使用独立 S 曲线, +中间路径不可预测,只检查起终点不能证明整段无碰撞。 + +```text +collision = pa.CollisionModel() +collision.in_collision([0.0] * 7) +collision.colliding_pairs(q) +``` + +VR/伺服控制所需的 `lite-vr` 精确微分逆解也已独立提供,不会覆盖固件位姿逆解 +`Arm.ik()`: + +```bash +pip install -e ".[ik]" +``` + +向 `litearm.exact_ik.exact_task_velocity(J, twist, lower, upper)` 传雅可比、任务速度和 +关节速度/行程盒;它要么完整交付 twist,要么返回零速度及结构化失败原因。不可完整交付时 +若希望继续做阻尼降级跟随,可调用 `bounded_task_velocity(...)`。雅可比与 twist 必须采用 +同一参考坐标系。 + Linux 需要串口权限 —— 先看设备节点本身怎么说: ```bash @@ -195,9 +237,28 @@ state = arm.get_state().value print(state.q, state.dq, state.tau) # 关节角 / 速度 / 力矩 print(state.enabled, state.faulted) # 使能中?有故障? print(arm.get_tcp().value) # 法兰位姿(固件正运动学) -print(arm.ik((0.30, 0.0, 0.35, 3.1416, 0.0, 0.0))) # 位姿 → 关节角 +print(arm.ik((0.30, 0.0, 0.35, 3.1416, 0.0, 0.0))) # 固件远程 IK + +q = state.q +print(arm.kinematics.fk(q, frame="flange")) # SDK 本地 FK,不发命令 +print(arm.kinematics.jacobian(q).sigma_min) # SDK 本地 Jacobian +q_seed = [0.0, -0.5, 0.0, -1.0, 0.0, 0.5, 0.0] +target_pose = arm.kinematics.fk(q_seed, frame="flange") +solution = arm.kinematics.ik(target_pose, q_seed=q_seed, frame="flange") +# arm.kinematics.move_pose(target_pose, speed=0.2) # 预检后仅发一条 0x07 ``` +`arm.kinematics.move_pose()` 使用当前状态为种子、读取 STM32 运行时软限位,并对实际将执行的 +关节直线按不大于 0.02 rad 的步长检查限位余量、自碰/5 mm 间隙和 +`sigma_min >= 0.05`。预检期间起点变化会重算一次,再变化则拒绝。通过后只调用一次 +`movej_sync`;STM32 继续负责速度、加速度和 jerk 限制。该检查只覆盖机械臂自碰,**不检测 +工作台、人员和外部障碍物**。 + +IK 残差表示数值求解收敛,不等于真机 TCP 到位精度。`move_pose()` 仍按现有的关节空间 +`q_tol` / `dq_tol` 判据等待,最终误差还包含跟踪、编码器和标定误差;对精度敏感时请对返回的 +`RobotState.q` 再调用 `arm.kinematics.fk()`。本次真机 review 的 2–10 mm 小步运动误差约 +0.7–1.1 mm,该数据仅供参考,不是规格承诺。 + ### 工具点(TCP)偏移 固件的 TCP 就是法兰。想控工具尖就声明一次偏移 —— 之后 `move_p` / `move_l` / `move_c` / @@ -444,6 +505,7 @@ p.start() - `02_movej.py` — 关节运动 - `03_move_p.py` — 笛卡尔点到点 - `04_ik_tcp.py` — 逆解与当前位姿 +- `10_local_kinematics.py` — SDK 本地 FK/IK/Jacobian 与安全位姿 PTP - `06_cartesian.py` — 笛卡尔直线 / 圆弧 / 多路点 - `07_vel_jitter_trace.py` — 300 Hz 逐拍采集 diff --git a/docs/DEVELOPER_GUIDE.md b/docs/DEVELOPER_GUIDE.md index a16ee37..6c76fdc 100644 --- a/docs/DEVELOPER_GUIDE.md +++ b/docs/DEVELOPER_GUIDE.md @@ -2,9 +2,9 @@ Python SDK for the LiteArm robotic arm, talking straight to the firmware over USB serial. -Planning, kinematics and dynamics live in the firmware; the PC side only encodes frames, sends -commands and decides arrival. A pose is 6 numbers (list or tuple), and `pyserial` is the only -dependency. +Real-time control, S-curves and dynamics live in firmware. The optional SDK model adds local +FK/IK/Jacobians and guarded joint PTP using the packaged URDF. A pose is 6 numbers (list or +tuple), and the default install still depends only on `pyserial`. ## Contents @@ -30,6 +30,7 @@ dependency. ```bash pip install -e . # install +pip install -e ".[robotics]" # local kinematics, self-collision and clearance checks ``` When `pip` and `python` point at different interpreters, use `python -m pip`. @@ -299,6 +300,10 @@ Failures raise `CartesianPlanError` and reject the whole move — the arm does n get_tcp(timeout=0.6) -> Msg[Optional[tuple]] get_tool_tcp(timeout=0.6) -> Msg[Optional[tuple]] ik(pose, q_seed=None, timeout=3.0) -> list[float] +kinematics.fk(q, frame="tool") -> tuple[6] +kinematics.jacobian(q, frame="tool") -> JacobianResult +kinematics.ik(pose, q_seed=None, frame="tool") -> IKSolution +kinematics.move_pose(pose, speed=0.2, frame="tool") -> RobotState set_tcp_offset(xyz=(0.0, 0.0, 0.0), rpy=(0.0, 0.0, 0.0), *, payload_mass=None, payload_com=None) clear_tcp_offset() tcp_offset @@ -312,10 +317,26 @@ load_tool(path=None) -> bool | `get_tcp()` | The **flange** (`ee_link`) pose from firmware forward kinematics, 6 numbers. Never affected by `set_tcp_offset()` | | `get_tool_tcp()` | The same pose with the tool offset applied — where the tool point actually is | | `ik(pose, q_seed=None)` | Inverse kinematics. It may return another equally valid branch, so a result far from `q_seed` is not necessarily an error | +| `kinematics.fk/ik/jacobian` | SDK-local calculations using the packaged URDF; no firmware IK/FK command is sent. `frame="tool"` applies the current TCP offset | +| `kinematics.move_pose()` | Local continuation IK plus full joint-line preflight, then exactly one `CMD_MOVE_J_SYNC 0x07` | | `set_tcp_offset(xyz, rpy)` | Declares where the tool point sits. See below: this is a host-side layer, not a firmware feature | -There is no `fk(q)`. The PC side carries no kinematic model, so forward kinematics is only -`get_tcp()`. +`Arm.ik()` and `get_tcp()` remain firmware-remote APIs. `arm.kinematics.*` explicitly selects +the SDK-local model, while `litearm.exact_ik.exact_task_velocity()` is differential velocity IK. + +`move_pose()` requires an enabled, fault-free arm outside zero-gravity and Cartesian planning. +It reads the STM32 runtime soft limits and samples the actual joint line at at most 0.02 rad, +checking a 0.01 rad limit margin, self-collision/non-adjacent-link 5 mm clearance, and +`sigma_min >= 0.05`. It replans once if the start drifts over 0.03 rad and rejects a second +drift. Non-finite values, model errors, and unavailable distances all fail before a motion frame +is sent. This protects against **self-collision only**, not benches, people, or external obstacles. + +The IK position/orientation residuals report numerical solver convergence; they are not a promise +of physical arrival accuracy. `move_pose()` ultimately waits with the same joint-space criteria as +`movej_sync` (`q_tol`, `dq_tol`, and `arrive_frames`), so encoder, tracking, calibration, and firmware +arrival tolerance remain in the final TCP error. Use `kinematics.fk(returned_state.q)` to evaluate +the measured final pose when accuracy matters. Hardware review observed roughly 0.7-1.1 mm TCP +error for 2-10 mm test moves; that measurement is informative, not a universal specification. #### Tool point (TCP) offset diff --git a/docs/DEVELOPER_GUIDE.zh-CN.md b/docs/DEVELOPER_GUIDE.zh-CN.md index c58cc7c..f6afea1 100644 --- a/docs/DEVELOPER_GUIDE.zh-CN.md +++ b/docs/DEVELOPER_GUIDE.zh-CN.md @@ -2,8 +2,9 @@ LiteArm 机械臂的 Python SDK,USB 串口直连固件。 -规划、运动学、动力学都在固件里,PC 侧只编解码帧、下发命令、判到位。 -位姿是 6 个数(list 或 tuple),只依赖 `pyserial`。 +实时控制、S 曲线和动力学在固件里;SDK 还可选提供基于随包 URDF 的本地 +FK/IK/Jacobian 与安全关节 PTP。位姿是 6 个数(list 或 tuple),默认安装只依赖 +`pyserial`。 ## 目录 @@ -23,12 +24,13 @@ LiteArm 机械臂的 Python SDK,USB 串口直连固件。 ## 1. 环境要求与安装 - Python **3.9 及以上** -- `pyserial >= 3.4`(唯一依赖) +- `pyserial >= 3.4`(默认安装唯一依赖) - 固件 **`Litearm1.5.0` 及以上**,版本串为 `Litearm<主.次.修>-{7J|1J}` - Linux 需串口权限:`sudo usermod -aG dialout $USER`(重新登录生效) ```bash pip install -e . # 安装 +pip install -e ".[robotics]" # 本地运动学、自碰与间隙检查 ``` `pip` 与 `python` 指向不同解释器时用 `python -m pip`。 @@ -290,6 +292,10 @@ set_speed(percent) get_tcp(timeout=0.6) -> Msg[Optional[tuple]] get_tool_tcp(timeout=0.6) -> Msg[Optional[tuple]] ik(pose, q_seed=None, timeout=3.0) -> list[float] +kinematics.fk(q, frame="tool") -> tuple[6] +kinematics.jacobian(q, frame="tool") -> JacobianResult +kinematics.ik(pose, q_seed=None, frame="tool") -> IKSolution +kinematics.move_pose(pose, speed=0.2, frame="tool") -> RobotState set_tcp_offset(xyz=(0.0, 0.0, 0.0), rpy=(0.0, 0.0, 0.0), *, payload_mass=None, payload_com=None) clear_tcp_offset() tcp_offset @@ -303,9 +309,24 @@ load_tool(path=None) -> bool | `get_tcp()` | **法兰**(`ee_link`)位姿,来自固件正运动学,6 个数。**不受 `set_tcp_offset()` 影响** | | `get_tool_tcp()` | 同一姿态套上工具偏移 —— 工具点此刻在哪 | | `ik(pose, q_seed=None)` | 反解。可能返回另一个同样有效的分支,与 `q_seed` 差得远未必是错误 | +| `kinematics.fk/ik/jacobian` | 使用随包 URDF 在 SDK 本地计算,不发送固件 IK/FK 命令;`frame="tool"` 应用当前 TCP offset | +| `kinematics.move_pose()` | 本地连续 IK + 完整关节直线预检,通过后只发一次 `CMD_MOVE_J_SYNC 0x07` | | `set_tcp_offset(xyz, rpy)` | 声明工具点位置。这是**宿主侧**一层换算,不是固件能力 —— 见下 | -没有 `fk(q)`。PC 侧不带运动学模型,正运动学只有 `get_tcp()` 这一条路。 +`Arm.ik()` 与 `get_tcp()` 保持为固件远程接口;`arm.kinematics.*` 明确表示 SDK 本地模型, +`litearm.exact_ik.exact_task_velocity()` 则是速度级微分 IK,三者不要混用。 + +`move_pose()` 要求设备已使能、无故障、非零重力且没有外部笛卡尔规划在途。它每次读取 +STM32 运行时软限位,沿实际执行的 `q(s)=q0+s(q1-q0)` 以最多 0.02 rad 步长检查 0.01 rad +限位余量、非相邻连杆 5 mm 间隙、自碰和 `sigma_min >= 0.05`。预检后起点漂移超过 +0.03 rad 会重算一次,再漂移即拒绝。任何非有限值、模型异常或距离不可得都在发运动帧前 +失败。这里只能保证自碰安全,**不能检测工作台、人员或外部障碍物**。 + +IK 返回的位置/姿态残差表示数值求解是否收敛,**不是机械臂物理到位精度承诺**。 +`move_pose()` 最终仍按 `movej_sync` 的关节空间判据(`q_tol`、`dq_tol`、`arrive_frames`) +等待,因此最终 TCP 误差还包含编码器、跟踪、标定和固件到位容差。对精度敏感时,应对返回的 +`RobotState.q` 再调用 `kinematics.fk()` 评估实测终点。本次真机 review 的 2–10 mm 小步运动 +测得约 0.7–1.1 mm TCP 误差;该结果仅供参考,不是所有机械臂的统一规格。 #### 工具点(TCP)偏移 diff --git a/examples/10_local_kinematics.py b/examples/10_local_kinematics.py new file mode 100644 index 0000000..89f6642 --- /dev/null +++ b/examples/10_local_kinematics.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""样例 10 · SDK 本地 FK/IK/Jacobian 与安全位姿 PTP。 + +默认只读:读取当前 q,在 SDK 本地计算 FK、Jacobian 和同位姿 IK,不发送固件 IK。 +加 --go 后,把本地工具位姿沿世界 x 方向移动 --dx(默认 5 mm),完整预检通过后只发一条 +CMD_MOVE_J_SYNC;STM32 负责同步 S 曲线、速度、加速度和 jerk 限制。 + +本地预检只覆盖软限位、奇异点和机械臂自碰,不检测工作台、人员或外部障碍物。 + +运行: + python3 examples/10_local_kinematics.py + python3 examples/10_local_kinematics.py --go --speed 0.2 --dx 0.005 +""" +import os +import sys +import time + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +import litearm as pa + +from _common import make_arm, parser + + +def _wait_enabled(arm, timeout=1.0): + """Wait for a fresh status frame to confirm enable; do not use a fixed sleep.""" + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + state = arm.get_state(refresh=True).value + if state is not None: + if state.faulted: + raise pa.MotorFaultError( + f"使能后检测到关节故障: {state.fault_detail}") + if state.enabled: + return state + time.sleep(0.01) + raise pa.LiteArmError(f"{timeout}s 内状态帧没有确认使能,请检查供电、故障和固件状态") + + +def main(): + ap = parser(__doc__) + ap.add_argument("--dx", type=float, default=0.005, + help="--go 时工具目标沿世界 x 的位移(m,默认 0.005)") + args = ap.parse_args() + + arm = make_arm(args) + try: + state = arm.get_state(refresh=True).value + q = state.q + pose = arm.kinematics.fk(q, frame="tool") + jac = arm.kinematics.jacobian(q, frame="tool") + solution = arm.kinematics.ik(pose, q_seed=q, frame="tool") + print("当前 q =", [round(v, 4) for v in q]) + print("本地工具 FK =", [round(v, 5) for v in pose]) + print(f"sigma_min = {jac.sigma_min:.6f}") + print(f"同位姿 IK Δq = {solution.max_joint_delta:.6g} rad") + + if not args.go: + print("只读完成;加 --go 才会使能并执行安全 PTP") + return + + target = list(pose) + target[0] += args.dx + arm.enable() + _wait_enabled(arm) + reached = arm.kinematics.move_pose(target, speed=args.speed, frame="tool") + print("安全 PTP 到位 q =", [round(v, 4) for v in reached.q]) + actual = arm.kinematics.fk(reached.q, frame="tool") + print("实测关节状态对应的工具 FK =", [round(v, 5) for v in actual]) + finally: + if args.go: + arm.disable() + arm.close() + + +if __name__ == "__main__": + main() diff --git a/examples/README.md b/examples/README.md index 0abab7d..5a838f1 100644 --- a/examples/README.md +++ b/examples/README.md @@ -78,18 +78,23 @@ solve is host-side. | [07_vel_jitter_trace.py](07_vel_jitter_trace.py) | Per-tick capture of a slow `movej` (300 Hz firmware log + 100 Hz live stream) | Yes | | [08_tcp_offset.py](08_tcp_offset.py) | Measure a tool TCP offset by multi-point least squares (pure `math`) and hand it to `set_tcp_offset` | No (moves nothing; `--go` starts sampling) | | [09_payload_identify.py](09_payload_identify.py) | Identify the end-of-arm load's mass and centre of mass against an empty-load baseline, then `set_payload` | Yes (and needs a baseline JSON) | +| [10_local_kinematics.py](10_local_kinematics.py) | SDK-local FK/IK/Jacobian; guarded pose PTP sends one synchronized joint target | No (moves only with `--go`) | Every script's docstring states how to run it and what to watch out for. ## Safety notes -The examples that move the arm (02 / 03 / 05 / 06 / 07 / 09) **really drive it**: +The examples that move the arm (02 / 03 / 05 / 06 / 07 / 09 / 10) **really drive it**: - keep `--speed` at 0.1–0.3 the first time; - stand by the emergency stop, and make sure nobody and nothing is in the workspace; - read the [troubleshooting guide](../TROUBLESHOOTING.md) first; - **`movej` does not check joint limits** — an out-of-range target travels the full stroke. +`10_local_kinematics.py` checks runtime soft limits, self-collision clearance and Jacobian +singularity along its synchronized joint path. It cannot see people, benches or other external +obstacles; those remain the operator's responsibility. + [08_tcp_offset.py](08_tcp_offset.py) is the exception: it sends no motion and enables nothing. It needs the arm **backdrivable** — call `arm.disable()` first if it is powered and holding. diff --git a/pyproject.toml b/pyproject.toml index 5d63d4d..27ca94d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "litearm-python" -version = "2.1.0" +version = "2.2.0" description = "Python SDK for the LiteArm robotic arm" readme = "README.md" requires-python = ">=3.9" @@ -28,6 +28,11 @@ classifiers = [ ] dependencies = ["pyserial>=3.4"] +[project.optional-dependencies] +collision = ["numpy>=1.23", "pin>=3.0"] +ik = ["numpy>=1.23", "scipy>=1.9"] +robotics = ["numpy>=1.23", "scipy>=1.9", "pin>=3.0"] + [project.urls] Repository = "https://github.com/nexform-tech/litearm-python" Issues = "https://github.com/nexform-tech/litearm-python/issues" @@ -40,4 +45,8 @@ litearm-python = "litearm.arm:main" where = ["src"] [tool.setuptools.package-data] -litearm = ["py.typed"] +litearm = [ + "py.typed", + "assets/litearm/urdf/*.urdf", + "assets/litearm/meshes/*.STL", +] diff --git a/src/litearm/__init__.py b/src/litearm/__init__.py index b4b0f65..7a9fbba 100644 --- a/src/litearm/__init__.py +++ b/src/litearm/__init__.py @@ -1,8 +1,8 @@ -"""litearm-python —— LiteArm STM32 直连后端 (薄协议 SDK)。 +"""litearm-python —— LiteArm STM32 直连 SDK。 -定位: 固件(litearm-stm32, 版本约定 ``Litearm1.5.0``+)已内置 S 曲线/IK/动力学/ -控制律, 本包直连 USB CDC 用高层子集镜像 pylitearm 常用用法, **不改 pylitearm 源码**、 -PC 端不做轨迹/运动学。 +定位: 固件(litearm-stm32, 版本约定 ``Litearm1.5.0``+)内置 S 曲线/IK/动力学/控制律; +本包直连 USB CDC,并可选提供基于随包 URDF 的本地 FK/IK/Jacobian 与安全 PTP 预检。 +固件仍负责最终的速度、加速度、jerk 约束和同步 S 曲线执行。 典型用法:: @@ -24,8 +24,9 @@ `diag.kin_bench`) 返回它 —— `value` 是原返回值, `hz` 是该类帧在本会话里的平均到达频率, `timestamp` 是最近一帧的本地时刻。`move_*`/`home` (动作结果) 与纯本地量不包。 -**笛卡尔运动** 只有固件规划一条路: `move_l` / `move_c` / `move_path` —— 采样/逐点 IK/播放 -全在固件里, PC 只发点、收 `0x4E` 结果帧, 返回 `CartPlan`。 +**笛卡尔路径** `move_l` / `move_c` / `move_path` 仍由固件规划。另一条互不冲突的入口是 +`arm.kinematics.move_pose()`:SDK 本地求 IK 并检查整条关节直线,然后只发送一条 +`CMD_MOVE_J_SYNC`,由固件执行同步 PTP;它不保证末端走笛卡尔直线。 ⚠ 2.0 起**移除了 PC 侧规划**: 原子包 `cartesian/` 与 `Cartesian` / `CartesianPlanner` / `MotionResult` / `arm.cartesian` / `movel` / `movec` 都不再存在, 替代是上面三个入口 @@ -34,9 +35,9 @@ ⚠ `move_p` (`0x02`) 是**关节空间**点到点, 与上面这些**笛卡尔**路径不是一回事: 它只收 单个位姿 (多路点是 `move_path`), 末端轨迹不保证 (实测 30mm 的目标末端横摆 17.6mm)。 -能力边界(刻意不做): 任意关节角 fk(q) —— 固件 `CMD_GET_TCP` 只能算**当前反馈 q** 的 -位姿, 没有"给 q 求位姿"的下行命令, 所以拿不到任意构型的 FK (连带影响: 起点奇异预检 -σmin 算不了); 阻抗控制 —— 高级场景走 pylitearm+server。 +接口边界: `Arm.ik()` / `Arm.get_tcp()` 保持为固件远程计算;任意构型的本地 +FK/IK/Jacobian 放在 `arm.kinematics`,微分速度 IK 放在 `litearm.exact_ik`。阻抗控制仍不 +在本包范围内。安全 PTP 只检查自碰,不检测工作台、人员或外部障碍物。 工具点 (TCP) 偏移是**宿主侧**的一层换算: 固件没有工具坐标系命令, `get_tcp()` 恒为法兰, 所以 `set_tcp_offset(xyz, rpy)` 只在 SDK 里把 `move_p`/`move_l`/`move_c`/`move_path`/`ik` @@ -48,9 +49,9 @@ (`0x39` 收任意 `q`, 配合临时 `set_payload`) 现算 ⇒ 仍然**不需要** PC 侧任何动力学模型。 产物直接喂 `set_payload()`。 -依赖边界(刻意保持): 只有 `pyserial`。PC 侧不做规划, 只用**纯 Python** 数学(`math` 模块, -见 `_rot.py` / `payload.py`)归一化位姿形态、判到位姿态、解 4 参数最小二乘, 不引入 -numpy/pinocchio —— 重型计算本来就在固件的 IK 与 RNEA 里。 +依赖边界: 默认安装仍只有 `pyserial`。`arm.kinematics` 与碰撞检测属于可选的 +`robotics` 依赖组(NumPy、SciPy、Pinocchio/hpp-fcl);没有安装时只在调用这些入口时给出 +明确提示,不影响轻量串口功能。 协议覆盖: 固件 `hal/usb_cmd.h` 里每条**已实现**的下行命令都有对应入口 (`arm.params.*` 关节级参数 / `arm.log.*` 300Hz 采集 / `arm.diag.kin_bench` 自检); @@ -62,6 +63,7 @@ from litearm.arm import (Arm, FIRMWARE_PREFIX, LicenseInfo, main, MIN_FW, Msg) from litearm._rot import as_pose, mat_to_rpy, rpy_to_mat from litearm.cart import CartPlan +from litearm.collision import CollisionModel, collision_urdf_path from litearm.diagnostics import KinBenchResult, parse_kin_bench from litearm.errors import ( ArmIsInDfuError, @@ -85,6 +87,8 @@ UnsupportedByFirmwareError, ) from litearm.log import LogReader, LogSample, parse_samples +from litearm.kinematics import (IKSolution, JacobianResult, Kinematics, + LocalKinematicsError) from litearm.model import (MODEL_MASK_WRITTEN, ModelParams, ModelStatus) from litearm.params import JointParam, JointParams from litearm.payload import Baseline, HoldSample, PayloadResult @@ -92,7 +96,7 @@ from litearm.tool import TcpOffset from litearm.transport import find_cdc_port -__version__ = "2.1.0" +__version__ = "2.2.0" __all__ = [ "Arm", "Msg", "LicenseInfo", "RobotState", "JointState", "find_cdc_port", "main", @@ -105,6 +109,8 @@ "NotRemoteable", "NotSupportedOnThisBackend", "TeleopLockedError", "TeleopBusyError", "CartPlan", + "CollisionModel", "collision_urdf_path", + "Kinematics", "JacobianResult", "IKSolution", "LocalKinematicsError", "JointParam", "JointParams", "ModelParams", "ModelStatus", "MODEL_MASK_WRITTEN", "LogSample", "LogReader", "parse_samples", "KinBenchResult", "parse_kin_bench", diff --git a/src/litearm/_robot_model.py b/src/litearm/_robot_model.py new file mode 100644 index 0000000..4127a9b --- /dev/null +++ b/src/litearm/_robot_model.py @@ -0,0 +1,146 @@ +"""Shared Pinocchio model used by local kinematics and self-collision checks.""" +from __future__ import annotations + +import importlib +import math +from pathlib import Path +import threading +from typing import Iterable, Optional, Sequence, Union + + +_ASSET_ROOT = Path(__file__).resolve().parent / "assets" +_DEFAULT_URDF = _ASSET_ROOT / "litearm" / "urdf" / "litearm.urdf" + + +def robot_urdf_path() -> str: + return str(_DEFAULT_URDF) + + +class RobotModel: + """One mutable Pinocchio model/data/geometry bundle. + + Pinocchio updates ``Data`` and ``GeometryData`` in-place, so every bundle + owns its data and serializes access. Kinematics and collision queries on + the same bundle therefore always use the same URDF and frame selection. + """ + + def __init__(self, urdf_path: Optional[Union[str, Path]] = None, + package_dirs: Optional[Union[ + str, Path, Sequence[Union[str, Path]]]] = None, + *, dependency_hint: str = "litearm-python[robotics]", + require_ee: bool = True): + try: + self.np = importlib.import_module("numpy") + self.pin = importlib.import_module("pinocchio") + except (ImportError, OSError) as exc: + raise ImportError( + "SDK 本地运动学/碰撞检测需要 NumPy 与 Pinocchio/hpp-fcl;" + f"请安装 {dependency_hint}。若当前 shell source 过 ROS,请用 " + "`env -u PYTHONPATH -u LD_LIBRARY_PATH python ...` 运行" + ) from exc + + path = Path(urdf_path) if urdf_path is not None else _DEFAULT_URDF + path = path.expanduser().resolve() + if not path.is_file(): + raise FileNotFoundError(f"LiteArm URDF 不存在: {path}") + if package_dirs is None: + packages = [str(_ASSET_ROOT)] + elif isinstance(package_dirs, (str, Path)): + packages = [str(Path(package_dirs).expanduser().resolve())] + else: + packages = [str(Path(p).expanduser().resolve()) for p in package_dirs] + + self.urdf_path = str(path) + self.package_dirs = tuple(packages) + self.model = self.pin.buildModelFromUrdf(self.urdf_path) + self.data = self.model.createData() + self.ee_frame_id = None + if require_ee: + matches = [i for i, frame in enumerate(self.model.frames) + if frame.name == "ee_link" and frame.type == self.pin.BODY] + if len(matches) != 1: + raise ValueError(f"URDF 应恰有一个 BODY ee_link,实得 {len(matches)}") + self.ee_frame_id = matches[0] + + self.geom = self.pin.buildGeomFromUrdf( + self.model, self.urdf_path, self.pin.COLLISION, + package_dirs=list(self.package_dirs)) + self.geom.addAllCollisionPairs() + self.n_all_pairs = len(self.geom.collisionPairs) + self._remove_adjacent_pairs() + self.n_active_pairs = len(self.geom.collisionPairs) + self.geom_data = self.geom.createData() + self.lock = threading.RLock() + + @property + def nq(self) -> int: + return int(self.model.nq) + + def q(self, values: Iterable[float]): + try: + vals = tuple(float(v) for v in values) + except (TypeError, ValueError) as exc: + raise ValueError(f"q 必须包含 {self.nq} 个有限数值") from exc + if len(vals) != self.nq or not all(math.isfinite(v) for v in vals): + raise ValueError(f"q 必须包含 {self.nq} 个有限数值") + return self.np.asarray(vals, dtype=float) + + def _remove_adjacent_pairs(self) -> None: + remove = [] + for pair in self.geom.collisionPairs: + first, second = int(pair.first), int(pair.second) + j1 = int(self.geom.geometryObjects[first].parentJoint) + j2 = int(self.geom.geometryObjects[second].parentJoint) + if (j1 == j2 or int(self.model.parents[j1]) == j2 + or int(self.model.parents[j2]) == j1): + remove.append((first, second)) + for first, second in remove: + self.geom.removeCollisionPair(self.pin.CollisionPair(first, second)) + + def placement(self, q): + if self.ee_frame_id is None: + raise RuntimeError("该 RobotModel 未启用 ee_link 运动学") + self.pin.forwardKinematics(self.model, self.data, q) + self.pin.updateFramePlacements(self.model, self.data) + return self.data.oMf[self.ee_frame_id] + + def jacobian(self, q): + if self.ee_frame_id is None: + raise RuntimeError("该 RobotModel 未启用 ee_link 运动学") + return self.np.asarray(self.pin.computeFrameJacobian( + self.model, self.data, q, self.ee_frame_id, + self.pin.LOCAL_WORLD_ALIGNED), dtype=float) + + def sigma_min(self, q) -> float: + singular = self.np.linalg.svd(self.jacobian(q), compute_uv=False) + value = float(singular[-1]) if singular.size else 0.0 + return value if math.isfinite(value) else 0.0 + + def in_collision(self, q) -> bool: + return bool(self.pin.computeCollisions( + self.model, self.data, self.geom, self.geom_data, q, True)) + + def colliding_pairs(self, q) -> list[tuple[str, str]]: + self.pin.computeCollisions( + self.model, self.data, self.geom, self.geom_data, q, False) + result = [] + for index, collision in enumerate(self.geom_data.collisionResults): + if collision.isCollision(): + pair = self.geom.collisionPairs[index] + result.append((str(self.geom.geometryObjects[int(pair.first)].name), + str(self.geom.geometryObjects[int(pair.second)].name))) + return result + + def minimum_distance(self, q) -> float: + if not self.geom.collisionPairs: + return math.inf + self.pin.computeDistances( + self.model, self.data, self.geom, self.geom_data, q) + values = [float(result.min_distance) + for result in self.geom_data.distanceResults] + if not values or not all(math.isfinite(v) for v in values): + raise ValueError("碰撞距离计算返回非有限值") + return min(values) + + +__all__ = ["RobotModel", "robot_urdf_path"] diff --git a/src/litearm/arm.py b/src/litearm/arm.py index d90348e..0bc0002 100644 --- a/src/litearm/arm.py +++ b/src/litearm/arm.py @@ -1,8 +1,9 @@ -"""Arm —— 高层子集: 直连 STM32 固件, 轨迹/IK/动力学/控制律全由固件内置承担。 +"""Arm —— 直连 STM32 固件,并提供可选的 SDK 本地运动学。 设计定位 (阶段 C「pylitearm STM32 直连后端」): - 单发语义: movej/move_p 一次下发, 固件 S 曲线自完成 + 到位受控静止保持(B); - - 不做 PC 侧轨迹/运动学; fk 仅当前位姿(get_tcp), ik 走固件 get_ik; + - `get_tcp` / `ik` 保持走固件;`arm.kinematics` 用随包 URDF 本地算 FK/IK/Jacobian, + 安全预检通过后以一条同步关节 PTP 交给固件执行; - 笛卡尔运动只有一条路: **固件规划** move_l/move_c/move_path (PC 只发点, 收 0x4E 结果); 阻抗控制不在本包 (高级走 pylitearm+server); - 连续伺服(move_js/send_mit)需调用方按 ≥10Hz 重发, 否则 0.1s 看门狗 fail-soft。 @@ -18,6 +19,7 @@ import threading import time import weakref +from functools import wraps from dataclasses import dataclass from typing import Any, Generic, List, Optional, Sequence, Tuple, TypeVar @@ -45,6 +47,7 @@ from litearm.cart import _CART_CLEARS_UPON, _CartPending, CartPlan from litearm.diagnostics import Diagnostics from litearm.log import ArmLog +from litearm.kinematics import Kinematics from litearm.model import ModelParams from litearm.params import JointParams from litearm.transport import SerialTransport, find_cdc_port, is_tcp_target @@ -129,6 +132,15 @@ DFU_VANISH_TIMEOUT_S = 0.3 +def _serialize_positive_motion(method): + """Serialize energy-adding motion calls; stop/disable paths never use this lock.""" + @wraps(method) + def wrapped(self, *args, **kwargs): + with self._motion_serial: + return method(self, *args, **kwargs) + return wrapped + + def _rpy_to_R(rpy: Sequence[float]): """rpy(roll, pitch, yaw) -> 3x3 行主序旋转矩阵 (扁平 9 元组)。 @@ -793,6 +805,11 @@ def __init__(self, port: Optional[str] = None, *, self._model: Optional["ModelParams"] = None # [2026-09-14] 模型在线导入 self._log: Optional[ArmLog] = None self._diag: Optional[Diagnostics] = None + self._kinematics: Optional[Kinematics] = None + #: Positive-motion serialization. It is deliberately separate from + #: `_cart_serial`: local IK preflight must exclude movej/move_p as well + #: as Cartesian commands. Emergency stop and disable never take it. + self._motion_serial = threading.RLock() #: 在途笛卡尔请求的 FIFO 配对队列 + `0x4E` 收集器 (见 `cart.py`)。 #: 与写口同寿: `_raw_write` 里挂着它的清队钩子, 故**必须早于任何写**存在。 #: 吸收额度的存活时间 = `cart_absorb_ttl(move_timeout)` (沿用既有旋钮 + 一个有 @@ -1800,6 +1817,13 @@ def diag(self) -> "Diagnostics": self._diag = Diagnostics(self) return self._diag + @property + def kinematics(self) -> "Kinematics": + """SDK-local FK/IK/Jacobian and guarded PTP motion.""" + if self._kinematics is None: + self._kinematics = Kinematics(self) + return self._kinematics + # ---------- 笛卡尔运动 (固件原生规划: 0x3A/0x3B/0x3C-0x3E + RSP_CART_PLAN) ---------- # 规划 (采样/逐点 IK/Hermite 播放) 全在固件里, PC 侧只发点、收 `0x4E` 结果帧。 # 能力 (受固件 `#if LITEARM_CART_PLAN` 约束) 在 `connect()` 时探一次, 缓存在 @@ -1810,6 +1834,7 @@ def diag(self) -> "Diagnostics": # **一条 `0x4E` 都不发** —— 那些命令在发出前就会清掉本 SDK 的待配队列。 # ⚠ `move_p` 在途时不要发笛卡尔请求: 固件后台 IK 算完后会再清一次队, 那条笛卡尔 # 会被静默作废 (且臂会朝上一条 `move_p` 的目标真的走掉)。 + @_serialize_positive_motion def move_l(self, pose, speed: float = 1.0, wait: bool = True) -> CartPlan: """笛卡尔**直线** (`CMD_MOVE_L 0x3A`): 末端沿起点→目标直线走, 姿态 slerp。 @@ -1836,6 +1861,7 @@ def move_l(self, pose, speed: float = 1.0, wait: bool = True) -> CartPlan: """ return cart_mod.move_l(self, self._to_flange(pose, "move_l"), speed, wait) + @_serialize_positive_motion def move_c(self, pose_start, pose_via, pose_goal, speed: float = 1.0, wait: bool = True) -> CartPlan: """笛卡尔**圆弧** (`CMD_MOVE_C 0x3B`): 起点 + `pose_via` + `pose_goal` 三点定圆。 @@ -1858,6 +1884,7 @@ def move_c(self, pose_start, pose_via, pose_goal, speed: float = 1.0, self._to_flange(pose_goal, "move_c goal"), speed, wait) + @_serialize_positive_motion def move_path(self, poses, speed: float = 1.0, wait: bool = True) -> CartPlan: """多路点 (`0x3C` BEGIN → `0x3D` ADD×n → `0x3E` RUN): 依次经过 `poses`, **尖角**。 @@ -1978,6 +2005,7 @@ def done(st: ST.RobotState, prev: Optional[Sequence[float]]) -> bool: a._cond.wait(left) raise MotionTimeoutError(f"未到位, 超时 {budget}s") + @_serialize_positive_motion def movej(self, q: Sequence[float], speed: float = 1.0) -> ST.RobotState: # 先判连接: 未连接时 self.n 还是 0, 拿它做 arity 校验只会给出误导性的 # "需要 0 个关节角"。 @@ -1992,6 +2020,7 @@ def movej(self, q: Sequence[float], speed: float = 1.0) -> ST.RobotState: a.expect(P.RSP_ACK, 1.0, "movej", echo_cmd=P.CMD_MOVE_J) # ACK 后进入等待到位 return self._arrive(list(q)) + @_serialize_positive_motion def movej_sync(self, q: Sequence[float], speed: float = 1.0) -> ST.RobotState: """**同步 PTP** movej: 全关节**同时到达**, 末端沿**关节空间直线** q(s) = q0 + s·Δq。 @@ -2020,6 +2049,7 @@ def movej_sync(self, q: Sequence[float], speed: float = 1.0) -> ST.RobotState: a.expect(P.RSP_ACK, 1.0, "movej_sync", echo_cmd=P.CMD_MOVE_J_SYNC) return self._arrive(list(q)) + @_serialize_positive_motion def move_p(self, pose, speed: float = 1.0, pos_tol: float = 0.006, rpy_tol: float = 0.03) -> ST.RobotState: """点到点运动 (`CMD_MOVE_P 0x02`): 固件后台自己 IK + 走 S 曲线, 本方法轮询 @@ -2309,6 +2339,7 @@ def ik(self, pose: Sequence[float], q_seed: Optional[Sequence[float]] = None, raise IKError("目标不可达/IK 失败") return qs + @_serialize_positive_motion def home(self, *, timeout: Optional[float] = None) -> ST.RobotState: """固件 `CMD_HOME 0x2A`: 各轴回 URDF 零位舒展姿。 @@ -2326,6 +2357,7 @@ def home(self, *, timeout: Optional[float] = None) -> ST.RobotState: return self._arrive([0.0] * self.n, timeout=timeout) # ---------- 连续伺服 / 透传 ---------- + @_serialize_positive_motion def move_js(self, q: Sequence[float], dq: Optional[Sequence[float]] = None, tau_ff: Optional[Sequence[float]] = None) -> None: """单发 move_js。连续伺服需调用方按 ≥10Hz 重发, 否则 0.1s 看门狗 fail-soft。""" @@ -2344,6 +2376,7 @@ def move_js(self, q: Sequence[float], dq: Optional[Sequence[float]] = None, payload += P.pack_f32s(tau_ff) self._cmd(P.CMD_MOVE_JS, payload, "move_js") + @_serialize_positive_motion def send_mit(self, idx: int, q: float, dq: float, kp: float, kd: float, tau: float) -> None: self._reject_if_in_dfu() # 排在 idx 预检之前 (终态下 self.n=0 ⇒ 否则报"idx 越界") @@ -2352,6 +2385,7 @@ def send_mit(self, idx: int, q: float, dq: float, payload = bytes([int(idx)]) + P.pack_f32s([q, dq, kp, kd, tau]) self._cmd(P.CMD_MOVE_MIT, payload, "send_mit") + @_serialize_positive_motion def send_mit_all(self, q, dq, kp, kd, tau) -> None: self._reject_if_in_dfu() # 排在 arity 预检之前 (终态下 self.n=0 ⇒ 否则报"需 N 个") for arr, label in ((q, "q"), (dq, "dq"), (kp, "kp"), (kd, "kd"), (tau, "tau")): @@ -2360,6 +2394,7 @@ def send_mit_all(self, q, dq, kp, kd, tau) -> None: payload = P.pack_f32s(list(q) + list(dq) + list(kp) + list(kd) + list(tau)) self._cmd(P.CMD_MOVE_MIT_ALL, payload, "send_mit_all") + @_serialize_positive_motion def joint_follow(self, q, dq, kp, kd) -> None: """[JOINT_FOLLOW] 从臂跟随:**伺服环在固件里**,这里只喂目标与增益。 diff --git a/src/litearm/assets/litearm/meshes/base_link.STL b/src/litearm/assets/litearm/meshes/base_link.STL new file mode 100644 index 0000000..c608e34 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/base_link.STL differ diff --git a/src/litearm/assets/litearm/meshes/ee_link.STL b/src/litearm/assets/litearm/meshes/ee_link.STL new file mode 100644 index 0000000..d74e838 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/ee_link.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint1.STL b/src/litearm/assets/litearm/meshes/joint1.STL new file mode 100644 index 0000000..4fe8e8e Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint1.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint2.STL b/src/litearm/assets/litearm/meshes/joint2.STL new file mode 100644 index 0000000..f438919 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint2.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint3.STL b/src/litearm/assets/litearm/meshes/joint3.STL new file mode 100644 index 0000000..74e7ee6 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint3.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint4.STL b/src/litearm/assets/litearm/meshes/joint4.STL new file mode 100644 index 0000000..5e68895 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint4.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint5.STL b/src/litearm/assets/litearm/meshes/joint5.STL new file mode 100644 index 0000000..847f57f Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint5.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint6.STL b/src/litearm/assets/litearm/meshes/joint6.STL new file mode 100644 index 0000000..addaf30 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint6.STL differ diff --git a/src/litearm/assets/litearm/meshes/joint7.STL b/src/litearm/assets/litearm/meshes/joint7.STL new file mode 100644 index 0000000..2ddefa2 Binary files /dev/null and b/src/litearm/assets/litearm/meshes/joint7.STL differ diff --git a/src/litearm/assets/litearm/urdf/litearm.urdf b/src/litearm/assets/litearm/urdf/litearm.urdf new file mode 100644 index 0000000..8855480 --- /dev/null +++ b/src/litearm/assets/litearm/urdf/litearm.urdf @@ -0,0 +1,256 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/litearm/collision.py b/src/litearm/collision.py new file mode 100644 index 0000000..e9fcb9a --- /dev/null +++ b/src/litearm/collision.py @@ -0,0 +1,67 @@ +"""Optional host-side self-collision and clearance checks.""" +from __future__ import annotations + +import importlib # compatibility injection point used by dependency tests +from pathlib import Path +from typing import Iterable, Optional, Sequence, Union + +from litearm._robot_model import RobotModel, robot_urdf_path + + +def collision_urdf_path() -> str: + return robot_urdf_path() + + +class CollisionModel: + """Pinocchio/hpp-fcl checker backed by the SDK's packaged URDF. + + Same-link and directly adjacent-link pairs are excluded because their + meshes normally touch by design. Each checker owns mutable Pinocchio data + and serializes calls on that instance. + """ + + def __init__(self, urdf_path: Optional[Union[str, Path]] = None, + package_dirs: Optional[Union[ + str, Path, Sequence[Union[str, Path]]]] = None, + *, _bundle: Optional[RobotModel] = None): + self._bundle = _bundle or RobotModel( + urdf_path, package_dirs, dependency_hint="litearm-python[collision]", + require_ee=False) + # Preserve the established diagnostic attributes. + self._np = self._bundle.np + self._pin = self._bundle.pin + self.urdf_path = self._bundle.urdf_path + self.package_dirs = self._bundle.package_dirs + self.model = self._bundle.model + self.data = self._bundle.data + self.geom = self._bundle.geom + self.geom_data = self._bundle.geom_data + self.n_all_pairs = self._bundle.n_all_pairs + self.n_active_pairs = self._bundle.n_active_pairs + self._lock = self._bundle.lock + + @property + def nq(self) -> int: + return self._bundle.nq + + def _q(self, q: Iterable[float]): + return self._bundle.q(q) + + def in_collision(self, q: Iterable[float]) -> bool: + values = self._q(q) + with self._lock: + return self._bundle.in_collision(values) + + def colliding_pairs(self, q: Iterable[float]) -> list[tuple[str, str]]: + values = self._q(q) + with self._lock: + return self._bundle.colliding_pairs(values) + + def minimum_distance(self, q: Iterable[float]) -> float: + """Minimum separation in metres over active non-adjacent pairs.""" + values = self._q(q) + with self._lock: + return self._bundle.minimum_distance(values) + + +__all__ = ["CollisionModel", "collision_urdf_path"] diff --git a/src/litearm/exact_ik.py b/src/litearm/exact_ik.py new file mode 100644 index 0000000..008c804 --- /dev/null +++ b/src/litearm/exact_ik.py @@ -0,0 +1,215 @@ +"""Exact differential inverse kinematics with joint-velocity bounds. + +This is the dependency-light SDK port of lite-vr's ``exact_ik`` backend. The +solver accepts a Jacobian, not a robot model: callers remain responsible for +computing the Jacobian and for expressing the twist in the same frame. + +NumPy is an optional dependency. Install ``litearm-python[ik]`` (or the +``robotics`` extra) before importing this module. +""" +from __future__ import annotations + +try: + import numpy as np +except ImportError as exc: # pragma: no cover - exercised in a clean environment + raise ImportError( + "litearm.exact_ik requires NumPy; install 'litearm-python[ik]'" + ) from exc + +FAIL_NONE = "none" +FAIL_SHAPE = "shape" +FAIL_NO_JACOBIAN = "no_jacobian" +FAIL_BOX = "box" +FAIL_RESIDUAL = "residual" +NEAR_SINGULAR_SIGMA = 0.05 + + +class TaskVelocitySolution: + """Result of :func:`exact_task_velocity`.""" + + __slots__ = ("velocity", "residual", "achieved", "failure", + "worst_joint", "worst_excess", "worst_side") + + def __init__(self, velocity, residual=0.0, achieved=False, + failure=FAIL_NONE, worst_joint=-1, worst_excess=0.0, + worst_side=0): + self.velocity = velocity + self.residual = float(residual) + self.achieved = bool(achieved) + self.failure = failure + self.worst_joint = int(worst_joint) + self.worst_excess = float(worst_excess) + self.worst_side = int(worst_side) + + +def null_space_interval(velocity, direction, lower, upper): + """Return the alpha interval keeping ``velocity + alpha*direction`` in bounds.""" + velocity = np.asarray(velocity, float).ravel() + direction = np.asarray(direction, float).ravel() + lower = np.asarray(lower, float).ravel() + upper = np.asarray(upper, float).ravel() + low, high = -np.inf, np.inf + n = velocity.size + if n == 0 or direction.size != n or lower.size != n or upper.size != n: + return low, high, True + for i in range(n): + d = direction[i] + if d == 0.0: + if velocity[i] < lower[i] or velocity[i] > upper[i]: + return low, high, True + continue + a = (lower[i] - velocity[i]) / d + b = (upper[i] - velocity[i]) / d + low = max(low, min(a, b)) + high = min(high, max(a, b)) + return low, high, low > high + + +def exact_task_velocity(jacobian, twist, lower, upper, + rank_tolerance=1e-9, residual_tolerance=1e-3): + """Solve ``J*dq = twist`` exactly inside a per-joint velocity box. + + The minimum-norm solution is used unless the one-dimensional redundant + direction must be spent to enter the box. If the task cannot be delivered + without scaling, ``achieved`` is false and the returned velocity is zero. + This matches lite-vr's deliberately binary gain (one or zero). + """ + jacobian = np.asarray(jacobian, float) + twist = np.asarray(twist, float).ravel() + lower = np.asarray(lower, float).ravel() + upper = np.asarray(upper, float).ravel() + if jacobian.ndim != 2: + return TaskVelocitySolution(np.zeros(0), residual=1.0, + failure=FAIL_SHAPE) + rows, cols = jacobian.shape + if (rows <= 0 or cols <= 0 or twist.size != rows or + lower.size != cols or upper.size != cols or + np.any(lower > upper) or not np.all(np.isfinite(jacobian)) or + not np.all(np.isfinite(twist)) or not np.all(np.isfinite(lower)) or + not np.all(np.isfinite(upper))): + return TaskVelocitySolution(np.zeros(max(cols, 0)), residual=1.0, + failure=FAIL_SHAPE) + + u, singular, vh = np.linalg.svd(jacobian, full_matrices=True) + if singular.size == 0 or singular[0] <= 0.0: + return TaskVelocitySolution(np.zeros(cols), residual=1.0, + failure=FAIL_NO_JACOBIAN) + keep = singular > float(rank_tolerance) * singular[0] + particular = (vh[:singular.size][keep].T @ + ((u[:, :singular.size][:, keep].T @ twist) / singular[keep])) + + # LiteArm is 6x7, so one null direction is sufficient. For a more general + # Jacobian with multiple null directions we intentionally retain the same + # last-vector policy as the source implementation. + direction = vh[cols - 1].copy() if cols > rows else np.zeros(cols) + low, high, empty = null_space_interval(particular, direction, lower, upper) + if empty: + breakpoints = [0.0] + for i in range(cols): + if direction[i] != 0.0: + breakpoints.extend(((lower[i] - particular[i]) / direction[i], + (upper[i] - particular[i]) / direction[i])) + best, worst_joint, worst_side = np.inf, -1, 0 + for alpha in breakpoints: + value = particular + alpha * direction + miss = np.maximum(0.0, np.maximum(lower - value, value - upper)) + j = int(np.argmax(miss)) + if miss[j] < best: + best = float(miss[j]) + worst_joint = j + worst_side = 1 if value[j] > upper[j] else -1 + return TaskVelocitySolution( + np.zeros(cols), residual=1.0, failure=FAIL_BOX, + worst_joint=worst_joint, + worst_excess=0.0 if not np.isfinite(best) else best, + worst_side=worst_side) + + alpha = float(min(high, max(low, 0.0))) + velocity = particular + alpha * direction + reference = max(float(np.linalg.norm(twist)), 1e-9) + ratio = float(np.linalg.norm(jacobian @ velocity - twist)) / reference + if ratio > float(residual_tolerance): + return TaskVelocitySolution(np.zeros(cols), residual=ratio, + failure=FAIL_RESIDUAL) + return TaskVelocitySolution(velocity, residual=ratio, achieved=True) + + +def box_binding(solution, box_lower, box_upper, speed_limit): + """Classify a box failure as a speed or joint-range constraint.""" + j = solution.worst_joint + if solution.failure != FAIL_BOX or j < 0 or solution.worst_side == 0: + return None + bound = box_upper[j] if solution.worst_side > 0 else box_lower[j] + ceiling = speed_limit if solution.worst_side > 0 else -speed_limit + return "speed" if abs(bound - ceiling) <= 1e-9 else "range" + + +def bounded_task_velocity(jacobian, twist, lower, upper, damping=0.02): + """Return the best feasible damped task velocity inside the joint box. + + This is lite-vr's continuous fallback for poses where + :func:`exact_task_velocity` cannot deliver the complete twist. It requires + SciPy in addition to NumPy (both are installed by the ``ik`` extra). + """ + try: + from scipy.optimize import lsq_linear + except ImportError as exc: # pragma: no cover - clean optional-dependency env + raise ImportError( + "bounded_task_velocity requires SciPy; install 'litearm-python[ik]'" + ) from exc + jacobian = np.asarray(jacobian, float) + target = np.asarray(twist, float) + lo, hi = np.asarray(lower, float), np.asarray(upper, float) + if (jacobian.ndim != 2 or target.shape != (jacobian.shape[0],) or + lo.shape != (jacobian.shape[1],) or hi.shape != lo.shape or + not all(np.all(np.isfinite(x)) for x in (jacobian, target, lo, hi)) or + np.any(lo > hi) or not np.isfinite(damping) or damping <= 0): + raise ValueError("invalid bounded task velocity inputs") + free = hi > lo + velocity = lo.copy() + if np.any(free): + count = int(np.sum(free)) + matrix = np.vstack((jacobian[:, free], damping * np.eye(count))) + rhs = np.concatenate((target - jacobian[:, ~free] @ velocity[~free], + np.zeros(count))) + result = lsq_linear(matrix, rhs, bounds=(lo[free], hi[free]), + method="bvls", tol=1e-10, max_iter=100) + velocity[free] = np.clip(result.x, lo[free], hi[free]) + return velocity + + +def describe_failure(solution, side="", binding=None, sigma_min=None, + near_singular_sigma=None): + """Return the actionable operator message used by lite-vr.""" + threshold = (NEAR_SINGULAR_SIGMA if near_singular_sigma is None + else near_singular_sigma) + tag = "[ok]" if solution.failure == FAIL_NONE else f"[{solution.failure}]" + if side: + tag = f"[{side}][{solution.failure}]" + if solution.failure == FAIL_BOX: + text = (f"{tag} 关节 {solution.worst_joint + 1} 需要的关节速度比该位姿允许的多 " + f"{solution.worst_excess:.2f} rad/s,且冗余方向上没有任何位置能让它合法。") + if binding == "speed": + text += "起作用的是关节速度上限:放慢手速,或调低手柄到臂的映射增益。" + elif binding == "range": + text += "起作用的是关节行程:该关节已到行程末端。把手往工作空间内移。" + if sigma_min is not None: + text += ("该位姿接近奇异(σ_min 低),把手往奇异点外移。" + if sigma_min < threshold else + "该位姿不接近奇异,这一次不是奇异点的问题。") + return text + if solution.failure == FAIL_NO_JACOBIAN: + return f"{tag} 此位姿的雅可比没有任何可用方向。把手往奇异点外移。" + if solution.failure == FAIL_RESIDUAL: + return f"{tag} 命令的 twist 有 {100.0 * solution.residual:.1f}% 在此位姿产生不了。" + if solution.failure == FAIL_SHAPE: + return f"{tag} 数组互相不匹配或含非法数值。" + return f"{tag} twist 已被精确交付。" + + +__all__ = [ + "FAIL_NONE", "FAIL_SHAPE", "FAIL_NO_JACOBIAN", "FAIL_BOX", + "FAIL_RESIDUAL", "NEAR_SINGULAR_SIGMA", "TaskVelocitySolution", + "null_space_interval", "exact_task_velocity", "bounded_task_velocity", "box_binding", + "describe_failure", +] diff --git a/src/litearm/kinematics.py b/src/litearm/kinematics.py new file mode 100644 index 0000000..f463aa4 --- /dev/null +++ b/src/litearm/kinematics.py @@ -0,0 +1,370 @@ +"""SDK-local LiteArm FK/IK/Jacobian and guarded joint-space PTP motion.""" +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import TYPE_CHECKING, Optional, Sequence + +from litearm import _rot +from litearm import _protocol as P +from litearm._robot_model import RobotModel +from litearm.errors import IKError, InvalidCommandError, MotorFaultError, TransportError + +if TYPE_CHECKING: + from litearm.arm import Arm + from litearm.state import RobotState + + +MAX_CONTINUATION_POS_M = 0.010 +MAX_CONTINUATION_ROT_RAD = math.radians(2.0) +MAX_IK_JOINT_STEP_RAD = 0.35 +PATH_SAMPLE_RAD = 0.020 +LIMIT_MARGIN_RAD = 0.010 +MIN_CLEARANCE_M = 0.005 +MIN_SIGMA = 0.05 +START_DRIFT_RAD = 0.030 +POS_TOL_M = 0.0005 +ROT_TOL_RAD = 0.005 + + +@dataclass(frozen=True) +class JacobianResult: + matrix: tuple[tuple[float, ...], ...] + sigma_min: float + near_singular: bool + + +@dataclass(frozen=True) +class IKSolution: + q: tuple[float, ...] + position_error: float + orientation_error: float + sigma_min: float + max_joint_delta: float + iterations: int + failure: str = "none" + + +class LocalKinematicsError(IKError): + """Local IK or safety preflight failed before any motion was sent.""" + + def __init__(self, message: str, *, reason: str, + sigma_min: Optional[float] = None, + max_joint_delta: Optional[float] = None): + super().__init__(message) + self.reason = reason + self.sigma_min = sigma_min + self.max_joint_delta = max_joint_delta + + +def _frame(frame: str) -> str: + value = str(frame).lower() + if value not in ("tool", "flange"): + raise InvalidCommandError("frame 只能是 'tool' 或 'flange'") + return value + + +class Kinematics: + """``arm.kinematics`` local model; never calls firmware IK/FK commands.""" + + def __init__(self, arm: "Arm"): + self._arm = arm + self._bundle: Optional[RobotModel] = None + + def _robot(self) -> RobotModel: + if self._bundle is None: + self._bundle = RobotModel(dependency_hint="litearm-python[robotics]") + return self._bundle + + @property + def model(self) -> RobotModel: + """Shared local model bundle (advanced diagnostics/testing).""" + return self._robot() + + def _pose_flange(self, q) -> tuple[list[float], list[list[float]]]: + robot = self._robot() + values = robot.q(q) + with robot.lock: + placement = robot.placement(values) + pos = [float(v) for v in placement.translation] + R = [[float(placement.rotation[i, j]) for j in range(3)] for i in range(3)] + return pos, R + + def fk(self, q: Sequence[float], frame: str = "tool") -> tuple[float, ...]: + """Compute local FK as ``(x,y,z,roll,pitch,yaw)`` without device I/O.""" + use_frame = _frame(frame) + pos, R = self._pose_flange(q) + pose = pos + _rot.mat_to_rpy(R) + if use_frame == "tool": + pose = self._arm._to_tool(pose, "kinematics.fk") + return tuple(float(v) for v in pose) + + def _jacobian_array(self, q, frame: str): + robot = self._robot() + values = robot.q(q) + with robot.lock: + placement = robot.placement(values) + J = robot.jacobian(values).copy() + if frame == "tool" and not self._arm._tcp_offset.is_identity: + offset = robot.np.asarray(self._arm._tcp_offset.xyz, dtype=float) + r_world = robot.np.asarray(placement.rotation) @ offset + for column in range(J.shape[1]): + J[:3, column] += robot.np.cross(J[3:, column], r_world) + return J + + def jacobian(self, q: Sequence[float], frame: str = "tool") -> JacobianResult: + """Return the world-aligned 6x7 geometric Jacobian and its smallest singular value.""" + use_frame = _frame(frame) + robot = self._robot() + J = self._jacobian_array(q, use_frame) + singular = robot.np.linalg.svd(J, compute_uv=False) + sigma = float(singular[-1]) if singular.size else 0.0 + if not math.isfinite(sigma): + sigma = 0.0 + return JacobianResult( + tuple(tuple(float(v) for v in row) for row in J), + sigma, sigma < MIN_SIGMA) + + def _limits(self, *, live: bool): + robot = self._robot() + if live: + params = self._arm.params.all_joint_params() + if len(params) != robot.nq: + raise InvalidCommandError( + f"本地模型是 {robot.nq} 轴,设备状态是 {len(params)} 轴") + lo = robot.np.asarray([p.q_min for p in params], dtype=float) + hi = robot.np.asarray([p.q_max for p in params], dtype=float) + else: + lo = robot.np.asarray(robot.model.lowerPositionLimit, dtype=float).copy() + hi = robot.np.asarray(robot.model.upperPositionLimit, dtype=float).copy() + if (lo.shape != (robot.nq,) or hi.shape != (robot.nq,) + or not robot.np.all(robot.np.isfinite(lo)) + or not robot.np.all(robot.np.isfinite(hi)) + or robot.np.any(lo >= hi)): + raise LocalKinematicsError("关节限位不可用", reason="invalid_limits") + return lo, hi + + def _target(self, pose, frame: str): + try: + pos, R = _rot.as_pose(pose) + except (TypeError, ValueError) as exc: + raise InvalidCommandError(f"kinematics.ik pose 非法: {exc}") from None + vec = list(pos) + _rot.mat_to_rpy(R) + if frame == "tool": + vec = self._arm._to_flange(vec, "kinematics.ik") + pos, R = _rot.as_pose(vec) + if not (all(math.isfinite(float(v)) for v in pos) + and all(math.isfinite(float(v)) for row in R for v in row)): + raise InvalidCommandError("kinematics.ik pose 含非有限值") + return self._robot().np.asarray(pos, dtype=float), self._robot().np.asarray(R, dtype=float) + + def _one_target(self, q0, pos_d, R_d, lo, hi): + robot = self._robot() + np = robot.np + q = np.clip(np.asarray(q0, dtype=float), lo, hi) + seed = q.copy() + middle = 0.5 * (lo + hi) + span = np.maximum(hi - lo, 1e-6) + damping = 1e-3 + best = None + for iteration in range(1, 201): + with robot.lock: + placement = robot.placement(q) + J = robot.jacobian(q).copy() + pos = np.asarray(placement.translation, dtype=float) + R = np.asarray(placement.rotation, dtype=float) + ep_vec = pos_d - pos + er_vec = np.asarray(robot.pin.log3(R_d @ R.T), dtype=float) + ep = float(np.linalg.norm(ep_vec)) + er = float(np.linalg.norm(er_vec)) + cost = ep + er + if best is None or cost < best[0]: + best = (cost, q.copy(), ep, er) + if ep < POS_TOL_M and er < ROT_TOL_RAD: + return q, ep, er, iteration + error = np.concatenate((ep_vec, er_vec)) + A = J @ J.T + damping * np.eye(6) + try: + primary = J.T @ np.linalg.solve(A, error) + pinv = J.T @ np.linalg.solve(A, np.eye(6)) + except np.linalg.LinAlgError: + damping = min(damping * 10.0, 1.0) + continue + null = np.eye(robot.nq) - pinv @ J + # Stay on the seeded redundant branch and softly move away from limits. + bias = 0.04 * (seed - q) + 0.01 * (middle - q) / span + dq = primary + null @ bias + norm = float(np.linalg.norm(dq)) + if norm > 0.20: + dq *= 0.20 / norm + accepted = False + for scale in (1.0, 0.5, 0.25, 0.125): + trial = np.clip(q + scale * dq, lo, hi) + with robot.lock: + trial_place = robot.placement(trial) + tep = float(np.linalg.norm(pos_d - np.asarray(trial_place.translation))) + ter = float(np.linalg.norm(robot.pin.log3( + R_d @ np.asarray(trial_place.rotation).T))) + if tep + ter < cost: + q = trial + damping = max(damping * 0.5, 1e-7) + accepted = True + break + if not accepted: + damping = min(damping * 5.0, 1.0) + assert best is not None + raise LocalKinematicsError( + f"本地 IK 未收敛: 位置残差 {best[2]:.6f}m, 姿态残差 {best[3]:.6f}rad", + reason="no_solution") + + def _solve(self, pose, seed, lo, hi, frame: str) -> IKSolution: + robot = self._robot() + np = robot.np + q_seed = robot.q(seed) + pos_goal, R_goal = self._target(pose, frame) + with robot.lock: + start = robot.placement(q_seed) + pos_start = np.asarray(start.translation, dtype=float).copy() + R_start = np.asarray(start.rotation, dtype=float).copy() + dist = float(np.linalg.norm(pos_goal - pos_start)) + angle = float(np.linalg.norm(robot.pin.log3(R_start.T @ R_goal))) + steps = max(1, int(math.ceil(dist / MAX_CONTINUATION_POS_M)), + int(math.ceil(angle / MAX_CONTINUATION_ROT_RAD))) + q = q_seed.copy() + total_iterations = 0 + pos_err = rot_err = 0.0 + rel_log = np.asarray(robot.pin.log3(R_start.T @ R_goal), dtype=float) + for index in range(1, steps + 1): + ratio = index / steps + pos_d = pos_start + ratio * (pos_goal - pos_start) + R_d = R_start @ robot.pin.exp3(ratio * rel_log) + prev = q.copy() + q, pos_err, rot_err, used = self._one_target(q, pos_d, R_d, lo, hi) + total_iterations += used + jump = float(np.max(np.abs(q - prev))) + if not math.isfinite(jump) or jump > MAX_IK_JOINT_STEP_RAD: + raise LocalKinematicsError( + f"IK 分支跳变 {jump:.4f}rad > {MAX_IK_JOINT_STEP_RAD:.2f}rad", + reason="branch_jump", max_joint_delta=jump) + J = self._jacobian_array(q, frame) + singular = np.linalg.svd(J, compute_uv=False) + sigma = float(singular[-1]) if singular.size else 0.0 + if not math.isfinite(sigma) or sigma < MIN_SIGMA: + raise LocalKinematicsError( + f"目标接近奇异点: sigma_min={sigma:.6f} < {MIN_SIGMA:.3f}", + reason="near_singular", sigma_min=sigma) + delta = float(np.max(np.abs(q - q_seed))) + return IKSolution(tuple(float(v) for v in q), pos_err, rot_err, + sigma, delta, total_iterations) + + def ik(self, pose, q_seed: Optional[Sequence[float]] = None, + frame: str = "tool") -> IKSolution: + """Solve pose IK locally; no command is sent to the controller.""" + use_frame = _frame(frame) + live = q_seed is None + if q_seed is None: + state = self._arm.get_state(refresh=True).value + if state is None: + raise TransportError("本地 IK 需要当前关节状态,但取不到状态帧") + q_seed = state.q + lo, hi = self._limits(live=live) + return self._solve(pose, q_seed, lo, hi, use_frame) + + def _check_path(self, start, goal, lo, hi, frame: str) -> None: + robot = self._robot() + np = robot.np + start = robot.q(start) + goal = robot.q(goal) + count = max(1, int(math.ceil(float(np.max(np.abs(goal - start))) / + PATH_SAMPLE_RAD))) + for index in range(count + 1): + q = start + (index / count) * (goal - start) + if np.any(q < lo + LIMIT_MARGIN_RAD) or np.any(q > hi - LIMIT_MARGIN_RAD): + raise LocalKinematicsError( + f"PTP 路径在采样点 {index}/{count} 进入软限位 {LIMIT_MARGIN_RAD:.3f}rad 余量", + reason="joint_limit") + with robot.lock: + if robot.in_collision(q): + pairs = robot.colliding_pairs(q) + raise LocalKinematicsError( + f"PTP 路径在采样点 {index}/{count} 自碰: {pairs}", + reason="collision") + clearance = robot.minimum_distance(q) + if not math.isfinite(clearance): + raise LocalKinematicsError( + "PTP 路径碰撞距离不可用", reason="invalid_clearance") + if clearance < MIN_CLEARANCE_M: + raise LocalKinematicsError( + f"PTP 路径最小间隙 {clearance:.6f}m < {MIN_CLEARANCE_M:.3f}m", + reason="clearance") + sigma = self.jacobian(q, frame=frame).sigma_min + if sigma < MIN_SIGMA: + raise LocalKinematicsError( + f"PTP 路径在采样点 {index}/{count} 接近奇异点: sigma_min={sigma:.6f}", + reason="near_singular", sigma_min=sigma) + + @staticmethod + def _validate_motion_state(state: "RobotState") -> None: + if not state.enabled: + raise InvalidCommandError("move_pose 要求机械臂已使能") + if state.faulted: + raise MotorFaultError(f"move_pose 拒绝故障状态: {state.fault_detail}") + if state.mode == P.MODE_ZERO_G: + raise InvalidCommandError("move_pose 拒绝零重力模式;先 zero_g_stop()") + if state.cart_busy: + raise InvalidCommandError("move_pose 拒绝笛卡尔规划/播放在途") + + def _fresh_state(self) -> "RobotState": + """Require state activity after entering the safety-critical read. + + ``Arm.get_state(refresh=True)`` intentionally preserves its historical + cached-state fallback. That is useful to ordinary callers but unsafe + for preflight: a silent controller must not let an old q pass as the + start we are about to execute from. Compare the reader sequence here + so the stricter rule remains local to ``move_pose``. + """ + ack = self._arm._require() + sequence = ack.status_seq + message = self._arm.get_state(refresh=True) + if message.value is None or ack.status_seq == sequence: + raise TransportError("move_pose 取不到最新状态帧") + return message.value + + def move_pose(self, pose, speed: float = 0.2, + frame: str = "tool") -> "RobotState": + """Locally solve/check a pose, then send one synchronized PTP target. + + The IK residual describes the local numerical solution, not guaranteed + physical arrival accuracy. The returned state uses the existing + ``movej_sync`` joint-space arrival criteria (``Arm.q_tol``, + ``Arm.dq_tol`` and ``Arm.arrive_frames``), and real accuracy also + includes encoder, tracking and calibration error. Call :meth:`fk` on + the returned ``state.q`` when the measured final tool pose matters. + """ + use_frame = _frame(frame) + if not 0.0 < float(speed) <= 1.0: + raise InvalidCommandError("move_pose speed 需在 (0,1],0 会产生永不到位的轨迹") + with self._arm._motion_serial: + for attempt in range(2): + state = self._fresh_state() + self._validate_motion_state(state) + lo, hi = self._limits(live=True) + solution = self._solve(pose, state.q, lo, hi, use_frame) + self._check_path(state.q, solution.q, lo, hi, use_frame) + fresh = self._fresh_state() + self._validate_motion_state(fresh) + drift = max(abs(fresh.q[i] - state.q[i]) for i in range(len(state.q))) + if drift <= START_DRIFT_RAD: + return self._arm.movej_sync(solution.q, speed=float(speed)) + if attempt: + raise LocalKinematicsError( + f"预检期间起点连续变化,最大漂移 {drift:.4f}rad", + reason="start_moved", max_joint_delta=drift) + raise AssertionError("unreachable") + + +__all__ = [ + "Kinematics", "JacobianResult", "IKSolution", "LocalKinematicsError", + "MAX_CONTINUATION_POS_M", "MAX_CONTINUATION_ROT_RAD", "MAX_IK_JOINT_STEP_RAD", + "PATH_SAMPLE_RAD", "LIMIT_MARGIN_RAD", "MIN_CLEARANCE_M", "MIN_SIGMA", +] diff --git a/src/litearm/testing.py b/src/litearm/testing.py index e072b02..cb066a6 100644 --- a/src/litearm/testing.py +++ b/src/litearm/testing.py @@ -446,6 +446,7 @@ def _status_frame(self, q, dq=None, mode=1): """状态帧 —— 力矩走 `tau_provider(q)` (有) 或静态 `self.tau` (无)。""" tau = self.tau_provider(q) if self.tau_provider is not None else self.tau return _status(q, dq=self.dq if dq is None else dq, mode=mode, + flags=(1 << P.FLAG_ENABLED_BIT) if self.enabled else 0, n=self.n, tau=tau) # ---- SerialTransport 接口 ---- @@ -809,7 +810,7 @@ def write_frame(self, cmd: int, payload: bytes = b"") -> None: self._push(self._status_frame(self.q, dq=[1.0] * self.n)) for _ in range(6): self._push(self._status_frame(self.q, dq=[0.0] * self.n)) - elif cmd == P.CMD_MOVE_J: + elif cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC): self._push(_ack(cmd)) q = P.unpack_f32s(payload, 0, self.n) # payload: q×N + sp self.q = [float(v) for v in q] diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..99416f5 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,18 @@ +"""Small pytest support for the optional local-kinematics regression tests.""" +from __future__ import annotations + +import pytest + +from litearm import Arm +from litearm.testing import FakeTransport + + +@pytest.fixture +def offline_arm(): + transport = FakeTransport() + arm = Arm(transport_factory=lambda _port: transport, move_timeout=2.0) + arm.connect("fake") + try: + yield arm + finally: + arm.close() diff --git a/tests/test_collision.py b/tests/test_collision.py new file mode 100644 index 0000000..05c1c1e --- /dev/null +++ b/tests/test_collision.py @@ -0,0 +1,171 @@ +"""Optional host-side collision checking (no hardware required).""" +from __future__ import annotations + +import importlib +from pathlib import Path +from types import SimpleNamespace + +try: + import pytest +except ImportError: + import unittest + raise unittest.SkipTest("pytest is not installed") + +import litearm as pa +import litearm.collision as collision_mod + + +class _Pair: + def __init__(self, first, second): + self.first = first + self.second = second + + +class _Result: + def __init__(self, hit=False): + self.hit = hit + + def isCollision(self): + return self.hit + + +class _Model: + nq = 7 + # universe, joint1, joint2, joint3 + parents = [0, 0, 1, 2] + + def createData(self): + return object() + + +class _Geometry: + def __init__(self): + self.geometryObjects = [ + SimpleNamespace(parentJoint=0, name="base"), + SimpleNamespace(parentJoint=1, name="link1"), + SimpleNamespace(parentJoint=2, name="link2"), + SimpleNamespace(parentJoint=3, name="link3"), + ] + self.collisionPairs = [] + + def addAllCollisionPairs(self): + self.collisionPairs = [ + _Pair(i, j) for i in range(4) for j in range(i + 1, 4) + ] + + def removeCollisionPair(self, target): + self.collisionPairs = [ + pair for pair in self.collisionPairs + if (pair.first, pair.second) != (target.first, target.second) + ] + + def createData(self): + return SimpleNamespace( + collisionResults=[_Result() for _ in self.collisionPairs]) + + +class _Pin: + COLLISION = object() + CollisionPair = _Pair + + def __init__(self): + self.geometries = [] + + @staticmethod + def buildModelFromUrdf(_path): + return _Model() + + def buildGeomFromUrdf(self, *_args, **_kwargs): + geometry = _Geometry() + self.geometries.append(geometry) + return geometry + + @staticmethod + def computeCollisions(_model, _data, geometry, geometry_data, q, stop): + # The fake's deterministic collision: positive J1 makes the first + # surviving pair collide. Keep result-vector order aligned with pairs. + hits = [bool(q[0] > 0.0 and index == 0) + for index in range(len(geometry.collisionPairs))] + geometry_data.collisionResults = [_Result(hit) for hit in hits] + return any(hits) if stop else False + + +@pytest.fixture +def fake_dependencies(monkeypatch): + real_import = importlib.import_module + pin = _Pin() + + def load(name): + if name == "pinocchio": + return pin + return real_import(name) + + monkeypatch.setattr(collision_mod.importlib, "import_module", load) + return pin + + +def test_assets_are_packaged_and_public_api_is_exported(): + urdf = Path(pa.collision_urdf_path()) + assert urdf.is_file() + assert len(list(urdf.parents[1].joinpath("meshes").glob("*.STL"))) == 9 + assert pa.CollisionModel is collision_mod.CollisionModel + assert "CollisionModel" in pa.__all__ + assert "collision_urdf_path" in pa.__all__ + + +def test_checker_excludes_same_or_adjacent_links_and_reports_pairs(fake_dependencies): + checker = pa.CollisionModel() + assert checker.n_all_pairs == 6 + assert checker.n_active_pairs == 3 + assert not checker.in_collision([0.0] * 7) + assert checker.in_collision([0.1] + [0.0] * 6) + assert checker.colliding_pairs([0.1] + [0.0] * 6) == [("base", "link2")] + + +@pytest.mark.parametrize("bad", [ + [], [0.0] * 6, [0.0] * 8, + [0.0] * 6 + [float("nan")], + [0.0] * 6 + [float("inf")], + [0.0] * 6 + ["not-a-number"], + None, +]) +def test_checker_strictly_validates_joint_vector(fake_dependencies, bad): + checker = pa.CollisionModel() + with pytest.raises(ValueError, match="7 个有限数值"): + checker.in_collision(bad) + + +def test_each_checker_owns_its_geometry_model(fake_dependencies): + first = pa.CollisionModel() + second = pa.CollisionModel() + assert first.geom is not second.geom + first.geom.collisionPairs.clear() + assert len(second.geom.collisionPairs) == second.n_active_pairs == 3 + + +def test_missing_optional_dependency_has_install_hint(monkeypatch): + real_import = importlib.import_module + + def load(name): + if name == "pinocchio": + raise ModuleNotFoundError(name) + return real_import(name) + + monkeypatch.setattr(collision_mod.importlib, "import_module", load) + with pytest.raises(ImportError, match=r"litearm-python\[collision\]"): + pa.CollisionModel() + + +def test_real_packaged_model_when_pinocchio_is_available(): + try: + importlib.import_module("pinocchio") + except (ImportError, OSError) as exc: + pytest.skip(f"本机 Pinocchio 不可用: {exc}") + + checker = pa.CollisionModel() + assert checker.nq == 7 + assert checker.n_active_pairs < checker.n_all_pairs + assert not checker.in_collision([0.0] * 7) + folded = [-0.753, 1.4236, 0.1053, -2.6041, -0.0948, -0.7629, 0.4072] + assert checker.in_collision(folded) + assert checker.colliding_pairs(folded) diff --git a/tests/test_exact_ik.py b/tests/test_exact_ik.py new file mode 100644 index 0000000..b9fe549 --- /dev/null +++ b/tests/test_exact_ik.py @@ -0,0 +1,59 @@ +import importlib.util + +try: + import pytest +except ImportError: + import unittest + raise unittest.SkipTest("pytest is not installed") + +pytestmark = pytest.mark.skipif(importlib.util.find_spec("numpy") is None, + reason="optional NumPy dependency is not installed") + + +def test_exact_solution_and_zero_space_policy(): + import numpy as np + from litearm.exact_ik import FAIL_NONE, exact_task_velocity + + solution = exact_task_velocity([[1.0, 0.0]], [0.5], [-1.0, -1.0], [1.0, 1.0]) + assert solution.achieved + assert solution.failure == FAIL_NONE + np.testing.assert_allclose(solution.velocity, [0.5, 0.0], atol=1e-12) + + +def test_box_failure_reports_joint_side_and_excess(): + from litearm.exact_ik import FAIL_BOX, box_binding, exact_task_velocity + + solution = exact_task_velocity([[1.0, 0.0]], [5.0], [-1.0, -1.0], [1.0, 1.0]) + assert not solution.achieved + assert solution.failure == FAIL_BOX + assert solution.worst_joint == 0 + assert solution.worst_side == 1 + assert solution.worst_excess == pytest.approx(4.0) + assert box_binding(solution, [-1.0, -1.0], [1.0, 1.0], 1.0) == "speed" + + +def test_rank_deficiency_is_measured_by_residual(): + from litearm.exact_ik import FAIL_RESIDUAL, exact_task_velocity + + solution = exact_task_velocity([[1.0, 0.0], [0.0, 0.0]], [0.0, 1.0], + [-2.0, -2.0], [2.0, 2.0]) + assert not solution.achieved + assert solution.failure == FAIL_RESIDUAL + assert solution.residual == pytest.approx(1.0) + + +def test_invalid_values_are_shape_failure(): + from litearm.exact_ik import FAIL_SHAPE, exact_task_velocity + + solution = exact_task_velocity([[float("nan")]], [0.0], [-1.0], [1.0]) + assert solution.failure == FAIL_SHAPE + + +def test_bounded_fallback_respects_saturation_and_fixed_joints(): + pytest.importorskip("scipy") + import numpy as np + from litearm.exact_ik import bounded_task_velocity + + velocity = bounded_task_velocity(np.eye(2), [10.0, 0.2], + [-0.3, 0.0], [0.3, 0.0]) + np.testing.assert_allclose(velocity, [0.3, 0.0], atol=1e-9) diff --git a/tests/test_firmware_model_consistency.py b/tests/test_firmware_model_consistency.py new file mode 100644 index 0000000..069c220 --- /dev/null +++ b/tests/test_firmware_model_consistency.py @@ -0,0 +1,71 @@ +"""Guard the packaged SDK URDF against the STM32 generated FK/Jacobian model.""" +from __future__ import annotations + +import ctypes +import os +from pathlib import Path +import random +import shutil +import subprocess + +try: + import pytest +except ImportError: + import unittest + raise unittest.SkipTest("pytest is not installed") + +import litearm as pa +from litearm import _rot + + +def _stm32_root() -> Path: + configured = os.environ.get("LITEARM_STM32_ROOT") + if configured: + return Path(configured).expanduser().resolve() + return Path(__file__).resolve().parents[2] / "litearm-stm32" + + +def test_sdk_model_matches_stm32_generated_fk_and_jacobian(tmp_path): + np = pytest.importorskip("numpy") + pytest.importorskip("pinocchio") + root = _stm32_root() + kin_c = root / "User/litearm/kinematics/kin.c" + if not kin_c.is_file() or shutil.which("gcc") is None: + pytest.skip("litearm-stm32 sibling checkout and gcc are needed for cross-repo guard") + + shared = tmp_path / "liblitearm_kin.so" + subprocess.run( + ["gcc", "-shared", "-fPIC", "-O2", "-o", str(shared), str(kin_c), "-lm"], + check=True, capture_output=True, text=True) + firmware = ctypes.CDLL(str(shared)) + firmware.kin_fk.argtypes = [ctypes.POINTER(ctypes.c_float), + ctypes.POINTER(ctypes.c_float), + ctypes.POINTER(ctypes.c_float)] + firmware.kin_jacobian.argtypes = [ctypes.POINTER(ctypes.c_float), + ctypes.POINTER(ctypes.c_float)] + + arm = pa.Arm() + points = [ + [0.0] * 7, + [0.0, -0.5, 0.0, -1.0, 0.0, 0.5, 0.0], + [0.15, -0.55, 0.10, -1.10, 0.05, 0.55, -0.05], + [-0.31, 0.72, -0.44, -1.38, 0.27, 0.81, -0.19], + ] + rng = random.Random(7) + points.extend([[rng.uniform(-2.0, 2.0) for _ in range(7)] + for _ in range(20)]) + for q in points: + cq = (ctypes.c_float * 7)(*q) + cpos = (ctypes.c_float * 3)() + crot = (ctypes.c_float * 9)() + cjac = (ctypes.c_float * 42)() + firmware.kin_fk(cq, cpos, crot) + firmware.kin_jacobian(cq, cjac) + + pose = arm.kinematics.fk(q, frame="flange") + sdk_rot = np.asarray(_rot.rpy_to_mat(pose[3:]), dtype=float) + sdk_jac = np.asarray( + arm.kinematics.jacobian(q, frame="flange").matrix, dtype=float) + np.testing.assert_allclose(pose[:3], np.asarray(cpos[:]), atol=2e-5) + np.testing.assert_allclose(sdk_rot, np.asarray(crot[:]).reshape(3, 3), atol=2e-5) + np.testing.assert_allclose(sdk_jac, np.asarray(cjac[:]).reshape(6, 7), atol=2e-4) diff --git a/tests/test_local_kinematics.py b/tests/test_local_kinematics.py new file mode 100644 index 0000000..43fbe7f --- /dev/null +++ b/tests/test_local_kinematics.py @@ -0,0 +1,187 @@ +"""SDK-local FK/IK/Jacobian and fail-closed PTP motion.""" +from __future__ import annotations + +import importlib.util +import math +import copy + +try: + import pytest +except ImportError: + import unittest + raise unittest.SkipTest("pytest is not installed") + +import litearm as pa +from litearm import _protocol as P + + +pytestmark = pytest.mark.skipif( + importlib.util.find_spec("pinocchio") is None, + reason="optional robotics dependency is not installed") + + +SAFE_Q = [0.0, -0.5, 0.0, -1.0, 0.0, 0.5, 0.0] +TARGET_Q = [0.15, -0.55, 0.10, -1.10, 0.05, 0.55, -0.05] + + +def test_fk_zero_matches_firmware_model_golden(): + arm = pa.Arm() + pose = arm.kinematics.fk([0.0] * 7, frame="flange") + assert pose[:3] == pytest.approx( + [-9.21449129e-7, 2.52723928e-6, 0.782999783], abs=2e-7) + assert pose[3:] == pytest.approx( + [-3.463023e-6, 8.229795e-6, 1.570792654], abs=2e-6) + + +def test_local_jacobian_matches_fk_finite_difference(): + import numpy as np + + arm = pa.Arm() + kin = arm.kinematics + J = np.asarray(kin.jacobian(SAFE_Q, frame="flange").matrix) + eps = 1e-6 + p0 = np.asarray(kin.fk(SAFE_Q, frame="flange")[:3]) + for joint in range(7): + q = list(SAFE_Q) + q[joint] += eps + dp = (np.asarray(kin.fk(q, frame="flange")[:3]) - p0) / eps + np.testing.assert_allclose(dp, J[:3, joint], atol=2e-5) + assert J.shape == (6, 7) + assert kin.jacobian(SAFE_Q, frame="flange").sigma_min > 0.05 + + +def test_local_ik_round_trip_stays_on_seeded_branch(): + arm = pa.Arm() + target = arm.kinematics.fk(TARGET_Q, frame="flange") + solution = arm.kinematics.ik(target, q_seed=SAFE_Q, frame="flange") + reached = arm.kinematics.fk(solution.q, frame="flange") + assert reached[:3] == pytest.approx(target[:3], abs=5e-4) + assert solution.position_error < 5e-4 + assert solution.orientation_error < 5e-3 + assert solution.max_joint_delta < 0.35 + assert solution.sigma_min >= 0.05 + + +def test_zero_configuration_is_rejected_as_singular(): + arm = pa.Arm() + pose = arm.kinematics.fk([0.0] * 7, frame="flange") + with pytest.raises(pa.LocalKinematicsError) as caught: + arm.kinematics.ik(pose, q_seed=[0.0] * 7, frame="flange") + assert caught.value.reason == "near_singular" + assert caught.value.sigma_min < 0.05 + + +def test_small_cartesian_step_from_zero_cannot_trigger_large_rebranch(): + arm = pa.Arm() + target = list(arm.kinematics.fk([0.0] * 7, frame="flange")) + target[0] += 0.005 + with pytest.raises(pa.LocalKinematicsError) as caught: + arm.kinematics.ik(target, q_seed=[0.0] * 7, frame="flange") + assert caught.value.reason in ("no_solution", "branch_jump", "near_singular") + + +def test_tool_frame_fk_and_ik_share_tcp_offset(): + arm = pa.Arm() + arm.set_tcp_offset((0.0, 0.0, 0.10)) + flange = arm.kinematics.fk(SAFE_Q, frame="flange") + tool = arm.kinematics.fk(SAFE_Q, frame="tool") + assert math.dist(flange[:3], tool[:3]) == pytest.approx(0.10, abs=1e-7) + solution = arm.kinematics.ik(tool, q_seed=SAFE_Q, frame="tool") + assert solution.max_joint_delta < 1e-8 + + +def test_move_pose_sends_only_one_synchronized_joint_target(offline_arm): + arm = offline_arm + arm._tr.q = list(SAFE_Q) + arm.enable() + arm.get_state(refresh=True) + target = arm.kinematics.fk(TARGET_Q, frame="flange") + start = len(arm._tr.tx_log) + state = arm.kinematics.move_pose(target, speed=0.2, frame="flange") + sent = arm._tr.tx_log[start:] + motion = [(cmd, payload) for cmd, payload in sent + if cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC, P.CMD_GET_IK)] + assert [cmd for cmd, _ in motion] == [P.CMD_MOVE_J_SYNC] + assert state.q == pytest.approx(P.unpack_f32s(motion[0][1], 0, 7), abs=1e-6) + + +def test_move_pose_failure_sends_no_motion(offline_arm, monkeypatch): + arm = offline_arm + arm._tr.q = list(SAFE_Q) + arm.enable() + arm.get_state(refresh=True) + target = arm.kinematics.fk(TARGET_Q, frame="flange") + + def reject(*_args, **_kwargs): + raise pa.LocalKinematicsError("注入碰撞", reason="collision") + + monkeypatch.setattr(arm.kinematics, "_check_path", reject) + start = len(arm._tr.tx_log) + with pytest.raises(pa.LocalKinematicsError, match="注入碰撞"): + arm.kinematics.move_pose(target, frame="flange") + assert not any(cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC, P.CMD_MOVE_P, + P.CMD_MOVE_L, P.CMD_MOVE_C, P.CMD_GET_IK) + for cmd, _ in arm._tr.tx_log[start:]) + + +def test_move_pose_rejects_nonfinite_target_without_motion(offline_arm): + arm = offline_arm + arm._tr.q = list(SAFE_Q) + arm.enable() + start = len(arm._tr.tx_log) + with pytest.raises(pa.InvalidCommandError, match="非有限"): + arm.kinematics.move_pose((math.nan, 0, 0, 0, 0, 0), frame="flange") + assert not any(cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC, P.CMD_GET_IK) + for cmd, _ in arm._tr.tx_log[start:]) + + +def test_move_pose_missing_fresh_state_sends_no_motion(offline_arm, monkeypatch): + arm = offline_arm + monkeypatch.setattr( + arm, "get_state", lambda **_kwargs: pa.Msg(None, 0.0, 0.0)) + start = len(arm._tr.tx_log) + with pytest.raises(pa.TransportError, match="取不到最新状态帧"): + arm.kinematics.move_pose((0, 0, 0, 0, 0, 0), frame="flange") + assert not any(cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC, P.CMD_GET_IK) + for cmd, _ in arm._tr.tx_log[start:]) + + +def test_move_pose_aborts_after_start_moves_twice(offline_arm, monkeypatch): + arm = offline_arm + arm._tr.q = list(SAFE_Q) + arm.enable() + base = arm.get_state(refresh=True).value + + def state_at(q0): + state = copy.deepcopy(base) + for joint, value in zip(state.joints, q0): + joint.q = value + return pa.Msg(state, 0.0, 0.0) + + moved1 = list(SAFE_Q) + moved1[0] += 0.04 + moved2 = list(moved1) + moved2[0] += 0.04 + replies = iter((state_at(SAFE_Q).value, state_at(moved1).value, + state_at(moved1).value, state_at(moved2).value)) + monkeypatch.setattr(arm.kinematics, "_fresh_state", lambda: next(replies)) + solution = pa.IKSolution(tuple(TARGET_Q), 0.0, 0.0, 0.1, 0.1, 1) + solves = [] + monkeypatch.setattr(arm.kinematics, "_solve", + lambda *_args, **_kwargs: solves.append(1) or solution) + monkeypatch.setattr(arm.kinematics, "_check_path", lambda *_args, **_kwargs: None) + + start = len(arm._tr.tx_log) + with pytest.raises(pa.LocalKinematicsError) as caught: + arm.kinematics.move_pose((0, 0, 0, 0, 0, 0), frame="flange") + assert caught.value.reason == "start_moved" + assert len(solves) == 2 + assert not any(cmd in (P.CMD_MOVE_J, P.CMD_MOVE_J_SYNC, P.CMD_GET_IK) + for cmd, _ in arm._tr.tx_log[start:]) + + +def test_collision_checker_exposes_clearance(): + checker = pa.CollisionModel() + assert checker.minimum_distance(SAFE_Q) > 0.005 + folded = [-0.753, 1.4236, 0.1053, -2.6041, -0.0948, -0.7629, 0.4072] + assert checker.minimum_distance(folded) <= 0.0 diff --git a/tests/test_motion_serial.py b/tests/test_motion_serial.py new file mode 100644 index 0000000..e89bd8c --- /dev/null +++ b/tests/test_motion_serial.py @@ -0,0 +1,101 @@ +"""Positive motion is serialized, while energy-reducing commands stay reachable.""" +from __future__ import annotations + +import threading +import time + +try: + import pytest +except ImportError: + import unittest + raise unittest.SkipTest("pytest is not installed") + +from litearm import _protocol as P + + +def test_positive_motion_waits_but_estop_bypasses_motion_lock(offline_arm): + arm = offline_arm + arm.enable() + target = [0.05, -0.10, 0.05, -0.20, 0.0, 0.10, 0.0] + entered = threading.Event() + finished = threading.Event() + failures = [] + + def move(): + entered.set() + try: + arm.movej(target, speed=0.2) + except BaseException as exc: # pragma: no cover - included in assertion below + failures.append(exc) + finally: + finished.set() + + with arm._motion_serial: + start = len(arm._tr.tx_log) + worker = threading.Thread(target=move, name="serialized-move", daemon=True) + worker.start() + assert entered.wait(1.0) + time.sleep(0.05) + assert not finished.is_set() + assert not any(cmd == P.CMD_MOVE_J for cmd, _ in arm._tr.tx_log[start:]) + + arm.emergency_stop() + assert any(cmd == P.CMD_EMERGENCY_STOP for cmd, _ in arm._tr.tx_log[start:]) + assert not finished.is_set() + + worker.join(2.0) + assert finished.is_set() + assert not failures + sent = [cmd for cmd, _ in arm._tr.tx_log[start:] + if cmd in (P.CMD_EMERGENCY_STOP, P.CMD_MOVE_J)] + assert sent == [P.CMD_EMERGENCY_STOP, P.CMD_MOVE_J] + + +def test_disable_bypasses_motion_lock(offline_arm): + arm = offline_arm + arm.enable() + with arm._motion_serial: + start = len(arm._tr.tx_log) + arm.disable() + assert [cmd for cmd, _ in arm._tr.tx_log[start:]] == [P.CMD_DISABLE] + + +@pytest.mark.parametrize(("expected", "invoke"), [ + (P.CMD_HOME, lambda arm: arm.home(timeout=1.0)), + (P.CMD_MOVE_JS, lambda arm: arm.move_js([0.0] * arm.n)), + (P.CMD_MOVE_MIT, + lambda arm: arm.send_mit(0, 0.0, 0.0, 1.0, 0.1, 0.0)), + (P.CMD_MOVE_MIT_ALL, + lambda arm: arm.send_mit_all(*([[0.0] * arm.n] * 5))), + (P.CMD_JOINT_FOLLOW, + lambda arm: arm.joint_follow(*([[0.0] * arm.n] * 4))), +]) +def test_remaining_positive_motion_entry_points_are_serialized( + offline_arm, expected, invoke): + arm = offline_arm + entered = threading.Event() + finished = threading.Event() + failures = [] + + def send(): + entered.set() + try: + invoke(arm) + except BaseException as exc: # pragma: no cover - asserted below + failures.append(exc) + finally: + finished.set() + + with arm._motion_serial: + start = len(arm._tr.tx_log) + worker = threading.Thread(target=send, daemon=True) + worker.start() + assert entered.wait(1.0) + time.sleep(0.03) + assert not finished.is_set() + assert not any(cmd == expected for cmd, _ in arm._tr.tx_log[start:]) + + worker.join(2.0) + assert finished.is_set() + assert not failures + assert any(cmd == expected for cmd, _ in arm._tr.tx_log[start:]) diff --git a/tests/test_smoke.py b/tests/test_smoke.py index c5de40b..b4b8d46 100644 --- a/tests/test_smoke.py +++ b/tests/test_smoke.py @@ -1,6 +1,8 @@ """Minimal offline smoke tests for the public API.""" import math +import threading +import time import unittest import litearm @@ -16,11 +18,12 @@ rpy_to_mat, ) from litearm.testing import FakeTransport +from litearm import _protocol as P class PublicApiTest(unittest.TestCase): def test_version(self): - self.assertEqual(litearm.__version__, "2.1.0") + self.assertEqual(litearm.__version__, "2.2.0") def test_error_hierarchy(self): self.assertTrue(issubclass(UnsupportedByFirmwareError, CommandRejectedError)) @@ -52,6 +55,67 @@ def test_fake_transport_session(self): finally: arm.close() + def test_all_positive_motion_entry_points_share_the_serial_lock(self): + q = [0.0] * 7 + cases = ( + ("home", P.CMD_HOME, lambda arm: arm.home(timeout=1.0)), + ("move_js", P.CMD_MOVE_JS, lambda arm: arm.move_js(q)), + ("send_mit", P.CMD_MOVE_MIT, + lambda arm: arm.send_mit(0, 0.0, 0.0, 1.0, 0.1, 0.0)), + ("send_mit_all", P.CMD_MOVE_MIT_ALL, + lambda arm: arm.send_mit_all(q, q, q, q, q)), + ("joint_follow", P.CMD_JOINT_FOLLOW, + lambda arm: arm.joint_follow(q, q, q, q)), + ) + for name, command, invoke in cases: + with self.subTest(name=name): + transport = FakeTransport() + arm = Arm(transport_factory=lambda _port: transport, + move_timeout=1.0).connect("fake") + entered = threading.Event() + finished = threading.Event() + failures = [] + + def run(): + entered.set() + try: + invoke(arm) + except BaseException as exc: + failures.append(exc) + finally: + finished.set() + + try: + with arm._motion_serial: + start = len(transport.tx_log) + worker = threading.Thread(target=run, daemon=True) + worker.start() + self.assertTrue(entered.wait(1.0)) + time.sleep(0.03) + self.assertFalse(finished.is_set()) + self.assertFalse(any(cmd == command + for cmd, _ in transport.tx_log[start:])) + worker.join(2.0) + self.assertTrue(finished.is_set()) + self.assertFalse(failures) + self.assertTrue(any(cmd == command + for cmd, _ in transport.tx_log[start:])) + finally: + arm.close() + + def test_estop_and_disable_bypass_the_motion_lock(self): + transport = FakeTransport() + arm = Arm(transport_factory=lambda _port: transport).connect("fake") + try: + with arm._motion_serial: + start = len(transport.tx_log) + arm.emergency_stop() + arm.disable() + commands = [cmd for cmd, _ in transport.tx_log[start:]] + self.assertEqual(commands, [P.CMD_EMERGENCY_STOP, P.CMD_DISABLE]) + finally: + arm.close() + if __name__ == "__main__": unittest.main()