125 lines
4.2 KiB
TypeScript
125 lines
4.2 KiB
TypeScript
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
|
|
});
|
|
});
|
|
});
|