# KDL WASM 计算接口设计

版本：0.1  
适用范围：GRL 通用机器人程序、虚拟控制器、离线编程、路径验证、轨迹回放  
运行环境：浏览器 TypeScript + Web Worker + Orocos KDL WebAssembly

## 1. 目标

本文定义 KDL WASM 需要暴露给 TypeScript 虚拟控制器和通用机器人程序 GRL 使用的计算函数。接口设计从 GRL 语法反推，而不是简单暴露 KDL C++ 类。

GRL 中直接依赖 KDL WASM 的语义包括：

1. `joint_target`、`pose_target` 的目标点解析和可达性验证。
2. `tool`、`frame`、`offset`、`offset_in` 的位姿变换。
3. `movej` 关节角度差分运行。
4. `movel` TCP 直线运行。
5. `movec` TCP 圆弧运行。
6. `path`、`run_path`、`operation` 的批量轨迹生成和批量诊断。
7. 速度、加速度、zone、采样周期、节拍估算。
8. 虚拟控制器运行时的当前 TCP、当前关节、轨迹采样点、报警诊断。

KDL WASM 不负责：

1. GRL 词法和语法解析。
2. 程序流程控制、变量、IO、wait、子程序调用。
3. 碰撞检测和几何布尔运算。
4. 真实品牌控制器的完整 look-ahead 和伺服细节。
5. OPFS 项目文件管理。

这些由 TypeScript 层、虚拟控制器、碰撞模块和 OPFS Workspace 实现。

## 2. 总体架构

```text
GRL Source / OLP Model / Brand Import
  |
TypeScript Parser + Semantic Analyzer
  |
Executable IR / MotionSegmentRequest
  |
KdlWorkerClient
  |
kdl.worker.ts
  |
KDL WASM C ABI / Embind Wrapper
  |
Orocos KDL
```

设计原则：

1. KDL WASM 运行在 Worker 中，避免阻塞 UI 主线程。
2. TypeScript 业务层只使用稳定接口，不直接操作 KDL C++ 对象。
3. WASM 内部使用 `RobotHandle` 管理机器人链、求解器和缓存。
4. 机器人结构源数据为 URDF，TypeScript 层负责解析 XML 并生成标准模型，WASM 层负责构造 KDL `Tree/Chain`。
5. 高频数据使用 `Float64Array`，结构化配置使用 JSON。
6. 所有计算函数返回统一诊断，不只返回成功/失败。

## 3. KDL 能力映射

| GRL/虚拟控制器能力 | KDL 或包装层能力 | 说明 |
| --- | --- | --- |
| URDF 串联链 | `Tree`、`Chain`、`Segment`、`Joint` | 包装层从标准模型构造 |
| `movej` 终点 FK | `ChainFkSolverPos_recursive` | 关节到法兰/TCP |
| `movel/movec` 逐点 IK | `ChainIkSolverPos_NR_JL`、`ChainIkSolverPos_LMA` | 带关节限位和诊断 |
| 雅可比和奇异性 | `ChainJntToJacSolver` | 输出 Jacobian 和指标 |
| TCP 直线路径 | `Path_Line` 或包装层等价实现 | MOVEL 采样 |
| TCP 圆弧路径 | `Path_Circle` 或包装层等价实现 | MOVEC 采样 |
| 梯形速度 | `VelocityProfile_Trap` | 生成 `s/sd/sdd` |
| 轨迹段 | `Trajectory_Segment` | 路径 + 速度曲线 |
| zone/blend | `Path_RoundedComposite` 或自研 blend | P1 阶段 |

## 4. TypeScript 顶层 API

建议对外提供一个异步客户端：

