同步KDL工程源码到云仓库
This commit is contained in:
wangdequan
197
kdl-wasm/web/tests/grl/controlFlow.test.ts
Normal file
197
kdl-wasm/web/tests/grl/controlFlow.test.ts
Normal file
@@ -0,0 +1,197 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { GrlProcedureDeclaration } from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { parseProcedureControlFlow } from "../../src/grl/semantic/index.js";
|
||||
|
||||
function procedure(source: string): GrlProcedureDeclaration {
|
||||
return parseGrl(source).module.declarations.find(
|
||||
(decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration"
|
||||
)!;
|
||||
}
|
||||
|
||||
describe("GRL control-flow compilation", () => {
|
||||
it("compiles if, elseif, else, while, for, switch, labels, and jumps", () => {
|
||||
const proc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
label retry
|
||||
if ready == true
|
||||
wait io.di[1] == true
|
||||
elseif fault == true
|
||||
jump recovery
|
||||
else
|
||||
jump retry
|
||||
end
|
||||
while all(io.di[1] == true, io.di[2] == false)
|
||||
continue
|
||||
end
|
||||
for i = 1 to 3 step 1
|
||||
movej home
|
||||
end
|
||||
switch mode
|
||||
case 1
|
||||
break
|
||||
case 2
|
||||
jump done
|
||||
default
|
||||
jump recovery
|
||||
end
|
||||
label recovery
|
||||
label done
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
const flow = parseProcedureControlFlow(proc);
|
||||
|
||||
expect(flow).toEqual([
|
||||
expect.objectContaining({ kind: "LABEL", name: "retry", scopePath: [] }),
|
||||
expect.objectContaining({
|
||||
kind: "IF",
|
||||
branches: [
|
||||
expect.objectContaining({
|
||||
branchKind: "if",
|
||||
condition: expect.objectContaining({ text: "ready == true" }),
|
||||
body: [expect.objectContaining({ kind: "RAW_STATEMENT", text: "wait io . di [ 1 ] == true" })]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
branchKind: "elseif",
|
||||
condition: expect.objectContaining({ text: "fault == true" }),
|
||||
body: [expect.objectContaining({ kind: "JUMP", label: "recovery" })]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
branchKind: "else",
|
||||
body: [expect.objectContaining({ kind: "JUMP", label: "retry" })]
|
||||
})
|
||||
]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
kind: "WHILE",
|
||||
condition: expect.objectContaining({
|
||||
text: "all ( io . di [ 1 ] == true , io . di [ 2 ] == false )"
|
||||
}),
|
||||
body: [expect.objectContaining({ kind: "CONTINUE" })]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
kind: "FOR",
|
||||
iterator: "i",
|
||||
from: expect.objectContaining({ text: "1" }),
|
||||
to: expect.objectContaining({ text: "3" }),
|
||||
step: expect.objectContaining({ text: "1" }),
|
||||
body: [expect.objectContaining({ kind: "RAW_STATEMENT", text: "movej home" })]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
kind: "SWITCH",
|
||||
expression: expect.objectContaining({ text: "mode" }),
|
||||
cases: [
|
||||
expect.objectContaining({ caseKind: "case", value: 1, body: [expect.objectContaining({ kind: "BREAK" })] }),
|
||||
expect.objectContaining({ caseKind: "case", value: 2, body: [expect.objectContaining({ kind: "JUMP", label: "done" })] }),
|
||||
expect.objectContaining({ caseKind: "default", body: [expect.objectContaining({ kind: "JUMP", label: "recovery" })] })
|
||||
]
|
||||
}),
|
||||
expect.objectContaining({ kind: "LABEL", name: "recovery", scopePath: [] }),
|
||||
expect.objectContaining({ kind: "LABEL", name: "done", scopePath: [] })
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports non-boolean control conditions", () => {
|
||||
const proc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
if 1
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => parseProcedureControlFlow(proc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_CONTROL_CONDITION_NOT_BOOL" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports break and continue outside valid blocks", () => {
|
||||
const breakProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
break
|
||||
end
|
||||
end
|
||||
`);
|
||||
const continueProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
switch mode
|
||||
case 1
|
||||
continue
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => parseProcedureControlFlow(breakProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_BREAK_OUTSIDE_FLOW" })
|
||||
);
|
||||
expect(() => parseProcedureControlFlow(continueProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_CONTINUE_OUTSIDE_LOOP" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports duplicate or non-constant switch cases", () => {
|
||||
const duplicateProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
switch mode
|
||||
case 1
|
||||
break
|
||||
case 1
|
||||
break
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
const nonConstantProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
switch mode
|
||||
case mode + 1
|
||||
break
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => parseProcedureControlFlow(duplicateProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_SWITCH_CASE_DUPLICATE" })
|
||||
);
|
||||
expect(() => parseProcedureControlFlow(nonConstantProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_SWITCH_CASE_NOT_CONSTANT" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports labels that cannot be reached by jump", () => {
|
||||
const intoBlockProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
jump inner
|
||||
if ready == true
|
||||
label inner
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
const missingLabelProc = procedure(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
jump missing
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => parseProcedureControlFlow(intoBlockProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_JUMP_INTO_BLOCK" })
|
||||
);
|
||||
expect(() => parseProcedureControlFlow(missingLabelProc)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_LABEL_NOT_FOUND" })
|
||||
);
|
||||
});
|
||||
});
|
||||
151
kdl-wasm/web/tests/grl/dataDeclarations.test.ts
Normal file
151
kdl-wasm/web/tests/grl/dataDeclarations.test.ts
Normal file
@@ -0,0 +1,151 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { GrlDataDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
compileGrlDataDeclaration,
|
||||
compileGrlTargetDeclaration,
|
||||
compileOffsetExpression
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
function dataDeclarations(source: string): GrlDataDeclaration[] {
|
||||
return parseGrl(source).module.declarations.filter(
|
||||
(declaration): declaration is GrlDataDeclaration => declaration.kind === "DataDeclaration"
|
||||
);
|
||||
}
|
||||
|
||||
function targetDeclarations(source: string): GrlTargetDeclaration[] {
|
||||
return parseGrl(source).module.declarations.filter(
|
||||
(declaration): declaration is GrlTargetDeclaration => declaration.kind === "TargetDeclaration"
|
||||
);
|
||||
}
|
||||
|
||||
describe("GRL data declarations and target compilation", () => {
|
||||
it("compiles tool and frame declarations into shared structures", () => {
|
||||
const [toolDecl, frameDecl] = dataDeclarations(`language grl 0.1
|
||||
module Main
|
||||
persistent tool gripper = tool {
|
||||
tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg),
|
||||
mass: 2.5 kg,
|
||||
cog: [0 mm, 0 mm, 80 mm]
|
||||
}
|
||||
persistent frame fixture = frame {
|
||||
origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
end
|
||||
`);
|
||||
|
||||
expect(toolDecl).toMatchObject({
|
||||
storage: "persistent",
|
||||
typeName: "tool",
|
||||
name: "gripper",
|
||||
initializer: { kind: "ObjectExpression", typeName: "tool" }
|
||||
});
|
||||
expect(compileGrlDataDeclaration(toolDecl!)).toMatchObject({
|
||||
name: "gripper",
|
||||
value: {
|
||||
tcp: {
|
||||
position: [0, 0, 0.18],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
},
|
||||
mass: 2.5,
|
||||
cog: [0, 0, 0.08]
|
||||
}
|
||||
});
|
||||
expect(compileGrlDataDeclaration(frameDecl!)).toMatchObject({
|
||||
name: "fixture",
|
||||
value: {
|
||||
origin: {
|
||||
position: [0.8, 0, 0.2],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
it("compiles speed and zone declarations", () => {
|
||||
const declarations = dataDeclarations(`language grl 0.1
|
||||
module Main
|
||||
const speed v_joint = joint(80 %)
|
||||
const speed v_pick = linear(300 mm/s)
|
||||
const speed v_slow = linear(100 mm/s, acc 500 mm/s2)
|
||||
const zone z_fine = fine
|
||||
const zone z10 = z(10 mm)
|
||||
const zone z_cnt = cnt(30)
|
||||
const zone z_cont = continuous
|
||||
end
|
||||
`);
|
||||
const compiled = declarations.map(compileGrlDataDeclaration);
|
||||
|
||||
expect(compiled).toMatchObject([
|
||||
{ name: "v_joint", value: { kind: "joint_percent", value: 0.8 } },
|
||||
{ name: "v_pick", value: { kind: "linear", velocity: 0.3 } },
|
||||
{ name: "v_slow", value: { kind: "linear", velocity: 0.1, acceleration: 0.5 } },
|
||||
{ name: "z_fine", value: { kind: "fine" } },
|
||||
{ name: "z10", value: { kind: "distance", value: 0.01 } },
|
||||
{ name: "z_cnt", value: { kind: "cnt", value: 30 } },
|
||||
{ name: "z_cont", value: { kind: "continuous" } }
|
||||
]);
|
||||
});
|
||||
|
||||
it("compiles joint_target and pose_target declarations", () => {
|
||||
const [home, pick] = targetDeclarations(`language grl 0.1
|
||||
module Main
|
||||
target home = joint_target {
|
||||
joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg]
|
||||
}
|
||||
target pick = pose_target {
|
||||
pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg),
|
||||
config: robot_config(0, 0, 1),
|
||||
tool: gripper,
|
||||
frame: fixture
|
||||
}
|
||||
end
|
||||
`);
|
||||
|
||||
expect(compileGrlTargetDeclaration(home!)).toMatchObject({
|
||||
name: "home",
|
||||
target: {
|
||||
joints: [0, -Math.PI / 6, Math.PI / 3, 0, Math.PI / 3, 0]
|
||||
}
|
||||
});
|
||||
|
||||
const compiledPick = compileGrlTargetDeclaration(pick!);
|
||||
expect(compiledPick.name).toBe("pick");
|
||||
expect("pose" in compiledPick.target).toBe(true);
|
||||
if ("pose" in compiledPick.target) {
|
||||
expect(compiledPick.target.pose.position).toEqual([0.5, 0.12, 0.3]);
|
||||
expect(compiledPick.target.config).toEqual({ shoulder: 0, elbow: 0, wrist: 1 });
|
||||
}
|
||||
});
|
||||
|
||||
it("parses and compiles offset expressions", () => {
|
||||
const [declFrame, declTool] = dataDeclarations(`language grl 0.1
|
||||
module Main
|
||||
var pose_target p2 = pick offset x 20 mm y -10 mm z 50 mm
|
||||
var pose_target p3 = pick offset_in tool z -50 mm
|
||||
end
|
||||
`);
|
||||
|
||||
expect(declFrame?.initializer).toMatchObject({
|
||||
kind: "OffsetExpression",
|
||||
mode: "frame",
|
||||
axes: [
|
||||
{ axis: "x" },
|
||||
{ axis: "y" },
|
||||
{ axis: "z" }
|
||||
]
|
||||
});
|
||||
if (declFrame?.initializer.kind === "OffsetExpression") {
|
||||
expect(compileOffsetExpression(declFrame.initializer)).toEqual({
|
||||
mode: "frame",
|
||||
xyz: [0.02, -0.01, 0.05]
|
||||
});
|
||||
}
|
||||
if (declTool?.initializer.kind === "OffsetExpression") {
|
||||
expect(compileOffsetExpression(declTool.initializer)).toEqual({
|
||||
mode: "tool",
|
||||
xyz: [0, 0, -0.05]
|
||||
});
|
||||
}
|
||||
});
|
||||
});
|
||||
125
kdl-wasm/web/tests/grl/exceptionCompile.test.ts
Normal file
125
kdl-wasm/web/tests/grl/exceptionCompile.test.ts
Normal file
@@ -0,0 +1,125 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { GrlProcedureDeclaration, GrlRawTopLevelDeclaration } from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
analyzeExceptionSemantics,
|
||||
parseProcedureExceptionFlow
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
trap recover_trap()
|
||||
raise E_STOP
|
||||
end
|
||||
task background cycle 10 ms
|
||||
call monitor()
|
||||
end
|
||||
proc main()
|
||||
alarm E_STOP "Emergency stop" severity fatal
|
||||
try
|
||||
raise E_STOP
|
||||
catch E_STOP
|
||||
alarm RECOVER "Recovering" severity warning
|
||||
finally
|
||||
alarm CLEANUP "Cleanup"
|
||||
end
|
||||
enable interrupt guard
|
||||
disable interrupt guard
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
function declarations() {
|
||||
return parseGrl(PROGRAM).module.declarations;
|
||||
}
|
||||
|
||||
describe("GRL alarm, raise, try/catch, interrupt, and task semantics", () => {
|
||||
it("keeps trap and task as parsed raw declarations for P1 diagnostics", () => {
|
||||
const raw = declarations().filter(
|
||||
(decl): decl is GrlRawTopLevelDeclaration => decl.kind === "RawTopLevelDeclaration"
|
||||
);
|
||||
|
||||
expect(raw[0]?.declarationType).toBe("trap");
|
||||
expect(raw[0]?.tokens[0]).toMatchObject({ raw: "trap" });
|
||||
expect(raw[0]?.tokens[1]).toMatchObject({ raw: "recover_trap" });
|
||||
expect(raw[1]?.declarationType).toBe("task");
|
||||
expect(raw[1]?.tokens[0]).toMatchObject({ raw: "task" });
|
||||
expect(raw[1]?.tokens[1]).toMatchObject({ raw: "background" });
|
||||
});
|
||||
|
||||
it("compiles alarm, raise, try/catch/finally, and interrupt diagnostics from procedure body", () => {
|
||||
const procedure = declarations().find(
|
||||
(decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration"
|
||||
)!;
|
||||
|
||||
expect(parseProcedureExceptionFlow(procedure)).toEqual([
|
||||
expect.objectContaining({
|
||||
kind: "ALARM",
|
||||
alarmId: "E_STOP",
|
||||
message: "Emergency stop",
|
||||
severity: "fatal"
|
||||
}),
|
||||
expect.objectContaining({
|
||||
kind: "TRY",
|
||||
body: [expect.objectContaining({ kind: "RAISE", alarmId: "E_STOP" })],
|
||||
catches: [
|
||||
expect.objectContaining({
|
||||
alarmId: "E_STOP",
|
||||
body: [
|
||||
expect.objectContaining({
|
||||
kind: "ALARM",
|
||||
alarmId: "RECOVER",
|
||||
message: "Recovering",
|
||||
severity: "warning"
|
||||
})
|
||||
]
|
||||
})
|
||||
],
|
||||
finally: expect.objectContaining({
|
||||
body: [expect.objectContaining({ kind: "ALARM", alarmId: "CLEANUP", message: "Cleanup" })]
|
||||
})
|
||||
}),
|
||||
expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "interrupt" }),
|
||||
expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "interrupt" })
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports P1 trap/task semantics as explicit unsupported diagnostics", () => {
|
||||
const analysis = analyzeExceptionSemantics(declarations());
|
||||
|
||||
expect(analysis.unsupported).toEqual([
|
||||
expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "trap" }),
|
||||
expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "task" })
|
||||
]);
|
||||
expect(analysis.diagnostics).toEqual([
|
||||
expect.objectContaining({ severity: "warning", code: "GRL_P1_UNIMPLEMENTED" }),
|
||||
expect.objectContaining({ severity: "warning", code: "GRL_P1_UNIMPLEMENTED" })
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports missing alarm ids and try blocks without handlers", () => {
|
||||
const missingAlarmId = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
alarm
|
||||
end
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
const tryWithoutHandler = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
try
|
||||
raise E_STOP
|
||||
end
|
||||
end
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
|
||||
expect(() => parseProcedureExceptionFlow(missingAlarmId)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_ALARM_ID_MISSING" })
|
||||
);
|
||||
expect(() => parseProcedureExceptionFlow(tryWithoutHandler)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_TRY_HANDLER_MISSING" })
|
||||
);
|
||||
});
|
||||
});
|
||||
149
kdl-wasm/web/tests/grl/generator/roundtrip.test.ts
Normal file
149
kdl-wasm/web/tests/grl/generator/roundtrip.test.ts
Normal file
@@ -0,0 +1,149 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { generateGrlProgram, type GrlProgramGenerationSpec } from "../../../src/grl/generator/index.js";
|
||||
import { postProcessAllBrands } from "../../../src/grl/post/index.js";
|
||||
import type { GrlOperationDeclaration, GrlPathDeclaration, GrlTargetDeclaration } from "../../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../../src/grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../../../src/grl/semantic/index.js";
|
||||
|
||||
const SPEC: GrlProgramGenerationSpec = {
|
||||
moduleName: "GeneratedCell",
|
||||
speeds: {
|
||||
v_linear: "linear(200 mm/s)",
|
||||
v_joint: "joint(50 %)"
|
||||
},
|
||||
zones: {
|
||||
z10: "z(10 mm)",
|
||||
zf: "fine"
|
||||
},
|
||||
targets: [
|
||||
{ name: "pick", kind: "pose", values: [500, 0, 0, 0, 0, 0] },
|
||||
{ name: "home", kind: "joint", values: [0] },
|
||||
{ name: "place", kind: "pose", values: [600, 0, 0, 0, 0, 0] }
|
||||
],
|
||||
path: {
|
||||
name: "generated_path",
|
||||
source: {
|
||||
type: "cad_curve",
|
||||
id: "edge_001",
|
||||
sample_distance: 5
|
||||
},
|
||||
defaults: {
|
||||
speed: "v_linear",
|
||||
zone: "z10"
|
||||
},
|
||||
points: [
|
||||
{ motion: "movej", target: "home", speed: "v_joint", zone: "zf" },
|
||||
{ motion: "movel", target: "pick" },
|
||||
{ id: "place_point", motion: "movel", target: "place", zone: "zf" }
|
||||
]
|
||||
},
|
||||
operation: {
|
||||
name: "generated_op",
|
||||
kind: "handling",
|
||||
path: "generated_path",
|
||||
startAction: "io.do[1] = true",
|
||||
endAction: "io.do[1] = false"
|
||||
}
|
||||
};
|
||||
|
||||
describe("GRL generator and roundtrip", () => {
|
||||
it("generates stable expanded GRL with target/path/operation first", () => {
|
||||
const first = generateGrlProgram(SPEC, "expanded");
|
||||
const second = generateGrlProgram(SPEC, "expanded");
|
||||
|
||||
expect(first).toEqual(second);
|
||||
expect(first.stableIds).toEqual({
|
||||
targets: ["home", "pick", "place"],
|
||||
points: ["p00", "p01", "place_point"],
|
||||
path: "generated_path",
|
||||
operation: "generated_op"
|
||||
});
|
||||
expect(first.text).toBe(`language grl 0.1
|
||||
module GeneratedCell
|
||||
const speed v_joint = joint(50 %)
|
||||
const speed v_linear = linear(200 mm/s)
|
||||
const zone z10 = z(10 mm)
|
||||
const zone zf = fine
|
||||
target home = joint_target {
|
||||
joints: [0 deg]
|
||||
}
|
||||
target pick = pose_target {
|
||||
pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
target place = pose_target {
|
||||
pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
path generated_path {
|
||||
source {
|
||||
id: "edge_001"
|
||||
sample_distance: 5
|
||||
type: cad_curve
|
||||
}
|
||||
defaults {
|
||||
speed: v_linear
|
||||
zone: z10
|
||||
}
|
||||
point p00 movej home speed v_joint zone zf
|
||||
point p01 movel pick
|
||||
point place_point movel place zone zf
|
||||
}
|
||||
operation generated_op {
|
||||
kind: handling
|
||||
path: generated_path
|
||||
start_action:
|
||||
io.do[1] = true
|
||||
end_action:
|
||||
io.do[1] = false
|
||||
}
|
||||
proc main()
|
||||
run_operation generated_op
|
||||
end
|
||||
end`);
|
||||
});
|
||||
|
||||
it("supports compact output that remains parseable", () => {
|
||||
const compact = generateGrlProgram(SPEC, "compact");
|
||||
|
||||
expect(compact.text).toContain("path generated_path { source { id: \"edge_001\"");
|
||||
expect(parseGrl(compact.text).module.name).toBe("GeneratedCell");
|
||||
});
|
||||
|
||||
it("roundtrips through parser, semantic IR, and postprocessors", () => {
|
||||
const generated = generateGrlProgram(SPEC, "expanded");
|
||||
const ast = parseGrl(generated.text);
|
||||
const declarations = ast.module.declarations;
|
||||
const targets = declarations.filter((decl): decl is GrlTargetDeclaration => decl.kind === "TargetDeclaration");
|
||||
const path = declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const operation = declarations.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!;
|
||||
const ir = compileSemanticProgram(ast, {
|
||||
startJoints: [0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
const post = postProcessAllBrands(ir);
|
||||
|
||||
expect(targets.map((target) => target.name)).toEqual(["home", "pick", "place"]);
|
||||
expect(path.items).toEqual(
|
||||
expect.arrayContaining([
|
||||
expect.objectContaining({ kind: "PathSourceBlock" }),
|
||||
expect.objectContaining({ kind: "PathDefaultsBlock" }),
|
||||
expect.objectContaining({ kind: "PathPoint", id: "p00" }),
|
||||
expect.objectContaining({ kind: "PathPoint", id: "p01" }),
|
||||
expect.objectContaining({ kind: "PathPoint", id: "place_point" })
|
||||
])
|
||||
);
|
||||
expect(operation).toMatchObject({
|
||||
name: "generated_op",
|
||||
operationKind: "handling",
|
||||
pathName: "generated_path"
|
||||
});
|
||||
expect(ir.symbols).toEqual(
|
||||
expect.arrayContaining([
|
||||
expect.objectContaining({ kind: "path", name: "generated_path" }),
|
||||
expect.objectContaining({ kind: "operation", name: "generated_op" })
|
||||
])
|
||||
);
|
||||
expect(post.outputs.abb.text).toContain("MODULE GeneratedCell");
|
||||
expect(post.outputs.fanuc.text).toContain("/PROG MAIN");
|
||||
expect(post.outputs.kuka.text).toContain("DEF Main()");
|
||||
});
|
||||
});
|
||||
205
kdl-wasm/web/tests/grl/ioCompile.test.ts
Normal file
205
kdl-wasm/web/tests/grl/ioCompile.test.ts
Normal file
@@ -0,0 +1,205 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type {
|
||||
GrlDataDeclaration,
|
||||
GrlOperationDeclaration,
|
||||
GrlPathDeclaration,
|
||||
GrlProcedureDeclaration,
|
||||
GrlTargetDeclaration
|
||||
} from "../../src/grl/ast/index.js";
|
||||
import type { PathEventInstruction } from "../../src/grl/ir/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
buildMotionContext,
|
||||
compileOperation,
|
||||
compileOperationActionIo,
|
||||
compilePathEventIo,
|
||||
compilePathToPlanRequest,
|
||||
parseIoFlowStatements,
|
||||
type IoMap
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
path io_path {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p0 movej home
|
||||
event at p0 distance 0 mm pulse io.do[20] duration 100 ms
|
||||
}
|
||||
operation io_op {
|
||||
kind: handling
|
||||
path: io_path
|
||||
start_action:
|
||||
wait io.di[4] == true timeout 500 ms on_timeout alarm "part missing"
|
||||
end_action:
|
||||
pulse io.do[5] duration 250 ms
|
||||
}
|
||||
proc main()
|
||||
io.do[1] = true
|
||||
io.go[2] = 16
|
||||
io.alias.grip_close = false
|
||||
wait all(io.di[1] == true, io.di[2] == false) timeout 2 s on_timeout alarm "Clamp close timeout"
|
||||
wait any(rising(io.di[3]), falling(io.di[4]), changed(io.ai[1]))
|
||||
wait io.di[5] == true timeout 1 s on_timeout call recover
|
||||
pulse io.do[3] duration 200 ms
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
const IO_MAP: IoMap = {
|
||||
aliases: {
|
||||
grip_close: { domain: "do", index: 6, raw: "io.do[6]" }
|
||||
},
|
||||
allowedRanges: {
|
||||
ai: { min: 1, max: 8 },
|
||||
di: { min: 1, max: 16 },
|
||||
do: { min: 1, max: 32 },
|
||||
go: { min: 1, max: 4 }
|
||||
}
|
||||
};
|
||||
|
||||
function declarations() {
|
||||
return parseGrl(PROGRAM).module.declarations;
|
||||
}
|
||||
|
||||
function motionContext(decls = declarations()) {
|
||||
return buildMotionContext(
|
||||
decls.filter(
|
||||
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
||||
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
function pathsByName(paths: GrlPathDeclaration[]) {
|
||||
return new Map(paths.map((path) => [path.name, path]));
|
||||
}
|
||||
|
||||
describe("GRL IO, wait, and pulse compilation", () => {
|
||||
it("compiles procedure IO writes, wait conditions, timeout actions, and pulse traces", () => {
|
||||
const procedure = declarations().find(
|
||||
(decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration"
|
||||
)!;
|
||||
|
||||
const instructions = parseIoFlowStatements(procedure.bodyTokens, IO_MAP);
|
||||
|
||||
expect(instructions).toHaveLength(7);
|
||||
expect(instructions[0]).toMatchObject({
|
||||
kind: "IO_WRITE",
|
||||
target: { domain: "do", index: 1, raw: "io.do[1]" },
|
||||
value: true
|
||||
});
|
||||
expect(instructions[1]).toMatchObject({
|
||||
kind: "IO_WRITE",
|
||||
target: { domain: "go", index: 2, raw: "io.go[2]" },
|
||||
value: 16
|
||||
});
|
||||
expect(instructions[2]).toMatchObject({
|
||||
kind: "IO_WRITE",
|
||||
target: { domain: "do", index: 6, raw: "io.do[6]" },
|
||||
value: false
|
||||
});
|
||||
expect(instructions[3]).toMatchObject({
|
||||
kind: "WAIT",
|
||||
condition: "all ( io . di [ 1 ] == true , io . di [ 2 ] == false )",
|
||||
timeout: 2,
|
||||
onTimeout: { kind: "alarm", value: "Clamp close timeout" }
|
||||
});
|
||||
expect(instructions[4]).toMatchObject({
|
||||
kind: "WAIT",
|
||||
condition: "any ( rising ( io . di [ 3 ] ) , falling ( io . di [ 4 ] ) , changed ( io . ai [ 1 ] ) )"
|
||||
});
|
||||
expect(instructions[5]).toMatchObject({
|
||||
kind: "WAIT",
|
||||
condition: "io . di [ 5 ] == true",
|
||||
timeout: 1,
|
||||
onTimeout: { kind: "call", value: "recover" }
|
||||
});
|
||||
expect(instructions[6]).toMatchObject({
|
||||
kind: "PULSE",
|
||||
target: { domain: "do", index: 3, raw: "io.do[3]" },
|
||||
duration: 0.2,
|
||||
trace: [
|
||||
{ time: 0, action: "set", target: { domain: "do", index: 3 }, value: true },
|
||||
{ time: 0.2, action: "reset", target: { domain: "do", index: 3 }, value: false }
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("validates IO addresses against configured ranges", () => {
|
||||
const procedure = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
io.do[99] = true
|
||||
end
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
|
||||
expect(() =>
|
||||
parseIoFlowStatements(procedure.bodyTokens, { allowedRanges: { do: { min: 1, max: 16 } } })
|
||||
).toThrowError(expect.objectContaining({ code: "GRL_IO_ADDRESS_NOT_FOUND" }));
|
||||
});
|
||||
|
||||
it("expands path event IO metadata into pulse IR without entering KDL motion segments", () => {
|
||||
const decls = declarations();
|
||||
const path = decls.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const compiled = compilePathToPlanRequest(path, motionContext(decls), {
|
||||
startJoints: [0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(compiled.request.segments).toHaveLength(1);
|
||||
expect(compiled.request.events).toHaveLength(1);
|
||||
expect(compilePathEventIo(compiled.events[0]!, IO_MAP)).toEqual([
|
||||
expect.objectContaining({
|
||||
kind: "PULSE",
|
||||
target: { domain: "do", index: 20, raw: "io.do[20]" },
|
||||
duration: 0.1
|
||||
})
|
||||
]);
|
||||
});
|
||||
|
||||
it("expands operation action metadata into wait and pulse IR with preserved units", () => {
|
||||
const decls = declarations();
|
||||
const operation = decls.find(
|
||||
(decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration"
|
||||
)!;
|
||||
const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration");
|
||||
const compiled = compileOperation(operation, pathsByName(paths));
|
||||
|
||||
expect(compileOperationActionIo(compiled.startActions[0]!, IO_MAP)).toEqual([
|
||||
expect.objectContaining({
|
||||
kind: "WAIT",
|
||||
condition: "io . di [ 4 ] == true",
|
||||
timeout: 0.5,
|
||||
onTimeout: { kind: "alarm", value: "part missing" }
|
||||
})
|
||||
]);
|
||||
expect(compileOperationActionIo(compiled.endActions[0]!, IO_MAP)).toEqual([
|
||||
expect.objectContaining({
|
||||
kind: "PULSE",
|
||||
target: { domain: "do", index: 5, raw: "io.do[5]" },
|
||||
duration: 0.25
|
||||
})
|
||||
]);
|
||||
});
|
||||
|
||||
it("lexes statement fallback metadata so unit literals and booleans remain typed", () => {
|
||||
const event: PathEventInstruction = {
|
||||
timing: "at",
|
||||
pointId: "p0",
|
||||
kind: "pulse",
|
||||
data: { statement: "pulse io.do[7] duration 125 ms" }
|
||||
};
|
||||
|
||||
expect(compilePathEventIo(event, IO_MAP)).toEqual([
|
||||
expect.objectContaining({
|
||||
kind: "PULSE",
|
||||
target: { domain: "do", index: 7, raw: "io.do[7]" },
|
||||
duration: 0.125
|
||||
})
|
||||
]);
|
||||
});
|
||||
});
|
||||
94
kdl-wasm/web/tests/grl/lexer.test.ts
Normal file
94
kdl-wasm/web/tests/grl/lexer.test.ts
Normal file
@@ -0,0 +1,94 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { GRL_KEYWORDS, lexGrl, normalizeUnitLiteral, normalizeUnitValue } from "../../src/grl/lexer/index.js";
|
||||
import type { GrlNumberToken } from "../../src/grl/lexer/index.js";
|
||||
|
||||
function numbers(source: string): GrlNumberToken[] {
|
||||
return lexGrl(source).filter((token): token is GrlNumberToken => token.kind === "number");
|
||||
}
|
||||
|
||||
describe("GRL lexer", () => {
|
||||
it("recognizes comments, keywords, identifiers, and source positions", () => {
|
||||
const tokens = lexGrl(`// generated\nlanguage grl 0.1\nmodule Main\n proc main()\n end\nend\n`);
|
||||
|
||||
expect(tokens[0]).toMatchObject({
|
||||
kind: "comment",
|
||||
style: "line",
|
||||
value: " generated",
|
||||
range: {
|
||||
start: { line: 1, column: 1 },
|
||||
end: { line: 1, column: 13 }
|
||||
}
|
||||
});
|
||||
expect(tokens.filter((token) => token.kind === "keyword").map((token) => token.raw)).toEqual([
|
||||
"language",
|
||||
"module",
|
||||
"proc",
|
||||
"end",
|
||||
"end"
|
||||
]);
|
||||
expect(tokens.find((token) => token.raw === "Main")).toMatchObject({
|
||||
kind: "identifier",
|
||||
range: {
|
||||
start: { line: 3, column: 8 }
|
||||
}
|
||||
});
|
||||
expect(tokens.at(-1)).toMatchObject({ kind: "eof" });
|
||||
});
|
||||
|
||||
it("normalizes numeric literals with GRL units into SI values", () => {
|
||||
const found = numbers("100 mm 0.25 m 180 deg 3.14159 rad 300 mm/s 50 % 200 ms 2.5 kg 500 mm/s2");
|
||||
|
||||
expect(found.map((token) => token.unit?.raw)).toEqual([
|
||||
"mm",
|
||||
"m",
|
||||
"deg",
|
||||
"rad",
|
||||
"mm/s",
|
||||
"%",
|
||||
"ms",
|
||||
"kg",
|
||||
"mm/s2"
|
||||
]);
|
||||
expect(found[0]?.unit?.normalizedValue).toBeCloseTo(0.1);
|
||||
expect(found[1]?.unit?.normalizedValue).toBeCloseTo(0.25);
|
||||
expect(found[2]?.unit?.normalizedValue).toBeCloseTo(Math.PI);
|
||||
expect(found[3]?.unit?.normalizedValue).toBeCloseTo(3.14159);
|
||||
expect(found[4]?.unit?.normalizedValue).toBeCloseTo(0.3);
|
||||
expect(found[5]?.unit?.normalizedValue).toBeCloseTo(0.5);
|
||||
expect(found[6]?.unit?.normalizedValue).toBeCloseTo(0.2);
|
||||
expect(found[7]?.unit?.normalizedValue).toBeCloseTo(2.5);
|
||||
expect(found[8]?.unit?.normalizedValue).toBeCloseTo(0.5);
|
||||
});
|
||||
|
||||
it("keeps unit raw text on number tokens", () => {
|
||||
const [token] = numbers("linear(300 mm/s)");
|
||||
|
||||
expect(token).toMatchObject({
|
||||
kind: "number",
|
||||
raw: "300 mm/s",
|
||||
value: 300,
|
||||
unit: {
|
||||
raw: "mm/s",
|
||||
kind: "linear_velocity",
|
||||
siUnit: "m/s"
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
it("exposes the full reserved keyword set from the specification", () => {
|
||||
expect(GRL_KEYWORDS).toContain("movej");
|
||||
expect(GRL_KEYWORDS).toContain("run_operation");
|
||||
expect(GRL_KEYWORDS).toContain("post_hint");
|
||||
expect(GRL_KEYWORDS).toContain("continuous");
|
||||
expect(GRL_KEYWORDS).toHaveLength(85);
|
||||
});
|
||||
|
||||
it("provides direct unit helpers for parser and semantic layers", () => {
|
||||
expect(normalizeUnitLiteral("deg/s")).toMatchObject({
|
||||
kind: "angular_velocity",
|
||||
siUnit: "rad/s"
|
||||
});
|
||||
expect(normalizeUnitValue(90, "deg/s")).toBeCloseTo(Math.PI / 2);
|
||||
expect(() => normalizeUnitLiteral("inch")).toThrow("Unknown GRL unit");
|
||||
});
|
||||
});
|
||||
124
kdl-wasm/web/tests/grl/motionCompile.test.ts
Normal file
124
kdl-wasm/web/tests/grl/motionCompile.test.ts
Normal file
@@ -0,0 +1,124 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { GrlDataDeclaration, GrlProcedureDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
buildMotionContext,
|
||||
compileMotionToKdlRequest,
|
||||
parseProcedureMotionInstructions
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
persistent tool gripper = tool {
|
||||
tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg),
|
||||
mass: 1 kg
|
||||
}
|
||||
persistent frame fixture = frame {
|
||||
origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
const speed v_joint = joint(60 %)
|
||||
const speed v_linear = linear(300 mm/s)
|
||||
const zone z10 = z(10 mm)
|
||||
target home = joint_target {
|
||||
joints: [0 deg, 0 deg]
|
||||
}
|
||||
target pick = pose_target {
|
||||
pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg),
|
||||
tool: gripper,
|
||||
frame: fixture
|
||||
}
|
||||
target mid = pose_target {
|
||||
pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
target arc_end = pose_target {
|
||||
pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
proc main()
|
||||
set_tool gripper
|
||||
set_frame fixture
|
||||
set_speed v_linear
|
||||
set_zone z10
|
||||
movej home speed v_joint zone fine
|
||||
movel pick
|
||||
movec via mid target arc_end speed linear(150 mm/s) zone fine
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
function declarations() {
|
||||
return parseGrl(PROGRAM).module.declarations;
|
||||
}
|
||||
|
||||
describe("GRL motion instruction compilation", () => {
|
||||
it("parses movej, movel, and movec from procedure body tokens", () => {
|
||||
const decls = declarations();
|
||||
const context = buildMotionContext(
|
||||
decls.filter(
|
||||
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
||||
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
||||
)
|
||||
);
|
||||
const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
const instructions = parseProcedureMotionInstructions(procedure, context);
|
||||
|
||||
expect(instructions.map((instruction) => instruction.kind)).toEqual(["MOVEJ", "MOVEL", "MOVEC"]);
|
||||
expect(instructions[0]).toMatchObject({
|
||||
kind: "MOVEJ",
|
||||
speed: { kind: "joint_percent", value: 0.6 },
|
||||
zone: { kind: "fine" },
|
||||
target: { joints: [0, 0] },
|
||||
sourceMap: { line: 32 }
|
||||
});
|
||||
expect(instructions[1]).toMatchObject({
|
||||
kind: "MOVEL",
|
||||
speed: { kind: "linear", velocity: 0.3 },
|
||||
zone: { kind: "distance", value: 0.01 }
|
||||
});
|
||||
expect(instructions[1]?.tool?.position).toEqual([0, 0, 0.1]);
|
||||
expect(instructions[1]?.frame?.position).toEqual([0.8, 0, 0]);
|
||||
expect(instructions[2]).toMatchObject({
|
||||
kind: "MOVEC",
|
||||
speed: { kind: "linear", velocity: 0.15 },
|
||||
zone: { kind: "fine" }
|
||||
});
|
||||
});
|
||||
|
||||
it("compiles motion instructions to KDL request shapes", () => {
|
||||
const decls = declarations();
|
||||
const context = buildMotionContext(
|
||||
decls.filter(
|
||||
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
||||
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
||||
)
|
||||
);
|
||||
const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
const [movej, movel, movec] = parseProcedureMotionInstructions(procedure, context);
|
||||
|
||||
expect(compileMotionToKdlRequest(movej!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
|
||||
startJoints: [0, 0],
|
||||
target: { joints: [0, 0] },
|
||||
speed: { kind: "joint_percent", value: 0.6 },
|
||||
zone: { kind: "fine" },
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(compileMotionToKdlRequest(movel!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
|
||||
startJoints: [0, 0],
|
||||
target: { pose: { position: [0.5, 0, 0] } },
|
||||
speed: { kind: "linear", velocity: 0.3 },
|
||||
zone: { kind: "distance", value: 0.01 },
|
||||
tool: { position: [0, 0, 0.1] },
|
||||
frame: { position: [0.8, 0, 0] },
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(compileMotionToKdlRequest(movec!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
|
||||
startJoints: [0, 0],
|
||||
via: { pose: { position: [0.55, 0.05, 0] } },
|
||||
target: { pose: { position: [0.6, 0, 0] } },
|
||||
speed: { kind: "linear", velocity: 0.15 },
|
||||
zone: { kind: "fine" },
|
||||
sampleTime: 0.004
|
||||
});
|
||||
});
|
||||
});
|
||||
153
kdl-wasm/web/tests/grl/operationCompile.test.ts
Normal file
153
kdl-wasm/web/tests/grl/operationCompile.test.ts
Normal file
@@ -0,0 +1,153 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type {
|
||||
GrlOperationDeclaration,
|
||||
GrlPathDeclaration,
|
||||
GrlProcedureDeclaration
|
||||
} from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
compileOperation,
|
||||
expandRunOperation,
|
||||
parseProcedureRunOperationStatements
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
path weld_path {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p0 movej home
|
||||
}
|
||||
operation weld_op_01 {
|
||||
kind: arc_welding
|
||||
path: weld_path
|
||||
process {
|
||||
weld_id: "WELD_1"
|
||||
voltage: 24.0
|
||||
current: 180.0
|
||||
weave: none
|
||||
}
|
||||
start_action:
|
||||
io.do[20] = true
|
||||
end_action:
|
||||
io.do[20] = false
|
||||
}
|
||||
proc main()
|
||||
run_operation weld_op_01
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
function declarations() {
|
||||
return parseGrl(PROGRAM).module.declarations;
|
||||
}
|
||||
|
||||
function pathsByName(paths: GrlPathDeclaration[]) {
|
||||
return new Map(paths.map((path) => [path.name, path]));
|
||||
}
|
||||
|
||||
describe("GRL operation compilation", () => {
|
||||
it("parses operation kind, path, process, and action blocks", () => {
|
||||
const operation = declarations().find(
|
||||
(decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration"
|
||||
)!;
|
||||
|
||||
expect(operation).toMatchObject({
|
||||
kind: "OperationDeclaration",
|
||||
name: "weld_op_01",
|
||||
operationKind: "arc_welding",
|
||||
pathName: "weld_path",
|
||||
items: [
|
||||
{ kind: "OperationProcessBlock" },
|
||||
{ kind: "OperationActionBlock", actionKind: "start_action" },
|
||||
{ kind: "OperationActionBlock", actionKind: "end_action" }
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("compiles operation process metadata and action statements", () => {
|
||||
const decls = declarations();
|
||||
const operation = decls.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!;
|
||||
const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration");
|
||||
const compiled = compileOperation(operation, pathsByName(paths));
|
||||
|
||||
expect(compiled).toMatchObject({
|
||||
operationId: "weld_op_01",
|
||||
kind: "arc_welding",
|
||||
pathId: "weld_path",
|
||||
process: {
|
||||
weld_id: "WELD_1",
|
||||
voltage: 24,
|
||||
current: 180,
|
||||
weave: "none"
|
||||
},
|
||||
startActions: [
|
||||
{
|
||||
kind: "ACTION",
|
||||
actionKind: "start_action",
|
||||
operationId: "weld_op_01",
|
||||
statement: "io . do [ 20 ] = true"
|
||||
}
|
||||
],
|
||||
endActions: [
|
||||
{
|
||||
kind: "ACTION",
|
||||
actionKind: "end_action",
|
||||
operationId: "weld_op_01",
|
||||
statement: "io . do [ 20 ] = false"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("extracts run_operation and expands to start action, path, and end action", () => {
|
||||
const decls = declarations();
|
||||
const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
const operation = decls.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!;
|
||||
const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration");
|
||||
const compiled = compileOperation(operation, pathsByName(paths));
|
||||
const [run] = parseProcedureRunOperationStatements(procedure);
|
||||
|
||||
expect(run).toEqual({
|
||||
kind: "RUN_OPERATION",
|
||||
operationId: "weld_op_01",
|
||||
sourceMap: {
|
||||
line: 25,
|
||||
column: 5
|
||||
}
|
||||
});
|
||||
expect(expandRunOperation(run!, new Map([[compiled.operationId, compiled]]))).toEqual([
|
||||
expect.objectContaining({ kind: "ACTION", actionKind: "start_action" }),
|
||||
{
|
||||
kind: "RUN_PATH",
|
||||
pathId: "weld_path",
|
||||
sourceMap: {
|
||||
line: 25,
|
||||
column: 5
|
||||
}
|
||||
},
|
||||
expect.objectContaining({ kind: "ACTION", actionKind: "end_action" })
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports operations that reference missing paths and missing run_operation targets", () => {
|
||||
const missingPathOperation = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
operation bad_op {
|
||||
kind: handling
|
||||
path: missing_path
|
||||
}
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!;
|
||||
|
||||
expect(() => compileOperation(missingPathOperation, new Map())).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_OPERATION_PATH_NOT_FOUND" })
|
||||
);
|
||||
|
||||
expect(() =>
|
||||
expandRunOperation({ kind: "RUN_OPERATION", operationId: "missing_op" }, new Map())
|
||||
).toThrowError(expect.objectContaining({ code: "GRL_OPERATION_NOT_FOUND" }));
|
||||
});
|
||||
});
|
||||
103
kdl-wasm/web/tests/grl/parser.test.ts
Normal file
103
kdl-wasm/web/tests/grl/parser.test.ts
Normal file
@@ -0,0 +1,103 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { GrlParseError, parseGrl } from "../../src/grl/parser/index.js";
|
||||
|
||||
describe("GRL parser", () => {
|
||||
it("parses the minimal language/module/proc skeleton with source ranges", () => {
|
||||
const ast = parseGrl(`language grl 0.1
|
||||
|
||||
module Main
|
||||
proc main()
|
||||
// body comment should not affect parser
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(ast).toMatchObject({
|
||||
kind: "Program",
|
||||
language: {
|
||||
kind: "LanguageDeclaration",
|
||||
language: "grl",
|
||||
version: "0.1",
|
||||
range: {
|
||||
start: { line: 1, column: 1 },
|
||||
end: { line: 1, column: 17 }
|
||||
}
|
||||
},
|
||||
module: {
|
||||
kind: "ModuleDeclaration",
|
||||
name: "Main",
|
||||
declarations: [
|
||||
{
|
||||
kind: "ProcedureDeclaration",
|
||||
name: "main",
|
||||
params: []
|
||||
}
|
||||
]
|
||||
}
|
||||
});
|
||||
expect(ast.module.range.start).toMatchObject({ line: 3, column: 1 });
|
||||
expect(ast.module.range.end).toMatchObject({ line: 7, column: 4 });
|
||||
});
|
||||
|
||||
it("parses imports, data declarations, targets, and procedure body tokens", () => {
|
||||
const ast = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
import CommonTools
|
||||
const speed v_pick = linear(300 mm/s)
|
||||
target home = joint_target {
|
||||
joints: [0 deg, 0 deg]
|
||||
}
|
||||
proc main()
|
||||
movej home
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(ast.module.declarations.map((decl) => decl.kind)).toEqual([
|
||||
"ImportDeclaration",
|
||||
"DataDeclaration",
|
||||
"TargetDeclaration",
|
||||
"ProcedureDeclaration"
|
||||
]);
|
||||
expect(ast.module.declarations[0]).toMatchObject({
|
||||
kind: "ImportDeclaration",
|
||||
moduleName: "CommonTools"
|
||||
});
|
||||
expect(ast.module.declarations[1]).toMatchObject({
|
||||
kind: "DataDeclaration",
|
||||
storage: "const",
|
||||
typeName: "speed",
|
||||
name: "v_pick",
|
||||
initializer: {
|
||||
kind: "CallExpression",
|
||||
callee: "linear"
|
||||
}
|
||||
});
|
||||
expect(ast.module.declarations[2]).toMatchObject({
|
||||
kind: "TargetDeclaration",
|
||||
name: "home",
|
||||
target: {
|
||||
kind: "ObjectExpression",
|
||||
typeName: "joint_target"
|
||||
}
|
||||
});
|
||||
expect(ast.module.declarations[3]).toMatchObject({
|
||||
kind: "ProcedureDeclaration",
|
||||
bodyTokens: [
|
||||
{
|
||||
kind: "keyword",
|
||||
raw: "movej"
|
||||
},
|
||||
{
|
||||
kind: "identifier",
|
||||
raw: "home"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("reports stable line and column on invalid syntax", () => {
|
||||
expect(() => parseGrl("language grl\nmodule Main\nend\n")).toThrow(GrlParseError);
|
||||
expect(() => parseGrl("language grl\nmodule Main\nend\n")).toThrow("Expected GRL language version at 2:1");
|
||||
});
|
||||
});
|
||||
251
kdl-wasm/web/tests/grl/pathCompile.test.ts
Normal file
251
kdl-wasm/web/tests/grl/pathCompile.test.ts
Normal file
@@ -0,0 +1,251 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type {
|
||||
GrlDataDeclaration,
|
||||
GrlPathDeclaration,
|
||||
GrlProcedureDeclaration,
|
||||
GrlTargetDeclaration
|
||||
} from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import {
|
||||
buildMotionContext,
|
||||
compilePathToPlanRequest,
|
||||
parseProcedureRunPathStatements
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
persistent tool gripper = tool {
|
||||
tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
persistent frame fixture = frame {
|
||||
origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
const speed v_joint = joint(60 %)
|
||||
const speed v_linear = linear(300 mm/s)
|
||||
const zone z10 = z(10 mm)
|
||||
target home = joint_target {
|
||||
joints: [0 deg, 0 deg]
|
||||
}
|
||||
target pick = pose_target {
|
||||
pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
target mid = pose_target {
|
||||
pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
target arc_end = pose_target {
|
||||
pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
|
||||
}
|
||||
path pick_path {
|
||||
source {
|
||||
type: cad_curve
|
||||
id: "edge_032"
|
||||
sample_distance: 5 mm
|
||||
}
|
||||
defaults {
|
||||
tool: gripper,
|
||||
frame: fixture,
|
||||
speed: v_linear,
|
||||
zone: z10
|
||||
}
|
||||
point approach movej home speed v_joint zone fine
|
||||
point p1 movel pick offset z 100 mm
|
||||
point p2 movec via mid target arc_end speed linear(150 mm/s) zone fine
|
||||
event before p1 io.do[10] = true
|
||||
event after p2 io.do[10] = false
|
||||
event at p1 distance -20 mm pulse io.do[20] duration 100 ms
|
||||
}
|
||||
proc main()
|
||||
run_path pick_path
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
function declarations() {
|
||||
return parseGrl(PROGRAM).module.declarations;
|
||||
}
|
||||
|
||||
function motionContext(decls = declarations()) {
|
||||
return buildMotionContext(
|
||||
decls.filter(
|
||||
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
||||
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
describe("GRL path compilation", () => {
|
||||
it("parses path defaults, source metadata, points, and events as AST nodes", () => {
|
||||
const path = declarations().find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
|
||||
expect(path).toMatchObject({
|
||||
kind: "PathDeclaration",
|
||||
name: "pick_path",
|
||||
items: [
|
||||
{ kind: "PathSourceBlock" },
|
||||
{ kind: "PathDefaultsBlock" },
|
||||
{ kind: "PathPoint", id: "approach" },
|
||||
{ kind: "PathPoint", id: "p1" },
|
||||
{ kind: "PathPoint", id: "p2" },
|
||||
{ kind: "PathEvent", timing: "before", pointId: "p1" },
|
||||
{ kind: "PathEvent", timing: "after", pointId: "p2" },
|
||||
{ kind: "PathEvent", timing: "at", pointId: "p1" }
|
||||
]
|
||||
});
|
||||
expect(path.items[0]).toMatchObject({
|
||||
properties: [
|
||||
{ key: "type", value: { kind: "IdentifierExpression", name: "cad_curve" } },
|
||||
{ key: "id", value: { kind: "StringLiteral", value: "edge_032" } },
|
||||
{ key: "sample_distance", value: { kind: "NumberLiteral" } }
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("compiles a path to PathPlanRequest with defaults, source map, source metadata, and events", () => {
|
||||
const decls = declarations();
|
||||
const path = decls.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const compiled = compilePathToPlanRequest(path, motionContext(decls), {
|
||||
startJoints: [0, 0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(compiled.pathId).toBe("pick_path");
|
||||
expect(compiled.request).toMatchObject({
|
||||
pathId: "pick_path",
|
||||
startJoints: [0, 0],
|
||||
sampleTime: 0.004,
|
||||
source: {
|
||||
type: "cad_curve",
|
||||
id: "edge_032",
|
||||
sample_distance: 0.005
|
||||
},
|
||||
segments: [
|
||||
{
|
||||
id: "approach",
|
||||
motion: "MOVEJ",
|
||||
targetId: "home",
|
||||
speed: { kind: "joint_percent", value: 0.6 },
|
||||
zone: { kind: "fine" }
|
||||
},
|
||||
{
|
||||
id: "p1",
|
||||
motion: "MOVEL",
|
||||
targetId: "pick",
|
||||
speed: { kind: "linear", velocity: 0.3 },
|
||||
zone: { kind: "distance", value: 0.01 },
|
||||
tool: { position: [0, 0, 0.1] },
|
||||
frame: { position: [0.8, 0, 0] },
|
||||
sourceMap: { line: 37 }
|
||||
},
|
||||
{
|
||||
id: "p2",
|
||||
motion: "MOVEC",
|
||||
targetId: "arc_end",
|
||||
speed: { kind: "linear", velocity: 0.15 },
|
||||
zone: { kind: "fine" }
|
||||
}
|
||||
],
|
||||
events: [
|
||||
{
|
||||
timing: "before",
|
||||
pointId: "p1",
|
||||
kind: "io",
|
||||
data: { statement: "io . do [ 10 ] = true" }
|
||||
},
|
||||
{
|
||||
timing: "after",
|
||||
pointId: "p2",
|
||||
kind: "io"
|
||||
},
|
||||
{
|
||||
timing: "at",
|
||||
pointId: "p1",
|
||||
distance: -0.02,
|
||||
kind: "pulse"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(compiled.request.segments[1]?.target).toMatchObject({
|
||||
pose: {
|
||||
position: [0.5, 0, 0.1]
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
it("extracts run_path statements from procedure body tokens", () => {
|
||||
const procedure = declarations().find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
|
||||
expect(parseProcedureRunPathStatements(procedure)).toEqual([
|
||||
{
|
||||
kind: "RUN_PATH",
|
||||
pathId: "pick_path",
|
||||
sourceMap: {
|
||||
line: 44,
|
||||
column: 5
|
||||
}
|
||||
}
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports empty paths and duplicate point names", () => {
|
||||
const emptyPath = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
path empty_path {
|
||||
}
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
|
||||
expect(() =>
|
||||
compilePathToPlanRequest(emptyPath, motionContext([]), { startJoints: [], sampleTime: 0.004 })
|
||||
).toThrowError(expect.objectContaining({ code: "GRL_PATH_EMPTY" }));
|
||||
|
||||
const duplicatePath = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
path dup_path {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p movej home
|
||||
point p movej home
|
||||
}
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const duplicateContext = motionContext(parseGrl(`language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
end
|
||||
`).module.declarations);
|
||||
|
||||
expect(() =>
|
||||
compilePathToPlanRequest(duplicatePath, duplicateContext, { startJoints: [0], sampleTime: 0.004 })
|
||||
).toThrowError(expect.objectContaining({ code: "GRL_PATH_POINT_DUPLICATE" }));
|
||||
});
|
||||
|
||||
it("reports events that reference missing points", () => {
|
||||
const path = parseGrl(`language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
path bad_event {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p movej home
|
||||
event after missing io.do[1] = true
|
||||
}
|
||||
end
|
||||
`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const context = motionContext(parseGrl(`language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(50 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
end
|
||||
`).module.declarations);
|
||||
|
||||
expect(() =>
|
||||
compilePathToPlanRequest(path, context, { startJoints: [0], sampleTime: 0.004 })
|
||||
).toThrowError(expect.objectContaining({ code: "GRL_PATH_EVENT_POINT_NOT_FOUND" }));
|
||||
});
|
||||
});
|
||||
201
kdl-wasm/web/tests/grl/procFunction.test.ts
Normal file
201
kdl-wasm/web/tests/grl/procFunction.test.ts
Normal file
@@ -0,0 +1,201 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { GrlFunctionDeclaration, GrlProcedureDeclaration } from "../../src/grl/ast/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { analyzeProcFunctionSemantics } from "../../src/grl/semantic/index.js";
|
||||
|
||||
function declarations(source: string) {
|
||||
return parseGrl(source).module.declarations;
|
||||
}
|
||||
|
||||
describe("GRL proc, func, call, return, and scope semantics", () => {
|
||||
it("parses function declarations and analyzes proc/func signatures, calls, returns, and warnings", () => {
|
||||
const decls = declarations(`language grl 0.1
|
||||
module Main
|
||||
var int global_count = 0
|
||||
proc read_sensor(out bool ok)
|
||||
ok = true
|
||||
end
|
||||
proc main(in bool start, out bool done, inout int count)
|
||||
call read_sensor(done)
|
||||
call helper(count)
|
||||
call self_check()
|
||||
return
|
||||
end
|
||||
proc self_check()
|
||||
call self_check()
|
||||
end
|
||||
func int helper(inout int value)
|
||||
var int global_count = 1
|
||||
return value
|
||||
end
|
||||
end
|
||||
`);
|
||||
const func = decls.find((decl): decl is GrlFunctionDeclaration => decl.kind === "FunctionDeclaration")!;
|
||||
|
||||
expect(func).toMatchObject({
|
||||
kind: "FunctionDeclaration",
|
||||
returnType: "int",
|
||||
name: "helper",
|
||||
bodyTokens: [
|
||||
{ raw: "var" },
|
||||
{ raw: "int" },
|
||||
{ raw: "global_count" },
|
||||
{ raw: "=" },
|
||||
{ raw: "1" },
|
||||
{ raw: "return" },
|
||||
{ raw: "value" }
|
||||
]
|
||||
});
|
||||
|
||||
const analysis = analyzeProcFunctionSemantics(decls);
|
||||
|
||||
expect(analysis.procedures).toEqual([
|
||||
expect.objectContaining({
|
||||
name: "read_sensor",
|
||||
parameters: [expect.objectContaining({ name: "ok", typeName: "bool", direction: "out" })]
|
||||
}),
|
||||
expect.objectContaining({
|
||||
name: "main",
|
||||
parameters: [
|
||||
expect.objectContaining({ name: "start", typeName: "bool", direction: "in" }),
|
||||
expect.objectContaining({ name: "done", typeName: "bool", direction: "out" }),
|
||||
expect.objectContaining({ name: "count", typeName: "int", direction: "inout" })
|
||||
]
|
||||
}),
|
||||
expect.objectContaining({ name: "self_check", parameters: [] })
|
||||
]);
|
||||
expect(analysis.functions).toEqual([
|
||||
expect.objectContaining({
|
||||
name: "helper",
|
||||
returnType: "int",
|
||||
parameters: [expect.objectContaining({ name: "value", typeName: "int", direction: "inout" })]
|
||||
})
|
||||
]);
|
||||
expect(analysis.calls).toEqual([
|
||||
expect.objectContaining({ kind: "CALL", target: "read_sensor", args: [expect.objectContaining({ text: "done" })] }),
|
||||
expect.objectContaining({ kind: "CALL", target: "helper", args: [expect.objectContaining({ text: "count" })] }),
|
||||
expect.objectContaining({ kind: "CALL", target: "self_check", args: [] }),
|
||||
expect.objectContaining({ kind: "CALL", target: "self_check", args: [] })
|
||||
]);
|
||||
expect(analysis.returns).toEqual([
|
||||
expect.objectContaining({ kind: "RETURN" }),
|
||||
expect.objectContaining({ kind: "RETURN", value: expect.objectContaining({ text: "value" }) })
|
||||
]);
|
||||
expect(analysis.diagnostics).toEqual([
|
||||
expect.objectContaining({ severity: "warning", code: "GRL_RECURSIVE_CALL" }),
|
||||
expect.objectContaining({ severity: "warning", code: "GRL_NAME_SHADOWS_OUTER_SCOPE" })
|
||||
]);
|
||||
});
|
||||
|
||||
it("reports out parameters that are not assigned on all normal return paths", () => {
|
||||
const decls = declarations(`language grl 0.1
|
||||
module Main
|
||||
proc main(out bool done)
|
||||
if ready == true
|
||||
done = true
|
||||
end
|
||||
return
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => analyzeProcFunctionSemantics(decls)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_OUT_PARAM_NOT_ASSIGNED" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports out and inout call arguments that are not lvalues", () => {
|
||||
const decls = declarations(`language grl 0.1
|
||||
module Main
|
||||
proc set_done(out bool done)
|
||||
done = true
|
||||
end
|
||||
proc main()
|
||||
call set_done(true)
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => analyzeProcFunctionSemantics(decls)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_ARGUMENT_NOT_LVALUE" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports missing or incompatible function returns", () => {
|
||||
const missingReturn = declarations(`language grl 0.1
|
||||
module Main
|
||||
func int bad(in bool ready)
|
||||
if ready == true
|
||||
return 1
|
||||
end
|
||||
end
|
||||
end
|
||||
`);
|
||||
const wrongReturn = declarations(`language grl 0.1
|
||||
module Main
|
||||
func bool bad()
|
||||
return 1
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => analyzeProcFunctionSemantics(missingReturn)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_FUNC_MISSING_RETURN" })
|
||||
);
|
||||
expect(() => analyzeProcFunctionSemantics(wrongReturn)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_RETURN_TYPE_MISMATCH" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports illegal function side effects and procedure return values", () => {
|
||||
const functionSideEffect = declarations(`language grl 0.1
|
||||
module Main
|
||||
func bool bad()
|
||||
wait io.di[1] == true
|
||||
return true
|
||||
end
|
||||
end
|
||||
`);
|
||||
const procedureReturnValue = declarations(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
return true
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => analyzeProcFunctionSemantics(functionSideEffect)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_FUNC_SIDE_EFFECT" })
|
||||
);
|
||||
expect(() => analyzeProcFunctionSemantics(procedureReturnValue)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_RETURN_VALUE_IN_PROC" })
|
||||
);
|
||||
});
|
||||
|
||||
it("reports call target and argument type errors", () => {
|
||||
const missingCall = declarations(`language grl 0.1
|
||||
module Main
|
||||
proc main()
|
||||
call missing()
|
||||
end
|
||||
end
|
||||
`);
|
||||
const typeMismatch = declarations(`language grl 0.1
|
||||
module Main
|
||||
proc expects_int(in int value)
|
||||
return
|
||||
end
|
||||
proc main()
|
||||
call expects_int("bad")
|
||||
end
|
||||
end
|
||||
`);
|
||||
|
||||
expect(() => analyzeProcFunctionSemantics(missingCall)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_CALL_TARGET_NOT_FOUND" })
|
||||
);
|
||||
expect(() => analyzeProcFunctionSemantics(typeMismatch)).toThrowError(
|
||||
expect.objectContaining({ code: "GRL_CALL_ARGUMENT_TYPE" })
|
||||
);
|
||||
});
|
||||
});
|
||||
148
kdl-wasm/web/tests/grl/semanticIr.test.ts
Normal file
148
kdl-wasm/web/tests/grl/semanticIr.test.ts
Normal file
@@ -0,0 +1,148 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module Main
|
||||
const speed vj = joint(50 %)
|
||||
const speed vl = linear(200 mm/s)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
target pick = pose_target { pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) }
|
||||
path pick_path {
|
||||
defaults { speed: vl, zone: zf }
|
||||
point p0 movej home speed vj zone fine
|
||||
point p1 movel pick
|
||||
event before p1 io.do[1] = true
|
||||
}
|
||||
operation pick_op {
|
||||
kind: handling
|
||||
path: pick_path
|
||||
start_action:
|
||||
io.do[2] = true
|
||||
end_action:
|
||||
io.do[2] = false
|
||||
}
|
||||
proc set_done(out bool done)
|
||||
done = true
|
||||
end
|
||||
proc main(out bool done)
|
||||
set_speed vl
|
||||
set_zone zf
|
||||
movej home speed vj zone fine
|
||||
io.do[3] = true
|
||||
wait io.di[1] == true timeout 1 s
|
||||
if done == false
|
||||
pulse io.do[4] duration 100 ms
|
||||
else
|
||||
alarm DONE "Already done"
|
||||
end
|
||||
call set_done(done)
|
||||
run_path pick_path
|
||||
run_operation pick_op
|
||||
return
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
describe("GRL semantic analyzer, executable IR, and source map", () => {
|
||||
it("compiles a complete program into unified executable IR and KDL bridge requests", () => {
|
||||
const ir = compileSemanticProgram(parseGrl(PROGRAM), {
|
||||
startJoints: [0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(ir.moduleName).toBe("Main");
|
||||
expect(ir.semanticChecks).toHaveLength(22);
|
||||
expect(ir.symbols).toEqual([
|
||||
expect.objectContaining({ kind: "data", name: "vj", typeName: "speed" }),
|
||||
expect.objectContaining({ kind: "data", name: "vl", typeName: "speed" }),
|
||||
expect.objectContaining({ kind: "data", name: "zf", typeName: "zone" }),
|
||||
expect.objectContaining({ kind: "target", name: "home" }),
|
||||
expect.objectContaining({ kind: "target", name: "pick" }),
|
||||
expect.objectContaining({ kind: "path", name: "pick_path" }),
|
||||
expect.objectContaining({ kind: "operation", name: "pick_op" }),
|
||||
expect.objectContaining({ kind: "procedure", name: "set_done" }),
|
||||
expect.objectContaining({ kind: "procedure", name: "main" })
|
||||
]);
|
||||
expect(ir.paths).toHaveLength(1);
|
||||
expect(ir.operations).toHaveLength(1);
|
||||
expect(ir.kdlBridge.pathRequests).toEqual([
|
||||
expect.objectContaining({
|
||||
pathId: "pick_path",
|
||||
segments: [
|
||||
expect.objectContaining({ id: "p0", motion: "MOVEJ" }),
|
||||
expect.objectContaining({ id: "p1", motion: "MOVEL" })
|
||||
]
|
||||
})
|
||||
]);
|
||||
expect(ir.kdlBridge.motionRequests).toEqual([
|
||||
expect.objectContaining({
|
||||
startJoints: [0],
|
||||
speed: { kind: "joint_percent", value: 0.5 },
|
||||
zone: { kind: "fine" },
|
||||
sampleTime: 0.004
|
||||
})
|
||||
]);
|
||||
|
||||
const main = ir.procedures.find((procedure) => procedure.name === "main")!;
|
||||
expect(main.instructions).toEqual([
|
||||
expect.objectContaining({ kind: "MOVEJ" }),
|
||||
expect.objectContaining({ kind: "IO_WRITE", target: expect.objectContaining({ domain: "do", index: 3 }) }),
|
||||
expect.objectContaining({ kind: "WAIT", timeout: 1 }),
|
||||
expect.objectContaining({
|
||||
kind: "EXEC_IF",
|
||||
branches: [
|
||||
expect.objectContaining({
|
||||
branchKind: "if",
|
||||
body: expect.arrayContaining([expect.objectContaining({ kind: "PULSE", duration: 0.1 })])
|
||||
}),
|
||||
expect.objectContaining({
|
||||
branchKind: "else",
|
||||
body: expect.arrayContaining([expect.objectContaining({ kind: "ALARM", alarmId: "DONE" })])
|
||||
})
|
||||
]
|
||||
}),
|
||||
expect.objectContaining({ kind: "CALL", target: "set_done" }),
|
||||
expect.objectContaining({ kind: "RUN_PATH", pathId: "pick_path" }),
|
||||
expect.objectContaining({ kind: "RUN_OPERATION", operationId: "pick_op" }),
|
||||
expect.objectContaining({ kind: "RETURN" })
|
||||
]);
|
||||
});
|
||||
|
||||
it("exposes source map entries for GRL procedure lines, path points, and operation actions", () => {
|
||||
const ir = compileSemanticProgram(parseGrl(PROGRAM), {
|
||||
startJoints: [0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(ir.sourceMap).toEqual(
|
||||
expect.arrayContaining([
|
||||
expect.objectContaining({ kind: "path_point", pathId: "pick_path", pointId: "p0" }),
|
||||
expect.objectContaining({ kind: "path_point", pathId: "pick_path", pointId: "p1" }),
|
||||
expect.objectContaining({ kind: "operation_action", operationId: "pick_op" }),
|
||||
expect.objectContaining({ kind: "MOVEJ", procedureId: "main", sourceMap: expect.objectContaining({ line: 28 }) }),
|
||||
expect.objectContaining({ kind: "EXEC_IF", procedureId: "main" }),
|
||||
expect.objectContaining({ kind: "RUN_OPERATION", procedureId: "main" })
|
||||
])
|
||||
);
|
||||
});
|
||||
|
||||
it("reports duplicate symbols through semantic diagnostics", () => {
|
||||
const ir = compileSemanticProgram(parseGrl(`language grl 0.1
|
||||
module Main
|
||||
const speed v = joint(10 %)
|
||||
const speed v = joint(20 %)
|
||||
proc main()
|
||||
end
|
||||
end
|
||||
`), {
|
||||
startJoints: [],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
|
||||
expect(ir.diagnostics).toEqual([
|
||||
expect.objectContaining({ severity: "error", code: "GRL_SYMBOL_DUPLICATE" })
|
||||
]);
|
||||
});
|
||||
});
|
||||
224
kdl-wasm/web/tests/kdl/cAbi.test.ts
Normal file
224
kdl-wasm/web/tests/kdl/cAbi.test.ts
Normal file
@@ -0,0 +1,224 @@
|
||||
import { access } from "node:fs/promises";
|
||||
import { fileURLToPath } from "node:url";
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { KDL_C_ABI_EXPORTS, KdlNativeAbi } from "../../src/kdl/nativeAbi.js";
|
||||
import type { NativeKdlModule } from "../../src/kdl/nativeModule.js";
|
||||
import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js";
|
||||
|
||||
const BUILD_DIR = new URL("../../../build-wasm/", import.meta.url);
|
||||
const WRAPPER_URL = new URL("kdl.js", BUILD_DIR);
|
||||
|
||||
type NativeFactory = (options?: {
|
||||
locateFile?: (path: string, prefix: string) => string;
|
||||
}) => Promise<NativeKdlModule>;
|
||||
|
||||
async function loadNativeModule(): Promise<NativeKdlModule> {
|
||||
await access(fileURLToPath(WRAPPER_URL));
|
||||
const imported = (await import(/* @vite-ignore */ WRAPPER_URL.href)) as {
|
||||
default?: NativeFactory;
|
||||
createKdlModule?: NativeFactory;
|
||||
};
|
||||
const factory = imported.default ?? imported.createKdlModule;
|
||||
if (!factory) {
|
||||
throw new Error("kdl.js did not export createKdlModule");
|
||||
}
|
||||
return factory({
|
||||
locateFile: (path) => fileURLToPath(new URL(path, BUILD_DIR))
|
||||
});
|
||||
}
|
||||
|
||||
const NATIVE_SOLVER_URDF = `
|
||||
<robot name="native_solver">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="4"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="0.5" acceleration="1"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
function writeFloat64Array(native: NativeKdlModule, values: number[]): number {
|
||||
const bytes = values.length * Float64Array.BYTES_PER_ELEMENT;
|
||||
const ptr = native._malloc?.(bytes);
|
||||
if (!ptr) {
|
||||
throw new Error(`Failed to allocate ${bytes} bytes`);
|
||||
}
|
||||
native.HEAPF64?.set(values, ptr / Float64Array.BYTES_PER_ELEMENT);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
function readFloat64Array(native: NativeKdlModule, ptr: number, length: number): number[] {
|
||||
return Array.from(native.HEAPF64?.subarray(
|
||||
ptr / Float64Array.BYTES_PER_ELEMENT,
|
||||
ptr / Float64Array.BYTES_PER_ELEMENT + length
|
||||
) ?? []);
|
||||
}
|
||||
|
||||
describe("KDL C ABI", () => {
|
||||
it("exports the stable P0 ABI names", async () => {
|
||||
const abi = new KdlNativeAbi(await loadNativeModule());
|
||||
|
||||
expect(() => abi.assertExports()).not.toThrow();
|
||||
expect(KDL_C_ABI_EXPORTS).toEqual([
|
||||
"kdl_init",
|
||||
"kdl_create_robot",
|
||||
"kdl_destroy_robot",
|
||||
"kdl_get_robot_info",
|
||||
"kdl_fk",
|
||||
"kdl_fk_all_links",
|
||||
"kdl_jacobian",
|
||||
"kdl_ik",
|
||||
"kdl_plan_movej",
|
||||
"kdl_plan_movel",
|
||||
"kdl_plan_movec",
|
||||
"kdl_plan_path",
|
||||
"kdl_sample_trap",
|
||||
"kdl_last_error"
|
||||
]);
|
||||
});
|
||||
|
||||
it("initializes, caches model handles, returns JSON info, and destroys handles", async () => {
|
||||
const abi = new KdlNativeAbi(await loadNativeModule());
|
||||
const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
|
||||
robotId: "abi",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
});
|
||||
|
||||
expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
|
||||
const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
|
||||
expect(handle).toBeGreaterThan(0);
|
||||
|
||||
const info = abi.readJsonCall<{ handle: number; nativeState: string; dof: number }>(
|
||||
"kdl_get_robot_info",
|
||||
["number"],
|
||||
[handle]
|
||||
);
|
||||
expect(info).toMatchObject({
|
||||
handle,
|
||||
dof: 2,
|
||||
nativeState: "kdl_chain"
|
||||
});
|
||||
|
||||
expect(abi.callNumber("kdl_destroy_robot", ["number"], [handle])).toBe(0);
|
||||
});
|
||||
|
||||
it("constructs a native KDL chain and returns real FK and Jacobian data", async () => {
|
||||
const native = await loadNativeModule();
|
||||
const abi = new KdlNativeAbi(native);
|
||||
const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
|
||||
robotId: "native",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
});
|
||||
|
||||
expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
|
||||
const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
|
||||
expect(handle).toBeGreaterThan(0);
|
||||
expect(abi.readJsonCall("kdl_get_robot_info", ["number"], [handle])).toMatchObject({
|
||||
handle,
|
||||
dof: 2,
|
||||
jointNames: ["joint_1", "joint_2"],
|
||||
nativeState: "kdl_chain"
|
||||
});
|
||||
|
||||
const joints = writeFloat64Array(native, [Math.PI / 2, 0.4]);
|
||||
const pose = native._malloc?.(7 * Float64Array.BYTES_PER_ELEMENT);
|
||||
const jacobian = native._malloc?.(12 * Float64Array.BYTES_PER_ELEMENT);
|
||||
expect(pose).toBeTruthy();
|
||||
expect(jacobian).toBeTruthy();
|
||||
|
||||
try {
|
||||
expect(abi.callNumber("kdl_fk", ["number", "number", "number", "number"], [handle, joints, 2, pose])).toBe(0);
|
||||
const pose7 = readFloat64Array(native, pose!, 7);
|
||||
expect(pose7[0]).toBeCloseTo(0);
|
||||
expect(pose7[1]).toBeCloseTo(0.4);
|
||||
expect(pose7[2]).toBeCloseTo(0);
|
||||
expect(pose7[5]).toBeCloseTo(Math.SQRT1_2);
|
||||
expect(pose7[6]).toBeCloseTo(Math.SQRT1_2);
|
||||
|
||||
expect(abi.callNumber("kdl_jacobian", ["number", "number", "number", "number"], [handle, joints, 2, jacobian])).toBe(0);
|
||||
const jac = readFloat64Array(native, jacobian!, 12);
|
||||
expect(jac[0]).toBeCloseTo(-0.4, 4);
|
||||
expect(jac[1]).toBeCloseTo(0, 4);
|
||||
expect(jac[2]).toBeCloseTo(0, 4);
|
||||
expect(jac[3]).toBeCloseTo(1, 4);
|
||||
expect(jac[10]).toBeCloseTo(1, 4);
|
||||
} finally {
|
||||
native._free?.(joints);
|
||||
if (pose) {
|
||||
native._free?.(pose);
|
||||
}
|
||||
if (jacobian) {
|
||||
native._free?.(jacobian);
|
||||
}
|
||||
abi.callNumber("kdl_destroy_robot", ["number"], [handle]);
|
||||
}
|
||||
});
|
||||
|
||||
it("normalizes C ABI failures through kdl_last_error", async () => {
|
||||
const abi = new KdlNativeAbi(await loadNativeModule());
|
||||
|
||||
expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
|
||||
const returnCode = abi.callNumber("kdl_ik", ["number", "number", "number", "number", "string", "number"], [1, 0, 0, 0, "{}", 0]);
|
||||
|
||||
expect(returnCode).toBe(-1);
|
||||
expect(abi.lastError()).toMatchObject({
|
||||
code: "KDL_NOT_IMPLEMENTED",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_NOT_IMPLEMENTED"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(() => abi.checkReturnCode(returnCode)).toThrowError(
|
||||
expect.objectContaining({
|
||||
code: "KDL_NOT_IMPLEMENTED"
|
||||
})
|
||||
);
|
||||
});
|
||||
|
||||
it("reports JSON output buffer errors without raw strings", async () => {
|
||||
const native = await loadNativeModule();
|
||||
const abi = new KdlNativeAbi(native);
|
||||
const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
|
||||
robotId: "abi",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
});
|
||||
|
||||
expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
|
||||
const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]);
|
||||
const ptr = native._malloc?.(4);
|
||||
expect(ptr).toBeTruthy();
|
||||
|
||||
try {
|
||||
const returnCode = abi.callNumber("kdl_get_robot_info", ["number", "number", "number"], [handle, ptr, 4]);
|
||||
expect(returnCode).toBe(-1);
|
||||
expect(abi.lastError()).toMatchObject({
|
||||
code: "KDL_BUFFER_TOO_SMALL",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_BUFFER_TOO_SMALL"
|
||||
}
|
||||
]
|
||||
});
|
||||
} finally {
|
||||
if (ptr) {
|
||||
native._free?.(ptr);
|
||||
}
|
||||
}
|
||||
});
|
||||
});
|
||||
178
kdl-wasm/web/tests/kdl/checks.test.ts
Normal file
178
kdl-wasm/web/tests/kdl/checks.test.ts
Normal file
@@ -0,0 +1,178 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { JacobianResult, PoseTarget, ReachabilityResult } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="planar_checks">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="4"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="0.5" acceleration="1"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
PLANAR_URDF,
|
||||
{
|
||||
robotId: "checks",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
function poseTarget(id: string, x: number, y: number): PoseTarget {
|
||||
return {
|
||||
id,
|
||||
pose: {
|
||||
position: [x, y, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
describe("Jacobian, singularity, limits, and reachability checks", () => {
|
||||
it("computes a 6xdof Jacobian with expected linear components", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "jacobian",
|
||||
payload: [handle, [Math.PI / 2, 0.4]]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const jacobian = response.result as JacobianResult;
|
||||
expect(jacobian.rows).toBe(6);
|
||||
expect(jacobian.cols).toBe(2);
|
||||
expect(jacobian.data).toHaveLength(12);
|
||||
expect(jacobian.data[0]).toBeCloseTo(-0.4, 4);
|
||||
expect(jacobian.data[1]).toBeCloseTo(0, 4);
|
||||
expect(jacobian.data[2]).toBeCloseTo(0, 4);
|
||||
expect(jacobian.data[3]).toBeCloseTo(1, 4);
|
||||
});
|
||||
|
||||
it("reports singularity warning for collapsed planar reach", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "checkSingularity",
|
||||
payload: [handle, [0, 0]]
|
||||
});
|
||||
|
||||
expect(response.result).toMatchObject({
|
||||
ok: true,
|
||||
nearSingularity: true,
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "warning",
|
||||
code: "KDL_SINGULARITY"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("checks joint and velocity limits with structured diagnostics", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const jointResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "checkJointLimits",
|
||||
payload: [handle, [0, 2]]
|
||||
});
|
||||
expect(jointResponse.result).toMatchObject({
|
||||
ok: false,
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_LIMIT"
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
const velocityResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "checkVelocityLimits",
|
||||
payload: [
|
||||
handle,
|
||||
{
|
||||
points: [
|
||||
{
|
||||
jointVelocity: [1, 0.75],
|
||||
jointAcceleration: [1, 1.5]
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(velocityResponse.result).toMatchObject({
|
||||
ok: false,
|
||||
maxJointVelocityRatio: 1.5,
|
||||
maxJointAccelerationRatio: 1.5,
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_VELOCITY_LIMIT",
|
||||
pointIndex: 0
|
||||
},
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_ACCEL_LIMIT",
|
||||
pointIndex: 0
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("checks reachability and preserves batch order", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const reachable = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 7,
|
||||
method: "checkReachability",
|
||||
payload: [handle, poseTarget("ok", 0, 0.3), { positionTolerance: 1e-9 }]
|
||||
});
|
||||
expect(reachable.result).toMatchObject({
|
||||
ok: true,
|
||||
reachable: true,
|
||||
targetId: "ok",
|
||||
joints: [Math.PI / 2, 0.3]
|
||||
});
|
||||
|
||||
const batch = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 8,
|
||||
method: "checkReachabilityBatch",
|
||||
payload: [handle, [poseTarget("a", 0.2, 0), poseTarget("b", 2, 0)], {}]
|
||||
});
|
||||
const results = batch.result as ReachabilityResult[];
|
||||
expect(results.map((result) => result.targetId)).toEqual(["a", "b"]);
|
||||
expect(results[0]?.reachable).toBe(true);
|
||||
expect(results[1]).toMatchObject({
|
||||
reachable: false,
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_LIMIT"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
});
|
||||
198
kdl-wasm/web/tests/kdl/fk.test.ts
Normal file
198
kdl-wasm/web/tests/kdl/fk.test.ts
Normal file
@@ -0,0 +1,198 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
|
||||
const SIMPLE_URDF = `
|
||||
<robot name="simple_fk">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="link_2"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<origin xyz="0 0 0.1" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="0.4" velocity="0.3" acceleration="1.2"/>
|
||||
</joint>
|
||||
<joint name="tool_fixed" type="fixed">
|
||||
<parent link="link_2"/>
|
||||
<child link="tool0"/>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRuntimeRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
SIMPLE_URDF,
|
||||
{
|
||||
robotId: "fk",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
describe("FK and fkAllLinks", () => {
|
||||
it("computes flange and tcp poses for the zero joint state", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "fk",
|
||||
payload: [handle, [0, 0]]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as {
|
||||
ok: boolean;
|
||||
joints: number[];
|
||||
diagnostics: unknown[];
|
||||
flange: { position: number[]; quaternion: number[] };
|
||||
tcp: { position: number[]; quaternion: number[] };
|
||||
};
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.joints).toEqual([0, 0]);
|
||||
expect(result.diagnostics).toEqual([]);
|
||||
expect(result.flange.position[0]).toBeCloseTo(0);
|
||||
expect(result.flange.position[1]).toBeCloseTo(0);
|
||||
expect(result.flange.position[2]).toBeCloseTo(0.35);
|
||||
expect(result.flange.quaternion).toEqual([0, 0, 0, 1]);
|
||||
expect(result.tcp.position[0]).toBeCloseTo(0);
|
||||
expect(result.tcp.position[1]).toBeCloseTo(0);
|
||||
expect(result.tcp.position[2]).toBeCloseTo(0.35);
|
||||
expect(result.tcp.quaternion).toEqual([0, 0, 0, 1]);
|
||||
});
|
||||
|
||||
it("applies revolute and prismatic joint motion in chain order", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "fk",
|
||||
payload: [handle, [Math.PI / 2, 0.2]]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as { flange: { position: number[]; quaternion: number[] } };
|
||||
expect(result.flange.position[0]).toBeCloseTo(0);
|
||||
expect(result.flange.position[1]).toBeCloseTo(0.2);
|
||||
expect(result.flange.position[2]).toBeCloseTo(0.35);
|
||||
expect(result.flange.quaternion[2]).toBeCloseTo(Math.SQRT1_2);
|
||||
expect(result.flange.quaternion[3]).toBeCloseTo(Math.SQRT1_2);
|
||||
});
|
||||
|
||||
it("returns link poses in base-to-tip order", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "fkAllLinks",
|
||||
payload: [handle, [0, 0.1]]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as {
|
||||
linkPoses: Array<{ link: string; pose: { position: number[] } }>;
|
||||
};
|
||||
expect(result.linkPoses.map((entry) => entry.link)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
|
||||
expect(result.linkPoses[0]?.pose.position).toEqual([0, 0, 0]);
|
||||
expect(result.linkPoses[3]?.pose.position[0]).toBeCloseTo(0.1);
|
||||
expect(result.linkPoses[3]?.pose.position[2]).toBeCloseTo(0.35);
|
||||
});
|
||||
|
||||
it("applies tool offset to tcp without changing flange", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "fk",
|
||||
payload: [
|
||||
handle,
|
||||
[0, 0],
|
||||
{
|
||||
tool: {
|
||||
position: [0, 0, 0.1],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as {
|
||||
flange: { position: number[] };
|
||||
tcp: { position: number[] };
|
||||
};
|
||||
expect(result.flange.position[2]).toBeCloseTo(0.35);
|
||||
expect(result.tcp.position[2]).toBeCloseTo(0.45);
|
||||
});
|
||||
|
||||
it("writes tcp pose into a reusable Float64Array", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const out = new Float64Array(7);
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 51,
|
||||
method: "fkPose7",
|
||||
payload: [handle, new Float64Array([Math.PI / 2, 0.2]), out]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toBe(out);
|
||||
expect(out[0]).toBeCloseTo(0);
|
||||
expect(out[1]).toBeCloseTo(0.2);
|
||||
expect(out[2]).toBeCloseTo(0.35);
|
||||
expect(out[5]).toBeCloseTo(Math.SQRT1_2);
|
||||
expect(out[6]).toBeCloseTo(Math.SQRT1_2);
|
||||
});
|
||||
|
||||
it("returns a structured error for undersized fkPose7 output buffers", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 52,
|
||||
method: "fkPose7",
|
||||
payload: [handle, [0, 0], new Float64Array(6)]
|
||||
});
|
||||
|
||||
expect(response).toMatchObject({
|
||||
ok: false,
|
||||
error: {
|
||||
code: "KDL_OUTPUT_DIMENSION_MISMATCH"
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
it("returns a structured dimension diagnostic", async () => {
|
||||
const { runtime, handle } = await createRuntimeRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "fk",
|
||||
payload: [handle, [0]]
|
||||
});
|
||||
|
||||
expect(response).toMatchObject({
|
||||
ok: false,
|
||||
error: {
|
||||
code: "KDL_JOINT_DIMENSION_MISMATCH",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_DIMENSION_MISMATCH"
|
||||
}
|
||||
]
|
||||
}
|
||||
});
|
||||
});
|
||||
});
|
||||
166
kdl-wasm/web/tests/kdl/ik.test.ts
Normal file
166
kdl-wasm/web/tests/kdl/ik.test.ts
Normal file
@@ -0,0 +1,166 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { IkResult, Pose } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="planar_ik">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="0.3" acceleration="1.2"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
const UNSUPPORTED_URDF = `
|
||||
<robot name="unsupported_ik">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="revolute">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
function pose(x: number, y: number, z = 0): Pose {
|
||||
return {
|
||||
position: [x, y, z],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
};
|
||||
}
|
||||
|
||||
async function createRobot(urdf = PLANAR_URDF) {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
urdf,
|
||||
{
|
||||
robotId: "ik",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
describe("IK and ikBatch", () => {
|
||||
it("solves a reachable planar target and FK back-substitution is within tolerance", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "ik",
|
||||
payload: [handle, [0, 0], pose(0, 0.4), { positionTolerance: 1e-9 }]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as IkResult;
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.joints?.[0]).toBeCloseTo(Math.PI / 2);
|
||||
expect(result.joints?.[1]).toBeCloseTo(0.4);
|
||||
expect(result.residualPosition).toBeLessThan(1e-9);
|
||||
|
||||
const fk = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "fk",
|
||||
payload: [handle, result.joints]
|
||||
});
|
||||
const fkResult = fk.result as { tcp: { position: number[] } };
|
||||
expect(fkResult.tcp.position[0]).toBeCloseTo(0);
|
||||
expect(fkResult.tcp.position[1]).toBeCloseTo(0.4);
|
||||
});
|
||||
|
||||
it("keeps ikBatch results in input order", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "ikBatch",
|
||||
payload: [
|
||||
handle,
|
||||
[
|
||||
[0, 0],
|
||||
[0, 0]
|
||||
],
|
||||
[pose(0.2, 0), pose(0, 0.3)],
|
||||
{}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const results = response.result as IkResult[];
|
||||
expect(results).toHaveLength(2);
|
||||
expect(results[0]?.joints?.[0]).toBeCloseTo(0);
|
||||
expect(results[0]?.joints?.[1]).toBeCloseTo(0.2);
|
||||
expect(results[1]?.joints?.[0]).toBeCloseTo(Math.PI / 2);
|
||||
expect(results[1]?.joints?.[1]).toBeCloseTo(0.3);
|
||||
});
|
||||
|
||||
it("returns joint_limit reason when the candidate exceeds limits", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "ik",
|
||||
payload: [handle, [0, 0], pose(2, 0), {}]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({
|
||||
ok: false,
|
||||
reason: "joint_limit",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_LIMIT"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("returns invalid_model reason for unsupported IK chains", async () => {
|
||||
const { runtime, handle } = await createRobot(UNSUPPORTED_URDF);
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 7,
|
||||
method: "ik",
|
||||
payload: [handle, [0, 0], pose(0.2, 0), {}]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({
|
||||
ok: false,
|
||||
reason: "invalid_model",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_IK_UNSUPPORTED_MODEL"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
});
|
||||
167
kdl-wasm/web/tests/kdl/path.test.ts
Normal file
167
kdl-wasm/web/tests/kdl/path.test.ts
Normal file
@@ -0,0 +1,167 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { PathPlanRequest, PathPlanResult, PathValidationResult } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="path_planar">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="10"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="1" acceleration="10"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
PLANAR_URDF,
|
||||
{
|
||||
robotId: "path",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
function pathRequest(overrides: Partial<PathPlanRequest> = {}): PathPlanRequest {
|
||||
return {
|
||||
startJoints: [0, 0],
|
||||
sampleTime: 0.05,
|
||||
segments: [
|
||||
{
|
||||
id: "move-home",
|
||||
motion: "MOVEJ",
|
||||
target: {
|
||||
id: "joint_goal",
|
||||
joints: [Math.PI / 2, 0.2]
|
||||
},
|
||||
speed: { kind: "joint_abs", velocity: 1, acceleration: 4 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { line: 10, column: 5 }
|
||||
},
|
||||
{
|
||||
id: "line-out",
|
||||
motion: "MOVEL",
|
||||
target: {
|
||||
id: "line_goal",
|
||||
pose: {
|
||||
position: [0, 0.4, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
},
|
||||
speed: { kind: "linear", velocity: 0.2, acceleration: 1 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { line: 11, column: 5 }
|
||||
}
|
||||
],
|
||||
...overrides
|
||||
};
|
||||
}
|
||||
|
||||
describe("planPath and validatePath", () => {
|
||||
it("plans multiple motion segments and merges points with segment metadata", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "planPath",
|
||||
payload: [handle, pathRequest()]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as PathPlanResult;
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.segments).toHaveLength(2);
|
||||
expect(result.points.length).toBeGreaterThan(result.segments[0]!.points.length);
|
||||
expect(result.points[0]).toMatchObject({
|
||||
index: 0,
|
||||
time: 0,
|
||||
segmentId: "move-home",
|
||||
targetId: "joint_goal",
|
||||
sourceMap: { line: 10 }
|
||||
});
|
||||
expect(result.points.at(-1)).toMatchObject({
|
||||
segmentId: "line-out",
|
||||
targetId: "line_goal",
|
||||
sourceMap: { line: 11 }
|
||||
});
|
||||
expect(result.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
|
||||
expect(result.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.4, 5);
|
||||
expect(result.duration).toBeCloseTo(result.segments[0]!.duration + result.segments[1]!.duration);
|
||||
for (let index = 1; index < result.points.length; index += 1) {
|
||||
expect(result.points[index]!.time).toBeGreaterThan(result.points[index - 1]!.time);
|
||||
expect(result.points[index]!.index).toBe(index);
|
||||
}
|
||||
});
|
||||
|
||||
it("validates a path and returns per-segment reports", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "validatePath",
|
||||
payload: [handle, pathRequest()]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as PathValidationResult;
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.reachable).toBe(true);
|
||||
expect(result.cycleTime).toBeGreaterThan(0);
|
||||
expect(result.segmentReports.map((report) => report.segmentId)).toEqual(["move-home", "line-out"]);
|
||||
expect(result.segmentReports[0]).toMatchObject({
|
||||
ok: true,
|
||||
motion: "MOVEJ"
|
||||
});
|
||||
expect(result.segmentReports[1]).toMatchObject({
|
||||
ok: true,
|
||||
motion: "MOVEL",
|
||||
maxCartesianError: 0
|
||||
});
|
||||
});
|
||||
|
||||
it("returns KDL_PATH_EMPTY for empty path requests", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "planPath",
|
||||
payload: [
|
||||
handle,
|
||||
pathRequest({
|
||||
segments: []
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({
|
||||
ok: false,
|
||||
duration: 0,
|
||||
segments: [],
|
||||
points: [],
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_PATH_EMPTY"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
});
|
||||
23
kdl-wasm/web/tests/kdl/performance.test.ts
Normal file
23
kdl-wasm/web/tests/kdl/performance.test.ts
Normal file
@@ -0,0 +1,23 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { runPerformanceBaseline } from "../../src/kdl/performanceBaseline.js";
|
||||
|
||||
describe("KDL performance baseline", () => {
|
||||
it("records TypedArray and batch baseline metrics", async () => {
|
||||
const result = await runPerformanceBaseline();
|
||||
const metrics = Object.fromEntries(result.metrics.map((metric) => [metric.name, metric]));
|
||||
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.diagnostics).toEqual([]);
|
||||
expect(metrics.robot_init_6_axis?.totalMs).toBeLessThanOrEqual(1_000);
|
||||
expect(metrics.fk_pose7_typed_array?.averageMs).toBeLessThanOrEqual(1);
|
||||
expect(metrics.ik_planar_average?.averageMs).toBeLessThanOrEqual(10);
|
||||
expect(metrics.reachability_batch_1000).toMatchObject({
|
||||
points: 1_000,
|
||||
ok: true
|
||||
});
|
||||
expect(metrics.trajectory_10s_4ms).toMatchObject({
|
||||
ok: true
|
||||
});
|
||||
expect(metrics.trajectory_10s_4ms?.points).toBeGreaterThanOrEqual(2_500);
|
||||
});
|
||||
});
|
||||
193
kdl-wasm/web/tests/kdl/planMoveC.test.ts
Normal file
193
kdl-wasm/web/tests/kdl/planMoveC.test.ts
Normal file
@@ -0,0 +1,193 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { MoveCRequest, TrajectoryResult } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="movec_planar">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="4" acceleration="20"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="2" acceleration="20"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
PLANAR_URDF,
|
||||
{
|
||||
robotId: "movec",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
function request(overrides: Partial<MoveCRequest>): MoveCRequest {
|
||||
return {
|
||||
startJoints: [0, 0.5],
|
||||
via: {
|
||||
id: "via",
|
||||
pose: {
|
||||
position: [0.5, 0.5, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
},
|
||||
target: {
|
||||
id: "arc_goal",
|
||||
pose: {
|
||||
position: [0, 0.5, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
},
|
||||
speed: {
|
||||
kind: "linear",
|
||||
velocity: 0.25,
|
||||
acceleration: 1
|
||||
},
|
||||
zone: {
|
||||
kind: "fine"
|
||||
},
|
||||
sampleTime: 0.05,
|
||||
...overrides
|
||||
};
|
||||
}
|
||||
|
||||
describe("planMoveC", () => {
|
||||
it("plans a circular TCP arc with circle metadata", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "planMoveC",
|
||||
payload: [handle, request({})]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.motion).toBe("MOVEC");
|
||||
expect(trajectory.points.length).toBeGreaterThan(2);
|
||||
expect(trajectory.points[0]).toMatchObject({
|
||||
index: 0,
|
||||
time: 0,
|
||||
s: 0,
|
||||
motion: "MOVEC",
|
||||
targetId: "arc_goal"
|
||||
});
|
||||
expect(trajectory.points.at(-1)?.s).toBe(1);
|
||||
expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
|
||||
expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.5, 5);
|
||||
|
||||
const circle = trajectory.meta?.circle as {
|
||||
center: number[];
|
||||
radius: number;
|
||||
angle: number;
|
||||
length: number;
|
||||
direction: "cw" | "ccw";
|
||||
maxArcError: number;
|
||||
};
|
||||
expect(circle.center[0]).toBeCloseTo(0.25);
|
||||
expect(circle.center[1]).toBeCloseTo(0.25);
|
||||
expect(circle.radius).toBeCloseTo(Math.SQRT1_2 / 2);
|
||||
expect(circle.angle).toBeCloseTo(Math.PI);
|
||||
expect(circle.length).toBeCloseTo((Math.SQRT1_2 / 2) * Math.PI);
|
||||
expect(circle.direction).toBe("ccw");
|
||||
expect(circle.maxArcError).toBeLessThan(1e-6);
|
||||
});
|
||||
|
||||
it("returns KDL_ARC_DEGENERATE for collinear points", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "planMoveC",
|
||||
payload: [
|
||||
handle,
|
||||
request({
|
||||
via: {
|
||||
id: "line_mid",
|
||||
pose: {
|
||||
position: [0.25, 0, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
},
|
||||
target: {
|
||||
id: "line_end",
|
||||
pose: {
|
||||
position: [0.75, 0, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({
|
||||
ok: false,
|
||||
motion: "MOVEC",
|
||||
points: [],
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_ARC_DEGENERATE"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("reports zone approximation and joint-speed approximation warnings", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "planMoveC",
|
||||
payload: [
|
||||
handle,
|
||||
request({
|
||||
speed: {
|
||||
kind: "joint_abs",
|
||||
velocity: 0.25,
|
||||
acceleration: 1
|
||||
},
|
||||
zone: {
|
||||
kind: "distance",
|
||||
value: 0.01
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "warning",
|
||||
code: "KDL_MOVEC_JOINT_SPEED_APPROX"
|
||||
})
|
||||
);
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "warning",
|
||||
code: "KDL_ZONE_APPROX_FINE"
|
||||
})
|
||||
);
|
||||
});
|
||||
});
|
||||
192
kdl-wasm/web/tests/kdl/planMoveJ.test.ts
Normal file
192
kdl-wasm/web/tests/kdl/planMoveJ.test.ts
Normal file
@@ -0,0 +1,192 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { MoveJRequest, PoseTarget, TrajectoryResult } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="movej_planar">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="1" acceleration="2"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="0.5" acceleration="1"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
PLANAR_URDF,
|
||||
{
|
||||
robotId: "movej",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
function baseRequest(overrides: Partial<MoveJRequest>): MoveJRequest {
|
||||
return {
|
||||
startJoints: [0, 0],
|
||||
target: {
|
||||
id: "joint_goal",
|
||||
joints: [0.5, 0.25]
|
||||
},
|
||||
speed: {
|
||||
kind: "joint_abs",
|
||||
velocity: 0.5,
|
||||
acceleration: 1
|
||||
},
|
||||
zone: {
|
||||
kind: "fine"
|
||||
},
|
||||
sampleTime: 0.1,
|
||||
...overrides
|
||||
};
|
||||
}
|
||||
|
||||
describe("planMoveJ", () => {
|
||||
it("plans a synchronized joint trajectory to a joint target", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "planMoveJ",
|
||||
payload: [handle, baseRequest({})]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.motion).toBe("MOVEJ");
|
||||
expect(trajectory.points.length).toBeGreaterThan(2);
|
||||
expect(trajectory.points[0]).toMatchObject({
|
||||
index: 0,
|
||||
time: 0,
|
||||
s: 0,
|
||||
joints: [0, 0],
|
||||
motion: "MOVEJ",
|
||||
targetId: "joint_goal"
|
||||
});
|
||||
expect(trajectory.points.at(-1)?.s).toBe(1);
|
||||
expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(0.5);
|
||||
expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.25);
|
||||
expect(trajectory.points.at(-1)?.jointVelocity[0]).toBeCloseTo(0);
|
||||
expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0.25 * Math.cos(0.5));
|
||||
expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.25 * Math.sin(0.5));
|
||||
expect(trajectory.meta).toMatchObject({
|
||||
targetType: "joint",
|
||||
qStart: [0, 0],
|
||||
qEnd: [0.5, 0.25]
|
||||
});
|
||||
});
|
||||
|
||||
it("uses IK for pose targets and warns when zone is approximated as fine", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const target: PoseTarget = {
|
||||
id: "pose_goal",
|
||||
pose: {
|
||||
position: [0, 0.3, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
};
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "planMoveJ",
|
||||
payload: [
|
||||
handle,
|
||||
baseRequest({
|
||||
target,
|
||||
zone: { kind: "distance", value: 0.01 }
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(Math.PI / 2);
|
||||
expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.3);
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "warning",
|
||||
code: "KDL_ZONE_APPROX_FINE"
|
||||
})
|
||||
);
|
||||
expect(trajectory.meta).toMatchObject({
|
||||
targetType: "pose"
|
||||
});
|
||||
});
|
||||
|
||||
it("returns a failed trajectory result for endpoint joint limit violations", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "planMoveJ",
|
||||
payload: [
|
||||
handle,
|
||||
baseRequest({
|
||||
target: {
|
||||
id: "bad_goal",
|
||||
joints: [0, 2]
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({
|
||||
ok: false,
|
||||
motion: "MOVEJ",
|
||||
points: [],
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_LIMIT"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("keeps velocity and acceleration within joint limits", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "planMoveJ",
|
||||
payload: [
|
||||
handle,
|
||||
baseRequest({
|
||||
speed: { kind: "joint_percent", value: 1 },
|
||||
target: {
|
||||
id: "limit_goal",
|
||||
joints: [1, 0.5]
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
for (const point of trajectory.points) {
|
||||
expect(Math.abs(point.jointVelocity[0]!)).toBeLessThanOrEqual(1 + 1e-9);
|
||||
expect(Math.abs(point.jointVelocity[1]!)).toBeLessThanOrEqual(0.5 + 1e-9);
|
||||
expect(Math.abs(point.jointAcceleration[0]!)).toBeLessThanOrEqual(2 + 1e-9);
|
||||
expect(Math.abs(point.jointAcceleration[1]!)).toBeLessThanOrEqual(1 + 1e-9);
|
||||
}
|
||||
});
|
||||
});
|
||||
175
kdl-wasm/web/tests/kdl/planMoveL.test.ts
Normal file
175
kdl-wasm/web/tests/kdl/planMoveL.test.ts
Normal file
@@ -0,0 +1,175 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { MoveLRequest, TrajectoryResult } from "../../src/kdl/types.js";
|
||||
|
||||
const PLANAR_URDF = `
|
||||
<robot name="movel_planar">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="10"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="tool0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="1" velocity="1" acceleration="10"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
async function createRobot() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
PLANAR_URDF,
|
||||
{
|
||||
robotId: "movel",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
expect(response.ok).toBe(true);
|
||||
return { runtime, handle: response.result as number };
|
||||
}
|
||||
|
||||
function request(overrides: Partial<MoveLRequest>): MoveLRequest {
|
||||
return {
|
||||
startJoints: [Math.PI / 2, 0.2],
|
||||
target: {
|
||||
id: "line_goal",
|
||||
pose: {
|
||||
position: [0, 0.6, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
},
|
||||
speed: {
|
||||
kind: "linear",
|
||||
velocity: 0.2,
|
||||
acceleration: 1
|
||||
},
|
||||
zone: {
|
||||
kind: "fine"
|
||||
},
|
||||
sampleTime: 0.05,
|
||||
...overrides
|
||||
};
|
||||
}
|
||||
|
||||
describe("planMoveL", () => {
|
||||
it("plans a TCP straight-line trajectory with continuous IK seeds", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "planMoveL",
|
||||
payload: [handle, request({})]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.motion).toBe("MOVEL");
|
||||
expect(trajectory.points.length).toBeGreaterThan(2);
|
||||
expect(trajectory.points[0]).toMatchObject({
|
||||
index: 0,
|
||||
time: 0,
|
||||
s: 0,
|
||||
motion: "MOVEL",
|
||||
targetId: "line_goal"
|
||||
});
|
||||
expect(trajectory.points.at(-1)?.s).toBe(1);
|
||||
expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 6);
|
||||
expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.6, 6);
|
||||
expect(trajectory.meta).toMatchObject({
|
||||
targetId: "line_goal",
|
||||
orientationMode: "fixed"
|
||||
});
|
||||
expect(trajectory.meta?.length as number).toBeCloseTo(0.4);
|
||||
|
||||
for (const point of trajectory.points) {
|
||||
expect(point.tcp.position[0]).toBeCloseTo(0, 5);
|
||||
expect(point.tcp.position[2]).toBeCloseTo(0, 5);
|
||||
expect(point.tcp.position[1]).toBeGreaterThanOrEqual(0.2 - 1e-9);
|
||||
expect(point.tcp.position[1]).toBeLessThanOrEqual(0.6 + 1e-9);
|
||||
}
|
||||
});
|
||||
|
||||
it("returns a failed trajectory when a sampled pose is unreachable", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "planMoveL",
|
||||
payload: [
|
||||
handle,
|
||||
request({
|
||||
target: {
|
||||
id: "far_goal",
|
||||
pose: {
|
||||
position: [0, 2, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
}
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(false);
|
||||
expect(trajectory.motion).toBe("MOVEL");
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "error",
|
||||
code: "KDL_JOINT_LIMIT"
|
||||
})
|
||||
);
|
||||
expect(trajectory.meta).toMatchObject({
|
||||
targetId: "far_goal"
|
||||
});
|
||||
});
|
||||
|
||||
it("reports zone approximation and joint-speed approximation warnings", async () => {
|
||||
const { runtime, handle } = await createRobot();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "planMoveL",
|
||||
payload: [
|
||||
handle,
|
||||
request({
|
||||
speed: {
|
||||
kind: "joint_abs",
|
||||
velocity: 0.2,
|
||||
acceleration: 1
|
||||
},
|
||||
zone: {
|
||||
kind: "distance",
|
||||
value: 0.01
|
||||
}
|
||||
})
|
||||
]
|
||||
});
|
||||
|
||||
const trajectory = response.result as TrajectoryResult;
|
||||
expect(trajectory.ok).toBe(true);
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "warning",
|
||||
code: "KDL_MOVEL_JOINT_SPEED_APPROX"
|
||||
})
|
||||
);
|
||||
expect(trajectory.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "warning",
|
||||
code: "KDL_ZONE_APPROX_FINE"
|
||||
})
|
||||
);
|
||||
});
|
||||
});
|
||||
152
kdl-wasm/web/tests/kdl/poseApi.test.ts
Normal file
152
kdl-wasm/web/tests/kdl/poseApi.test.ts
Normal file
@@ -0,0 +1,152 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { Pose, PoseTarget } from "../../src/kdl/types.js";
|
||||
|
||||
async function createRuntime() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
return runtime;
|
||||
}
|
||||
|
||||
function pose(x: number, y: number, z: number): Pose {
|
||||
return {
|
||||
position: [x, y, z],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
};
|
||||
}
|
||||
|
||||
describe("pose transform and offset API", () => {
|
||||
it("normalizes pose inputs from rpy and quaternion forms", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const rpyResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "normalizePose",
|
||||
payload: [{ xyz: [1, 2, 3], rpy: [0, 0, Math.PI / 2] }]
|
||||
});
|
||||
const quatResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "normalizePose",
|
||||
payload: [{ xyz: [0, 0, 0], quat: [0, 0, 0, 2] }]
|
||||
});
|
||||
|
||||
expect(rpyResponse.ok).toBe(true);
|
||||
expect((rpyResponse.result as Pose).position).toEqual([1, 2, 3]);
|
||||
expect((rpyResponse.result as Pose).quaternion[2]).toBeCloseTo(Math.SQRT1_2);
|
||||
expect((rpyResponse.result as Pose).quaternion[3]).toBeCloseTo(Math.SQRT1_2);
|
||||
expect(quatResponse.result).toMatchObject({
|
||||
position: [0, 0, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
});
|
||||
});
|
||||
|
||||
it("composes poses and computes an inverse pose", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const composeResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "composePose",
|
||||
payload: [pose(1, 0, 0), pose(0, 2, 0)]
|
||||
});
|
||||
expect(composeResponse.result).toMatchObject({
|
||||
position: [1, 2, 0],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
});
|
||||
|
||||
const inverseResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "inversePose",
|
||||
payload: [pose(1, 2, 3)]
|
||||
});
|
||||
expect(inverseResponse.result).toMatchObject({
|
||||
position: [-1, -2, -3],
|
||||
quaternion: [0, 0, 0, 1]
|
||||
});
|
||||
|
||||
const identityResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "composePose",
|
||||
payload: [pose(1, 2, 3), inverseResponse.result]
|
||||
});
|
||||
expect((identityResponse.result as Pose).position[0]).toBeCloseTo(0);
|
||||
expect((identityResponse.result as Pose).position[1]).toBeCloseTo(0);
|
||||
expect((identityResponse.result as Pose).position[2]).toBeCloseTo(0);
|
||||
});
|
||||
|
||||
it("applies frame, target, and tool using the same order as FK", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const target: PoseTarget = {
|
||||
id: "pick",
|
||||
pose: pose(0.5, 0.1, 0.2)
|
||||
};
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 7,
|
||||
method: "applyToolAndFrame",
|
||||
payload: [target, pose(0, 0, 0.18), pose(0.8, 0, 0.2)]
|
||||
});
|
||||
|
||||
const result = response.result as Pose;
|
||||
expect(result.position[0]).toBeCloseTo(1.3);
|
||||
expect(result.position[1]).toBeCloseTo(0.1);
|
||||
expect(result.position[2]).toBeCloseTo(0.58);
|
||||
expect(result.quaternion).toEqual([0, 0, 0, 1]);
|
||||
});
|
||||
|
||||
it("applies offset in frame/world by left composition and tool by right composition", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const target: PoseTarget = {
|
||||
id: "pick",
|
||||
pose: {
|
||||
position: [1, 2, 3],
|
||||
quaternion: [0, 0, Math.SQRT1_2, Math.SQRT1_2]
|
||||
},
|
||||
frame: pose(10, 0, 0)
|
||||
};
|
||||
|
||||
const frameOffset = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 8,
|
||||
method: "applyOffset",
|
||||
payload: [target, { mode: "frame", xyz: [0.1, 0, 0] }]
|
||||
});
|
||||
const worldOffset = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 9,
|
||||
method: "applyOffset",
|
||||
payload: [target, { mode: "world", xyz: [0, 0.2, 0] }]
|
||||
});
|
||||
const toolOffset = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 10,
|
||||
method: "applyOffset",
|
||||
payload: [target, { mode: "tool", xyz: [0.1, 0, 0] }]
|
||||
});
|
||||
|
||||
expect((frameOffset.result as PoseTarget).id).toBe("pick");
|
||||
expect((frameOffset.result as PoseTarget).frame).toEqual(target.frame);
|
||||
expect((frameOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1.1);
|
||||
expect((frameOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2);
|
||||
expect((worldOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
|
||||
expect((worldOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.2);
|
||||
expect((toolOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
|
||||
expect((toolOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.1);
|
||||
});
|
||||
|
||||
it("returns structured diagnostics for invalid pose inputs", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 11,
|
||||
method: "normalizePose",
|
||||
payload: [{ xyz: [1, 2], rpy: [0, 0, 0] }]
|
||||
});
|
||||
|
||||
expect(response).toMatchObject({
|
||||
ok: false,
|
||||
error: {
|
||||
code: "KDL_INVALID_POSE",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_INVALID_POSE"
|
||||
}
|
||||
]
|
||||
}
|
||||
});
|
||||
});
|
||||
});
|
||||
206
kdl-wasm/web/tests/kdl/rpc.test.ts
Normal file
206
kdl-wasm/web/tests/kdl/rpc.test.ts
Normal file
@@ -0,0 +1,206 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { KdlWorkerClient, type KdlWorkerLike } from "../../src/kdl/kdlClient.js";
|
||||
import type { NativeKdlModule } from "../../src/kdl/nativeModule.js";
|
||||
import { KdlStructuredError, type KdlRpcRequest, type KdlRpcResponse } from "../../src/kdl/rpc.js";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
|
||||
class FakeWorker implements KdlWorkerLike {
|
||||
readonly sent: Array<KdlRpcRequest<unknown[]>> = [];
|
||||
terminated = false;
|
||||
private messageListeners = new Set<(event: MessageEvent<KdlRpcResponse>) => void>();
|
||||
private errorListeners = new Set<(event: ErrorEvent) => void>();
|
||||
|
||||
postMessage(message: KdlRpcRequest<unknown[]>): void {
|
||||
this.sent.push(message);
|
||||
}
|
||||
|
||||
terminate(): void {
|
||||
this.terminated = true;
|
||||
}
|
||||
|
||||
addEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
|
||||
addEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
|
||||
addEventListener(type: "message" | "error", listener: unknown): void {
|
||||
if (type === "message") {
|
||||
this.messageListeners.add(listener as (event: MessageEvent<KdlRpcResponse>) => void);
|
||||
return;
|
||||
}
|
||||
this.errorListeners.add(listener as (event: ErrorEvent) => void);
|
||||
}
|
||||
|
||||
removeEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
|
||||
removeEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
|
||||
removeEventListener(type: "message" | "error", listener: unknown): void {
|
||||
if (type === "message") {
|
||||
this.messageListeners.delete(listener as (event: MessageEvent<KdlRpcResponse>) => void);
|
||||
return;
|
||||
}
|
||||
this.errorListeners.delete(listener as (event: ErrorEvent) => void);
|
||||
}
|
||||
|
||||
emitResponse(response: KdlRpcResponse): void {
|
||||
const event = { data: response } as MessageEvent<KdlRpcResponse>;
|
||||
for (const listener of this.messageListeners) {
|
||||
listener(event);
|
||||
}
|
||||
}
|
||||
|
||||
emitError(message: string): void {
|
||||
const event = { message, error: new Error(message) } as ErrorEvent;
|
||||
for (const listener of this.errorListeners) {
|
||||
listener(event);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
describe("KDL Worker RPC", () => {
|
||||
it("loads the native WASM module during init when a loader is configured", async () => {
|
||||
const calls: unknown[][] = [];
|
||||
const native: NativeKdlModule = {
|
||||
ccall: (...args) => {
|
||||
calls.push(args);
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
const runtime = createKdlWorkerRuntime(async () => native);
|
||||
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 1,
|
||||
method: "init",
|
||||
payload: [{ wasmBuild: "native-test" }]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
expect(response.result).toMatchObject({ wasmBuild: "native-test" });
|
||||
expect(calls).toEqual([["kdl_init", "number", ["string"], ['{"wasmBuild":"native-test"}']]]);
|
||||
});
|
||||
|
||||
it("normalizes native WASM initialization failures", async () => {
|
||||
const runtime = createKdlWorkerRuntime(async () => {
|
||||
throw new Error("cannot load kdl.js");
|
||||
});
|
||||
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 11,
|
||||
method: "init",
|
||||
payload: [{}]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(false);
|
||||
expect(response.error).toMatchObject({
|
||||
code: "KDL_WASM_INIT_FAILED",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_WASM_INIT_FAILED"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("dispatches init and dispose through structured responses", async () => {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
const initResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 1,
|
||||
method: "init",
|
||||
payload: [{ wasmBuild: "test", useThreads: true }]
|
||||
});
|
||||
|
||||
expect(initResponse.ok).toBe(true);
|
||||
expect(initResponse.result).toMatchObject({
|
||||
version: "0.1.0",
|
||||
wasmBuild: "test",
|
||||
supportsThreads: true
|
||||
});
|
||||
|
||||
const disposeResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "dispose",
|
||||
payload: []
|
||||
});
|
||||
|
||||
expect(disposeResponse).toMatchObject({ id: 2, ok: true });
|
||||
});
|
||||
|
||||
it("returns a structured error when an implemented method is called before init", async () => {
|
||||
const response = await dispatchKdlRpcRequest(createKdlWorkerRuntime(), {
|
||||
id: 7,
|
||||
method: "fk",
|
||||
payload: [1, new Float64Array([0])]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(false);
|
||||
expect(response.error).toMatchObject({
|
||||
code: "KDL_NOT_INITIALIZED",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_NOT_INITIALIZED"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("uses unique request ids and resolves responses by id", async () => {
|
||||
const workers: FakeWorker[] = [];
|
||||
const client = new KdlWorkerClient(() => {
|
||||
const worker = new FakeWorker();
|
||||
workers.push(worker);
|
||||
return worker;
|
||||
});
|
||||
|
||||
const first = client.call("init", { wasmBuild: "a" });
|
||||
const second = client.call("dispose");
|
||||
|
||||
expect(workers).toHaveLength(1);
|
||||
expect(workers[0]?.sent.map((request) => request.id)).toEqual([1, 2]);
|
||||
|
||||
workers[0]?.emitResponse({ id: 2, ok: true });
|
||||
workers[0]?.emitResponse({
|
||||
id: 1,
|
||||
ok: true,
|
||||
result: {
|
||||
version: "0.1.0",
|
||||
wasmBuild: "a",
|
||||
supportsThreads: false,
|
||||
supportsWasmFs: false
|
||||
}
|
||||
});
|
||||
|
||||
await expect(second).resolves.toBeUndefined();
|
||||
await expect(first).resolves.toMatchObject({ wasmBuild: "a" });
|
||||
});
|
||||
|
||||
it("rejects pending requests on worker failure and can create a fresh worker", async () => {
|
||||
const workers: FakeWorker[] = [];
|
||||
const client = new KdlWorkerClient(() => {
|
||||
const worker = new FakeWorker();
|
||||
workers.push(worker);
|
||||
return worker;
|
||||
});
|
||||
|
||||
const pending = client.init();
|
||||
workers[0]?.emitError("boom");
|
||||
|
||||
await expect(pending).rejects.toMatchObject({
|
||||
code: "KDL_WORKER_CRASHED"
|
||||
});
|
||||
expect(workers[0]?.terminated).toBe(true);
|
||||
|
||||
const restarted = client.init({ wasmBuild: "restart" });
|
||||
expect(workers).toHaveLength(2);
|
||||
workers[1]?.emitResponse({
|
||||
id: 2,
|
||||
ok: true,
|
||||
result: {
|
||||
version: "0.1.0",
|
||||
wasmBuild: "restart",
|
||||
supportsThreads: false,
|
||||
supportsWasmFs: false
|
||||
}
|
||||
});
|
||||
|
||||
await expect(restarted).resolves.toMatchObject({ wasmBuild: "restart" });
|
||||
});
|
||||
});
|
||||
179
kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts
Normal file
179
kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts
Normal file
@@ -0,0 +1,179 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { CycleTimeResult, PathPlanResult, TrajectoryResult } from "../../src/kdl/types.js";
|
||||
|
||||
function pose(x: number, y: number, z: number) {
|
||||
return {
|
||||
position: [x, y, z] as [number, number, number],
|
||||
quaternion: [0, 0, 0, 1] as [number, number, number, number]
|
||||
};
|
||||
}
|
||||
|
||||
function trajectory(overrides: Partial<TrajectoryResult> = {}): TrajectoryResult {
|
||||
return {
|
||||
ok: true,
|
||||
motion: "MOVEJ",
|
||||
duration: 1,
|
||||
sampleTime: 0.5,
|
||||
events: [],
|
||||
diagnostics: [],
|
||||
points: [
|
||||
{
|
||||
index: 0,
|
||||
time: 0,
|
||||
dt: 0,
|
||||
s: 0,
|
||||
sd: 0,
|
||||
sdd: 0,
|
||||
joints: [0, 0],
|
||||
jointVelocity: [0, 0],
|
||||
jointAcceleration: [0, 0],
|
||||
flange: pose(0, 0, 0),
|
||||
tcp: pose(0, 0, 0),
|
||||
motion: "MOVEJ",
|
||||
segmentId: "s1",
|
||||
diagnostics: []
|
||||
},
|
||||
{
|
||||
index: 1,
|
||||
time: 1,
|
||||
dt: 1,
|
||||
s: 1,
|
||||
sd: 0,
|
||||
sdd: 0,
|
||||
joints: [1, 2],
|
||||
jointVelocity: [0, 0],
|
||||
jointAcceleration: [0, 0],
|
||||
flange: pose(1, 0, 0),
|
||||
tcp: pose(1, 2, 0),
|
||||
motion: "MOVEJ",
|
||||
segmentId: "s1",
|
||||
diagnostics: []
|
||||
}
|
||||
],
|
||||
...overrides
|
||||
};
|
||||
}
|
||||
|
||||
async function createRuntime() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
return runtime;
|
||||
}
|
||||
|
||||
describe("cycle-time, resample, and diagnostics utilities", () => {
|
||||
it("estimates cycle time for a trajectory and a path plan", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const singleResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "estimateCycleTime",
|
||||
payload: [trajectory()]
|
||||
});
|
||||
expect(singleResponse.result).toMatchObject({
|
||||
ok: true,
|
||||
motionTime: 1,
|
||||
totalTime: 1,
|
||||
segmentTimes: [
|
||||
{
|
||||
segmentId: "s1",
|
||||
motion: "MOVEJ",
|
||||
duration: 1
|
||||
}
|
||||
],
|
||||
diagnostics: []
|
||||
});
|
||||
|
||||
const path: PathPlanResult = {
|
||||
ok: true,
|
||||
duration: 3,
|
||||
segments: [
|
||||
trajectory(),
|
||||
trajectory({
|
||||
motion: "MOVEL",
|
||||
duration: 2,
|
||||
points: trajectory().points.map((point) => ({ ...point, motion: "MOVEL", segmentId: "s2" }))
|
||||
})
|
||||
],
|
||||
points: [],
|
||||
diagnostics: []
|
||||
};
|
||||
const pathResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "estimateCycleTime",
|
||||
payload: [path]
|
||||
});
|
||||
expect(pathResponse.result).toMatchObject({
|
||||
ok: true,
|
||||
motionTime: 3,
|
||||
totalTime: 3,
|
||||
segmentTimes: [
|
||||
{ segmentId: "s1", motion: "MOVEJ", duration: 1 },
|
||||
{ segmentId: "s2", motion: "MOVEL", duration: 2 }
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("resamples a trajectory with stable time and point ordering", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "resampleTrajectory",
|
||||
payload: [trajectory(), 0.25]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const result = response.result as TrajectoryResult;
|
||||
expect(result.sampleTime).toBe(0.25);
|
||||
expect(result.points.map((point) => point.time)).toEqual([0, 0.25, 0.5, 0.75, 1]);
|
||||
expect(result.points.map((point) => point.index)).toEqual([0, 1, 2, 3, 4]);
|
||||
expect(result.points[2]?.joints).toEqual([0.5, 1]);
|
||||
expect(result.points[2]?.tcp.position).toEqual([0.5, 1, 0]);
|
||||
expect(result.diagnostics).toContainEqual(
|
||||
expect.objectContaining({
|
||||
severity: "info",
|
||||
code: "KDL_TRAJECTORY_RESAMPLED"
|
||||
})
|
||||
);
|
||||
});
|
||||
|
||||
it("keeps structured diagnostics for warning and error cases", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const emptyResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "resampleTrajectory",
|
||||
payload: [
|
||||
trajectory({
|
||||
points: []
|
||||
}),
|
||||
0.1
|
||||
]
|
||||
});
|
||||
expect(emptyResponse.result).toMatchObject({
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "warning",
|
||||
code: "KDL_RESAMPLE_EMPTY_TRAJECTORY"
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
const invalidResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "resampleTrajectory",
|
||||
payload: [trajectory(), 0]
|
||||
});
|
||||
expect(invalidResponse).toMatchObject({
|
||||
ok: false,
|
||||
error: {
|
||||
code: "KDL_INVALID_SAMPLE_TIME",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_INVALID_SAMPLE_TIME"
|
||||
}
|
||||
]
|
||||
}
|
||||
});
|
||||
});
|
||||
});
|
||||
138
kdl-wasm/web/tests/kdl/trapProfile.test.ts
Normal file
138
kdl-wasm/web/tests/kdl/trapProfile.test.ts
Normal file
@@ -0,0 +1,138 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import type { TrapProfileResult, TrapSample } from "../../src/kdl/types.js";
|
||||
|
||||
async function createRuntime() {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
return runtime;
|
||||
}
|
||||
|
||||
function expectMonotonic(samples: TrapSample[]) {
|
||||
for (let index = 1; index < samples.length; index += 1) {
|
||||
expect(samples[index]!.time).toBeGreaterThan(samples[index - 1]!.time);
|
||||
expect(samples[index]!.s).toBeGreaterThanOrEqual(samples[index - 1]!.s);
|
||||
}
|
||||
}
|
||||
|
||||
describe("trapezoid velocity profile API", () => {
|
||||
it("creates a trapezoid profile when the path can reach max velocity", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "makeTrapProfile",
|
||||
payload: [
|
||||
2,
|
||||
{
|
||||
maxVelocity: 1,
|
||||
maxAcceleration: 1,
|
||||
sampleTime: 0.25
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const profile = response.result as TrapProfileResult;
|
||||
expect(profile).toMatchObject({
|
||||
ok: true,
|
||||
type: "trapezoid",
|
||||
length: 2,
|
||||
duration: 3,
|
||||
tAccel: 1,
|
||||
tConst: 1,
|
||||
tDecel: 1,
|
||||
vPeak: 1,
|
||||
diagnostics: []
|
||||
});
|
||||
expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
|
||||
expect(profile.samples.at(-1)).toMatchObject({ time: 3, s: 1 });
|
||||
expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
|
||||
expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
|
||||
expectMonotonic(profile.samples);
|
||||
});
|
||||
|
||||
it("falls back to a triangle profile for short paths", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "makeTrapProfile",
|
||||
payload: [
|
||||
0.5,
|
||||
{
|
||||
maxVelocity: 2,
|
||||
maxAcceleration: 1,
|
||||
sampleTime: 0.1
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const profile = response.result as TrapProfileResult;
|
||||
expect(profile.type).toBe("triangle");
|
||||
expect(profile.tConst).toBe(0);
|
||||
expect(profile.vPeak).toBeCloseTo(Math.sqrt(0.5));
|
||||
expect(profile.duration).toBeCloseTo(2 * Math.sqrt(0.5));
|
||||
expect(profile.samples[0]?.s).toBe(0);
|
||||
expect(profile.samples.at(-1)?.s).toBe(1);
|
||||
expect(profile.diagnostics).toMatchObject([
|
||||
{
|
||||
severity: "info",
|
||||
code: "KDL_TRAP_TRIANGLE_PROFILE"
|
||||
}
|
||||
]);
|
||||
expectMonotonic(profile.samples);
|
||||
});
|
||||
|
||||
it("returns samples from sampleTrapProfile with strict endpoint samples", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "sampleTrapProfile",
|
||||
payload: [
|
||||
1,
|
||||
{
|
||||
maxVelocity: 1,
|
||||
maxAcceleration: 2,
|
||||
sampleTime: 0.2
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response.ok).toBe(true);
|
||||
const samples = response.result as TrapSample[];
|
||||
expect(samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
|
||||
expect(samples.at(-1)?.s).toBe(1);
|
||||
expect(samples.at(-1)?.time).toBeCloseTo(1.5);
|
||||
expectMonotonic(samples);
|
||||
});
|
||||
|
||||
it("returns a structured diagnostic for invalid trap profile inputs", async () => {
|
||||
const runtime = await createRuntime();
|
||||
const response = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "makeTrapProfile",
|
||||
payload: [
|
||||
1,
|
||||
{
|
||||
maxVelocity: 0,
|
||||
maxAcceleration: 1,
|
||||
sampleTime: 0.01
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(response).toMatchObject({
|
||||
ok: false,
|
||||
error: {
|
||||
code: "KDL_INVALID_TRAP_PROFILE",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_INVALID_TRAP_PROFILE"
|
||||
}
|
||||
]
|
||||
}
|
||||
});
|
||||
});
|
||||
});
|
||||
182
kdl-wasm/web/tests/kdl/urdf.test.ts
Normal file
182
kdl-wasm/web/tests/kdl/urdf.test.ts
Normal file
@@ -0,0 +1,182 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { KdlStructuredError } from "../../src/kdl/rpc.js";
|
||||
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
||||
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
||||
import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js";
|
||||
|
||||
const SIMPLE_URDF = `
|
||||
<robot name="simple6">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="link_2"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<origin xyz="0 0 0.1" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
|
||||
</joint>
|
||||
<joint name="joint_2" type="prismatic">
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 1.57"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="0" upper="0.4" velocity="0.3" acceleration="1.2"/>
|
||||
</joint>
|
||||
<joint name="tool_fixed" type="fixed">
|
||||
<parent link="link_2"/>
|
||||
<child link="tool0"/>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
describe("URDF to NormalizedRobotModel", () => {
|
||||
it("parses links, joints, origins, axes, limits, stable active joint names, and source hash", () => {
|
||||
const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
|
||||
robotId: "r1",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
});
|
||||
|
||||
expect(model).toMatchObject({
|
||||
robotId: "r1",
|
||||
name: "simple6",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0",
|
||||
activeJointNames: ["joint_1", "joint_2"],
|
||||
source: { type: "urdf" }
|
||||
});
|
||||
expect(model.source.urdfHash).toHaveLength(64);
|
||||
expect(model.links.map((link) => link.name)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
|
||||
expect(model.joints[0]).toMatchObject({
|
||||
name: "joint_1",
|
||||
type: "revolute",
|
||||
parent: "base_link",
|
||||
child: "link_1",
|
||||
origin: { xyz: [0, 0, 0.1], rpy: [0, 0, 0] },
|
||||
axis: [0, 0, 1]
|
||||
});
|
||||
expect(model.limits).toEqual([
|
||||
{ name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
|
||||
{ name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
|
||||
]);
|
||||
});
|
||||
|
||||
it("applies joint order and limit overrides", () => {
|
||||
const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
|
||||
robotId: "r1",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0",
|
||||
jointOrder: ["joint_2", "joint_1"],
|
||||
overrideLimits: [{ name: "joint_2", velocity: 0.2 }]
|
||||
});
|
||||
|
||||
expect(model.activeJointNames).toEqual(["joint_2", "joint_1"]);
|
||||
expect(model.limits[0]).toMatchObject({ name: "joint_2", velocity: 0.2 });
|
||||
});
|
||||
|
||||
it("returns structured diagnostics for disconnected base and tip links", () => {
|
||||
let thrown: unknown;
|
||||
try {
|
||||
loadRobotFromUrdfModel(SIMPLE_URDF, {
|
||||
robotId: "r1",
|
||||
baseLink: "tool0",
|
||||
tipLink: "base_link"
|
||||
});
|
||||
} catch (error) {
|
||||
thrown = error;
|
||||
}
|
||||
|
||||
expect(thrown).toBeInstanceOf(KdlStructuredError);
|
||||
expect(thrown).toMatchObject({
|
||||
code: "KDL_INVALID_MODEL",
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_INVALID_MODEL"
|
||||
}
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("rejects unsupported joint types", () => {
|
||||
const urdf = SIMPLE_URDF.replace('type="prismatic"', 'type="floating"');
|
||||
let thrown: unknown;
|
||||
try {
|
||||
loadRobotFromUrdfModel(urdf, {
|
||||
robotId: "r1",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
});
|
||||
} catch (error) {
|
||||
thrown = error;
|
||||
}
|
||||
|
||||
expect(thrown).toBeInstanceOf(KdlStructuredError);
|
||||
expect(thrown).toMatchObject({
|
||||
code: "KDL_INVALID_MODEL",
|
||||
message: expect.stringContaining("Unsupported joint type")
|
||||
});
|
||||
});
|
||||
|
||||
it("supports RobotHandle lifecycle through the worker runtime", async () => {
|
||||
const runtime = createKdlWorkerRuntime();
|
||||
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
||||
const createResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 2,
|
||||
method: "loadRobotFromUrdf",
|
||||
payload: [
|
||||
SIMPLE_URDF,
|
||||
{
|
||||
robotId: "r1",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(createResponse).toMatchObject({ ok: true, result: 1 });
|
||||
|
||||
const infoResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 3,
|
||||
method: "getRobotInfo",
|
||||
payload: [1]
|
||||
});
|
||||
expect(infoResponse.result).toMatchObject({
|
||||
handle: 1,
|
||||
robotId: "r1",
|
||||
name: "simple6",
|
||||
dof: 2,
|
||||
jointNames: ["joint_1", "joint_2"]
|
||||
});
|
||||
|
||||
const limitsResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 4,
|
||||
method: "getJointLimits",
|
||||
payload: [1]
|
||||
});
|
||||
expect(limitsResponse.result).toEqual([
|
||||
{ name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
|
||||
{ name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
|
||||
]);
|
||||
|
||||
const destroyResponse = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 5,
|
||||
method: "destroyRobot",
|
||||
payload: [1]
|
||||
});
|
||||
expect(destroyResponse.ok).toBe(true);
|
||||
|
||||
const afterDestroy = await dispatchKdlRpcRequest(runtime, {
|
||||
id: 6,
|
||||
method: "getRobotInfo",
|
||||
payload: [1]
|
||||
});
|
||||
expect(afterDestroy).toMatchObject({
|
||||
ok: false,
|
||||
error: { code: "KDL_INVALID_HANDLE" }
|
||||
});
|
||||
});
|
||||
});
|
||||
88
kdl-wasm/web/tests/post/postProcessor.test.ts
Normal file
88
kdl-wasm/web/tests/post/postProcessor.test.ts
Normal file
@@ -0,0 +1,88 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { postProcessAllBrands } from "../../src/grl/post/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../../src/grl/semantic/index.js";
|
||||
|
||||
const PROGRAM = `language grl 0.1
|
||||
module PostDemo
|
||||
post_hint abb
|
||||
const speed vj = joint(50 %)
|
||||
const speed vl = linear(200 mm/s)
|
||||
const zone z10 = z(10 mm)
|
||||
target home = joint_target { joints: [0 deg] }
|
||||
target pick = pose_target { pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) }
|
||||
target mid = pose_target { pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg) }
|
||||
target place = pose_target { pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) }
|
||||
proc main()
|
||||
set_speed vl
|
||||
set_zone z10
|
||||
movej home speed vj zone fine
|
||||
movel pick speed vl zone z10
|
||||
movec via mid target place speed vl zone fine
|
||||
io.do[1] = true
|
||||
wait io.di[1] == true timeout 1 s
|
||||
pulse io.do[2] duration 100 ms
|
||||
alarm DONE "done"
|
||||
end
|
||||
end
|
||||
`;
|
||||
|
||||
function postAll() {
|
||||
const ir = compileSemanticProgram(parseGrl(PROGRAM), {
|
||||
startJoints: [0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
return postProcessAllBrands(ir);
|
||||
}
|
||||
|
||||
describe("GRL multi-brand postprocessor", () => {
|
||||
it("emits stable ABB, FANUC, and KUKA golden text", () => {
|
||||
const result = postAll();
|
||||
|
||||
expect(result.outputs.abb.text).toBe(`MODULE PostDemo
|
||||
PROC main()
|
||||
MoveJ home,v50,fine,tool0;
|
||||
MoveL pick,v200,z10,tool0;
|
||||
MoveC mid,place,v200,fine,tool0;
|
||||
SetDO io.do[1],TRUE;
|
||||
WaitUntil io . di [ 1 ] == true;
|
||||
PulseDO io.do[2],0.100;
|
||||
! unsupported ALARM
|
||||
ENDPROC
|
||||
ENDMODULE`);
|
||||
expect(result.outputs.fanuc.text).toBe(`/PROG MAIN
|
||||
/MN
|
||||
1: J home 50% FINE ;
|
||||
2: L pick 200mm/sec CNT10 ;
|
||||
3: C mid place 200mm/sec FINE ;
|
||||
4: DO[1]=TRUE ;
|
||||
5: WAIT (io . di [ 1 ] == true) ;
|
||||
6: PULSE DO[2] 100ms ;
|
||||
7: ! unsupported ALARM ;
|
||||
/END`);
|
||||
expect(result.outputs.kuka.text).toBe(`DEF Main()
|
||||
PTP home Vel=50%
|
||||
LIN pick Vel=0.200m/s C_DIS
|
||||
CIRC mid, place Vel=0.200m/s
|
||||
$OUT[1] = TRUE
|
||||
WAIT FOR io . di [ 1 ] == true
|
||||
PULSE $OUT[2] 0.100
|
||||
! unsupported ALARM
|
||||
END`);
|
||||
});
|
||||
|
||||
it("reports unsupported semantics and ignored brand hints", () => {
|
||||
const result = postAll();
|
||||
|
||||
expect(result.outputs.abb.filename).toBe("PostDemo.mod");
|
||||
expect(result.outputs.fanuc.filename).toBe("PostDemo.ls");
|
||||
expect(result.outputs.kuka.filename).toBe("PostDemo.src");
|
||||
expect(result.report).toEqual([
|
||||
expect.objectContaining({ brand: "abb", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") }),
|
||||
expect.objectContaining({ brand: "fanuc", code: "GRL_POST_HINT_IGNORED" }),
|
||||
expect.objectContaining({ brand: "fanuc", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") }),
|
||||
expect.objectContaining({ brand: "kuka", code: "GRL_POST_HINT_IGNORED" }),
|
||||
expect.objectContaining({ brand: "kuka", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") })
|
||||
]);
|
||||
});
|
||||
});
|
||||
Reference in New Issue
Block a user