同步KDL工程源码到云仓库
This commit is contained in:
wangdequan
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" }));
|
||||
});
|
||||
});
|
||||
Reference in New Issue
Block a user