```ts
interface KdlWasmApi {
  init(options?: KdlInitOptions): Promise<KdlRuntimeInfo>;
  dispose(): Promise<void>;

  loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise<RobotHandle>;
  createRobotFromModel(model: NormalizedRobotModel): Promise<RobotHandle>;
  destroyRobot(handle: RobotHandle): Promise<void>;
  getRobotInfo(handle: RobotHandle): Promise<RobotInfo>;
  getJointLimits(handle: RobotHandle): Promise<JointLimits[]>;

  normalizePose(input: PoseLike, options?: PoseNormalizeOptions): Promise<Pose>;
  composePose(a: Pose, b: Pose): Promise<Pose>;
  inversePose(pose: Pose): Promise<Pose>;
  applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Promise<Pose>;
  applyOffset(target: PoseTarget, offset: OffsetSpec): Promise<PoseTarget>;

  fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise<FkResult>;
  fkAllLinks(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise<LinkPoseResult>;
  jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise<JacobianResult>;
  ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise<IkResult>;
  ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise<IkResult[]>;

  checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise<LimitCheckResult>;
  checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): Promise<LimitCheckResult>;
  checkSingularity(handle: RobotHandle, joints: Float64Array): Promise<SingularityResult>;
  checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions): Promise<ReachabilityResult>;
  checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options?: IkOptions): Promise<ReachabilityResult[]>;

  makeTrapProfile(length: number, options: TrapProfileOptions): Promise<TrapProfileResult>;
  sampleTrapProfile(length: number, options: TrapProfileOptions): Promise<TrapSample[]>;

  planMoveJ(handle: RobotHandle, request: MoveJRequest): Promise<TrajectoryResult>;
  planMoveL(handle: RobotHandle, request: MoveLRequest): Promise<TrajectoryResult>;
  planMoveC(handle: RobotHandle, request: MoveCRequest): Promise<TrajectoryResult>;

  planPath(handle: RobotHandle, request: PathPlanRequest): Promise<PathPlanResult>;
  validatePath(handle: RobotHandle, request: PathPlanRequest): Promise<PathValidationResult>;
  estimateCycleTime(input: TrajectoryResult | PathPlanResult): Promise<CycleTimeResult>;
  resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise<TrajectoryResult>;
}
```

API 命名使用 `planMoveJ/planMoveL/planMoveC`，避免与 GRL 语句名混淆。GRL 编译器把 `movej/movel/movec` 解析成 IR 后，由虚拟控制器调用这些函数。

## 5. 基础数据类型

### 5.1 句柄和运行信息

```ts
type RobotHandle = number;

interface KdlRuntimeInfo {
  version: string;
  kdlVersion?: string;
  wasmBuild: string;
  supportsThreads: boolean;
  supportsWasmFs: boolean;
}
```

### 5.2 位姿

内部统一使用位置 + 四元数：

```ts
interface Pose {
  position: [number, number, number];       // meter
  quaternion: [number, number, number, number]; // x, y, z, w
}

type PoseLike =
  | Pose
  | { xyz: [number, number, number]; rpy: [number, number, number] }
  | { xyz: [number, number, number]; quat: [number, number, number, number] };
```

GRL 中 `pose(x, y, z, rx, ry, rz)` 由 TypeScript 编译器规范化为 `Pose` 后传给 KDL WASM。

### 5.3 目标点

```ts
interface PoseTarget {
  id?: string;
  pose: Pose;
  config?: RobotConfiguration;
  tool?: Pose;
  frame?: Pose;
  extAxis?: number[];
  sourceMap?: MotionSourceMap;
}

interface JointTarget {
  id?: string;
  joints: number[];
  extAxis?: number[];
  sourceMap?: MotionSourceMap;
}

interface RobotConfiguration {
  shoulder?: -1 | 0 | 1;
  elbow?: -1 | 0 | 1;
  wrist?: -1 | 0 | 1;
  turnNumbers?: number[];
}
```

### 5.4 速度和 zone

```ts
type SpeedSpec =
  | { kind: "joint_percent"; value: number }
  | { kind: "joint_abs"; velocity: number; acceleration?: number }
  | { kind: "linear"; velocity: number; acceleration?: number; angularVelocity?: number };

type ZoneSpec =
  | { kind: "fine" }
  | { kind: "distance"; value: number }
  | { kind: "cnt"; value: number }
  | { kind: "continuous" };
```

首版 `zone` 可只用于诊断和后处理，运动规划先按 `fine` 到点执行。P1 阶段再实现连续 blend。

### 5.5 诊断

```ts
interface MotionDiagnostic {
  severity: "info" | "warning" | "error";
  code: string;
  message: string;
  time?: number;
  pointIndex?: number;
  segmentId?: string;
  targetId?: string;
  sourceMap?: MotionSourceMap;
  data?: Record<string, unknown>;
}

interface MotionSourceMap {
  file?: string;
  line?: number;
  column?: number;
  module?: string;
  proc?: string;
  pathId?: string;
  pathPointId?: string;
  operationId?: string;
  brandSource?: "abb" | "fanuc" | "kuka" | "grl";
}
```

