wangdequan/KDL_WORK

Fork 0

Files

wangdequan 95c684fc4d 同步KDL工程源码到云仓库

2026-06-27 08:45:38 -04:00

29 KiB

Raw Blame History

KDL WASM 计算接口设计

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

1. 目标

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

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

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

KDL WASM 不负责：

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

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

2. 总体架构

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

设计原则：

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

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

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

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 句柄和运行信息

type RobotHandle = number;

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

5.2 位姿

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

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 目标点

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

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 诊断

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 加载

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;
  };
}

推荐实现分工：

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

6.2 机器人信息

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 提供一致的计算函数，避免数值约定不一致。

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

函数要求：

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

8. FK、IK、Jacobian API

8.1 正解 FK

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

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

用途：

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

8.2 全 link 正解

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

用途：

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

8.3 逆解 IK

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 使用规则：

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

8.4 Jacobian 和奇异性

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

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

用途：

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

9. 梯形速度 API

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

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[];
}

规则：

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

10. 轨迹数据结构

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>;
}

轨迹点用于：

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

11. MOVEJ 计算函数

11.1 请求

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：

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

算法：

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

11.3 必须返回的诊断

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

12. MOVEL 计算函数

12.1 请求

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：

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

算法：

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

12.3 必须返回的诊断

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

13. MOVEC 计算函数

13.1 请求

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：

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

算法：

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

13.3 圆弧元数据

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 必须返回的诊断

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

14. Path 和 Operation 批量接口

14.1 Path 请求

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 展开后的整条路径轨迹生成：

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

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

14.3 `validatePath`

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

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 单点可达性

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

15.2 批量可达性

checkReachabilityBatch 用于：

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

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

16. 节拍估算

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 建议使用统一消息格式：

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[];
  };
}

要求：

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

18. C ABI / Embind 暴露建议

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

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);
}

说明：

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

19. 内存和性能要求

首版目标：

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

实现建议：

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

20. 错误处理

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

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

错误分级：

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

虚拟控制器处理规则：

error：进入 alarm 或 hold。
warning：允许仿真继续，但报告中必须显示。
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 阶段必须完成以下函数：

init
loadRobotFromUrdf
createRobotFromModel
destroyRobot
getRobotInfo
getJointLimits
normalizePose
composePose
inversePose
applyOffset
applyToolAndFrame
fk
fkAllLinks
jacobian
ik
ikBatch
checkJointLimits
checkSingularity
checkReachability
checkReachabilityBatch
makeTrapProfile
sampleTrapProfile
planMoveJ
planMoveL
planMoveC
planPath
validatePath
estimateCycleTime
resampleTrajectory

P1 扩展：

planBlendPath
planMoveSpline
checkCollisionInputPoses，只提供 link poses，不做碰撞本身。
optimizeSeedSequence
compareTrajectory
外部轴协调相关函数。

23. 测试要求

23.1 单元测试

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

23.2 运动测试

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

23.3 集成测试

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

24. 实施顺序

建议按以下顺序开发：

WASM 工程骨架和 Worker RPC。
NormalizedRobotModel 到 KDL Chain。
FK、fkAllLinks。
IK、ikBatch。
Jacobian 和奇异性。
位姿变换和 offset。
梯形速度曲线。
planMoveJ。
planMoveL。
planMoveC。
planPath、validatePath。
节拍估算和诊断报告。
性能优化和 TypedArray 批量接口。

25. 结论

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

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

29 KiB Raw Blame History Unescape Escape