1033 lines
29 KiB
Markdown
1033 lines
29 KiB
Markdown
# 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 批量验证和诊断，就可以支撑通用机器人程序的离线编程、虚拟运行、轨迹回放和多品牌后处理。
|