诊断 code 建议固定：

| code | 含义 |
| --- | --- |
| `KDL_INVALID_MODEL` | 机器人模型非法 |
| `KDL_TARGET_UNREACHABLE` | 目标不可达 |
| `KDL_IK_FAILED` | IK 求解失败 |
| `KDL_JOINT_LIMIT` | 关节超限 |
| `KDL_VELOCITY_LIMIT` | 速度超限 |
| `KDL_ACCEL_LIMIT` | 加速度超限 |
| `KDL_SINGULARITY` | 接近奇异 |
| `KDL_ARC_DEGENERATE` | 圆弧退化 |
| `KDL_PATH_EMPTY` | 空路径 |
| `KDL_ZONE_APPROXIMATED` | zone 被近似处理 |

## 6. 机器人模型 API

### 6.1 URDF 加载

```ts
interface UrdfLoadOptions {
  robotId: string;
  baseLink: string;
  tipLink: string;
  tool?: Pose;
  base?: Pose;
  jointOrder?: string[];
  overrideLimits?: JointLimitOverride[];
}

interface NormalizedRobotModel {
  robotId: string;
  name: string;
  baseLink: string;
  tipLink: string;
  links: LinkModel[];
  joints: JointModel[];
  activeJointNames: string[];
  limits: JointLimits[];
  source: {
    type: "urdf";
    urdfHash: string;
  };
}
```

推荐实现分工：

1. TypeScript 解析 URDF XML，检查 link/joint 连通性和单位。
2. TypeScript 生成 `NormalizedRobotModel`。
3. WASM 根据标准模型构造 KDL `Tree` 和从 `baseLink` 到 `tipLink` 的 `Chain`。
4. WASM 创建 FK、IK、Jacobian 求解器并绑定到 `RobotHandle`。

### 6.2 机器人信息

```ts
interface RobotInfo {
  handle: RobotHandle;
  robotId: string;
  name: string;
  baseLink: string;
  tipLink: string;
  jointNames: string[];
  dof: number;
  limits: JointLimits[];
}

interface JointLimits {
  name: string;
  lower: number;
  upper: number;
  velocity: number;
  acceleration: number;
  jerk?: number;
}
```

GRL 编译器在编译 `joint_target` 时应检查数组长度与 `dof` 一致。

## 7. 位姿和坐标变换 API

GRL 中 `tool`、`frame`、`offset`、`offset_in` 都需要位姿变换。虽然 TypeScript 也可实现这些基础变换，但建议 KDL WASM 提供一致的计算函数，避免数值约定不一致。

```ts
interface OffsetSpec {
  mode: "frame" | "tool" | "world";
  xyz?: [number, number, number];
  rpy?: [number, number, number];
  quaternion?: [number, number, number, number];
}
```

函数要求：

1. `normalizePose`：把欧拉角或四元数输入规范化。
2. `composePose`：计算 `a * b`。
3. `inversePose`：计算位姿逆。
4. `applyToolAndFrame`：把目标点、工具、工件坐标转换为机器人基坐标下 TCP 目标。
5. `applyOffset`：实现 GRL `pick offset z 100 mm` 和 `offset_in tool z -50 mm`。

## 8. FK、IK、Jacobian API

### 8.1 正解 FK

```ts
interface FkOptions {
  tool?: Pose;
  frame?: Pose;
  includeFlange?: boolean;
}

interface FkResult {
  ok: boolean;
  flange: Pose;
  tcp: Pose;
  joints: number[];
  diagnostics: MotionDiagnostic[];
}
```

用途：

1. 虚拟控制器显示当前 TCP。
2. `movel/movec` 计算当前 TCP 起点。
3. 轨迹采样点生成 TCP 位姿。
4. 3D 机器人 link 位姿显示。

### 8.2 全 link 正解

```ts
interface LinkPoseResult {
  ok: boolean;
  linkPoses: Array<{ link: string; pose: Pose }>;
  diagnostics: MotionDiagnostic[];
}
```

用途：

1. 机器人模型显示。
2. 未来碰撞检测前置数据。
3. 轨迹回放时显示每个连杆。

