416 lines
14 KiB
TypeScript
416 lines
14 KiB
TypeScript
import { describe, expect, it } from "vitest";
|
|
import type { GrlDataDeclaration, GrlPathDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
|
|
import { parseGrl } from "../../src/grl/parser/index.js";
|
|
import { postProcessAllBrands } from "../../src/grl/post/index.js";
|
|
import {
|
|
buildMotionContext,
|
|
compilePathToPlanRequest,
|
|
parseProcedureRunPathStatements
|
|
} from "../../src/grl/semantic/index.js";
|
|
import { importBrandProgram } from "../../src/importers/index.js";
|
|
import type { KdlRuntimeHandlers } from "../../src/kdl/rpc.js";
|
|
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
|
|
import type {
|
|
FkResult,
|
|
IkResult,
|
|
JacobianResult,
|
|
KdlApiMethod,
|
|
LimitCheckResult,
|
|
MoveJRequest,
|
|
PathPlanRequest,
|
|
PathPlanResult,
|
|
RobotInfo,
|
|
SingularityResult,
|
|
TrajectoryResult
|
|
} from "../../src/kdl/types.js";
|
|
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
|
|
import { KdlRuntimeBridge, MotionQueue, type MotionPlanner } from "../../src/runtime/index.js";
|
|
import {
|
|
ABB_IRB120_3_58_URDF,
|
|
ABB_IRB120_HOME_TO_PICK_JOINTS,
|
|
ABB_IRB120_LOAD_OPTIONS,
|
|
ABB_IRB120_PICK_JOINTS,
|
|
ABB_IRB120_PLACE_JOINTS,
|
|
ABB_IRB120_URDF_SOURCE,
|
|
ABB_IRB120_ZERO_JOINTS
|
|
} from "../fixtures/abbIrb120.js";
|
|
|
|
const ABB120_GRL_PROGRAM = `language grl 0.1
|
|
module Abb120Cell
|
|
const speed v_fast = joint(40 %)
|
|
const speed v_slow = joint(20 %)
|
|
const speed v_linear = linear(150 mm/s)
|
|
const zone z10 = z(10 mm)
|
|
target home = joint_target {
|
|
joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg]
|
|
}
|
|
target pick = joint_target {
|
|
joints: [11.459156 deg, -20.053523 deg, 25.783101 deg, 5.729578 deg, -11.459156 deg, 17.188734 deg]
|
|
}
|
|
target place = joint_target {
|
|
joints: [-20.053523 deg, -14.323945 deg, 20.053523 deg, -14.323945 deg, 8.594367 deg, -22.918312 deg]
|
|
}
|
|
target unreachable_pose = pose_target {
|
|
pose: pose(300 mm, 100 mm, 500 mm, 0 deg, 90 deg, 0 deg)
|
|
}
|
|
path joint_pick_place {
|
|
defaults {
|
|
speed: v_fast,
|
|
zone: z10
|
|
}
|
|
point p_home movej home zone fine
|
|
point p_pick movej pick speed v_slow
|
|
point p_place movej place
|
|
event before p_pick io.do[1] = true
|
|
event after p_place io.do[1] = false
|
|
}
|
|
proc main()
|
|
run_path joint_pick_place
|
|
movej unreachable_pose speed v_linear zone fine
|
|
end
|
|
end
|
|
`;
|
|
|
|
const ABB120_RAPID = `MODULE ABB120_PICK_PLACE
|
|
CONST jointtarget jHome := [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
|
CONST jointtarget jPick := [[11.459156,-20.053523,25.783101,5.729578,-11.459156,17.188734],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
|
CONST jointtarget jPlace := [[-20.053523,-14.323945,20.053523,-14.323945,8.594367,-22.918312],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
|
CONST robtarget pScan := [[300,100,500],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
|
PROC main()
|
|
MoveJ jHome,v100,fine,tool0;
|
|
MoveJ jPick,v50,z10,tool0;
|
|
MoveJ jPlace,v50,fine,tool0;
|
|
MoveL pScan,v100,fine,tool0;
|
|
ENDPROC
|
|
ENDMODULE`;
|
|
|
|
async function createAbb120Robot() {
|
|
const runtime = createKdlWorkerRuntime();
|
|
await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
|
|
const response = await dispatchKdlRpcRequest(runtime, {
|
|
id: 2,
|
|
method: "loadRobotFromUrdf",
|
|
payload: [ABB_IRB120_3_58_URDF, ABB_IRB120_LOAD_OPTIONS]
|
|
});
|
|
expect(response.ok).toBe(true);
|
|
return { runtime, handle: response.result as number };
|
|
}
|
|
|
|
async function rpc<T>(
|
|
runtime: KdlRuntimeHandlers,
|
|
id: number,
|
|
method: KdlApiMethod,
|
|
payload: unknown[]
|
|
): Promise<T> {
|
|
const response = await dispatchKdlRpcRequest(runtime, { id, method, payload });
|
|
expect(response.ok).toBe(true);
|
|
return response.result as T;
|
|
}
|
|
|
|
function abb120PathRequest(): PathPlanRequest {
|
|
const program = parseGrl(ABB120_GRL_PROGRAM);
|
|
const declarations = program.module.declarations;
|
|
const path = declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
|
const context = buildMotionContext(
|
|
declarations.filter(
|
|
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
|
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
|
)
|
|
);
|
|
return compilePathToPlanRequest(path, context, {
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
sampleTime: 0.02
|
|
}).request;
|
|
}
|
|
|
|
function moveJRequest(targetJoints: readonly number[]): MoveJRequest {
|
|
return {
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
target: {
|
|
id: "abb120_pick",
|
|
joints: [...targetJoints]
|
|
},
|
|
speed: {
|
|
kind: "joint_percent",
|
|
value: 0.35
|
|
},
|
|
zone: {
|
|
kind: "fine"
|
|
},
|
|
sampleTime: 0.02
|
|
};
|
|
}
|
|
|
|
function expectCloseArray(actual: number[] | undefined, expected: readonly number[], digits = 6): void {
|
|
expect(actual).toHaveLength(expected.length);
|
|
for (const [index, expectedValue] of expected.entries()) {
|
|
expect(actual?.[index]).toBeCloseTo(expectedValue, digits);
|
|
}
|
|
}
|
|
|
|
describe("ABB IRB120 programs with ROS-Industrial URDF", () => {
|
|
it("loads the ABB120 URDF fixture with source provenance and expected 6R chain", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const info = await rpc<RobotInfo>(runtime, 3, "getRobotInfo", [handle]);
|
|
|
|
expect(ABB_IRB120_URDF_SOURCE).toMatchObject({
|
|
repository: "https://github.com/ros-industrial/abb",
|
|
entrypoint: "abb_irb120_support/urdf/irb120_3_58.xacro"
|
|
});
|
|
expect(info).toMatchObject({
|
|
robotId: "abb_irb120_3_58",
|
|
name: "abb_irb120_3_58",
|
|
baseLink: "base_link",
|
|
tipLink: "tool0",
|
|
dof: 6,
|
|
jointNames: ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
|
|
});
|
|
expect(info.limits.map((limit) => [limit.name, limit.lower, limit.upper])).toEqual([
|
|
["joint_1", -2.87979, 2.87979],
|
|
["joint_2", -1.91986, 1.91986],
|
|
["joint_3", -1.91986, 1.22173],
|
|
["joint_4", -2.79253, 2.79253],
|
|
["joint_5", -2.094395, 2.094395],
|
|
["joint_6", -6.98132, 6.98132]
|
|
]);
|
|
});
|
|
|
|
it("runs FK, link poses, Jacobian, limit checks, and singularity diagnostics", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const zeroFk = await rpc<FkResult>(runtime, 4, "fk", [handle, [...ABB_IRB120_ZERO_JOINTS]]);
|
|
const pickFk = await rpc<FkResult>(runtime, 5, "fk", [handle, [...ABB_IRB120_PICK_JOINTS]]);
|
|
const links = await rpc<{ linkPoses: Array<{ link: string }> }>(runtime, 6, "fkAllLinks", [
|
|
handle,
|
|
[...ABB_IRB120_ZERO_JOINTS]
|
|
]);
|
|
const jacobian = await rpc<JacobianResult>(runtime, 7, "jacobian", [handle, [...ABB_IRB120_PICK_JOINTS]]);
|
|
const limits = await rpc<LimitCheckResult>(runtime, 8, "checkJointLimits", [
|
|
handle,
|
|
[3.2, 0, 0, 0, 0, 0]
|
|
]);
|
|
const singularity = await rpc<SingularityResult>(runtime, 9, "checkSingularity", [
|
|
handle,
|
|
[...ABB_IRB120_ZERO_JOINTS]
|
|
]);
|
|
|
|
expect(zeroFk.tcp.position[0]).toBeCloseTo(0.374);
|
|
expect(zeroFk.tcp.position[1]).toBeCloseTo(0);
|
|
expect(zeroFk.tcp.position[2]).toBeCloseTo(0.63);
|
|
expect(zeroFk.tcp.quaternion[1]).toBeCloseTo(Math.SQRT1_2);
|
|
expect(zeroFk.tcp.quaternion[3]).toBeCloseTo(Math.SQRT1_2);
|
|
expect(pickFk.tcp.position).not.toEqual(zeroFk.tcp.position);
|
|
expect(links.linkPoses.map((entry) => entry.link)).toEqual([
|
|
"base_link",
|
|
"link_1",
|
|
"link_2",
|
|
"link_3",
|
|
"link_4",
|
|
"link_5",
|
|
"link_6",
|
|
"flange",
|
|
"tool0"
|
|
]);
|
|
expect(jacobian).toMatchObject({ ok: true, rows: 6, cols: 6 });
|
|
expect(Array.from(jacobian.data)).toHaveLength(36);
|
|
expect(limits.ok).toBe(false);
|
|
expect(limits.diagnostics[0]).toMatchObject({
|
|
code: "KDL_JOINT_LIMIT",
|
|
severity: "error"
|
|
});
|
|
expect(singularity.ok).toBe(true);
|
|
expect(typeof singularity.nearSingularity).toBe("boolean");
|
|
expect(Number.isFinite(singularity.manipulability)).toBe(true);
|
|
expect(Number.isFinite(singularity.conditionNumber)).toBe(true);
|
|
});
|
|
|
|
it("plans ABB120 MoveJ and multi-segment joint path programs", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const moveJ = await rpc<TrajectoryResult>(runtime, 10, "planMoveJ", [
|
|
handle,
|
|
moveJRequest(ABB_IRB120_PICK_JOINTS)
|
|
]);
|
|
const path = await rpc<PathPlanResult>(runtime, 11, "planPath", [handle, abb120PathRequest()]);
|
|
const validation = await rpc<{ ok: boolean; reachable: boolean; cycleTime?: number }>(runtime, 12, "validatePath", [
|
|
handle,
|
|
abb120PathRequest()
|
|
]);
|
|
|
|
expect(moveJ).toMatchObject({
|
|
ok: true,
|
|
motion: "MOVEJ",
|
|
meta: {
|
|
targetType: "joint",
|
|
qStart: [...ABB_IRB120_ZERO_JOINTS],
|
|
qEnd: [...ABB_IRB120_PICK_JOINTS]
|
|
}
|
|
});
|
|
expect(moveJ.points.length).toBeGreaterThan(2);
|
|
expectCloseArray(moveJ.points.at(-1)?.joints, ABB_IRB120_PICK_JOINTS);
|
|
expect(path.ok).toBe(true);
|
|
expect(path.segments.map((segment) => segment.motion)).toEqual(["MOVEJ", "MOVEJ", "MOVEJ"]);
|
|
expectCloseArray(path.points.at(-1)?.joints, ABB_IRB120_PLACE_JOINTS);
|
|
expect(validation).toMatchObject({
|
|
ok: true,
|
|
reachable: true
|
|
});
|
|
expect(validation.cycleTime).toBeGreaterThan(0);
|
|
});
|
|
|
|
it("compiles GRL path/procedure, posts all brands, and replays through MotionQueue", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const program = parseGrl(ABB120_GRL_PROGRAM);
|
|
const pathRequest = abb120PathRequest();
|
|
const ir = await import("../../src/grl/semantic/index.js").then(({ compileSemanticProgram }) =>
|
|
compileSemanticProgram(program, {
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
sampleTime: 0.02
|
|
})
|
|
);
|
|
const procedure = program.module.declarations.find((decl) => decl.kind === "ProcedureDeclaration");
|
|
expect(procedure && parseProcedureRunPathStatements(procedure).map((instruction) => instruction.pathId)).toEqual([
|
|
"joint_pick_place"
|
|
]);
|
|
expect(ir.kdlBridge.pathRequests[0]).toMatchObject({
|
|
pathId: "joint_pick_place",
|
|
segments: [
|
|
{ id: "p_home", motion: "MOVEJ" },
|
|
{ id: "p_pick", motion: "MOVEJ" },
|
|
{ id: "p_place", motion: "MOVEJ" }
|
|
]
|
|
});
|
|
expect(ir.procedures[0]?.instructions.map((instruction) => instruction.kind)).toEqual([
|
|
"RUN_PATH",
|
|
"MOVEJ"
|
|
]);
|
|
|
|
const posted = postProcessAllBrands(ir);
|
|
expect(posted.outputs.abb.text).toContain("MoveJ unreachable_pose,v150,fine,tool0;");
|
|
expect(posted.outputs.fanuc.text).toContain("J unreachable_pose 150mm/sec FINE");
|
|
expect(posted.outputs.kuka.text).toContain("PTP unreachable_pose Vel=0.150m/s");
|
|
|
|
const planner: MotionPlanner = {
|
|
planPath: (_robotHandle, request) =>
|
|
dispatchKdlRpcRequest(runtime, {
|
|
id: 20,
|
|
method: "planPath",
|
|
payload: [handle, request]
|
|
}).then((response) => response.result as PathPlanResult)
|
|
};
|
|
const queue = new MotionQueue({
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
sampleTime: 0.02,
|
|
robotHandle: handle,
|
|
planner
|
|
});
|
|
const bridge = new KdlRuntimeBridge(ir, queue);
|
|
const pathId = pathRequest.pathId;
|
|
expect(pathId).toBe("joint_pick_place");
|
|
const items = bridge.enqueueInstruction({ kind: "RUN_PATH", pathId: pathId! });
|
|
await queue.planAll();
|
|
const plannedDuration = queue.snapshot().items[0]?.planned?.duration ?? 0;
|
|
const halfway = queue.advance(plannedDuration / 2);
|
|
|
|
expect(items).toHaveLength(1);
|
|
expect(items[0]).toMatchObject({
|
|
kind: "path",
|
|
source: {
|
|
pathId: "joint_pick_place"
|
|
}
|
|
});
|
|
expect(queue.snapshot().items[0]?.planned).toMatchObject({
|
|
ok: true
|
|
});
|
|
expect(halfway?.source).toMatchObject({
|
|
pathId: "joint_pick_place"
|
|
});
|
|
expect(halfway?.point.joints).toHaveLength(6);
|
|
});
|
|
|
|
it("solves 6R ABB120 pose IK and reports straight-line MOVEL sampling limits", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const pickPose = await rpc<FkResult>(runtime, 30, "fk", [handle, [...ABB_IRB120_HOME_TO_PICK_JOINTS]]);
|
|
const ik = await rpc<IkResult>(runtime, 31, "ik", [
|
|
handle,
|
|
[...ABB_IRB120_ZERO_JOINTS],
|
|
pickPose.tcp,
|
|
{ positionTolerance: 1e-6 }
|
|
]);
|
|
const moveL = await rpc<TrajectoryResult>(runtime, 32, "planMoveL", [
|
|
handle,
|
|
{
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
target: {
|
|
id: "fk_pose_target",
|
|
pose: pickPose.tcp
|
|
},
|
|
speed: {
|
|
kind: "linear",
|
|
velocity: 0.1
|
|
},
|
|
zone: {
|
|
kind: "fine"
|
|
},
|
|
sampleTime: 0.02
|
|
}
|
|
]);
|
|
|
|
expect(ik).toMatchObject({
|
|
ok: true,
|
|
diagnostics: []
|
|
});
|
|
expect(ik.joints).toHaveLength(6);
|
|
expect(ik.residualPosition).toBeLessThanOrEqual(1e-6);
|
|
expect(moveL).toMatchObject({
|
|
ok: false,
|
|
motion: "MOVEL",
|
|
meta: {
|
|
targetId: "fk_pose_target"
|
|
}
|
|
});
|
|
expect(moveL.diagnostics).toContainEqual(
|
|
expect.objectContaining({
|
|
severity: "error"
|
|
})
|
|
);
|
|
});
|
|
|
|
it("imports an ABB RAPID program with ABB120-style joint targets and reports unreachable pose moves", async () => {
|
|
const { runtime, handle } = await createAbb120Robot();
|
|
const imported = importBrandProgram("abb", [{ name: "abb120.mod", text: ABB120_RAPID }], {
|
|
projectName: "Abb120Imported",
|
|
generatedAt: "2026-06-27T00:00:00.000Z"
|
|
});
|
|
const pathRequest = imported.ir.kdlBridge.pathRequests[0]!;
|
|
const planned = await rpc<PathPlanResult>(runtime, 40, "planPath", [
|
|
handle,
|
|
{
|
|
...pathRequest,
|
|
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
|
sampleTime: 0.02,
|
|
stopOnError: false
|
|
}
|
|
]);
|
|
|
|
expect(imported.report.status).toBe("warn");
|
|
expect(imported.ir.paths[0]?.motions.map((motion) => motion.kind)).toEqual([
|
|
"MOVEJ",
|
|
"MOVEJ",
|
|
"MOVEJ",
|
|
"MOVEL"
|
|
]);
|
|
expect(postProcessAllBrands(imported.ir).outputs.abb.text).toContain("MoveL pScan");
|
|
expect(planned.segments.map((segment) => [segment.motion, segment.ok])).toEqual([
|
|
["MOVEJ", true],
|
|
["MOVEJ", true],
|
|
["MOVEJ", true],
|
|
["MOVEL", false]
|
|
]);
|
|
expect(planned.diagnostics).toContainEqual(
|
|
expect.objectContaining({
|
|
code: "KDL_TARGET_UNREACHABLE",
|
|
segmentId: "import_p03"
|
|
})
|
|
);
|
|
});
|
|
});
|