252 lines
7.3 KiB
TypeScript
252 lines
7.3 KiB
TypeScript
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" }));
|
|
});
|
|
});
|