### 8.3 逆解 IK

```ts
interface IkOptions {
  tool?: Pose;
  frame?: Pose;
  qMin?: number[];
  qMax?: number[];
  maxIterations?: number;
  positionTolerance?: number;
  orientationTolerance?: number;
  seeds?: number[][];
  preferredConfig?: RobotConfiguration;
  allowApproximate?: boolean;
}

interface IkResult {
  ok: boolean;
  joints?: number[];
  iterations: number;
  residualPosition?: number;
  residualOrientation?: number;
  configuration?: RobotConfiguration;
  reason?: "unreachable" | "joint_limit" | "singularity" | "max_iteration" | "invalid_model";
  diagnostics: MotionDiagnostic[];
}
```

IK 使用规则：

1. `movej pose_target` 需要 IK 一次，求终点关节。
2. `movel` 每个 TCP 采样点需要 IK。
3. `movec` 每个圆弧采样点需要 IK。
4. `ikBatch` 用于批量路径可达性检查。
5. 连续轨迹中每个采样点的 seed 使用上一采样点关节，减少姿态跳变。

### 8.4 Jacobian 和奇异性

```ts
interface JacobianResult {
  ok: boolean;
  rows: number;
  cols: number;
  data: Float64Array;
  diagnostics: MotionDiagnostic[];
}

interface SingularityResult {
  ok: boolean;
  nearSingularity: boolean;
  manipulability?: number;
  conditionNumber?: number;
  diagnostics: MotionDiagnostic[];
}
```

用途：

1. 运动前诊断。
2. MOVEL/MOVEC 采样点奇异性警告。
3. 可达性报告和路径优化提示。

## 9. 梯形速度 API

GRL 速度和加速度需要转换为轨迹采样的路径参数。P0 阶段实现梯形速度曲线。

```ts
interface TrapProfileOptions {
  maxVelocity: number;
  maxAcceleration: number;
  sampleTime: number;
  startVelocity?: number;
  endVelocity?: number;
}

interface TrapSample {
  index: number;
  time: number;
  s: number;
  sd: number;
  sdd: number;
}

interface TrapProfileResult {
  ok: boolean;
  type: "trapezoid" | "triangle";
  length: number;
  duration: number;
  tAccel: number;
  tConst: number;
  tDecel: number;
  vPeak: number;
  samples: TrapSample[];
  diagnostics: MotionDiagnostic[];
}
```

规则：

1. `length` 为路径长度，单位 meter、radian 或归一化长度，由调用方按运动类型决定。
2. 距离足够长时生成梯形速度曲线。
3. 距离不足时自动退化为三角速度曲线。
4. 首末采样点必须严格对应 `s=0` 和 `s=1`。
5. 所有 `TrajectoryResult` 必须保留实际速度曲线采样，便于节拍报告。

## 10. 轨迹数据结构

```ts
interface TrajectoryPoint {
  index: number;
  time: number;
  dt: number;
  s: number;
  sd: number;
  sdd: number;
  joints: number[];
  jointVelocity: number[];
  jointAcceleration: number[];
  flange: Pose;
  tcp: Pose;
  tcpVelocity?: [number, number, number, number, number, number];
  tcpAcceleration?: [number, number, number, number, number, number];
  motion: "MOVEJ" | "MOVEL" | "MOVEC";
  segmentId?: string;
  targetId?: string;
  sourceMap?: MotionSourceMap;
  diagnostics: MotionDiagnostic[];
}

interface TrajectoryResult {
  ok: boolean;
  motion: "MOVEJ" | "MOVEL" | "MOVEC";
  duration: number;
  sampleTime: number;
  points: TrajectoryPoint[];
  events: TrajectoryEvent[];
  diagnostics: MotionDiagnostic[];
  meta?: Record<string, unknown>;
}
```

轨迹点用于：

1. 虚拟控制器 Motion Queue。
2. 3D 仿真回放。
3. 可达性报告。
4. 节拍报告。
5. 轨迹导出和 OPFS trace。

## 11. MOVEJ 计算函数

### 11.1 请求

```ts
interface MoveJRequest {
  startJoints: number[];
  target: JointTarget | PoseTarget;
  speed: SpeedSpec;
  zone: ZoneSpec;
  tool?: Pose;
  frame?: Pose;
  sampleTime: number;
  speedOverride?: number;
  sourceMap?: MotionSourceMap;
}
```

### 11.2 计算语义

`planMoveJ` 对应 GRL：

```text
movej TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame]
```

算法：

1. 校验 `startJoints` 长度和关节限位。
2. 如果 target 是 `joint_target`，直接得到 `qEnd`。
3. 如果 target 是 `pose_target`，先调用 IK 得到 `qEnd`。
4. 计算每个关节角度差 `dq[i] = qEnd[i] - qStart[i]`。
5. 根据关节速度、加速度限制计算同步运动时长。
6. 生成梯形速度曲线。
7. 对每个采样点计算关节位置、速度、加速度。
8. 对每个采样点 FK，输出 TCP。
9. 检查关节限位、速度、加速度和奇异性。

### 11.3 必须返回的诊断

1. 目标 IK 失败。
2. 起点或终点关节超限。
3. 采样点速度或加速度超限。
4. 接近奇异点。
5. `zone` 在 P0 阶段被近似为 fine。

## 12. MOVEL 计算函数

### 12.1 请求

```ts
interface MoveLRequest {
  startJoints: number[];
  target: PoseTarget;
  speed: SpeedSpec;
  zone: ZoneSpec;
  tool?: Pose;
  frame?: Pose;
  sampleTime: number;
  orientationMode?: "fixed" | "slerp" | "tool_z_lock";
  ik?: IkOptions;
  speedOverride?: number;
  sourceMap?: MotionSourceMap;
}
```

### 12.2 计算语义

`planMoveL` 对应 GRL：

```text
movel TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame]
```

算法：

1. 对 `startJoints` 做 FK，得到起点 TCP。
2. 将目标点应用 tool/frame/offset，得到终点 TCP。
3. 计算直线长度。
4. 使用 `linear` 速度生成梯形速度曲线。
5. 对每个采样点计算直线位置和姿态插补。
6. 对每个采样点 IK，seed 使用上一采样点关节。
7. 检查 IK 连续性、关节限位、速度、加速度、奇异性。
8. 输出轨迹和 TCP 直线误差。

### 12.3 必须返回的诊断

1. 目标不可达。
2. 某个采样点 IK 失败。
3. TCP 直线误差超过容差。
4. 姿态误差超过容差。
5. 关节配置突变。
6. 速度或加速度超限。

## 13. MOVEC 计算函数

### 13.1 请求

```ts
interface MoveCRequest {
  startJoints: number[];
  via: PoseTarget;
  target: PoseTarget;
  speed: SpeedSpec;
  zone: ZoneSpec;
  tool?: Pose;
  frame?: Pose;
  sampleTime: number;
  orientationMode?: "fixed" | "slerp";
  arcMode?: "via" | "center" | "radius";
  circleDirection?: "short" | "long" | "cw" | "ccw";
  ik?: IkOptions;
  speedOverride?: number;
  sourceMap?: MotionSourceMap;
}
```

### 13.2 计算语义

`planMoveC` 对应 GRL：

```text
movec via ViaTargetExpr target EndTargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame]
```

算法：

1. 对 `startJoints` 做 FK，得到起点 TCP。
2. 将 via 和 target 应用 tool/frame/offset。
3. 检查三点是否重合或共线。
4. 计算圆心、半径、法向量、圆弧角度和圆弧长度。
5. 使用 `linear` 速度按圆弧长度生成梯形速度曲线。
6. 对每个采样点计算圆弧 TCP 位姿。
7. 对每个采样点 IK，seed 使用上一采样点关节。
8. 检查圆弧误差、IK 连续性、关节限位、速度、加速度、奇异性。

### 13.3 圆弧元数据

```ts
interface CirclePlanMeta {
  center: [number, number, number];
  radius: number;
  normal: [number, number, number];
  angle: number;
  length: number;
  direction: "cw" | "ccw";
  maxArcError: number;
}
```

`TrajectoryResult.meta.circle` 必须包含 `CirclePlanMeta`。

### 13.4 必须返回的诊断

1. via 或 target 不可达。
2. 三点重合、近似重合或近似共线。
3. 半径过小或圆弧长度过短。
4. 某个采样点 IK 失败。
5. 圆弧误差超过容差。
6. 速度或加速度超限。

## 14. Path 和 Operation 批量接口

### 14.1 Path 请求

```ts
interface MotionSegmentRequest {
  id: string;
  motion: "MOVEJ" | "MOVEL" | "MOVEC";
  target?: JointTarget | PoseTarget;
  via?: PoseTarget;
  speed: SpeedSpec;
  zone: ZoneSpec;
  tool?: Pose;
  frame?: Pose;
  sourceMap?: MotionSourceMap;
}

interface PathPlanRequest {
  startJoints: number[];
  segments: MotionSegmentRequest[];
  sampleTime: number;
  speedOverride?: number;
  stopOnError?: boolean;
}
```

### 14.2 `planPath`

`planPath` 用于 `run_path` 展开后的整条路径轨迹生成：

1. 按 segment 顺序调用 `planMoveJ/planMoveL/planMoveC`。
2. 每段终点关节作为下一段起点。
3. 合并所有轨迹点，重新编号和更新时间。
4. 保留每段 `sourceMap`。
5. 生成整条路径的总时长。

```ts
interface PathPlanResult {
  ok: boolean;
  duration: number;
  segments: TrajectoryResult[];
  points: TrajectoryPoint[];
  diagnostics: MotionDiagnostic[];
}
```

### 14.3 `validatePath`

`validatePath` 用于离线编程路径验证，不要求必须返回完整轨迹点，可按配置只返回诊断：

```ts
interface PathValidationResult {
  ok: boolean;
  reachable: boolean;
  cycleTime?: number;
  segmentReports: SegmentValidationReport[];
  diagnostics: MotionDiagnostic[];
}

interface SegmentValidationReport {
  segmentId: string;
  ok: boolean;
  motion: "MOVEJ" | "MOVEL" | "MOVEC";
  duration?: number;
  maxJointVelocityRatio?: number;
  maxJointAccelerationRatio?: number;
  maxCartesianError?: number;
  diagnostics: MotionDiagnostic[];
}
```

`run_operation` 不需要 KDL WASM 单独理解工艺，只需要 TypeScript 把 operation 展开为 start action、path、end action。KDL WASM 只处理其中的 motion segment。

## 15. 可达性和批量检查

### 15.1 单点可达性

```ts
interface ReachabilityResult {
  ok: boolean;
  reachable: boolean;
  targetId?: string;
  joints?: number[];
  residualPosition?: number;
  residualOrientation?: number;
  nearestPose?: Pose;
  diagnostics: MotionDiagnostic[];
}
```

### 15.2 批量可达性

`checkReachabilityBatch` 用于：

1. Path 编辑器批量目标点检查。
2. CAD 曲线采样点预检查。
3. 自动编程生成后快速诊断。
4. 品牌程序导入后的目标点检查。

批量函数必须保持输入顺序，返回结果与输入 target 一一对应。

## 16. 节拍估算

```ts
interface CycleTimeResult {
  ok: boolean;
  motionTime: number;
  waitTime?: number;
  ioTime?: number;
  totalTime: number;
  segmentTimes: Array<{
    segmentId?: string;
    motion: "MOVEJ" | "MOVEL" | "MOVEC";
    duration: number;
  }>;
  diagnostics: MotionDiagnostic[];
}
```

KDL WASM 只估算运动时间。`waitTime`、IO 脚本延迟、工艺设备延迟由虚拟控制器补充。

## 17. Worker RPC 协议

主线程和 Worker 建议使用统一消息格式：

```ts
interface KdlRpcRequest<T = unknown> {
  id: number;
  method: keyof KdlWasmApi;
  payload: T;
}

interface KdlRpcResponse<T = unknown> {
  id: number;
  ok: boolean;
  result?: T;
  error?: {
    code: string;
    message: string;
    diagnostics?: MotionDiagnostic[];
  };
}
```

要求：

1. 大数组使用 Transferable 或共享内存策略，避免频繁复制。
2. 每个请求必须有唯一 id。
3. Worker 崩溃或 WASM 初始化失败时，主线程能恢复并重新初始化。
4. 对长路径计算提供进度回调或分块计算，避免 Worker 长时间无响应。

## 18. C ABI / Embind 暴露建议

不建议把 KDL C++ 类完整暴露给 TypeScript。建议底层导出少量稳定函数：

```cpp
extern "C" {
  int kdl_init(const char* options_json);
  int kdl_create_robot(const char* model_json);
  int kdl_destroy_robot(int robot_handle);
  int kdl_get_robot_info(int robot_handle, char* out_json, int out_len);

  int kdl_fk(int robot_handle, const double* joints, int n, double* out_pose7);
  int kdl_fk_all_links(int robot_handle, const double* joints, int n, char* out_json, int out_len);
  int kdl_jacobian(int robot_handle, const double* joints, int n, double* out_matrix);
  int kdl_ik(int robot_handle, const double* seed, int n, const double* target_pose7, const char* options_json, double* out_joints);

  int kdl_plan_movej(int robot_handle, const char* request_json, char* out_json, int out_len);
  int kdl_plan_movel(int robot_handle, const char* request_json, char* out_json, int out_len);
  int kdl_plan_movec(int robot_handle, const char* request_json, char* out_json, int out_len);
  int kdl_plan_path(int robot_handle, const char* request_json, char* out_json, int out_len);

  int kdl_sample_trap(double length, const char* options_json, char* out_json, int out_len);
  int kdl_last_error(char* out_json, int out_len);
}
```

说明：

1. P0 可用 JSON 输入输出实现，简单可靠。
2. 高频 FK/IK 批量计算可增加 TypedArray 版本，减少 JSON 开销。
3. TypeScript API 层负责把 C ABI 包装成 Promise。
4. 所有 C ABI 返回 `0` 表示成功，非 `0` 表示错误，错误详情通过 `kdl_last_error` 获取。

## 19. 内存和性能要求

首版目标：

1. 单机器人 6 轴模型初始化小于 1 秒。
2. 单次 FK 小于 1 ms。
3. 单次 IK 平均小于 10 ms，复杂点允许更长但必须有超时。
4. 1000 个目标点批量可达性检查可在可接受交互时间内完成。
5. 10 秒轨迹按 4 ms 采样约 2500 点，必须能稳定生成和回放。

实现建议：

1. `RobotHandle` 内缓存 FK、IK、Jacobian solver。
2. `ikBatch` 中复用上一点结果作为 seed。
3. 对长路径分段计算，及时返回进度。
4. 避免每个采样点跨 Worker 往返，轨迹整段在 Worker 内完成。
5. 大轨迹 trace 写 OPFS 由 TypeScript 层完成，WASM 不直接管理项目文件。

## 20. 错误处理

所有 API 不抛裸字符串错误，必须返回结构化错误：

```ts
interface KdlError {
  code: string;
  message: string;
  diagnostics: MotionDiagnostic[];
}
```

错误分级：

1. `error`：不能生成可执行轨迹，例如 IK 失败、模型非法。
2. `warning`：可生成轨迹但存在风险，例如接近限位、zone 被近似。
3. `info`：辅助信息，例如使用了三角速度曲线。

虚拟控制器处理规则：

1. `error`：进入 alarm 或 hold。
2. `warning`：允许仿真继续，但报告中必须显示。
3. `info`：写入 trace 或调试面板。

## 21. 与 GRL 的调用关系

| GRL 语法 | TypeScript 编译结果 | KDL WASM 函数 |
| --- | --- | --- |
| `target home = joint_target` | `JointTarget` | `checkJointLimits` |
| `target pick = pose_target` | `PoseTarget` | `checkReachability` |
| `pick offset z 100 mm` | `OffsetSpec` | `applyOffset` |
| `movej home` | `MoveJRequest` | `planMoveJ` |
| `movel pick` | `MoveLRequest` | `planMoveL` |
| `movec via mid target end` | `MoveCRequest` | `planMoveC` |
| `run_path pick_path` | `PathPlanRequest` | `planPath` |
| Path 可达性检查 | `PathPlanRequest` | `validatePath` |
| 节拍报告 | `TrajectoryResult/PathPlanResult` | `estimateCycleTime` |

`if/for/switch/call/wait/io` 不直接调用 KDL WASM，但它们会影响何时调用运动函数和当前运行上下文。

### 21.1 不应暴露给 KDL WASM 的 GRL 语义

以下 GRL 语义由 TypeScript 虚拟控制器执行，不进入 KDL WASM：

| GRL 语义 | 执行位置 | 说明 |
| --- | --- | --- |
| `if/elseif/else` | 虚拟控制器 | 判断分支，决定是否执行后续运动 |
| `while/for/switch` | 虚拟控制器 | 控制程序流，可能多次触发运动函数 |
| `proc/func/call/return` | 虚拟控制器 | 调用栈、参数、作用域不属于 KDL |
| `io.do/di/ai/ao` | IO Service | KDL 不管理 IO 状态 |
| `wait/pulse/timer` | 虚拟控制器 + IO Service | KDL 不阻塞等待 IO |
| `alarm/raise/try/catch` | 虚拟控制器 | KDL 只返回诊断，不执行异常流程 |
| `operation.process` | 工艺模块 | KDL 只处理 operation 展开后的 motion segment |

边界原则：KDL WASM 只接收已经解析好的运动请求，不读取 GRL 源码，不维护程序变量，不执行子程序。

## 22. P0 必须暴露的函数

P0 阶段必须完成以下函数：

1. `init`
2. `loadRobotFromUrdf`
3. `createRobotFromModel`
4. `destroyRobot`
5. `getRobotInfo`
6. `getJointLimits`
7. `normalizePose`
8. `composePose`
9. `inversePose`
10. `applyOffset`
11. `applyToolAndFrame`
12. `fk`
13. `fkAllLinks`
14. `jacobian`
15. `ik`
16. `ikBatch`
17. `checkJointLimits`
18. `checkSingularity`
19. `checkReachability`
20. `checkReachabilityBatch`
21. `makeTrapProfile`
22. `sampleTrapProfile`
23. `planMoveJ`
24. `planMoveL`
25. `planMoveC`
26. `planPath`
27. `validatePath`
28. `estimateCycleTime`
29. `resampleTrajectory`

P1 扩展：

1. `planBlendPath`
2. `planMoveSpline`
3. `checkCollisionInputPoses`，只提供 link poses，不做碰撞本身。
4. `optimizeSeedSequence`
5. `compareTrajectory`
6. 外部轴协调相关函数。

## 23. 测试要求

### 23.1 单元测试

1. URDF 到 KDL Chain 的 joint 顺序测试。
2. FK 与原生 KDL 结果对比。
3. IK 后再 FK，误差小于容差。
4. Jacobian 尺寸和数值测试。
5. 梯形速度曲线长距离/短距离测试。
6. `applyOffset` 在 frame/tool/world 三种模式下测试。

### 23.2 运动测试

1. `planMoveJ` 关节同起同停。
2. `planMoveL` TCP 直线误差小于容差。
3. `planMoveC` 圆心、半径、弧长和圆弧误差正确。
4. `planMoveC` 三点共线时返回 `KDL_ARC_DEGENERATE`。
5. 每种运动都检查速度、加速度、限位和 source map。

### 23.3 集成测试

1. 从 GRL `movej/movel/movec` 编译为请求并生成轨迹。
2. 从 GRL `path` 编译为 `PathPlanRequest` 并生成整条路径。
3. 轨迹写入 OPFS 后重新加载回放。
4. 长路径批量验证性能测试。
5. Worker 初始化、崩溃恢复和取消请求测试。

## 24. 实施顺序

建议按以下顺序开发：

1. WASM 工程骨架和 Worker RPC。
2. `NormalizedRobotModel` 到 KDL Chain。
3. FK、fkAllLinks。
4. IK、ikBatch。
5. Jacobian 和奇异性。
6. 位姿变换和 offset。
7. 梯形速度曲线。
8. planMoveJ。
9. planMoveL。
10. planMoveC。
11. planPath、validatePath。
12. 节拍估算和诊断报告。
13. 性能优化和 TypedArray 批量接口。

## 25. 结论

KDL WASM 对 GRL 的定位是“运动学和轨迹计算内核”。它不解释完整机器人程序，也不处理 IO/Wait/流程控制。TypeScript 虚拟控制器负责执行 GRL/IR，当遇到运动相关语义时，把已经解析好的机器人模型、当前关节、目标点、速度、zone、tool、frame 传给 KDL WASM。

P0 阶段只要稳定实现 URDF 模型加载、FK/IK/Jacobian、MOVEJ/MOVEL/MOVEC、梯形速度、Path 批量验证和诊断，就可以支撑通用机器人程序的离线编程、虚拟运行、轨迹回放和多品牌后处理。