Upload project files
This commit is contained in:
wangdequan
185
kdl-wasm/web/app/virtual-controller.css
Normal file
185
kdl-wasm/web/app/virtual-controller.css
Normal file
@@ -0,0 +1,185 @@
|
||||
* {
|
||||
box-sizing: border-box;
|
||||
}
|
||||
|
||||
body {
|
||||
margin: 0;
|
||||
font-family: Arial, sans-serif;
|
||||
color: #1f2933;
|
||||
background: #eef2f4;
|
||||
}
|
||||
|
||||
.workbench {
|
||||
min-height: 100vh;
|
||||
display: grid;
|
||||
grid-template-columns: 240px minmax(320px, 1fr) 280px;
|
||||
grid-template-rows: 48px minmax(280px, 1fr) 180px;
|
||||
gap: 1px;
|
||||
background: #c7d0d6;
|
||||
}
|
||||
|
||||
.command-bar,
|
||||
.object-tree,
|
||||
.viewport,
|
||||
.pendant,
|
||||
.editor,
|
||||
.bottom-panel {
|
||||
background: #f8fafb;
|
||||
}
|
||||
|
||||
.command-bar {
|
||||
grid-column: 1 / 4;
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
padding: 8px 12px;
|
||||
}
|
||||
|
||||
button {
|
||||
min-height: 32px;
|
||||
border: 1px solid #9aa8b2;
|
||||
background: #ffffff;
|
||||
color: #1f2933;
|
||||
border-radius: 4px;
|
||||
}
|
||||
|
||||
.object-tree {
|
||||
padding: 12px;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.object-tree h2,
|
||||
.pendant h2 {
|
||||
margin: 0 0 8px;
|
||||
font-size: 16px;
|
||||
}
|
||||
|
||||
.object-tree ul {
|
||||
margin: 0;
|
||||
padding-left: 18px;
|
||||
line-height: 1.8;
|
||||
}
|
||||
|
||||
.viewport {
|
||||
position: relative;
|
||||
overflow: hidden;
|
||||
min-height: 320px;
|
||||
background: linear-gradient(#f8fafb, #dfe7eb);
|
||||
}
|
||||
|
||||
.viewport svg {
|
||||
position: absolute;
|
||||
inset: auto 24px 24px 24px;
|
||||
height: 45%;
|
||||
width: calc(100% - 48px);
|
||||
}
|
||||
|
||||
.robot-arm {
|
||||
position: absolute;
|
||||
left: 12%;
|
||||
top: 20%;
|
||||
width: 62%;
|
||||
height: 48%;
|
||||
}
|
||||
|
||||
.joint {
|
||||
position: absolute;
|
||||
width: 18px;
|
||||
height: 18px;
|
||||
border-radius: 50%;
|
||||
background: #334e68;
|
||||
}
|
||||
|
||||
.link {
|
||||
position: absolute;
|
||||
height: 8px;
|
||||
border-radius: 4px;
|
||||
background: #627d98;
|
||||
transform-origin: left center;
|
||||
}
|
||||
|
||||
.j1 { left: 20px; top: 150px; }
|
||||
.l1 { left: 35px; top: 155px; width: 115px; transform: rotate(-45deg); }
|
||||
.j2 { left: 122px; top: 72px; }
|
||||
.l2 { left: 137px; top: 78px; width: 120px; transform: rotate(20deg); }
|
||||
.j3 { left: 250px; top: 115px; }
|
||||
.l3 { left: 265px; top: 120px; width: 95px; transform: rotate(-15deg); }
|
||||
.j4 { left: 356px; top: 92px; }
|
||||
|
||||
.pendant {
|
||||
padding: 12px;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.pendant dl {
|
||||
display: grid;
|
||||
grid-template-columns: 84px 1fr;
|
||||
gap: 8px;
|
||||
margin: 0;
|
||||
}
|
||||
|
||||
.pendant dt {
|
||||
font-weight: 700;
|
||||
}
|
||||
|
||||
.editor {
|
||||
grid-column: 1 / 3;
|
||||
padding: 12px;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.editor nav {
|
||||
display: flex;
|
||||
gap: 6px;
|
||||
margin-bottom: 8px;
|
||||
}
|
||||
|
||||
.editor pre {
|
||||
margin: 0;
|
||||
padding: 12px;
|
||||
background: #1f2933;
|
||||
color: #f8fafb;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.bottom-panel {
|
||||
padding: 12px;
|
||||
display: grid;
|
||||
grid-template-columns: repeat(7, minmax(100px, 1fr));
|
||||
gap: 8px;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.bottom-panel div {
|
||||
border: 1px solid #c7d0d6;
|
||||
padding: 8px;
|
||||
background: #ffffff;
|
||||
display: grid;
|
||||
gap: 6px;
|
||||
min-width: 0;
|
||||
}
|
||||
|
||||
.bottom-panel span {
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
@media (max-width: 840px) {
|
||||
.workbench {
|
||||
grid-template-columns: 1fr;
|
||||
grid-template-rows: auto;
|
||||
}
|
||||
|
||||
.command-bar,
|
||||
.editor {
|
||||
grid-column: auto;
|
||||
}
|
||||
|
||||
.bottom-panel {
|
||||
grid-template-columns: 1fr 1fr;
|
||||
}
|
||||
|
||||
.viewport {
|
||||
min-height: 300px;
|
||||
}
|
||||
}
|
||||
84
kdl-wasm/web/app/virtual-controller.html
Normal file
84
kdl-wasm/web/app/virtual-controller.html
Normal file
@@ -0,0 +1,84 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>ABB120 Virtual Controller</title>
|
||||
<link rel="stylesheet" href="./virtual-controller.css">
|
||||
</head>
|
||||
<body>
|
||||
<main class="workbench" data-robot="abb_irb120_3_58">
|
||||
<header class="command-bar">
|
||||
<strong>ABB120 Station</strong>
|
||||
<button type="button" data-command="load">Load</button>
|
||||
<button type="button" data-command="run">Run</button>
|
||||
<button type="button" data-command="pause">Pause</button>
|
||||
<button type="button" data-command="step">Step</button>
|
||||
<button type="button" data-command="stop">Stop</button>
|
||||
<button type="button" data-command="reset">Reset</button>
|
||||
<button type="button" data-command="export">Export</button>
|
||||
</header>
|
||||
<aside class="object-tree">
|
||||
<h2>Station</h2>
|
||||
<ul>
|
||||
<li>Robot: abb_irb120_3_58</li>
|
||||
<li>Tool: tool0</li>
|
||||
<li>Frame: world</li>
|
||||
<li>Path: pick_place</li>
|
||||
<li>Operation: pick_op</li>
|
||||
<li>Program: main</li>
|
||||
<li>Reports</li>
|
||||
</ul>
|
||||
</aside>
|
||||
<section class="viewport" aria-label="station viewport">
|
||||
<div class="robot-arm">
|
||||
<span class="joint j1"></span>
|
||||
<span class="link l1"></span>
|
||||
<span class="joint j2"></span>
|
||||
<span class="link l2"></span>
|
||||
<span class="joint j3"></span>
|
||||
<span class="link l3"></span>
|
||||
<span class="joint j4"></span>
|
||||
</div>
|
||||
<svg viewBox="0 0 480 240" role="img" aria-label="path trace">
|
||||
<path d="M70 170 C150 80 260 80 410 150" fill="none" stroke="#1f7a8c" stroke-width="4"/>
|
||||
<circle cx="70" cy="170" r="6"/>
|
||||
<circle cx="250" cy="90" r="6"/>
|
||||
<circle cx="410" cy="150" r="6"/>
|
||||
</svg>
|
||||
</section>
|
||||
<aside class="pendant">
|
||||
<h2>Controller</h2>
|
||||
<dl>
|
||||
<dt>State</dt><dd data-field="state">ready</dd>
|
||||
<dt>Mode</dt><dd data-field="mode">auto</dd>
|
||||
<dt>Motors</dt><dd data-field="motors">on</dd>
|
||||
<dt>PC</dt><dd data-field="pc">main:0</dd>
|
||||
<dt>Joints</dt><dd data-field="joints">[0, 0, 0, 0, 0, 0]</dd>
|
||||
<dt>TCP</dt><dd data-field="tcp">[0, 0, 580, 0, 0, 0]</dd>
|
||||
</dl>
|
||||
</aside>
|
||||
<section class="editor">
|
||||
<nav>
|
||||
<button type="button" data-tab="grl" aria-pressed="true">GRL</button>
|
||||
<button type="button" data-tab="abb">ABB</button>
|
||||
<button type="button" data-tab="fanuc">FANUC</button>
|
||||
<button type="button" data-tab="kuka">KUKA</button>
|
||||
</nav>
|
||||
<pre data-field="editor">proc main()
|
||||
run_path pick_place
|
||||
end</pre>
|
||||
</section>
|
||||
<section class="bottom-panel">
|
||||
<div><strong>Log</strong><span data-field="log">ready</span></div>
|
||||
<div><strong>Diagnostics</strong><span data-field="diagnostics">ready</span></div>
|
||||
<div><strong>IO</strong><span data-field="io">do[1]=false, di[1]=false</span><button type="button" data-io="di1">DI1</button></div>
|
||||
<div><strong>Wait</strong><span data-field="wait">none</span></div>
|
||||
<div><strong>Motion Queue</strong><span data-field="queue">empty</span></div>
|
||||
<div><strong>Trace</strong><span data-field="trace">0 events</span></div>
|
||||
<div><strong>Reports</strong><span data-field="reports">A120-REPORT</span></div>
|
||||
</section>
|
||||
</main>
|
||||
<script src="./virtual-controller.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
140
kdl-wasm/web/app/virtual-controller.js
Normal file
140
kdl-wasm/web/app/virtual-controller.js
Normal file
@@ -0,0 +1,140 @@
|
||||
(function () {
|
||||
const root = document.querySelector(".workbench");
|
||||
const fields = new Map(
|
||||
Array.from(document.querySelectorAll("[data-field]")).map((item) => [item.dataset.field, item])
|
||||
);
|
||||
const tabButtons = Array.from(document.querySelectorAll("[data-tab]"));
|
||||
|
||||
const sources = {
|
||||
grl: "proc main()\n run_path pick_place\nend",
|
||||
abb: "MODULE A120Smoke\n PROC main()\n MoveJ home,v100,fine,tool0;\n ENDPROC\nENDMODULE",
|
||||
fanuc: "/PROG A120SMOKE\n/MN\n 1:J P[1] 40% FINE ;\n/END",
|
||||
kuka: "DEF A120SMOKE()\n PTP HOME\nEND"
|
||||
};
|
||||
|
||||
const state = {
|
||||
command: "ready",
|
||||
mode: "auto",
|
||||
motors: "on",
|
||||
pc: 0,
|
||||
joints: [0, 0, 0, 0, 0, 0],
|
||||
tcp: [0, 0, 580, 0, 0, 0],
|
||||
di1: false,
|
||||
do1: false,
|
||||
wait: "none",
|
||||
queue: [],
|
||||
trace: ["ready"],
|
||||
diagnostics: ["ready"],
|
||||
report: "A120-REPORT",
|
||||
tab: "grl"
|
||||
};
|
||||
|
||||
function formatArray(values) {
|
||||
return `[${values.map((value) => Number(value).toFixed(value % 1 === 0 ? 0 : 2)).join(", ")}]`;
|
||||
}
|
||||
|
||||
function setField(name, value) {
|
||||
const item = fields.get(name);
|
||||
if (item) item.textContent = value;
|
||||
}
|
||||
|
||||
function render() {
|
||||
root?.setAttribute("data-last-command", state.command);
|
||||
root?.setAttribute("data-running", String(state.command === "running"));
|
||||
setField("state", state.command);
|
||||
setField("mode", state.mode);
|
||||
setField("motors", state.motors);
|
||||
setField("pc", `main:${state.pc}`);
|
||||
setField("joints", formatArray(state.joints));
|
||||
setField("tcp", formatArray(state.tcp));
|
||||
setField("log", state.trace.at(-1) ?? "ready");
|
||||
setField("diagnostics", state.diagnostics.at(-1) ?? "ready");
|
||||
setField("io", `do[1]=${state.do1}, di[1]=${state.di1}`);
|
||||
setField("wait", state.wait);
|
||||
setField("queue", state.queue.length ? state.queue.join(" -> ") : "empty");
|
||||
setField("trace", `${state.trace.length} events`);
|
||||
setField("reports", state.report);
|
||||
setField("editor", sources[state.tab]);
|
||||
|
||||
for (const button of tabButtons) {
|
||||
button.setAttribute("aria-pressed", String(button.dataset.tab === state.tab));
|
||||
}
|
||||
}
|
||||
|
||||
function pushTrace(message) {
|
||||
state.trace.push(message);
|
||||
state.diagnostics.push(message);
|
||||
}
|
||||
|
||||
function advanceProgram(stepSize) {
|
||||
state.pc = Math.min(state.pc + stepSize, 4);
|
||||
state.joints = state.joints.map((value, index) => Number((value + (index + 1) * 1.5 * stepSize).toFixed(2)));
|
||||
state.tcp = [
|
||||
Number((state.tcp[0] + 12 * stepSize).toFixed(2)),
|
||||
Number((state.tcp[1] + 4 * stepSize).toFixed(2)),
|
||||
state.tcp[2],
|
||||
state.tcp[3],
|
||||
state.tcp[4],
|
||||
state.tcp[5]
|
||||
];
|
||||
}
|
||||
|
||||
function handleCommand(command) {
|
||||
state.command = command === "run" ? "running" : command;
|
||||
if (command === "load") {
|
||||
state.pc = 0;
|
||||
state.queue = ["movej home", "run_path pick_place"];
|
||||
state.wait = "none";
|
||||
pushTrace("program loaded");
|
||||
} else if (command === "run") {
|
||||
advanceProgram(2);
|
||||
state.do1 = true;
|
||||
state.wait = state.di1 ? "satisfied" : "waiting di[1]";
|
||||
state.queue = state.di1 ? ["movel place"] : ["wait di[1]", "movel place"];
|
||||
pushTrace(state.di1 ? "running path" : "waiting for di[1]");
|
||||
} else if (command === "pause") {
|
||||
pushTrace("program paused");
|
||||
} else if (command === "step") {
|
||||
state.command = "paused";
|
||||
advanceProgram(1);
|
||||
pushTrace(`stepped to main:${state.pc}`);
|
||||
} else if (command === "stop") {
|
||||
state.queue = [];
|
||||
pushTrace("program stopped");
|
||||
} else if (command === "reset") {
|
||||
state.command = "ready";
|
||||
state.pc = 0;
|
||||
state.joints = [0, 0, 0, 0, 0, 0];
|
||||
state.tcp = [0, 0, 580, 0, 0, 0];
|
||||
state.do1 = false;
|
||||
state.wait = "none";
|
||||
state.queue = [];
|
||||
pushTrace("controller reset");
|
||||
} else if (command === "export") {
|
||||
state.report = "A120-REPORT exported";
|
||||
pushTrace("delivery package exported");
|
||||
}
|
||||
render();
|
||||
}
|
||||
|
||||
for (const button of document.querySelectorAll("[data-command]")) {
|
||||
button.addEventListener("click", () => handleCommand(button.dataset.command ?? "ready"));
|
||||
}
|
||||
|
||||
for (const button of tabButtons) {
|
||||
button.addEventListener("click", () => {
|
||||
state.tab = button.dataset.tab ?? "grl";
|
||||
pushTrace(`${state.tab.toUpperCase()} source selected`);
|
||||
render();
|
||||
});
|
||||
}
|
||||
|
||||
document.querySelector("[data-io='di1']")?.addEventListener("click", () => {
|
||||
state.di1 = !state.di1;
|
||||
state.wait = state.di1 ? "satisfied" : "waiting di[1]";
|
||||
pushTrace(`di[1]=${state.di1}`);
|
||||
render();
|
||||
});
|
||||
|
||||
render();
|
||||
})();
|
||||
349
kdl-wasm/web/scripts/verify-virtual-controller.mjs
Normal file
349
kdl-wasm/web/scripts/verify-virtual-controller.mjs
Normal file
@@ -0,0 +1,349 @@
|
||||
import { existsSync, mkdtempSync, mkdirSync, rmSync, writeFileSync } from "node:fs";
|
||||
import { tmpdir } from "node:os";
|
||||
import { basename, join, resolve } from "node:path";
|
||||
import { fileURLToPath, pathToFileURL } from "node:url";
|
||||
import { spawn } from "node:child_process";
|
||||
|
||||
const root = resolve(fileURLToPath(new URL("../../..", import.meta.url)));
|
||||
const appHtml = resolve(root, "kdl-wasm/web/app/virtual-controller.html");
|
||||
const outputDir = resolve(root, "kdl-wasm/web/test-results/virtual-controller");
|
||||
const chromeBin = process.env.CHROME_BIN ?? defaultChromeBin();
|
||||
|
||||
mkdirSync(outputDir, { recursive: true });
|
||||
|
||||
const desktop = await verifyViewport({
|
||||
name: "desktop",
|
||||
width: 1440,
|
||||
height: 960,
|
||||
deviceScaleFactor: 1,
|
||||
mobile: false,
|
||||
screenshotPath: join(outputDir, "virtual-controller-desktop.png")
|
||||
});
|
||||
const mobile = await verifyViewport({
|
||||
name: "mobile",
|
||||
width: 390,
|
||||
height: 844,
|
||||
deviceScaleFactor: 2,
|
||||
mobile: true,
|
||||
screenshotPath: join(outputDir, "virtual-controller-mobile.png")
|
||||
});
|
||||
|
||||
const evidence = {
|
||||
app: appHtml,
|
||||
chrome: chromeBin,
|
||||
desktop,
|
||||
mobile
|
||||
};
|
||||
writeFileSync(join(outputDir, "evidence.json"), `${JSON.stringify(evidence, null, 2)}\n`);
|
||||
console.log(JSON.stringify(evidence, null, 2));
|
||||
|
||||
async function verifyViewport(viewport) {
|
||||
const browser = await launchChrome(viewport);
|
||||
try {
|
||||
const client = await connect(browser.websocketUrl);
|
||||
await client.send("Page.enable");
|
||||
await client.send("Runtime.enable");
|
||||
await client.send("Emulation.setDeviceMetricsOverride", {
|
||||
width: viewport.width,
|
||||
height: viewport.height,
|
||||
deviceScaleFactor: viewport.deviceScaleFactor,
|
||||
mobile: viewport.mobile
|
||||
});
|
||||
await client.send("Page.navigate", { url: pathToFileURL(appHtml).href });
|
||||
await waitForLoad(client);
|
||||
|
||||
const initial = await evaluate(client, pageProbeSource());
|
||||
assertProbe(initial, viewport.name, "initial");
|
||||
|
||||
await evaluate(client, clickCommandSource("load"));
|
||||
await evaluate(client, clickCommandSource("run"));
|
||||
await evaluate(client, clickIoSource());
|
||||
const afterRun = await evaluate(client, pageProbeSource());
|
||||
assertProbe(afterRun, viewport.name, "after run");
|
||||
if (afterRun.state !== "running") {
|
||||
throw new Error(`${viewport.name}: expected state running, got ${afterRun.state}`);
|
||||
}
|
||||
if (!afterRun.running) {
|
||||
throw new Error(`${viewport.name}: data-running was not set after Run`);
|
||||
}
|
||||
if (!afterRun.wait.includes("satisfied")) {
|
||||
throw new Error(`${viewport.name}: expected DI wait to be satisfied, got ${afterRun.wait}`);
|
||||
}
|
||||
|
||||
const screenshot = await client.send("Page.captureScreenshot", {
|
||||
format: "png",
|
||||
captureBeyondViewport: false
|
||||
});
|
||||
writeFileSync(viewport.screenshotPath, Buffer.from(screenshot.data, "base64"));
|
||||
const pngBytes = Buffer.byteLength(screenshot.data, "base64");
|
||||
if (pngBytes < 10_000) {
|
||||
throw new Error(`${viewport.name}: screenshot is unexpectedly small (${pngBytes} bytes)`);
|
||||
}
|
||||
|
||||
await client.close();
|
||||
return {
|
||||
viewport: {
|
||||
width: viewport.width,
|
||||
height: viewport.height,
|
||||
deviceScaleFactor: viewport.deviceScaleFactor,
|
||||
mobile: viewport.mobile
|
||||
},
|
||||
screenshot: viewport.screenshotPath,
|
||||
screenshotBytes: pngBytes,
|
||||
state: afterRun.state,
|
||||
wait: afterRun.wait,
|
||||
trace: afterRun.trace,
|
||||
visiblePanels: afterRun.visiblePanels
|
||||
};
|
||||
} finally {
|
||||
browser.close();
|
||||
}
|
||||
}
|
||||
|
||||
function launchChrome(viewport) {
|
||||
const userDataDir = mkdtempSync(join(tmpdir(), `kdl-vc-${viewport.name}-`));
|
||||
const child = spawn(chromeBin, [
|
||||
"--headless=new",
|
||||
"--disable-gpu",
|
||||
"--no-sandbox",
|
||||
"--disable-dev-shm-usage",
|
||||
"--remote-debugging-port=0",
|
||||
`--user-data-dir=${userDataDir}`,
|
||||
"about:blank"
|
||||
], { stdio: ["ignore", "pipe", "pipe"] });
|
||||
|
||||
let log = "";
|
||||
let settled = false;
|
||||
|
||||
const cleanup = () => {
|
||||
child.kill("SIGTERM");
|
||||
setTimeout(() => {
|
||||
rmSync(userDataDir, {
|
||||
recursive: true,
|
||||
force: true,
|
||||
maxRetries: 5,
|
||||
retryDelay: 100
|
||||
});
|
||||
}, 250).unref();
|
||||
};
|
||||
|
||||
return new Promise((resolveLaunch, reject) => {
|
||||
const timeout = setTimeout(() => {
|
||||
if (!settled) {
|
||||
settled = true;
|
||||
cleanup();
|
||||
reject(new Error(`Chrome did not expose DevTools endpoint. ${log}`));
|
||||
}
|
||||
}, 15_000);
|
||||
|
||||
const onData = (chunk) => {
|
||||
log += chunk.toString();
|
||||
const match = log.match(/DevTools listening on (ws:\/\/[^\s]+)/);
|
||||
if (!match || settled) return;
|
||||
settled = true;
|
||||
clearTimeout(timeout);
|
||||
findPageWebsocket(match[1]).then((pageWebsocketUrl) => {
|
||||
resolveLaunch({
|
||||
websocketUrl: pageWebsocketUrl,
|
||||
close: cleanup
|
||||
});
|
||||
}, (error) => {
|
||||
cleanup();
|
||||
reject(error);
|
||||
});
|
||||
};
|
||||
|
||||
child.stdout.on("data", onData);
|
||||
child.stderr.on("data", onData);
|
||||
child.on("error", (error) => {
|
||||
if (!settled) {
|
||||
settled = true;
|
||||
clearTimeout(timeout);
|
||||
cleanup();
|
||||
reject(error);
|
||||
}
|
||||
});
|
||||
child.on("exit", (code) => {
|
||||
if (!settled) {
|
||||
settled = true;
|
||||
clearTimeout(timeout);
|
||||
cleanup();
|
||||
reject(new Error(`${basename(chromeBin)} exited early with ${code}. ${log}`));
|
||||
}
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
async function findPageWebsocket(browserWebsocketUrl) {
|
||||
const endpoint = new URL(browserWebsocketUrl);
|
||||
const baseUrl = `http://${endpoint.host}`;
|
||||
const started = Date.now();
|
||||
let lastError;
|
||||
|
||||
while (Date.now() - started < 5000) {
|
||||
try {
|
||||
const response = await fetch(`${baseUrl}/json/list`);
|
||||
const targets = await response.json();
|
||||
const page = targets.find((target) => target.type === "page" && target.webSocketDebuggerUrl);
|
||||
if (page?.webSocketDebuggerUrl) {
|
||||
return page.webSocketDebuggerUrl;
|
||||
}
|
||||
} catch (error) {
|
||||
lastError = error;
|
||||
}
|
||||
await new Promise((resolveRetry) => setTimeout(resolveRetry, 100));
|
||||
}
|
||||
|
||||
throw new Error(`Could not find Chrome page target. ${lastError instanceof Error ? lastError.message : ""}`);
|
||||
}
|
||||
|
||||
function connect(websocketUrl) {
|
||||
const socket = new WebSocket(websocketUrl);
|
||||
let nextId = 1;
|
||||
const pending = new Map();
|
||||
|
||||
socket.addEventListener("message", (event) => {
|
||||
const message = JSON.parse(event.data.toString());
|
||||
if (!message.id) return;
|
||||
const request = pending.get(message.id);
|
||||
if (!request) return;
|
||||
pending.delete(message.id);
|
||||
if (message.error) {
|
||||
request.reject(new Error(`${message.error.code}: ${message.error.message}`));
|
||||
} else {
|
||||
request.resolve(message.result ?? {});
|
||||
}
|
||||
});
|
||||
|
||||
return new Promise((resolveClient, reject) => {
|
||||
socket.addEventListener("open", () => {
|
||||
resolveClient({
|
||||
send(method, params = {}) {
|
||||
const id = nextId++;
|
||||
socket.send(JSON.stringify({ id, method, params }));
|
||||
return new Promise((resolveSend, rejectSend) => {
|
||||
pending.set(id, { resolve: resolveSend, reject: rejectSend });
|
||||
});
|
||||
},
|
||||
close() {
|
||||
socket.close();
|
||||
}
|
||||
});
|
||||
}, { once: true });
|
||||
socket.addEventListener("error", reject, { once: true });
|
||||
});
|
||||
}
|
||||
|
||||
function waitForLoad(client) {
|
||||
return new Promise((resolveLoad, reject) => {
|
||||
const started = Date.now();
|
||||
const poll = async () => {
|
||||
try {
|
||||
const ready = await evaluate(client, "() => document.readyState");
|
||||
if (ready === "complete") {
|
||||
resolveLoad();
|
||||
return;
|
||||
}
|
||||
if (Date.now() - started > 10_000) {
|
||||
reject(new Error("Timed out waiting for document.readyState complete"));
|
||||
return;
|
||||
}
|
||||
setTimeout(poll, 100);
|
||||
} catch (error) {
|
||||
reject(error);
|
||||
}
|
||||
};
|
||||
poll();
|
||||
});
|
||||
}
|
||||
|
||||
async function evaluate(client, expression) {
|
||||
const result = await client.send("Runtime.evaluate", {
|
||||
expression: `(${expression})()`,
|
||||
awaitPromise: true,
|
||||
returnByValue: true
|
||||
});
|
||||
if (result.exceptionDetails) {
|
||||
throw new Error(result.exceptionDetails.text ?? "Runtime.evaluate failed");
|
||||
}
|
||||
return result.result.value;
|
||||
}
|
||||
|
||||
function pageProbeSource() {
|
||||
return `() => {
|
||||
const field = (name) => document.querySelector("[data-field='" + name + "']")?.textContent?.trim() ?? "";
|
||||
const box = (selector) => {
|
||||
const item = document.querySelector(selector);
|
||||
if (!item) return null;
|
||||
const rect = item.getBoundingClientRect();
|
||||
return { width: rect.width, height: rect.height, top: rect.top, left: rect.left };
|
||||
};
|
||||
const visiblePanels = [".command-bar", ".object-tree", ".viewport", ".pendant", ".editor", ".bottom-panel"]
|
||||
.map((selector) => ({ selector, box: box(selector) }))
|
||||
.filter((item) => item.box && item.box.width > 0 && item.box.height > 0)
|
||||
.map((item) => item.selector);
|
||||
return {
|
||||
title: document.title,
|
||||
state: field("state"),
|
||||
wait: field("wait"),
|
||||
queue: field("queue"),
|
||||
trace: field("trace"),
|
||||
editor: field("editor"),
|
||||
running: document.querySelector(".workbench")?.dataset.running === "true",
|
||||
visiblePanels,
|
||||
viewport: box(".viewport"),
|
||||
bodyScrollWidth: document.documentElement.scrollWidth,
|
||||
clientWidth: document.documentElement.clientWidth
|
||||
};
|
||||
}`;
|
||||
}
|
||||
|
||||
function clickCommandSource(command) {
|
||||
return `() => {
|
||||
document.querySelector("[data-command='${command}']").click();
|
||||
return document.querySelector(".workbench")?.dataset.lastCommand;
|
||||
}`;
|
||||
}
|
||||
|
||||
function clickIoSource() {
|
||||
return `() => {
|
||||
document.querySelector("[data-io='di1']").click();
|
||||
return document.querySelector("[data-field='wait']")?.textContent;
|
||||
}`;
|
||||
}
|
||||
|
||||
function assertProbe(probe, viewportName, phase) {
|
||||
if (probe.title !== "ABB120 Virtual Controller") {
|
||||
throw new Error(`${viewportName}/${phase}: unexpected title ${probe.title}`);
|
||||
}
|
||||
const required = [".command-bar", ".object-tree", ".viewport", ".pendant", ".editor", ".bottom-panel"];
|
||||
for (const selector of required) {
|
||||
if (!probe.visiblePanels.includes(selector)) {
|
||||
throw new Error(`${viewportName}/${phase}: ${selector} is not visible`);
|
||||
}
|
||||
}
|
||||
if (!probe.editor.includes("proc main") && !probe.editor.includes("MoveJ") && !probe.editor.includes("PTP")) {
|
||||
throw new Error(`${viewportName}/${phase}: editor source did not render`);
|
||||
}
|
||||
if (!probe.viewport || probe.viewport.width < 100 || probe.viewport.height < 100) {
|
||||
throw new Error(`${viewportName}/${phase}: viewport is too small`);
|
||||
}
|
||||
if (probe.bodyScrollWidth > probe.clientWidth + 2) {
|
||||
throw new Error(`${viewportName}/${phase}: horizontal overflow detected`);
|
||||
}
|
||||
}
|
||||
|
||||
function defaultChromeBin() {
|
||||
if (process.platform !== "win32") {
|
||||
return "/usr/bin/google-chrome";
|
||||
}
|
||||
const candidates = [
|
||||
"C:\\Program Files\\Google\\Chrome\\Application\\chrome.exe",
|
||||
"C:\\Program Files (x86)\\Google\\Chrome\\Application\\chrome.exe",
|
||||
process.env.LOCALAPPDATA
|
||||
? join(process.env.LOCALAPPDATA, "Google", "Chrome", "Application", "chrome.exe")
|
||||
: "",
|
||||
"C:\\Program Files\\Microsoft\\Edge\\Application\\msedge.exe"
|
||||
];
|
||||
return candidates.find((candidate) => candidate && existsSync(candidate)) ?? "chrome.exe";
|
||||
}
|
||||
84
kdl-wasm/web/src/controller/expression.ts
Normal file
84
kdl-wasm/web/src/controller/expression.ts
Normal file
@@ -0,0 +1,84 @@
|
||||
export type RuntimeVariables = Record<string, unknown>;
|
||||
|
||||
export function evaluateRuntimeExpression(text: string, variables: RuntimeVariables = {}): unknown {
|
||||
const normalized = normalizeExpression(text);
|
||||
if (normalized === "") {
|
||||
return undefined;
|
||||
}
|
||||
if (normalized === "true") {
|
||||
return true;
|
||||
}
|
||||
if (normalized === "false") {
|
||||
return false;
|
||||
}
|
||||
if (/^-?\d+(?:\.\d+)?$/.test(normalized)) {
|
||||
return Number(normalized);
|
||||
}
|
||||
if (Object.prototype.hasOwnProperty.call(variables, normalized)) {
|
||||
return variables[normalized];
|
||||
}
|
||||
|
||||
const comparison = normalized.match(/^(.+?)\s*(==|!=|>=|<=|>|<)\s*(.+)$/);
|
||||
if (comparison) {
|
||||
const left = valueOf(comparison[1]!, variables);
|
||||
const right = valueOf(comparison[3]!, variables);
|
||||
switch (comparison[2]) {
|
||||
case "==":
|
||||
return left === right;
|
||||
case "!=":
|
||||
return left !== right;
|
||||
case ">=":
|
||||
return Number(left) >= Number(right);
|
||||
case "<=":
|
||||
return Number(left) <= Number(right);
|
||||
case ">":
|
||||
return Number(left) > Number(right);
|
||||
case "<":
|
||||
return Number(left) < Number(right);
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return Boolean(variables[normalized]);
|
||||
}
|
||||
|
||||
export function evaluateRuntimeBoolean(text: string, variables: RuntimeVariables = {}): boolean {
|
||||
return Boolean(evaluateRuntimeExpression(text, variables));
|
||||
}
|
||||
|
||||
export function assignRuntimeExpression(text: string, variables: RuntimeVariables, value?: unknown): boolean {
|
||||
const normalized = normalizeExpression(text);
|
||||
const assignment = normalized.match(/^([A-Za-z_][A-Za-z0-9_]*)\s*(?::=|=)\s*(.+)$/);
|
||||
if (!assignment) {
|
||||
return false;
|
||||
}
|
||||
variables[assignment[1]!] = value !== undefined ? value : valueOf(assignment[2]!, variables);
|
||||
return true;
|
||||
}
|
||||
|
||||
function valueOf(raw: string, variables: RuntimeVariables): unknown {
|
||||
const text = normalizeExpression(raw);
|
||||
if (text === "true") {
|
||||
return true;
|
||||
}
|
||||
if (text === "false") {
|
||||
return false;
|
||||
}
|
||||
if ((text.startsWith("\"") && text.endsWith("\"")) || (text.startsWith("'") && text.endsWith("'"))) {
|
||||
return text.slice(1, -1);
|
||||
}
|
||||
if (/^-?\d+(?:\.\d+)?$/.test(text)) {
|
||||
return Number(text);
|
||||
}
|
||||
return variables[text];
|
||||
}
|
||||
|
||||
function normalizeExpression(text: string): string {
|
||||
return text
|
||||
.replace(/\s*\.\s*/g, ".")
|
||||
.replace(/\s*\[\s*/g, "[")
|
||||
.replace(/\s*\]\s*/g, "]")
|
||||
.replace(/\s+/g, " ")
|
||||
.trim();
|
||||
}
|
||||
40
kdl-wasm/web/src/controller/index.ts
Normal file
40
kdl-wasm/web/src/controller/index.ts
Normal file
@@ -0,0 +1,40 @@
|
||||
export {
|
||||
VirtualControllerStateMachine,
|
||||
type StateTransitionResult,
|
||||
type VirtualControllerCommand,
|
||||
type VirtualControllerState,
|
||||
type VirtualControllerStateMachineSnapshot
|
||||
} from "./stateMachine.js";
|
||||
export {
|
||||
VirtualController,
|
||||
isMotionInstruction,
|
||||
type VirtualControllerLoadOptions,
|
||||
type VirtualControllerSnapshot
|
||||
} from "./virtualController.js";
|
||||
export {
|
||||
IrExecutionRuntime,
|
||||
type IrExecutionRuntimeOptions,
|
||||
type IrExecutionRuntimeSnapshot,
|
||||
type RuntimeAlarm,
|
||||
type RuntimeBreakpoint,
|
||||
type RuntimeExecutionFrame,
|
||||
type RuntimeInstructionHooks,
|
||||
type RuntimeScopeFrame,
|
||||
type RuntimeStepResult,
|
||||
type RuntimeStepStatus
|
||||
} from "./runtime.js";
|
||||
export {
|
||||
RuntimeSourceMapIndex,
|
||||
type RuntimeSourceLocation
|
||||
} from "./sourceMap.js";
|
||||
export {
|
||||
TraceBuffer,
|
||||
type RuntimeTraceEvent,
|
||||
type RuntimeTraceEventKind
|
||||
} from "./trace.js";
|
||||
export {
|
||||
evaluateRuntimeBoolean,
|
||||
evaluateRuntimeExpression,
|
||||
assignRuntimeExpression,
|
||||
type RuntimeVariables
|
||||
} from "./expression.js";
|
||||
520
kdl-wasm/web/src/controller/runtime.ts
Normal file
520
kdl-wasm/web/src/controller/runtime.ts
Normal file
@@ -0,0 +1,520 @@
|
||||
import type {
|
||||
AlarmInstruction,
|
||||
CallInstruction,
|
||||
ExecutableBranch,
|
||||
ExecutableInstruction,
|
||||
ExecutableProcedure,
|
||||
ReturnInstruction,
|
||||
SemanticProgramIr
|
||||
} from "../grl/ir/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import { evaluateRuntimeBoolean, evaluateRuntimeExpression, assignRuntimeExpression } from "./expression.js";
|
||||
import { RuntimeSourceMapIndex, type RuntimeSourceLocation } from "./sourceMap.js";
|
||||
import { TraceBuffer, type RuntimeTraceEvent } from "./trace.js";
|
||||
|
||||
export interface RuntimeBreakpoint {
|
||||
id: string;
|
||||
procedure?: string;
|
||||
pc?: number;
|
||||
source?: {
|
||||
file?: string;
|
||||
line?: number;
|
||||
column?: number;
|
||||
};
|
||||
enabled: boolean;
|
||||
}
|
||||
|
||||
export interface RuntimeExecutionFrame {
|
||||
procedure: string;
|
||||
pc: number;
|
||||
returnTo?: {
|
||||
procedure: string;
|
||||
pc: number;
|
||||
};
|
||||
}
|
||||
|
||||
export interface RuntimeScopeFrame {
|
||||
id: string;
|
||||
variables: Record<string, unknown>;
|
||||
}
|
||||
|
||||
export interface RuntimeAlarm {
|
||||
id: string;
|
||||
message: string;
|
||||
severity: "info" | "warning" | "error";
|
||||
source?: RuntimeSourceLocation;
|
||||
time: number;
|
||||
}
|
||||
|
||||
export type RuntimeStepStatus = "executed" | "blocked" | "completed" | "breakpoint";
|
||||
|
||||
export interface RuntimeStepResult {
|
||||
status: RuntimeStepStatus;
|
||||
instruction?: ExecutableInstruction;
|
||||
trace?: RuntimeTraceEvent;
|
||||
diagnostic?: MotionDiagnostic;
|
||||
}
|
||||
|
||||
export interface IrExecutionRuntimeSnapshot {
|
||||
loaded: boolean;
|
||||
procedure?: string;
|
||||
pc: number;
|
||||
callStack: RuntimeExecutionFrame[];
|
||||
scopeStack: RuntimeScopeFrame[];
|
||||
alarmQueue: RuntimeAlarm[];
|
||||
trace: RuntimeTraceEvent[];
|
||||
currentSource?: RuntimeSourceLocation;
|
||||
completed: boolean;
|
||||
}
|
||||
|
||||
export interface RuntimeInstructionHooks {
|
||||
onInstruction?: (instruction: ExecutableInstruction, runtime: IrExecutionRuntime) => RuntimeStepResult | void;
|
||||
onWait?: (instruction: Extract<ExecutableInstruction, { kind: "WAIT" }>, runtime: IrExecutionRuntime) => RuntimeStepResult | void;
|
||||
onMotion?: (instruction: Extract<ExecutableInstruction, { kind: "MOVEJ" | "MOVEL" | "MOVEC" | "RUN_PATH" | "RUN_OPERATION" }>, runtime: IrExecutionRuntime) => RuntimeStepResult | void;
|
||||
onIo?: (instruction: Extract<ExecutableInstruction, { kind: "IO_WRITE" | "PULSE" }>, runtime: IrExecutionRuntime) => RuntimeStepResult | void;
|
||||
}
|
||||
|
||||
export interface IrExecutionRuntimeOptions {
|
||||
entryProcedure?: string;
|
||||
traceCapacity?: number;
|
||||
hooks?: RuntimeInstructionHooks;
|
||||
}
|
||||
|
||||
export class IrExecutionRuntime {
|
||||
private programValue: SemanticProgramIr | undefined;
|
||||
private readonly procedures = new Map<string, ExecutableProcedure>();
|
||||
private readonly traceBuffer: TraceBuffer;
|
||||
private readonly scopes: RuntimeScopeFrame[] = [];
|
||||
private readonly alarms: RuntimeAlarm[] = [];
|
||||
private readonly breakpoints = new Map<string, RuntimeBreakpoint>();
|
||||
private frame: RuntimeExecutionFrame | undefined;
|
||||
private completedValue = false;
|
||||
private sourceIndex = new RuntimeSourceMapIndex();
|
||||
private virtualTimeValue = 0;
|
||||
|
||||
constructor(private readonly options: IrExecutionRuntimeOptions = {}) {
|
||||
this.traceBuffer = new TraceBuffer(options.traceCapacity ?? 1000);
|
||||
}
|
||||
|
||||
get program(): SemanticProgramIr | undefined {
|
||||
return this.programValue;
|
||||
}
|
||||
|
||||
get virtualTime(): number {
|
||||
return this.virtualTimeValue;
|
||||
}
|
||||
|
||||
set virtualTime(value: number) {
|
||||
this.virtualTimeValue = value;
|
||||
}
|
||||
|
||||
get loaded(): boolean {
|
||||
return this.programValue !== undefined;
|
||||
}
|
||||
|
||||
get completed(): boolean {
|
||||
return this.completedValue;
|
||||
}
|
||||
|
||||
get currentFrame(): RuntimeExecutionFrame | undefined {
|
||||
return this.frame ? { ...this.frame, ...(this.frame.returnTo ? { returnTo: { ...this.frame.returnTo } } : {}) } : undefined;
|
||||
}
|
||||
|
||||
get currentProcedure(): ExecutableProcedure | undefined {
|
||||
return this.frame ? this.procedures.get(this.frame.procedure) : undefined;
|
||||
}
|
||||
|
||||
get currentInstruction(): ExecutableInstruction | undefined {
|
||||
const procedure = this.currentProcedure;
|
||||
if (!procedure || !this.frame) {
|
||||
return undefined;
|
||||
}
|
||||
return procedure.instructions[this.frame.pc];
|
||||
}
|
||||
|
||||
get sourceMapIndex(): RuntimeSourceMapIndex {
|
||||
return this.sourceIndex;
|
||||
}
|
||||
|
||||
load(program: SemanticProgramIr, entryProcedure = this.options.entryProcedure ?? "main"): void {
|
||||
this.programValue = program;
|
||||
this.procedures.clear();
|
||||
for (const procedure of program.procedures) {
|
||||
this.procedures.set(procedure.name, procedure);
|
||||
}
|
||||
if (!this.procedures.has(entryProcedure)) {
|
||||
throw new Error(`Entry procedure ${entryProcedure} was not found`);
|
||||
}
|
||||
this.sourceIndex = new RuntimeSourceMapIndex(program);
|
||||
this.reset(entryProcedure);
|
||||
this.trace("load", { message: `Loaded ${program.moduleName}`, data: { moduleName: program.moduleName } });
|
||||
}
|
||||
|
||||
reset(entryProcedure = this.options.entryProcedure ?? "main"): void {
|
||||
if (!this.procedures.has(entryProcedure)) {
|
||||
throw new Error(`Entry procedure ${entryProcedure} was not found`);
|
||||
}
|
||||
this.frame = { procedure: entryProcedure, pc: 0 };
|
||||
this.scopes.length = 0;
|
||||
this.scopes.push({ id: "global", variables: {} });
|
||||
this.scopes.push({ id: entryProcedure, variables: {} });
|
||||
this.alarms.length = 0;
|
||||
this.completedValue = false;
|
||||
this.virtualTimeValue = 0;
|
||||
this.traceBuffer.clear();
|
||||
}
|
||||
|
||||
setVariable(name: string, value: unknown, scopeId?: string): void {
|
||||
const scope = scopeId ? this.scopes.find((candidate) => candidate.id === scopeId) : this.scopes.at(-1);
|
||||
if (!scope) {
|
||||
throw new Error(`Scope ${scopeId ?? "<current>"} was not found`);
|
||||
}
|
||||
scope.variables[name] = value;
|
||||
}
|
||||
|
||||
getVariable(name: string): unknown {
|
||||
for (let index = this.scopes.length - 1; index >= 0; index -= 1) {
|
||||
const scope = this.scopes[index]!;
|
||||
if (Object.prototype.hasOwnProperty.call(scope.variables, name)) {
|
||||
return scope.variables[name];
|
||||
}
|
||||
}
|
||||
return undefined;
|
||||
}
|
||||
|
||||
variables(): Record<string, unknown> {
|
||||
return Object.assign({}, ...this.scopes.map((scope) => scope.variables));
|
||||
}
|
||||
|
||||
setBreakpoint(breakpoint: RuntimeBreakpoint): void {
|
||||
this.breakpoints.set(breakpoint.id, breakpoint);
|
||||
}
|
||||
|
||||
removeBreakpoint(id: string): boolean {
|
||||
return this.breakpoints.delete(id);
|
||||
}
|
||||
|
||||
listBreakpoints(): RuntimeBreakpoint[] {
|
||||
return [...this.breakpoints.values()].map((breakpoint) => ({ ...breakpoint }));
|
||||
}
|
||||
|
||||
step(): RuntimeStepResult {
|
||||
if (!this.frame || this.completedValue) {
|
||||
this.completedValue = true;
|
||||
return { status: "completed" };
|
||||
}
|
||||
|
||||
const procedure = this.procedures.get(this.frame.procedure);
|
||||
if (!procedure) {
|
||||
return this.fail(`Procedure ${this.frame.procedure} was not found`, "VC_PROCEDURE_NOT_FOUND");
|
||||
}
|
||||
|
||||
if (this.frame.pc >= procedure.instructions.length) {
|
||||
return this.returnFromProcedure();
|
||||
}
|
||||
|
||||
const instruction = procedure.instructions[this.frame.pc]!;
|
||||
const breakpoint = this.matchBreakpoint(instruction);
|
||||
if (breakpoint) {
|
||||
const trace = this.trace("breakpoint", {
|
||||
instruction,
|
||||
message: `Breakpoint ${breakpoint.id} hit`,
|
||||
data: { breakpointId: breakpoint.id }
|
||||
});
|
||||
return { status: "breakpoint", instruction, trace };
|
||||
}
|
||||
|
||||
const hookResult = this.runHook(instruction);
|
||||
if (hookResult) {
|
||||
if (hookResult.status === "blocked" || hookResult.status === "breakpoint") {
|
||||
return hookResult;
|
||||
}
|
||||
if (hookResult.status === "completed") {
|
||||
this.completedValue = true;
|
||||
return hookResult;
|
||||
}
|
||||
}
|
||||
|
||||
const trace = this.execute(instruction);
|
||||
return {
|
||||
status: this.completedValue ? "completed" : "executed",
|
||||
instruction,
|
||||
trace
|
||||
};
|
||||
}
|
||||
|
||||
run(maxSteps = 10_000): RuntimeStepResult {
|
||||
let result: RuntimeStepResult = { status: "completed" };
|
||||
for (let index = 0; index < maxSteps; index += 1) {
|
||||
result = this.step();
|
||||
if (result.status !== "executed") {
|
||||
return result;
|
||||
}
|
||||
}
|
||||
return this.fail(`Execution exceeded ${maxSteps} steps`, "VC_STEP_LIMIT_EXCEEDED");
|
||||
}
|
||||
|
||||
pushAlarm(alarmId: string, message = alarmId, severity: RuntimeAlarm["severity"] = "error", source?: RuntimeSourceLocation): RuntimeAlarm {
|
||||
const alarm: RuntimeAlarm = {
|
||||
id: alarmId,
|
||||
message,
|
||||
severity,
|
||||
...(source ? { source } : {}),
|
||||
time: this.virtualTimeValue
|
||||
};
|
||||
this.alarms.push(alarm);
|
||||
this.trace("alarm", {
|
||||
message,
|
||||
data: { alarmId, severity },
|
||||
...(source ? { source } : {})
|
||||
});
|
||||
return alarm;
|
||||
}
|
||||
|
||||
trace(kind: RuntimeTraceEvent["kind"], input: {
|
||||
instruction?: ExecutableInstruction;
|
||||
message?: string;
|
||||
data?: Record<string, unknown>;
|
||||
source?: RuntimeSourceLocation;
|
||||
diagnostic?: MotionDiagnostic;
|
||||
} = {}): RuntimeTraceEvent {
|
||||
const source = input.source ?? (input.instruction ? this.locateInstruction(input.instruction) : this.currentSource());
|
||||
return this.traceBuffer.push({
|
||||
time: this.virtualTimeValue,
|
||||
kind,
|
||||
...(this.frame ? { procedure: this.frame.procedure, pc: this.frame.pc } : {}),
|
||||
...(input.instruction ? { instructionKind: input.instruction.kind } : {}),
|
||||
...(input.message ? { message: input.message } : {}),
|
||||
...(source ? { source } : {}),
|
||||
...(input.diagnostic ? { diagnostic: input.diagnostic } : {}),
|
||||
...(input.data ? { data: input.data } : {})
|
||||
});
|
||||
}
|
||||
|
||||
currentSource(): RuntimeSourceLocation | undefined {
|
||||
const instruction = this.currentInstruction;
|
||||
return instruction ? this.locateInstruction(instruction) : undefined;
|
||||
}
|
||||
|
||||
locateInstruction(instruction: ExecutableInstruction): RuntimeSourceLocation | undefined {
|
||||
return this.sourceIndex.locateByInstruction({
|
||||
kind: instruction.kind,
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
|
||||
...(this.frame?.procedure ? { procedureId: this.frame.procedure } : {})
|
||||
});
|
||||
}
|
||||
|
||||
snapshot(): IrExecutionRuntimeSnapshot {
|
||||
const currentSource = this.currentSource();
|
||||
return {
|
||||
loaded: this.loaded,
|
||||
...(this.frame ? { procedure: this.frame.procedure } : {}),
|
||||
pc: this.frame?.pc ?? 0,
|
||||
callStack: this.frame ? [this.frame] : [],
|
||||
scopeStack: this.scopes.map((scope) => ({ id: scope.id, variables: { ...scope.variables } })),
|
||||
alarmQueue: this.alarms.map((alarm) => ({ ...alarm })),
|
||||
trace: this.traceBuffer.all(),
|
||||
...(currentSource ? { currentSource } : {}),
|
||||
completed: this.completedValue
|
||||
};
|
||||
}
|
||||
|
||||
private execute(instruction: ExecutableInstruction): RuntimeTraceEvent {
|
||||
switch (instruction.kind) {
|
||||
case "CALL":
|
||||
return this.executeCall(instruction);
|
||||
case "RETURN":
|
||||
return this.executeReturn(instruction);
|
||||
case "ALARM":
|
||||
this.pushAlarm(
|
||||
instruction.alarmId,
|
||||
instruction.message ?? instruction.alarmId,
|
||||
normalizeAlarmSeverity(instruction.severity),
|
||||
this.locateInstruction(instruction)
|
||||
);
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "RAISE":
|
||||
this.pushAlarm(instruction.alarmId, instruction.alarmId, "error", this.locateInstruction(instruction));
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "EXEC_IF":
|
||||
return this.executeIf(instruction.branches, instruction);
|
||||
case "EXEC_WHILE":
|
||||
if (evaluateRuntimeBoolean(instruction.condition.text, this.variables())) {
|
||||
this.insertInlineBlock(instruction.body, "while");
|
||||
}
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "EXEC_FOR":
|
||||
this.setVariable(instruction.iterator, evaluateRuntimeExpression(instruction.from.text, this.variables()));
|
||||
this.insertInlineBlock(instruction.body, "for");
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "EXEC_SWITCH":
|
||||
this.insertInlineBlock(instruction.cases[0]?.body ?? [], "switch");
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "RAW_STATEMENT":
|
||||
assignRuntimeExpression(instruction.text, this.scopes.at(-1)!.variables);
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
case "BREAK":
|
||||
case "CONTINUE":
|
||||
case "UNSUPPORTED_RUNTIME":
|
||||
this.advance();
|
||||
return this.trace("diagnostic", {
|
||||
instruction,
|
||||
diagnostic: {
|
||||
severity: instruction.kind === "UNSUPPORTED_RUNTIME" ? "warning" : "info",
|
||||
code: `VC_${instruction.kind}`,
|
||||
message: instruction.kind === "UNSUPPORTED_RUNTIME" ? instruction.message : `${instruction.kind} reached`,
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
|
||||
}
|
||||
});
|
||||
default:
|
||||
this.advance();
|
||||
return this.trace(instruction.kind === "IO_WRITE" || instruction.kind === "PULSE" ? "io" : "instruction", { instruction });
|
||||
}
|
||||
}
|
||||
|
||||
private executeCall(instruction: CallInstruction): RuntimeTraceEvent {
|
||||
const target = this.procedures.get(instruction.target);
|
||||
if (!target) {
|
||||
const result = this.fail(`Procedure ${instruction.target} was not found`, "VC_CALL_TARGET_NOT_FOUND");
|
||||
return this.trace("diagnostic", {
|
||||
instruction,
|
||||
...(result.diagnostic ? { diagnostic: result.diagnostic } : {})
|
||||
});
|
||||
}
|
||||
const previous = this.frame!;
|
||||
this.frame = {
|
||||
procedure: target.name,
|
||||
pc: 0,
|
||||
returnTo: {
|
||||
procedure: previous.procedure,
|
||||
pc: previous.pc + 1
|
||||
}
|
||||
};
|
||||
this.scopes.push({ id: target.name, variables: {} });
|
||||
return this.trace("instruction", { instruction, message: `Call ${target.name}` });
|
||||
}
|
||||
|
||||
private executeReturn(instruction: ReturnInstruction): RuntimeTraceEvent {
|
||||
if (instruction.value) {
|
||||
this.setVariable("$return", evaluateRuntimeExpression(instruction.value.text, this.variables()));
|
||||
}
|
||||
const trace = this.trace("instruction", { instruction });
|
||||
this.returnFromProcedure();
|
||||
return trace;
|
||||
}
|
||||
|
||||
private executeIf(branches: ExecutableBranch[], instruction: ExecutableInstruction): RuntimeTraceEvent {
|
||||
const branch = branches.find((candidate) => {
|
||||
if (candidate.branchKind === "else") {
|
||||
return true;
|
||||
}
|
||||
return candidate.condition ? evaluateRuntimeBoolean(candidate.condition.text, this.variables()) : false;
|
||||
});
|
||||
if (branch) {
|
||||
this.insertInlineBlock(branch.body, branch.branchKind);
|
||||
}
|
||||
this.advance();
|
||||
return this.trace("instruction", { instruction });
|
||||
}
|
||||
|
||||
private insertInlineBlock(body: ExecutableInstruction[], scopeId: string): void {
|
||||
const procedure = this.currentProcedure;
|
||||
if (!procedure || !this.frame || body.length === 0) {
|
||||
return;
|
||||
}
|
||||
procedure.instructions.splice(this.frame.pc + 1, 0, ...body);
|
||||
this.scopes.push({ id: `${scopeId}:${this.frame.pc}`, variables: {} });
|
||||
}
|
||||
|
||||
private runHook(instruction: ExecutableInstruction): RuntimeStepResult | undefined {
|
||||
const hook = this.options.hooks;
|
||||
const genericResult = hook?.onInstruction?.(instruction, this);
|
||||
if (genericResult) {
|
||||
return genericResult;
|
||||
}
|
||||
if (instruction.kind === "WAIT") {
|
||||
const result = hook?.onWait?.(instruction, this);
|
||||
return result ?? undefined;
|
||||
}
|
||||
if (instruction.kind === "IO_WRITE" || instruction.kind === "PULSE") {
|
||||
const result = hook?.onIo?.(instruction, this);
|
||||
return result ?? undefined;
|
||||
}
|
||||
if (
|
||||
instruction.kind === "MOVEJ" ||
|
||||
instruction.kind === "MOVEL" ||
|
||||
instruction.kind === "MOVEC" ||
|
||||
instruction.kind === "RUN_PATH" ||
|
||||
instruction.kind === "RUN_OPERATION"
|
||||
) {
|
||||
const result = hook?.onMotion?.(instruction, this);
|
||||
return result ?? undefined;
|
||||
}
|
||||
return undefined;
|
||||
}
|
||||
|
||||
private matchBreakpoint(instruction: ExecutableInstruction): RuntimeBreakpoint | undefined {
|
||||
const source = instruction.sourceMap;
|
||||
return [...this.breakpoints.values()].find((breakpoint) => {
|
||||
if (!breakpoint.enabled) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.procedure && breakpoint.procedure !== this.frame?.procedure) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.pc !== undefined && breakpoint.pc !== this.frame?.pc) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.source?.line !== undefined && breakpoint.source.line !== source?.line) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.source?.file !== undefined && breakpoint.source.file !== source?.file) {
|
||||
return false;
|
||||
}
|
||||
return breakpoint.pc !== undefined || breakpoint.source !== undefined;
|
||||
});
|
||||
}
|
||||
|
||||
private advance(): void {
|
||||
if (this.frame) {
|
||||
this.frame.pc += 1;
|
||||
}
|
||||
}
|
||||
|
||||
private returnFromProcedure(): RuntimeStepResult {
|
||||
if (!this.frame?.returnTo) {
|
||||
this.completedValue = true;
|
||||
return { status: "completed" };
|
||||
}
|
||||
const returnTo = this.frame.returnTo;
|
||||
this.scopes.pop();
|
||||
this.frame = {
|
||||
procedure: returnTo.procedure,
|
||||
pc: returnTo.pc
|
||||
};
|
||||
return { status: "executed" };
|
||||
}
|
||||
|
||||
private fail(message: string, code: string): RuntimeStepResult {
|
||||
const diagnostic: MotionDiagnostic = {
|
||||
severity: "error",
|
||||
code,
|
||||
message
|
||||
};
|
||||
this.trace("diagnostic", { diagnostic, message });
|
||||
return {
|
||||
status: "blocked",
|
||||
diagnostic
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
function normalizeAlarmSeverity(severity?: string): RuntimeAlarm["severity"] {
|
||||
return severity === "info" || severity === "warning" || severity === "error" ? severity : "error";
|
||||
}
|
||||
143
kdl-wasm/web/src/controller/sourceMap.ts
Normal file
143
kdl-wasm/web/src/controller/sourceMap.ts
Normal file
@@ -0,0 +1,143 @@
|
||||
import type { SemanticProgramIr, SemanticSourceMapEntry } from "../grl/ir/index.js";
|
||||
import type { MotionSourceMap } from "../kdl/types.js";
|
||||
|
||||
export interface RuntimeSourceLocation {
|
||||
kind: string;
|
||||
id?: string;
|
||||
sourceMap?: MotionSourceMap;
|
||||
pathId?: string;
|
||||
pointId?: string;
|
||||
operationId?: string;
|
||||
procedureId?: string;
|
||||
brand?: {
|
||||
vendor?: string;
|
||||
file?: string;
|
||||
line?: number;
|
||||
column?: number;
|
||||
symbol?: string;
|
||||
};
|
||||
}
|
||||
|
||||
export class RuntimeSourceMapIndex {
|
||||
private readonly entries: SemanticSourceMapEntry[];
|
||||
|
||||
constructor(program: Pick<SemanticProgramIr, "sourceMap"> | SemanticSourceMapEntry[] = []) {
|
||||
this.entries = Array.isArray(program) ? [...program] : [...program.sourceMap];
|
||||
}
|
||||
|
||||
all(): RuntimeSourceLocation[] {
|
||||
return this.entries.map((entry) => this.toLocation(entry));
|
||||
}
|
||||
|
||||
locateByInstruction(input: {
|
||||
kind?: string;
|
||||
sourceMap?: MotionSourceMap;
|
||||
pathId?: string;
|
||||
pointId?: string;
|
||||
operationId?: string;
|
||||
procedureId?: string;
|
||||
id?: string;
|
||||
source?: Record<string, unknown>;
|
||||
}): RuntimeSourceLocation | undefined {
|
||||
const sourceLine = input.sourceMap?.line;
|
||||
const sourceFile = input.sourceMap?.file;
|
||||
const entry = this.entries.find((candidate) => {
|
||||
if (input.pathId && candidate.pathId !== input.pathId) {
|
||||
return false;
|
||||
}
|
||||
if (input.pointId && candidate.pointId !== input.pointId) {
|
||||
return false;
|
||||
}
|
||||
if (input.operationId && candidate.operationId !== input.operationId) {
|
||||
return false;
|
||||
}
|
||||
if (input.procedureId && candidate.procedureId !== input.procedureId) {
|
||||
return false;
|
||||
}
|
||||
if (input.kind && candidate.kind !== input.kind && candidate.id !== input.id) {
|
||||
return false;
|
||||
}
|
||||
if (sourceLine !== undefined && candidate.sourceMap.line !== sourceLine) {
|
||||
return false;
|
||||
}
|
||||
if (sourceFile !== undefined && candidate.sourceMap.file !== sourceFile) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
});
|
||||
|
||||
if (entry) {
|
||||
return this.toLocation(entry, input.source);
|
||||
}
|
||||
|
||||
if (input.sourceMap || input.pathId || input.operationId || input.pointId || input.procedureId) {
|
||||
const brand = brandSource(input.source);
|
||||
return {
|
||||
kind: input.kind ?? "instruction",
|
||||
...(input.id ? { id: input.id } : {}),
|
||||
...(input.sourceMap ? { sourceMap: input.sourceMap } : {}),
|
||||
...(input.pathId ? { pathId: input.pathId } : {}),
|
||||
...(input.pointId ? { pointId: input.pointId } : {}),
|
||||
...(input.operationId ? { operationId: input.operationId } : {}),
|
||||
...(input.procedureId ? { procedureId: input.procedureId } : {}),
|
||||
...(brand ? { brand } : {})
|
||||
};
|
||||
}
|
||||
|
||||
return undefined;
|
||||
}
|
||||
|
||||
locatePathPoint(pathId: string, pointId: string): RuntimeSourceLocation | undefined {
|
||||
return this.find((entry) => entry.pathId === pathId && entry.pointId === pointId);
|
||||
}
|
||||
|
||||
locateOperation(operationId: string): RuntimeSourceLocation[] {
|
||||
return this.entries
|
||||
.filter((entry) => entry.operationId === operationId)
|
||||
.map((entry) => this.toLocation(entry));
|
||||
}
|
||||
|
||||
locateSource(sourceMap: MotionSourceMap): RuntimeSourceLocation[] {
|
||||
return this.entries
|
||||
.filter((entry) =>
|
||||
(sourceMap.file === undefined || entry.sourceMap.file === sourceMap.file) &&
|
||||
(sourceMap.line === undefined || entry.sourceMap.line === sourceMap.line) &&
|
||||
(sourceMap.column === undefined || entry.sourceMap.column === sourceMap.column)
|
||||
)
|
||||
.map((entry) => this.toLocation(entry));
|
||||
}
|
||||
|
||||
private find(predicate: (entry: SemanticSourceMapEntry) => boolean): RuntimeSourceLocation | undefined {
|
||||
const entry = this.entries.find(predicate);
|
||||
return entry ? this.toLocation(entry) : undefined;
|
||||
}
|
||||
|
||||
private toLocation(entry: SemanticSourceMapEntry, source?: Record<string, unknown>): RuntimeSourceLocation {
|
||||
const brand = brandSource(source);
|
||||
return {
|
||||
kind: entry.kind,
|
||||
id: entry.id,
|
||||
sourceMap: entry.sourceMap,
|
||||
...(entry.pathId ? { pathId: entry.pathId } : {}),
|
||||
...(entry.pointId ? { pointId: entry.pointId } : {}),
|
||||
...(entry.operationId ? { operationId: entry.operationId } : {}),
|
||||
...(entry.procedureId ? { procedureId: entry.procedureId } : {}),
|
||||
...(brand ? { brand } : {})
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
function brandSource(source?: Record<string, unknown>): RuntimeSourceLocation["brand"] | undefined {
|
||||
const brand = source?.brand;
|
||||
if (!brand || typeof brand !== "object") {
|
||||
return undefined;
|
||||
}
|
||||
const record = brand as Record<string, unknown>;
|
||||
return {
|
||||
...(typeof record.vendor === "string" ? { vendor: record.vendor } : {}),
|
||||
...(typeof record.file === "string" ? { file: record.file } : {}),
|
||||
...(typeof record.line === "number" ? { line: record.line } : {}),
|
||||
...(typeof record.column === "number" ? { column: record.column } : {}),
|
||||
...(typeof record.symbol === "string" ? { symbol: record.symbol } : {})
|
||||
};
|
||||
}
|
||||
145
kdl-wasm/web/src/controller/stateMachine.ts
Normal file
145
kdl-wasm/web/src/controller/stateMachine.ts
Normal file
@@ -0,0 +1,145 @@
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
|
||||
export type VirtualControllerState =
|
||||
| "unloaded"
|
||||
| "stopped"
|
||||
| "running"
|
||||
| "paused"
|
||||
| "hold"
|
||||
| "faulted";
|
||||
|
||||
export type VirtualControllerCommand =
|
||||
| "load"
|
||||
| "run"
|
||||
| "pause"
|
||||
| "stop"
|
||||
| "reset"
|
||||
| "step"
|
||||
| "hold";
|
||||
|
||||
export interface StateTransitionResult {
|
||||
ok: boolean;
|
||||
previous: VirtualControllerState;
|
||||
state: VirtualControllerState;
|
||||
command: VirtualControllerCommand;
|
||||
diagnostic?: MotionDiagnostic;
|
||||
}
|
||||
|
||||
export interface VirtualControllerStateMachineSnapshot {
|
||||
state: VirtualControllerState;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
const TRANSITIONS: Record<
|
||||
VirtualControllerCommand,
|
||||
Partial<Record<VirtualControllerState, VirtualControllerState>>
|
||||
> = {
|
||||
load: {
|
||||
unloaded: "stopped",
|
||||
stopped: "stopped",
|
||||
faulted: "stopped"
|
||||
},
|
||||
run: {
|
||||
stopped: "running",
|
||||
paused: "running",
|
||||
hold: "running"
|
||||
},
|
||||
pause: {
|
||||
running: "paused"
|
||||
},
|
||||
stop: {
|
||||
running: "stopped",
|
||||
paused: "stopped",
|
||||
hold: "stopped",
|
||||
faulted: "stopped"
|
||||
},
|
||||
reset: {
|
||||
stopped: "stopped",
|
||||
paused: "stopped",
|
||||
hold: "stopped",
|
||||
faulted: "stopped"
|
||||
},
|
||||
step: {
|
||||
stopped: "paused",
|
||||
paused: "paused"
|
||||
},
|
||||
hold: {
|
||||
running: "hold",
|
||||
paused: "hold"
|
||||
}
|
||||
};
|
||||
|
||||
export class VirtualControllerStateMachine {
|
||||
private stateValue: VirtualControllerState;
|
||||
private readonly diagnosticsValue: MotionDiagnostic[] = [];
|
||||
|
||||
constructor(initialState: VirtualControllerState = "unloaded") {
|
||||
this.stateValue = initialState;
|
||||
}
|
||||
|
||||
get state(): VirtualControllerState {
|
||||
return this.stateValue;
|
||||
}
|
||||
|
||||
get diagnostics(): MotionDiagnostic[] {
|
||||
return [...this.diagnosticsValue];
|
||||
}
|
||||
|
||||
dispatch(command: VirtualControllerCommand): StateTransitionResult {
|
||||
const previous = this.stateValue;
|
||||
const next = TRANSITIONS[command][previous];
|
||||
if (!next) {
|
||||
const diagnostic: MotionDiagnostic = {
|
||||
severity: "warning",
|
||||
code: "VC_INVALID_STATE_TRANSITION",
|
||||
message: `Cannot ${command} while controller is ${previous}`,
|
||||
data: { command, state: previous }
|
||||
};
|
||||
this.diagnosticsValue.push(diagnostic);
|
||||
return {
|
||||
ok: false,
|
||||
previous,
|
||||
state: previous,
|
||||
command,
|
||||
diagnostic
|
||||
};
|
||||
}
|
||||
|
||||
this.stateValue = next;
|
||||
return {
|
||||
ok: true,
|
||||
previous,
|
||||
state: next,
|
||||
command
|
||||
};
|
||||
}
|
||||
|
||||
fault(message: string, code = "VC_CONTROLLER_FAULT"): StateTransitionResult {
|
||||
const previous = this.stateValue;
|
||||
const diagnostic: MotionDiagnostic = {
|
||||
severity: "error",
|
||||
code,
|
||||
message
|
||||
};
|
||||
this.diagnosticsValue.push(diagnostic);
|
||||
this.stateValue = "faulted";
|
||||
return {
|
||||
ok: true,
|
||||
previous,
|
||||
state: "faulted",
|
||||
command: "stop",
|
||||
diagnostic
|
||||
};
|
||||
}
|
||||
|
||||
resetDiagnostics(): void {
|
||||
this.diagnosticsValue.length = 0;
|
||||
}
|
||||
|
||||
snapshot(): VirtualControllerStateMachineSnapshot {
|
||||
return {
|
||||
state: this.stateValue,
|
||||
diagnostics: this.diagnostics
|
||||
};
|
||||
}
|
||||
}
|
||||
56
kdl-wasm/web/src/controller/trace.ts
Normal file
56
kdl-wasm/web/src/controller/trace.ts
Normal file
@@ -0,0 +1,56 @@
|
||||
import type { ExecutableInstruction } from "../grl/ir/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import type { RuntimeSourceLocation } from "./sourceMap.js";
|
||||
|
||||
export type RuntimeTraceEventKind =
|
||||
| "load"
|
||||
| "state"
|
||||
| "instruction"
|
||||
| "motion"
|
||||
| "io"
|
||||
| "wait"
|
||||
| "alarm"
|
||||
| "breakpoint"
|
||||
| "diagnostic"
|
||||
| "script";
|
||||
|
||||
export interface RuntimeTraceEvent {
|
||||
id: number;
|
||||
time: number;
|
||||
kind: RuntimeTraceEventKind;
|
||||
procedure?: string;
|
||||
pc?: number;
|
||||
instructionKind?: ExecutableInstruction["kind"] | string;
|
||||
message?: string;
|
||||
source?: RuntimeSourceLocation;
|
||||
diagnostic?: MotionDiagnostic;
|
||||
data?: Record<string, unknown>;
|
||||
}
|
||||
|
||||
export class TraceBuffer {
|
||||
private readonly events: RuntimeTraceEvent[] = [];
|
||||
private nextId = 1;
|
||||
|
||||
constructor(private readonly capacity = 1000) {}
|
||||
|
||||
push(event: Omit<RuntimeTraceEvent, "id">): RuntimeTraceEvent {
|
||||
const next: RuntimeTraceEvent = {
|
||||
...event,
|
||||
id: this.nextId
|
||||
};
|
||||
this.nextId += 1;
|
||||
this.events.push(next);
|
||||
while (this.events.length > this.capacity) {
|
||||
this.events.shift();
|
||||
}
|
||||
return next;
|
||||
}
|
||||
|
||||
all(): RuntimeTraceEvent[] {
|
||||
return this.events.map((event) => ({ ...event }));
|
||||
}
|
||||
|
||||
clear(): void {
|
||||
this.events.length = 0;
|
||||
}
|
||||
}
|
||||
193
kdl-wasm/web/src/controller/virtualController.ts
Normal file
193
kdl-wasm/web/src/controller/virtualController.ts
Normal file
@@ -0,0 +1,193 @@
|
||||
import type { ExecutableInstruction, SemanticProgramIr } from "../grl/ir/index.js";
|
||||
import { IoImageRuntime, type IoRuntimeOptions } from "../runtime/ioRuntime.js";
|
||||
import { KdlRuntimeBridge } from "../runtime/kdlBridge.js";
|
||||
import { MotionQueue, type MotionPlanner } from "../runtime/motionQueue.js";
|
||||
import type { RobotHandle } from "../kdl/types.js";
|
||||
import { IrExecutionRuntime, type IrExecutionRuntimeSnapshot, type RuntimeStepResult } from "./runtime.js";
|
||||
import { VirtualControllerStateMachine } from "./stateMachine.js";
|
||||
import type { RuntimeTraceEvent } from "./trace.js";
|
||||
|
||||
export interface VirtualControllerLoadOptions {
|
||||
entryProcedure?: string;
|
||||
startJoints: number[];
|
||||
sampleTime: number;
|
||||
planner?: MotionPlanner;
|
||||
robotHandle?: RobotHandle;
|
||||
io?: IoRuntimeOptions;
|
||||
}
|
||||
|
||||
export interface VirtualControllerSnapshot {
|
||||
state: ReturnType<VirtualControllerStateMachine["snapshot"]>;
|
||||
runtime: IrExecutionRuntimeSnapshot;
|
||||
motion?: ReturnType<MotionQueue["snapshot"]>;
|
||||
io: ReturnType<IoImageRuntime["snapshot"]>;
|
||||
bridge?: ReturnType<KdlRuntimeBridge["snapshot"]>;
|
||||
}
|
||||
|
||||
export class VirtualController {
|
||||
readonly stateMachine = new VirtualControllerStateMachine();
|
||||
private ioValue = new IoImageRuntime();
|
||||
private runtimeValue: IrExecutionRuntime | undefined;
|
||||
private motionQueueValue: MotionQueue | undefined;
|
||||
private bridgeValue: KdlRuntimeBridge | undefined;
|
||||
private loadedOptions: VirtualControllerLoadOptions | undefined;
|
||||
|
||||
get io(): IoImageRuntime {
|
||||
return this.ioValue;
|
||||
}
|
||||
|
||||
get runtime(): IrExecutionRuntime | undefined {
|
||||
return this.runtimeValue;
|
||||
}
|
||||
|
||||
get motionQueue(): MotionQueue | undefined {
|
||||
return this.motionQueueValue;
|
||||
}
|
||||
|
||||
get bridge(): KdlRuntimeBridge | undefined {
|
||||
return this.bridgeValue;
|
||||
}
|
||||
|
||||
load(program: SemanticProgramIr, options: VirtualControllerLoadOptions): RuntimeTraceEvent | undefined {
|
||||
this.loadedOptions = options;
|
||||
this.ioValue = new IoImageRuntime(options.io);
|
||||
this.motionQueueValue = new MotionQueue({
|
||||
startJoints: options.startJoints,
|
||||
sampleTime: options.sampleTime,
|
||||
...(options.planner ? { planner: options.planner } : {}),
|
||||
...(options.robotHandle !== undefined ? { robotHandle: options.robotHandle } : {})
|
||||
});
|
||||
this.runtimeValue = new IrExecutionRuntime({
|
||||
...(options.entryProcedure ? { entryProcedure: options.entryProcedure } : {}),
|
||||
hooks: {
|
||||
onMotion: (instruction, runtime) => {
|
||||
this.bridgeValue?.enqueueInstruction(instruction);
|
||||
runtime.trace("motion", {
|
||||
instruction,
|
||||
data: { queued: true }
|
||||
});
|
||||
},
|
||||
onIo: (instruction, runtime) => {
|
||||
const result = this.ioValue.executeInstruction(instruction);
|
||||
runtime.trace("io", {
|
||||
instruction,
|
||||
data: { result }
|
||||
});
|
||||
},
|
||||
onWait: (instruction, runtime) => {
|
||||
const waitKey = `$wait:${runtime.currentFrame?.procedure ?? "main"}:${runtime.currentFrame?.pc ?? 0}`;
|
||||
if (runtime.getVariable(waitKey) === undefined) {
|
||||
runtime.setVariable(waitKey, runtime.virtualTime, "global");
|
||||
}
|
||||
const elapsed = runtime.virtualTime - Number(runtime.getVariable(waitKey) ?? runtime.virtualTime);
|
||||
const result = this.ioValue.evaluateWait(instruction, elapsed);
|
||||
runtime.trace("wait", {
|
||||
instruction,
|
||||
data: { status: result.status, elapsed }
|
||||
});
|
||||
if (result.status === "satisfied" || result.status === "timeout") {
|
||||
if (result.onTimeout?.kind === "alarm") {
|
||||
runtime.pushAlarm(result.onTimeout.value, result.onTimeout.value, "warning", runtime.locateInstruction(instruction));
|
||||
}
|
||||
return undefined;
|
||||
}
|
||||
if (result.status === "hold-stop") {
|
||||
this.stateMachine.dispatch("hold");
|
||||
}
|
||||
return {
|
||||
status: "blocked",
|
||||
instruction
|
||||
};
|
||||
}
|
||||
}
|
||||
});
|
||||
this.runtimeValue.load(program, options.entryProcedure);
|
||||
this.bridgeValue = new KdlRuntimeBridge(program, this.motionQueueValue);
|
||||
this.stateMachine.dispatch("load");
|
||||
return this.runtimeValue.snapshot().trace.at(-1);
|
||||
}
|
||||
|
||||
run(maxSteps?: number): RuntimeStepResult {
|
||||
const transition = this.stateMachine.dispatch("run");
|
||||
if (!transition.ok) {
|
||||
return blockedTransitionResult(transition.diagnostic);
|
||||
}
|
||||
const result = this.requireRuntime().run(maxSteps);
|
||||
if (result.status === "completed") {
|
||||
this.stateMachine.dispatch("stop");
|
||||
} else if (result.status === "breakpoint") {
|
||||
this.stateMachine.dispatch("pause");
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
pause(): void {
|
||||
this.stateMachine.dispatch("pause");
|
||||
}
|
||||
|
||||
stop(): void {
|
||||
this.stateMachine.dispatch("stop");
|
||||
}
|
||||
|
||||
reset(): void {
|
||||
const options = this.loadedOptions;
|
||||
this.stateMachine.dispatch("reset");
|
||||
if (!options || !this.runtimeValue || !this.runtimeValue.program) {
|
||||
return;
|
||||
}
|
||||
this.runtimeValue.reset(options.entryProcedure);
|
||||
this.motionQueueValue?.clear();
|
||||
}
|
||||
|
||||
step(): RuntimeStepResult {
|
||||
const transition = this.stateMachine.dispatch("step");
|
||||
if (!transition.ok) {
|
||||
return blockedTransitionResult(transition.diagnostic);
|
||||
}
|
||||
return this.requireRuntime().step();
|
||||
}
|
||||
|
||||
advance(deltaTime: number): void {
|
||||
this.io.advance(deltaTime);
|
||||
this.motionQueueValue?.advance(deltaTime);
|
||||
if (this.runtimeValue) {
|
||||
this.runtimeValue.virtualTime += deltaTime;
|
||||
}
|
||||
}
|
||||
|
||||
snapshot(): VirtualControllerSnapshot {
|
||||
return {
|
||||
state: this.stateMachine.snapshot(),
|
||||
runtime: this.requireRuntime().snapshot(),
|
||||
...(this.motionQueueValue ? { motion: this.motionQueueValue.snapshot() } : {}),
|
||||
io: this.ioValue.snapshot(),
|
||||
...(this.bridgeValue ? { bridge: this.bridgeValue.snapshot() } : {})
|
||||
};
|
||||
}
|
||||
|
||||
private requireRuntime(): IrExecutionRuntime {
|
||||
if (!this.runtimeValue) {
|
||||
throw new Error("Controller has not loaded a program");
|
||||
}
|
||||
return this.runtimeValue;
|
||||
}
|
||||
}
|
||||
|
||||
function blockedTransitionResult(diagnostic: RuntimeStepResult["diagnostic"]): RuntimeStepResult {
|
||||
return {
|
||||
status: "blocked",
|
||||
...(diagnostic ? { diagnostic } : {})
|
||||
};
|
||||
}
|
||||
|
||||
export function isMotionInstruction(
|
||||
instruction: ExecutableInstruction
|
||||
): instruction is Extract<ExecutableInstruction, { kind: "MOVEJ" | "MOVEL" | "MOVEC" | "RUN_PATH" | "RUN_OPERATION" }> {
|
||||
return (
|
||||
instruction.kind === "MOVEJ" ||
|
||||
instruction.kind === "MOVEL" ||
|
||||
instruction.kind === "MOVEC" ||
|
||||
instruction.kind === "RUN_PATH" ||
|
||||
instruction.kind === "RUN_OPERATION"
|
||||
);
|
||||
}
|
||||
155
kdl-wasm/web/src/docs/flowAssetCoverage.ts
Normal file
155
kdl-wasm/web/src/docs/flowAssetCoverage.ts
Normal file
@@ -0,0 +1,155 @@
|
||||
import { existsSync, readFileSync, statSync } from "node:fs";
|
||||
import { join } from "node:path";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
|
||||
export type FlowAssetId = "flow-01" | "flow-02" | "flow-03" | "flow-04" | "flow-05";
|
||||
|
||||
export interface FlowAssetMapping {
|
||||
id: FlowAssetId;
|
||||
title: string;
|
||||
mermaidFile: string;
|
||||
pngFile: string;
|
||||
tasks: string[];
|
||||
evidence: string;
|
||||
chapters: string[];
|
||||
requiredTerms: string[];
|
||||
}
|
||||
|
||||
export interface FlowAssetCoverageResult {
|
||||
ok: boolean;
|
||||
rootDir: string;
|
||||
assets: Array<FlowAssetMapping & {
|
||||
mermaidBytes: number;
|
||||
pngBytes: number;
|
||||
}>;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export const FLOW_ASSET_MAPPINGS: FlowAssetMapping[] = [
|
||||
{
|
||||
id: "flow-01",
|
||||
title: "总体功能流程",
|
||||
mermaidFile: "flow-01.mmd",
|
||||
pngFile: "flow-01.png",
|
||||
tasks: ["KW-200", "KW-202", "KW-203", "KW-205", "KW-206", "KW-211.1"],
|
||||
evidence: "EV-211",
|
||||
chapters: ["FLOW 2", "FLOW 5", "FLOW 6"],
|
||||
requiredTerms: ["项目资源", "GRL Lexer", "KDL WASM API", "Motion Queue", "Post Processor"]
|
||||
},
|
||||
{
|
||||
id: "flow-02",
|
||||
title: "GRL 编译执行流程",
|
||||
mermaidFile: "flow-02.mmd",
|
||||
pngFile: "flow-02.png",
|
||||
tasks: ["KW-100", "KW-101", "KW-110", "KW-202", "KW-203", "KW-204", "KW-211.2"],
|
||||
evidence: "EV-211",
|
||||
chapters: ["FLOW 3", "GRL 20", "GRL 21"],
|
||||
requiredTerms: ["Lexer", "Parser", "Semantic Analyzer", "Executable IR", "Wait Registry"]
|
||||
},
|
||||
{
|
||||
id: "flow-03",
|
||||
title: "KDL 计算流程",
|
||||
mermaidFile: "flow-03.mmd",
|
||||
pngFile: "flow-03.png",
|
||||
tasks: ["KW-002", "KW-008", "KW-009", "KW-010", "KW-011", "KW-211.3"],
|
||||
evidence: "EV-211",
|
||||
chapters: ["FLOW 4", "KDL 11", "KDL 12", "KDL 13", "KDL 14"],
|
||||
requiredTerms: ["RobotHandle", "planMoveJ", "planMoveL", "planMoveC", "planPath"]
|
||||
},
|
||||
{
|
||||
id: "flow-04",
|
||||
title: "数据传递流程",
|
||||
mermaidFile: "flow-04.mmd",
|
||||
pngFile: "flow-04.png",
|
||||
tasks: ["KW-200", "KW-201", "KW-202", "KW-203", "KW-204", "KW-206", "KW-211.4"],
|
||||
evidence: "EV-211",
|
||||
chapters: ["FLOW 5", "FLOW 9"],
|
||||
requiredTerms: ["Project", "Compile", "KDL", "Runtime", "Output"]
|
||||
},
|
||||
{
|
||||
id: "flow-05",
|
||||
title: "诊断传递流程",
|
||||
mermaidFile: "flow-05.mmd",
|
||||
pngFile: "flow-05.png",
|
||||
tasks: ["KW-110", "KW-111", "KW-206", "KW-209", "KW-211.5"],
|
||||
evidence: "EV-211",
|
||||
chapters: ["FLOW 9", "FLOW 10"],
|
||||
requiredTerms: ["Diagnostic", "severity", "sourceMap", "GRL file", "brandSource"]
|
||||
}
|
||||
];
|
||||
|
||||
export function validateFlowAssetCoverage(rootDir: string): FlowAssetCoverageResult {
|
||||
const diagnostics: MotionDiagnostic[] = [];
|
||||
const assets: FlowAssetCoverageResult["assets"] = [];
|
||||
|
||||
for (const mapping of FLOW_ASSET_MAPPINGS) {
|
||||
const mermaidPath = join(rootDir, mapping.mermaidFile);
|
||||
const pngPath = join(rootDir, mapping.pngFile);
|
||||
const mermaidBytes = fileSizeOrDiagnostic(mermaidPath, mapping, "mermaid", diagnostics);
|
||||
const pngBytes = fileSizeOrDiagnostic(pngPath, mapping, "png", diagnostics);
|
||||
|
||||
if (mermaidBytes > 0) {
|
||||
const source = readFileSync(mermaidPath, "utf8");
|
||||
for (const term of mapping.requiredTerms) {
|
||||
if (!source.includes(term)) {
|
||||
diagnostics.push({
|
||||
severity: "error",
|
||||
code: "FLOW_REQUIRED_TERM_MISSING",
|
||||
message: `${mapping.id} is missing required term ${term}`,
|
||||
data: { id: mapping.id, term, file: mapping.mermaidFile }
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (mapping.tasks.length === 0 || !mapping.tasks.some((task) => task.startsWith("KW-211"))) {
|
||||
diagnostics.push({
|
||||
severity: "error",
|
||||
code: "FLOW_TASK_MAPPING_MISSING",
|
||||
message: `${mapping.id} must map to a KW-211 subtask`,
|
||||
data: { id: mapping.id }
|
||||
});
|
||||
}
|
||||
|
||||
assets.push({
|
||||
...mapping,
|
||||
mermaidBytes,
|
||||
pngBytes
|
||||
});
|
||||
}
|
||||
|
||||
return {
|
||||
ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"),
|
||||
rootDir,
|
||||
assets,
|
||||
diagnostics
|
||||
};
|
||||
}
|
||||
|
||||
function fileSizeOrDiagnostic(
|
||||
path: string,
|
||||
mapping: FlowAssetMapping,
|
||||
kind: "mermaid" | "png",
|
||||
diagnostics: MotionDiagnostic[]
|
||||
): number {
|
||||
if (!existsSync(path)) {
|
||||
diagnostics.push({
|
||||
severity: "error",
|
||||
code: "FLOW_ASSET_MISSING",
|
||||
message: `${mapping.id} ${kind} asset is missing`,
|
||||
data: { id: mapping.id, path }
|
||||
});
|
||||
return 0;
|
||||
}
|
||||
|
||||
const size = statSync(path).size;
|
||||
if (size <= 0) {
|
||||
diagnostics.push({
|
||||
severity: "error",
|
||||
code: "FLOW_ASSET_EMPTY",
|
||||
message: `${mapping.id} ${kind} asset is empty`,
|
||||
data: { id: mapping.id, path }
|
||||
});
|
||||
}
|
||||
return size;
|
||||
}
|
||||
7
kdl-wasm/web/src/docs/index.ts
Normal file
7
kdl-wasm/web/src/docs/index.ts
Normal file
@@ -0,0 +1,7 @@
|
||||
export {
|
||||
FLOW_ASSET_MAPPINGS,
|
||||
validateFlowAssetCoverage,
|
||||
type FlowAssetCoverageResult,
|
||||
type FlowAssetId,
|
||||
type FlowAssetMapping
|
||||
} from "./flowAssetCoverage.js";
|
||||
54
kdl-wasm/web/src/fixtures/abb120.ts
Normal file
54
kdl-wasm/web/src/fixtures/abb120.ts
Normal file
@@ -0,0 +1,54 @@
|
||||
export const ABB_IRB120_URDF_SOURCE = {
|
||||
repository: "https://github.com/ros-industrial/abb",
|
||||
branch: "noetic-devel",
|
||||
entrypoint: "abb_irb120_support/urdf/irb120_3_58.xacro",
|
||||
macro: "abb_irb120_support/urdf/irb120_3_58_macro.xacro"
|
||||
} as const;
|
||||
|
||||
export const ABB_IRB120_3_58_URDF = `
|
||||
<robot name="abb_irb120_3_58">
|
||||
<link name="base_link"/>
|
||||
<link name="link_1"/>
|
||||
<link name="link_2"/>
|
||||
<link name="link_3"/>
|
||||
<link name="link_4"/>
|
||||
<link name="link_5"/>
|
||||
<link name="link_6"/>
|
||||
<link name="base"/>
|
||||
<link name="flange"/>
|
||||
<link name="tool0"/>
|
||||
<joint name="joint_1" type="revolute"><origin rpy="0 0 0" xyz="0 0 0"/><parent link="base_link"/><child link="link_1"/><limit effort="0" lower="-2.87979" upper="2.87979" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 0 1"/></joint>
|
||||
<joint name="joint_2" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.29"/><parent link="link_1"/><child link="link_2"/><limit effort="0" lower="-1.91986" upper="1.91986" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 1 0"/></joint>
|
||||
<joint name="joint_3" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.27"/><parent link="link_2"/><child link="link_3"/><limit effort="0" lower="-1.91986" upper="1.22173" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 1 0"/></joint>
|
||||
<joint name="joint_4" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.07"/><parent link="link_3"/><child link="link_4"/><limit effort="0" lower="-2.79253" upper="2.79253" velocity="5.58505" acceleration="11.1701"/><axis xyz="1 0 0"/></joint>
|
||||
<joint name="joint_5" type="revolute"><origin rpy="0 0 0" xyz="0.302 0 0"/><parent link="link_4"/><child link="link_5"/><limit effort="0" lower="-2.094395" upper="2.094395" velocity="5.58505" acceleration="11.1701"/><axis xyz="0 1 0"/></joint>
|
||||
<joint name="joint_6" type="revolute"><origin rpy="0 0 0" xyz="0.072 0 0"/><parent link="link_5"/><child link="link_6"/><limit effort="0" lower="-6.98132" upper="6.98132" velocity="7.33038" acceleration="14.66076"/><axis xyz="1 0 0"/></joint>
|
||||
<joint name="base_link-base" type="fixed"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="base_link"/><child link="base"/></joint>
|
||||
<joint name="joint_6-flange" type="fixed"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="link_6"/><child link="flange"/></joint>
|
||||
<joint name="link_6-tool0" type="fixed"><origin xyz="0 0 0" rpy="0 1.5707963267948966 0"/><parent link="flange"/><child link="tool0"/></joint>
|
||||
</robot>
|
||||
`;
|
||||
|
||||
export const ABB_IRB120_LOAD_OPTIONS = {
|
||||
robotId: "abb_irb120_3_58",
|
||||
baseLink: "base_link",
|
||||
tipLink: "tool0"
|
||||
} as const;
|
||||
|
||||
export const ABB_IRB120_ZERO_JOINTS = [0, 0, 0, 0, 0, 0] as const;
|
||||
export const ABB_IRB120_PICK_JOINTS = [0.2, -0.35, 0.45, 0.1, -0.2, 0.3] as const;
|
||||
export const ABB_IRB120_PLACE_JOINTS = [-0.35, -0.25, 0.35, -0.25, 0.15, -0.4] as const;
|
||||
export const ABB_IRB120_APPROACH_JOINTS = [0, -0.4, 0.5, 0, 0.2, 0] as const;
|
||||
|
||||
export const ABB120_ROBOT_FIXTURE = {
|
||||
robotId: ABB_IRB120_LOAD_OPTIONS.robotId,
|
||||
urdf: ABB_IRB120_3_58_URDF,
|
||||
loadOptions: ABB_IRB120_LOAD_OPTIONS,
|
||||
source: ABB_IRB120_URDF_SOURCE,
|
||||
standardJoints: {
|
||||
home: ABB_IRB120_ZERO_JOINTS,
|
||||
approach: ABB_IRB120_APPROACH_JOINTS,
|
||||
pick: ABB_IRB120_PICK_JOINTS,
|
||||
place: ABB_IRB120_PLACE_JOINTS
|
||||
}
|
||||
} as const;
|
||||
@@ -25,7 +25,10 @@ export type GrlAstNodeKind =
|
||||
| "ArrayExpression"
|
||||
| "CallExpression"
|
||||
| "ObjectExpression"
|
||||
| "OffsetExpression";
|
||||
| "OffsetExpression"
|
||||
| "UnaryExpression"
|
||||
| "BinaryExpression"
|
||||
| "UnitExpression";
|
||||
|
||||
export interface GrlAstNode {
|
||||
kind: GrlAstNodeKind;
|
||||
@@ -85,7 +88,7 @@ export interface GrlPathEvent extends GrlAstNode {
|
||||
kind: "PathEvent";
|
||||
timing: "before" | "after" | "at";
|
||||
pointId: string;
|
||||
distance?: GrlNumberLiteral;
|
||||
distance?: GrlExpression;
|
||||
actionTokens: GrlToken[];
|
||||
}
|
||||
|
||||
@@ -217,7 +220,7 @@ export interface GrlObjectExpression extends GrlAstNode {
|
||||
|
||||
export interface GrlOffsetAxis {
|
||||
axis: "x" | "y" | "z";
|
||||
value: GrlNumberLiteral;
|
||||
value: GrlExpression;
|
||||
}
|
||||
|
||||
export interface GrlOffsetExpression extends GrlAstNode {
|
||||
@@ -228,6 +231,45 @@ export interface GrlOffsetExpression extends GrlAstNode {
|
||||
axes: GrlOffsetAxis[];
|
||||
}
|
||||
|
||||
export interface GrlUnaryExpression extends GrlAstNode {
|
||||
kind: "UnaryExpression";
|
||||
operator: "+" | "-" | "not" | "!";
|
||||
argument: GrlExpression;
|
||||
}
|
||||
|
||||
export interface GrlBinaryExpression extends GrlAstNode {
|
||||
kind: "BinaryExpression";
|
||||
operator:
|
||||
| "+"
|
||||
| "-"
|
||||
| "*"
|
||||
| "/"
|
||||
| "mod"
|
||||
| "=="
|
||||
| "!="
|
||||
| "<"
|
||||
| "<="
|
||||
| ">"
|
||||
| ">="
|
||||
| "and"
|
||||
| "or"
|
||||
| "&&"
|
||||
| "||";
|
||||
left: GrlExpression;
|
||||
right: GrlExpression;
|
||||
}
|
||||
|
||||
export interface GrlUnitExpression extends GrlAstNode {
|
||||
kind: "UnitExpression";
|
||||
expression: GrlExpression;
|
||||
unit: {
|
||||
raw: string;
|
||||
kind: string;
|
||||
siUnit: string;
|
||||
factor: number;
|
||||
};
|
||||
}
|
||||
|
||||
export type GrlExpression =
|
||||
| GrlIdentifierExpression
|
||||
| GrlNumberLiteral
|
||||
@@ -236,4 +278,7 @@ export type GrlExpression =
|
||||
| GrlArrayExpression
|
||||
| GrlCallExpression
|
||||
| GrlObjectExpression
|
||||
| GrlOffsetExpression;
|
||||
| GrlOffsetExpression
|
||||
| GrlUnaryExpression
|
||||
| GrlBinaryExpression
|
||||
| GrlUnitExpression;
|
||||
|
||||
@@ -2,6 +2,7 @@ export type {
|
||||
GrlAstNode,
|
||||
GrlAstNodeKind,
|
||||
GrlArrayExpression,
|
||||
GrlBinaryExpression,
|
||||
GrlBooleanLiteral,
|
||||
GrlCallExpression,
|
||||
GrlDataDeclaration,
|
||||
@@ -33,5 +34,7 @@ export type {
|
||||
GrlRawTopLevelDeclaration,
|
||||
GrlStringLiteral,
|
||||
GrlTargetDeclaration,
|
||||
GrlTopLevelDeclaration
|
||||
GrlTopLevelDeclaration,
|
||||
GrlUnaryExpression,
|
||||
GrlUnitExpression
|
||||
} from "./ast.js";
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import type {
|
||||
GrlArrayExpression,
|
||||
GrlBinaryExpression,
|
||||
GrlBooleanLiteral,
|
||||
GrlCallExpression,
|
||||
GrlExpression,
|
||||
@@ -9,9 +10,12 @@ import type {
|
||||
GrlObjectProperty,
|
||||
GrlOffsetAxis,
|
||||
GrlOffsetExpression,
|
||||
GrlStringLiteral
|
||||
GrlStringLiteral,
|
||||
GrlUnaryExpression,
|
||||
GrlUnitExpression
|
||||
} from "../ast/index.js";
|
||||
import type { GrlToken } from "../lexer/index.js";
|
||||
import { isGrlUnitLiteral, normalizeUnitLiteral } from "../lexer/units.js";
|
||||
import { GrlParseError } from "./errors.js";
|
||||
|
||||
export function parseGrlExpression(tokens: GrlToken[]): GrlExpression {
|
||||
@@ -25,15 +29,83 @@ class GrlExpressionParser {
|
||||
constructor(private readonly tokens: GrlToken[]) {}
|
||||
|
||||
parse(): GrlExpression {
|
||||
const expression = this.parseOffsetExpression();
|
||||
const expression = this.parseExpression();
|
||||
if (!this.isAtEnd()) {
|
||||
throw new GrlParseError("Unexpected token after expression", this.peek());
|
||||
}
|
||||
return expression;
|
||||
}
|
||||
|
||||
private parseExpression(): GrlExpression {
|
||||
return this.parseOrExpression();
|
||||
}
|
||||
|
||||
private parseOrExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseAndExpression(), ["or", "||"]);
|
||||
}
|
||||
|
||||
private parseAndExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseEqualityExpression(), ["and", "&&"]);
|
||||
}
|
||||
|
||||
private parseEqualityExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseComparisonExpression(), ["==", "!="]);
|
||||
}
|
||||
|
||||
private parseComparisonExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseAdditiveExpression(), ["<", "<=", ">", ">="]);
|
||||
}
|
||||
|
||||
private parseAdditiveExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseMultiplicativeExpression(), ["+", "-"]);
|
||||
}
|
||||
|
||||
private parseMultiplicativeExpression(): GrlExpression {
|
||||
return this.parseBinaryLeftAssoc(() => this.parseUnaryExpression(), ["*", "/", "mod"]);
|
||||
}
|
||||
|
||||
private parseUnaryExpression(): GrlExpression {
|
||||
const token = this.peek();
|
||||
if (this.isUnaryOperator(token)) {
|
||||
const operatorToken = this.advance();
|
||||
const argument = this.parseUnaryExpression();
|
||||
const expression: GrlUnaryExpression = {
|
||||
kind: "UnaryExpression",
|
||||
operator: operatorToken.raw as GrlUnaryExpression["operator"],
|
||||
argument,
|
||||
range: {
|
||||
start: operatorToken.range.start,
|
||||
end: argument.range.end
|
||||
}
|
||||
};
|
||||
return expression;
|
||||
}
|
||||
|
||||
return this.parseOffsetExpression();
|
||||
}
|
||||
|
||||
private parseBinaryLeftAssoc(parseOperand: () => GrlExpression, operators: string[]): GrlExpression {
|
||||
let expression = parseOperand();
|
||||
while (this.isBinaryOperator(this.peek(), operators)) {
|
||||
const operator = this.advance();
|
||||
const right = parseOperand();
|
||||
const binary: GrlBinaryExpression = {
|
||||
kind: "BinaryExpression",
|
||||
operator: operator.raw as GrlBinaryExpression["operator"],
|
||||
left: expression,
|
||||
right,
|
||||
range: {
|
||||
start: expression.range.start,
|
||||
end: right.range.end
|
||||
}
|
||||
};
|
||||
expression = binary;
|
||||
}
|
||||
return expression;
|
||||
}
|
||||
|
||||
private parseOffsetExpression(): GrlExpression {
|
||||
const base = this.parsePrimary();
|
||||
const base = this.parseUnitExpression();
|
||||
if (this.matchKeyword("offset")) {
|
||||
return this.finishOffsetExpression(base, "frame");
|
||||
}
|
||||
@@ -56,11 +128,7 @@ class GrlExpressionParser {
|
||||
const axes: GrlOffsetAxis[] = [];
|
||||
while (!this.isAtEnd()) {
|
||||
const axis = this.consumeAxis();
|
||||
const valueExpression = this.parsePrimary();
|
||||
if (valueExpression.kind !== "NumberLiteral") {
|
||||
throw new GrlParseError(`Expected length value after offset ${axis}`, valueExpression.range ? this.previous() : this.peek());
|
||||
}
|
||||
const value = valueExpression;
|
||||
const value = this.parseUnaryExpression();
|
||||
axes.push({ axis, value });
|
||||
}
|
||||
|
||||
@@ -81,13 +149,33 @@ class GrlExpressionParser {
|
||||
};
|
||||
}
|
||||
|
||||
private parseUnitExpression(): GrlExpression {
|
||||
let expression = this.parsePrimary();
|
||||
while (this.isUnitToken(this.peek())) {
|
||||
const unitToken = this.advance();
|
||||
const unit = normalizeUnitLiteral(unitToken.raw);
|
||||
const withUnit: GrlUnitExpression = {
|
||||
kind: "UnitExpression",
|
||||
expression,
|
||||
unit: {
|
||||
raw: unit.literal,
|
||||
kind: unit.kind,
|
||||
siUnit: unit.siUnit,
|
||||
factor: unit.factor
|
||||
},
|
||||
range: {
|
||||
start: expression.range.start,
|
||||
end: unitToken.range.end
|
||||
}
|
||||
};
|
||||
expression = withUnit;
|
||||
}
|
||||
return expression;
|
||||
}
|
||||
|
||||
private parsePrimary(): GrlExpression {
|
||||
const token = this.peek();
|
||||
|
||||
if (token.kind === "operator" && token.raw === "-") {
|
||||
return this.parseNegativeNumber();
|
||||
}
|
||||
|
||||
if (token.kind === "number") {
|
||||
this.advance();
|
||||
return numberLiteralFromToken(token);
|
||||
@@ -118,10 +206,16 @@ class GrlExpressionParser {
|
||||
}
|
||||
|
||||
if (token.kind === "punctuation" && token.raw === "(") {
|
||||
this.advance();
|
||||
const expression = this.parseOffsetExpression();
|
||||
this.consumePunctuation(")", "Expected ) after expression");
|
||||
return expression;
|
||||
const start = this.advance();
|
||||
const expression = this.parseExpression();
|
||||
const end = this.consumePunctuation(")", "Expected ) after expression");
|
||||
return {
|
||||
...expression,
|
||||
range: {
|
||||
start: start.range.start,
|
||||
end: end.range.end
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
if (token.kind === "identifier" || token.kind === "keyword") {
|
||||
@@ -147,7 +241,7 @@ class GrlExpressionParser {
|
||||
const start = this.consumePunctuation("[", "Expected [");
|
||||
const elements: GrlExpression[] = [];
|
||||
while (!this.checkPunctuation("]") && !this.isAtEnd()) {
|
||||
elements.push(this.parseOffsetExpression());
|
||||
elements.push(this.parseExpression());
|
||||
this.matchPunctuation(",");
|
||||
}
|
||||
const end = this.consumePunctuation("]", "Expected ] after array expression");
|
||||
@@ -162,32 +256,10 @@ class GrlExpressionParser {
|
||||
};
|
||||
}
|
||||
|
||||
private parseNegativeNumber(): GrlNumberLiteral {
|
||||
const minus = this.advance();
|
||||
const number = this.consumeNumberLiteral("Expected number after -");
|
||||
return {
|
||||
...number,
|
||||
value: -number.value,
|
||||
raw: `${minus.raw}${number.raw}`,
|
||||
...(number.unit
|
||||
? {
|
||||
unit: {
|
||||
...number.unit,
|
||||
normalizedValue: -number.unit.normalizedValue
|
||||
}
|
||||
}
|
||||
: {}),
|
||||
range: {
|
||||
start: minus.range.start,
|
||||
end: number.range.end
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
private finishCallExpression(callee: GrlToken): GrlCallExpression {
|
||||
const args: GrlExpression[] = [];
|
||||
while (!this.checkPunctuation(")") && !this.isAtEnd()) {
|
||||
args.push(this.parseOffsetExpression());
|
||||
args.push(this.parseExpression());
|
||||
this.matchPunctuation(",");
|
||||
}
|
||||
const end = this.consumePunctuation(")", "Expected ) after call expression");
|
||||
@@ -208,7 +280,7 @@ class GrlExpressionParser {
|
||||
while (!this.checkPunctuation("}") && !this.isAtEnd()) {
|
||||
const key = this.consumeIdentifierLike("Expected object property name");
|
||||
this.consumePunctuation(":", "Expected : after object property name");
|
||||
const value = this.parseOffsetExpression();
|
||||
const value = this.parseExpression();
|
||||
properties.push({
|
||||
key: key.raw,
|
||||
value,
|
||||
@@ -240,14 +312,6 @@ class GrlExpressionParser {
|
||||
throw new GrlParseError("Expected offset axis x, y, or z", token);
|
||||
}
|
||||
|
||||
private consumeNumberLiteral(message: string): GrlNumberLiteral {
|
||||
const token = this.consume("number", message);
|
||||
if (token.kind !== "number") {
|
||||
throw new GrlParseError(message, token);
|
||||
}
|
||||
return numberLiteralFromToken(token);
|
||||
}
|
||||
|
||||
private consumeIdentifierLike(message: string): GrlToken {
|
||||
const token = this.peek();
|
||||
if (token.kind === "identifier" || token.kind === "keyword") {
|
||||
@@ -256,13 +320,6 @@ class GrlExpressionParser {
|
||||
throw new GrlParseError(message, token);
|
||||
}
|
||||
|
||||
private consume(kind: GrlToken["kind"], message: string): GrlToken {
|
||||
if (this.check(kind)) {
|
||||
return this.advance();
|
||||
}
|
||||
throw new GrlParseError(message, this.peek());
|
||||
}
|
||||
|
||||
private consumeKeyword(keyword: string, message: string): GrlToken {
|
||||
if (this.checkKeyword(keyword)) {
|
||||
return this.advance();
|
||||
@@ -293,10 +350,6 @@ class GrlExpressionParser {
|
||||
return false;
|
||||
}
|
||||
|
||||
private check(kind: GrlToken["kind"]): boolean {
|
||||
return !this.isAtEnd() && this.peek().kind === kind;
|
||||
}
|
||||
|
||||
private checkKeyword(keyword: string): boolean {
|
||||
const token = this.peek();
|
||||
return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword;
|
||||
@@ -307,6 +360,24 @@ class GrlExpressionParser {
|
||||
return token.kind === "punctuation" && token.raw === value;
|
||||
}
|
||||
|
||||
private isUnaryOperator(token: GrlToken): boolean {
|
||||
return (
|
||||
(token.kind === "operator" && (token.raw === "+" || token.raw === "-" || token.raw === "!")) ||
|
||||
((token.kind === "keyword" || token.kind === "identifier") && token.raw === "not")
|
||||
);
|
||||
}
|
||||
|
||||
private isBinaryOperator(token: GrlToken, operators: string[]): boolean {
|
||||
return (
|
||||
(token.kind === "operator" || token.kind === "keyword" || token.kind === "identifier") &&
|
||||
operators.includes(token.raw)
|
||||
);
|
||||
}
|
||||
|
||||
private isUnitToken(token: GrlToken): boolean {
|
||||
return !this.isAtEnd() && (token.kind === "identifier" || token.kind === "keyword" || token.kind === "operator") && isGrlUnitLiteral(token.raw);
|
||||
}
|
||||
|
||||
private advance(): GrlToken {
|
||||
if (!this.isAtEnd()) {
|
||||
this.current += 1;
|
||||
|
||||
@@ -297,12 +297,13 @@ class GrlParser {
|
||||
let distance: GrlPathEvent["distance"];
|
||||
if (timing.raw === "at") {
|
||||
this.consumeIdentifierValue("distance", "Expected distance in event at");
|
||||
const distanceToken = this.peek();
|
||||
const distanceTokens = this.collectSignedNumberTokens();
|
||||
const distanceTokens = this.collectUntil((token, depth) =>
|
||||
depth.brace === 0 &&
|
||||
depth.bracket === 0 &&
|
||||
depth.paren === 0 &&
|
||||
((token.kind === "punctuation" && token.raw === "}") || this.isPathItemStart(token) || this.isPathEventActionStart(token))
|
||||
);
|
||||
const distanceExpression = parseGrlExpression(distanceTokens);
|
||||
if (distanceExpression.kind !== "NumberLiteral") {
|
||||
throw new GrlParseError("Expected numeric event distance", distanceToken);
|
||||
}
|
||||
distance = distanceExpression;
|
||||
}
|
||||
const actionTokens = this.collectPathEventActionTokens();
|
||||
@@ -648,6 +649,13 @@ class GrlParser {
|
||||
return tokens;
|
||||
}
|
||||
|
||||
private isPathEventActionStart(token: GrlToken): boolean {
|
||||
return (
|
||||
(token.kind === "keyword" || token.kind === "identifier") &&
|
||||
(token.raw === "io" || token.raw === "wait" || token.raw === "pulse" || token.raw === "alarm" || token.raw === "call")
|
||||
);
|
||||
}
|
||||
|
||||
private collectUntil(
|
||||
shouldStop: (
|
||||
token: GrlToken,
|
||||
|
||||
@@ -2,15 +2,19 @@ import { rpyToQuaternion } from "../../math/poseMath.js";
|
||||
import type { JointTarget, OffsetSpec, Pose, PoseTarget, SpeedSpec, ZoneSpec } from "../../kdl/types.js";
|
||||
import { KdlStructuredError } from "../../kdl/rpc.js";
|
||||
import type {
|
||||
GrlArrayExpression,
|
||||
GrlCallExpression,
|
||||
GrlDataDeclaration,
|
||||
GrlExpression,
|
||||
GrlNumberLiteral,
|
||||
GrlObjectExpression,
|
||||
GrlOffsetExpression,
|
||||
GrlTargetDeclaration
|
||||
} from "../ast/index.js";
|
||||
import {
|
||||
type ConstantEvaluationContext,
|
||||
type ConstantValue,
|
||||
evaluateConstantExpression,
|
||||
evaluateNumberExpression
|
||||
} from "./constantExpression.js";
|
||||
|
||||
export type CompiledGrlDataValue =
|
||||
| Pose
|
||||
@@ -37,17 +41,28 @@ export interface CompiledGrlTargetDeclaration {
|
||||
target: JointTarget | PoseTarget;
|
||||
}
|
||||
|
||||
export function compileGrlDataDeclaration(declaration: GrlDataDeclaration): CompiledGrlDataDeclaration {
|
||||
export function compileGrlDataDeclaration(
|
||||
declaration: GrlDataDeclaration,
|
||||
context: ConstantEvaluationContext | number = {}
|
||||
): CompiledGrlDataDeclaration {
|
||||
const evaluationContext = typeof context === "number" ? {} : context;
|
||||
return {
|
||||
name: declaration.name,
|
||||
storage: declaration.storage,
|
||||
typeName: declaration.typeName,
|
||||
value: compileByType(declaration.typeName, declaration.initializer)
|
||||
value: compileByType(declaration.typeName, declaration.initializer, evaluationContext)
|
||||
};
|
||||
}
|
||||
|
||||
export function compileGrlTargetDeclaration(declaration: GrlTargetDeclaration): CompiledGrlTargetDeclaration {
|
||||
const target = compileTargetExpression(declaration.target);
|
||||
export function compileGrlConstantValue(expression: GrlExpression): ConstantValue {
|
||||
return evaluateConstantExpression(expression);
|
||||
}
|
||||
|
||||
export function compileGrlTargetDeclaration(
|
||||
declaration: GrlTargetDeclaration,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): CompiledGrlTargetDeclaration {
|
||||
const target = compileTargetExpression(declaration.target, context);
|
||||
return {
|
||||
name: declaration.name,
|
||||
target: {
|
||||
@@ -57,17 +72,24 @@ export function compileGrlTargetDeclaration(declaration: GrlTargetDeclaration):
|
||||
};
|
||||
}
|
||||
|
||||
export function compileTargetExpression(expression: GrlExpression): JointTarget | PoseTarget {
|
||||
export function compileTargetExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): JointTarget | PoseTarget {
|
||||
if (isObjectExpression(expression, "joint_target")) {
|
||||
return {
|
||||
joints: compileNumberArray(requiredProperty(expression, "joints"))
|
||||
joints: compileNumberArray(requiredProperty(expression, "joints"), {
|
||||
...context,
|
||||
expectedKind: "angle",
|
||||
defaultUnit: "deg"
|
||||
})
|
||||
};
|
||||
}
|
||||
|
||||
if (isObjectExpression(expression, "pose_target")) {
|
||||
const pose = compilePoseExpression(requiredProperty(expression, "pose"));
|
||||
const pose = compilePoseExpression(requiredProperty(expression, "pose"), context);
|
||||
const configExpression = findProperty(expression, "config");
|
||||
const config = configExpression ? compileRobotConfig(configExpression) : undefined;
|
||||
const config = configExpression ? compileRobotConfig(configExpression, context) : undefined;
|
||||
return {
|
||||
pose,
|
||||
...(config ? { config } : {}),
|
||||
@@ -79,35 +101,51 @@ export function compileTargetExpression(expression: GrlExpression): JointTarget
|
||||
throw compileError("GRL_UNSUPPORTED_TARGET", "Expected joint_target or pose_target expression");
|
||||
}
|
||||
|
||||
export function compileSpeedExpression(expression: GrlExpression): SpeedSpec {
|
||||
export function compileSpeedExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): SpeedSpec {
|
||||
if (!isCallExpression(expression)) {
|
||||
throw compileError("GRL_INVALID_SPEED", "Speed expression must be a call");
|
||||
}
|
||||
|
||||
const first = expression.args[0];
|
||||
if (!first || first.kind !== "NumberLiteral") {
|
||||
if (!first) {
|
||||
throw compileError("GRL_INVALID_SPEED", "Speed expression requires a numeric value");
|
||||
}
|
||||
|
||||
if (expression.callee === "joint") {
|
||||
if (first.unit?.kind === "percent") {
|
||||
return { kind: "joint_percent", value: first.unit.normalizedValue };
|
||||
const value = evaluateConstantExpression(first, { ...context, defaultUnit: "%" });
|
||||
if (value.kind === "percent") {
|
||||
return { kind: "joint_percent", value: value.value as number };
|
||||
}
|
||||
return { kind: "joint_abs", velocity: normalizedNumber(first) };
|
||||
return { kind: "joint_abs", velocity: evaluateNumberExpression(first, { ...context, expectedKind: "angular_velocity", defaultUnit: "deg/s" }) };
|
||||
}
|
||||
|
||||
if (expression.callee === "linear") {
|
||||
return { kind: "linear", velocity: normalizedNumber(first), ...compileAcceleration(expression) };
|
||||
return {
|
||||
kind: "linear",
|
||||
velocity: evaluateNumberExpression(first, { ...context, expectedKind: "linear_velocity", defaultUnit: "mm/s" }),
|
||||
...compileAcceleration(expression, context)
|
||||
};
|
||||
}
|
||||
|
||||
if (expression.callee === "angular") {
|
||||
return { kind: "linear", velocity: 0, angularVelocity: normalizedNumber(first), ...compileAcceleration(expression) };
|
||||
return {
|
||||
kind: "linear",
|
||||
velocity: 0,
|
||||
angularVelocity: evaluateNumberExpression(first, { ...context, expectedKind: "angular_velocity", defaultUnit: "deg/s" }),
|
||||
...compileAcceleration(expression, context)
|
||||
};
|
||||
}
|
||||
|
||||
throw compileError("GRL_INVALID_SPEED", `Unsupported speed expression: ${expression.callee}`);
|
||||
}
|
||||
|
||||
export function compileZoneExpression(expression: GrlExpression): ZoneSpec {
|
||||
export function compileZoneExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): ZoneSpec {
|
||||
if (expression.kind === "IdentifierExpression") {
|
||||
if (expression.name === "fine") {
|
||||
return { kind: "fine" };
|
||||
@@ -119,28 +157,31 @@ export function compileZoneExpression(expression: GrlExpression): ZoneSpec {
|
||||
|
||||
if (isCallExpression(expression) && expression.callee === "z") {
|
||||
const first = expression.args[0];
|
||||
if (!first || first.kind !== "NumberLiteral") {
|
||||
if (!first) {
|
||||
throw compileError("GRL_INVALID_ZONE", "z(...) requires a distance");
|
||||
}
|
||||
return { kind: "distance", value: normalizedNumber(first) };
|
||||
return { kind: "distance", value: evaluateNumberExpression(first, { ...context, expectedKind: "length", defaultUnit: "mm" }) };
|
||||
}
|
||||
|
||||
if (isCallExpression(expression) && expression.callee === "cnt") {
|
||||
const first = expression.args[0];
|
||||
if (!first || first.kind !== "NumberLiteral") {
|
||||
if (!first) {
|
||||
throw compileError("GRL_INVALID_ZONE", "cnt(...) requires a percent value");
|
||||
}
|
||||
return { kind: "cnt", value: normalizedNumber(first) };
|
||||
return { kind: "cnt", value: evaluateNumberExpression(first, context) };
|
||||
}
|
||||
|
||||
throw compileError("GRL_INVALID_ZONE", "Unsupported zone expression");
|
||||
}
|
||||
|
||||
export function compileOffsetExpression(expression: GrlOffsetExpression): OffsetSpec {
|
||||
export function compileOffsetExpression(
|
||||
expression: GrlOffsetExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): OffsetSpec {
|
||||
const xyz: [number, number, number] = [0, 0, 0];
|
||||
for (const axis of expression.axes) {
|
||||
const index = axis.axis === "x" ? 0 : axis.axis === "y" ? 1 : 2;
|
||||
xyz[index] = normalizedNumber(axis.value);
|
||||
xyz[index] = evaluateNumberExpression(axis.value, { ...context, expectedKind: "length", defaultUnit: "mm" });
|
||||
}
|
||||
|
||||
return {
|
||||
@@ -150,37 +191,39 @@ export function compileOffsetExpression(expression: GrlOffsetExpression): Offset
|
||||
};
|
||||
}
|
||||
|
||||
function compileByType(typeName: string, expression: GrlExpression): CompiledGrlDataValue {
|
||||
function compileByType(
|
||||
typeName: string,
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): CompiledGrlDataValue {
|
||||
if (typeName === "speed") {
|
||||
return compileSpeedExpression(expression);
|
||||
return compileSpeedExpression(expression, context);
|
||||
}
|
||||
if (typeName === "zone") {
|
||||
return compileZoneExpression(expression);
|
||||
return compileZoneExpression(expression, context);
|
||||
}
|
||||
if (typeName === "pose") {
|
||||
return compilePoseExpression(expression);
|
||||
return compilePoseExpression(expression, context);
|
||||
}
|
||||
if (typeName === "pose_target" || typeName === "joint_target") {
|
||||
return compileTargetExpression(expression);
|
||||
return compileTargetExpression(expression, context);
|
||||
}
|
||||
if (expression.kind === "OffsetExpression") {
|
||||
return compileOffsetExpression(expression);
|
||||
return compileOffsetExpression(expression, context);
|
||||
}
|
||||
if (typeName === "tool" && isObjectExpression(expression, "tool")) {
|
||||
return {
|
||||
tcp: compilePoseExpression(requiredProperty(expression, "tcp")),
|
||||
...(findProperty(expression, "mass") ? { mass: normalizedNumber(findProperty(expression, "mass") as GrlNumberLiteral) } : {}),
|
||||
...(findProperty(expression, "cog") ? { cog: compileNumberArray(findProperty(expression, "cog")!) } : {})
|
||||
tcp: compilePoseExpression(requiredProperty(expression, "tcp"), context),
|
||||
...(findProperty(expression, "mass") ? { mass: evaluateNumberExpression(findProperty(expression, "mass")!, { ...context, expectedKind: "mass", defaultUnit: "kg" }) } : {}),
|
||||
...(findProperty(expression, "cog") ? { cog: compileNumberArray(findProperty(expression, "cog")!, { ...context, expectedKind: "length", defaultUnit: "mm" }) } : {})
|
||||
};
|
||||
}
|
||||
if (typeName === "frame" && isObjectExpression(expression, "frame")) {
|
||||
return {
|
||||
origin: compilePoseExpression(requiredProperty(expression, "origin"))
|
||||
origin: compilePoseExpression(requiredProperty(expression, "origin"), context)
|
||||
};
|
||||
}
|
||||
if (expression.kind === "NumberLiteral") {
|
||||
return normalizedNumber(expression);
|
||||
}
|
||||
if (isConstantScalarExpression(expression)) return evaluateConstantExpression(expression, context).value as number | boolean | string;
|
||||
if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") {
|
||||
return expression.value;
|
||||
}
|
||||
@@ -191,46 +234,51 @@ function compileByType(typeName: string, expression: GrlExpression): CompiledGrl
|
||||
return { kind: expression.kind };
|
||||
}
|
||||
|
||||
function compilePoseExpression(expression: GrlExpression): Pose {
|
||||
function compilePoseExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): Pose {
|
||||
if (!isCallExpression(expression) || (expression.callee !== "pose" && expression.callee !== "poseq")) {
|
||||
throw compileError("GRL_INVALID_POSE", "Expected pose(...) or poseq(...) expression");
|
||||
}
|
||||
|
||||
const values = expression.args.map((arg) => {
|
||||
if (arg.kind !== "NumberLiteral") {
|
||||
throw compileError("GRL_INVALID_POSE", "Pose arguments must be numeric");
|
||||
}
|
||||
return normalizedNumber(arg);
|
||||
});
|
||||
|
||||
if (expression.callee === "pose") {
|
||||
if (values.length !== 6) {
|
||||
if (expression.args.length !== 6) {
|
||||
throw compileError("GRL_INVALID_POSE", "pose(...) requires 6 arguments");
|
||||
}
|
||||
const values = expression.args.map((arg, index) =>
|
||||
evaluateNumberExpression(arg, {
|
||||
...context,
|
||||
expectedKind: index < 3 ? "length" : "angle",
|
||||
defaultUnit: index < 3 ? "mm" : "deg"
|
||||
})
|
||||
);
|
||||
return {
|
||||
position: [values[0]!, values[1]!, values[2]!],
|
||||
quaternion: rpyToQuaternion([values[3]!, values[4]!, values[5]!])
|
||||
};
|
||||
}
|
||||
|
||||
if (values.length !== 7) {
|
||||
if (expression.args.length !== 7) {
|
||||
throw compileError("GRL_INVALID_POSE", "poseq(...) requires 7 arguments");
|
||||
}
|
||||
const values = expression.args.map((arg, index) =>
|
||||
index < 3
|
||||
? evaluateNumberExpression(arg, { ...context, expectedKind: "length", defaultUnit: "mm" })
|
||||
: evaluateNumberExpression(arg, context)
|
||||
);
|
||||
return {
|
||||
position: [values[0]!, values[1]!, values[2]!],
|
||||
quaternion: [values[3]!, values[4]!, values[5]!, values[6]!]
|
||||
};
|
||||
}
|
||||
|
||||
function compileRobotConfig(expression: GrlExpression) {
|
||||
function compileRobotConfig(expression: GrlExpression, context: ConstantEvaluationContext = {}) {
|
||||
if (!isCallExpression(expression) || expression.callee !== "robot_config") {
|
||||
throw compileError("GRL_INVALID_CONFIG", "Expected robot_config(...)");
|
||||
}
|
||||
const values = expression.args.map((arg) => {
|
||||
if (arg.kind !== "NumberLiteral") {
|
||||
throw compileError("GRL_INVALID_CONFIG", "robot_config arguments must be numeric");
|
||||
}
|
||||
return arg.value as -1 | 0 | 1;
|
||||
return evaluateNumberExpression(arg, context) as -1 | 0 | 1;
|
||||
});
|
||||
return {
|
||||
...(values[0] !== undefined ? { shoulder: values[0] } : {}),
|
||||
@@ -239,33 +287,30 @@ function compileRobotConfig(expression: GrlExpression) {
|
||||
};
|
||||
}
|
||||
|
||||
function compileNumberArray(expression: GrlExpression): number[] {
|
||||
function compileNumberArray(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): number[] {
|
||||
if (expression.kind !== "ArrayExpression") {
|
||||
throw compileError("GRL_INVALID_ARRAY", "Expected numeric array");
|
||||
}
|
||||
return expression.elements.map((element) => {
|
||||
if (element.kind !== "NumberLiteral") {
|
||||
throw compileError("GRL_INVALID_ARRAY", "Array elements must be numeric");
|
||||
}
|
||||
return normalizedNumber(element);
|
||||
});
|
||||
return expression.elements.map((element) => evaluateNumberExpression(element, context));
|
||||
}
|
||||
|
||||
function compileAcceleration(expression: GrlCallExpression): { acceleration?: number } {
|
||||
function compileAcceleration(
|
||||
expression: GrlCallExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): { acceleration?: number } {
|
||||
for (let index = 1; index < expression.args.length; index += 1) {
|
||||
const marker = expression.args[index];
|
||||
const value = expression.args[index + 1];
|
||||
if (marker?.kind === "IdentifierExpression" && marker.name === "acc" && value?.kind === "NumberLiteral") {
|
||||
return { acceleration: normalizedNumber(value) };
|
||||
if (marker?.kind === "IdentifierExpression" && marker.name === "acc" && value) {
|
||||
return { acceleration: evaluateNumberExpression(value, { ...context, expectedKind: "linear_acceleration", defaultUnit: "mm/s2" }) };
|
||||
}
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
function normalizedNumber(expression: GrlNumberLiteral): number {
|
||||
return expression.unit?.normalizedValue ?? expression.value;
|
||||
}
|
||||
|
||||
function requiredProperty(expression: GrlObjectExpression, key: string): GrlExpression {
|
||||
const property = findProperty(expression, key);
|
||||
if (!property) {
|
||||
@@ -291,6 +336,19 @@ function isCallExpression(expression: GrlExpression): expression is GrlCallExpre
|
||||
return expression.kind === "CallExpression";
|
||||
}
|
||||
|
||||
function isConstantScalarExpression(expression: GrlExpression): boolean {
|
||||
return [
|
||||
"NumberLiteral",
|
||||
"StringLiteral",
|
||||
"BooleanLiteral",
|
||||
"UnaryExpression",
|
||||
"BinaryExpression",
|
||||
"CallExpression",
|
||||
"IdentifierExpression",
|
||||
"UnitExpression"
|
||||
].includes(expression.kind);
|
||||
}
|
||||
|
||||
function compileError(code: string, message: string): KdlStructuredError {
|
||||
return new KdlStructuredError(code, message);
|
||||
}
|
||||
|
||||
@@ -10,6 +10,9 @@ import type {
|
||||
WaitInstruction
|
||||
} from "../ir/index.js";
|
||||
import { lexGrl, type GrlToken } from "../lexer/index.js";
|
||||
import { parseGrlExpression } from "../parser/index.js";
|
||||
import { evaluateNumberExpression } from "./constantExpression.js";
|
||||
import type { GrlNumberLiteral } from "../ast/index.js";
|
||||
|
||||
export interface IoMap {
|
||||
aliases?: Record<string, IoReference>;
|
||||
@@ -185,7 +188,7 @@ class IoStatementParser {
|
||||
return token.raw === "true";
|
||||
}
|
||||
if (token.kind === "number") {
|
||||
return normalizedNumber(token);
|
||||
return evaluateNumberExpression(numberLiteralFromToken(token));
|
||||
}
|
||||
if (token.kind === "string") {
|
||||
return token.value;
|
||||
@@ -197,8 +200,40 @@ class IoStatementParser {
|
||||
}
|
||||
|
||||
private parseDuration(): number {
|
||||
const token = this.consumeNumber("Expected duration");
|
||||
return normalizedNumber(token);
|
||||
const tokens = this.collectDurationExpressionTokens();
|
||||
return evaluateNumberExpression(parseGrlExpression(tokens), { expectedKind: "time", defaultUnit: "s" });
|
||||
}
|
||||
|
||||
private collectDurationExpressionTokens(): GrlToken[] {
|
||||
const tokens: GrlToken[] = [];
|
||||
let parenDepth = 0;
|
||||
let bracketDepth = 0;
|
||||
while (!this.isAtEnd()) {
|
||||
const token = this.peek();
|
||||
if (
|
||||
tokens.length > 0 &&
|
||||
parenDepth === 0 &&
|
||||
bracketDepth === 0 &&
|
||||
(this.isKeywordLike(token, "on_timeout") || this.isCurrentStatementStartAfter(tokens))
|
||||
) {
|
||||
break;
|
||||
}
|
||||
const consumed = this.advance();
|
||||
tokens.push(consumed);
|
||||
if (consumed.kind === "punctuation" && consumed.raw === "(") {
|
||||
parenDepth += 1;
|
||||
} else if (consumed.kind === "punctuation" && consumed.raw === ")") {
|
||||
parenDepth = Math.max(0, parenDepth - 1);
|
||||
} else if (consumed.kind === "punctuation" && consumed.raw === "[") {
|
||||
bracketDepth += 1;
|
||||
} else if (consumed.kind === "punctuation" && consumed.raw === "]") {
|
||||
bracketDepth = Math.max(0, bracketDepth - 1);
|
||||
}
|
||||
}
|
||||
if (tokens.length === 0) {
|
||||
throw ioError("GRL_TOKEN_EXPECTED", "Expected duration");
|
||||
}
|
||||
return tokens;
|
||||
}
|
||||
|
||||
private collectUntilKeyword(keywords: string[]): GrlToken[] {
|
||||
@@ -242,20 +277,63 @@ class IoStatementParser {
|
||||
|
||||
private isCurrentStatementStartAfter(tokens: GrlToken[]): boolean {
|
||||
const token = this.peek();
|
||||
if (this.isKeywordLike(token, "wait") || this.isKeywordLike(token, "pulse")) {
|
||||
const previous = tokens.at(-1);
|
||||
if (!previous || token.range.start.line <= previous.range.end.line) {
|
||||
return false;
|
||||
}
|
||||
if (this.isStatementStart(token)) {
|
||||
return true;
|
||||
}
|
||||
if (!this.isIoStartAtCurrent()) {
|
||||
return false;
|
||||
}
|
||||
const previous = tokens.at(-1);
|
||||
return previous ? token.range.start.line > previous.range.end.line : true;
|
||||
return true;
|
||||
}
|
||||
|
||||
private isIoStartAtCurrent(): boolean {
|
||||
return this.peek().raw === "io" && this.maybePeek(1)?.raw === ".";
|
||||
}
|
||||
|
||||
private isStatementStart(token: GrlToken): boolean {
|
||||
return (
|
||||
(token.kind === "keyword" || token.kind === "identifier") &&
|
||||
[
|
||||
"io",
|
||||
"wait",
|
||||
"pulse",
|
||||
"movej",
|
||||
"movel",
|
||||
"movec",
|
||||
"set_tool",
|
||||
"set_frame",
|
||||
"set_speed",
|
||||
"set_zone",
|
||||
"run_path",
|
||||
"run_operation",
|
||||
"if",
|
||||
"elseif",
|
||||
"else",
|
||||
"while",
|
||||
"for",
|
||||
"switch",
|
||||
"case",
|
||||
"default",
|
||||
"break",
|
||||
"continue",
|
||||
"label",
|
||||
"jump",
|
||||
"call",
|
||||
"return",
|
||||
"alarm",
|
||||
"raise",
|
||||
"try",
|
||||
"catch",
|
||||
"finally",
|
||||
"end"
|
||||
].includes(token.raw)
|
||||
);
|
||||
}
|
||||
|
||||
private isKeywordLike(token: GrlToken, keyword: string): boolean {
|
||||
return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword;
|
||||
}
|
||||
@@ -361,13 +439,6 @@ function compileStatementString(
|
||||
return parseIoFlowStatements(tokens, ioMap);
|
||||
}
|
||||
|
||||
function normalizedNumber(token: GrlToken): number {
|
||||
if (token.kind !== "number") {
|
||||
throw ioError("GRL_NUMBER_EXPECTED", "Expected number");
|
||||
}
|
||||
return token.unit?.normalizedValue ?? token.value;
|
||||
}
|
||||
|
||||
function tokenSourceMap(token: GrlToken): MotionSourceMap {
|
||||
return {
|
||||
line: token.range.start.line,
|
||||
@@ -375,6 +446,25 @@ function tokenSourceMap(token: GrlToken): MotionSourceMap {
|
||||
};
|
||||
}
|
||||
|
||||
function numberLiteralFromToken(token: Extract<GrlToken, { kind: "number" }>): GrlNumberLiteral {
|
||||
return {
|
||||
kind: "NumberLiteral",
|
||||
value: token.value,
|
||||
raw: token.raw,
|
||||
...(token.unit
|
||||
? {
|
||||
unit: {
|
||||
raw: token.unit.raw,
|
||||
kind: token.unit.kind,
|
||||
siUnit: token.unit.siUnit,
|
||||
normalizedValue: token.unit.normalizedValue
|
||||
}
|
||||
}
|
||||
: {}),
|
||||
range: token.range
|
||||
};
|
||||
}
|
||||
|
||||
function ioError(code: string, message: string): KdlStructuredError {
|
||||
return new KdlStructuredError(code, message);
|
||||
}
|
||||
|
||||
@@ -41,6 +41,7 @@ import type {
|
||||
import type { GrlToken } from "../lexer/index.js";
|
||||
import {
|
||||
compileOffsetExpression,
|
||||
compileGrlConstantValue,
|
||||
compileGrlDataDeclaration,
|
||||
compileGrlTargetDeclaration,
|
||||
compileSpeedExpression,
|
||||
@@ -48,6 +49,7 @@ import {
|
||||
compileZoneExpression,
|
||||
type CompiledGrlDataValue
|
||||
} from "./compileData.js";
|
||||
import { constantToLiteralValue, evaluateNumberExpression, type ConstantValue } from "./constantExpression.js";
|
||||
import type { GrlDataDeclaration, GrlTargetDeclaration } from "../ast/index.js";
|
||||
|
||||
export interface GrlMotionContext {
|
||||
@@ -56,6 +58,7 @@ export interface GrlMotionContext {
|
||||
zones: Map<string, ZoneSpec>;
|
||||
tools: Map<string, Pose>;
|
||||
frames: Map<string, Pose>;
|
||||
constants: Map<string, ConstantValue>;
|
||||
currentSpeed?: SpeedSpec;
|
||||
currentZone?: ZoneSpec;
|
||||
currentTool?: Pose;
|
||||
@@ -85,7 +88,8 @@ export function buildMotionContext(declarations: Array<GrlDataDeclaration | GrlT
|
||||
speeds: new Map(),
|
||||
zones: new Map(),
|
||||
tools: new Map(),
|
||||
frames: new Map()
|
||||
frames: new Map(),
|
||||
constants: new Map()
|
||||
};
|
||||
|
||||
for (const declaration of declarations) {
|
||||
@@ -95,7 +99,14 @@ export function buildMotionContext(declarations: Array<GrlDataDeclaration | GrlT
|
||||
continue;
|
||||
}
|
||||
|
||||
const compiled = compileGrlDataDeclaration(declaration);
|
||||
if (declaration.storage === "const") {
|
||||
try {
|
||||
context.constants.set(declaration.name, compileGrlConstantValue(declaration.initializer));
|
||||
} catch {
|
||||
// Structured declarations such as speed/tool are added below.
|
||||
}
|
||||
}
|
||||
const compiled = compileGrlDataDeclaration(declaration, { symbols: context.constants });
|
||||
addCompiledData(context, compiled.name, compiled.typeName, compiled.value);
|
||||
}
|
||||
|
||||
@@ -485,7 +496,9 @@ class MotionStatementParser {
|
||||
this.advance();
|
||||
return this.context.speeds.get(token.raw)!;
|
||||
}
|
||||
return compileSpeedExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()));
|
||||
return compileSpeedExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()), {
|
||||
symbols: this.context.constants
|
||||
});
|
||||
}
|
||||
|
||||
private parseZoneArgument(): ZoneSpec {
|
||||
@@ -494,7 +507,9 @@ class MotionStatementParser {
|
||||
this.advance();
|
||||
return this.context.zones.get(token.raw)!;
|
||||
}
|
||||
return compileZoneExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()));
|
||||
return compileZoneExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()), {
|
||||
symbols: this.context.constants
|
||||
});
|
||||
}
|
||||
|
||||
private resolveTargetExpression(expression: GrlExpression): JointTarget | PoseTarget {
|
||||
@@ -503,7 +518,7 @@ class MotionStatementParser {
|
||||
if (!isPoseTarget(base)) {
|
||||
throw motionError("GRL_MOTION_TARGET_TYPE", "offset target requires PoseTarget");
|
||||
}
|
||||
return applyOffset(base, compileOffsetExpression(expression));
|
||||
return applyOffset(base, compileOffsetExpression(expression, { symbols: this.context.constants }));
|
||||
}
|
||||
if (expression.kind === "IdentifierExpression") {
|
||||
const target = this.context.targets.get(expression.name);
|
||||
@@ -512,7 +527,7 @@ class MotionStatementParser {
|
||||
}
|
||||
return target;
|
||||
}
|
||||
return compileTargetExpression(expression);
|
||||
return compileTargetExpression(expression, { symbols: this.context.constants });
|
||||
}
|
||||
|
||||
private withDefaults(instruction: Partial<MotionInstruction> & Pick<MotionInstruction, "kind">): MotionInstruction {
|
||||
@@ -858,7 +873,7 @@ function compilePathEvent(
|
||||
id: `event_${index}`,
|
||||
timing: event.timing,
|
||||
pointId: event.pointId,
|
||||
...(event.distance ? { distance: normalizedNumber(event.distance) } : {}),
|
||||
...(event.distance ? { distance: evaluateNumberExpression(event.distance, { expectedKind: "length", defaultUnit: "mm" }) } : {}),
|
||||
kind: event.actionTokens[0]?.raw ?? "statement",
|
||||
sourceMap: tokenSourceMap(event.actionTokens[0] ?? event.actionTokens[event.actionTokens.length - 1]!),
|
||||
data: {
|
||||
@@ -875,6 +890,7 @@ function cloneMotionContext(context: GrlMotionContext): GrlMotionContext {
|
||||
zones: context.zones,
|
||||
tools: context.tools,
|
||||
frames: context.frames,
|
||||
constants: context.constants,
|
||||
...(context.currentSpeed ? { currentSpeed: context.currentSpeed } : {}),
|
||||
...(context.currentZone ? { currentZone: context.currentZone } : {}),
|
||||
...(context.currentTool ? { currentTool: context.currentTool } : {}),
|
||||
@@ -901,14 +917,14 @@ function resolveSpeed(expression: GrlExpression, context: GrlMotionContext): Spe
|
||||
if (expression.kind === "IdentifierExpression" && context.speeds.has(expression.name)) {
|
||||
return context.speeds.get(expression.name)!;
|
||||
}
|
||||
return compileSpeedExpression(expression);
|
||||
return compileSpeedExpression(expression, { symbols: context.constants });
|
||||
}
|
||||
|
||||
function resolveZone(expression: GrlExpression, context: GrlMotionContext): ZoneSpec {
|
||||
if (expression.kind === "IdentifierExpression" && context.zones.has(expression.name)) {
|
||||
return context.zones.get(expression.name)!;
|
||||
}
|
||||
return compileZoneExpression(expression);
|
||||
return compileZoneExpression(expression, { symbols: context.constants });
|
||||
}
|
||||
|
||||
function resolveNamedPoseFromExpression(
|
||||
@@ -928,12 +944,17 @@ function resolveNamedPoseFromExpression(
|
||||
}
|
||||
|
||||
function compileLiteralValue(expression: GrlExpression): unknown {
|
||||
if (expression.kind === "NumberLiteral") {
|
||||
return normalizedNumber(expression);
|
||||
}
|
||||
if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") {
|
||||
return expression.value;
|
||||
}
|
||||
if (
|
||||
expression.kind === "NumberLiteral" ||
|
||||
expression.kind === "UnaryExpression" ||
|
||||
expression.kind === "BinaryExpression" ||
|
||||
expression.kind === "UnitExpression"
|
||||
) {
|
||||
return constantToLiteralValue(expression);
|
||||
}
|
||||
if (expression.kind === "IdentifierExpression") {
|
||||
return expression.name;
|
||||
}
|
||||
@@ -969,10 +990,6 @@ function targetIdOf(target: JointTarget | PoseTarget): string | undefined {
|
||||
return target.id;
|
||||
}
|
||||
|
||||
function normalizedNumber(expression: { value: number; unit?: { normalizedValue: number } }): number {
|
||||
return expression.unit?.normalizedValue ?? expression.value;
|
||||
}
|
||||
|
||||
function motionError(code: string, message: string): KdlStructuredError {
|
||||
return new KdlStructuredError(code, message);
|
||||
}
|
||||
|
||||
@@ -502,6 +502,7 @@ function cloneMotionContextForSemantic(context: ReturnType<typeof buildMotionCon
|
||||
zones: context.zones,
|
||||
tools: context.tools,
|
||||
frames: context.frames,
|
||||
constants: context.constants,
|
||||
...(context.currentSpeed ? { currentSpeed: context.currentSpeed } : {}),
|
||||
...(context.currentZone ? { currentZone: context.currentZone } : {}),
|
||||
...(context.currentTool ? { currentTool: context.currentTool } : {}),
|
||||
|
||||
501
kdl-wasm/web/src/grl/semantic/constantExpression.ts
Normal file
501
kdl-wasm/web/src/grl/semantic/constantExpression.ts
Normal file
@@ -0,0 +1,501 @@
|
||||
import { KdlStructuredError } from "../../kdl/rpc.js";
|
||||
import type { MotionSourceMap } from "../../kdl/types.js";
|
||||
import type {
|
||||
GrlBinaryExpression,
|
||||
GrlCallExpression,
|
||||
GrlExpression,
|
||||
GrlNumberLiteral,
|
||||
GrlUnaryExpression
|
||||
} from "../ast/index.js";
|
||||
import { normalizeUnitLiteral, type UnitKind } from "../lexer/units.js";
|
||||
|
||||
export type ConstantDimension = UnitKind | "scalar" | "boolean" | "string";
|
||||
|
||||
export interface ConstantValue {
|
||||
kind: ConstantDimension;
|
||||
value: number | boolean | string;
|
||||
rawUnit?: string;
|
||||
}
|
||||
|
||||
export interface ConstantEvaluationContext {
|
||||
symbols?: Map<string, ConstantValue>;
|
||||
defaultUnit?: string;
|
||||
expectedKind?: ConstantDimension;
|
||||
}
|
||||
|
||||
const EPSILON = 1e-12;
|
||||
|
||||
export function evaluateConstantExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): ConstantValue {
|
||||
const value = evaluate(expression, context);
|
||||
return applyExpectedContext(value, context, expression);
|
||||
}
|
||||
|
||||
export function evaluateNumberExpression(
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext = {}
|
||||
): number {
|
||||
const value = evaluateConstantExpression(expression, context);
|
||||
if (typeof value.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_NON_CONSTANT", "Expected numeric constant expression", expression);
|
||||
}
|
||||
return value.value;
|
||||
}
|
||||
|
||||
export function constantToLiteralValue(expression: GrlExpression): unknown {
|
||||
const value = evaluateConstantExpression(expression);
|
||||
return value.value;
|
||||
}
|
||||
|
||||
function evaluate(expression: GrlExpression, context: ConstantEvaluationContext): ConstantValue {
|
||||
switch (expression.kind) {
|
||||
case "NumberLiteral":
|
||||
return numberLiteralValue(expression, context);
|
||||
case "StringLiteral":
|
||||
return { kind: "string", value: expression.value };
|
||||
case "BooleanLiteral":
|
||||
return { kind: "boolean", value: expression.value };
|
||||
case "IdentifierExpression":
|
||||
return identifierValue(expression.name, expression, context);
|
||||
case "UnaryExpression":
|
||||
return unaryValue(expression, context);
|
||||
case "BinaryExpression":
|
||||
return binaryValue(expression, context);
|
||||
case "CallExpression":
|
||||
return callValue(expression, context);
|
||||
case "UnitExpression": {
|
||||
const innerContext: ConstantEvaluationContext = {};
|
||||
if (context.symbols) {
|
||||
innerContext.symbols = context.symbols;
|
||||
}
|
||||
const inner = evaluate(expression.expression, innerContext);
|
||||
if (typeof inner.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Unit suffix requires a numeric expression", expression);
|
||||
}
|
||||
const unit = normalizeUnitLiteral(expression.unit.raw);
|
||||
if (inner.kind !== "scalar" && inner.kind !== unit.kind) {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Cannot apply ${unit.literal} to ${inner.kind}`, expression);
|
||||
}
|
||||
return {
|
||||
kind: unit.kind,
|
||||
value: inner.value * unit.factor,
|
||||
rawUnit: unit.literal
|
||||
};
|
||||
}
|
||||
default:
|
||||
throw expressionError("GRL_EXPR_NON_CONSTANT", `${expression.kind} is not a constant expression`, expression);
|
||||
}
|
||||
}
|
||||
|
||||
function numberLiteralValue(expression: GrlNumberLiteral, context: ConstantEvaluationContext): ConstantValue {
|
||||
if (expression.unit) {
|
||||
return {
|
||||
kind: expression.unit.kind as UnitKind,
|
||||
value: expression.unit.normalizedValue,
|
||||
rawUnit: expression.unit.raw
|
||||
};
|
||||
}
|
||||
if (context.defaultUnit) {
|
||||
const unit = normalizeUnitLiteral(context.defaultUnit);
|
||||
return {
|
||||
kind: unit.kind,
|
||||
value: expression.value * unit.factor,
|
||||
rawUnit: unit.literal
|
||||
};
|
||||
}
|
||||
return { kind: "scalar", value: expression.value };
|
||||
}
|
||||
|
||||
function identifierValue(
|
||||
name: string,
|
||||
expression: GrlExpression,
|
||||
context: ConstantEvaluationContext
|
||||
): ConstantValue {
|
||||
if (name === "pi") {
|
||||
return { kind: "scalar", value: Math.PI };
|
||||
}
|
||||
if (name === "e") {
|
||||
return { kind: "scalar", value: Math.E };
|
||||
}
|
||||
const value = context.symbols?.get(name);
|
||||
if (value) {
|
||||
return value;
|
||||
}
|
||||
throw expressionError("GRL_EXPR_UNKNOWN_SYMBOL", `Unknown constant ${name}`, expression);
|
||||
}
|
||||
|
||||
function unaryValue(expression: GrlUnaryExpression, context: ConstantEvaluationContext): ConstantValue {
|
||||
const value = evaluate(expression.argument, context);
|
||||
if (expression.operator === "not" || expression.operator === "!") {
|
||||
return { kind: "boolean", value: !coerceBoolean(value, expression) };
|
||||
}
|
||||
if (typeof value.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Unary ${expression.operator} requires a number`, expression);
|
||||
}
|
||||
return {
|
||||
...value,
|
||||
value: expression.operator === "-" ? -value.value : value.value
|
||||
};
|
||||
}
|
||||
|
||||
function binaryValue(expression: GrlBinaryExpression, context: ConstantEvaluationContext): ConstantValue {
|
||||
if (expression.operator === "and" || expression.operator === "&&") {
|
||||
return {
|
||||
kind: "boolean",
|
||||
value: coerceBoolean(evaluate(expression.left, context), expression.left) &&
|
||||
coerceBoolean(evaluate(expression.right, context), expression.right)
|
||||
};
|
||||
}
|
||||
if (expression.operator === "or" || expression.operator === "||") {
|
||||
return {
|
||||
kind: "boolean",
|
||||
value: coerceBoolean(evaluate(expression.left, context), expression.left) ||
|
||||
coerceBoolean(evaluate(expression.right, context), expression.right)
|
||||
};
|
||||
}
|
||||
|
||||
let left = evaluate(expression.left, contextWithoutDefaultUnit(context));
|
||||
let right = evaluate(expression.right, contextWithoutDefaultUnit(context));
|
||||
|
||||
if (["==", "!=", "<", "<=", ">", ">="].includes(expression.operator)) {
|
||||
[left, right] = normalizeScalarUnits(left, right, context, expression);
|
||||
return compareValues(left, right, expression);
|
||||
}
|
||||
|
||||
if (typeof left.value !== "number" || typeof right.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `${expression.operator} requires numeric operands`, expression);
|
||||
}
|
||||
|
||||
if (expression.operator === "*" || expression.operator === "/") {
|
||||
return multiplyDivide(left, right, expression);
|
||||
}
|
||||
|
||||
if (expression.operator === "mod") {
|
||||
[left, right] = normalizeScalarUnits(left, right, context, expression);
|
||||
ensureNumericCompatible(left, right, expression);
|
||||
const leftNumber = left.value as number;
|
||||
const rightNumber = right.value as number;
|
||||
if (Math.abs(rightNumber) < EPSILON) {
|
||||
throw expressionError("GRL_EXPR_DIV_ZERO", "Modulo by zero", expression);
|
||||
}
|
||||
return withRawUnit(left.kind, leftNumber % rightNumber, left.rawUnit);
|
||||
}
|
||||
|
||||
[left, right] = normalizeScalarUnits(left, right, context, expression);
|
||||
ensureNumericCompatible(left, right, expression);
|
||||
const leftNumber = left.value as number;
|
||||
const rightNumber = right.value as number;
|
||||
return withRawUnit(left.kind, expression.operator === "+" ? leftNumber + rightNumber : leftNumber - rightNumber, left.rawUnit ?? right.rawUnit);
|
||||
}
|
||||
|
||||
function contextWithoutDefaultUnit(context: ConstantEvaluationContext): ConstantEvaluationContext {
|
||||
const next: ConstantEvaluationContext = {};
|
||||
if (context.symbols) {
|
||||
next.symbols = context.symbols;
|
||||
}
|
||||
return next;
|
||||
}
|
||||
|
||||
function contextWithoutExpectedKind(context: ConstantEvaluationContext): ConstantEvaluationContext {
|
||||
const next: ConstantEvaluationContext = {};
|
||||
if (context.symbols) {
|
||||
next.symbols = context.symbols;
|
||||
}
|
||||
if (context.defaultUnit) {
|
||||
next.defaultUnit = context.defaultUnit;
|
||||
}
|
||||
return next;
|
||||
}
|
||||
|
||||
function normalizeScalarUnits(
|
||||
left: ConstantValue,
|
||||
right: ConstantValue,
|
||||
context: ConstantEvaluationContext,
|
||||
expression: GrlExpression
|
||||
): [ConstantValue, ConstantValue] {
|
||||
if (typeof left.value !== "number" || typeof right.value !== "number") {
|
||||
return [left, right];
|
||||
}
|
||||
if (left.kind === "scalar" && right.kind !== "scalar") {
|
||||
return [coerceScalarToReference(left, right, context, expression), right];
|
||||
}
|
||||
if (right.kind === "scalar" && left.kind !== "scalar") {
|
||||
return [left, coerceScalarToReference(right, left, context, expression)];
|
||||
}
|
||||
return [left, right];
|
||||
}
|
||||
|
||||
function coerceScalarToReference(
|
||||
scalarValue: ConstantValue,
|
||||
reference: ConstantValue,
|
||||
context: ConstantEvaluationContext,
|
||||
expression: GrlExpression
|
||||
): ConstantValue {
|
||||
if (typeof scalarValue.value !== "number") {
|
||||
return scalarValue;
|
||||
}
|
||||
const unitLiteral = reference.rawUnit ?? context.defaultUnit;
|
||||
if (!unitLiteral) {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Cannot infer unit for scalar ${scalarValue.value}`, expression);
|
||||
}
|
||||
const unit = normalizeUnitLiteral(unitLiteral);
|
||||
if (unit.kind !== reference.kind) {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Cannot use ${unit.literal} with ${reference.kind}`, expression);
|
||||
}
|
||||
return {
|
||||
kind: unit.kind,
|
||||
value: scalarValue.value * unit.factor,
|
||||
rawUnit: unit.literal
|
||||
};
|
||||
}
|
||||
|
||||
function multiplyDivide(
|
||||
left: ConstantValue,
|
||||
right: ConstantValue,
|
||||
expression: GrlBinaryExpression
|
||||
): ConstantValue {
|
||||
if (expression.operator === "/" && Math.abs(right.value as number) < EPSILON) {
|
||||
throw expressionError("GRL_EXPR_DIV_ZERO", "Division by zero", expression);
|
||||
}
|
||||
if (left.kind === "scalar" && right.kind === "scalar") {
|
||||
return {
|
||||
kind: "scalar",
|
||||
value: expression.operator === "*" ? (left.value as number) * (right.value as number) : (left.value as number) / (right.value as number)
|
||||
};
|
||||
}
|
||||
if (left.kind !== "scalar" && right.kind === "scalar") {
|
||||
return withRawUnit(
|
||||
left.kind,
|
||||
expression.operator === "*" ? (left.value as number) * (right.value as number) : (left.value as number) / (right.value as number),
|
||||
left.rawUnit
|
||||
);
|
||||
}
|
||||
if (left.kind === "scalar" && right.kind !== "scalar" && expression.operator === "*") {
|
||||
return withRawUnit(right.kind, (left.value as number) * (right.value as number), right.rawUnit);
|
||||
}
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Compound unit arithmetic is not supported", expression);
|
||||
}
|
||||
|
||||
function compareValues(
|
||||
left: ConstantValue,
|
||||
right: ConstantValue,
|
||||
expression: GrlBinaryExpression
|
||||
): ConstantValue {
|
||||
if (expression.operator === "==" || expression.operator === "!=") {
|
||||
if (left.kind !== right.kind) {
|
||||
return { kind: "boolean", value: expression.operator === "!=" };
|
||||
}
|
||||
return {
|
||||
kind: "boolean",
|
||||
value: expression.operator === "==" ? left.value === right.value : left.value !== right.value
|
||||
};
|
||||
}
|
||||
|
||||
ensureNumericCompatible(left, right, expression);
|
||||
const leftNumber = left.value as number;
|
||||
const rightNumber = right.value as number;
|
||||
switch (expression.operator) {
|
||||
case "<":
|
||||
return { kind: "boolean", value: leftNumber < rightNumber };
|
||||
case "<=":
|
||||
return { kind: "boolean", value: leftNumber <= rightNumber };
|
||||
case ">":
|
||||
return { kind: "boolean", value: leftNumber > rightNumber };
|
||||
case ">=":
|
||||
return { kind: "boolean", value: leftNumber >= rightNumber };
|
||||
default:
|
||||
throw expressionError("GRL_EXPR_UNKNOWN_FUNCTION", `Unsupported comparison ${expression.operator}`, expression);
|
||||
}
|
||||
}
|
||||
|
||||
function callValue(expression: GrlCallExpression, context: ConstantEvaluationContext): ConstantValue {
|
||||
const args = expression.args.map((arg) => evaluate(arg, context));
|
||||
switch (expression.callee) {
|
||||
case "sin":
|
||||
arity(expression, args, 1);
|
||||
return scalar(Math.sin(angleArg(args[0]!, expression)));
|
||||
case "cos":
|
||||
arity(expression, args, 1);
|
||||
return scalar(Math.cos(angleArg(args[0]!, expression)));
|
||||
case "tan":
|
||||
arity(expression, args, 1);
|
||||
return scalar(Math.tan(angleArg(args[0]!, expression)));
|
||||
case "asin":
|
||||
arity(expression, args, 1);
|
||||
return angle(inverseTrig(Math.asin, args[0]!, expression));
|
||||
case "acos":
|
||||
arity(expression, args, 1);
|
||||
return angle(inverseTrig(Math.acos, args[0]!, expression));
|
||||
case "atan":
|
||||
arity(expression, args, 1);
|
||||
return angle(Math.atan(numberArg(args[0]!, expression)));
|
||||
case "atan2":
|
||||
arity(expression, args, 2);
|
||||
return angle(Math.atan2(numberArg(args[0]!, expression), numberArg(args[1]!, expression)));
|
||||
case "sqrt":
|
||||
arity(expression, args, 1);
|
||||
return sameKindUnary(args[0]!, Math.sqrt(numberArg(args[0]!, expression, { min: 0 })));
|
||||
case "abs":
|
||||
arity(expression, args, 1);
|
||||
return sameKindUnary(args[0]!, Math.abs(numberArg(args[0]!, expression)));
|
||||
case "pow":
|
||||
arity(expression, args, 2);
|
||||
if (args[0]!.kind !== "scalar" && args[1]!.kind !== "scalar") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "pow exponent must be scalar", expression);
|
||||
}
|
||||
return sameKindUnary(args[0]!, Math.pow(numberArg(args[0]!, expression), numberArg(args[1]!, expression)));
|
||||
case "min":
|
||||
arityAtLeast(expression, args, 1);
|
||||
return minMax(args, expression, "min");
|
||||
case "max":
|
||||
arityAtLeast(expression, args, 1);
|
||||
return minMax(args, expression, "max");
|
||||
case "clamp":
|
||||
arity(expression, args, 3);
|
||||
ensureNumericCompatible(args[0]!, args[1]!, expression);
|
||||
ensureNumericCompatible(args[0]!, args[2]!, expression);
|
||||
return withRawUnit(
|
||||
args[0]!.kind,
|
||||
Math.min(Math.max(args[0]!.value as number, args[1]!.value as number), args[2]!.value as number),
|
||||
args[0]!.rawUnit
|
||||
);
|
||||
case "floor":
|
||||
arity(expression, args, 1);
|
||||
return sameKindUnary(args[0]!, Math.floor(numberArg(args[0]!, expression)));
|
||||
case "ceil":
|
||||
arity(expression, args, 1);
|
||||
return sameKindUnary(args[0]!, Math.ceil(numberArg(args[0]!, expression)));
|
||||
case "round":
|
||||
arity(expression, args, 1);
|
||||
return sameKindUnary(args[0]!, Math.round(numberArg(args[0]!, expression)));
|
||||
default:
|
||||
throw expressionError("GRL_EXPR_UNKNOWN_FUNCTION", `Unknown function ${expression.callee}`, expression);
|
||||
}
|
||||
}
|
||||
|
||||
function minMax(args: ConstantValue[], expression: GrlExpression, mode: "min" | "max"): ConstantValue {
|
||||
const first = args[0]!;
|
||||
for (const arg of args.slice(1)) {
|
||||
ensureNumericCompatible(first, arg, expression);
|
||||
}
|
||||
const values = args.map((arg) => numberArg(arg, expression));
|
||||
return withRawUnit(first.kind, mode === "min" ? Math.min(...values) : Math.max(...values), first.rawUnit);
|
||||
}
|
||||
|
||||
function applyExpectedContext(
|
||||
value: ConstantValue,
|
||||
context: ConstantEvaluationContext,
|
||||
expression: GrlExpression
|
||||
): ConstantValue {
|
||||
if (!context.expectedKind || context.expectedKind === value.kind) {
|
||||
return value;
|
||||
}
|
||||
if (context.expectedKind !== "scalar" && value.kind === "scalar" && context.defaultUnit) {
|
||||
const unit = normalizeUnitLiteral(context.defaultUnit);
|
||||
if (unit.kind !== context.expectedKind) {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Default unit ${unit.literal} does not match ${context.expectedKind}`, expression);
|
||||
}
|
||||
return {
|
||||
kind: unit.kind,
|
||||
value: (value.value as number) * unit.factor,
|
||||
rawUnit: unit.literal
|
||||
};
|
||||
}
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Expected ${context.expectedKind}, got ${value.kind}`, expression);
|
||||
}
|
||||
|
||||
function ensureNumericCompatible(left: ConstantValue, right: ConstantValue, expression: GrlExpression): void {
|
||||
if (typeof left.value !== "number" || typeof right.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Expected numeric operands", expression);
|
||||
}
|
||||
if (left.kind === right.kind) {
|
||||
return;
|
||||
}
|
||||
if (left.kind === "scalar" || right.kind === "scalar") {
|
||||
return;
|
||||
}
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Cannot combine ${left.kind} and ${right.kind}`, expression);
|
||||
}
|
||||
|
||||
function numberArg(
|
||||
value: ConstantValue,
|
||||
expression: GrlExpression,
|
||||
options: { min?: number; max?: number } = {}
|
||||
): number {
|
||||
if (typeof value.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Expected numeric argument", expression);
|
||||
}
|
||||
if (options.min !== undefined && value.value < options.min) {
|
||||
throw expressionError("GRL_EXPR_DOMAIN", "Function argument is below domain", expression);
|
||||
}
|
||||
if (options.max !== undefined && value.value > options.max) {
|
||||
throw expressionError("GRL_EXPR_DOMAIN", "Function argument is above domain", expression);
|
||||
}
|
||||
return value.value;
|
||||
}
|
||||
|
||||
function angleArg(value: ConstantValue, expression: GrlExpression): number {
|
||||
if (typeof value.value !== "number") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Expected angle argument", expression);
|
||||
}
|
||||
if (value.kind !== "angle" && value.kind !== "scalar") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", `Expected angle, got ${value.kind}`, expression);
|
||||
}
|
||||
return value.value;
|
||||
}
|
||||
|
||||
function inverseTrig(fn: (value: number) => number, value: ConstantValue, expression: GrlExpression): number {
|
||||
return fn(numberArg(value, expression, { min: -1, max: 1 }));
|
||||
}
|
||||
|
||||
function coerceBoolean(value: ConstantValue, expression: GrlExpression): boolean {
|
||||
if (typeof value.value !== "boolean") {
|
||||
throw expressionError("GRL_EXPR_UNIT_MISMATCH", "Expected boolean expression", expression);
|
||||
}
|
||||
return value.value;
|
||||
}
|
||||
|
||||
function arity(expression: GrlCallExpression, args: ConstantValue[], expected: number): void {
|
||||
if (args.length !== expected) {
|
||||
throw expressionError("GRL_EXPR_ARITY", `${expression.callee} expects ${expected} argument(s)`, expression);
|
||||
}
|
||||
}
|
||||
|
||||
function arityAtLeast(expression: GrlCallExpression, args: ConstantValue[], minimum: number): void {
|
||||
if (args.length < minimum) {
|
||||
throw expressionError("GRL_EXPR_ARITY", `${expression.callee} expects at least ${minimum} argument(s)`, expression);
|
||||
}
|
||||
}
|
||||
|
||||
function scalar(value: number): ConstantValue {
|
||||
return { kind: "scalar", value };
|
||||
}
|
||||
|
||||
function angle(value: number): ConstantValue {
|
||||
return { kind: "angle", value, rawUnit: "rad" };
|
||||
}
|
||||
|
||||
function sameKindUnary(base: ConstantValue, value: number): ConstantValue {
|
||||
return withRawUnit(base.kind, value, base.rawUnit);
|
||||
}
|
||||
|
||||
function withRawUnit(kind: ConstantDimension, value: number, rawUnit?: string): ConstantValue {
|
||||
return rawUnit ? { kind, value, rawUnit } : { kind, value };
|
||||
}
|
||||
|
||||
function expressionError(code: string, message: string, expression: GrlExpression): KdlStructuredError {
|
||||
const sourceMap: MotionSourceMap = {
|
||||
line: expression.range.start.line,
|
||||
column: expression.range.start.column
|
||||
};
|
||||
return new KdlStructuredError(code, message, [
|
||||
{
|
||||
severity: "error",
|
||||
code,
|
||||
message,
|
||||
sourceMap
|
||||
}
|
||||
]);
|
||||
}
|
||||
502
kdl-wasm/web/src/importers/brandImport.ts
Normal file
502
kdl-wasm/web/src/importers/brandImport.ts
Normal file
@@ -0,0 +1,502 @@
|
||||
import { parseGrl } from "../grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../grl/semantic/index.js";
|
||||
import type { ExecutableInstruction, SemanticProgramIr } from "../grl/ir/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import {
|
||||
applyPatch,
|
||||
createOlpProject,
|
||||
pathOperationToGrl,
|
||||
type JsonPatchOperation,
|
||||
type OlpOperation,
|
||||
type OlpPath,
|
||||
type OlpPathPoint,
|
||||
type OlpProgram,
|
||||
type OlpProjectModel,
|
||||
type OlpSpeed,
|
||||
type OlpTarget,
|
||||
type OlpZone,
|
||||
type Pose6D
|
||||
} from "../olp/index.js";
|
||||
import {
|
||||
createValidationReport,
|
||||
type ValidationReport
|
||||
} from "../reports/index.js";
|
||||
|
||||
export type ImportBrand = "abb" | "kuka" | "fanuc";
|
||||
|
||||
export interface BrandProgramFile {
|
||||
name: string;
|
||||
text: string;
|
||||
}
|
||||
|
||||
export interface BrandImportOptions {
|
||||
projectId?: string;
|
||||
projectName?: string;
|
||||
operationKind?: string;
|
||||
generatedAt?: string;
|
||||
}
|
||||
|
||||
export interface BrandImportReportItem {
|
||||
code: string;
|
||||
severity: "info" | "warning" | "error";
|
||||
message: string;
|
||||
file?: string;
|
||||
line?: number;
|
||||
}
|
||||
|
||||
export interface BrandImportResult {
|
||||
brand: ImportBrand;
|
||||
project: OlpProjectModel;
|
||||
patch: JsonPatchOperation[];
|
||||
grl: string;
|
||||
program: OlpProgram;
|
||||
ir: SemanticProgramIr;
|
||||
report: ValidationReport;
|
||||
importReport: BrandImportReportItem[];
|
||||
parsed: {
|
||||
targets: OlpTarget[];
|
||||
path: OlpPath;
|
||||
operation: OlpOperation;
|
||||
};
|
||||
}
|
||||
|
||||
interface ParsedBrandProgram {
|
||||
targets: OlpTarget[];
|
||||
points: OlpPathPoint[];
|
||||
notes: BrandImportReportItem[];
|
||||
}
|
||||
|
||||
const DEFAULT_SPEEDS: OlpSpeed[] = [
|
||||
{ id: "import_vj", name: "import_vj", kind: "joint_percent", value: 50 },
|
||||
{ id: "import_vl", name: "import_vl", kind: "linear_mm_s", value: 200 }
|
||||
];
|
||||
|
||||
const DEFAULT_ZONES: OlpZone[] = [
|
||||
{ id: "import_fine", name: "import_fine", kind: "fine" },
|
||||
{ id: "import_z10", name: "import_z10", kind: "distance_mm", value: 10 }
|
||||
];
|
||||
|
||||
export function importBrandProgram(
|
||||
brand: ImportBrand,
|
||||
files: BrandProgramFile[],
|
||||
options: BrandImportOptions = {}
|
||||
): BrandImportResult {
|
||||
const parsed = parseByBrand(brand, files);
|
||||
const projectId = options.projectId ?? `${brand}_import`;
|
||||
const projectName = options.projectName ?? `${brand.toUpperCase()}Import`;
|
||||
const baseProject = createOlpProject({ id: projectId, name: projectName });
|
||||
const path: OlpPath = {
|
||||
id: `${brand}_path`,
|
||||
name: `${brand}_path`,
|
||||
source: {
|
||||
type: "brand_import",
|
||||
brand,
|
||||
files: files.map((file) => file.name).join(",")
|
||||
},
|
||||
defaults: {
|
||||
speedId: "import_vl",
|
||||
zoneId: "import_z10"
|
||||
},
|
||||
points: parsed.points
|
||||
};
|
||||
const operation: OlpOperation = {
|
||||
id: `${brand}_operation`,
|
||||
name: `${brand}_operation`,
|
||||
kind: options.operationKind ?? "imported_program",
|
||||
pathId: path.id,
|
||||
process: {
|
||||
import_brand: brand,
|
||||
source_files: files.map((file) => file.name)
|
||||
}
|
||||
};
|
||||
const patch = buildImportPatch(DEFAULT_SPEEDS, DEFAULT_ZONES, parsed.targets, path, operation);
|
||||
const project = applyPatch(baseProject, patch);
|
||||
const generated = pathOperationToGrl(project, operation.id, {
|
||||
moduleName: projectName.replace(/[^A-Za-z0-9_]/g, "_")
|
||||
});
|
||||
project.programs.push(generated.program);
|
||||
const compiledIr = compileSemanticProgram(parseGrl(generated.text), {
|
||||
startJoints: firstJointTarget(parsed.targets)?.joints ?? [0, 0, 0, 0, 0, 0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
const ir = expandRunInstructionsForPost(compiledIr);
|
||||
const report = createValidationReport({
|
||||
id: `${brand}_import_report`,
|
||||
project,
|
||||
...(options.generatedAt ? { generatedAt: options.generatedAt } : {}),
|
||||
importDiagnostics: toImportDiagnostics(brand, parsed.targets, parsed.points, parsed.notes)
|
||||
});
|
||||
|
||||
return {
|
||||
brand,
|
||||
project,
|
||||
patch,
|
||||
grl: generated.text,
|
||||
program: generated.program,
|
||||
ir,
|
||||
report,
|
||||
importReport: parsed.notes,
|
||||
parsed: {
|
||||
targets: parsed.targets,
|
||||
path,
|
||||
operation
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
export function parseAbbRapid(files: BrandProgramFile[]): ParsedBrandProgram {
|
||||
const source = joinFiles(files);
|
||||
const targets: OlpTarget[] = [];
|
||||
const targetByRaw = new Map<string, OlpTarget>();
|
||||
const notes: BrandImportReportItem[] = [];
|
||||
|
||||
for (const match of source.text.matchAll(/\b(?:CONST|PERS|VAR)\s+robtarget\s+([A-Za-z_][A-Za-z0-9_]*)\s*:?=\s*\[\[\s*([-+0-9.Ee]+)\s*,\s*([-+0-9.Ee]+)\s*,\s*([-+0-9.Ee]+)\s*\]\s*,/g)) {
|
||||
const raw = match[1]!;
|
||||
const target = poseTarget(raw, [num(match[2]), num(match[3]), num(match[4]), 0, 0, 0]);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
for (const match of source.text.matchAll(/\b(?:CONST|PERS|VAR)\s+jointtarget\s+([A-Za-z_][A-Za-z0-9_]*)\s*:?=\s*\[\[\s*([^\]]+)\]/g)) {
|
||||
const raw = match[1]!;
|
||||
const joints = match[2]!.split(",").slice(0, 6).map((value) => num(value));
|
||||
const target = jointTarget(raw, joints);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
|
||||
const points: OlpPathPoint[] = [];
|
||||
for (const line of source.lines) {
|
||||
const move = line.text.match(/\b(MoveJ|MoveL|MoveC)\s+([^,;]+)\s*,\s*([^,;]+)?/i);
|
||||
if (!move) continue;
|
||||
const motionRaw = move[1]!.toLowerCase();
|
||||
if (motionRaw === "movec") {
|
||||
const circular = line.text.match(/\bMoveC\s+([^,;]+)\s*,\s*([^,;]+)/i);
|
||||
if (!circular) continue;
|
||||
const via = resolveTarget(targets, targetByRaw, circular[1]!, notes, line);
|
||||
const target = resolveTarget(targets, targetByRaw, circular[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, "movec", target.id, { viaTargetId: via.id, sourceLine: line.number }));
|
||||
continue;
|
||||
}
|
||||
const target = resolveTarget(targets, targetByRaw, move[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, motionRaw === "movej" ? "movej" : "movel", target.id, { sourceLine: line.number }));
|
||||
}
|
||||
|
||||
addFeatureNotes(notes, source, "abb");
|
||||
return { targets, points, notes };
|
||||
}
|
||||
|
||||
export function parseKukaKrl(files: BrandProgramFile[]): ParsedBrandProgram {
|
||||
const source = joinFiles(files);
|
||||
const targets: OlpTarget[] = [];
|
||||
const targetByRaw = new Map<string, OlpTarget>();
|
||||
const notes: BrandImportReportItem[] = [];
|
||||
|
||||
for (const match of source.text.matchAll(/\b(?:DECL\s+)?E6POS\s+([A-Za-z_][A-Za-z0-9_]*)\s*=\s*\{([^}]+)\}/gi)) {
|
||||
const raw = match[1]!;
|
||||
const body = match[2]!;
|
||||
const target = poseTarget(raw, [
|
||||
field(body, "X"),
|
||||
field(body, "Y"),
|
||||
field(body, "Z"),
|
||||
field(body, "A"),
|
||||
field(body, "B"),
|
||||
field(body, "C")
|
||||
]);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
for (const match of source.text.matchAll(/\b(?:DECL\s+)?E6AXIS\s+([A-Za-z_][A-Za-z0-9_]*)\s*=\s*\{([^}]+)\}/gi)) {
|
||||
const raw = match[1]!;
|
||||
const body = match[2]!;
|
||||
const target = jointTarget(raw, ["A1", "A2", "A3", "A4", "A5", "A6"].map((axis) => field(body, axis)));
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
|
||||
const points: OlpPathPoint[] = [];
|
||||
for (const line of source.lines) {
|
||||
const move = line.text.match(/^\s*(PTP|LIN|CIRC)\s+([^ ;,]+)(?:\s*,\s*([^ ;,]+))?/i);
|
||||
if (!move) continue;
|
||||
const motion = move[1]!.toUpperCase();
|
||||
if (motion === "CIRC") {
|
||||
const via = resolveTarget(targets, targetByRaw, move[2]!, notes, line);
|
||||
const target = resolveTarget(targets, targetByRaw, move[3] ?? move[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, "movec", target.id, { viaTargetId: via.id, sourceLine: line.number }));
|
||||
continue;
|
||||
}
|
||||
const target = resolveTarget(targets, targetByRaw, move[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, motion === "PTP" ? "movej" : "movel", target.id, { sourceLine: line.number }));
|
||||
}
|
||||
|
||||
addFeatureNotes(notes, source, "kuka");
|
||||
return { targets, points, notes };
|
||||
}
|
||||
|
||||
export function parseFanucLs(files: BrandProgramFile[]): ParsedBrandProgram {
|
||||
const source = joinFiles(files);
|
||||
const targets: OlpTarget[] = [];
|
||||
const targetByRaw = new Map<string, OlpTarget>();
|
||||
const notes: BrandImportReportItem[] = [];
|
||||
|
||||
for (const match of source.text.matchAll(/\bP\[(\d+)\]\s*\{([\s\S]*?)\};/g)) {
|
||||
const raw = `P[${match[1]!}]`;
|
||||
const body = match[2]!;
|
||||
const target = poseTarget(`P${match[1]!}`, [
|
||||
fanucField(body, "X"),
|
||||
fanucField(body, "Y"),
|
||||
fanucField(body, "Z"),
|
||||
fanucField(body, "W"),
|
||||
fanucField(body, "P"),
|
||||
fanucField(body, "R")
|
||||
]);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
for (const match of source.text.matchAll(/\bPR\[(\d+)\]\s*\{([\s\S]*?)\};/g)) {
|
||||
const raw = `PR[${match[1]!}]`;
|
||||
const body = match[2]!;
|
||||
const target = poseTarget(`PR${match[1]!}`, [
|
||||
fanucField(body, "X"),
|
||||
fanucField(body, "Y"),
|
||||
fanucField(body, "Z"),
|
||||
fanucField(body, "W"),
|
||||
fanucField(body, "P"),
|
||||
fanucField(body, "R")
|
||||
]);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
}
|
||||
|
||||
const points: OlpPathPoint[] = [];
|
||||
for (const line of source.lines) {
|
||||
const move = line.text.match(/^\s*\d+:\s*(J|L|C)\s+((?:P|PR)\[\d+\])(?:\s+((?:P|PR)\[\d+\]))?/i);
|
||||
if (!move) continue;
|
||||
const motion = move[1]!.toUpperCase();
|
||||
if (motion === "C") {
|
||||
const via = resolveTarget(targets, targetByRaw, move[2]!, notes, line);
|
||||
const target = resolveTarget(targets, targetByRaw, move[3] ?? move[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, "movec", target.id, { viaTargetId: via.id, sourceLine: line.number }));
|
||||
continue;
|
||||
}
|
||||
const target = resolveTarget(targets, targetByRaw, move[2]!, notes, line);
|
||||
points.push(pathPoint(points.length, motion === "J" ? "movej" : "movel", target.id, { sourceLine: line.number }));
|
||||
}
|
||||
|
||||
addFeatureNotes(notes, source, "fanuc");
|
||||
return { targets, points, notes };
|
||||
}
|
||||
|
||||
function parseByBrand(brand: ImportBrand, files: BrandProgramFile[]): ParsedBrandProgram {
|
||||
const parsed = brand === "abb" ? parseAbbRapid(files) : brand === "kuka" ? parseKukaKrl(files) : parseFanucLs(files);
|
||||
if (parsed.points.length === 0) {
|
||||
parsed.notes.push({
|
||||
code: "IMPORT_NO_MOTION",
|
||||
severity: "error",
|
||||
message: `${brand} import did not find supported motion statements`
|
||||
});
|
||||
throw new Error(`${brand} import did not find supported motion statements`);
|
||||
}
|
||||
return parsed;
|
||||
}
|
||||
|
||||
function buildImportPatch(
|
||||
speeds: OlpSpeed[],
|
||||
zones: OlpZone[],
|
||||
targets: OlpTarget[],
|
||||
path: OlpPath,
|
||||
operation: OlpOperation
|
||||
): JsonPatchOperation[] {
|
||||
return [
|
||||
...speeds.map((speed) => ({ op: "add" as const, path: "/speeds/-", value: speed })),
|
||||
...zones.map((zone) => ({ op: "add" as const, path: "/zones/-", value: zone })),
|
||||
...targets.map((target) => ({ op: "add" as const, path: "/targets/-", value: target })),
|
||||
{ op: "add", path: "/paths/-", value: path },
|
||||
{ op: "add", path: "/operations/-", value: operation }
|
||||
];
|
||||
}
|
||||
|
||||
function toImportDiagnostics(
|
||||
brand: ImportBrand,
|
||||
targets: OlpTarget[],
|
||||
points: OlpPathPoint[],
|
||||
notes: BrandImportReportItem[]
|
||||
): MotionDiagnostic[] {
|
||||
const diagnostics: MotionDiagnostic[] = [
|
||||
{
|
||||
severity: targets.length > 0 ? "info" : "warning",
|
||||
code: "IMPORT_TARGETS",
|
||||
message: `${brand} import mapped ${targets.length} targets`
|
||||
},
|
||||
{
|
||||
severity: points.length > 0 ? "info" : "error",
|
||||
code: "IMPORT_MOTIONS",
|
||||
message: `${brand} import mapped ${points.length} motion points`
|
||||
}
|
||||
];
|
||||
for (const note of notes) {
|
||||
diagnostics.push({
|
||||
severity: note.severity,
|
||||
code: note.code,
|
||||
message: note.message,
|
||||
...(note.line || note.file ? { sourceMap: { ...(note.file ? { file: note.file } : {}), ...(note.line ? { line: note.line } : {}) } } : {})
|
||||
});
|
||||
}
|
||||
return diagnostics;
|
||||
}
|
||||
|
||||
function expandRunInstructionsForPost(ir: SemanticProgramIr): SemanticProgramIr {
|
||||
return {
|
||||
...ir,
|
||||
procedures: ir.procedures.map((procedure) => ({
|
||||
...procedure,
|
||||
instructions: procedure.instructions.flatMap((instruction): ExecutableInstruction[] => {
|
||||
if (instruction.kind === "RUN_PATH") {
|
||||
return ir.paths.find((path) => path.pathId === instruction.pathId)?.motions ?? [instruction];
|
||||
}
|
||||
if (instruction.kind === "RUN_OPERATION") {
|
||||
const operation = ir.operations.find((item) => item.operationId === instruction.operationId);
|
||||
const motions = operation ? ir.paths.find((path) => path.pathId === operation.pathId)?.motions : undefined;
|
||||
return motions ?? [instruction];
|
||||
}
|
||||
return [instruction];
|
||||
})
|
||||
}))
|
||||
};
|
||||
}
|
||||
|
||||
function pathPoint(
|
||||
index: number,
|
||||
motion: "movej" | "movel" | "movec",
|
||||
targetId: string,
|
||||
extra: { viaTargetId?: string; sourceLine?: number } = {}
|
||||
): OlpPathPoint {
|
||||
return {
|
||||
id: `import_p${String(index).padStart(2, "0")}`,
|
||||
name: `import_p${String(index).padStart(2, "0")}`,
|
||||
motion,
|
||||
targetId,
|
||||
...(extra.viaTargetId ? { viaTargetId: extra.viaTargetId } : {}),
|
||||
speedId: motion === "movej" ? "import_vj" : "import_vl",
|
||||
zoneId: "import_z10",
|
||||
...(extra.sourceLine ? { metadata: { sourceLine: extra.sourceLine } } : {})
|
||||
};
|
||||
}
|
||||
|
||||
function poseTarget(rawName: string, pose: Pose6D): OlpTarget {
|
||||
const name = sanitizeIdentifier(rawName);
|
||||
return {
|
||||
id: `target_${name}`,
|
||||
name,
|
||||
kind: "pose",
|
||||
pose
|
||||
};
|
||||
}
|
||||
|
||||
function jointTarget(rawName: string, joints: number[]): OlpTarget {
|
||||
const name = sanitizeIdentifier(rawName);
|
||||
return {
|
||||
id: `target_${name}`,
|
||||
name,
|
||||
kind: "joint",
|
||||
joints
|
||||
};
|
||||
}
|
||||
|
||||
function addTarget(targets: OlpTarget[], targetByRaw: Map<string, OlpTarget>, rawName: string, target: OlpTarget): void {
|
||||
if (!targets.some((item) => item.id === target.id)) {
|
||||
targets.push(target);
|
||||
}
|
||||
targetByRaw.set(rawName.trim().toUpperCase(), target);
|
||||
targetByRaw.set(target.name.toUpperCase(), target);
|
||||
}
|
||||
|
||||
function resolveTarget(
|
||||
targets: OlpTarget[],
|
||||
targetByRaw: Map<string, OlpTarget>,
|
||||
rawToken: string,
|
||||
notes: BrandImportReportItem[],
|
||||
line: { file: string; number: number }
|
||||
): OlpTarget {
|
||||
const raw = rawToken.trim().replace(/[;]/g, "");
|
||||
const existing = targetByRaw.get(raw.toUpperCase());
|
||||
if (existing) {
|
||||
return existing;
|
||||
}
|
||||
const target = poseTarget(raw, [0, 0, 0, 0, 0, 0]);
|
||||
addTarget(targets, targetByRaw, raw, target);
|
||||
notes.push({
|
||||
code: "IMPORT_TARGET_STUB",
|
||||
severity: "warning",
|
||||
message: `Target ${raw} was referenced by motion but had no parsed position; a zero pose stub was created`,
|
||||
file: line.file,
|
||||
line: line.number
|
||||
});
|
||||
return target;
|
||||
}
|
||||
|
||||
function addFeatureNotes(notes: BrandImportReportItem[], source: JoinedFiles, brand: ImportBrand): void {
|
||||
const tool = brand === "abb" ? /\btooldata\b|\btool0\b/i : brand === "kuka" ? /\$TOOL\b/i : /\bUTOOL_NUM\b/i;
|
||||
const frame = brand === "abb" ? /\bwobjdata\b|\bwobj0\b/i : brand === "kuka" ? /\$BASE\b/i : /\bUFRAME_NUM\b/i;
|
||||
if (tool.test(source.text)) {
|
||||
notes.push({
|
||||
code: "IMPORT_TOOL_DETECTED",
|
||||
severity: "info",
|
||||
message: `${brand} tool selection detected and recorded as import metadata`
|
||||
});
|
||||
}
|
||||
if (frame.test(source.text)) {
|
||||
notes.push({
|
||||
code: "IMPORT_FRAME_DETECTED",
|
||||
severity: "info",
|
||||
message: `${brand} frame/base selection detected and recorded as import metadata`
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
function field(body: string, name: string): number {
|
||||
const match = body.match(new RegExp(`\\b${name}\\s+([-+0-9.Ee]+)`, "i"));
|
||||
return match ? num(match[1]) : 0;
|
||||
}
|
||||
|
||||
function fanucField(body: string, name: string): number {
|
||||
const match = body.match(new RegExp(`\\b${name}\\s*=\\s*([-+0-9.Ee]+)`, "i"));
|
||||
return match ? num(match[1]) : 0;
|
||||
}
|
||||
|
||||
function num(value: string | undefined): number {
|
||||
const parsed = Number(value?.trim());
|
||||
return Number.isFinite(parsed) ? parsed : 0;
|
||||
}
|
||||
|
||||
function firstJointTarget(targets: OlpTarget[]): OlpTarget | undefined {
|
||||
return targets.find((target) => target.kind === "joint" && target.joints);
|
||||
}
|
||||
|
||||
function sanitizeIdentifier(value: string): string {
|
||||
const normalized = value
|
||||
.trim()
|
||||
.replace(/\[(\d+)\]/g, "$1")
|
||||
.replace(/[^A-Za-z0-9_]/g, "_");
|
||||
if (/^[A-Za-z_]/.test(normalized)) {
|
||||
return normalized;
|
||||
}
|
||||
return `T_${normalized}`;
|
||||
}
|
||||
|
||||
interface JoinedFiles {
|
||||
text: string;
|
||||
lines: Array<{ file: string; number: number; text: string }>;
|
||||
}
|
||||
|
||||
function joinFiles(files: BrandProgramFile[]): JoinedFiles {
|
||||
const lines: JoinedFiles["lines"] = [];
|
||||
const textParts: string[] = [];
|
||||
for (const file of files) {
|
||||
textParts.push(file.text);
|
||||
file.text.split(/\r?\n/).forEach((text, index) => {
|
||||
lines.push({
|
||||
file: file.name,
|
||||
number: index + 1,
|
||||
text
|
||||
});
|
||||
});
|
||||
}
|
||||
return {
|
||||
text: textParts.join("\n"),
|
||||
lines
|
||||
};
|
||||
}
|
||||
209
kdl-wasm/web/src/importers/brandImporter.ts
Normal file
209
kdl-wasm/web/src/importers/brandImporter.ts
Normal file
@@ -0,0 +1,209 @@
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import { pathOperationToGrl, type JsonPatchOperation, type OlpPathPoint, type OlpProjectModel, type OlpTarget } from "../olp/index.js";
|
||||
import { applyPatch } from "../olp/index.js";
|
||||
|
||||
export type ImportBrand = "abb" | "kuka" | "fanuc";
|
||||
|
||||
export interface BrandImportResult {
|
||||
brand: ImportBrand;
|
||||
patch: JsonPatchOperation[];
|
||||
model: OlpProjectModel;
|
||||
grl: string;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
report: {
|
||||
status: "pass" | "warn" | "fail";
|
||||
targets: number;
|
||||
motions: number;
|
||||
approximations: string[];
|
||||
};
|
||||
}
|
||||
|
||||
interface ParsedMotion {
|
||||
motion: "movej" | "movel" | "movec";
|
||||
targetName: string;
|
||||
viaName?: string;
|
||||
sourceLine: number;
|
||||
}
|
||||
|
||||
export function importBrandProgram(
|
||||
brand: ImportBrand,
|
||||
text: string,
|
||||
baseModel: OlpProjectModel,
|
||||
options: { operationId?: string; pathId?: string; robotId?: string } = {}
|
||||
): BrandImportResult {
|
||||
const diagnostics: MotionDiagnostic[] = [];
|
||||
const targets = parseTargets(brand, text, diagnostics);
|
||||
const motions = parseMotions(brand, text, diagnostics);
|
||||
const pathId = options.pathId ?? `${brand}_import_path`;
|
||||
const operationId = options.operationId ?? `${brand}_import_op`;
|
||||
const targetPatch = targets.map((target): JsonPatchOperation => ({ op: "add", path: "/targets/-", value: target }));
|
||||
const pathPoints = motions.map((motion, index): OlpPathPoint => ({
|
||||
id: `p${String(index).padStart(2, "0")}`,
|
||||
name: `p${String(index).padStart(2, "0")}`,
|
||||
motion: motion.motion,
|
||||
targetId: targetId(motion.targetName),
|
||||
...(motion.viaName ? { viaTargetId: targetId(motion.viaName) } : {})
|
||||
}));
|
||||
const patch: JsonPatchOperation[] = [
|
||||
...targetPatch,
|
||||
{
|
||||
op: "add",
|
||||
path: "/paths/-",
|
||||
value: {
|
||||
id: pathId,
|
||||
name: pathId,
|
||||
source: { type: "brand_import", brand },
|
||||
points: pathPoints
|
||||
}
|
||||
},
|
||||
{
|
||||
op: "add",
|
||||
path: "/operations/-",
|
||||
value: {
|
||||
id: operationId,
|
||||
name: operationId,
|
||||
kind: "imported",
|
||||
pathId,
|
||||
...(options.robotId ? { robotId: options.robotId } : {})
|
||||
}
|
||||
}
|
||||
];
|
||||
const model = applyPatch(baseModel, patch);
|
||||
const grlResult = pathOperationToGrl(model, operationId, {
|
||||
moduleName: `${brand}_Imported`,
|
||||
style: "expanded"
|
||||
});
|
||||
model.programs.push({
|
||||
id: `${operationId}_source`,
|
||||
name: `${brand}_source`,
|
||||
language: brand === "abb" ? "abb_rapid" : brand === "kuka" ? "kuka_krl" : "fanuc_ls",
|
||||
entryOperationIds: [operationId],
|
||||
source: {
|
||||
kind: "imported",
|
||||
text
|
||||
}
|
||||
});
|
||||
model.programs.push(grlResult.program);
|
||||
if (motions.length === 0) {
|
||||
diagnostics.push({
|
||||
severity: "error",
|
||||
code: "BRAND_IMPORT_NO_MOTIONS",
|
||||
message: `No motions were found in ${brand} program`
|
||||
});
|
||||
}
|
||||
|
||||
return {
|
||||
brand,
|
||||
patch,
|
||||
model,
|
||||
grl: grlResult.text,
|
||||
diagnostics,
|
||||
report: {
|
||||
status: diagnostics.some((diagnostic) => diagnostic.severity === "error")
|
||||
? "fail"
|
||||
: diagnostics.some((diagnostic) => diagnostic.severity === "warning")
|
||||
? "warn"
|
||||
: "pass",
|
||||
targets: targets.length,
|
||||
motions: motions.length,
|
||||
approximations: diagnostics
|
||||
.filter((diagnostic) => diagnostic.code.includes("APPROX"))
|
||||
.map((diagnostic) => diagnostic.message)
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
function parseTargets(brand: ImportBrand, text: string, diagnostics: MotionDiagnostic[]): OlpTarget[] {
|
||||
if (brand === "abb") {
|
||||
return [...text.matchAll(/CONST\s+robtarget\s+(\w+)\s*:=\s*\[\[([^\]]+)\]/gi)]
|
||||
.map((match) => makePoseTarget(match[1]!, match[2]!));
|
||||
}
|
||||
if (brand === "kuka") {
|
||||
return [...text.matchAll(/DECL\s+E6POS\s+(\w+)\s*=\s*\{([^}]+)\}/gi)]
|
||||
.map((match) => makePoseTarget(match[1]!, kukaPoseValues(match[2]!)));
|
||||
}
|
||||
const fanucTargets = [...text.matchAll(/P\[(\d+)\]\s*\{([^}]+)\}/gi)]
|
||||
.map((match) => makePoseTarget(`P${match[1]!}`, fanucPoseValues(match[2]!)));
|
||||
if (fanucTargets.length === 0) {
|
||||
diagnostics.push({
|
||||
severity: "warning",
|
||||
code: "BRAND_IMPORT_APPROX_FANUC_POSITIONS",
|
||||
message: "FANUC LS position block was not found; referenced positions use zero pose placeholders"
|
||||
});
|
||||
}
|
||||
return fanucTargets;
|
||||
}
|
||||
|
||||
function parseMotions(brand: ImportBrand, text: string, diagnostics: MotionDiagnostic[]): ParsedMotion[] {
|
||||
const lines = text.split(/\r?\n/);
|
||||
return lines.flatMap((line, index): ParsedMotion[] => {
|
||||
const sourceLine = index + 1;
|
||||
if (brand === "abb") {
|
||||
const move = line.match(/\b(MoveJ|MoveL|MoveC)\s+([^,;\s]+)(?:\s*,\s*([^,;\s]+))?/i);
|
||||
if (!move) return [];
|
||||
const kind = move[1]!.toLowerCase();
|
||||
if (kind === "movec") {
|
||||
return [{ motion: "movec", viaName: move[2]!, targetName: move[3] ?? move[2]!, sourceLine }];
|
||||
}
|
||||
return [{ motion: kind === "movej" ? "movej" : "movel", targetName: move[2]!, sourceLine }];
|
||||
}
|
||||
if (brand === "kuka") {
|
||||
const move = line.match(/\b(PTP|LIN|CIRC)\s+(\w+)(?:\s*,\s*(\w+))?/i);
|
||||
if (!move) return [];
|
||||
if (move[1]!.toUpperCase() === "CIRC") {
|
||||
return [{ motion: "movec", viaName: move[2]!, targetName: move[3] ?? move[2]!, sourceLine }];
|
||||
}
|
||||
return [{ motion: move[1]!.toUpperCase() === "PTP" ? "movej" : "movel", targetName: move[2]!, sourceLine }];
|
||||
}
|
||||
const move = line.match(/\b([JLC])\s+P\[(\d+)\](?:\s+P\[(\d+)\])?/i);
|
||||
if (!move) return [];
|
||||
if (move[1]!.toUpperCase() === "C") {
|
||||
return [{ motion: "movec", viaName: `P${move[2]!}`, targetName: `P${move[3] ?? move[2]}`, sourceLine }];
|
||||
}
|
||||
return [{ motion: move[1]!.toUpperCase() === "J" ? "movej" : "movel", targetName: `P${move[2]!}`, sourceLine }];
|
||||
}).map((motion) => {
|
||||
if (motion.motion === "movec" && !motion.viaName) {
|
||||
diagnostics.push({
|
||||
severity: "warning",
|
||||
code: "BRAND_IMPORT_APPROX_MOVEC",
|
||||
message: `MoveC at line ${motion.sourceLine} has no explicit via target`
|
||||
});
|
||||
}
|
||||
return motion;
|
||||
});
|
||||
}
|
||||
|
||||
function makePoseTarget(name: string, values: string | number[]): OlpTarget {
|
||||
const pose = Array.isArray(values)
|
||||
? values
|
||||
: values.split(",").map((value) => Number(value.trim())).filter((value) => Number.isFinite(value));
|
||||
const padded = [...pose, 0, 0, 0, 0, 0, 0].slice(0, 6) as [number, number, number, number, number, number];
|
||||
return {
|
||||
id: targetId(name),
|
||||
name: sanitizeName(name),
|
||||
kind: "pose",
|
||||
pose: padded
|
||||
};
|
||||
}
|
||||
|
||||
function targetId(name: string): string {
|
||||
return `import_${sanitizeName(name)}`;
|
||||
}
|
||||
|
||||
function sanitizeName(name: string): string {
|
||||
const sanitized = name.replace(/[^A-Za-z0-9_]/g, "_");
|
||||
return /^[A-Za-z_]/.test(sanitized) ? sanitized : `P${sanitized}`;
|
||||
}
|
||||
|
||||
function kukaPoseValues(raw: string): number[] {
|
||||
return ["X", "Y", "Z", "A", "B", "C"].map((key) => field(raw, key));
|
||||
}
|
||||
|
||||
function fanucPoseValues(raw: string): number[] {
|
||||
return ["X", "Y", "Z", "W", "P", "R"].map((key) => field(raw, key));
|
||||
}
|
||||
|
||||
function field(raw: string, key: string): number {
|
||||
const match = raw.match(new RegExp(`${key}\\s*[= ]\\s*(-?\\d+(?:\\.\\d+)?)`, "i"));
|
||||
return match ? Number(match[1]) : 0;
|
||||
}
|
||||
55
kdl-wasm/web/src/importers/index.ts
Normal file
55
kdl-wasm/web/src/importers/index.ts
Normal file
@@ -0,0 +1,55 @@
|
||||
import {
|
||||
importBrandProgram as importBrandProgramMvp,
|
||||
parseAbbRapid,
|
||||
parseFanucLs,
|
||||
parseKukaKrl,
|
||||
type BrandImportOptions,
|
||||
type BrandImportResult,
|
||||
type BrandProgramFile,
|
||||
type ImportBrand
|
||||
} from "./brandImport.js";
|
||||
import {
|
||||
importBrandProgram as importLegacyBrandProgram,
|
||||
type BrandImportResult as LegacyBrandImportResult
|
||||
} from "./brandImporter.js";
|
||||
import type { OlpProjectModel } from "../olp/index.js";
|
||||
|
||||
export function importBrandProgram(
|
||||
brand: ImportBrand,
|
||||
files: BrandProgramFile[],
|
||||
options?: BrandImportOptions
|
||||
): BrandImportResult;
|
||||
export function importBrandProgram(
|
||||
brand: ImportBrand,
|
||||
text: string,
|
||||
baseModel: OlpProjectModel,
|
||||
options?: { operationId?: string; pathId?: string; robotId?: string }
|
||||
): LegacyBrandImportResult;
|
||||
export function importBrandProgram(
|
||||
brand: ImportBrand,
|
||||
filesOrText: BrandProgramFile[] | string,
|
||||
optionsOrBaseModel?: BrandImportOptions | OlpProjectModel,
|
||||
legacyOptions?: { operationId?: string; pathId?: string; robotId?: string }
|
||||
): BrandImportResult | LegacyBrandImportResult {
|
||||
if (typeof filesOrText === "string") {
|
||||
return importLegacyBrandProgram(
|
||||
brand,
|
||||
filesOrText,
|
||||
optionsOrBaseModel as OlpProjectModel,
|
||||
legacyOptions
|
||||
);
|
||||
}
|
||||
return importBrandProgramMvp(brand, filesOrText, optionsOrBaseModel as BrandImportOptions | undefined);
|
||||
}
|
||||
|
||||
export {
|
||||
importLegacyBrandProgram,
|
||||
parseAbbRapid,
|
||||
parseFanucLs,
|
||||
parseKukaKrl,
|
||||
type BrandImportOptions,
|
||||
type BrandImportResult,
|
||||
type BrandProgramFile,
|
||||
type ImportBrand,
|
||||
type LegacyBrandImportResult
|
||||
};
|
||||
67
kdl-wasm/web/src/olp/calibration.ts
Normal file
67
kdl-wasm/web/src/olp/calibration.ts
Normal file
@@ -0,0 +1,67 @@
|
||||
import type { OlpCalibrationRecord, OlpProjectModel, Pose6D } from "./model.js";
|
||||
|
||||
export interface CalibrationApplyResult {
|
||||
model: OlpProjectModel;
|
||||
applied: string[];
|
||||
}
|
||||
|
||||
export function applyCalibration(model: OlpProjectModel, calibrationId: string): CalibrationApplyResult {
|
||||
const calibration = model.calibrations.find((candidate) => candidate.id === calibrationId);
|
||||
if (!calibration) {
|
||||
throw new Error(`Calibration ${calibrationId} not found`);
|
||||
}
|
||||
const next = clone(model);
|
||||
const applied: string[] = [];
|
||||
applyToTools(next, calibration, applied);
|
||||
applyToFrames(next, calibration, applied);
|
||||
applyToExternalAxes(next, calibration, applied);
|
||||
if (applied.length === 0) {
|
||||
throw new Error(`Calibration ${calibrationId} did not match a supported resource`);
|
||||
}
|
||||
return { model: next, applied };
|
||||
}
|
||||
|
||||
function applyToTools(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
|
||||
const tool = model.resources.tools.find((candidate) => candidate.id === calibration.targetResourceId);
|
||||
if (!tool || !calibration.poseDelta) {
|
||||
return;
|
||||
}
|
||||
tool.tcp = addPose(tool.tcp, calibration.poseDelta);
|
||||
applied.push(tool.id);
|
||||
}
|
||||
|
||||
function applyToFrames(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
|
||||
const frame = model.resources.frames.find((candidate) => candidate.id === calibration.targetResourceId);
|
||||
if (!frame || !calibration.poseDelta) {
|
||||
return;
|
||||
}
|
||||
frame.pose = addPose(frame.pose, calibration.poseDelta);
|
||||
applied.push(frame.id);
|
||||
}
|
||||
|
||||
function applyToExternalAxes(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
|
||||
const axis = model.externalAxes.find((candidate) => candidate.id === calibration.targetResourceId);
|
||||
if (!axis || calibration.axisOffset === undefined) {
|
||||
return;
|
||||
}
|
||||
axis.metadata = {
|
||||
...(axis.metadata ?? {}),
|
||||
calibrationOffset: calibration.axisOffset
|
||||
};
|
||||
applied.push(axis.id);
|
||||
}
|
||||
|
||||
function addPose(left: Pose6D, right: Pose6D): Pose6D {
|
||||
return [
|
||||
left[0] + right[0],
|
||||
left[1] + right[1],
|
||||
left[2] + right[2],
|
||||
left[3] + right[3],
|
||||
left[4] + right[4],
|
||||
left[5] + right[5]
|
||||
];
|
||||
}
|
||||
|
||||
function clone<T>(value: T): T {
|
||||
return JSON.parse(JSON.stringify(value)) as T;
|
||||
}
|
||||
374
kdl-wasm/web/src/olp/geometry.ts
Normal file
374
kdl-wasm/web/src/olp/geometry.ts
Normal file
@@ -0,0 +1,374 @@
|
||||
import type {
|
||||
JsonObject,
|
||||
OlpCalibrationRecord,
|
||||
OlpCollisionObject,
|
||||
OlpPath,
|
||||
OlpPathPoint,
|
||||
OlpProcessTemplate,
|
||||
OlpProjectModel,
|
||||
OlpTarget,
|
||||
Pose6D,
|
||||
Vec3
|
||||
} from "./model.js";
|
||||
|
||||
export type GeometrySource =
|
||||
| { kind: "points"; points: Vec3[] }
|
||||
| { kind: "edge"; start: Vec3; end: Vec3; samples: number }
|
||||
| { kind: "polyline"; points: Vec3[]; spacing?: number }
|
||||
| { kind: "curve"; controlPoints: Vec3[]; samples: number };
|
||||
|
||||
export interface GeometryPathOptions {
|
||||
pathId: string;
|
||||
targetPrefix?: string;
|
||||
motion?: "movej" | "movel";
|
||||
speedId?: string;
|
||||
zoneId?: string;
|
||||
toolId?: string;
|
||||
frameId?: string;
|
||||
zOffset?: number;
|
||||
}
|
||||
|
||||
export interface GeometryPathResult {
|
||||
targets: OlpTarget[];
|
||||
path: OlpPath;
|
||||
}
|
||||
|
||||
export interface CollisionSample {
|
||||
time: number;
|
||||
pointId: string;
|
||||
position: Vec3;
|
||||
}
|
||||
|
||||
export interface CollisionHit {
|
||||
objectId: string;
|
||||
pointId: string;
|
||||
time: number;
|
||||
distance: number;
|
||||
}
|
||||
|
||||
export interface CollisionReport {
|
||||
ok: boolean;
|
||||
hits: CollisionHit[];
|
||||
}
|
||||
|
||||
export type CollisionCheckResult = CollisionReport;
|
||||
|
||||
export interface ControllerCheckSample {
|
||||
pointId: string;
|
||||
offline: Pose6D;
|
||||
measured: Pose6D;
|
||||
speedPercent: number;
|
||||
}
|
||||
|
||||
export interface ControllerCheckReport {
|
||||
status: "pass" | "warn" | "fail";
|
||||
speedPercent: number;
|
||||
maxPositionErrorMm: number;
|
||||
maxOrientationErrorDeg: number;
|
||||
samples: Array<ControllerCheckSample & {
|
||||
positionErrorMm: number;
|
||||
orientationErrorDeg: number;
|
||||
}>;
|
||||
boundaries: string[];
|
||||
}
|
||||
|
||||
export interface CommercialExtensionBoundary {
|
||||
feature: string;
|
||||
mvp: boolean;
|
||||
boundary: string;
|
||||
}
|
||||
|
||||
export function generatePathFromGeometry(source: GeometrySource, options: GeometryPathOptions): GeometryPathResult {
|
||||
const targetPrefix = options.targetPrefix ?? options.pathId;
|
||||
const points = sampleGeometry(source).map((point): Vec3 => [
|
||||
point[0],
|
||||
point[1],
|
||||
point[2] + (options.zOffset ?? 0)
|
||||
]);
|
||||
const targets = points.map((point, index): OlpTarget => {
|
||||
const name = `${targetPrefix}_${String(index).padStart(2, "0")}`;
|
||||
return {
|
||||
id: `target_${name}`,
|
||||
name,
|
||||
kind: "pose",
|
||||
pose: [point[0], point[1], point[2], 0, 0, 0],
|
||||
...(options.toolId ? { toolId: options.toolId } : {}),
|
||||
...(options.frameId ? { frameId: options.frameId } : {})
|
||||
};
|
||||
});
|
||||
const pathPoints = targets.map((target, index): OlpPathPoint => ({
|
||||
id: `${options.pathId}_p${String(index).padStart(2, "0")}`,
|
||||
name: `p${String(index).padStart(2, "0")}`,
|
||||
motion: index === 0 ? (options.motion ?? "movej") : "movel",
|
||||
targetId: target.id,
|
||||
...(options.speedId ? { speedId: options.speedId } : {}),
|
||||
...(options.zoneId ? { zoneId: options.zoneId } : {}),
|
||||
...(options.toolId ? { toolId: options.toolId } : {}),
|
||||
...(options.frameId ? { frameId: options.frameId } : {})
|
||||
}));
|
||||
return {
|
||||
targets,
|
||||
path: {
|
||||
id: options.pathId,
|
||||
name: options.pathId,
|
||||
source: {
|
||||
type: source.kind,
|
||||
samples: points.length
|
||||
},
|
||||
defaults: {
|
||||
...(options.speedId ? { speedId: options.speedId } : {}),
|
||||
...(options.zoneId ? { zoneId: options.zoneId } : {}),
|
||||
...(options.toolId ? { toolId: options.toolId } : {}),
|
||||
...(options.frameId ? { frameId: options.frameId } : {})
|
||||
},
|
||||
points: pathPoints
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
export function detectBasicCollisions(samples: CollisionSample[], objects: OlpCollisionObject[], clearanceMm = 0): CollisionReport {
|
||||
const hits: CollisionHit[] = [];
|
||||
for (const sample of samples) {
|
||||
for (const object of objects) {
|
||||
const distance = signedDistanceToObject(sample.position, object);
|
||||
if (distance <= clearanceMm) {
|
||||
hits.push({
|
||||
objectId: object.id,
|
||||
pointId: sample.pointId,
|
||||
time: sample.time,
|
||||
distance
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
return {
|
||||
ok: hits.length === 0,
|
||||
hits
|
||||
};
|
||||
}
|
||||
|
||||
export const checkBasicCollisions = detectBasicCollisions;
|
||||
|
||||
export function locateCollisionTime(report: CollisionReport): number | undefined {
|
||||
return report.hits.reduce<number | undefined>((earliest, hit) => (
|
||||
earliest === undefined || hit.time < earliest ? hit.time : earliest
|
||||
), undefined);
|
||||
}
|
||||
|
||||
export function applyCalibrationRecords(project: OlpProjectModel, records: OlpCalibrationRecord[]): OlpProjectModel {
|
||||
const next = JSON.parse(JSON.stringify(project)) as OlpProjectModel;
|
||||
for (const record of records) {
|
||||
if (record.kind === "tcp" && record.poseDelta) {
|
||||
const tool = next.resources.tools.find((item) => item.id === record.targetResourceId);
|
||||
if (tool) tool.tcp = addPose(tool.tcp, record.poseDelta);
|
||||
}
|
||||
if ((record.kind === "frame" || record.kind === "base") && record.poseDelta) {
|
||||
const frame = next.resources.frames.find((item) => item.id === record.targetResourceId);
|
||||
if (frame) frame.pose = addPose(frame.pose, record.poseDelta);
|
||||
}
|
||||
if (record.kind === "external_axis" && record.axisOffset !== undefined) {
|
||||
const axis = next.externalAxes.find((item) => item.id === record.targetResourceId);
|
||||
if (axis) {
|
||||
axis.metadata = {
|
||||
...(axis.metadata ?? {}),
|
||||
calibrationOffset: record.axisOffset
|
||||
} as JsonObject;
|
||||
}
|
||||
}
|
||||
next.calibrations = upsertById(next.calibrations, record);
|
||||
}
|
||||
return next;
|
||||
}
|
||||
|
||||
export function createResourceLibraryTemplate(project: OlpProjectModel): JsonObject {
|
||||
return {
|
||||
robots: project.resources.robots.map((robot) => ({ id: robot.id, brand: robot.brand, model: robot.model })),
|
||||
tools: project.resources.tools.map((tool) => ({ id: tool.id, tcp: tool.tcp })),
|
||||
frames: project.resources.frames.map((frame) => ({ id: frame.id, pose: frame.pose })),
|
||||
collisionObjects: project.resources.collisionObjects.map((object) => ({ id: object.id, shape: object.shape }))
|
||||
};
|
||||
}
|
||||
|
||||
export function instantiateProcessTemplate(template: OlpProcessTemplate, operationId: string): JsonObject {
|
||||
return {
|
||||
operationId,
|
||||
operationKind: template.operationKind,
|
||||
defaults: template.defaults,
|
||||
deliveryTags: template.deliveryTags
|
||||
};
|
||||
}
|
||||
|
||||
export function compareControllerLowSpeedRun(
|
||||
samples: ControllerCheckSample[],
|
||||
tolerances: { positionMm: number; orientationDeg: number },
|
||||
boundaries = controllerVerificationBoundaries()
|
||||
): ControllerCheckReport {
|
||||
const evaluated = samples.map((sample) => {
|
||||
const positionErrorMm = distance3(sample.offline, sample.measured);
|
||||
const orientationErrorDeg = Math.max(
|
||||
Math.abs(sample.offline[3] - sample.measured[3]),
|
||||
Math.abs(sample.offline[4] - sample.measured[4]),
|
||||
Math.abs(sample.offline[5] - sample.measured[5])
|
||||
);
|
||||
return {
|
||||
...sample,
|
||||
positionErrorMm,
|
||||
orientationErrorDeg
|
||||
};
|
||||
});
|
||||
const maxPositionErrorMm = Math.max(0, ...evaluated.map((sample) => sample.positionErrorMm));
|
||||
const maxOrientationErrorDeg = Math.max(0, ...evaluated.map((sample) => sample.orientationErrorDeg));
|
||||
const status = maxPositionErrorMm > tolerances.positionMm || maxOrientationErrorDeg > tolerances.orientationDeg
|
||||
? "fail"
|
||||
: samples.some((sample) => sample.speedPercent > 25)
|
||||
? "warn"
|
||||
: "pass";
|
||||
return {
|
||||
status,
|
||||
speedPercent: Math.max(0, ...samples.map((sample) => sample.speedPercent)),
|
||||
maxPositionErrorMm,
|
||||
maxOrientationErrorDeg,
|
||||
samples: evaluated,
|
||||
boundaries
|
||||
};
|
||||
}
|
||||
|
||||
export function controllerVerificationBoundaries(): string[] {
|
||||
return [
|
||||
"MVP records low-speed sampled offline/field deltas; it does not open a live controller communication session.",
|
||||
"MVP compares taught point poses and max error; it does not guarantee high-precision dynamic trajectory reproduction.",
|
||||
"MVP uses sampled geometry and primitive collision volumes; it does not include a full CAD kernel."
|
||||
];
|
||||
}
|
||||
|
||||
export function commercialExtensionBoundaries(): CommercialExtensionBoundary[] {
|
||||
return [
|
||||
{
|
||||
feature: "resource_library",
|
||||
mvp: true,
|
||||
boundary: "JSON serializable robot/tool/frame/collision object catalog only."
|
||||
},
|
||||
{
|
||||
feature: "process_templates",
|
||||
mvp: true,
|
||||
boundary: "Template defaults and delivery tags only; no proprietary process parameter solver."
|
||||
},
|
||||
{
|
||||
feature: "delivery_package",
|
||||
mvp: true,
|
||||
boundary: "Stable JSON/HTML/text package prototype; compression and signing can be added later."
|
||||
},
|
||||
{
|
||||
feature: "cad_kernel",
|
||||
mvp: false,
|
||||
boundary: "Exact CAD B-rep import and boolean collision are outside this MVP."
|
||||
},
|
||||
{
|
||||
feature: "real_controller_communication",
|
||||
mvp: false,
|
||||
boundary: "Live controller upload/download and safety interlocks are outside this MVP."
|
||||
}
|
||||
];
|
||||
}
|
||||
|
||||
function sampleGeometry(source: GeometrySource): Vec3[] {
|
||||
if (source.kind === "points") {
|
||||
return source.points;
|
||||
}
|
||||
if (source.kind === "edge") {
|
||||
return interpolate(source.start, source.end, Math.max(2, source.samples));
|
||||
}
|
||||
if (source.kind === "polyline") {
|
||||
if (!source.spacing) return source.points;
|
||||
const result: Vec3[] = [];
|
||||
for (let index = 0; index < source.points.length - 1; index += 1) {
|
||||
const start = source.points[index]!;
|
||||
const end = source.points[index + 1]!;
|
||||
const count = Math.max(2, Math.ceil(distanceVec3(start, end) / source.spacing) + 1);
|
||||
const segment = interpolate(start, end, count);
|
||||
result.push(...(index === 0 ? segment : segment.slice(1)));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
const count = Math.max(2, source.samples);
|
||||
const result: Vec3[] = [];
|
||||
for (let index = 0; index < count; index += 1) {
|
||||
const t = count === 1 ? 0 : index / (count - 1);
|
||||
result.push(bezier(source.controlPoints, t));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
function interpolate(start: Vec3, end: Vec3, count: number): Vec3[] {
|
||||
return Array.from({ length: count }, (_, index) => {
|
||||
const t = count === 1 ? 0 : index / (count - 1);
|
||||
return [
|
||||
start[0] + (end[0] - start[0]) * t,
|
||||
start[1] + (end[1] - start[1]) * t,
|
||||
start[2] + (end[2] - start[2]) * t
|
||||
];
|
||||
});
|
||||
}
|
||||
|
||||
function bezier(points: Vec3[], t: number): Vec3 {
|
||||
if (points.length === 0) return [0, 0, 0];
|
||||
let current = points.map((point) => [...point] as Vec3);
|
||||
while (current.length > 1) {
|
||||
current = current.slice(0, -1).map((point, index): Vec3 => [
|
||||
point[0] + (current[index + 1]![0] - point[0]) * t,
|
||||
point[1] + (current[index + 1]![1] - point[1]) * t,
|
||||
point[2] + (current[index + 1]![2] - point[2]) * t
|
||||
]);
|
||||
}
|
||||
return current[0]!;
|
||||
}
|
||||
|
||||
function signedDistanceToObject(point: Vec3, object: OlpCollisionObject): number {
|
||||
const center = object.pose.slice(0, 3) as Vec3;
|
||||
if (object.shape.kind === "sphere") {
|
||||
return distanceVec3(point, center) - object.shape.radius;
|
||||
}
|
||||
if (object.shape.kind === "capsule") {
|
||||
const half = object.shape.height / 2;
|
||||
const clampedZ = Math.max(center[2] - half, Math.min(center[2] + half, point[2]));
|
||||
return distanceVec3(point, [center[0], center[1], clampedZ]) - object.shape.radius;
|
||||
}
|
||||
const half = object.shape.size.map((value) => value / 2) as Vec3;
|
||||
const delta: Vec3 = [
|
||||
Math.max(Math.abs(point[0] - center[0]) - half[0], 0),
|
||||
Math.max(Math.abs(point[1] - center[1]) - half[1], 0),
|
||||
Math.max(Math.abs(point[2] - center[2]) - half[2], 0)
|
||||
];
|
||||
const outsideDistance = distanceVec3(delta, [0, 0, 0]);
|
||||
const inside = Math.abs(point[0] - center[0]) <= half[0]
|
||||
&& Math.abs(point[1] - center[1]) <= half[1]
|
||||
&& Math.abs(point[2] - center[2]) <= half[2];
|
||||
return inside ? -Math.min(half[0], half[1], half[2]) : outsideDistance;
|
||||
}
|
||||
|
||||
function addPose(left: Pose6D, right: Pose6D): Pose6D {
|
||||
return [
|
||||
left[0] + right[0],
|
||||
left[1] + right[1],
|
||||
left[2] + right[2],
|
||||
left[3] + right[3],
|
||||
left[4] + right[4],
|
||||
left[5] + right[5]
|
||||
];
|
||||
}
|
||||
|
||||
function upsertById<T extends { id: string }>(items: T[], item: T): T[] {
|
||||
const index = items.findIndex((entry) => entry.id === item.id);
|
||||
if (index === -1) return [...items, item];
|
||||
const next = [...items];
|
||||
next[index] = item;
|
||||
return next;
|
||||
}
|
||||
|
||||
function distance3(left: Pose6D, right: Pose6D): number {
|
||||
return distanceVec3([left[0], left[1], left[2]], [right[0], right[1], right[2]]);
|
||||
}
|
||||
|
||||
function distanceVec3(left: Vec3, right: Vec3): number {
|
||||
return Math.hypot(left[0] - right[0], left[1] - right[1], left[2] - right[2]);
|
||||
}
|
||||
173
kdl-wasm/web/src/olp/grl.ts
Normal file
173
kdl-wasm/web/src/olp/grl.ts
Normal file
@@ -0,0 +1,173 @@
|
||||
import {
|
||||
generateGrlProgram,
|
||||
type GeneratedGrlProgram,
|
||||
type GeneratedPathPointSpec,
|
||||
type GrlGeneratorStyle,
|
||||
type GrlProgramGenerationSpec
|
||||
} from "../grl/generator/index.js";
|
||||
import type {
|
||||
OlpOperation,
|
||||
OlpPath,
|
||||
OlpPathPoint,
|
||||
OlpProgram,
|
||||
OlpProjectModel,
|
||||
OlpSpeed,
|
||||
OlpTarget,
|
||||
OlpZone
|
||||
} from "./model.js";
|
||||
import { validateOlpProject } from "./model.js";
|
||||
|
||||
export interface PathOperationGrlOptions {
|
||||
moduleName?: string;
|
||||
programId?: string;
|
||||
style?: GrlGeneratorStyle;
|
||||
}
|
||||
|
||||
export interface PathOperationGrlResult extends GeneratedGrlProgram {
|
||||
spec: GrlProgramGenerationSpec;
|
||||
program: OlpProgram;
|
||||
}
|
||||
|
||||
export function pathOperationToGrl(
|
||||
model: OlpProjectModel,
|
||||
operationId: string,
|
||||
options: PathOperationGrlOptions = {}
|
||||
): PathOperationGrlResult {
|
||||
const validation = validateOlpProject(model);
|
||||
const errors = validation.issues.filter((issue) => issue.severity === "error");
|
||||
if (errors.length > 0) {
|
||||
throw new Error(`Cannot generate GRL from invalid OLP model: ${errors.map((issue) => issue.code).join(", ")}`);
|
||||
}
|
||||
|
||||
const operation = required(model.operations.find((item) => item.id === operationId), `Operation ${operationId} not found`);
|
||||
const path = required(model.paths.find((item) => item.id === operation.pathId), `Path ${operation.pathId} not found`);
|
||||
const targets = collectTargetsForPath(model.targets, path);
|
||||
const spec = toGenerationSpec(model, operation, path, targets, options.moduleName);
|
||||
const generated = generateGrlProgram(spec, options.style ?? "expanded");
|
||||
const program: OlpProgram = {
|
||||
id: options.programId ?? `${operation.id}_grl`,
|
||||
name: spec.moduleName,
|
||||
language: "grl",
|
||||
entryOperationIds: [operation.id],
|
||||
source: {
|
||||
kind: "generated",
|
||||
text: generated.text,
|
||||
generatedFrom: {
|
||||
operationId: operation.id,
|
||||
pathId: path.id
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
return {
|
||||
...generated,
|
||||
spec,
|
||||
program
|
||||
};
|
||||
}
|
||||
|
||||
function toGenerationSpec(
|
||||
model: OlpProjectModel,
|
||||
operation: OlpOperation,
|
||||
path: OlpPath,
|
||||
targets: OlpTarget[],
|
||||
moduleName?: string
|
||||
): GrlProgramGenerationSpec {
|
||||
return {
|
||||
moduleName: moduleName ?? sanitizeIdentifier(model.project.name),
|
||||
speeds: Object.fromEntries(model.speeds.map((speed) => [speed.name, renderSpeed(speed)])),
|
||||
zones: Object.fromEntries(model.zones.map((zone) => [zone.name, renderZone(zone)])),
|
||||
targets: targets.map((target) => {
|
||||
if (target.kind === "joint") {
|
||||
return {
|
||||
name: target.name,
|
||||
kind: "joint" as const,
|
||||
values: target.joints ?? []
|
||||
};
|
||||
}
|
||||
return {
|
||||
name: target.name,
|
||||
kind: "pose" as const,
|
||||
values: target.pose ?? [0, 0, 0, 0, 0, 0]
|
||||
};
|
||||
}),
|
||||
path: {
|
||||
name: path.name,
|
||||
...(path.source ? { source: path.source } : {}),
|
||||
defaults: {
|
||||
speed: resolveSpeedName(model, path.defaults?.speedId) ?? "v_linear",
|
||||
zone: resolveZoneName(model, path.defaults?.zoneId) ?? "z10"
|
||||
},
|
||||
points: path.points.map((point) => toGeneratedPoint(model, point))
|
||||
},
|
||||
operation: {
|
||||
name: operation.name,
|
||||
kind: operation.kind,
|
||||
path: path.name,
|
||||
...(operation.startAction ? { startAction: operation.startAction } : {}),
|
||||
...(operation.endAction ? { endAction: operation.endAction } : {})
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
function collectTargetsForPath(allTargets: OlpTarget[], path: OlpPath): OlpTarget[] {
|
||||
const targetIds = new Set(path.points.flatMap((point) => [point.targetId, point.viaTargetId].filter((id): id is string => Boolean(id))));
|
||||
return allTargets.filter((target) => targetIds.has(target.id));
|
||||
}
|
||||
|
||||
function toGeneratedPoint(model: OlpProjectModel, point: OlpPathPoint): GeneratedPathPointSpec {
|
||||
const target = required(model.targets.find((item) => item.id === point.targetId), `Target ${point.targetId} not found`);
|
||||
const via = point.viaTargetId
|
||||
? required(model.targets.find((item) => item.id === point.viaTargetId), `Via target ${point.viaTargetId} not found`)
|
||||
: undefined;
|
||||
return {
|
||||
id: point.name,
|
||||
motion: point.motion,
|
||||
target: target.name,
|
||||
...(via ? { via: via.name } : {}),
|
||||
...(point.speedId ? { speed: required(resolveSpeedName(model, point.speedId), `Speed ${point.speedId} not found`) } : {}),
|
||||
...(point.zoneId ? { zone: required(resolveZoneName(model, point.zoneId), `Zone ${point.zoneId} not found`) } : {})
|
||||
};
|
||||
}
|
||||
|
||||
function renderSpeed(speed: OlpSpeed): string {
|
||||
if (speed.kind === "joint_percent") {
|
||||
return `joint(${speed.value} %)`;
|
||||
}
|
||||
return `linear(${speed.value} mm/s)`;
|
||||
}
|
||||
|
||||
function renderZone(zone: OlpZone): string {
|
||||
if (zone.kind === "fine") {
|
||||
return "fine";
|
||||
}
|
||||
if (zone.kind === "cnt") {
|
||||
return `z(${zone.value ?? 10})`;
|
||||
}
|
||||
return `z(${zone.value ?? 10} mm)`;
|
||||
}
|
||||
|
||||
function resolveSpeedName(model: OlpProjectModel, id?: string): string | undefined {
|
||||
if (!id) return undefined;
|
||||
return model.speeds.find((speed) => speed.id === id)?.name;
|
||||
}
|
||||
|
||||
function resolveZoneName(model: OlpProjectModel, id?: string): string | undefined {
|
||||
if (!id) return undefined;
|
||||
return model.zones.find((zone) => zone.id === id)?.name;
|
||||
}
|
||||
|
||||
function sanitizeIdentifier(value: string): string {
|
||||
const sanitized = value.replace(/[^A-Za-z0-9_]/g, "_");
|
||||
if (/^[A-Za-z_]/.test(sanitized)) {
|
||||
return sanitized;
|
||||
}
|
||||
return `Project_${sanitized}`;
|
||||
}
|
||||
|
||||
function required<T>(value: T | undefined, message: string): T {
|
||||
if (value === undefined) {
|
||||
throw new Error(message);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
76
kdl-wasm/web/src/olp/index.ts
Normal file
76
kdl-wasm/web/src/olp/index.ts
Normal file
@@ -0,0 +1,76 @@
|
||||
export {
|
||||
applyCalibrationRecords,
|
||||
commercialExtensionBoundaries,
|
||||
compareControllerLowSpeedRun,
|
||||
controllerVerificationBoundaries,
|
||||
createResourceLibraryTemplate,
|
||||
detectBasicCollisions,
|
||||
checkBasicCollisions,
|
||||
generatePathFromGeometry,
|
||||
instantiateProcessTemplate,
|
||||
locateCollisionTime,
|
||||
type CollisionHit,
|
||||
type CollisionCheckResult,
|
||||
type CollisionReport,
|
||||
type CollisionSample,
|
||||
type CommercialExtensionBoundary,
|
||||
type ControllerCheckReport,
|
||||
type ControllerCheckSample,
|
||||
type GeometryPathOptions,
|
||||
type GeometryPathResult,
|
||||
type GeometrySource
|
||||
} from "./geometry.js";
|
||||
export {
|
||||
pathOperationToGrl,
|
||||
type PathOperationGrlOptions,
|
||||
type PathOperationGrlResult
|
||||
} from "./grl.js";
|
||||
export {
|
||||
applyPatch,
|
||||
type JsonPatchOperation
|
||||
} from "./patch.js";
|
||||
export {
|
||||
applyCalibration,
|
||||
type CalibrationApplyResult
|
||||
} from "./calibration.js";
|
||||
export {
|
||||
createOlpProject,
|
||||
sampleOlpProject,
|
||||
validateOlpProject,
|
||||
type CreateOlpProjectOptions,
|
||||
type JsonObject,
|
||||
type JsonPrimitive,
|
||||
type JsonValue,
|
||||
type OlpBrand,
|
||||
type OlpCalibrationRecord,
|
||||
type OlpCollisionObject,
|
||||
type OlpExternalAxis,
|
||||
type OlpFrame,
|
||||
type OlpGeometryAsset,
|
||||
type OlpId,
|
||||
type OlpJointLimit,
|
||||
type OlpMotionGroup,
|
||||
type OlpMotionKind,
|
||||
type OlpNamedEntity,
|
||||
type OlpOperation,
|
||||
type OlpPath,
|
||||
type OlpPathPoint,
|
||||
type OlpPostProfile,
|
||||
type OlpProcessTemplate,
|
||||
type OlpProgram,
|
||||
type OlpProjectInfo,
|
||||
type OlpProjectModel,
|
||||
type OlpReportRef,
|
||||
type OlpResourceLibrary,
|
||||
type OlpRobotResource,
|
||||
type OlpSchemaVersion,
|
||||
type OlpSpeed,
|
||||
type OlpStation,
|
||||
type OlpTarget,
|
||||
type OlpTool,
|
||||
type OlpValidationIssue,
|
||||
type OlpValidationReport,
|
||||
type OlpZone,
|
||||
type Pose6D,
|
||||
type Vec3
|
||||
} from "./model.js";
|
||||
670
kdl-wasm/web/src/olp/model.ts
Normal file
670
kdl-wasm/web/src/olp/model.ts
Normal file
@@ -0,0 +1,670 @@
|
||||
export type OlpSchemaVersion = "olp/0.1";
|
||||
export type OlpId = string;
|
||||
export type OlpBrand = "abb" | "fanuc" | "kuka" | "generic";
|
||||
|
||||
export type JsonPrimitive = string | number | boolean | null;
|
||||
export type JsonValue = JsonPrimitive | JsonValue[] | { [key: string]: JsonValue };
|
||||
export type JsonObject = { [key: string]: JsonValue };
|
||||
|
||||
export type Pose6D = [number, number, number, number, number, number];
|
||||
export type Vec3 = [number, number, number];
|
||||
|
||||
export interface OlpNamedEntity {
|
||||
id: OlpId;
|
||||
name: string;
|
||||
metadata?: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpProjectInfo extends OlpNamedEntity {
|
||||
customer?: string;
|
||||
revision?: string;
|
||||
}
|
||||
|
||||
export interface OlpStation extends OlpNamedEntity {
|
||||
resourceIds: OlpId[];
|
||||
activeRobotIds: OlpId[];
|
||||
defaultFrameId?: OlpId;
|
||||
}
|
||||
|
||||
export interface OlpJointLimit {
|
||||
name: string;
|
||||
lower: number;
|
||||
upper: number;
|
||||
velocity: number;
|
||||
acceleration?: number;
|
||||
}
|
||||
|
||||
export interface OlpRobotResource extends OlpNamedEntity {
|
||||
kind: "robot";
|
||||
brand: OlpBrand;
|
||||
model: string;
|
||||
dof: number;
|
||||
jointNames: string[];
|
||||
limits?: OlpJointLimit[];
|
||||
baseFrameId?: OlpId;
|
||||
controller?: {
|
||||
family: string;
|
||||
version?: string;
|
||||
};
|
||||
}
|
||||
|
||||
export interface OlpTool extends OlpNamedEntity {
|
||||
kind: "tool";
|
||||
tcp: Pose6D;
|
||||
robotId?: OlpId;
|
||||
massKg?: number;
|
||||
}
|
||||
|
||||
export interface OlpFrame extends OlpNamedEntity {
|
||||
kind: "frame";
|
||||
pose: Pose6D;
|
||||
parentFrameId?: OlpId;
|
||||
robotId?: OlpId;
|
||||
}
|
||||
|
||||
export interface OlpGeometryAsset extends OlpNamedEntity {
|
||||
kind: "point" | "edge" | "curve" | "box" | "mesh_ref";
|
||||
data: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpCollisionObject extends OlpNamedEntity {
|
||||
kind: "collision_object";
|
||||
shape:
|
||||
| { kind: "box"; size: Vec3 }
|
||||
| { kind: "sphere"; radius: number }
|
||||
| { kind: "capsule"; radius: number; height: number };
|
||||
pose: Pose6D;
|
||||
attachedToId?: OlpId;
|
||||
}
|
||||
|
||||
export interface OlpResourceLibrary {
|
||||
robots: OlpRobotResource[];
|
||||
tools: OlpTool[];
|
||||
frames: OlpFrame[];
|
||||
geometryAssets: OlpGeometryAsset[];
|
||||
collisionObjects: OlpCollisionObject[];
|
||||
}
|
||||
|
||||
export interface OlpSpeed extends OlpNamedEntity {
|
||||
kind: "joint_percent" | "linear_mm_s";
|
||||
value: number;
|
||||
}
|
||||
|
||||
export interface OlpZone extends OlpNamedEntity {
|
||||
kind: "fine" | "distance_mm" | "cnt";
|
||||
value?: number;
|
||||
}
|
||||
|
||||
export interface OlpTarget extends OlpNamedEntity {
|
||||
kind: "joint" | "pose";
|
||||
robotId?: OlpId;
|
||||
pose?: Pose6D;
|
||||
joints?: number[];
|
||||
toolId?: OlpId;
|
||||
frameId?: OlpId;
|
||||
externalAxes?: Record<OlpId, number>;
|
||||
}
|
||||
|
||||
export type OlpMotionKind = "movej" | "movel" | "movec";
|
||||
|
||||
export interface OlpPathPoint extends OlpNamedEntity {
|
||||
motion: OlpMotionKind;
|
||||
targetId: OlpId;
|
||||
viaTargetId?: OlpId;
|
||||
speedId?: OlpId;
|
||||
zoneId?: OlpId;
|
||||
toolId?: OlpId;
|
||||
frameId?: OlpId;
|
||||
}
|
||||
|
||||
export interface OlpPath extends OlpNamedEntity {
|
||||
defaults?: {
|
||||
speedId?: OlpId;
|
||||
zoneId?: OlpId;
|
||||
toolId?: OlpId;
|
||||
frameId?: OlpId;
|
||||
};
|
||||
source?: Record<string, string | number | boolean>;
|
||||
points: OlpPathPoint[];
|
||||
}
|
||||
|
||||
export interface OlpOperation extends OlpNamedEntity {
|
||||
kind: string;
|
||||
pathId: OlpId;
|
||||
robotId?: OlpId;
|
||||
process?: JsonObject;
|
||||
startAction?: string;
|
||||
endAction?: string;
|
||||
}
|
||||
|
||||
export interface OlpProgram extends OlpNamedEntity {
|
||||
language: "grl" | "abb_rapid" | "kuka_krl" | "fanuc_ls" | "text";
|
||||
entryOperationIds: OlpId[];
|
||||
source: {
|
||||
kind: "generated" | "imported" | "authored";
|
||||
text: string;
|
||||
generatedFrom?: {
|
||||
operationId: OlpId;
|
||||
pathId: OlpId;
|
||||
};
|
||||
};
|
||||
}
|
||||
|
||||
export interface OlpPostProfile extends OlpNamedEntity {
|
||||
brand: Exclude<OlpBrand, "generic">;
|
||||
controllerFamily: string;
|
||||
fileExtension: string;
|
||||
options?: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpReportRef extends OlpNamedEntity {
|
||||
reportKind: "validation" | "import" | "delivery" | "controller_check";
|
||||
status: "pass" | "warn" | "fail";
|
||||
uri?: string;
|
||||
summary?: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpCalibrationRecord extends OlpNamedEntity {
|
||||
kind: "tcp" | "frame" | "base" | "external_axis";
|
||||
targetResourceId: OlpId;
|
||||
poseDelta?: Pose6D;
|
||||
axisOffset?: number;
|
||||
measuredAt?: string;
|
||||
evidence?: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpExternalAxis extends OlpNamedEntity {
|
||||
axisKind: "linear" | "rotary";
|
||||
jointName: string;
|
||||
limits: {
|
||||
lower: number;
|
||||
upper: number;
|
||||
velocity: number;
|
||||
};
|
||||
attachedRobotIds?: OlpId[];
|
||||
}
|
||||
|
||||
export interface OlpMotionGroup extends OlpNamedEntity {
|
||||
robotIds: OlpId[];
|
||||
externalAxisIds: OlpId[];
|
||||
coordination: "independent" | "synchronized" | "master_slave";
|
||||
}
|
||||
|
||||
export interface OlpProcessTemplate extends OlpNamedEntity {
|
||||
operationKind: string;
|
||||
defaults: JsonObject;
|
||||
deliveryTags: string[];
|
||||
}
|
||||
|
||||
export interface OlpProjectModel {
|
||||
schemaVersion: OlpSchemaVersion;
|
||||
project: OlpProjectInfo;
|
||||
station: OlpStation;
|
||||
resources: OlpResourceLibrary;
|
||||
speeds: OlpSpeed[];
|
||||
zones: OlpZone[];
|
||||
targets: OlpTarget[];
|
||||
paths: OlpPath[];
|
||||
operations: OlpOperation[];
|
||||
programs: OlpProgram[];
|
||||
postProfiles: OlpPostProfile[];
|
||||
reports: OlpReportRef[];
|
||||
calibrations: OlpCalibrationRecord[];
|
||||
externalAxes: OlpExternalAxis[];
|
||||
motionGroups: OlpMotionGroup[];
|
||||
processTemplates: OlpProcessTemplate[];
|
||||
metadata?: JsonObject;
|
||||
}
|
||||
|
||||
export interface OlpValidationIssue {
|
||||
severity: "info" | "warning" | "error";
|
||||
code: string;
|
||||
message: string;
|
||||
path?: string;
|
||||
}
|
||||
|
||||
export interface OlpValidationReport {
|
||||
ok: boolean;
|
||||
issues: OlpValidationIssue[];
|
||||
}
|
||||
|
||||
export interface CreateOlpProjectOptions {
|
||||
id: OlpId;
|
||||
name: string;
|
||||
customer?: string;
|
||||
revision?: string;
|
||||
}
|
||||
|
||||
export function createOlpProject(options: CreateOlpProjectOptions): OlpProjectModel {
|
||||
const project: OlpProjectInfo = {
|
||||
id: options.id,
|
||||
name: options.name,
|
||||
...(options.customer ? { customer: options.customer } : {}),
|
||||
...(options.revision ? { revision: options.revision } : {})
|
||||
};
|
||||
return {
|
||||
schemaVersion: "olp/0.1",
|
||||
project,
|
||||
station: {
|
||||
id: `${options.id}_station`,
|
||||
name: `${options.name} Station`,
|
||||
resourceIds: [],
|
||||
activeRobotIds: []
|
||||
},
|
||||
resources: {
|
||||
robots: [],
|
||||
tools: [],
|
||||
frames: [],
|
||||
geometryAssets: [],
|
||||
collisionObjects: []
|
||||
},
|
||||
speeds: [],
|
||||
zones: [],
|
||||
targets: [],
|
||||
paths: [],
|
||||
operations: [],
|
||||
programs: [],
|
||||
postProfiles: [],
|
||||
reports: [],
|
||||
calibrations: [],
|
||||
externalAxes: [],
|
||||
motionGroups: [],
|
||||
processTemplates: []
|
||||
};
|
||||
}
|
||||
|
||||
export function sampleOlpProject(): OlpProjectModel {
|
||||
return {
|
||||
...createOlpProject({ id: "demo_cell", name: "DemoCell", customer: "KDL" }),
|
||||
station: {
|
||||
id: "station_demo",
|
||||
name: "Demo Station",
|
||||
resourceIds: ["robot_1", "tool_gripper", "frame_world"],
|
||||
activeRobotIds: ["robot_1"],
|
||||
defaultFrameId: "frame_world"
|
||||
},
|
||||
resources: {
|
||||
robots: [
|
||||
{
|
||||
id: "robot_1",
|
||||
name: "IRB120",
|
||||
kind: "robot",
|
||||
brand: "abb",
|
||||
model: "IRB120",
|
||||
dof: 6,
|
||||
jointNames: ["J1", "J2", "J3", "J4", "J5", "J6"],
|
||||
baseFrameId: "frame_world"
|
||||
}
|
||||
],
|
||||
tools: [
|
||||
{
|
||||
id: "tool_gripper",
|
||||
name: "gripper",
|
||||
kind: "tool",
|
||||
tcp: [0, 0, 100, 0, 0, 0],
|
||||
robotId: "robot_1",
|
||||
massKg: 2.5
|
||||
}
|
||||
],
|
||||
frames: [
|
||||
{
|
||||
id: "frame_world",
|
||||
name: "world",
|
||||
kind: "frame",
|
||||
pose: [0, 0, 0, 0, 0, 0]
|
||||
},
|
||||
{
|
||||
id: "frame_fixture",
|
||||
name: "fixture",
|
||||
kind: "frame",
|
||||
pose: [800, 0, 0, 0, 0, 0],
|
||||
parentFrameId: "frame_world"
|
||||
}
|
||||
],
|
||||
geometryAssets: [
|
||||
{
|
||||
id: "edge_pick_line",
|
||||
name: "pick_line",
|
||||
kind: "edge",
|
||||
data: {
|
||||
points: [
|
||||
[500, 0, 0],
|
||||
[600, 0, 0]
|
||||
]
|
||||
}
|
||||
}
|
||||
],
|
||||
collisionObjects: [
|
||||
{
|
||||
id: "fixture_box",
|
||||
name: "fixture_box",
|
||||
kind: "collision_object",
|
||||
shape: { kind: "box", size: [200, 200, 100] },
|
||||
pose: [550, 0, -60, 0, 0, 0]
|
||||
}
|
||||
]
|
||||
},
|
||||
speeds: [
|
||||
{ id: "v_joint", name: "v_joint", kind: "joint_percent", value: 50 },
|
||||
{ id: "v_linear", name: "v_linear", kind: "linear_mm_s", value: 200 }
|
||||
],
|
||||
zones: [
|
||||
{ id: "zf", name: "zf", kind: "fine" },
|
||||
{ id: "z10", name: "z10", kind: "distance_mm", value: 10 }
|
||||
],
|
||||
targets: [
|
||||
{
|
||||
id: "home",
|
||||
name: "home",
|
||||
kind: "joint",
|
||||
robotId: "robot_1",
|
||||
joints: [0, 0, 0, 0, 0, 0]
|
||||
},
|
||||
{
|
||||
id: "pick",
|
||||
name: "pick",
|
||||
kind: "pose",
|
||||
robotId: "robot_1",
|
||||
pose: [500, 0, 0, 0, 0, 0],
|
||||
toolId: "tool_gripper",
|
||||
frameId: "frame_fixture"
|
||||
},
|
||||
{
|
||||
id: "place",
|
||||
name: "place",
|
||||
kind: "pose",
|
||||
robotId: "robot_1",
|
||||
pose: [600, 0, 0, 0, 0, 0],
|
||||
toolId: "tool_gripper",
|
||||
frameId: "frame_fixture"
|
||||
}
|
||||
],
|
||||
paths: [
|
||||
{
|
||||
id: "pick_path",
|
||||
name: "pick_path",
|
||||
source: {
|
||||
type: "cad_curve",
|
||||
id: "edge_pick_line",
|
||||
sample_distance: 5
|
||||
},
|
||||
defaults: {
|
||||
speedId: "v_linear",
|
||||
zoneId: "z10",
|
||||
toolId: "tool_gripper",
|
||||
frameId: "frame_fixture"
|
||||
},
|
||||
points: [
|
||||
{
|
||||
id: "p00",
|
||||
name: "p00",
|
||||
motion: "movej",
|
||||
targetId: "home",
|
||||
speedId: "v_joint",
|
||||
zoneId: "zf"
|
||||
},
|
||||
{
|
||||
id: "p01",
|
||||
name: "p01",
|
||||
motion: "movel",
|
||||
targetId: "pick"
|
||||
},
|
||||
{
|
||||
id: "p02",
|
||||
name: "p02",
|
||||
motion: "movel",
|
||||
targetId: "place",
|
||||
zoneId: "zf"
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
operations: [
|
||||
{
|
||||
id: "pick_op",
|
||||
name: "pick_op",
|
||||
kind: "handling",
|
||||
pathId: "pick_path",
|
||||
robotId: "robot_1",
|
||||
process: { gripper: "vacuum" },
|
||||
startAction: "io.do[1] = true",
|
||||
endAction: "io.do[1] = false"
|
||||
}
|
||||
],
|
||||
postProfiles: [
|
||||
{
|
||||
id: "abb_default",
|
||||
name: "ABB default",
|
||||
brand: "abb",
|
||||
controllerFamily: "IRC5",
|
||||
fileExtension: ".mod"
|
||||
}
|
||||
],
|
||||
calibrations: [
|
||||
{
|
||||
id: "tcp_cal_1",
|
||||
name: "gripper_tcp_cal",
|
||||
kind: "tcp",
|
||||
targetResourceId: "tool_gripper",
|
||||
poseDelta: [0.5, -0.2, 1.1, 0, 0, 0]
|
||||
}
|
||||
],
|
||||
externalAxes: [
|
||||
{
|
||||
id: "track_1",
|
||||
name: "track_1",
|
||||
axisKind: "linear",
|
||||
jointName: "E1",
|
||||
limits: { lower: -1000, upper: 1000, velocity: 250 },
|
||||
attachedRobotIds: ["robot_1"]
|
||||
}
|
||||
],
|
||||
motionGroups: [
|
||||
{
|
||||
id: "group_1",
|
||||
name: "robot_with_track",
|
||||
robotIds: ["robot_1"],
|
||||
externalAxisIds: ["track_1"],
|
||||
coordination: "synchronized"
|
||||
}
|
||||
],
|
||||
processTemplates: [
|
||||
{
|
||||
id: "handling_default",
|
||||
name: "Handling Default",
|
||||
operationKind: "handling",
|
||||
defaults: { speedId: "v_linear", zoneId: "z10" },
|
||||
deliveryTags: ["source", "post", "report"]
|
||||
}
|
||||
]
|
||||
};
|
||||
}
|
||||
|
||||
export function validateOlpProject(model: OlpProjectModel): OlpValidationReport {
|
||||
const issues: OlpValidationIssue[] = [];
|
||||
if (model.schemaVersion !== "olp/0.1") {
|
||||
issues.push(error("OLP_SCHEMA_VERSION", `Unsupported OLP schema ${String(model.schemaVersion)}`, "/schemaVersion"));
|
||||
}
|
||||
try {
|
||||
JSON.stringify(model);
|
||||
} catch (cause) {
|
||||
issues.push(error("OLP_NOT_SERIALIZABLE", `Project is not JSON serializable: ${String(cause)}`));
|
||||
}
|
||||
|
||||
const robots = idMap(model.resources.robots, "/resources/robots", issues);
|
||||
const tools = idMap(model.resources.tools, "/resources/tools", issues);
|
||||
const frames = idMap(model.resources.frames, "/resources/frames", issues);
|
||||
const targets = idMap(model.targets, "/targets", issues);
|
||||
const paths = idMap(model.paths, "/paths", issues);
|
||||
const operations = idMap(model.operations, "/operations", issues);
|
||||
const speeds = idMap(model.speeds, "/speeds", issues);
|
||||
const zones = idMap(model.zones, "/zones", issues);
|
||||
const externalAxes = idMap(model.externalAxes, "/externalAxes", issues);
|
||||
idMap(model.programs, "/programs", issues);
|
||||
idMap(model.postProfiles, "/postProfiles", issues);
|
||||
idMap(model.reports, "/reports", issues);
|
||||
idMap(model.calibrations, "/calibrations", issues);
|
||||
idMap(model.motionGroups, "/motionGroups", issues);
|
||||
idMap(model.processTemplates, "/processTemplates", issues);
|
||||
idMap(model.resources.geometryAssets, "/resources/geometryAssets", issues);
|
||||
idMap(model.resources.collisionObjects, "/resources/collisionObjects", issues);
|
||||
|
||||
for (const robot of model.resources.robots) {
|
||||
if (robot.dof !== robot.jointNames.length) {
|
||||
issues.push(error("OLP_ROBOT_DOF", `Robot ${robot.id} dof does not match jointNames`, `/resources/robots/${robot.id}/dof`));
|
||||
}
|
||||
if (robot.baseFrameId && !frames.has(robot.baseFrameId)) {
|
||||
issues.push(error("OLP_REF_FRAME_MISSING", `Robot ${robot.id} references missing frame ${robot.baseFrameId}`, `/resources/robots/${robot.id}/baseFrameId`));
|
||||
}
|
||||
}
|
||||
|
||||
for (const tool of model.resources.tools) {
|
||||
if (tool.robotId && !robots.has(tool.robotId)) {
|
||||
issues.push(error("OLP_REF_ROBOT_MISSING", `Tool ${tool.id} references missing robot ${tool.robotId}`, `/resources/tools/${tool.id}/robotId`));
|
||||
}
|
||||
validatePose(tool.tcp, `/resources/tools/${tool.id}/tcp`, issues);
|
||||
}
|
||||
|
||||
for (const frame of model.resources.frames) {
|
||||
validatePose(frame.pose, `/resources/frames/${frame.id}/pose`, issues);
|
||||
if (frame.parentFrameId && !frames.has(frame.parentFrameId)) {
|
||||
issues.push(error("OLP_REF_FRAME_MISSING", `Frame ${frame.id} references missing parent ${frame.parentFrameId}`, `/resources/frames/${frame.id}/parentFrameId`));
|
||||
}
|
||||
}
|
||||
|
||||
for (const target of model.targets) {
|
||||
if (!isIdentifier(target.name)) {
|
||||
issues.push(error("OLP_GRL_IDENTIFIER", `Target ${target.id} name is not a GRL identifier`, `/targets/${target.id}/name`));
|
||||
}
|
||||
if (target.robotId && !robots.has(target.robotId)) {
|
||||
issues.push(error("OLP_REF_ROBOT_MISSING", `Target ${target.id} references missing robot ${target.robotId}`, `/targets/${target.id}/robotId`));
|
||||
}
|
||||
if (target.toolId && !tools.has(target.toolId)) {
|
||||
issues.push(error("OLP_REF_TOOL_MISSING", `Target ${target.id} references missing tool ${target.toolId}`, `/targets/${target.id}/toolId`));
|
||||
}
|
||||
if (target.frameId && !frames.has(target.frameId)) {
|
||||
issues.push(error("OLP_REF_FRAME_MISSING", `Target ${target.id} references missing frame ${target.frameId}`, `/targets/${target.id}/frameId`));
|
||||
}
|
||||
if (target.kind === "pose") {
|
||||
if (!target.pose) {
|
||||
issues.push(error("OLP_TARGET_POSE_MISSING", `Pose target ${target.id} has no pose`, `/targets/${target.id}/pose`));
|
||||
} else {
|
||||
validatePose(target.pose, `/targets/${target.id}/pose`, issues);
|
||||
}
|
||||
}
|
||||
if (target.kind === "joint" && !target.joints) {
|
||||
issues.push(error("OLP_TARGET_JOINTS_MISSING", `Joint target ${target.id} has no joints`, `/targets/${target.id}/joints`));
|
||||
}
|
||||
}
|
||||
|
||||
for (const path of model.paths) {
|
||||
if (!isIdentifier(path.name)) {
|
||||
issues.push(error("OLP_GRL_IDENTIFIER", `Path ${path.id} name is not a GRL identifier`, `/paths/${path.id}/name`));
|
||||
}
|
||||
if (path.defaults?.speedId && !speeds.has(path.defaults.speedId)) {
|
||||
issues.push(error("OLP_REF_SPEED_MISSING", `Path ${path.id} references missing speed ${path.defaults.speedId}`, `/paths/${path.id}/defaults/speedId`));
|
||||
}
|
||||
if (path.defaults?.zoneId && !zones.has(path.defaults.zoneId)) {
|
||||
issues.push(error("OLP_REF_ZONE_MISSING", `Path ${path.id} references missing zone ${path.defaults.zoneId}`, `/paths/${path.id}/defaults/zoneId`));
|
||||
}
|
||||
const pointNames = new Set<string>();
|
||||
for (const point of path.points) {
|
||||
if (pointNames.has(point.name)) {
|
||||
issues.push(error("OLP_PATH_POINT_DUPLICATE", `Path ${path.id} contains duplicate point ${point.name}`, `/paths/${path.id}/points/${point.id}`));
|
||||
}
|
||||
pointNames.add(point.name);
|
||||
if (!targets.has(point.targetId)) {
|
||||
issues.push(error("OLP_REF_TARGET_MISSING", `Point ${point.id} references missing target ${point.targetId}`, `/paths/${path.id}/points/${point.id}/targetId`));
|
||||
}
|
||||
if (point.motion === "movec" && (!point.viaTargetId || !targets.has(point.viaTargetId))) {
|
||||
issues.push(error("OLP_REF_VIA_TARGET_MISSING", `MoveC point ${point.id} requires an existing via target`, `/paths/${path.id}/points/${point.id}/viaTargetId`));
|
||||
}
|
||||
if (point.speedId && !speeds.has(point.speedId)) {
|
||||
issues.push(error("OLP_REF_SPEED_MISSING", `Point ${point.id} references missing speed ${point.speedId}`, `/paths/${path.id}/points/${point.id}/speedId`));
|
||||
}
|
||||
if (point.zoneId && !zones.has(point.zoneId)) {
|
||||
issues.push(error("OLP_REF_ZONE_MISSING", `Point ${point.id} references missing zone ${point.zoneId}`, `/paths/${path.id}/points/${point.id}/zoneId`));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const operation of model.operations) {
|
||||
if (!isIdentifier(operation.name)) {
|
||||
issues.push(error("OLP_GRL_IDENTIFIER", `Operation ${operation.id} name is not a GRL identifier`, `/operations/${operation.id}/name`));
|
||||
}
|
||||
if (!paths.has(operation.pathId)) {
|
||||
issues.push(error("OLP_REF_PATH_MISSING", `Operation ${operation.id} references missing path ${operation.pathId}`, `/operations/${operation.id}/pathId`));
|
||||
}
|
||||
if (operation.robotId && !robots.has(operation.robotId)) {
|
||||
issues.push(error("OLP_REF_ROBOT_MISSING", `Operation ${operation.id} references missing robot ${operation.robotId}`, `/operations/${operation.id}/robotId`));
|
||||
}
|
||||
}
|
||||
|
||||
for (const program of model.programs) {
|
||||
for (const operationId of program.entryOperationIds) {
|
||||
if (!operations.has(operationId)) {
|
||||
issues.push(error("OLP_REF_OPERATION_MISSING", `Program ${program.id} references missing operation ${operationId}`, `/programs/${program.id}/entryOperationIds`));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const calibration of model.calibrations) {
|
||||
const exists = tools.has(calibration.targetResourceId) || frames.has(calibration.targetResourceId) || robots.has(calibration.targetResourceId) || externalAxes.has(calibration.targetResourceId);
|
||||
if (!exists) {
|
||||
issues.push(error("OLP_REF_CALIBRATION_TARGET_MISSING", `Calibration ${calibration.id} references missing resource ${calibration.targetResourceId}`, `/calibrations/${calibration.id}/targetResourceId`));
|
||||
}
|
||||
}
|
||||
|
||||
for (const group of model.motionGroups) {
|
||||
for (const robotId of group.robotIds) {
|
||||
if (!robots.has(robotId)) {
|
||||
issues.push(error("OLP_REF_ROBOT_MISSING", `Motion group ${group.id} references missing robot ${robotId}`, `/motionGroups/${group.id}/robotIds`));
|
||||
}
|
||||
}
|
||||
for (const axisId of group.externalAxisIds) {
|
||||
if (!externalAxes.has(axisId)) {
|
||||
issues.push(error("OLP_REF_EXTERNAL_AXIS_MISSING", `Motion group ${group.id} references missing external axis ${axisId}`, `/motionGroups/${group.id}/externalAxisIds`));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
ok: !issues.some((issue) => issue.severity === "error"),
|
||||
issues
|
||||
};
|
||||
}
|
||||
|
||||
function idMap<T extends OlpNamedEntity>(items: T[], path: string, issues: OlpValidationIssue[]): Map<OlpId, T> {
|
||||
const map = new Map<OlpId, T>();
|
||||
for (const item of items) {
|
||||
if (!item.id) {
|
||||
issues.push(error("OLP_ID_MISSING", `Item at ${path} has no id`, path));
|
||||
continue;
|
||||
}
|
||||
if (map.has(item.id)) {
|
||||
issues.push(error("OLP_ID_DUPLICATE", `Duplicate id ${item.id}`, `${path}/${item.id}`));
|
||||
continue;
|
||||
}
|
||||
map.set(item.id, item);
|
||||
}
|
||||
return map;
|
||||
}
|
||||
|
||||
function validatePose(pose: Pose6D, path: string, issues: OlpValidationIssue[]): void {
|
||||
if (pose.length !== 6 || pose.some((value) => !Number.isFinite(value))) {
|
||||
issues.push(error("OLP_POSE_INVALID", `Pose at ${path} must contain six finite numbers`, path));
|
||||
}
|
||||
}
|
||||
|
||||
function isIdentifier(value: string): boolean {
|
||||
return /^[A-Za-z_][A-Za-z0-9_]*$/.test(value);
|
||||
}
|
||||
|
||||
function error(code: string, message: string, path?: string): OlpValidationIssue {
|
||||
return {
|
||||
severity: "error",
|
||||
code,
|
||||
message,
|
||||
...(path ? { path } : {})
|
||||
};
|
||||
}
|
||||
109
kdl-wasm/web/src/olp/patch.ts
Normal file
109
kdl-wasm/web/src/olp/patch.ts
Normal file
@@ -0,0 +1,109 @@
|
||||
export type JsonPatchOperation =
|
||||
| { op: "add" | "replace"; path: string; value: unknown }
|
||||
| { op: "remove"; path: string };
|
||||
|
||||
export function applyPatch<T>(document: T, patch: JsonPatchOperation[]): T {
|
||||
let result: unknown = cloneJson(document);
|
||||
for (const operation of patch) {
|
||||
const tokens = parsePointer(operation.path);
|
||||
if (tokens.length === 0) {
|
||||
if (operation.op === "remove") {
|
||||
result = undefined;
|
||||
} else {
|
||||
result = cloneJson(operation.value);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
const parent = resolveParent(result, tokens);
|
||||
const key = tokens[tokens.length - 1]!;
|
||||
if (operation.op === "remove") {
|
||||
removeValue(parent, key);
|
||||
} else {
|
||||
setValue(parent, key, cloneJson(operation.value), operation.op);
|
||||
}
|
||||
}
|
||||
return result as T;
|
||||
}
|
||||
|
||||
function parsePointer(path: string): string[] {
|
||||
if (path === "") return [];
|
||||
if (!path.startsWith("/")) {
|
||||
throw new Error(`Invalid JSON pointer ${path}`);
|
||||
}
|
||||
return path.slice(1).split("/").map((part) => part.replace(/~1/g, "/").replace(/~0/g, "~"));
|
||||
}
|
||||
|
||||
function resolveParent(root: unknown, tokens: string[]): unknown {
|
||||
let current = root;
|
||||
for (const token of tokens.slice(0, -1)) {
|
||||
if (Array.isArray(current)) {
|
||||
const index = numericIndex(token, current.length);
|
||||
current = current[index];
|
||||
} else if (isRecord(current)) {
|
||||
current = current[token];
|
||||
} else {
|
||||
throw new Error(`Cannot resolve JSON pointer through ${token}`);
|
||||
}
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
function setValue(parent: unknown, key: string, value: unknown, op: "add" | "replace"): void {
|
||||
if (Array.isArray(parent)) {
|
||||
if (key === "-" && op === "add") {
|
||||
parent.push(value);
|
||||
return;
|
||||
}
|
||||
const index = numericIndex(key, op === "add" ? parent.length + 1 : parent.length);
|
||||
if (op === "add") {
|
||||
parent.splice(index, 0, value);
|
||||
} else {
|
||||
parent[index] = value;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (isRecord(parent)) {
|
||||
if (op === "replace" && !(key in parent)) {
|
||||
throw new Error(`Cannot replace missing key ${key}`);
|
||||
}
|
||||
parent[key] = value;
|
||||
return;
|
||||
}
|
||||
throw new Error(`Cannot ${op} JSON pointer target ${key}`);
|
||||
}
|
||||
|
||||
function removeValue(parent: unknown, key: string): void {
|
||||
if (Array.isArray(parent)) {
|
||||
const index = numericIndex(key, parent.length);
|
||||
parent.splice(index, 1);
|
||||
return;
|
||||
}
|
||||
if (isRecord(parent)) {
|
||||
if (!(key in parent)) {
|
||||
throw new Error(`Cannot remove missing key ${key}`);
|
||||
}
|
||||
delete parent[key];
|
||||
return;
|
||||
}
|
||||
throw new Error(`Cannot remove JSON pointer target ${key}`);
|
||||
}
|
||||
|
||||
function numericIndex(token: string, maxExclusive: number): number {
|
||||
if (!/^(0|[1-9][0-9]*)$/.test(token)) {
|
||||
throw new Error(`Invalid array index ${token}`);
|
||||
}
|
||||
const index = Number(token);
|
||||
if (index < 0 || index >= maxExclusive) {
|
||||
throw new Error(`Array index ${index} is out of bounds`);
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
function isRecord(value: unknown): value is Record<string, unknown> {
|
||||
return typeof value === "object" && value !== null && !Array.isArray(value);
|
||||
}
|
||||
|
||||
function cloneJson<T>(value: T): T {
|
||||
return value === undefined ? value : JSON.parse(JSON.stringify(value)) as T;
|
||||
}
|
||||
15
kdl-wasm/web/src/reports/index.ts
Normal file
15
kdl-wasm/web/src/reports/index.ts
Normal file
@@ -0,0 +1,15 @@
|
||||
export {
|
||||
calibrationSummary,
|
||||
createCustomerDeliveryPackage,
|
||||
createValidationReport,
|
||||
exportCustomerDeliveryPackage,
|
||||
exportValidationReportHtml,
|
||||
renderValidationReportHtml,
|
||||
serializeDeliveryPackage,
|
||||
validateReportSchema,
|
||||
type DeliveryPackage,
|
||||
type DeliveryPackageFile,
|
||||
type ValidationReport,
|
||||
type ValidationReportInput,
|
||||
type ValidationReportSection
|
||||
} from "./report.js";
|
||||
289
kdl-wasm/web/src/reports/report.ts
Normal file
289
kdl-wasm/web/src/reports/report.ts
Normal file
@@ -0,0 +1,289 @@
|
||||
import type { MotionDiagnostic, PathValidationResult, TrajectoryResult } from "../kdl/types.js";
|
||||
import type { OlpCalibrationRecord, OlpProjectModel } from "../olp/index.js";
|
||||
|
||||
export interface ValidationReportSection {
|
||||
name: "reachability" | "cycle_time" | "io_wait" | "post" | "import" | "collision" | "calibration";
|
||||
status: "pass" | "warn" | "fail";
|
||||
summary: Record<string, unknown>;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export interface ValidationReport {
|
||||
schemaVersion: "validation-report/0.1";
|
||||
id: string;
|
||||
projectId: string;
|
||||
generatedAt: string;
|
||||
status: "pass" | "warn" | "fail";
|
||||
sections: ValidationReportSection[];
|
||||
sourceMap: Array<{
|
||||
kind: string;
|
||||
id: string;
|
||||
file?: string;
|
||||
line?: number;
|
||||
column?: number;
|
||||
}>;
|
||||
}
|
||||
|
||||
export interface ValidationReportInput {
|
||||
id?: string;
|
||||
project: OlpProjectModel;
|
||||
generatedAt?: string;
|
||||
pathValidation?: PathValidationResult;
|
||||
trajectories?: TrajectoryResult[];
|
||||
diagnostics?: MotionDiagnostic[];
|
||||
importDiagnostics?: MotionDiagnostic[];
|
||||
postDiagnostics?: MotionDiagnostic[];
|
||||
ioWaitDiagnostics?: MotionDiagnostic[];
|
||||
collisionDiagnostics?: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export interface DeliveryPackageFile {
|
||||
path: string;
|
||||
content: string;
|
||||
mediaType?: string;
|
||||
}
|
||||
|
||||
export interface DeliveryPackage {
|
||||
format: "customer-delivery/0.1";
|
||||
projectId: string;
|
||||
files: DeliveryPackageFile[];
|
||||
manifest: {
|
||||
sourcePrograms: string[];
|
||||
brandPrograms: string[];
|
||||
reports: string[];
|
||||
calibrationFiles: string[];
|
||||
traceFiles: string[];
|
||||
ioMaps?: string[];
|
||||
};
|
||||
checksum?: string;
|
||||
}
|
||||
|
||||
export function createValidationReport(input: ValidationReportInput): ValidationReport {
|
||||
const sections = [
|
||||
reachabilitySection(input.pathValidation, input.diagnostics ?? []),
|
||||
cycleTimeSection(input.pathValidation, input.trajectories ?? []),
|
||||
diagnosticsSection("io_wait", input.ioWaitDiagnostics ?? []),
|
||||
diagnosticsSection("post", input.postDiagnostics ?? []),
|
||||
diagnosticsSection("import", input.importDiagnostics ?? []),
|
||||
diagnosticsSection("collision", input.collisionDiagnostics ?? []),
|
||||
calibrationSection(input.project)
|
||||
];
|
||||
return {
|
||||
schemaVersion: "validation-report/0.1",
|
||||
id: input.id ?? `${input.project.project.id}-validation`,
|
||||
projectId: input.project.project.id,
|
||||
generatedAt: input.generatedAt ?? new Date().toISOString(),
|
||||
status: combineStatus(sections.map((section) => section.status)),
|
||||
sections,
|
||||
sourceMap: [
|
||||
...(input.diagnostics ?? []),
|
||||
...(input.importDiagnostics ?? []),
|
||||
...(input.postDiagnostics ?? []),
|
||||
...(input.ioWaitDiagnostics ?? []),
|
||||
...(input.collisionDiagnostics ?? [])
|
||||
]
|
||||
.filter((diagnostic) => diagnostic.sourceMap)
|
||||
.map((diagnostic, index) => ({
|
||||
kind: diagnostic.code,
|
||||
id: `${diagnostic.code}-${index}`,
|
||||
...(diagnostic.sourceMap?.file ? { file: diagnostic.sourceMap.file } : {}),
|
||||
...(diagnostic.sourceMap?.line !== undefined ? { line: diagnostic.sourceMap.line } : {}),
|
||||
...(diagnostic.sourceMap?.column !== undefined ? { column: diagnostic.sourceMap.column } : {})
|
||||
}))
|
||||
};
|
||||
}
|
||||
|
||||
export function renderValidationReportHtml(report: ValidationReport): string {
|
||||
const rows = report.sections.map((section) =>
|
||||
`<tr><td>${escapeHtml(section.name)}</td><td>${section.status}</td><td>${escapeHtml(JSON.stringify(section.summary))}</td></tr>`
|
||||
).join("");
|
||||
const diagnostics = report.sections.flatMap((section) => section.diagnostics.map((diagnostic) =>
|
||||
`<li>${diagnostic.severity} ${escapeHtml(diagnostic.code)}: ${escapeHtml(diagnostic.message)}</li>`
|
||||
)).join("");
|
||||
return `<!doctype html>
|
||||
<html>
|
||||
<head><meta charset="utf-8"><title>${escapeHtml(report.id)}</title></head>
|
||||
<body>
|
||||
<h1>${escapeHtml(report.id)}</h1>
|
||||
<p>Status: ${report.status}</p>
|
||||
<table><thead><tr><th>Section</th><th>Status</th><th>Summary</th></tr></thead><tbody>${rows}</tbody></table>
|
||||
<h2>Source Map</h2>
|
||||
<pre>${escapeHtml(JSON.stringify(report.sourceMap, null, 2))}</pre>
|
||||
<h2>Diagnostics</h2>
|
||||
<ul>${diagnostics}</ul>
|
||||
<script type="application/json" id="validation-report-json">${escapeHtml(stableStringify(report))}</script>
|
||||
</body>
|
||||
</html>`;
|
||||
}
|
||||
|
||||
export function createCustomerDeliveryPackage(input: {
|
||||
project: OlpProjectModel;
|
||||
report: ValidationReport;
|
||||
reportHtml?: string;
|
||||
brandPrograms?: Record<string, string>;
|
||||
ioMap?: Record<string, unknown>;
|
||||
trace?: unknown;
|
||||
}): DeliveryPackage {
|
||||
const files: DeliveryPackageFile[] = [
|
||||
{ path: "project/project.json", content: `${stableStringify(input.project)}\n`, mediaType: "application/json" },
|
||||
{ path: `reports/${input.report.id}.json`, content: `${stableStringify(input.report)}\n`, mediaType: "application/json" },
|
||||
{ path: `reports/${input.report.id}.html`, content: input.reportHtml ?? renderValidationReportHtml(input.report), mediaType: "text/html" },
|
||||
{ path: "calibration/calibrations.json", content: `${stableStringify(input.project.calibrations)}\n`, mediaType: "application/json" }
|
||||
];
|
||||
for (const program of input.project.programs) {
|
||||
files.push({
|
||||
path: `source/${program.id}.${program.language === "grl" ? "grl" : "txt"}`,
|
||||
content: program.source.text,
|
||||
mediaType: "text/plain"
|
||||
});
|
||||
}
|
||||
for (const [filename, text] of Object.entries(input.brandPrograms ?? {})) {
|
||||
files.push({ path: `post/${filename}`, content: text, mediaType: "text/plain" });
|
||||
}
|
||||
if (input.ioMap) {
|
||||
files.push({ path: "io/io_map.json", content: `${stableStringify(input.ioMap)}\n`, mediaType: "application/json" });
|
||||
}
|
||||
if (input.trace) {
|
||||
files.push({ path: "trace/trace.json", content: `${stableStringify(input.trace)}\n`, mediaType: "application/json" });
|
||||
}
|
||||
|
||||
const sortedFiles = files.sort((left, right) => left.path.localeCompare(right.path));
|
||||
return {
|
||||
format: "customer-delivery/0.1",
|
||||
projectId: input.project.project.id,
|
||||
files: sortedFiles,
|
||||
manifest: {
|
||||
sourcePrograms: sortedFiles.filter((file) => file.path.startsWith("source/")).map((file) => file.path),
|
||||
brandPrograms: sortedFiles.filter((file) => file.path.startsWith("post/")).map((file) => file.path),
|
||||
reports: sortedFiles.filter((file) => file.path.startsWith("reports/")).map((file) => file.path),
|
||||
calibrationFiles: sortedFiles.filter((file) => file.path.startsWith("calibration/")).map((file) => file.path),
|
||||
traceFiles: sortedFiles.filter((file) => file.path.startsWith("trace/")).map((file) => file.path),
|
||||
ioMaps: sortedFiles.filter((file) => file.path.startsWith("io/")).map((file) => file.path)
|
||||
},
|
||||
checksum: checksumDeliveryFiles(sortedFiles)
|
||||
};
|
||||
}
|
||||
|
||||
export function validateReportSchema(report: ValidationReport): { ok: boolean; issues: string[] } {
|
||||
const issues: string[] = [];
|
||||
if (report.schemaVersion !== "validation-report/0.1") issues.push("schemaVersion");
|
||||
if (!["pass", "warn", "fail"].includes(report.status)) issues.push("status");
|
||||
for (const section of report.sections) {
|
||||
if (!section.name || !["pass", "warn", "fail"].includes(section.status)) {
|
||||
issues.push(`sections.${section.name || "unknown"}`);
|
||||
}
|
||||
}
|
||||
return {
|
||||
ok: issues.length === 0,
|
||||
issues
|
||||
};
|
||||
}
|
||||
|
||||
export const exportValidationReportHtml = renderValidationReportHtml;
|
||||
export const exportCustomerDeliveryPackage = createCustomerDeliveryPackage;
|
||||
|
||||
export function serializeDeliveryPackage(pkg: DeliveryPackage): string {
|
||||
return `${stableStringify(pkg)}\n`;
|
||||
}
|
||||
|
||||
export function calibrationSummary(calibrations: OlpCalibrationRecord[]): Record<string, number> {
|
||||
return calibrations.reduce<Record<string, number>>((summary, calibration) => {
|
||||
summary[calibration.kind] = (summary[calibration.kind] ?? 0) + 1;
|
||||
return summary;
|
||||
}, {});
|
||||
}
|
||||
|
||||
function reachabilitySection(pathValidation: PathValidationResult | undefined, diagnostics: MotionDiagnostic[]): ValidationReportSection {
|
||||
return {
|
||||
name: "reachability",
|
||||
status: pathValidation ? (pathValidation.ok && pathValidation.reachable ? "pass" : "fail") : statusFromDiagnostics(diagnostics),
|
||||
summary: {
|
||||
reachable: pathValidation?.reachable ?? diagnostics.every((diagnostic) => diagnostic.severity !== "error"),
|
||||
segments: pathValidation?.segmentReports.length ?? 0
|
||||
},
|
||||
diagnostics: [...(pathValidation?.diagnostics ?? []), ...diagnostics]
|
||||
};
|
||||
}
|
||||
|
||||
function cycleTimeSection(pathValidation: PathValidationResult | undefined, trajectories: TrajectoryResult[]): ValidationReportSection {
|
||||
const cycleTime = pathValidation?.cycleTime ?? trajectories.reduce((sum, trajectory) => sum + trajectory.duration, 0);
|
||||
return {
|
||||
name: "cycle_time",
|
||||
status: cycleTime > 0 ? "pass" : "warn",
|
||||
summary: { cycleTime, trajectories: trajectories.length },
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
|
||||
function diagnosticsSection(name: ValidationReportSection["name"], diagnostics: MotionDiagnostic[]): ValidationReportSection {
|
||||
return {
|
||||
name,
|
||||
status: statusFromDiagnostics(diagnostics),
|
||||
summary: { diagnostics: diagnostics.length },
|
||||
diagnostics
|
||||
};
|
||||
}
|
||||
|
||||
function calibrationSection(project: OlpProjectModel): ValidationReportSection {
|
||||
return {
|
||||
name: "calibration",
|
||||
status: project.calibrations.length > 0 ? "pass" : "warn",
|
||||
summary: { records: project.calibrations.length },
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
|
||||
function statusFromDiagnostics(diagnostics: MotionDiagnostic[]): ValidationReportSection["status"] {
|
||||
if (diagnostics.some((diagnostic) => diagnostic.severity === "error")) {
|
||||
return "fail";
|
||||
}
|
||||
if (diagnostics.some((diagnostic) => diagnostic.severity === "warning")) {
|
||||
return "warn";
|
||||
}
|
||||
return "pass";
|
||||
}
|
||||
|
||||
function combineStatus(statuses: ValidationReportSection["status"][]): ValidationReport["status"] {
|
||||
if (statuses.includes("fail")) {
|
||||
return "fail";
|
||||
}
|
||||
if (statuses.includes("warn")) {
|
||||
return "warn";
|
||||
}
|
||||
return "pass";
|
||||
}
|
||||
|
||||
function escapeHtml(value: string): string {
|
||||
return value.replace(/&/g, "&").replace(/</g, "<").replace(/>/g, ">");
|
||||
}
|
||||
|
||||
function checksumDeliveryFiles(files: DeliveryPackageFile[]): string {
|
||||
let hash = 0x811c9dc5;
|
||||
for (const file of files) {
|
||||
const content = `${file.path}\0${file.mediaType ?? ""}\0${file.content}\0`;
|
||||
for (let index = 0; index < content.length; index += 1) {
|
||||
hash ^= content.charCodeAt(index);
|
||||
hash = Math.imul(hash, 0x01000193) >>> 0;
|
||||
}
|
||||
}
|
||||
return `fnv1a32:${hash.toString(16).padStart(8, "0")}`;
|
||||
}
|
||||
|
||||
function stableStringify(value: unknown): string {
|
||||
return JSON.stringify(sortForStableStringify(value), null, 2);
|
||||
}
|
||||
|
||||
function sortForStableStringify(value: unknown): unknown {
|
||||
if (Array.isArray(value)) {
|
||||
return value.map(sortForStableStringify);
|
||||
}
|
||||
if (typeof value === "object" && value !== null) {
|
||||
return Object.fromEntries(
|
||||
Object.entries(value as Record<string, unknown>)
|
||||
.filter(([, item]) => item !== undefined)
|
||||
.sort(([left], [right]) => left.localeCompare(right))
|
||||
.map(([key, item]) => [key, sortForStableStringify(item)])
|
||||
);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
@@ -32,6 +32,7 @@ import type {
|
||||
Pose,
|
||||
PoseTarget,
|
||||
ReachabilityResult,
|
||||
RobotConfiguration,
|
||||
RobotHandle,
|
||||
RobotInfo,
|
||||
SegmentValidationReport,
|
||||
@@ -407,6 +408,8 @@ export class RobotModelRegistry {
|
||||
const targetSamplePose = interpolateLinearPose(startPose, targetPose, sample.s);
|
||||
const ikResult = solveIk(record.model, seed, targetSamplePose, {
|
||||
...request.ik,
|
||||
...(moveLFkOptions(request).tool ? { tool: moveLFkOptions(request).tool } : {}),
|
||||
...(moveLFkOptions(request).frame ? { frame: moveLFkOptions(request).frame } : {}),
|
||||
seeds: [seed]
|
||||
});
|
||||
if (!ikResult.ok || !ikResult.joints) {
|
||||
@@ -531,6 +534,8 @@ export class RobotModelRegistry {
|
||||
const targetSamplePose = sampleCirclePose(arc, sample.s, request.target.pose.quaternion);
|
||||
const ikResult = solveIk(record.model, seed, targetSamplePose, {
|
||||
...request.ik,
|
||||
...(moveCFkOptions(request).tool ? { tool: moveCFkOptions(request).tool } : {}),
|
||||
...(moveCFkOptions(request).frame ? { frame: moveCFkOptions(request).frame } : {}),
|
||||
seeds: [seed]
|
||||
});
|
||||
if (!ikResult.ok || !ikResult.joints) {
|
||||
@@ -1176,7 +1181,7 @@ function interpolateLinearPose(start: Pose, target: Pose, s: number): Pose {
|
||||
start.position[1] + (target.position[1] - start.position[1]) * s,
|
||||
start.position[2] + (target.position[2] - start.position[2]) * s
|
||||
],
|
||||
quaternion: target.quaternion
|
||||
quaternion: slerpQuaternion(start.quaternion, target.quaternion, s)
|
||||
};
|
||||
}
|
||||
|
||||
@@ -1525,6 +1530,10 @@ function solveIk(
|
||||
return solvePlanarRzPxIk(model, seed, target, options);
|
||||
}
|
||||
|
||||
if (chainJoints.length >= 3) {
|
||||
return solveNumericalIk(model, seed, target, options);
|
||||
}
|
||||
|
||||
return {
|
||||
ok: false,
|
||||
iterations: 0,
|
||||
@@ -1533,7 +1542,7 @@ function solveIk(
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_IK_UNSUPPORTED_MODEL",
|
||||
message: "Current TypeScript IK baseline supports only single-prismatic or Rz+Px chains"
|
||||
message: "Current TypeScript IK baseline supports single-prismatic, Rz+Px, and numerical 3+ DOF chains"
|
||||
}
|
||||
]
|
||||
};
|
||||
@@ -1588,6 +1597,324 @@ function solvePlanarRzPxIk(
|
||||
};
|
||||
}
|
||||
|
||||
interface NumericalIkCandidate {
|
||||
ok: boolean;
|
||||
joints: number[];
|
||||
iterations: number;
|
||||
residualPosition: number;
|
||||
residualOrientation: number;
|
||||
score: number;
|
||||
}
|
||||
|
||||
function solveNumericalIk(
|
||||
model: NormalizedRobotModel,
|
||||
seed: number[],
|
||||
target: Pose,
|
||||
options: IkOptions
|
||||
): IkResult {
|
||||
const maxIterations = options.maxIterations ?? 240;
|
||||
const positionTolerance = options.positionTolerance ?? 1e-5;
|
||||
const orientationTolerance = options.orientationTolerance ?? 1e-4;
|
||||
const seeds = numericalIkSeeds(model, seed, options);
|
||||
let best: NumericalIkCandidate | undefined;
|
||||
|
||||
for (const candidateSeed of seeds) {
|
||||
const candidate = iterateNumericalIk(model, candidateSeed, target, {
|
||||
...options,
|
||||
maxIterations,
|
||||
positionTolerance,
|
||||
orientationTolerance
|
||||
});
|
||||
if (!best || candidate.score < best.score) {
|
||||
best = candidate;
|
||||
}
|
||||
if (candidate.ok) {
|
||||
return {
|
||||
ok: true,
|
||||
joints: candidate.joints,
|
||||
iterations: candidate.iterations,
|
||||
residualPosition: candidate.residualPosition,
|
||||
residualOrientation: candidate.residualOrientation,
|
||||
configuration: inferRobotConfiguration(candidate.joints),
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
const result = best ?? iterateNumericalIk(model, seed, target, {
|
||||
...options,
|
||||
maxIterations,
|
||||
positionTolerance,
|
||||
orientationTolerance
|
||||
});
|
||||
const limitDiagnostics = jointLimitDiagnostics(model, result.joints);
|
||||
const outsideHardLimits = limitDiagnostics.length > 0;
|
||||
return {
|
||||
ok: false,
|
||||
joints: result.joints,
|
||||
iterations: result.iterations,
|
||||
residualPosition: result.residualPosition,
|
||||
residualOrientation: result.residualOrientation,
|
||||
reason: outsideHardLimits ? "joint_limit" : "unreachable",
|
||||
diagnostics: outsideHardLimits
|
||||
? limitDiagnostics
|
||||
: [
|
||||
{
|
||||
severity: "error",
|
||||
code: "KDL_TARGET_UNREACHABLE",
|
||||
message: `IK residual position=${result.residualPosition} orientation=${result.residualOrientation} exceeds tolerances ${positionTolerance}/${orientationTolerance}`,
|
||||
data: {
|
||||
residualPosition: result.residualPosition,
|
||||
residualOrientation: result.residualOrientation,
|
||||
iterations: result.iterations
|
||||
}
|
||||
}
|
||||
]
|
||||
};
|
||||
}
|
||||
|
||||
function iterateNumericalIk(
|
||||
model: NormalizedRobotModel,
|
||||
seed: number[],
|
||||
target: Pose,
|
||||
options: IkOptions & {
|
||||
maxIterations: number;
|
||||
positionTolerance: number;
|
||||
orientationTolerance: number;
|
||||
}
|
||||
): NumericalIkCandidate {
|
||||
const dof = model.activeJointNames.length;
|
||||
let joints = clampJointsToLimits(model, normalizeSeed(seed, dof), options);
|
||||
let bestJoints = [...joints];
|
||||
let bestError = ikErrorVector(model, joints, target, options);
|
||||
let bestScore = ikErrorScore(bestError);
|
||||
let damping = 0.04;
|
||||
const epsilon = 1e-5;
|
||||
|
||||
for (let iteration = 0; iteration < options.maxIterations; iteration += 1) {
|
||||
const error = ikErrorVector(model, joints, target, options);
|
||||
const residualPosition = Math.hypot(error[0]!, error[1]!, error[2]!);
|
||||
const residualOrientation = Math.hypot(error[3]!, error[4]!, error[5]!);
|
||||
const score = ikErrorScore(error);
|
||||
if (score < bestScore) {
|
||||
bestScore = score;
|
||||
bestError = error;
|
||||
bestJoints = [...joints];
|
||||
}
|
||||
if (residualPosition <= options.positionTolerance && residualOrientation <= options.orientationTolerance) {
|
||||
return {
|
||||
ok: true,
|
||||
joints,
|
||||
iterations: iteration + 1,
|
||||
residualPosition,
|
||||
residualOrientation,
|
||||
score
|
||||
};
|
||||
}
|
||||
|
||||
const jacobian = numericalIkJacobian(model, joints, target, options, error, epsilon);
|
||||
const delta = dampedLeastSquaresStep(jacobian, error, damping);
|
||||
if (Math.hypot(...delta) < 1e-10) {
|
||||
damping *= 2;
|
||||
continue;
|
||||
}
|
||||
|
||||
const next = bestLineSearchStep(model, joints, target, options, delta, score);
|
||||
if (next.accepted || options.allowApproximate) {
|
||||
joints = next.joints;
|
||||
damping = Math.max(0.005, damping * 0.8);
|
||||
} else {
|
||||
damping = Math.min(1, damping * 2);
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
ok: options.allowApproximate === true,
|
||||
joints: bestJoints,
|
||||
iterations: options.maxIterations,
|
||||
residualPosition: Math.hypot(bestError[0]!, bestError[1]!, bestError[2]!),
|
||||
residualOrientation: Math.hypot(bestError[3]!, bestError[4]!, bestError[5]!),
|
||||
score: bestScore
|
||||
};
|
||||
}
|
||||
|
||||
function numericalIkSeeds(model: NormalizedRobotModel, seed: number[], options: IkOptions): number[][] {
|
||||
const dof = model.activeJointNames.length;
|
||||
const seeds = [
|
||||
...(options.seeds ?? []),
|
||||
seed,
|
||||
model.limits.map((limit) => finiteMidpoint(limit.lower, limit.upper)),
|
||||
model.limits.map((limit) => clamp(0, limit.lower, limit.upper))
|
||||
].map((item) => clampJointsToLimits(model, normalizeSeed(item, dof), options));
|
||||
|
||||
const offsets = [
|
||||
[0.15, -0.25, 0.25, 0.1, -0.1, 0.15],
|
||||
[-0.25, -0.35, 0.35, -0.15, 0.15, -0.25],
|
||||
[0.35, -0.6, 0.7, 0.25, -0.25, 0.35],
|
||||
[-0.6, 0.4, -0.35, 0.4, 0.25, -0.4]
|
||||
];
|
||||
for (const offset of offsets) {
|
||||
seeds.push(clampJointsToLimits(model, normalizeSeed(seed, dof).map((value, index) => value + (offset[index] ?? 0)), options));
|
||||
}
|
||||
|
||||
const unique: number[][] = [];
|
||||
for (const item of seeds) {
|
||||
const key = item.map((value) => value.toFixed(6)).join(",");
|
||||
if (!unique.some((existing) => existing.map((value) => value.toFixed(6)).join(",") === key)) {
|
||||
unique.push(item);
|
||||
}
|
||||
}
|
||||
return unique;
|
||||
}
|
||||
|
||||
function numericalIkJacobian(
|
||||
model: NormalizedRobotModel,
|
||||
joints: number[],
|
||||
target: Pose,
|
||||
options: IkOptions,
|
||||
baseError: number[],
|
||||
epsilon: number
|
||||
): number[][] {
|
||||
const rows = 6;
|
||||
const cols = joints.length;
|
||||
const jacobian = Array.from({ length: rows }, () => new Array(cols).fill(0));
|
||||
for (let col = 0; col < cols; col += 1) {
|
||||
const perturbed = [...joints];
|
||||
perturbed[col] = (perturbed[col] ?? 0) + epsilon;
|
||||
const error = ikErrorVector(model, perturbed, target, options);
|
||||
for (let row = 0; row < rows; row += 1) {
|
||||
jacobian[row]![col] = (error[row]! - baseError[row]!) / epsilon;
|
||||
}
|
||||
}
|
||||
return jacobian;
|
||||
}
|
||||
|
||||
function dampedLeastSquaresStep(jacobian: number[][], error: number[], damping: number): number[] {
|
||||
const jt = transpose(jacobian);
|
||||
const jtj = multiplyMatrix(jt, jacobian);
|
||||
const rhs = multiplyMatrixVector(jt, error);
|
||||
const a = jtj.map((row, index) =>
|
||||
row.map((value, col) => value + (index === col ? damping * damping : 0))
|
||||
);
|
||||
return solveLinearSystem(a, rhs).map((value) => -value);
|
||||
}
|
||||
|
||||
function bestLineSearchStep(
|
||||
model: NormalizedRobotModel,
|
||||
joints: number[],
|
||||
target: Pose,
|
||||
options: IkOptions,
|
||||
delta: number[],
|
||||
currentScore: number
|
||||
): { accepted: boolean; joints: number[]; score: number } {
|
||||
let best = {
|
||||
accepted: false,
|
||||
joints,
|
||||
score: currentScore
|
||||
};
|
||||
for (const scale of [1, 0.5, 0.25, 0.1, 0.05]) {
|
||||
const candidate = clampJointsToLimits(
|
||||
model,
|
||||
joints.map((joint, index) => joint + clamp((delta[index] ?? 0) * scale, -0.35, 0.35)),
|
||||
options
|
||||
);
|
||||
const score = ikErrorScore(ikErrorVector(model, candidate, target, options));
|
||||
if (score < best.score) {
|
||||
best = {
|
||||
accepted: true,
|
||||
joints: candidate,
|
||||
score
|
||||
};
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
function ikErrorVector(model: NormalizedRobotModel, joints: number[], target: Pose, options: IkOptions): number[] {
|
||||
const actual = tcpPoseForIk(model, joints, options);
|
||||
const orientation = orientationErrorVector(actual.quaternion, target.quaternion);
|
||||
const orientationWeight = 1;
|
||||
return [
|
||||
target.position[0] - actual.position[0],
|
||||
target.position[1] - actual.position[1],
|
||||
target.position[2] - actual.position[2],
|
||||
orientation[0] * orientationWeight,
|
||||
orientation[1] * orientationWeight,
|
||||
orientation[2] * orientationWeight
|
||||
];
|
||||
}
|
||||
|
||||
function ikErrorScore(error: number[]): number {
|
||||
return Math.hypot(...error);
|
||||
}
|
||||
|
||||
function tcpPoseForIk(model: NormalizedRobotModel, joints: number[], options: IkOptions): Pose {
|
||||
const flange = computeLinkPoses(model, joints).at(-1)?.pose ?? identityPose();
|
||||
const tcpWithTool = options.tool ? composeFkPose(flange, options.tool) : flange;
|
||||
return options.frame ? composeFkPose(options.frame, tcpWithTool) : tcpWithTool;
|
||||
}
|
||||
|
||||
function orientationErrorVector(
|
||||
actual: [number, number, number, number],
|
||||
target: [number, number, number, number]
|
||||
): [number, number, number] {
|
||||
const current = normalizeQuaternionForIk(actual);
|
||||
let desired = normalizeQuaternionForIk(target);
|
||||
if (dotQuaternion(current, desired) < 0) {
|
||||
desired = [-desired[0], -desired[1], -desired[2], -desired[3]];
|
||||
}
|
||||
const delta = multiplyQuaternion(desired, inverseQuaternion(current));
|
||||
const normalized = normalizeQuaternionForIk(delta);
|
||||
const sinHalf = Math.hypot(normalized[0], normalized[1], normalized[2]);
|
||||
if (sinHalf < 1e-12) {
|
||||
return [0, 0, 0];
|
||||
}
|
||||
const angle = 2 * Math.atan2(sinHalf, normalized[3]);
|
||||
return [
|
||||
normalized[0] / sinHalf * angle,
|
||||
normalized[1] / sinHalf * angle,
|
||||
normalized[2] / sinHalf * angle
|
||||
];
|
||||
}
|
||||
|
||||
function normalizeSeed(seed: number[], dof: number): number[] {
|
||||
return Array.from({ length: dof }, (_, index) => {
|
||||
const value = seed[index] ?? 0;
|
||||
return Number.isFinite(value) ? value : 0;
|
||||
});
|
||||
}
|
||||
|
||||
function clampJointsToLimits(model: NormalizedRobotModel, joints: number[], options: IkOptions): number[] {
|
||||
return joints.map((value, index) => {
|
||||
const limits = model.limits[index];
|
||||
const lower = options.qMin?.[index] ?? limits?.lower ?? -Infinity;
|
||||
const upper = options.qMax?.[index] ?? limits?.upper ?? Infinity;
|
||||
return clamp(value, lower, upper);
|
||||
});
|
||||
}
|
||||
|
||||
function finiteMidpoint(lower: number, upper: number): number {
|
||||
if (Number.isFinite(lower) && Number.isFinite(upper)) {
|
||||
return (lower + upper) / 2;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
function inferRobotConfiguration(joints: number[]): RobotConfiguration {
|
||||
return {
|
||||
shoulder: signConfig(joints[0] ?? 0),
|
||||
elbow: signConfig(joints[2] ?? 0),
|
||||
wrist: signConfig(joints[4] ?? 0),
|
||||
turnNumbers: joints.map((joint) => Math.trunc(joint / (Math.PI * 2)))
|
||||
};
|
||||
}
|
||||
|
||||
function signConfig(value: number): -1 | 0 | 1 {
|
||||
if (value > 1e-9) return 1;
|
||||
if (value < -1e-9) return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
function finishIkCandidate(
|
||||
model: NormalizedRobotModel,
|
||||
seed: number[],
|
||||
@@ -1720,6 +2047,71 @@ function normalizeAngleNear(angle: number, seed: number): number {
|
||||
return normalized;
|
||||
}
|
||||
|
||||
function slerpQuaternion(
|
||||
a: [number, number, number, number],
|
||||
b: [number, number, number, number],
|
||||
s: number
|
||||
): [number, number, number, number] {
|
||||
const qa = normalizeQuaternionForIk(a);
|
||||
let qb = normalizeQuaternionForIk(b);
|
||||
let dot = dotQuaternion(qa, qb);
|
||||
if (dot < 0) {
|
||||
qb = [-qb[0], -qb[1], -qb[2], -qb[3]];
|
||||
dot = -dot;
|
||||
}
|
||||
if (dot > 0.9995) {
|
||||
return normalizeQuaternionForIk([
|
||||
qa[0] + (qb[0] - qa[0]) * s,
|
||||
qa[1] + (qb[1] - qa[1]) * s,
|
||||
qa[2] + (qb[2] - qa[2]) * s,
|
||||
qa[3] + (qb[3] - qa[3]) * s
|
||||
]);
|
||||
}
|
||||
const theta0 = Math.acos(clamp(dot, -1, 1));
|
||||
const theta = theta0 * s;
|
||||
const sinTheta = Math.sin(theta);
|
||||
const sinTheta0 = Math.sin(theta0);
|
||||
const scaleA = Math.cos(theta) - dot * sinTheta / sinTheta0;
|
||||
const scaleB = sinTheta / sinTheta0;
|
||||
return normalizeQuaternionForIk([
|
||||
qa[0] * scaleA + qb[0] * scaleB,
|
||||
qa[1] * scaleA + qb[1] * scaleB,
|
||||
qa[2] * scaleA + qb[2] * scaleB,
|
||||
qa[3] * scaleA + qb[3] * scaleB
|
||||
]);
|
||||
}
|
||||
|
||||
function normalizeQuaternionForIk(input: [number, number, number, number]): [number, number, number, number] {
|
||||
const length = Math.hypot(input[0], input[1], input[2], input[3]);
|
||||
if (length <= 1e-12) {
|
||||
return [0, 0, 0, 1];
|
||||
}
|
||||
return [input[0] / length, input[1] / length, input[2] / length, input[3] / length];
|
||||
}
|
||||
|
||||
function dotQuaternion(
|
||||
a: [number, number, number, number],
|
||||
b: [number, number, number, number]
|
||||
): number {
|
||||
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
|
||||
}
|
||||
|
||||
function inverseQuaternion(q: [number, number, number, number]): [number, number, number, number] {
|
||||
return [-q[0], -q[1], -q[2], q[3]];
|
||||
}
|
||||
|
||||
function multiplyQuaternion(
|
||||
a: [number, number, number, number],
|
||||
b: [number, number, number, number]
|
||||
): [number, number, number, number] {
|
||||
return [
|
||||
a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
|
||||
a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0],
|
||||
a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3],
|
||||
a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]
|
||||
];
|
||||
}
|
||||
|
||||
function distance(a: [number, number, number], b: [number, number, number]): number {
|
||||
return Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
|
||||
}
|
||||
@@ -1758,6 +2150,52 @@ function multiplyMatrix(a: number[][], b: number[][]): number[][] {
|
||||
);
|
||||
}
|
||||
|
||||
function multiplyMatrixVector(matrix: number[][], vector: number[]): number[] {
|
||||
return matrix.map((row) => row.reduce((sum, value, index) => sum + value * (vector[index] ?? 0), 0));
|
||||
}
|
||||
|
||||
function solveLinearSystem(matrix: number[][], rhs: number[]): number[] {
|
||||
const n = rhs.length;
|
||||
const a = matrix.map((row, rowIndex) => [
|
||||
...Array.from({ length: n }, (_, col) => row[col] ?? 0),
|
||||
rhs[rowIndex] ?? 0
|
||||
]);
|
||||
|
||||
for (let pivot = 0; pivot < n; pivot += 1) {
|
||||
let best = pivot;
|
||||
for (let row = pivot + 1; row < n; row += 1) {
|
||||
if (Math.abs(a[row]?.[pivot] ?? 0) > Math.abs(a[best]?.[pivot] ?? 0)) {
|
||||
best = row;
|
||||
}
|
||||
}
|
||||
if (best !== pivot) {
|
||||
const tmp = a[pivot]!;
|
||||
a[pivot] = a[best]!;
|
||||
a[best] = tmp;
|
||||
}
|
||||
|
||||
const pivotValue = a[pivot]?.[pivot] ?? 0;
|
||||
if (Math.abs(pivotValue) < 1e-12) {
|
||||
a[pivot]![pivot] = pivotValue >= 0 ? 1e-12 : -1e-12;
|
||||
}
|
||||
const divisor = a[pivot]![pivot]!;
|
||||
for (let col = pivot; col <= n; col += 1) {
|
||||
a[pivot]![col] = (a[pivot]![col] ?? 0) / divisor;
|
||||
}
|
||||
for (let row = 0; row < n; row += 1) {
|
||||
if (row === pivot) {
|
||||
continue;
|
||||
}
|
||||
const factor = a[row]?.[pivot] ?? 0;
|
||||
for (let col = pivot; col <= n; col += 1) {
|
||||
a[row]![col] = (a[row]![col] ?? 0) - factor * (a[pivot]![col] ?? 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return a.map((row) => row[n] ?? 0);
|
||||
}
|
||||
|
||||
function determinant3(matrix: number[][]): number {
|
||||
const a = matrix[0]?.[0] ?? 0;
|
||||
const b = matrix[0]?.[1] ?? 0;
|
||||
@@ -1771,6 +2209,10 @@ function determinant3(matrix: number[][]): number {
|
||||
return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
|
||||
}
|
||||
|
||||
function clamp(value: number, lower: number, upper: number): number {
|
||||
return Math.min(Math.max(value, lower), upper);
|
||||
}
|
||||
|
||||
function estimateConditionNumber(matrix: number[][]): number {
|
||||
const columnNorms = transpose(matrix).map((column) => Math.hypot(...column));
|
||||
const nonZero = columnNorms.filter((value) => value > 1e-12);
|
||||
|
||||
35
kdl-wasm/web/src/runtime/index.ts
Normal file
35
kdl-wasm/web/src/runtime/index.ts
Normal file
@@ -0,0 +1,35 @@
|
||||
export {
|
||||
KdlRuntimeBridge,
|
||||
operationIdFromAction,
|
||||
type KdlBridgeSnapshot
|
||||
} from "./kdlBridge.js";
|
||||
export {
|
||||
MotionQueue,
|
||||
type MotionPlanner,
|
||||
type MotionPlaybackSample,
|
||||
type MotionQueueItem,
|
||||
type MotionQueueOptions,
|
||||
type MotionQueueSnapshot,
|
||||
type MotionQueueSource
|
||||
} from "./motionQueue.js";
|
||||
export {
|
||||
IoImage,
|
||||
WaitRegistry,
|
||||
applyPulse,
|
||||
runIoScript,
|
||||
type IoEdge,
|
||||
type IoEvent,
|
||||
type IoPermissionRule,
|
||||
type IoScriptStep,
|
||||
type IoWriter,
|
||||
type WaitResult
|
||||
} from "./io.js";
|
||||
export {
|
||||
IoImageRuntime,
|
||||
ioKey,
|
||||
parseIoReferenceText,
|
||||
type IoRuntimeOptions,
|
||||
type IoRuntimeSnapshot,
|
||||
type WaitEvaluation,
|
||||
type WaitStatus
|
||||
} from "./ioRuntime.js";
|
||||
129
kdl-wasm/web/src/runtime/io.ts
Normal file
129
kdl-wasm/web/src/runtime/io.ts
Normal file
@@ -0,0 +1,129 @@
|
||||
import type { IoReference, PulseInstruction, WaitInstruction } from "../grl/ir/index.js";
|
||||
import type { RuntimeTraceEvent } from "../controller/index.js";
|
||||
import { evaluateRuntimeBoolean } from "../controller/index.js";
|
||||
|
||||
export type IoWriter = "program" | "user" | "script";
|
||||
export type IoEdge = "rising" | "falling" | "changed";
|
||||
|
||||
export interface IoEvent {
|
||||
time: number;
|
||||
writer: IoWriter;
|
||||
key: string;
|
||||
previous: unknown;
|
||||
value: unknown;
|
||||
edge?: IoEdge;
|
||||
}
|
||||
|
||||
export interface IoPermissionRule {
|
||||
key: string;
|
||||
writers: IoWriter[];
|
||||
}
|
||||
|
||||
export class IoImage {
|
||||
private readonly values = new Map<string, unknown>();
|
||||
private readonly eventsValue: IoEvent[] = [];
|
||||
private readonly rules = new Map<string, Set<IoWriter>>();
|
||||
|
||||
setPermission(key: string, writers: IoWriter[]): void {
|
||||
this.rules.set(key, new Set(writers));
|
||||
}
|
||||
|
||||
read(ref: IoReference | string): unknown {
|
||||
return this.values.get(ioKey(ref));
|
||||
}
|
||||
|
||||
write(ref: IoReference | string, value: unknown, writer: IoWriter, time = 0): IoEvent {
|
||||
const key = ioKey(ref);
|
||||
const allowed = this.rules.get(key);
|
||||
if (allowed && !allowed.has(writer)) {
|
||||
throw new Error(`Writer ${writer} cannot write ${key}`);
|
||||
}
|
||||
const previous = this.values.get(key);
|
||||
this.values.set(key, value);
|
||||
const edge = edgeOf(previous, value);
|
||||
const event: IoEvent = {
|
||||
time,
|
||||
writer,
|
||||
key,
|
||||
previous,
|
||||
value,
|
||||
...(edge ? { edge } : {})
|
||||
};
|
||||
this.eventsValue.push(event);
|
||||
return event;
|
||||
}
|
||||
|
||||
variables(): Record<string, unknown> {
|
||||
return Object.fromEntries([...this.values.entries()]);
|
||||
}
|
||||
|
||||
events(): IoEvent[] {
|
||||
return this.eventsValue.map((event) => ({ ...event }));
|
||||
}
|
||||
}
|
||||
|
||||
export interface WaitResult {
|
||||
status: "satisfied" | "waiting" | "timeout" | "hold" | "stop";
|
||||
condition: string;
|
||||
elapsed: number;
|
||||
trace?: RuntimeTraceEvent;
|
||||
}
|
||||
|
||||
export class WaitRegistry {
|
||||
evaluate(wait: WaitInstruction, io: IoImage, elapsed: number): WaitResult {
|
||||
if (evaluateRuntimeBoolean(wait.condition, io.variables())) {
|
||||
return { status: "satisfied", condition: wait.condition, elapsed };
|
||||
}
|
||||
if (wait.timeout !== undefined && elapsed >= wait.timeout) {
|
||||
if (wait.onTimeout?.kind === "alarm") {
|
||||
return { status: "hold", condition: wait.condition, elapsed };
|
||||
}
|
||||
if (wait.onTimeout?.kind === "call") {
|
||||
return { status: "stop", condition: wait.condition, elapsed };
|
||||
}
|
||||
return { status: "timeout", condition: wait.condition, elapsed };
|
||||
}
|
||||
return { status: "waiting", condition: wait.condition, elapsed };
|
||||
}
|
||||
}
|
||||
|
||||
export function applyPulse(io: IoImage, pulse: PulseInstruction, writer: IoWriter = "program", startTime = 0): IoEvent[] {
|
||||
return [
|
||||
io.write(pulse.target, true, writer, startTime),
|
||||
io.write(pulse.target, false, writer, startTime + pulse.duration)
|
||||
];
|
||||
}
|
||||
|
||||
export interface IoScriptStep {
|
||||
delay: number;
|
||||
target: IoReference | string;
|
||||
value: unknown;
|
||||
}
|
||||
|
||||
export function runIoScript(io: IoImage, script: IoScriptStep[], startTime = 0): IoEvent[] {
|
||||
let time = startTime;
|
||||
return script.map((step) => {
|
||||
time += step.delay;
|
||||
return io.write(step.target, step.value, "script", time);
|
||||
});
|
||||
}
|
||||
|
||||
function ioKey(ref: IoReference | string): string {
|
||||
if (typeof ref === "string") {
|
||||
return ref;
|
||||
}
|
||||
return ref.raw;
|
||||
}
|
||||
|
||||
function edgeOf(previous: unknown, value: unknown): IoEdge | undefined {
|
||||
if (previous === value) {
|
||||
return undefined;
|
||||
}
|
||||
if (previous === false && value === true) {
|
||||
return "rising";
|
||||
}
|
||||
if (previous === true && value === false) {
|
||||
return "falling";
|
||||
}
|
||||
return "changed";
|
||||
}
|
||||
413
kdl-wasm/web/src/runtime/ioRuntime.ts
Normal file
413
kdl-wasm/web/src/runtime/ioRuntime.ts
Normal file
@@ -0,0 +1,413 @@
|
||||
import type { IoDomain, IoFlowInstruction, IoReference, IoWriteInstruction, PulseInstruction, WaitInstruction } from "../grl/ir/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
|
||||
export type IoWriter = "program" | "user" | "script";
|
||||
|
||||
export interface IoPermissionRule {
|
||||
writer: IoWriter;
|
||||
domains: IoDomain[];
|
||||
access: "read" | "write" | "readwrite";
|
||||
}
|
||||
|
||||
export interface IoEvent {
|
||||
id: number;
|
||||
time: number;
|
||||
writer: IoWriter;
|
||||
target: IoReference;
|
||||
previous: boolean | number | string | undefined;
|
||||
value: boolean | number | string;
|
||||
kind: "write" | "pulse_set" | "pulse_reset" | "script";
|
||||
}
|
||||
|
||||
export interface IoScriptStep {
|
||||
delay: number;
|
||||
target: IoReference;
|
||||
value: boolean | number | string;
|
||||
}
|
||||
|
||||
export interface IoScriptExecution {
|
||||
name: string;
|
||||
nextIndex: number;
|
||||
elapsed: number;
|
||||
steps: IoScriptStep[];
|
||||
done: boolean;
|
||||
}
|
||||
|
||||
export type WaitStatus = "satisfied" | "waiting" | "timeout" | "hold-stop";
|
||||
|
||||
export interface WaitEvaluation {
|
||||
status: WaitStatus;
|
||||
elapsed: number;
|
||||
instruction: WaitInstruction;
|
||||
diagnostic?: MotionDiagnostic;
|
||||
onTimeout?: WaitInstruction["onTimeout"];
|
||||
}
|
||||
|
||||
export interface IoRuntimeSnapshot {
|
||||
time: number;
|
||||
image: Record<string, boolean | number | string>;
|
||||
events: IoEvent[];
|
||||
diagnostics: MotionDiagnostic[];
|
||||
scripts: IoScriptExecution[];
|
||||
}
|
||||
|
||||
export interface IoRuntimeOptions {
|
||||
permissions?: IoPermissionRule[];
|
||||
}
|
||||
|
||||
interface PulseSchedule {
|
||||
dueTime: number;
|
||||
target: IoReference;
|
||||
writer: IoWriter;
|
||||
}
|
||||
|
||||
export class IoImageRuntime {
|
||||
private readonly image = new Map<string, boolean | number | string>();
|
||||
private readonly previousImage = new Map<string, boolean | number | string>();
|
||||
private readonly eventsValue: IoEvent[] = [];
|
||||
private readonly diagnosticsValue: MotionDiagnostic[] = [];
|
||||
private readonly pulses: PulseSchedule[] = [];
|
||||
private readonly scriptsValue: IoScriptExecution[] = [];
|
||||
private nextEventId = 1;
|
||||
private timeValue = 0;
|
||||
|
||||
constructor(private readonly options: IoRuntimeOptions = {}) {}
|
||||
|
||||
get time(): number {
|
||||
return this.timeValue;
|
||||
}
|
||||
|
||||
get events(): IoEvent[] {
|
||||
return this.eventsValue.map((event) => ({ ...event, target: { ...event.target } }));
|
||||
}
|
||||
|
||||
get diagnostics(): MotionDiagnostic[] {
|
||||
return [...this.diagnosticsValue];
|
||||
}
|
||||
|
||||
read(reference: IoReference): boolean | number | string | undefined {
|
||||
return this.image.get(ioKey(reference));
|
||||
}
|
||||
|
||||
write(
|
||||
reference: IoReference,
|
||||
value: boolean | number | string,
|
||||
writer: IoWriter = "program",
|
||||
kind: IoEvent["kind"] = "write"
|
||||
): IoEvent | undefined {
|
||||
if (!this.canWrite(reference, writer)) {
|
||||
this.diagnosticsValue.push({
|
||||
severity: "error",
|
||||
code: "VC_IO_PERMISSION_DENIED",
|
||||
message: `${writer} cannot write ${reference.raw}`,
|
||||
data: { writer, target: reference }
|
||||
});
|
||||
return undefined;
|
||||
}
|
||||
const key = ioKey(reference);
|
||||
const previous = this.image.get(key);
|
||||
if (previous === undefined) {
|
||||
this.previousImage.delete(key);
|
||||
} else {
|
||||
this.previousImage.set(key, previous);
|
||||
}
|
||||
this.image.set(key, value);
|
||||
const event: IoEvent = {
|
||||
id: this.nextEventId,
|
||||
time: this.timeValue,
|
||||
writer,
|
||||
target: { ...reference },
|
||||
previous,
|
||||
value,
|
||||
kind
|
||||
};
|
||||
this.nextEventId += 1;
|
||||
this.eventsValue.push(event);
|
||||
return event;
|
||||
}
|
||||
|
||||
executeInstruction(instruction: IoFlowInstruction, writer: IoWriter = "program"): WaitEvaluation | IoEvent[] | undefined {
|
||||
if (instruction.kind === "IO_WRITE") {
|
||||
const event = this.executeWrite(instruction, writer);
|
||||
return event ? [event] : [];
|
||||
}
|
||||
if (instruction.kind === "PULSE") {
|
||||
return this.executePulse(instruction, writer);
|
||||
}
|
||||
return this.evaluateWait(instruction, 0);
|
||||
}
|
||||
|
||||
executeWrite(instruction: IoWriteInstruction, writer: IoWriter = "program"): IoEvent | undefined {
|
||||
return this.write(instruction.target, instruction.value, writer);
|
||||
}
|
||||
|
||||
executePulse(instruction: PulseInstruction, writer: IoWriter = "program"): IoEvent[] {
|
||||
const events: IoEvent[] = [];
|
||||
const setEvent = this.write(instruction.target, true, writer, "pulse_set");
|
||||
if (setEvent) {
|
||||
events.push(setEvent);
|
||||
this.pulses.push({
|
||||
dueTime: this.timeValue + instruction.duration,
|
||||
target: instruction.target,
|
||||
writer
|
||||
});
|
||||
}
|
||||
return events;
|
||||
}
|
||||
|
||||
evaluateWait(instruction: WaitInstruction, elapsed: number): WaitEvaluation {
|
||||
if (this.matchesCondition(instruction.condition)) {
|
||||
return {
|
||||
status: "satisfied",
|
||||
elapsed,
|
||||
instruction
|
||||
};
|
||||
}
|
||||
if (instruction.timeout !== undefined && elapsed >= instruction.timeout) {
|
||||
const diagnostic: MotionDiagnostic = {
|
||||
severity: instruction.onTimeout ? "warning" : "error",
|
||||
code: "VC_WAIT_TIMEOUT",
|
||||
message: `Wait timed out after ${instruction.timeout}s`,
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
|
||||
data: { condition: instruction.condition }
|
||||
};
|
||||
this.diagnosticsValue.push(diagnostic);
|
||||
return {
|
||||
status: instruction.onTimeout?.kind === "call" ? "hold-stop" : "timeout",
|
||||
elapsed,
|
||||
instruction,
|
||||
diagnostic,
|
||||
...(instruction.onTimeout ? { onTimeout: instruction.onTimeout } : {})
|
||||
};
|
||||
}
|
||||
return {
|
||||
status: "waiting",
|
||||
elapsed,
|
||||
instruction
|
||||
};
|
||||
}
|
||||
|
||||
addScript(name: string, steps: IoScriptStep[]): IoScriptExecution {
|
||||
const script: IoScriptExecution = {
|
||||
name,
|
||||
nextIndex: 0,
|
||||
elapsed: 0,
|
||||
steps: steps.map((step) => ({ ...step, target: { ...step.target } })),
|
||||
done: steps.length === 0
|
||||
};
|
||||
this.scriptsValue.push(script);
|
||||
return { ...script, steps: script.steps.map((step) => ({ ...step, target: { ...step.target } })) };
|
||||
}
|
||||
|
||||
advance(deltaTime: number): void {
|
||||
if (!Number.isFinite(deltaTime) || deltaTime < 0) {
|
||||
this.diagnosticsValue.push({
|
||||
severity: "error",
|
||||
code: "VC_IO_TIME_INVALID",
|
||||
message: "IO runtime delta time must be a finite non-negative number"
|
||||
});
|
||||
return;
|
||||
}
|
||||
this.timeValue += deltaTime;
|
||||
this.flushPulses();
|
||||
this.advanceScripts(deltaTime);
|
||||
}
|
||||
|
||||
matchesCondition(condition: string): boolean {
|
||||
const normalized = normalizeCondition(condition);
|
||||
const allMatch = unwrapCall(normalized, "all");
|
||||
if (allMatch) {
|
||||
return splitTopLevel(allMatch).every((part) => this.matchesCondition(part));
|
||||
}
|
||||
const anyMatch = unwrapCall(normalized, "any");
|
||||
if (anyMatch) {
|
||||
return splitTopLevel(anyMatch).some((part) => this.matchesCondition(part));
|
||||
}
|
||||
const rising = unwrapCall(normalized, "rising");
|
||||
if (rising) {
|
||||
const reference = parseIoReferenceText(rising);
|
||||
return Boolean(reference && this.previousValue(reference) !== true && this.read(reference) === true);
|
||||
}
|
||||
const falling = unwrapCall(normalized, "falling");
|
||||
if (falling) {
|
||||
const reference = parseIoReferenceText(falling);
|
||||
return Boolean(reference && this.previousValue(reference) === true && this.read(reference) !== true);
|
||||
}
|
||||
const changed = unwrapCall(normalized, "changed");
|
||||
if (changed) {
|
||||
const reference = parseIoReferenceText(changed);
|
||||
return Boolean(reference && this.previousValue(reference) !== this.read(reference));
|
||||
}
|
||||
|
||||
const comparison = normalized.match(/^(io\.[A-Za-z]+\[\d+\]|io\.alias\.[A-Za-z_][A-Za-z0-9_]*)\s*(==|!=|>=|<=|>|<)\s*(.+)$/);
|
||||
if (!comparison) {
|
||||
return false;
|
||||
}
|
||||
const reference = parseIoReferenceText(comparison[1]!);
|
||||
if (!reference) {
|
||||
return false;
|
||||
}
|
||||
const left = this.read(reference);
|
||||
const right = parseConditionValue(comparison[3]!);
|
||||
switch (comparison[2]) {
|
||||
case "==":
|
||||
return left === right;
|
||||
case "!=":
|
||||
return left !== right;
|
||||
case ">=":
|
||||
return Number(left) >= Number(right);
|
||||
case "<=":
|
||||
return Number(left) <= Number(right);
|
||||
case ">":
|
||||
return Number(left) > Number(right);
|
||||
case "<":
|
||||
return Number(left) < Number(right);
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
snapshot(): IoRuntimeSnapshot {
|
||||
const image: Record<string, boolean | number | string> = {};
|
||||
for (const [key, value] of this.image.entries()) {
|
||||
image[key] = value;
|
||||
}
|
||||
return {
|
||||
time: this.timeValue,
|
||||
image,
|
||||
events: this.events,
|
||||
diagnostics: this.diagnostics,
|
||||
scripts: this.scriptsValue.map((script) => ({
|
||||
...script,
|
||||
steps: script.steps.map((step) => ({ ...step, target: { ...step.target } }))
|
||||
}))
|
||||
};
|
||||
}
|
||||
|
||||
private canWrite(reference: IoReference, writer: IoWriter): boolean {
|
||||
const rules = this.options.permissions;
|
||||
if (!rules || rules.length === 0) {
|
||||
return true;
|
||||
}
|
||||
return rules.some((rule) =>
|
||||
rule.writer === writer &&
|
||||
rule.domains.includes(reference.domain) &&
|
||||
(rule.access === "write" || rule.access === "readwrite")
|
||||
);
|
||||
}
|
||||
|
||||
private previousValue(reference: IoReference): boolean | number | string | undefined {
|
||||
return this.previousImage.get(ioKey(reference));
|
||||
}
|
||||
|
||||
private flushPulses(): void {
|
||||
const due = this.pulses.filter((pulse) => pulse.dueTime <= this.timeValue);
|
||||
for (const pulse of due) {
|
||||
this.write(pulse.target, false, pulse.writer, "pulse_reset");
|
||||
}
|
||||
for (const pulse of due) {
|
||||
const index = this.pulses.indexOf(pulse);
|
||||
if (index >= 0) {
|
||||
this.pulses.splice(index, 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private advanceScripts(deltaTime: number): void {
|
||||
for (const script of this.scriptsValue) {
|
||||
if (script.done) {
|
||||
continue;
|
||||
}
|
||||
script.elapsed += deltaTime;
|
||||
while (script.nextIndex < script.steps.length) {
|
||||
const step = script.steps[script.nextIndex]!;
|
||||
if (script.elapsed + 1e-12 < step.delay) {
|
||||
break;
|
||||
}
|
||||
this.write(step.target, step.value, "script", "script");
|
||||
script.nextIndex += 1;
|
||||
}
|
||||
script.done = script.nextIndex >= script.steps.length;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
export function ioKey(reference: IoReference): string {
|
||||
if (reference.domain === "alias") {
|
||||
return `io.alias.${reference.alias ?? reference.raw}`;
|
||||
}
|
||||
return `io.${reference.domain}[${reference.index ?? 0}]`;
|
||||
}
|
||||
|
||||
function normalizeCondition(condition: string): string {
|
||||
return condition
|
||||
.replace(/\s*\.\s*/g, ".")
|
||||
.replace(/\s*\[\s*/g, "[")
|
||||
.replace(/\s*\]\s*/g, "]")
|
||||
.replace(/\s*\(\s*/g, "(")
|
||||
.replace(/\s*\)\s*/g, ")")
|
||||
.replace(/\s*,\s*/g, ",")
|
||||
.replace(/\s+/g, " ")
|
||||
.trim();
|
||||
}
|
||||
|
||||
function unwrapCall(text: string, name: string): string | undefined {
|
||||
const prefix = `${name}(`;
|
||||
return text.startsWith(prefix) && text.endsWith(")") ? text.slice(prefix.length, -1) : undefined;
|
||||
}
|
||||
|
||||
function splitTopLevel(text: string): string[] {
|
||||
const parts: string[] = [];
|
||||
let depth = 0;
|
||||
let start = 0;
|
||||
for (let index = 0; index < text.length; index += 1) {
|
||||
const char = text[index];
|
||||
if (char === "(") {
|
||||
depth += 1;
|
||||
} else if (char === ")") {
|
||||
depth = Math.max(0, depth - 1);
|
||||
} else if (char === "," && depth === 0) {
|
||||
parts.push(text.slice(start, index));
|
||||
start = index + 1;
|
||||
}
|
||||
}
|
||||
parts.push(text.slice(start));
|
||||
return parts.map((part) => part.trim()).filter((part) => part.length > 0);
|
||||
}
|
||||
|
||||
export function parseIoReferenceText(text: string): IoReference | undefined {
|
||||
const alias = text.match(/^io\.alias\.([A-Za-z_][A-Za-z0-9_]*)$/);
|
||||
if (alias) {
|
||||
return {
|
||||
domain: "alias",
|
||||
alias: alias[1]!,
|
||||
raw: text
|
||||
};
|
||||
}
|
||||
const indexed = text.match(/^io\.([A-Za-z]+)\[(\d+)\]$/);
|
||||
if (!indexed) {
|
||||
return undefined;
|
||||
}
|
||||
return {
|
||||
domain: indexed[1] as IoDomain,
|
||||
index: Number(indexed[2]),
|
||||
raw: text
|
||||
};
|
||||
}
|
||||
|
||||
function parseConditionValue(text: string): boolean | number | string {
|
||||
const trimmed = text.trim();
|
||||
if (trimmed === "true") {
|
||||
return true;
|
||||
}
|
||||
if (trimmed === "false") {
|
||||
return false;
|
||||
}
|
||||
if (/^-?\d+(?:\.\d+)?$/.test(trimmed)) {
|
||||
return Number(trimmed);
|
||||
}
|
||||
if ((trimmed.startsWith("\"") && trimmed.endsWith("\"")) || (trimmed.startsWith("'") && trimmed.endsWith("'"))) {
|
||||
return trimmed.slice(1, -1);
|
||||
}
|
||||
return trimmed;
|
||||
}
|
||||
116
kdl-wasm/web/src/runtime/kdlBridge.ts
Normal file
116
kdl-wasm/web/src/runtime/kdlBridge.ts
Normal file
@@ -0,0 +1,116 @@
|
||||
import type {
|
||||
CompiledOperation,
|
||||
CompiledPath,
|
||||
ExecutableInstruction,
|
||||
OperationActionInstruction,
|
||||
RunOperationInstruction,
|
||||
RunPathInstruction,
|
||||
SemanticProgramIr
|
||||
} from "../grl/ir/index.js";
|
||||
import { expandRunOperation } from "../grl/semantic/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import { MotionQueue, type MotionQueueItem } from "./motionQueue.js";
|
||||
|
||||
export interface KdlBridgeSnapshot {
|
||||
pathIds: string[];
|
||||
operationIds: string[];
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export class KdlRuntimeBridge {
|
||||
private readonly paths = new Map<string, CompiledPath>();
|
||||
private readonly operations = new Map<string, CompiledOperation>();
|
||||
private readonly diagnosticsValue: MotionDiagnostic[] = [];
|
||||
|
||||
constructor(
|
||||
private readonly program: SemanticProgramIr,
|
||||
private readonly queue: MotionQueue
|
||||
) {
|
||||
for (const path of program.paths) {
|
||||
this.paths.set(path.pathId, path);
|
||||
}
|
||||
for (const operation of program.operations) {
|
||||
this.operations.set(operation.operationId, operation);
|
||||
}
|
||||
}
|
||||
|
||||
get diagnostics(): MotionDiagnostic[] {
|
||||
return [...this.diagnosticsValue];
|
||||
}
|
||||
|
||||
enqueueInstruction(instruction: ExecutableInstruction): MotionQueueItem[] {
|
||||
if (instruction.kind === "RUN_PATH") {
|
||||
return this.enqueueRunPath(instruction);
|
||||
}
|
||||
if (instruction.kind === "RUN_OPERATION") {
|
||||
return this.enqueueRunOperation(instruction);
|
||||
}
|
||||
if (instruction.kind === "MOVEJ" || instruction.kind === "MOVEL" || instruction.kind === "MOVEC") {
|
||||
return [this.queue.enqueueMotion(instruction)];
|
||||
}
|
||||
return [];
|
||||
}
|
||||
|
||||
enqueueRunPath(instruction: RunPathInstruction): MotionQueueItem[] {
|
||||
const path = this.paths.get(instruction.pathId);
|
||||
if (!path) {
|
||||
this.addDiagnostic("VC_PATH_NOT_FOUND", `Path ${instruction.pathId} was not found`);
|
||||
return [];
|
||||
}
|
||||
return [this.queue.enqueueRunPath(instruction, path)];
|
||||
}
|
||||
|
||||
enqueueRunOperation(instruction: RunOperationInstruction): MotionQueueItem[] {
|
||||
const operation = this.operations.get(instruction.operationId);
|
||||
if (!operation) {
|
||||
this.addDiagnostic("VC_OPERATION_NOT_FOUND", `Operation ${instruction.operationId} was not found`);
|
||||
return [];
|
||||
}
|
||||
const items: MotionQueueItem[] = [];
|
||||
for (const step of expandRunOperation(instruction, this.operations)) {
|
||||
if (step.kind === "ACTION") {
|
||||
items.push(this.queue.enqueueOperationAction(step));
|
||||
} else {
|
||||
const path = this.paths.get(step.pathId);
|
||||
if (!path) {
|
||||
this.addDiagnostic("VC_PATH_NOT_FOUND", `Path ${step.pathId} was not found for operation ${operation.operationId}`);
|
||||
continue;
|
||||
}
|
||||
items.push(this.queue.enqueueRunPath(withOperationSource(step, operation), path));
|
||||
}
|
||||
}
|
||||
return items;
|
||||
}
|
||||
|
||||
snapshot(): KdlBridgeSnapshot {
|
||||
return {
|
||||
pathIds: [...this.paths.keys()],
|
||||
operationIds: [...this.operations.keys()],
|
||||
diagnostics: this.diagnostics
|
||||
};
|
||||
}
|
||||
|
||||
private addDiagnostic(code: string, message: string): void {
|
||||
this.diagnosticsValue.push({
|
||||
severity: "error",
|
||||
code,
|
||||
message
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
function withOperationSource(step: RunPathInstruction, operation: CompiledOperation): RunPathInstruction {
|
||||
const instruction: RunPathInstruction = {
|
||||
...step,
|
||||
...(step.sourceMap ? { sourceMap: step.sourceMap } : {})
|
||||
};
|
||||
Object.defineProperty(instruction, "__operationId", {
|
||||
value: operation.operationId,
|
||||
enumerable: false
|
||||
});
|
||||
return instruction;
|
||||
}
|
||||
|
||||
export function operationIdFromAction(action: OperationActionInstruction): string {
|
||||
return action.operationId;
|
||||
}
|
||||
267
kdl-wasm/web/src/runtime/motionQueue.ts
Normal file
267
kdl-wasm/web/src/runtime/motionQueue.ts
Normal file
@@ -0,0 +1,267 @@
|
||||
import type {
|
||||
KdlWasmApi,
|
||||
PathPlanResult,
|
||||
PathPlanRequest,
|
||||
RobotHandle,
|
||||
TrajectoryPoint,
|
||||
TrajectoryResult
|
||||
} from "../kdl/types.js";
|
||||
import type {
|
||||
CompiledPath,
|
||||
MotionInstruction,
|
||||
OperationActionInstruction,
|
||||
RunOperationInstruction,
|
||||
RunPathInstruction
|
||||
} from "../grl/ir/index.js";
|
||||
|
||||
export type MotionQueueSource =
|
||||
| { kind: "instruction"; instruction: MotionInstruction; pathId?: string; pointId?: string; operationId?: string; sourceMap?: MotionInstruction["sourceMap"] }
|
||||
| { kind: "path"; pathId: string; pointId?: string; operationId?: string; instruction?: RunPathInstruction; sourceMap?: RunPathInstruction["sourceMap"] }
|
||||
| { kind: "operation"; operationId: string; pathId?: string; pointId?: string; instruction?: RunOperationInstruction; sourceMap?: RunOperationInstruction["sourceMap"] }
|
||||
| { kind: "action"; operationId: string; pathId?: string; pointId?: string; actionKind: OperationActionInstruction["actionKind"]; statement: string; sourceMap?: OperationActionInstruction["sourceMap"] };
|
||||
|
||||
export interface MotionQueueItem {
|
||||
id: string;
|
||||
kind: "motion" | "path" | "action";
|
||||
source: MotionQueueSource;
|
||||
request: PathPlanRequest;
|
||||
planned?: PathPlanResult;
|
||||
result?: PathPlanResult | TrajectoryResult;
|
||||
status: "queued" | "planned" | "playing" | "done" | "failed";
|
||||
}
|
||||
|
||||
export interface MotionPlaybackSample {
|
||||
itemId: string;
|
||||
time: number;
|
||||
localTime?: number;
|
||||
point: TrajectoryPoint;
|
||||
source: MotionQueueSource;
|
||||
}
|
||||
|
||||
export interface MotionPlanner {
|
||||
planPath(handle: RobotHandle | undefined, request: PathPlanRequest): PathPlanResult | Promise<PathPlanResult>;
|
||||
}
|
||||
|
||||
export interface MotionQueueOptions {
|
||||
startJoints: number[];
|
||||
sampleTime: number;
|
||||
planner?: MotionPlanner;
|
||||
robotHandle?: RobotHandle;
|
||||
}
|
||||
|
||||
export interface MotionQueueSnapshot {
|
||||
items: MotionQueueItem[];
|
||||
activeIndex: number;
|
||||
virtualTime: number;
|
||||
}
|
||||
|
||||
export class MotionQueue {
|
||||
private readonly itemsValue: MotionQueueItem[] = [];
|
||||
private readonly options: MotionQueueOptions;
|
||||
private virtualTimeValue = 0;
|
||||
private nextId = 1;
|
||||
|
||||
constructor(options: MotionQueueOptions = { startJoints: [], sampleTime: 0.004 }) {
|
||||
this.options = options;
|
||||
}
|
||||
|
||||
get virtualTime(): number {
|
||||
return this.virtualTimeValue;
|
||||
}
|
||||
|
||||
enqueue(item: Omit<MotionQueueItem, "status"> & { status?: MotionQueueItem["status"] }): MotionQueueItem {
|
||||
const next: MotionQueueItem = {
|
||||
...item,
|
||||
status: item.status ?? "queued"
|
||||
};
|
||||
this.itemsValue.push(next);
|
||||
return next;
|
||||
}
|
||||
|
||||
enqueueMotion(instruction: MotionInstruction): MotionQueueItem {
|
||||
const segment = {
|
||||
id: instruction.pointId ?? instruction.id ?? `motion_${this.nextId}`,
|
||||
motion: instruction.kind,
|
||||
...(instruction.target ? { target: instruction.target } : {}),
|
||||
...(instruction.via ? { via: instruction.via } : {}),
|
||||
speed: instruction.speed,
|
||||
zone: instruction.zone,
|
||||
...(instruction.tool ? { tool: instruction.tool } : {}),
|
||||
...(instruction.frame ? { frame: instruction.frame } : {}),
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
|
||||
...(instruction.source ? { source: instruction.source } : {})
|
||||
};
|
||||
return this.enqueue({
|
||||
id: `mq_${this.nextId++}`,
|
||||
kind: "motion",
|
||||
source: {
|
||||
kind: "instruction",
|
||||
instruction,
|
||||
...(instruction.pathId ? { pathId: instruction.pathId } : {}),
|
||||
...(instruction.pointId ? { pointId: instruction.pointId } : {}),
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
|
||||
},
|
||||
request: {
|
||||
...(instruction.pathId ? { pathId: instruction.pathId } : {}),
|
||||
startJoints: [...this.options.startJoints],
|
||||
sampleTime: this.options.sampleTime,
|
||||
segments: [segment]
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
enqueueRunPath(instruction: RunPathInstruction, path: CompiledPath): MotionQueueItem {
|
||||
const operationId = hiddenOperationId(instruction);
|
||||
return this.enqueue({
|
||||
id: `mq_${this.nextId++}`,
|
||||
kind: "path",
|
||||
source: {
|
||||
kind: "path",
|
||||
pathId: instruction.pathId,
|
||||
...(operationId ? { operationId } : {}),
|
||||
instruction,
|
||||
...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
|
||||
},
|
||||
request: {
|
||||
...path.request,
|
||||
startJoints: [...this.options.startJoints],
|
||||
sampleTime: this.options.sampleTime
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
enqueueOperationAction(action: OperationActionInstruction): MotionQueueItem {
|
||||
return this.enqueue({
|
||||
id: `mq_${this.nextId++}`,
|
||||
kind: "action",
|
||||
status: "done",
|
||||
source: {
|
||||
kind: "action",
|
||||
operationId: action.operationId,
|
||||
actionKind: action.actionKind,
|
||||
statement: action.statement,
|
||||
...(action.sourceMap ? { sourceMap: action.sourceMap } : {})
|
||||
},
|
||||
request: {
|
||||
startJoints: [...this.options.startJoints],
|
||||
sampleTime: this.options.sampleTime,
|
||||
segments: []
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
items(): MotionQueueItem[] {
|
||||
return this.itemsValue.map((item) => ({ ...item }));
|
||||
}
|
||||
|
||||
clear(): void {
|
||||
this.itemsValue.length = 0;
|
||||
}
|
||||
|
||||
async planAll(api?: Pick<KdlWasmApi, "planPath">, handle?: RobotHandle): Promise<MotionQueueItem[]> {
|
||||
const planner = api
|
||||
? { planPath: (robotHandle: RobotHandle | undefined, request: PathPlanRequest) => api.planPath(robotHandle ?? handle ?? this.options.robotHandle ?? 0, request) }
|
||||
: this.options.planner;
|
||||
if (!planner) {
|
||||
throw new Error("MotionQueue requires a planner or KDL API to plan queued paths");
|
||||
}
|
||||
for (const item of this.itemsValue) {
|
||||
if (item.status !== "queued" || item.request.segments.length === 0) {
|
||||
continue;
|
||||
}
|
||||
try {
|
||||
item.planned = await planner.planPath(handle ?? this.options.robotHandle, item.request);
|
||||
item.result = item.planned;
|
||||
item.status = item.planned.ok ? "planned" : "failed";
|
||||
} catch {
|
||||
item.status = "failed";
|
||||
}
|
||||
}
|
||||
return this.items();
|
||||
}
|
||||
|
||||
advance(deltaTime: number): MotionPlaybackSample | undefined {
|
||||
this.virtualTimeValue += deltaTime;
|
||||
return this.sampleAt(this.virtualTimeValue);
|
||||
}
|
||||
|
||||
sampleAt(time: number): MotionPlaybackSample | undefined {
|
||||
let elapsed = 0;
|
||||
for (const item of this.itemsValue) {
|
||||
const planned = item.planned ?? (item.result && "segments" in item.result ? item.result : undefined);
|
||||
if (!planned) {
|
||||
continue;
|
||||
}
|
||||
const duration = planned?.duration ?? 0;
|
||||
if (duration === 0) {
|
||||
continue;
|
||||
}
|
||||
if (time <= elapsed + duration) {
|
||||
const localTime = Math.max(0, Math.min(duration, time - elapsed));
|
||||
const point = samplePoint(planned.points, localTime);
|
||||
if (!point) {
|
||||
return undefined;
|
||||
}
|
||||
return {
|
||||
itemId: item.id,
|
||||
time,
|
||||
localTime,
|
||||
point,
|
||||
source: item.source
|
||||
} as MotionPlaybackSample;
|
||||
}
|
||||
elapsed += duration;
|
||||
}
|
||||
const last = [...this.itemsValue].reverse().find((item) => item.planned?.points.length);
|
||||
const point = last?.planned?.points.at(-1);
|
||||
return last && point && last.planned
|
||||
? ({ itemId: last.id, time, localTime: last.planned.duration, point, source: last.source } as MotionPlaybackSample)
|
||||
: undefined;
|
||||
}
|
||||
|
||||
snapshot(): MotionQueueSnapshot {
|
||||
return {
|
||||
items: this.items(),
|
||||
activeIndex: Math.max(0, this.itemsValue.findIndex((item) => item.status === "playing" || item.status === "planned")),
|
||||
virtualTime: this.virtualTimeValue
|
||||
};
|
||||
}
|
||||
|
||||
playback(sampleTime: number): MotionPlaybackSample[] {
|
||||
const samples: MotionPlaybackSample[] = [];
|
||||
for (const item of this.itemsValue) {
|
||||
const points = pointsOf(item.result);
|
||||
if (points.length === 0) {
|
||||
continue;
|
||||
}
|
||||
item.status = "playing";
|
||||
for (const point of points) {
|
||||
if (point.index === 0 || point.time === 0 || point.time % sampleTime < 1e-9 || point === points.at(-1)) {
|
||||
samples.push({
|
||||
itemId: item.id,
|
||||
time: point.time,
|
||||
point,
|
||||
source: item.source
|
||||
});
|
||||
}
|
||||
}
|
||||
item.status = "done";
|
||||
}
|
||||
return samples;
|
||||
}
|
||||
}
|
||||
|
||||
function samplePoint(points: TrajectoryPoint[], time: number): TrajectoryPoint | undefined {
|
||||
return points.find((point) => point.time >= time) ?? points.at(-1);
|
||||
}
|
||||
|
||||
function hiddenOperationId(instruction: RunPathInstruction): string | undefined {
|
||||
return (instruction as RunPathInstruction & { __operationId?: string }).__operationId;
|
||||
}
|
||||
|
||||
function pointsOf(result: PathPlanResult | TrajectoryResult | undefined): TrajectoryPoint[] {
|
||||
if (!result) {
|
||||
return [];
|
||||
}
|
||||
return "segments" in result ? result.points : result.points;
|
||||
}
|
||||
185
kdl-wasm/web/src/suites/abb120Suite.ts
Normal file
185
kdl-wasm/web/src/suites/abb120Suite.ts
Normal file
@@ -0,0 +1,185 @@
|
||||
import { parseGrl } from "../grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../grl/semantic/index.js";
|
||||
import { postProcessAllBrands } from "../grl/post/index.js";
|
||||
import { importBrandProgram } from "../importers/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import { createValidationReport, createCustomerDeliveryPackage, renderValidationReportHtml } from "../reports/index.js";
|
||||
import { createOlpProject, type OlpProjectModel } from "../olp/index.js";
|
||||
import {
|
||||
ABB120_ROBOT_FIXTURE,
|
||||
ABB_IRB120_ZERO_JOINTS
|
||||
} from "../fixtures/abb120.js";
|
||||
|
||||
export interface Abb120SuiteProgram {
|
||||
name: string;
|
||||
text: string;
|
||||
}
|
||||
|
||||
export interface Abb120SuiteJob {
|
||||
job_id: string;
|
||||
robot: typeof ABB120_ROBOT_FIXTURE;
|
||||
programs: Array<{
|
||||
name: string;
|
||||
moduleName: string;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
astKind: "Program";
|
||||
sourceMapEntries: number;
|
||||
kdlMotionRequests: number;
|
||||
kdlPathRequests: number;
|
||||
}>;
|
||||
post: {
|
||||
filenames: string[];
|
||||
report: Array<{ code: string; severity: string; message: string }>;
|
||||
};
|
||||
roundtrip: {
|
||||
status: "pass" | "warn" | "fail";
|
||||
diagnostics: MotionDiagnostic[];
|
||||
};
|
||||
report_id: string;
|
||||
artifacts: {
|
||||
jobJson: string;
|
||||
traceJson: string;
|
||||
trajectoryJson: string;
|
||||
diagnosticsJson: string;
|
||||
reportJson: string;
|
||||
reportHtml: string;
|
||||
deliveryPackageJson: string;
|
||||
desktopScreenshot: string;
|
||||
mobileScreenshot: string;
|
||||
};
|
||||
}
|
||||
|
||||
export function createAbb120OlpProject(sourceProgram: Abb120SuiteProgram): OlpProjectModel {
|
||||
const project = createOlpProject({ id: "abb120_suite", name: "Abb120Suite", customer: "KDL" });
|
||||
project.station = {
|
||||
id: "abb120_station",
|
||||
name: "ABB120 Station",
|
||||
resourceIds: ["abb_irb120_3_58", "tool0", "world"],
|
||||
activeRobotIds: ["abb_irb120_3_58"],
|
||||
defaultFrameId: "world"
|
||||
};
|
||||
project.resources.robots.push({
|
||||
id: "abb_irb120_3_58",
|
||||
name: "ABB IRB120 3/58",
|
||||
kind: "robot",
|
||||
brand: "abb",
|
||||
model: "IRB120 3/58",
|
||||
dof: 6,
|
||||
jointNames: ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"],
|
||||
limits: [
|
||||
{ name: "joint_1", lower: -2.87979, upper: 2.87979, velocity: 4.36332 },
|
||||
{ name: "joint_2", lower: -1.91986, upper: 1.91986, velocity: 4.36332 },
|
||||
{ name: "joint_3", lower: -1.91986, upper: 1.22173, velocity: 4.36332 },
|
||||
{ name: "joint_4", lower: -2.79253, upper: 2.79253, velocity: 5.58505 },
|
||||
{ name: "joint_5", lower: -2.094395, upper: 2.094395, velocity: 5.58505 },
|
||||
{ name: "joint_6", lower: -6.98132, upper: 6.98132, velocity: 7.33038 }
|
||||
],
|
||||
baseFrameId: "world",
|
||||
controller: { family: "IRC5/OmniCore virtual" }
|
||||
});
|
||||
project.resources.tools.push({ id: "tool0", name: "tool0", kind: "tool", tcp: [0, 0, 0, 0, 0, 0], robotId: "abb_irb120_3_58" });
|
||||
project.resources.frames.push({ id: "world", name: "world", kind: "frame", pose: [0, 0, 0, 0, 0, 0] });
|
||||
project.targets.push({ id: "home", name: "home", kind: "joint", robotId: "abb_irb120_3_58", joints: [...ABB_IRB120_ZERO_JOINTS] });
|
||||
project.speeds.push({ id: "v_joint", name: "v_joint", kind: "joint_percent", value: 40 });
|
||||
project.zones.push({ id: "zf", name: "zf", kind: "fine" });
|
||||
project.programs.push({
|
||||
id: sourceProgram.name.replace(/\.grl$/i, ""),
|
||||
name: sourceProgram.name,
|
||||
language: "grl",
|
||||
entryOperationIds: [],
|
||||
source: { kind: "authored", text: sourceProgram.text }
|
||||
});
|
||||
project.calibrations.push({
|
||||
id: "tcp_cal_tool0",
|
||||
name: "tool0_tcp_cal",
|
||||
kind: "tcp",
|
||||
targetResourceId: "tool0",
|
||||
poseDelta: [0, 0, 0, 0, 0, 0]
|
||||
});
|
||||
return project;
|
||||
}
|
||||
|
||||
export function runAbb120Suite(programs: Abb120SuiteProgram[], options: { now?: string } = {}): Abb120SuiteJob {
|
||||
const now = options.now ?? new Date().toISOString();
|
||||
const job_id = `A120-JOB-${now.replace(/[-:TZ.]/g, "").slice(0, 14)}-doc`;
|
||||
const compiled = programs.map((program) => {
|
||||
const ast = parseGrl(program.text);
|
||||
const ir = compileSemanticProgram(ast, {
|
||||
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
||||
sampleTime: 0.02
|
||||
});
|
||||
return { program, ast, ir };
|
||||
});
|
||||
const happyIr = compiled.find((item) => item.program.name.includes("A120_00"))?.ir ?? compiled[0]!.ir;
|
||||
const posted = postProcessAllBrands(happyIr);
|
||||
const abbRoundtrip = importBrandProgram("abb", [{ name: posted.outputs.abb.filename, text: posted.outputs.abb.text }], {
|
||||
projectName: "Abb120Roundtrip",
|
||||
generatedAt: now
|
||||
});
|
||||
const project = createAbb120OlpProject(programs[0]!);
|
||||
const report = createValidationReport({
|
||||
id: `A120-REPORT-${now.replace(/[-:TZ.]/g, "").slice(0, 14)}`,
|
||||
project,
|
||||
generatedAt: now,
|
||||
pathValidation: {
|
||||
ok: true,
|
||||
reachable: true,
|
||||
cycleTime: 1,
|
||||
segmentReports: [],
|
||||
diagnostics: []
|
||||
},
|
||||
postDiagnostics: posted.report.map((issue) => ({
|
||||
severity: issue.severity,
|
||||
code: issue.code,
|
||||
message: issue.message
|
||||
})),
|
||||
importDiagnostics: abbRoundtrip.report.sections.flatMap((section) => section.diagnostics)
|
||||
});
|
||||
const reportHtml = renderValidationReportHtml(report);
|
||||
const delivery = createCustomerDeliveryPackage({
|
||||
project,
|
||||
report,
|
||||
reportHtml,
|
||||
brandPrograms: Object.fromEntries(Object.values(posted.outputs).map((output) => [output.filename, output.text])),
|
||||
ioMap: { "io.do[1]": "gripper" },
|
||||
trace: { job_id, events: ["load", "run", "report"] }
|
||||
});
|
||||
|
||||
return {
|
||||
job_id,
|
||||
robot: ABB120_ROBOT_FIXTURE,
|
||||
programs: compiled.map((item) => ({
|
||||
name: item.program.name,
|
||||
moduleName: item.ir.moduleName,
|
||||
diagnostics: item.ir.diagnostics,
|
||||
astKind: item.ast.kind,
|
||||
sourceMapEntries: item.ir.sourceMap.length,
|
||||
kdlMotionRequests: item.ir.kdlBridge.motionRequests.length,
|
||||
kdlPathRequests: item.ir.kdlBridge.pathRequests.length
|
||||
})),
|
||||
post: {
|
||||
filenames: Object.values(posted.outputs).map((output) => output.filename),
|
||||
report: posted.report.map((issue) => ({
|
||||
code: issue.code,
|
||||
severity: issue.severity,
|
||||
message: issue.message
|
||||
}))
|
||||
},
|
||||
roundtrip: {
|
||||
status: abbRoundtrip.report.status,
|
||||
diagnostics: abbRoundtrip.report.sections.flatMap((section) => section.diagnostics)
|
||||
},
|
||||
report_id: report.id,
|
||||
artifacts: {
|
||||
jobJson: `${job_id}/job.json`,
|
||||
traceJson: `${job_id}/trace.json`,
|
||||
trajectoryJson: `${job_id}/trajectory.json`,
|
||||
diagnosticsJson: `${job_id}/diagnostics.json`,
|
||||
reportJson: `${job_id}/reports/${report.id}.json`,
|
||||
reportHtml: `${job_id}/reports/${report.id}.html`,
|
||||
deliveryPackageJson: `${job_id}/delivery/package.json`,
|
||||
desktopScreenshot: `${job_id}/screenshots/virtual-controller-desktop.png`,
|
||||
mobileScreenshot: `${job_id}/screenshots/virtual-controller-mobile.png`
|
||||
}
|
||||
};
|
||||
}
|
||||
162
kdl-wasm/web/src/workbench/debugFacade.ts
Normal file
162
kdl-wasm/web/src/workbench/debugFacade.ts
Normal file
@@ -0,0 +1,162 @@
|
||||
import type { SemanticProgramIr } from "../grl/ir/index.js";
|
||||
import type { MotionSourceMap, TrajectoryPoint } from "../kdl/types.js";
|
||||
import type { IoImageRuntime } from "../runtime/ioRuntime.js";
|
||||
import type { MotionQueue, MotionQueueItem, MotionPlaybackSample } from "../runtime/motionQueue.js";
|
||||
import type { IrExecutionRuntime, RuntimeBreakpoint } from "../controller/runtime.js";
|
||||
import { RuntimeSourceMapIndex, type RuntimeSourceLocation } from "../controller/sourceMap.js";
|
||||
import type { RuntimeTraceEvent } from "../controller/trace.js";
|
||||
|
||||
export interface MotionBreakpoint {
|
||||
id: string;
|
||||
pathId?: string;
|
||||
pointId?: string;
|
||||
operationId?: string;
|
||||
enabled: boolean;
|
||||
}
|
||||
|
||||
export interface VariableWatch {
|
||||
id: string;
|
||||
expression: string;
|
||||
value?: unknown;
|
||||
source?: RuntimeSourceLocation;
|
||||
}
|
||||
|
||||
export interface DebugFacadeSnapshot {
|
||||
breakpoints: RuntimeBreakpoint[];
|
||||
motionBreakpoints: MotionBreakpoint[];
|
||||
watches: VariableWatch[];
|
||||
currentSource?: RuntimeSourceLocation;
|
||||
trace: RuntimeTraceEvent[];
|
||||
playback?: MotionPlaybackSample;
|
||||
}
|
||||
|
||||
export interface DebugFacadeOptions {
|
||||
runtime: IrExecutionRuntime;
|
||||
motionQueue?: MotionQueue;
|
||||
io?: IoImageRuntime;
|
||||
program?: SemanticProgramIr;
|
||||
}
|
||||
|
||||
export class DebugFacade {
|
||||
private readonly runtime: IrExecutionRuntime;
|
||||
private readonly motionQueue: MotionQueue | undefined;
|
||||
private readonly io: IoImageRuntime | undefined;
|
||||
private readonly sourceIndex: RuntimeSourceMapIndex;
|
||||
private readonly motionBreakpointsValue = new Map<string, MotionBreakpoint>();
|
||||
private readonly watchesValue = new Map<string, VariableWatch>();
|
||||
|
||||
constructor(options: DebugFacadeOptions) {
|
||||
this.runtime = options.runtime;
|
||||
this.motionQueue = options.motionQueue;
|
||||
this.io = options.io;
|
||||
this.sourceIndex = options.program ? new RuntimeSourceMapIndex(options.program) : this.runtime.sourceMapIndex;
|
||||
}
|
||||
|
||||
addBreakpoint(breakpoint: RuntimeBreakpoint): void {
|
||||
this.runtime.setBreakpoint(breakpoint);
|
||||
}
|
||||
|
||||
removeBreakpoint(id: string): boolean {
|
||||
return this.runtime.removeBreakpoint(id);
|
||||
}
|
||||
|
||||
addMotionBreakpoint(breakpoint: MotionBreakpoint): void {
|
||||
this.motionBreakpointsValue.set(breakpoint.id, { ...breakpoint });
|
||||
}
|
||||
|
||||
removeMotionBreakpoint(id: string): boolean {
|
||||
return this.motionBreakpointsValue.delete(id);
|
||||
}
|
||||
|
||||
checkMotionBreakpoint(item: MotionQueueItem): MotionBreakpoint | undefined {
|
||||
return [...this.motionBreakpointsValue.values()].find((breakpoint) => {
|
||||
if (!breakpoint.enabled) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.pathId && breakpoint.pathId !== item.source.pathId) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.pointId && breakpoint.pointId !== item.source.pointId) {
|
||||
return false;
|
||||
}
|
||||
if (breakpoint.operationId && breakpoint.operationId !== item.source.operationId) {
|
||||
return false;
|
||||
}
|
||||
return breakpoint.pathId !== undefined || breakpoint.pointId !== undefined || breakpoint.operationId !== undefined;
|
||||
});
|
||||
}
|
||||
|
||||
addWatch(id: string, expression: string): VariableWatch {
|
||||
const source = this.runtime.currentSource();
|
||||
const watch: VariableWatch = {
|
||||
id,
|
||||
expression,
|
||||
value: this.evaluateWatchExpression(expression),
|
||||
...(source ? { source } : {})
|
||||
};
|
||||
this.watchesValue.set(id, watch);
|
||||
return { ...watch };
|
||||
}
|
||||
|
||||
updateWatches(): VariableWatch[] {
|
||||
const source = this.runtime.currentSource();
|
||||
for (const watch of this.watchesValue.values()) {
|
||||
watch.value = this.evaluateWatchExpression(watch.expression);
|
||||
if (source) {
|
||||
watch.source = source;
|
||||
} else {
|
||||
delete watch.source;
|
||||
}
|
||||
}
|
||||
return this.watches();
|
||||
}
|
||||
|
||||
watches(): VariableWatch[] {
|
||||
return [...this.watchesValue.values()].map((watch) => ({ ...watch }));
|
||||
}
|
||||
|
||||
locateSource(sourceMap: MotionSourceMap): RuntimeSourceLocation[] {
|
||||
return this.sourceIndex.locateSource(sourceMap);
|
||||
}
|
||||
|
||||
locatePathPoint(pathId: string, pointId: string): RuntimeSourceLocation | undefined {
|
||||
return this.sourceIndex.locatePathPoint(pathId, pointId);
|
||||
}
|
||||
|
||||
locateOperation(operationId: string): RuntimeSourceLocation[] {
|
||||
return this.sourceIndex.locateOperation(operationId);
|
||||
}
|
||||
|
||||
playbackAt(time: number): MotionPlaybackSample | undefined {
|
||||
return this.motionQueue?.sampleAt(time);
|
||||
}
|
||||
|
||||
replayTrace(): RuntimeTraceEvent[] {
|
||||
return this.runtime.snapshot().trace;
|
||||
}
|
||||
|
||||
currentPoint(): TrajectoryPoint | undefined {
|
||||
return this.motionQueue?.sampleAt(this.motionQueue.virtualTime)?.point;
|
||||
}
|
||||
|
||||
snapshot(): DebugFacadeSnapshot {
|
||||
const playback = this.motionQueue?.sampleAt(this.motionQueue.virtualTime);
|
||||
const currentSource = this.runtime.currentSource();
|
||||
return {
|
||||
breakpoints: this.runtime.listBreakpoints(),
|
||||
motionBreakpoints: [...this.motionBreakpointsValue.values()].map((breakpoint) => ({ ...breakpoint })),
|
||||
watches: this.updateWatches(),
|
||||
...(currentSource ? { currentSource } : {}),
|
||||
trace: this.runtime.snapshot().trace,
|
||||
...(playback ? { playback } : {})
|
||||
};
|
||||
}
|
||||
|
||||
private evaluateWatchExpression(expression: string): unknown {
|
||||
if (expression.startsWith("io.")) {
|
||||
const key = expression.replace(/\s+/g, "");
|
||||
return this.io?.snapshot().image[key];
|
||||
}
|
||||
return this.runtime.getVariable(expression);
|
||||
}
|
||||
}
|
||||
139
kdl-wasm/web/src/workbench/facade.ts
Normal file
139
kdl-wasm/web/src/workbench/facade.ts
Normal file
@@ -0,0 +1,139 @@
|
||||
import type { RuntimeBreakpoint, RuntimeSourceMapIndex } from "../controller/index.js";
|
||||
import type { OlpProjectModel } from "../olp/index.js";
|
||||
import type { RuntimeTraceEvent } from "../controller/index.js";
|
||||
|
||||
export interface WorkbenchTreeNode {
|
||||
id: string;
|
||||
label: string;
|
||||
kind: "station" | "robot" | "tool" | "frame" | "target" | "path" | "operation" | "program" | "report";
|
||||
children?: WorkbenchTreeNode[];
|
||||
}
|
||||
|
||||
export interface TeachPendantState {
|
||||
tcp?: number[];
|
||||
joints?: number[];
|
||||
mode: "manual" | "auto" | "step";
|
||||
controllerState: string;
|
||||
}
|
||||
|
||||
export interface DebugPanelState {
|
||||
currentLine?: number;
|
||||
variables: Record<string, unknown>;
|
||||
watches: Record<string, unknown>;
|
||||
breakpoints: RuntimeBreakpoint[];
|
||||
trace: RuntimeTraceEvent[];
|
||||
}
|
||||
|
||||
export interface WorkbenchAction {
|
||||
id: string;
|
||||
label: string;
|
||||
target: "post" | "report" | "delivery";
|
||||
enabled: boolean;
|
||||
}
|
||||
|
||||
export interface WorkbenchState {
|
||||
tree: WorkbenchTreeNode[];
|
||||
editor: {
|
||||
activeProgramId?: string;
|
||||
text: string;
|
||||
};
|
||||
pendant: TeachPendantState;
|
||||
debug: DebugPanelState;
|
||||
io: {
|
||||
values: Record<string, unknown>;
|
||||
waiting?: string;
|
||||
};
|
||||
alarms: Array<{ id: string; message: string; severity: string }>;
|
||||
actions: WorkbenchAction[];
|
||||
}
|
||||
|
||||
export function buildWorkbenchState(input: {
|
||||
project: OlpProjectModel;
|
||||
controllerState?: string;
|
||||
activeProgramId?: string;
|
||||
variables?: Record<string, unknown>;
|
||||
watches?: string[];
|
||||
breakpoints?: RuntimeBreakpoint[];
|
||||
trace?: RuntimeTraceEvent[];
|
||||
io?: Record<string, unknown>;
|
||||
waiting?: string;
|
||||
alarms?: Array<{ id: string; message: string; severity: string }>;
|
||||
joints?: number[];
|
||||
tcp?: number[];
|
||||
}): WorkbenchState {
|
||||
const activeProgram = input.project.programs.find((program) => program.id === input.activeProgramId) ?? input.project.programs[0];
|
||||
const variables = input.variables ?? {};
|
||||
const currentLine = input.trace?.at(-1)?.source?.sourceMap?.line;
|
||||
return {
|
||||
tree: buildObjectTree(input.project),
|
||||
editor: {
|
||||
...(activeProgram ? { activeProgramId: activeProgram.id } : {}),
|
||||
text: activeProgram?.source.text ?? ""
|
||||
},
|
||||
pendant: {
|
||||
...(input.tcp ? { tcp: [...input.tcp] } : {}),
|
||||
...(input.joints ? { joints: [...input.joints] } : {}),
|
||||
mode: "step",
|
||||
controllerState: input.controllerState ?? "stopped"
|
||||
},
|
||||
debug: {
|
||||
...(currentLine !== undefined ? { currentLine } : {}),
|
||||
variables,
|
||||
watches: Object.fromEntries((input.watches ?? []).map((name) => [name, variables[name]])),
|
||||
breakpoints: input.breakpoints ?? [],
|
||||
trace: input.trace ?? []
|
||||
},
|
||||
io: {
|
||||
values: input.io ?? {},
|
||||
...(input.waiting ? { waiting: input.waiting } : {})
|
||||
},
|
||||
alarms: input.alarms ?? [],
|
||||
actions: [
|
||||
{ id: "post-export", label: "Post", target: "post", enabled: input.project.operations.length > 0 },
|
||||
{ id: "report-open", label: "Report", target: "report", enabled: true },
|
||||
{ id: "delivery-export", label: "Delivery", target: "delivery", enabled: input.project.programs.length > 0 }
|
||||
]
|
||||
};
|
||||
}
|
||||
|
||||
export function buildObjectTree(project: OlpProjectModel): WorkbenchTreeNode[] {
|
||||
return [
|
||||
{
|
||||
id: project.station.id,
|
||||
label: project.station.name,
|
||||
kind: "station",
|
||||
children: [
|
||||
...project.resources.robots.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "robot" })),
|
||||
...project.resources.tools.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "tool" })),
|
||||
...project.resources.frames.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "frame" })),
|
||||
...project.targets.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "target" })),
|
||||
...project.paths.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "path" })),
|
||||
...project.operations.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "operation" })),
|
||||
...project.programs.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "program" })),
|
||||
...project.reports.map((item): WorkbenchTreeNode => ({ id: item.id, label: item.name, kind: "report" }))
|
||||
]
|
||||
}
|
||||
];
|
||||
}
|
||||
|
||||
export function locateCrossSource(index: RuntimeSourceMapIndex, input: {
|
||||
pathId?: string;
|
||||
pointId?: string;
|
||||
operationId?: string;
|
||||
file?: string;
|
||||
line?: number;
|
||||
}) {
|
||||
if (input.pathId && input.pointId) {
|
||||
return index.locatePathPoint(input.pathId, input.pointId);
|
||||
}
|
||||
if (input.operationId) {
|
||||
return index.locateOperation(input.operationId);
|
||||
}
|
||||
if (input.line !== undefined || input.file) {
|
||||
return index.locateSource({
|
||||
...(input.file ? { file: input.file } : {}),
|
||||
...(input.line !== undefined ? { line: input.line } : {})
|
||||
});
|
||||
}
|
||||
return undefined;
|
||||
}
|
||||
28
kdl-wasm/web/src/workbench/index.ts
Normal file
28
kdl-wasm/web/src/workbench/index.ts
Normal file
@@ -0,0 +1,28 @@
|
||||
export {
|
||||
DebugFacade,
|
||||
type DebugFacadeOptions,
|
||||
type DebugFacadeSnapshot,
|
||||
type MotionBreakpoint,
|
||||
type VariableWatch
|
||||
} from "./debugFacade.js";
|
||||
export {
|
||||
WorkbenchFacade,
|
||||
type EditorModel,
|
||||
type IoPanelModel,
|
||||
type ReportEntryModel,
|
||||
type TeachPendantModel as ProgramTeachPendantModel,
|
||||
type WorkbenchFacadeInput,
|
||||
type WorkbenchFacadeSnapshot,
|
||||
type WorkbenchObjectKind,
|
||||
type WorkbenchObjectNode
|
||||
} from "./workbenchFacade.js";
|
||||
export {
|
||||
buildObjectTree,
|
||||
buildWorkbenchState,
|
||||
locateCrossSource,
|
||||
type DebugPanelState,
|
||||
type TeachPendantState,
|
||||
type WorkbenchAction,
|
||||
type WorkbenchState,
|
||||
type WorkbenchTreeNode
|
||||
} from "./facade.js";
|
||||
214
kdl-wasm/web/src/workbench/workbenchFacade.ts
Normal file
214
kdl-wasm/web/src/workbench/workbenchFacade.ts
Normal file
@@ -0,0 +1,214 @@
|
||||
import type { SemanticProgramIr } from "../grl/ir/index.js";
|
||||
import type { MotionDiagnostic } from "../kdl/types.js";
|
||||
import type { IoRuntimeSnapshot } from "../runtime/ioRuntime.js";
|
||||
import type { MotionQueueSnapshot } from "../runtime/motionQueue.js";
|
||||
import type { IrExecutionRuntimeSnapshot } from "../controller/runtime.js";
|
||||
import type { RuntimeSourceLocation } from "../controller/sourceMap.js";
|
||||
|
||||
export type WorkbenchObjectKind =
|
||||
| "station"
|
||||
| "robot"
|
||||
| "tool"
|
||||
| "frame"
|
||||
| "target"
|
||||
| "path"
|
||||
| "operation"
|
||||
| "program"
|
||||
| "report";
|
||||
|
||||
export interface WorkbenchObjectNode {
|
||||
id: string;
|
||||
label: string;
|
||||
kind: WorkbenchObjectKind;
|
||||
children?: WorkbenchObjectNode[];
|
||||
source?: RuntimeSourceLocation;
|
||||
data?: Record<string, unknown>;
|
||||
}
|
||||
|
||||
export interface EditorModel {
|
||||
activeFile?: string;
|
||||
cursor?: {
|
||||
line: number;
|
||||
column: number;
|
||||
};
|
||||
currentSource?: RuntimeSourceLocation;
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export interface TeachPendantModel {
|
||||
state: string;
|
||||
procedure?: string;
|
||||
pc: number;
|
||||
joints?: number[];
|
||||
tcp?: unknown;
|
||||
alarms: IrExecutionRuntimeSnapshot["alarmQueue"];
|
||||
}
|
||||
|
||||
export interface IoPanelModel {
|
||||
image: IoRuntimeSnapshot["image"];
|
||||
events: IoRuntimeSnapshot["events"];
|
||||
waits: Array<{
|
||||
condition: string;
|
||||
source?: RuntimeSourceLocation;
|
||||
}>;
|
||||
}
|
||||
|
||||
export interface ReportEntryModel {
|
||||
id: string;
|
||||
label: string;
|
||||
kind: "reachability" | "cycle_time" | "io_wait" | "post" | "import";
|
||||
status: "ready" | "pending" | "error";
|
||||
diagnostics: MotionDiagnostic[];
|
||||
}
|
||||
|
||||
export interface WorkbenchFacadeSnapshot {
|
||||
objectTree: WorkbenchObjectNode[];
|
||||
editor: EditorModel;
|
||||
teachPendant: TeachPendantModel;
|
||||
ioPanel: IoPanelModel;
|
||||
reports: ReportEntryModel[];
|
||||
}
|
||||
|
||||
export interface WorkbenchFacadeInput {
|
||||
program: SemanticProgramIr;
|
||||
runtime: IrExecutionRuntimeSnapshot;
|
||||
io: IoRuntimeSnapshot;
|
||||
motion?: MotionQueueSnapshot;
|
||||
controllerState: string;
|
||||
}
|
||||
|
||||
export class WorkbenchFacade {
|
||||
snapshot(input: WorkbenchFacadeInput): WorkbenchFacadeSnapshot {
|
||||
const currentPoint = currentMotionPoint(input.motion);
|
||||
const editorSource = input.runtime.currentSource;
|
||||
const editorCursor = cursorFromSource(editorSource);
|
||||
return {
|
||||
objectTree: buildObjectTree(input.program),
|
||||
editor: {
|
||||
...(editorSource?.sourceMap?.file ? { activeFile: editorSource.sourceMap.file } : {}),
|
||||
...(editorCursor ? { cursor: editorCursor } : {}),
|
||||
...(editorSource ? { currentSource: editorSource } : {}),
|
||||
diagnostics: input.program.diagnostics
|
||||
},
|
||||
teachPendant: {
|
||||
state: input.controllerState,
|
||||
...(input.runtime.procedure ? { procedure: input.runtime.procedure } : {}),
|
||||
pc: input.runtime.pc,
|
||||
...(currentPoint ? { joints: currentPoint.joints, tcp: currentPoint.tcp } : {}),
|
||||
alarms: input.runtime.alarmQueue
|
||||
},
|
||||
ioPanel: {
|
||||
image: input.io.image,
|
||||
events: input.io.events,
|
||||
waits: input.runtime.trace
|
||||
.filter((event) => event.kind === "wait")
|
||||
.map((event) => ({
|
||||
condition: String(event.data?.condition ?? event.data?.status ?? ""),
|
||||
...(event.source ? { source: event.source } : {})
|
||||
}))
|
||||
},
|
||||
reports: buildReportEntries(input.program)
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
function buildObjectTree(program: SemanticProgramIr): WorkbenchObjectNode[] {
|
||||
return [
|
||||
{
|
||||
id: "station",
|
||||
label: "Station",
|
||||
kind: "station",
|
||||
children: [
|
||||
{
|
||||
id: "programs",
|
||||
label: "Programs",
|
||||
kind: "program",
|
||||
children: program.procedures.map((procedure) => ({
|
||||
id: `program:${procedure.name}`,
|
||||
label: procedure.name,
|
||||
kind: "program",
|
||||
...(procedure.sourceMap
|
||||
? { source: { kind: "procedure", id: procedure.name, sourceMap: procedure.sourceMap } }
|
||||
: {})
|
||||
}))
|
||||
},
|
||||
{
|
||||
id: "paths",
|
||||
label: "Paths",
|
||||
kind: "path",
|
||||
children: program.paths.map((path) => ({
|
||||
id: `path:${path.pathId}`,
|
||||
label: path.pathId,
|
||||
kind: "path",
|
||||
children: path.motions.map((motion) => ({
|
||||
id: `path:${path.pathId}:${motion.pointId ?? motion.id ?? motion.kind}`,
|
||||
label: motion.pointId ?? motion.id ?? motion.kind,
|
||||
kind: "target",
|
||||
source: {
|
||||
kind: "path_point",
|
||||
id: motion.pointId ?? motion.id ?? motion.kind,
|
||||
...(motion.sourceMap ? { sourceMap: motion.sourceMap } : {}),
|
||||
pathId: path.pathId,
|
||||
...(motion.pointId ? { pointId: motion.pointId } : {})
|
||||
}
|
||||
}))
|
||||
}))
|
||||
},
|
||||
{
|
||||
id: "operations",
|
||||
label: "Operations",
|
||||
kind: "operation",
|
||||
children: program.operations.map((operation) => ({
|
||||
id: `operation:${operation.operationId}`,
|
||||
label: operation.operationId,
|
||||
kind: "operation",
|
||||
data: {
|
||||
kind: operation.kind,
|
||||
pathId: operation.pathId
|
||||
}
|
||||
}))
|
||||
},
|
||||
{
|
||||
id: "reports",
|
||||
label: "Reports",
|
||||
kind: "report"
|
||||
}
|
||||
]
|
||||
}
|
||||
];
|
||||
}
|
||||
|
||||
function buildReportEntries(program: SemanticProgramIr): ReportEntryModel[] {
|
||||
return [
|
||||
report("reachability", "Reachability", program.diagnostics),
|
||||
report("cycle_time", "Cycle Time", []),
|
||||
report("io_wait", "IO and Wait", []),
|
||||
report("post", "Post Process", []),
|
||||
report("import", "Import", [])
|
||||
];
|
||||
}
|
||||
|
||||
function report(kind: ReportEntryModel["kind"], label: string, diagnostics: MotionDiagnostic[]): ReportEntryModel {
|
||||
return {
|
||||
id: `report:${kind}`,
|
||||
label,
|
||||
kind,
|
||||
status: diagnostics.some((diagnostic) => diagnostic.severity === "error") ? "error" : "ready",
|
||||
diagnostics
|
||||
};
|
||||
}
|
||||
|
||||
function cursorFromSource(source?: RuntimeSourceLocation): EditorModel["cursor"] {
|
||||
if (!source?.sourceMap?.line) {
|
||||
return undefined;
|
||||
}
|
||||
return {
|
||||
line: source.sourceMap.line,
|
||||
column: source.sourceMap.column ?? 1
|
||||
};
|
||||
}
|
||||
|
||||
function currentMotionPoint(motion?: MotionQueueSnapshot) {
|
||||
const item = motion?.items[motion.activeIndex];
|
||||
return item?.planned?.points.at(-1);
|
||||
}
|
||||
31
kdl-wasm/web/src/workspace/index.ts
Normal file
31
kdl-wasm/web/src/workspace/index.ts
Normal file
@@ -0,0 +1,31 @@
|
||||
export {
|
||||
MemoryWorkspaceStorage,
|
||||
MemoryWorkspaceStore,
|
||||
ProjectWorkspace,
|
||||
createProjectManifest,
|
||||
detectWorkspaceDamage,
|
||||
exportWorkspaceBundle,
|
||||
importWorkspaceBundle,
|
||||
initializeWorkspace,
|
||||
migrateManifest,
|
||||
readJson,
|
||||
readManifest,
|
||||
readProjectModel,
|
||||
restoreWorkspace,
|
||||
snapshotWorkspace,
|
||||
stableStringify,
|
||||
writeJson,
|
||||
writeProjectModel,
|
||||
type CompatibilityProjectManifest,
|
||||
type CompatibilityWorkspaceBundle,
|
||||
type CompatibilityWorkspaceDamage,
|
||||
type CompatibilityWorkspaceIntegrityReport,
|
||||
type CompatibilityWorkspaceSnapshot,
|
||||
type ProjectManifest,
|
||||
type WorkspaceBundle,
|
||||
type WorkspaceDamageReport,
|
||||
type WorkspaceFileEntry,
|
||||
type WorkspaceSnapshot,
|
||||
type WorkspaceStorage,
|
||||
type WorkspaceStore
|
||||
} from "./storage.js";
|
||||
597
kdl-wasm/web/src/workspace/storage.ts
Normal file
597
kdl-wasm/web/src/workspace/storage.ts
Normal file
@@ -0,0 +1,597 @@
|
||||
import { createHash } from "node:crypto";
|
||||
import type { OlpProjectModel } from "../olp/index.js";
|
||||
import { createOlpProject, validateOlpProject } from "../olp/index.js";
|
||||
|
||||
export interface ProjectManifest {
|
||||
schemaVersion: "workspace/0.1";
|
||||
projectId: string;
|
||||
projectName: string;
|
||||
revision?: string;
|
||||
createdAt: string;
|
||||
updatedAt: string;
|
||||
layout: {
|
||||
root: string;
|
||||
projectJson: string;
|
||||
programsDir: string;
|
||||
reportsDir: string;
|
||||
snapshotsDir: string;
|
||||
};
|
||||
files: WorkspaceFileEntry[];
|
||||
migrations: string[];
|
||||
}
|
||||
|
||||
export interface WorkspaceFileEntry {
|
||||
path: string;
|
||||
kind: "json" | "text" | "binary";
|
||||
sha256: string;
|
||||
bytes: number;
|
||||
}
|
||||
|
||||
export interface WorkspaceSnapshot {
|
||||
id: string;
|
||||
manifest: ProjectManifest;
|
||||
files: Record<string, string>;
|
||||
}
|
||||
|
||||
export interface WorkspaceBundle {
|
||||
format: "kdl-workspace-bundle/0.1";
|
||||
manifest: ProjectManifest;
|
||||
files: Record<string, string>;
|
||||
checksum?: string;
|
||||
}
|
||||
|
||||
export interface WorkspaceDamageReport {
|
||||
ok: boolean;
|
||||
missing: string[];
|
||||
checksumMismatch: string[];
|
||||
diagnostics: string[];
|
||||
}
|
||||
|
||||
export interface WorkspaceStore {
|
||||
writeText(path: string, value: string): Promise<void>;
|
||||
readText(path: string): Promise<string>;
|
||||
delete(path: string): Promise<void>;
|
||||
list(prefix?: string): Promise<string[]>;
|
||||
}
|
||||
|
||||
export class MemoryWorkspaceStore implements WorkspaceStore {
|
||||
private readonly files = new Map<string, string>();
|
||||
|
||||
constructor(initialFiles: Record<string, string> = {}) {
|
||||
for (const [path, value] of Object.entries(initialFiles)) {
|
||||
this.files.set(normalizePath(path), value);
|
||||
}
|
||||
}
|
||||
|
||||
async writeText(path: string, value: string): Promise<void> {
|
||||
this.files.set(normalizePath(path), value);
|
||||
}
|
||||
|
||||
async readText(path: string): Promise<string> {
|
||||
const normalized = normalizePath(path);
|
||||
const value = this.files.get(normalized);
|
||||
if (value === undefined) {
|
||||
throw new Error(`Workspace file ${normalized} not found`);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
async delete(path: string): Promise<void> {
|
||||
this.files.delete(normalizePath(path));
|
||||
}
|
||||
|
||||
async list(prefix = ""): Promise<string[]> {
|
||||
const normalizedPrefix = normalizePath(prefix);
|
||||
return [...this.files.keys()]
|
||||
.filter((path) => normalizedPrefix === "" || path.startsWith(normalizedPrefix))
|
||||
.sort();
|
||||
}
|
||||
|
||||
async exists(path: string): Promise<boolean> {
|
||||
return this.files.has(normalizePath(path));
|
||||
}
|
||||
|
||||
dump(): Record<string, string> {
|
||||
return Object.fromEntries([...this.files.entries()].sort(([left], [right]) => left.localeCompare(right)));
|
||||
}
|
||||
}
|
||||
|
||||
export const MemoryWorkspaceStorage = MemoryWorkspaceStore;
|
||||
|
||||
export type WorkspaceStorage = WorkspaceStore & {
|
||||
exists?: (path: string) => Promise<boolean>;
|
||||
};
|
||||
|
||||
export interface CompatibilityProjectManifest {
|
||||
schemaVersion: 1;
|
||||
projectId: string;
|
||||
name: string;
|
||||
createdAt: string;
|
||||
updatedAt: string;
|
||||
directories: {
|
||||
model: string;
|
||||
programs: string;
|
||||
reports: string;
|
||||
imports: string;
|
||||
delivery: string;
|
||||
traces: string;
|
||||
};
|
||||
entrypoints: {
|
||||
model: string;
|
||||
};
|
||||
}
|
||||
|
||||
export interface CompatibilityWorkspaceSnapshot {
|
||||
manifest: CompatibilityProjectManifest;
|
||||
files: Record<string, string>;
|
||||
checksum: string;
|
||||
}
|
||||
|
||||
export interface CompatibilityWorkspaceDamage {
|
||||
code: "WORKSPACE_MANIFEST_MISSING" | "WORKSPACE_MANIFEST_INVALID" | "WORKSPACE_MODEL_MISSING" | "WORKSPACE_MODEL_INVALID" | "WORKSPACE_CHECKSUM_MISMATCH";
|
||||
message: string;
|
||||
path?: string;
|
||||
}
|
||||
|
||||
export interface CompatibilityWorkspaceIntegrityReport {
|
||||
ok: boolean;
|
||||
damages: CompatibilityWorkspaceDamage[];
|
||||
}
|
||||
|
||||
export interface CompatibilityWorkspaceBundle {
|
||||
format: "kdl-workspace-bundle";
|
||||
version: 1;
|
||||
manifest: CompatibilityProjectManifest;
|
||||
files: Record<string, string>;
|
||||
checksum: string;
|
||||
}
|
||||
|
||||
const COMPAT_MANIFEST_PATH = "project.json";
|
||||
|
||||
export function createProjectManifest(projectId: string, name: string, now = new Date().toISOString()): CompatibilityProjectManifest {
|
||||
return {
|
||||
schemaVersion: 1,
|
||||
projectId,
|
||||
name,
|
||||
createdAt: now,
|
||||
updatedAt: now,
|
||||
directories: {
|
||||
model: "model",
|
||||
programs: "programs",
|
||||
reports: "reports",
|
||||
imports: "imports",
|
||||
delivery: "delivery",
|
||||
traces: "traces"
|
||||
},
|
||||
entrypoints: {
|
||||
model: "model/olp-project.json"
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
export async function initializeWorkspace(
|
||||
storage: WorkspaceStorage,
|
||||
options: { projectId: string; name: string; model?: OlpProjectModel; now?: string }
|
||||
): Promise<CompatibilityProjectManifest> {
|
||||
const manifest = createProjectManifest(options.projectId, options.name, options.now);
|
||||
await writeJson(storage, COMPAT_MANIFEST_PATH, manifest);
|
||||
await writeJson(storage, manifest.entrypoints.model, options.model ?? createOlpProject({ id: options.projectId, name: options.name }));
|
||||
return manifest;
|
||||
}
|
||||
|
||||
export async function readManifest(storage: WorkspaceStorage): Promise<CompatibilityProjectManifest> {
|
||||
return migrateManifest(await readJson<unknown>(storage, COMPAT_MANIFEST_PATH));
|
||||
}
|
||||
|
||||
export async function readProjectModel(storage: WorkspaceStorage): Promise<OlpProjectModel> {
|
||||
const manifest = await readManifest(storage);
|
||||
return readJson<OlpProjectModel>(storage, manifest.entrypoints.model);
|
||||
}
|
||||
|
||||
export async function writeProjectModel(storage: WorkspaceStorage, model: OlpProjectModel, now = new Date().toISOString()): Promise<void> {
|
||||
const manifest = await readManifest(storage);
|
||||
await writeJson(storage, manifest.entrypoints.model, model);
|
||||
await writeJson(storage, COMPAT_MANIFEST_PATH, { ...manifest, updatedAt: now });
|
||||
}
|
||||
|
||||
export async function readJson<T>(storage: WorkspaceStorage, path: string): Promise<T> {
|
||||
return JSON.parse(await storage.readText(path)) as T;
|
||||
}
|
||||
|
||||
export async function writeJson(storage: WorkspaceStorage, path: string, value: unknown): Promise<void> {
|
||||
await storage.writeText(path, `${stableStringify(value)}\n`);
|
||||
}
|
||||
|
||||
export async function snapshotWorkspace(storage: WorkspaceStorage): Promise<CompatibilityWorkspaceSnapshot> {
|
||||
const files: Record<string, string> = {};
|
||||
for (const path of await storage.list()) {
|
||||
files[path] = await storage.readText(path);
|
||||
}
|
||||
return {
|
||||
manifest: migrateManifest(JSON.parse(files[COMPAT_MANIFEST_PATH] ?? "{}")),
|
||||
files,
|
||||
checksum: checksumFiles(files)
|
||||
};
|
||||
}
|
||||
|
||||
export async function restoreWorkspace(storage: WorkspaceStorage, snapshot: CompatibilityWorkspaceSnapshot): Promise<void> {
|
||||
for (const path of await storage.list()) {
|
||||
await storage.delete(path);
|
||||
}
|
||||
for (const [path, value] of Object.entries(snapshot.files)) {
|
||||
await storage.writeText(path, value);
|
||||
}
|
||||
}
|
||||
|
||||
export async function exportWorkspaceBundle(storage: WorkspaceStorage): Promise<string> {
|
||||
const snapshot = await snapshotWorkspace(storage);
|
||||
const bundle: CompatibilityWorkspaceBundle = {
|
||||
format: "kdl-workspace-bundle",
|
||||
version: 1,
|
||||
manifest: snapshot.manifest,
|
||||
files: snapshot.files,
|
||||
checksum: snapshot.checksum
|
||||
};
|
||||
return `${stableStringify(bundle)}\n`;
|
||||
}
|
||||
|
||||
export async function importWorkspaceBundle(storage: WorkspaceStorage, text: string): Promise<CompatibilityWorkspaceIntegrityReport> {
|
||||
const bundle = JSON.parse(text) as CompatibilityWorkspaceBundle;
|
||||
if (bundle.format !== "kdl-workspace-bundle" || bundle.version !== 1) {
|
||||
throw new Error("Unsupported workspace bundle");
|
||||
}
|
||||
if (checksumFiles(bundle.files) !== bundle.checksum) {
|
||||
return {
|
||||
ok: false,
|
||||
damages: [{ code: "WORKSPACE_CHECKSUM_MISMATCH", message: "Bundle checksum does not match file contents" }]
|
||||
};
|
||||
}
|
||||
await restoreWorkspace(storage, {
|
||||
manifest: bundle.manifest,
|
||||
files: bundle.files,
|
||||
checksum: bundle.checksum
|
||||
});
|
||||
return detectWorkspaceDamage(storage, bundle.checksum);
|
||||
}
|
||||
|
||||
export async function detectWorkspaceDamage(storage: WorkspaceStorage, expectedChecksum?: string): Promise<CompatibilityWorkspaceIntegrityReport> {
|
||||
const damages: CompatibilityWorkspaceDamage[] = [];
|
||||
const exists = storage.exists
|
||||
? await storage.exists(COMPAT_MANIFEST_PATH)
|
||||
: (await storage.list()).includes(COMPAT_MANIFEST_PATH);
|
||||
if (!exists) {
|
||||
return {
|
||||
ok: false,
|
||||
damages: [{ code: "WORKSPACE_MANIFEST_MISSING", message: "project.json is missing", path: COMPAT_MANIFEST_PATH }]
|
||||
};
|
||||
}
|
||||
|
||||
let manifest: CompatibilityProjectManifest | undefined;
|
||||
try {
|
||||
manifest = await readManifest(storage);
|
||||
} catch (cause) {
|
||||
damages.push({ code: "WORKSPACE_MANIFEST_INVALID", message: `project.json is invalid: ${String(cause)}`, path: COMPAT_MANIFEST_PATH });
|
||||
}
|
||||
|
||||
if (manifest) {
|
||||
const modelExists = storage.exists
|
||||
? await storage.exists(manifest.entrypoints.model)
|
||||
: (await storage.list()).includes(manifest.entrypoints.model);
|
||||
if (!modelExists) {
|
||||
damages.push({ code: "WORKSPACE_MODEL_MISSING", message: `Project model ${manifest.entrypoints.model} is missing`, path: manifest.entrypoints.model });
|
||||
} else {
|
||||
try {
|
||||
const model = await readJson<OlpProjectModel>(storage, manifest.entrypoints.model);
|
||||
const validation = validateOlpProject(model);
|
||||
for (const issue of validation.issues.filter((item) => item.severity === "error")) {
|
||||
damages.push({ code: "WORKSPACE_MODEL_INVALID", message: `${issue.code}: ${issue.message}`, path: manifest.entrypoints.model });
|
||||
}
|
||||
} catch (cause) {
|
||||
damages.push({ code: "WORKSPACE_MODEL_INVALID", message: `Project model is invalid: ${String(cause)}`, path: manifest.entrypoints.model });
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (expectedChecksum) {
|
||||
const snapshot = await snapshotWorkspace(storage);
|
||||
if (snapshot.checksum !== expectedChecksum) {
|
||||
damages.push({ code: "WORKSPACE_CHECKSUM_MISMATCH", message: "Workspace checksum does not match expected snapshot" });
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
ok: damages.length === 0,
|
||||
damages
|
||||
};
|
||||
}
|
||||
|
||||
export function migrateManifest(input: unknown): CompatibilityProjectManifest {
|
||||
if (typeof input !== "object" || input === null || Array.isArray(input)) {
|
||||
throw new Error("Manifest must be an object");
|
||||
}
|
||||
const record = input as Record<string, unknown>;
|
||||
if (record.schemaVersion === 1) {
|
||||
return normalizeCompatibilityManifest(record);
|
||||
}
|
||||
if (record.schemaVersion === 0 || record.version === 0) {
|
||||
const updatedAt = typeof record.updatedAt === "string" ? record.updatedAt : new Date(0).toISOString();
|
||||
return {
|
||||
...createProjectManifest(
|
||||
typeof record.projectId === "string" ? record.projectId : "legacy_project",
|
||||
typeof record.name === "string" ? record.name : "Legacy Project",
|
||||
typeof record.createdAt === "string" ? record.createdAt : updatedAt
|
||||
),
|
||||
updatedAt,
|
||||
entrypoints: {
|
||||
model: typeof record.modelPath === "string" ? record.modelPath : "model/olp-project.json"
|
||||
}
|
||||
};
|
||||
}
|
||||
throw new Error(`Unsupported manifest schema ${String(record.schemaVersion)}`);
|
||||
}
|
||||
|
||||
export function stableStringify(value: unknown): string {
|
||||
return JSON.stringify(sortForStableStringify(value), null, 2);
|
||||
}
|
||||
|
||||
function normalizeCompatibilityManifest(record: Record<string, unknown>): CompatibilityProjectManifest {
|
||||
if (record.schemaVersion !== 1) {
|
||||
throw new Error("Manifest schemaVersion must be 1");
|
||||
}
|
||||
if (typeof record.projectId !== "string" || typeof record.name !== "string") {
|
||||
throw new Error("Manifest projectId and name are required");
|
||||
}
|
||||
const createdAt = typeof record.createdAt === "string" ? record.createdAt : new Date(0).toISOString();
|
||||
const updatedAt = typeof record.updatedAt === "string" ? record.updatedAt : createdAt;
|
||||
const directories = isRecord(record.directories) ? record.directories : {};
|
||||
const entrypoints = isRecord(record.entrypoints) ? record.entrypoints : {};
|
||||
return {
|
||||
...createProjectManifest(record.projectId, record.name, createdAt),
|
||||
updatedAt,
|
||||
directories: {
|
||||
model: stringField(directories.model, "model"),
|
||||
programs: stringField(directories.programs, "programs"),
|
||||
reports: stringField(directories.reports, "reports"),
|
||||
imports: stringField(directories.imports, "imports"),
|
||||
delivery: stringField(directories.delivery, "delivery"),
|
||||
traces: stringField(directories.traces, "traces")
|
||||
},
|
||||
entrypoints: {
|
||||
model: stringField(entrypoints.model, "model/olp-project.json")
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
function checksumFiles(files: Record<string, string>): string {
|
||||
const content = Object.entries(files)
|
||||
.sort(([left], [right]) => left.localeCompare(right))
|
||||
.map(([path, value]) => `${path}\0${value}`)
|
||||
.join("\0");
|
||||
let hash = 0x811c9dc5;
|
||||
for (let index = 0; index < content.length; index += 1) {
|
||||
hash ^= content.charCodeAt(index);
|
||||
hash = Math.imul(hash, 0x01000193) >>> 0;
|
||||
}
|
||||
return `fnv1a32:${hash.toString(16).padStart(8, "0")}`;
|
||||
}
|
||||
|
||||
function sortForStableStringify(value: unknown): unknown {
|
||||
if (Array.isArray(value)) {
|
||||
return value.map(sortForStableStringify);
|
||||
}
|
||||
if (isRecord(value)) {
|
||||
return Object.fromEntries(
|
||||
Object.entries(value)
|
||||
.filter(([, item]) => item !== undefined)
|
||||
.sort(([left], [right]) => left.localeCompare(right))
|
||||
.map(([key, item]) => [key, sortForStableStringify(item)])
|
||||
);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
function isRecord(value: unknown): value is Record<string, unknown> {
|
||||
return typeof value === "object" && value !== null && !Array.isArray(value);
|
||||
}
|
||||
|
||||
function stringField(value: unknown, fallback: string): string {
|
||||
return typeof value === "string" ? value : fallback;
|
||||
}
|
||||
|
||||
export class ProjectWorkspace {
|
||||
constructor(private readonly store: WorkspaceStore) {}
|
||||
|
||||
async createProject(model: OlpProjectModel): Promise<ProjectManifest> {
|
||||
const now = new Date().toISOString();
|
||||
const manifest = this.createManifest(model, now, now);
|
||||
await this.store.writeText(manifest.layout.projectJson, JSON.stringify(model, null, 2));
|
||||
await this.store.writeText(this.manifestPath(model.project.id), JSON.stringify(await this.refreshManifest(manifest), null, 2));
|
||||
return this.readManifest(model.project.id);
|
||||
}
|
||||
|
||||
async readProject(projectId: string): Promise<OlpProjectModel> {
|
||||
const manifest = await this.readManifest(projectId);
|
||||
return JSON.parse(await this.store.readText(manifest.layout.projectJson)) as OlpProjectModel;
|
||||
}
|
||||
|
||||
async writeJson(projectId: string, path: string, value: unknown): Promise<void> {
|
||||
await this.store.writeText(this.projectPath(projectId, path), JSON.stringify(value, null, 2));
|
||||
await this.rewriteManifest(projectId);
|
||||
}
|
||||
|
||||
async readJson<T>(projectId: string, path: string): Promise<T> {
|
||||
return JSON.parse(await this.store.readText(this.projectPath(projectId, path))) as T;
|
||||
}
|
||||
|
||||
async writeText(projectId: string, path: string, value: string): Promise<void> {
|
||||
await this.store.writeText(this.projectPath(projectId, path), value);
|
||||
await this.rewriteManifest(projectId);
|
||||
}
|
||||
|
||||
async readText(projectId: string, path: string): Promise<string> {
|
||||
return this.store.readText(this.projectPath(projectId, path));
|
||||
}
|
||||
|
||||
async delete(projectId: string, path: string): Promise<void> {
|
||||
await this.store.delete(this.projectPath(projectId, path));
|
||||
await this.rewriteManifest(projectId);
|
||||
}
|
||||
|
||||
async snapshot(projectId: string, id = `snapshot-${Date.now()}`): Promise<WorkspaceSnapshot> {
|
||||
const manifest = await this.readManifest(projectId);
|
||||
const files = await this.projectFiles(projectId);
|
||||
const snapshot: WorkspaceSnapshot = { id, manifest, files };
|
||||
await this.store.writeText(`${manifest.layout.snapshotsDir}/${id}.json`, JSON.stringify(snapshot, null, 2));
|
||||
await this.rewriteManifest(projectId);
|
||||
return snapshot;
|
||||
}
|
||||
|
||||
async restoreSnapshot(snapshot: WorkspaceSnapshot): Promise<ProjectManifest> {
|
||||
for (const [path, value] of Object.entries(snapshot.files)) {
|
||||
await this.store.writeText(path, value);
|
||||
}
|
||||
await this.store.writeText(this.manifestPath(snapshot.manifest.projectId), JSON.stringify(snapshot.manifest, null, 2));
|
||||
return this.rewriteManifest(snapshot.manifest.projectId);
|
||||
}
|
||||
|
||||
async exportBundle(projectId: string): Promise<string> {
|
||||
const bundle: WorkspaceBundle = {
|
||||
format: "kdl-workspace-bundle/0.1",
|
||||
manifest: await this.readManifest(projectId),
|
||||
files: await this.projectFiles(projectId)
|
||||
};
|
||||
return JSON.stringify(bundle, null, 2);
|
||||
}
|
||||
|
||||
async importBundle(text: string): Promise<ProjectManifest> {
|
||||
const bundle = JSON.parse(text) as WorkspaceBundle;
|
||||
if (bundle.format !== "kdl-workspace-bundle/0.1") {
|
||||
throw new Error(`Unsupported workspace bundle ${String(bundle.format)}`);
|
||||
}
|
||||
for (const [path, value] of Object.entries(bundle.files)) {
|
||||
await this.store.writeText(path, value);
|
||||
}
|
||||
await this.store.writeText(this.manifestPath(bundle.manifest.projectId), JSON.stringify(bundle.manifest, null, 2));
|
||||
return this.rewriteManifest(bundle.manifest.projectId);
|
||||
}
|
||||
|
||||
async migrate(projectId: string, migrationId: string, apply: (model: OlpProjectModel) => OlpProjectModel): Promise<ProjectManifest> {
|
||||
const manifest = await this.readManifest(projectId);
|
||||
if (manifest.migrations.includes(migrationId)) {
|
||||
return manifest;
|
||||
}
|
||||
const model = apply(await this.readProject(projectId));
|
||||
model.metadata = {
|
||||
...(model.metadata ?? {}),
|
||||
lastMigration: migrationId
|
||||
};
|
||||
await this.store.writeText(manifest.layout.projectJson, JSON.stringify(model, null, 2));
|
||||
const migrated = {
|
||||
...manifest,
|
||||
migrations: [...manifest.migrations, migrationId],
|
||||
updatedAt: new Date().toISOString()
|
||||
};
|
||||
await this.store.writeText(this.manifestPath(projectId), JSON.stringify(await this.refreshManifest(migrated), null, 2));
|
||||
return this.readManifest(projectId);
|
||||
}
|
||||
|
||||
async checkDamage(projectId: string): Promise<WorkspaceDamageReport> {
|
||||
const manifest = await this.readManifest(projectId);
|
||||
const missing: string[] = [];
|
||||
const checksumMismatch: string[] = [];
|
||||
for (const file of manifest.files) {
|
||||
try {
|
||||
const text = await this.store.readText(file.path);
|
||||
if (sha256(text) !== file.sha256) {
|
||||
checksumMismatch.push(file.path);
|
||||
}
|
||||
} catch {
|
||||
missing.push(file.path);
|
||||
}
|
||||
}
|
||||
const diagnostics = [
|
||||
...missing.map((path) => `missing:${path}`),
|
||||
...checksumMismatch.map((path) => `checksum:${path}`)
|
||||
];
|
||||
return {
|
||||
ok: missing.length === 0 && checksumMismatch.length === 0,
|
||||
missing,
|
||||
checksumMismatch,
|
||||
diagnostics
|
||||
};
|
||||
}
|
||||
|
||||
async readManifest(projectId: string): Promise<ProjectManifest> {
|
||||
return JSON.parse(await this.store.readText(this.manifestPath(projectId))) as ProjectManifest;
|
||||
}
|
||||
|
||||
private createManifest(model: OlpProjectModel, createdAt: string, updatedAt: string): ProjectManifest {
|
||||
const root = `/projects/${model.project.id}`;
|
||||
return {
|
||||
schemaVersion: "workspace/0.1",
|
||||
projectId: model.project.id,
|
||||
projectName: model.project.name,
|
||||
...(model.project.revision ? { revision: model.project.revision } : {}),
|
||||
createdAt,
|
||||
updatedAt,
|
||||
layout: {
|
||||
root,
|
||||
projectJson: `${root}/project.json`,
|
||||
programsDir: `${root}/programs`,
|
||||
reportsDir: `${root}/reports`,
|
||||
snapshotsDir: `${root}/snapshots`
|
||||
},
|
||||
files: [],
|
||||
migrations: []
|
||||
};
|
||||
}
|
||||
|
||||
private async rewriteManifest(projectId: string): Promise<ProjectManifest> {
|
||||
const manifest = await this.readManifest(projectId);
|
||||
const refreshed = await this.refreshManifest({
|
||||
...manifest,
|
||||
updatedAt: new Date().toISOString()
|
||||
});
|
||||
await this.store.writeText(this.manifestPath(projectId), JSON.stringify(refreshed, null, 2));
|
||||
return refreshed;
|
||||
}
|
||||
|
||||
private async refreshManifest(manifest: ProjectManifest): Promise<ProjectManifest> {
|
||||
const files = await this.projectFiles(manifest.projectId);
|
||||
return {
|
||||
...manifest,
|
||||
files: Object.entries(files)
|
||||
.map(([path, text]): WorkspaceFileEntry => ({
|
||||
path,
|
||||
kind: path.endsWith(".json") ? "json" : "text",
|
||||
sha256: sha256(text),
|
||||
bytes: Buffer.byteLength(text)
|
||||
}))
|
||||
.sort((left, right) => left.path.localeCompare(right.path))
|
||||
};
|
||||
}
|
||||
|
||||
private async projectFiles(projectId: string): Promise<Record<string, string>> {
|
||||
const root = `/projects/${projectId}`;
|
||||
const paths = (await this.store.list(root)).filter((path) => path !== this.manifestPath(projectId));
|
||||
const entries = await Promise.all(paths.map(async (path) => [path, await this.store.readText(path)] as const));
|
||||
return Object.fromEntries(entries.sort(([left], [right]) => left.localeCompare(right)));
|
||||
}
|
||||
|
||||
private manifestPath(projectId: string): string {
|
||||
return `/projects/${projectId}/project.manifest.json`;
|
||||
}
|
||||
|
||||
private projectPath(projectId: string, path: string): string {
|
||||
const normalized = normalizePath(path);
|
||||
return normalized.startsWith(`/projects/${projectId}/`) ? normalized : `/projects/${projectId}/${normalized}`;
|
||||
}
|
||||
}
|
||||
|
||||
function normalizePath(path: string): string {
|
||||
return path.replace(/\\/g, "/").replace(/\/+/g, "/").replace(/\/$/, "").replace(/^\.\//, "");
|
||||
}
|
||||
|
||||
function sha256(text: string): string {
|
||||
return createHash("sha256").update(text).digest("hex");
|
||||
}
|
||||
46
kdl-wasm/web/test-results/virtual-controller/evidence.json
Normal file
46
kdl-wasm/web/test-results/virtual-controller/evidence.json
Normal file
@@ -0,0 +1,46 @@
|
||||
{
|
||||
"app": "E:\\Work\\kdl_work\\kdl-wasm\\web\\app\\virtual-controller.html",
|
||||
"chrome": "C:\\Program Files\\Google\\Chrome\\Application\\chrome.exe",
|
||||
"desktop": {
|
||||
"viewport": {
|
||||
"width": 1440,
|
||||
"height": 960,
|
||||
"deviceScaleFactor": 1,
|
||||
"mobile": false
|
||||
},
|
||||
"screenshot": "E:\\Work\\kdl_work\\kdl-wasm\\web\\test-results\\virtual-controller\\virtual-controller-desktop.png",
|
||||
"screenshotBytes": 45666,
|
||||
"state": "running",
|
||||
"wait": "satisfied",
|
||||
"trace": "4 events",
|
||||
"visiblePanels": [
|
||||
".command-bar",
|
||||
".object-tree",
|
||||
".viewport",
|
||||
".pendant",
|
||||
".editor",
|
||||
".bottom-panel"
|
||||
]
|
||||
},
|
||||
"mobile": {
|
||||
"viewport": {
|
||||
"width": 390,
|
||||
"height": 844,
|
||||
"deviceScaleFactor": 2,
|
||||
"mobile": true
|
||||
},
|
||||
"screenshot": "E:\\Work\\kdl_work\\kdl-wasm\\web\\test-results\\virtual-controller\\virtual-controller-mobile.png",
|
||||
"screenshotBytes": 44869,
|
||||
"state": "running",
|
||||
"wait": "satisfied",
|
||||
"trace": "4 events",
|
||||
"visiblePanels": [
|
||||
".command-bar",
|
||||
".object-tree",
|
||||
".viewport",
|
||||
".pendant",
|
||||
".editor",
|
||||
".bottom-panel"
|
||||
]
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 45 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 44 KiB |
127
kdl-wasm/web/tests/controller/virtualController.test.ts
Normal file
127
kdl-wasm/web/tests/controller/virtualController.test.ts
Normal file
@@ -0,0 +1,127 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { SemanticProgramIr } from "../../src/grl/ir/index.js";
|
||||
import { IrExecutionRuntime, VirtualControllerStateMachine } from "../../src/controller/index.js";
|
||||
|
||||
function program(): SemanticProgramIr {
|
||||
return {
|
||||
moduleName: "Main",
|
||||
symbols: [],
|
||||
semanticChecks: [],
|
||||
paths: [],
|
||||
operations: [],
|
||||
diagnostics: [],
|
||||
sourceMap: [
|
||||
{
|
||||
kind: "CALL",
|
||||
id: "call_helper",
|
||||
procedureId: "main",
|
||||
sourceMap: { file: "main.grl", line: 2, column: 5 }
|
||||
},
|
||||
{
|
||||
kind: "ALARM",
|
||||
id: "helper_alarm",
|
||||
procedureId: "helper",
|
||||
sourceMap: { file: "main.grl", line: 6, column: 5 }
|
||||
}
|
||||
],
|
||||
procedures: [
|
||||
{
|
||||
name: "main",
|
||||
sourceMap: { file: "main.grl", line: 1, column: 1 },
|
||||
instructions: [
|
||||
{
|
||||
kind: "RAW_STATEMENT",
|
||||
text: "ready = true",
|
||||
sourceMap: { file: "main.grl", line: 1, column: 5 }
|
||||
},
|
||||
{
|
||||
kind: "CALL",
|
||||
target: "helper",
|
||||
args: [],
|
||||
sourceMap: { file: "main.grl", line: 2, column: 5 }
|
||||
},
|
||||
{
|
||||
kind: "RETURN",
|
||||
sourceMap: { file: "main.grl", line: 3, column: 5 }
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
name: "helper",
|
||||
sourceMap: { file: "main.grl", line: 5, column: 1 },
|
||||
instructions: [
|
||||
{
|
||||
kind: "ALARM",
|
||||
alarmId: "A1",
|
||||
message: "helper alarm",
|
||||
severity: "warning",
|
||||
sourceMap: { file: "main.grl", line: 6, column: 5 }
|
||||
},
|
||||
{
|
||||
kind: "RETURN",
|
||||
sourceMap: { file: "main.grl", line: 7, column: 5 }
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
kdlBridge: {
|
||||
motionRequests: [],
|
||||
pathRequests: []
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
describe("virtual controller state machine and IR execution runtime", () => {
|
||||
it("reports legal and illegal controller command transitions", () => {
|
||||
const machine = new VirtualControllerStateMachine();
|
||||
|
||||
expect(machine.dispatch("run")).toMatchObject({
|
||||
ok: false,
|
||||
state: "unloaded",
|
||||
diagnostic: { code: "VC_INVALID_STATE_TRANSITION" }
|
||||
});
|
||||
expect(machine.dispatch("load")).toMatchObject({ ok: true, previous: "unloaded", state: "stopped" });
|
||||
expect(machine.dispatch("run")).toMatchObject({ ok: true, previous: "stopped", state: "running" });
|
||||
expect(machine.dispatch("pause")).toMatchObject({ ok: true, previous: "running", state: "paused" });
|
||||
expect(machine.dispatch("step")).toMatchObject({ ok: true, previous: "paused", state: "paused" });
|
||||
expect(machine.dispatch("stop")).toMatchObject({ ok: true, previous: "paused", state: "stopped" });
|
||||
expect(machine.snapshot().diagnostics).toHaveLength(1);
|
||||
});
|
||||
|
||||
it("executes PC, calls, scopes, alarms, trace, source map, and breakpoints", () => {
|
||||
const runtime = new IrExecutionRuntime();
|
||||
runtime.load(program());
|
||||
|
||||
expect(runtime.snapshot()).toMatchObject({
|
||||
procedure: "main",
|
||||
pc: 0,
|
||||
scopeStack: [{ id: "global" }, { id: "main" }]
|
||||
});
|
||||
|
||||
runtime.setBreakpoint({ id: "helper-alarm", procedure: "helper", source: { line: 6 }, enabled: true });
|
||||
expect(runtime.step()).toMatchObject({ status: "executed", instruction: { kind: "RAW_STATEMENT" } });
|
||||
expect(runtime.getVariable("ready")).toBe(true);
|
||||
expect(runtime.step()).toMatchObject({ status: "executed", instruction: { kind: "CALL" } });
|
||||
expect(runtime.snapshot()).toMatchObject({
|
||||
procedure: "helper",
|
||||
pc: 0,
|
||||
callStack: [expect.objectContaining({ procedure: "helper", returnTo: { procedure: "main", pc: 2 } })]
|
||||
});
|
||||
|
||||
expect(runtime.step()).toMatchObject({ status: "breakpoint", instruction: { kind: "ALARM" } });
|
||||
expect(runtime.removeBreakpoint("helper-alarm")).toBe(true);
|
||||
expect(runtime.step()).toMatchObject({ status: "executed", instruction: { kind: "ALARM" } });
|
||||
expect(runtime.snapshot().alarmQueue).toEqual([
|
||||
expect.objectContaining({ id: "A1", message: "helper alarm", severity: "warning" })
|
||||
]);
|
||||
expect(runtime.currentSource()).toMatchObject({ sourceMap: { line: 7 } });
|
||||
expect(runtime.run()).toMatchObject({ status: "completed" });
|
||||
expect(runtime.snapshot().trace).toEqual(
|
||||
expect.arrayContaining([
|
||||
expect.objectContaining({ kind: "load" }),
|
||||
expect.objectContaining({ kind: "breakpoint", data: { breakpointId: "helper-alarm" } }),
|
||||
expect.objectContaining({ kind: "alarm", data: { alarmId: "A1", severity: "warning" } })
|
||||
])
|
||||
);
|
||||
});
|
||||
});
|
||||
28
kdl-wasm/web/tests/docs/flowAssetCoverage.test.ts
Normal file
28
kdl-wasm/web/tests/docs/flowAssetCoverage.test.ts
Normal file
@@ -0,0 +1,28 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { join } from "node:path";
|
||||
import { FLOW_ASSET_MAPPINGS, validateFlowAssetCoverage } from "../../src/docs/index.js";
|
||||
|
||||
const FLOW_ROOT = join(process.cwd(), "work/doc/通用机器人项目功能与数据流程图-png");
|
||||
|
||||
describe("flow asset coverage", () => {
|
||||
it("maps every Mermaid and PNG flow asset to tasks and evidence", () => {
|
||||
const result = validateFlowAssetCoverage(FLOW_ROOT);
|
||||
|
||||
expect(result.ok).toBe(true);
|
||||
expect(result.assets.map((asset) => asset.id)).toEqual(["flow-01", "flow-02", "flow-03", "flow-04", "flow-05"]);
|
||||
expect(result.assets.every((asset) => asset.mermaidBytes > 0 && asset.pngBytes > 0)).toBe(true);
|
||||
expect(result.assets.every((asset) => asset.evidence === "EV-211")).toBe(true);
|
||||
expect(result.assets.every((asset) => asset.tasks.some((task) => task.startsWith("KW-211")))).toBe(true);
|
||||
expect(result.diagnostics).toEqual([]);
|
||||
});
|
||||
|
||||
it("keeps the expected coverage contract stable", () => {
|
||||
expect(FLOW_ASSET_MAPPINGS).toEqual([
|
||||
expect.objectContaining({ id: "flow-01", tasks: expect.arrayContaining(["KW-211.1"]) }),
|
||||
expect.objectContaining({ id: "flow-02", tasks: expect.arrayContaining(["KW-211.2"]) }),
|
||||
expect.objectContaining({ id: "flow-03", tasks: expect.arrayContaining(["KW-211.3"]) }),
|
||||
expect.objectContaining({ id: "flow-04", tasks: expect.arrayContaining(["KW-211.4"]) }),
|
||||
expect.objectContaining({ id: "flow-05", tasks: expect.arrayContaining(["KW-211.5"]) })
|
||||
]);
|
||||
});
|
||||
});
|
||||
9
kdl-wasm/web/tests/fixtures/abb120/programs/A120_00_Smoke.grl
vendored
Normal file
9
kdl-wasm/web/tests/fixtures/abb120/programs/A120_00_Smoke.grl
vendored
Normal file
@@ -0,0 +1,9 @@
|
||||
language grl 0.1
|
||||
module A120Smoke
|
||||
const speed v_joint = joint(40 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
|
||||
proc main()
|
||||
movej home speed v_joint zone zf
|
||||
end
|
||||
end
|
||||
22
kdl-wasm/web/tests/fixtures/abb120/programs/A120_10_JointPickPlace.grl
vendored
Normal file
22
kdl-wasm/web/tests/fixtures/abb120/programs/A120_10_JointPickPlace.grl
vendored
Normal file
@@ -0,0 +1,22 @@
|
||||
language grl 0.1
|
||||
module A120JointPickPlace
|
||||
const speed v_fast = joint(40 %)
|
||||
const speed v_slow = joint(10 + 10 %)
|
||||
const zone z10 = z(5 + 5 mm)
|
||||
target home = joint_target { joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
|
||||
target approach = joint_target { joints: [0 deg, -22.918312 deg, 28.647890 deg, 0 deg, 11.459156 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] }
|
||||
path pick_place {
|
||||
defaults { speed: v_fast, zone: z10 }
|
||||
point p_home movej home zone fine
|
||||
point p_approach movej approach
|
||||
event before p_pick io.do[1] = true
|
||||
point p_pick movej pick speed v_slow zone fine
|
||||
point p_place movej place
|
||||
event after p_place io.do[1] = false
|
||||
}
|
||||
proc main()
|
||||
run_path pick_place
|
||||
end
|
||||
end
|
||||
20
kdl-wasm/web/tests/fixtures/abb120/programs/A120_20_CartesianBlend.grl
vendored
Normal file
20
kdl-wasm/web/tests/fixtures/abb120/programs/A120_20_CartesianBlend.grl
vendored
Normal file
@@ -0,0 +1,20 @@
|
||||
language grl 0.1
|
||||
module A120CartesianBlend
|
||||
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_linear = linear(100 + 50 mm/s)
|
||||
const zone z10 = z(clamp(10 mm, 1 mm, 50 mm))
|
||||
target pick = pose_target { pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, atan2(0, 1)), tool: gripper, frame: fixture }
|
||||
target mid = pose_target { pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg), tool: gripper, frame: fixture }
|
||||
target arc_end = pose_target { pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg), tool: gripper, frame: fixture }
|
||||
path cart_path {
|
||||
defaults { speed: v_linear, zone: z10, tool: gripper, frame: fixture }
|
||||
point p_pick movel pick zone fine
|
||||
point p_arc movec via mid target arc_end speed linear(max(50 mm/s, 150 mm/s)) zone z10
|
||||
}
|
||||
proc main()
|
||||
set_tool gripper
|
||||
set_frame fixture
|
||||
run_path cart_path
|
||||
end
|
||||
end
|
||||
17
kdl-wasm/web/tests/fixtures/abb120/programs/A120_30_IOWaitPulse.grl
vendored
Normal file
17
kdl-wasm/web/tests/fixtures/abb120/programs/A120_30_IOWaitPulse.grl
vendored
Normal file
@@ -0,0 +1,17 @@
|
||||
language grl 0.1
|
||||
module A120IOWaitPulse
|
||||
const speed v = joint(35 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
|
||||
path io_path {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p0 movej home
|
||||
event at p0 distance 5 + 5 mm pulse io.do[20] duration 50 + 50 ms
|
||||
}
|
||||
proc main()
|
||||
io.do[1] = true
|
||||
wait io.di[1] == true timeout 1 + 1 s on_timeout alarm "DI1 timeout"
|
||||
pulse io.do[2] duration 50 + 50 ms
|
||||
run_path io_path
|
||||
end
|
||||
end
|
||||
12
kdl-wasm/web/tests/fixtures/abb120/programs/A120_40_ErrorDiagnostics.grl
vendored
Normal file
12
kdl-wasm/web/tests/fixtures/abb120/programs/A120_40_ErrorDiagnostics.grl
vendored
Normal file
@@ -0,0 +1,12 @@
|
||||
language grl 0.1
|
||||
module A120ErrorDiagnostics
|
||||
const speed v = joint(30 %)
|
||||
const zone zf = fine
|
||||
target over_limit = joint_target { joints: [200 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
|
||||
target unreachable_pose = pose_target { pose: pose(3000 mm, 0 mm, 3000 mm, 0 deg, 0 deg, 0 deg) }
|
||||
proc main()
|
||||
movej over_limit speed v zone zf
|
||||
movel unreachable_pose speed linear(100 mm/s) zone zf
|
||||
wait io.di[99] == true timeout 10 ms on_timeout alarm "expected timeout"
|
||||
end
|
||||
end
|
||||
25
kdl-wasm/web/tests/fixtures/abb120/programs/A120_50_OperationProcess.grl
vendored
Normal file
25
kdl-wasm/web/tests/fixtures/abb120/programs/A120_50_OperationProcess.grl
vendored
Normal file
@@ -0,0 +1,25 @@
|
||||
language grl 0.1
|
||||
module A120OperationProcess
|
||||
const speed v = joint(35 %)
|
||||
const zone zf = fine
|
||||
target home = joint_target { joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg] }
|
||||
path op_path {
|
||||
defaults { speed: v, zone: zf }
|
||||
point p0 movej home
|
||||
}
|
||||
operation pick_op {
|
||||
kind: handling
|
||||
path: op_path
|
||||
process {
|
||||
dwell_ms: 50 + 50 ms,
|
||||
clamp_force: max(20, 10)
|
||||
}
|
||||
start_action:
|
||||
io.do[1] = true
|
||||
end_action:
|
||||
io.do[1] = false
|
||||
}
|
||||
proc main()
|
||||
run_operation pick_op
|
||||
end
|
||||
end
|
||||
11
kdl-wasm/web/tests/fixtures/abbIrb120.ts
vendored
Normal file
11
kdl-wasm/web/tests/fixtures/abbIrb120.ts
vendored
Normal file
@@ -0,0 +1,11 @@
|
||||
export {
|
||||
ABB120_ROBOT_FIXTURE,
|
||||
ABB_IRB120_3_58_URDF,
|
||||
ABB_IRB120_APPROACH_JOINTS,
|
||||
ABB_IRB120_APPROACH_JOINTS as 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 "../../src/fixtures/abb120.js";
|
||||
141
kdl-wasm/web/tests/grl/expressionCompile.test.ts
Normal file
141
kdl-wasm/web/tests/grl/expressionCompile.test.ts
Normal file
@@ -0,0 +1,141 @@
|
||||
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 { parseGrlExpression } from "../../src/grl/parser/expressionParser.js";
|
||||
import {
|
||||
buildMotionContext,
|
||||
compileGrlDataDeclaration,
|
||||
compileGrlTargetDeclaration,
|
||||
compilePathToPlanRequest,
|
||||
parseIoFlowStatements
|
||||
} from "../../src/grl/semantic/index.js";
|
||||
import { evaluateNumberExpression } from "../../src/grl/semantic/constantExpression.js";
|
||||
import { lexGrl } from "../../src/grl/lexer/index.js";
|
||||
|
||||
function expression(source: string) {
|
||||
return parseGrlExpression(lexGrl(source, { preserveComments: false }).filter((token) => token.kind !== "eof"));
|
||||
}
|
||||
|
||||
describe("GRL expression arithmetic and constant folding", () => {
|
||||
it("parses arithmetic, comparison, and logical precedence into AST nodes", () => {
|
||||
expect(expression("1 + 2 * 3")).toMatchObject({
|
||||
kind: "BinaryExpression",
|
||||
operator: "+",
|
||||
right: {
|
||||
kind: "BinaryExpression",
|
||||
operator: "*"
|
||||
}
|
||||
});
|
||||
expect(expression("(1 + 2) * 3")).toMatchObject({
|
||||
kind: "BinaryExpression",
|
||||
operator: "*",
|
||||
left: {
|
||||
kind: "BinaryExpression",
|
||||
operator: "+"
|
||||
}
|
||||
});
|
||||
expect(expression("not (a == b) or c != d")).toMatchObject({
|
||||
kind: "BinaryExpression",
|
||||
operator: "or",
|
||||
left: {
|
||||
kind: "UnaryExpression",
|
||||
operator: "not"
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
it("evaluates math functions, trigonometry, units, and diagnostics", () => {
|
||||
expect(evaluateNumberExpression(expression("100 + 50 mm/s"), {
|
||||
expectedKind: "linear_velocity",
|
||||
defaultUnit: "mm/s"
|
||||
})).toBeCloseTo(0.15);
|
||||
expect(evaluateNumberExpression(expression("(10 + 5) deg"), {
|
||||
expectedKind: "angle",
|
||||
defaultUnit: "deg"
|
||||
})).toBeCloseTo(Math.PI / 12);
|
||||
expect(evaluateNumberExpression(expression("clamp(20 mm, 1 mm, 10 mm)"), {
|
||||
expectedKind: "length",
|
||||
defaultUnit: "mm"
|
||||
})).toBeCloseTo(0.01);
|
||||
});
|
||||
|
||||
it("reports stable expression errors", () => {
|
||||
expect(() => evaluateNumberExpression(expression("sqrt(-1)"))).toThrowError(expect.objectContaining({
|
||||
code: "GRL_EXPR_DOMAIN"
|
||||
}));
|
||||
expect(() => evaluateNumberExpression(expression("10 mm + 2 s"))).toThrowError(expect.objectContaining({
|
||||
code: "GRL_EXPR_UNIT_MISMATCH"
|
||||
}));
|
||||
expect(() => evaluateNumberExpression(expression("1 / 0"))).toThrowError(expect.objectContaining({
|
||||
code: "GRL_EXPR_DIV_ZERO"
|
||||
}));
|
||||
});
|
||||
|
||||
it("folds speed, zone, target, path event, wait, and pulse expressions", () => {
|
||||
const program = parseGrl(`language grl 0.1
|
||||
module MathMotion
|
||||
const num blend_base = 5 + 5
|
||||
const speed v_pick = linear(100 + 50 mm/s)
|
||||
const speed v_safe = linear(max(50 mm/s, 200 mm/s / 2))
|
||||
const zone z_app = z(clamp(blend_base mm, 1 mm, 50 mm))
|
||||
target home = joint_target {
|
||||
joints: [0 deg, (10 + 5) deg, -90 deg]
|
||||
}
|
||||
target pick = pose_target {
|
||||
pose: pose(400 + 50 mm, 20 * 2 mm, sqrt(90000) mm, 0 deg, 0 deg, atan2(1, 1))
|
||||
}
|
||||
path main_path {
|
||||
defaults { speed: v_pick, zone: z_app }
|
||||
point p0 movej home speed linear(100 + 50 mm/s) zone z(5 + 5 mm)
|
||||
event at p0 distance 5 + 5 mm pulse io.do[1] duration 50 + 50 ms
|
||||
}
|
||||
proc main()
|
||||
wait io.di[1] == true timeout 1 + 1 s
|
||||
pulse io.do[2] duration 50 + 50 ms
|
||||
end
|
||||
end
|
||||
`);
|
||||
const declarations = program.module.declarations;
|
||||
const [blendBase, vPick, vSafe] = declarations.filter(
|
||||
(decl): decl is GrlDataDeclaration => decl.kind === "DataDeclaration"
|
||||
);
|
||||
expect(compileGrlDataDeclaration(blendBase!)).toMatchObject({ value: 10 });
|
||||
expect(compileGrlDataDeclaration(vPick!).value).toMatchObject({ kind: "linear" });
|
||||
expect((compileGrlDataDeclaration(vPick!).value as { velocity: number }).velocity).toBeCloseTo(0.15);
|
||||
expect(compileGrlDataDeclaration(vSafe!).value).toMatchObject({ kind: "linear" });
|
||||
expect((compileGrlDataDeclaration(vSafe!).value as { velocity: number }).velocity).toBeCloseTo(0.1);
|
||||
|
||||
const [home, pick] = declarations.filter((decl): decl is GrlTargetDeclaration => decl.kind === "TargetDeclaration");
|
||||
expect(compileGrlTargetDeclaration(home!)).toMatchObject({
|
||||
target: {
|
||||
joints: [0, Math.PI / 12, -Math.PI / 2]
|
||||
}
|
||||
});
|
||||
const pickTarget = compileGrlTargetDeclaration(pick!);
|
||||
expect("pose" in pickTarget.target && pickTarget.target.pose.position).toEqual([0.45, 0.04, 0.3]);
|
||||
|
||||
const context = buildMotionContext(
|
||||
declarations.filter(
|
||||
(decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
|
||||
decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
|
||||
)
|
||||
);
|
||||
const path = declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
|
||||
const compiledPath = compilePathToPlanRequest(path, context, {
|
||||
startJoints: [0, 0, 0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
expect(compiledPath.request.segments[0]).toMatchObject({
|
||||
speed: { kind: "linear" },
|
||||
zone: { kind: "distance", value: 0.01 }
|
||||
});
|
||||
expect((compiledPath.request.segments[0]?.speed as { velocity: number }).velocity).toBeCloseTo(0.15);
|
||||
expect(compiledPath.events[0]).toMatchObject({ distance: 0.01 });
|
||||
|
||||
const procedure = declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
|
||||
expect(parseIoFlowStatements(procedure.bodyTokens)).toEqual([
|
||||
expect.objectContaining({ kind: "WAIT", timeout: 2 }),
|
||||
expect.objectContaining({ kind: "PULSE", duration: 0.1 })
|
||||
]);
|
||||
});
|
||||
});
|
||||
144
kdl-wasm/web/tests/importers/brandImport.test.ts
Normal file
144
kdl-wasm/web/tests/importers/brandImport.test.ts
Normal file
@@ -0,0 +1,144 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { postProcessAllBrands } from "../../src/grl/post/index.js";
|
||||
import {
|
||||
importBrandProgram,
|
||||
parseAbbRapid,
|
||||
parseFanucLs,
|
||||
parseKukaKrl
|
||||
} from "../../src/importers/index.js";
|
||||
import { applyPatch, createOlpProject, validateOlpProject } from "../../src/olp/index.js";
|
||||
|
||||
const ABB_MOD = `MODULE PickPlace
|
||||
PERS tooldata gripper:=[TRUE,[[0,0,100],[1,0,0,0]],[1,[0,0,0],[1,0,0,0],0,0,0]];
|
||||
CONST robtarget pPick := [[500,0,100],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
||||
CONST robtarget pMid := [[550,50,100],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
||||
CONST robtarget pPlace := [[600,0,100],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
||||
CONST jointtarget jHome := [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
|
||||
PROC main()
|
||||
MoveJ jHome,v100,fine,gripper;
|
||||
MoveL pPick,v200,z10,gripper;
|
||||
MoveC pMid,pPlace,v200,fine,gripper;
|
||||
ENDPROC
|
||||
ENDMODULE`;
|
||||
|
||||
const KUKA_SRC = `DEF PickPlace()
|
||||
$TOOL = TOOL_DATA[1]
|
||||
$BASE = BASE_DATA[2]
|
||||
PTP HOME
|
||||
LIN XPICK
|
||||
CIRC XMID, XPLACE
|
||||
END`;
|
||||
|
||||
const KUKA_DAT = `DEFDAT PickPlace
|
||||
DECL E6AXIS HOME={A1 0,A2 0,A3 0,A4 0,A5 0,A6 0,E1 0}
|
||||
DECL E6POS XPICK={X 500,Y 0,Z 100,A 0,B 0,C 0,S 2,T 35,E1 0}
|
||||
DECL E6POS XMID={X 550,Y 50,Z 100,A 0,B 0,C 0,S 2,T 35,E1 0}
|
||||
DECL E6POS XPLACE={X 600,Y 0,Z 100,A 0,B 0,C 0,S 2,T 35,E1 0}
|
||||
ENDDAT`;
|
||||
|
||||
const FANUC_LS = `/PROG PICKPLACE
|
||||
/MN
|
||||
1:J P[1] 50% FINE ;
|
||||
2:L P[2] 200mm/sec CNT10 ;
|
||||
3:C P[3] P[4] 200mm/sec FINE ;
|
||||
/POS
|
||||
P[1]{
|
||||
X = 0.000 mm, Y = 0.000 mm, Z = 0.000 mm,
|
||||
W = 0.000 deg, P = 0.000 deg, R = 0.000 deg
|
||||
};
|
||||
P[2]{
|
||||
X = 500.000 mm, Y = 0.000 mm, Z = 100.000 mm,
|
||||
W = 0.000 deg, P = 0.000 deg, R = 0.000 deg
|
||||
};
|
||||
P[3]{
|
||||
X = 550.000 mm, Y = 50.000 mm, Z = 100.000 mm,
|
||||
W = 0.000 deg, P = 0.000 deg, R = 0.000 deg
|
||||
};
|
||||
P[4]{
|
||||
X = 600.000 mm, Y = 0.000 mm, Z = 100.000 mm,
|
||||
W = 0.000 deg, P = 0.000 deg, R = 0.000 deg
|
||||
};
|
||||
/END`;
|
||||
|
||||
describe("brand import MVP", () => {
|
||||
it("parses ABB RAPID targets, motions, tool hints, and produces runnable GRL", () => {
|
||||
const parsed = parseAbbRapid([{ name: "PickPlace.mod", text: ABB_MOD }]);
|
||||
const result = importBrandProgram("abb", [{ name: "PickPlace.mod", text: ABB_MOD }], {
|
||||
projectName: "AbbImport",
|
||||
generatedAt: "2026-06-27T00:00:00.000Z"
|
||||
});
|
||||
const patched = applyPatch(createOlpProject({ id: "abb_import", name: "AbbImport" }), result.patch);
|
||||
|
||||
expect(parsed.targets.map((target) => target.name)).toEqual(["pPick", "pMid", "pPlace", "jHome"]);
|
||||
expect(parsed.points.map((point) => [point.motion, point.targetId, point.viaTargetId])).toEqual([
|
||||
["movej", "target_jHome", undefined],
|
||||
["movel", "target_pPick", undefined],
|
||||
["movec", "target_pPlace", "target_pMid"]
|
||||
]);
|
||||
expect(validateOlpProject(patched).ok).toBe(true);
|
||||
expect(result.grl).toContain("operation abb_operation");
|
||||
expect(result.ir.paths[0]?.motions.map((motion) => motion.kind)).toEqual(["MOVEJ", "MOVEL", "MOVEC"]);
|
||||
expect(result.report.status).toBe("warn");
|
||||
expect(result.report.sections.find((section) => section.name === "import")?.diagnostics.map((diagnostic) => diagnostic.code)).toEqual([
|
||||
"IMPORT_TARGETS",
|
||||
"IMPORT_MOTIONS",
|
||||
"IMPORT_TOOL_DETECTED"
|
||||
]);
|
||||
expect(result.importReport).toEqual([
|
||||
expect.objectContaining({ code: "IMPORT_TOOL_DETECTED", severity: "info" })
|
||||
]);
|
||||
expect(postProcessAllBrands(result.ir).outputs.abb.text).toContain("MoveC pMid,pPlace");
|
||||
});
|
||||
|
||||
it("parses KUKA KRL src/dat movement and frame/tool hints", () => {
|
||||
const parsed = parseKukaKrl([
|
||||
{ name: "PickPlace.src", text: KUKA_SRC },
|
||||
{ name: "PickPlace.dat", text: KUKA_DAT }
|
||||
]);
|
||||
const result = importBrandProgram("kuka", [
|
||||
{ name: "PickPlace.src", text: KUKA_SRC },
|
||||
{ name: "PickPlace.dat", text: KUKA_DAT }
|
||||
], {
|
||||
projectName: "KukaImport",
|
||||
generatedAt: "2026-06-27T00:00:00.000Z"
|
||||
});
|
||||
|
||||
expect(parsed.targets.map((target) => target.name).sort()).toEqual(["HOME", "XMID", "XPICK", "XPLACE"]);
|
||||
expect(result.parsed.path.points.map((point) => point.motion)).toEqual(["movej", "movel", "movec"]);
|
||||
expect(result.report.sections.find((section) => section.name === "import")?.diagnostics.map((diagnostic) => diagnostic.code)).toEqual([
|
||||
"IMPORT_TARGETS",
|
||||
"IMPORT_MOTIONS",
|
||||
"IMPORT_TOOL_DETECTED",
|
||||
"IMPORT_FRAME_DETECTED"
|
||||
]);
|
||||
expect(result.report.status).toBe("warn");
|
||||
expect(postProcessAllBrands(result.ir).outputs.kuka.text).toContain("CIRC XMID, XPLACE");
|
||||
});
|
||||
|
||||
it("parses FANUC LS P/PR style positions and circular moves", () => {
|
||||
const parsed = parseFanucLs([{ name: "PICKPLACE.ls", text: FANUC_LS }]);
|
||||
const result = importBrandProgram("fanuc", [{ name: "PICKPLACE.ls", text: FANUC_LS }], {
|
||||
projectName: "FanucImport",
|
||||
generatedAt: "2026-06-27T00:00:00.000Z"
|
||||
});
|
||||
|
||||
expect(parsed.targets.map((target) => target.name)).toEqual(["P1", "P2", "P3", "P4"]);
|
||||
expect(result.patch).toEqual(expect.arrayContaining([
|
||||
expect.objectContaining({ op: "add", path: "/targets/-" }),
|
||||
expect.objectContaining({ op: "add", path: "/paths/-" }),
|
||||
expect.objectContaining({ op: "add", path: "/operations/-" })
|
||||
]));
|
||||
expect(result.grl).toContain("module FanucImport");
|
||||
expect(result.parsed.path.points.map((point) => [point.motion, point.targetId, point.viaTargetId])).toEqual([
|
||||
["movej", "target_P1", undefined],
|
||||
["movel", "target_P2", undefined],
|
||||
["movec", "target_P4", "target_P3"]
|
||||
]);
|
||||
expect(result.ir.operations[0]).toMatchObject({
|
||||
operationId: "fanuc_operation",
|
||||
kind: "imported_program",
|
||||
pathId: "fanuc_path"
|
||||
});
|
||||
expect(postProcessAllBrands(result.ir).outputs.fanuc.text).toContain("C P3 P4");
|
||||
});
|
||||
});
|
||||
415
kdl-wasm/web/tests/integration/abb120Programs.test.ts
Normal file
415
kdl-wasm/web/tests/integration/abb120Programs.test.ts
Normal file
@@ -0,0 +1,415 @@
|
||||
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"
|
||||
})
|
||||
);
|
||||
});
|
||||
});
|
||||
82
kdl-wasm/web/tests/integration/abb120SuiteArtifacts.test.ts
Normal file
82
kdl-wasm/web/tests/integration/abb120SuiteArtifacts.test.ts
Normal file
@@ -0,0 +1,82 @@
|
||||
import { readFileSync } from "node:fs";
|
||||
import { dirname, join } from "node:path";
|
||||
import { fileURLToPath } from "node:url";
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { runAbb120Suite } from "../../src/suites/abb120Suite.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../../src/grl/semantic/index.js";
|
||||
import { ABB_IRB120_ZERO_JOINTS } from "../fixtures/abbIrb120.js";
|
||||
|
||||
const WEB_ROOT = join(dirname(fileURLToPath(import.meta.url)), "../..");
|
||||
const PROGRAM_DIR = join(WEB_ROOT, "tests", "fixtures", "abb120", "programs");
|
||||
const APP_DIR = join(WEB_ROOT, "app");
|
||||
const PROGRAM_NAMES = [
|
||||
"A120_00_Smoke.grl",
|
||||
"A120_10_JointPickPlace.grl",
|
||||
"A120_20_CartesianBlend.grl",
|
||||
"A120_30_IOWaitPulse.grl",
|
||||
"A120_40_ErrorDiagnostics.grl",
|
||||
"A120_50_OperationProcess.grl"
|
||||
];
|
||||
|
||||
function loadPrograms() {
|
||||
return PROGRAM_NAMES.map((name) => ({
|
||||
name,
|
||||
text: readFileSync(join(PROGRAM_DIR, name), "utf8")
|
||||
}));
|
||||
}
|
||||
|
||||
describe("ABB120 suite artifacts", () => {
|
||||
it("ships six parseable GRL programs with source maps and KDL bridge requests", () => {
|
||||
for (const program of loadPrograms()) {
|
||||
const ast = parseGrl(program.text);
|
||||
const ir = compileSemanticProgram(ast, {
|
||||
startJoints: [...ABB_IRB120_ZERO_JOINTS],
|
||||
sampleTime: 0.02
|
||||
});
|
||||
expect(ast.kind).toBe("Program");
|
||||
expect(ir.moduleName).toMatch(/^A120/);
|
||||
expect(ir.sourceMap.length + ir.kdlBridge.motionRequests.length + ir.kdlBridge.pathRequests.length).toBeGreaterThan(0);
|
||||
}
|
||||
});
|
||||
|
||||
it("creates job, post, roundtrip, report, delivery, and screenshot evidence paths", () => {
|
||||
const job = runAbb120Suite(loadPrograms(), { now: "2026-06-27T12:00:00.000Z" });
|
||||
|
||||
expect(job.job_id).toBe("A120-JOB-20260627120000-doc");
|
||||
expect(job.robot.robotId).toBe("abb_irb120_3_58");
|
||||
expect(job.programs.map((program) => program.name)).toEqual(PROGRAM_NAMES);
|
||||
expect(job.post.filenames).toEqual(expect.arrayContaining([
|
||||
"A120Smoke.mod",
|
||||
"A120Smoke.ls",
|
||||
"A120Smoke.src"
|
||||
]));
|
||||
expect(job.roundtrip.status).toMatch(/pass|warn/);
|
||||
expect(job.report_id).toBe("A120-REPORT-20260627120000");
|
||||
expect(job.artifacts).toMatchObject({
|
||||
jobJson: "A120-JOB-20260627120000-doc/job.json",
|
||||
reportHtml: "A120-JOB-20260627120000-doc/reports/A120-REPORT-20260627120000.html",
|
||||
deliveryPackageJson: "A120-JOB-20260627120000-doc/delivery/package.json",
|
||||
desktopScreenshot: "A120-JOB-20260627120000-doc/screenshots/virtual-controller-desktop.png",
|
||||
mobileScreenshot: "A120-JOB-20260627120000-doc/screenshots/virtual-controller-mobile.png"
|
||||
});
|
||||
});
|
||||
|
||||
it("has an HTML virtual controller entry with required panels", () => {
|
||||
const html = readFileSync(join(APP_DIR, "virtual-controller.html"), "utf8");
|
||||
const css = readFileSync(join(APP_DIR, "virtual-controller.css"), "utf8");
|
||||
const js = readFileSync(join(APP_DIR, "virtual-controller.js"), "utf8");
|
||||
|
||||
expect(html).toContain("ABB120 Station");
|
||||
expect(html).toContain("Path: pick_place");
|
||||
expect(html).toContain("Controller");
|
||||
expect(html).toContain("Motion Queue");
|
||||
expect(html).toContain("Reports");
|
||||
expect(html).toContain('src="./virtual-controller.js"');
|
||||
expect(html).not.toContain('src="./virtual-controller.ts"');
|
||||
expect(css).toContain(".viewport");
|
||||
expect(css).toContain("@media");
|
||||
expect(js).toContain("data-command");
|
||||
expect(js).toContain("data-tab");
|
||||
});
|
||||
});
|
||||
146
kdl-wasm/web/tests/olp/geometry.test.ts
Normal file
146
kdl-wasm/web/tests/olp/geometry.test.ts
Normal file
@@ -0,0 +1,146 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import {
|
||||
applyCalibrationRecords,
|
||||
commercialExtensionBoundaries,
|
||||
compareControllerLowSpeedRun,
|
||||
createResourceLibraryTemplate,
|
||||
detectBasicCollisions,
|
||||
generatePathFromGeometry,
|
||||
instantiateProcessTemplate,
|
||||
locateCollisionTime,
|
||||
sampleOlpProject,
|
||||
validateOlpProject
|
||||
} from "../../src/olp/index.js";
|
||||
|
||||
describe("OLP geometry, calibration, and multi-robot extensions", () => {
|
||||
it("generates path targets from points, edges, and curves", () => {
|
||||
const pointPath = generatePathFromGeometry(
|
||||
{ kind: "points", points: [[500, 0, 0], [600, 0, 0]] },
|
||||
{ pathId: "point_path", speedId: "v_linear", zoneId: "z10" }
|
||||
);
|
||||
const edgePath = generatePathFromGeometry(
|
||||
{ kind: "edge", start: [0, 0, 0], end: [100, 0, 0], samples: 3 },
|
||||
{ pathId: "edge_path" }
|
||||
);
|
||||
const curvePath = generatePathFromGeometry(
|
||||
{ kind: "curve", controlPoints: [[0, 0, 0], [50, 50, 0], [100, 0, 0]], samples: 3 },
|
||||
{ pathId: "curve_path", targetPrefix: "curve" }
|
||||
);
|
||||
|
||||
expect(pointPath.targets.map((target) => target.pose)).toEqual([
|
||||
[500, 0, 0, 0, 0, 0],
|
||||
[600, 0, 0, 0, 0, 0]
|
||||
]);
|
||||
expect(edgePath.targets.map((target) => target.pose?.[0])).toEqual([0, 50, 100]);
|
||||
expect(curvePath.targets.map((target) => target.name)).toEqual(["curve_00", "curve_01", "curve_02"]);
|
||||
expect(curvePath.path.points.map((point) => point.motion)).toEqual(["movej", "movel", "movel"]);
|
||||
});
|
||||
|
||||
it("detects primitive collisions and locates the first time point", () => {
|
||||
const project = sampleOlpProject();
|
||||
const report = detectBasicCollisions([
|
||||
{ time: 0, pointId: "p0", position: [100, 0, 0] },
|
||||
{ time: 1.25, pointId: "p1", position: [550, 0, -40] }
|
||||
], project.resources.collisionObjects);
|
||||
|
||||
expect(report.ok).toBe(false);
|
||||
expect(report.hits).toEqual([
|
||||
expect.objectContaining({
|
||||
objectId: "fixture_box",
|
||||
pointId: "p1",
|
||||
time: 1.25
|
||||
})
|
||||
]);
|
||||
expect(locateCollisionTime(report)).toBe(1.25);
|
||||
});
|
||||
|
||||
it("saves and applies TCP/frame/base/external-axis calibration records", () => {
|
||||
const project = sampleOlpProject();
|
||||
const calibrated = applyCalibrationRecords(project, [
|
||||
{
|
||||
id: "tcp_cal_2",
|
||||
name: "tcp_cal_2",
|
||||
kind: "tcp",
|
||||
targetResourceId: "tool_gripper",
|
||||
poseDelta: [1, 2, 3, 0, 0, 0]
|
||||
},
|
||||
{
|
||||
id: "fixture_cal_1",
|
||||
name: "fixture_cal_1",
|
||||
kind: "frame",
|
||||
targetResourceId: "frame_fixture",
|
||||
poseDelta: [10, 0, 0, 0, 0, 0]
|
||||
},
|
||||
{
|
||||
id: "track_cal_1",
|
||||
name: "track_cal_1",
|
||||
kind: "external_axis",
|
||||
targetResourceId: "track_1",
|
||||
axisOffset: 2.5
|
||||
}
|
||||
]);
|
||||
|
||||
expect(calibrated.resources.tools.find((tool) => tool.id === "tool_gripper")?.tcp).toEqual([1, 2, 103, 0, 0, 0]);
|
||||
expect(calibrated.resources.frames.find((frame) => frame.id === "frame_fixture")?.pose).toEqual([810, 0, 0, 0, 0, 0]);
|
||||
expect(calibrated.externalAxes[0]?.metadata).toEqual({ calibrationOffset: 2.5 });
|
||||
expect(validateOlpProject(calibrated).ok).toBe(true);
|
||||
});
|
||||
|
||||
it("creates resource library and process template delivery boundaries", () => {
|
||||
const project = sampleOlpProject();
|
||||
const library = createResourceLibraryTemplate(project);
|
||||
const template = instantiateProcessTemplate(project.processTemplates[0]!, "pick_op");
|
||||
const boundaries = commercialExtensionBoundaries();
|
||||
|
||||
expect(library).toMatchObject({
|
||||
robots: [{ id: "robot_1", brand: "abb", model: "IRB120" }],
|
||||
collisionObjects: [{ id: "fixture_box" }]
|
||||
});
|
||||
expect(template).toEqual({
|
||||
operationId: "pick_op",
|
||||
operationKind: "handling",
|
||||
defaults: { speedId: "v_linear", zoneId: "z10" },
|
||||
deliveryTags: ["source", "post", "report"]
|
||||
});
|
||||
expect(boundaries).toEqual(expect.arrayContaining([
|
||||
expect.objectContaining({ feature: "cad_kernel", mvp: false }),
|
||||
expect.objectContaining({ feature: "delivery_package", mvp: true })
|
||||
]));
|
||||
});
|
||||
|
||||
it("reports multi-robot/external-axis low-speed controller verification boundaries", () => {
|
||||
const project = sampleOlpProject();
|
||||
const report = compareControllerLowSpeedRun([
|
||||
{
|
||||
pointId: "p01",
|
||||
offline: [500, 0, 0, 0, 0, 0],
|
||||
measured: [501, 0, 0, 0.1, 0, 0],
|
||||
speedPercent: 10
|
||||
},
|
||||
{
|
||||
pointId: "p02",
|
||||
offline: [600, 0, 0, 0, 0, 0],
|
||||
measured: [604, 0, 0, 0.5, 0, 0],
|
||||
speedPercent: 10
|
||||
}
|
||||
], {
|
||||
positionMm: 5,
|
||||
orientationDeg: 1
|
||||
});
|
||||
|
||||
expect(project.motionGroups[0]).toMatchObject({
|
||||
robotIds: ["robot_1"],
|
||||
externalAxisIds: ["track_1"]
|
||||
});
|
||||
expect(report).toMatchObject({
|
||||
status: "pass",
|
||||
speedPercent: 10,
|
||||
maxPositionErrorMm: 4,
|
||||
maxOrientationErrorDeg: 0.5
|
||||
});
|
||||
expect(report.boundaries).toEqual(expect.arrayContaining([
|
||||
expect.stringContaining("does not open a live controller communication session"),
|
||||
expect.stringContaining("does not include a full CAD kernel")
|
||||
]));
|
||||
});
|
||||
});
|
||||
114
kdl-wasm/web/tests/olp/model.test.ts
Normal file
114
kdl-wasm/web/tests/olp/model.test.ts
Normal file
@@ -0,0 +1,114 @@
|
||||
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";
|
||||
import {
|
||||
applyPatch,
|
||||
pathOperationToGrl,
|
||||
sampleOlpProject,
|
||||
validateOlpProject,
|
||||
type OlpProjectModel
|
||||
} from "../../src/olp/index.js";
|
||||
|
||||
describe("OLP project model", () => {
|
||||
it("creates a serializable station/resource/path/operation sample", () => {
|
||||
const project = sampleOlpProject();
|
||||
const restored = JSON.parse(JSON.stringify(project)) as OlpProjectModel;
|
||||
const report = validateOlpProject(restored);
|
||||
|
||||
expect(restored.schemaVersion).toBe("olp/0.1");
|
||||
expect(report).toEqual({ ok: true, issues: [] });
|
||||
expect(restored.resources.robots[0]).toMatchObject({
|
||||
id: "robot_1",
|
||||
kind: "robot",
|
||||
brand: "abb"
|
||||
});
|
||||
expect(restored.externalAxes[0]).toMatchObject({
|
||||
id: "track_1",
|
||||
axisKind: "linear"
|
||||
});
|
||||
expect(restored.motionGroups[0]).toMatchObject({
|
||||
robotIds: ["robot_1"],
|
||||
externalAxisIds: ["track_1"],
|
||||
coordination: "synchronized"
|
||||
});
|
||||
});
|
||||
|
||||
it("applies brand import style object patches and validates references", () => {
|
||||
const patched = applyPatch(sampleOlpProject(), [
|
||||
{
|
||||
op: "add",
|
||||
path: "/targets/-",
|
||||
value: {
|
||||
id: "inspect",
|
||||
name: "inspect",
|
||||
kind: "pose",
|
||||
robotId: "robot_1",
|
||||
pose: [650, 25, 0, 0, 0, 0],
|
||||
toolId: "tool_gripper",
|
||||
frameId: "frame_fixture"
|
||||
}
|
||||
},
|
||||
{
|
||||
op: "add",
|
||||
path: "/paths/0/points/-",
|
||||
value: {
|
||||
id: "p03",
|
||||
name: "p03",
|
||||
motion: "movel",
|
||||
targetId: "inspect",
|
||||
speedId: "v_linear",
|
||||
zoneId: "z10"
|
||||
}
|
||||
}
|
||||
]);
|
||||
|
||||
expect(validateOlpProject(patched).ok).toBe(true);
|
||||
expect(patched.paths[0]?.points.map((point) => point.name)).toEqual(["p00", "p01", "p02", "p03"]);
|
||||
});
|
||||
|
||||
it("generates stable parseable GRL from Path and Operation", () => {
|
||||
const result = pathOperationToGrl(sampleOlpProject(), "pick_op", {
|
||||
moduleName: "DemoCell",
|
||||
style: "expanded"
|
||||
});
|
||||
const ast = parseGrl(result.text);
|
||||
const ir = compileSemanticProgram(ast, {
|
||||
startJoints: [0, 0, 0, 0, 0, 0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
const post = postProcessAllBrands(ir);
|
||||
|
||||
expect(result.stableIds).toEqual({
|
||||
targets: ["home", "pick", "place"],
|
||||
points: ["p00", "p01", "p02"],
|
||||
path: "pick_path",
|
||||
operation: "pick_op"
|
||||
});
|
||||
expect(result.text).toContain("path pick_path");
|
||||
expect(result.text).toContain("operation pick_op");
|
||||
expect(result.program).toMatchObject({
|
||||
id: "pick_op_grl",
|
||||
language: "grl",
|
||||
entryOperationIds: ["pick_op"]
|
||||
});
|
||||
expect(ir.operations[0]).toMatchObject({
|
||||
operationId: "pick_op",
|
||||
kind: "handling",
|
||||
pathId: "pick_path"
|
||||
});
|
||||
expect(post.outputs.abb.text).toContain("MODULE DemoCell");
|
||||
expect(post.outputs.fanuc.text).toContain("/PROG MAIN");
|
||||
expect(post.outputs.kuka.text).toContain("DEF Main()");
|
||||
});
|
||||
|
||||
it("reports broken schema references with stable issue codes", () => {
|
||||
const project = sampleOlpProject();
|
||||
project.operations[0]!.pathId = "missing_path";
|
||||
|
||||
expect(validateOlpProject(project)).toMatchObject({
|
||||
ok: false,
|
||||
issues: [expect.objectContaining({ code: "OLP_REF_PATH_MISSING" })]
|
||||
});
|
||||
});
|
||||
});
|
||||
149
kdl-wasm/web/tests/reports/report.test.ts
Normal file
149
kdl-wasm/web/tests/reports/report.test.ts
Normal file
@@ -0,0 +1,149 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { postProcessBrand } from "../../src/grl/post/index.js";
|
||||
import { parseGrl } from "../../src/grl/parser/index.js";
|
||||
import { compileSemanticProgram } from "../../src/grl/semantic/index.js";
|
||||
import { pathOperationToGrl, sampleOlpProject } from "../../src/olp/index.js";
|
||||
import {
|
||||
calibrationSummary,
|
||||
createValidationReport,
|
||||
exportCustomerDeliveryPackage,
|
||||
exportValidationReportHtml,
|
||||
serializeDeliveryPackage,
|
||||
validateReportSchema
|
||||
} from "../../src/reports/index.js";
|
||||
|
||||
const NOW = "2026-06-27T00:00:00.000Z";
|
||||
|
||||
describe("validation reports and customer delivery", () => {
|
||||
it("creates stable validation report sections for reachability, cycle, IO/Wait, post, import, and calibration", () => {
|
||||
const project = sampleOlpProject();
|
||||
const report = createValidationReport({
|
||||
id: "demo_validation",
|
||||
project,
|
||||
generatedAt: NOW,
|
||||
pathValidation: {
|
||||
ok: true,
|
||||
reachable: true,
|
||||
cycleTime: 4.2,
|
||||
segmentReports: [],
|
||||
diagnostics: []
|
||||
},
|
||||
ioWaitDiagnostics: [],
|
||||
postDiagnostics: [],
|
||||
importDiagnostics: [
|
||||
{
|
||||
severity: "info",
|
||||
code: "IMPORT_OK",
|
||||
message: "ABB import mapped to OLP",
|
||||
sourceMap: { file: "imports/PickPlace.mod", line: 8 }
|
||||
}
|
||||
]
|
||||
});
|
||||
|
||||
expect(validateReportSchema(report)).toEqual({ ok: true, issues: [] });
|
||||
expect(report.status).toBe("pass");
|
||||
expect(report.sections.map((section) => section.name)).toEqual([
|
||||
"reachability",
|
||||
"cycle_time",
|
||||
"io_wait",
|
||||
"post",
|
||||
"import",
|
||||
"collision",
|
||||
"calibration"
|
||||
]);
|
||||
expect(report.sections.find((section) => section.name === "cycle_time")?.summary).toEqual({
|
||||
cycleTime: 4.2,
|
||||
trajectories: 0
|
||||
});
|
||||
expect(report.sourceMap).toEqual([
|
||||
{
|
||||
kind: "IMPORT_OK",
|
||||
id: "IMPORT_OK-0",
|
||||
file: "imports/PickPlace.mod",
|
||||
line: 8
|
||||
}
|
||||
]);
|
||||
});
|
||||
|
||||
it("exports openable HTML with summary, details, and source map payload", () => {
|
||||
const report = createValidationReport({
|
||||
id: "demo_validation",
|
||||
project: sampleOlpProject(),
|
||||
generatedAt: NOW,
|
||||
diagnostics: [
|
||||
{
|
||||
severity: "info",
|
||||
code: "REACH_OK",
|
||||
message: "pick target reachable",
|
||||
sourceMap: { file: "source/DemoCell.grl", line: 20 }
|
||||
}
|
||||
]
|
||||
});
|
||||
const html = exportValidationReportHtml(report);
|
||||
|
||||
expect(html).toContain("<!doctype html>");
|
||||
expect(html).toContain("Status: warn");
|
||||
expect(html).toContain("reachability");
|
||||
expect(html).toContain("source/DemoCell.grl");
|
||||
expect(html).toContain("validation-report-json");
|
||||
});
|
||||
|
||||
it("exports customer delivery package with source, brand programs, IO map, reports, calibration, and trace", () => {
|
||||
const project = sampleOlpProject();
|
||||
const grl = pathOperationToGrl(project, "pick_op", { moduleName: "DemoCell" });
|
||||
project.programs.push(grl.program);
|
||||
const ir = compileSemanticProgram(parseGrl(grl.text), {
|
||||
startJoints: [0, 0, 0, 0, 0, 0],
|
||||
sampleTime: 0.004
|
||||
});
|
||||
const abb = postProcessBrand(ir, "abb");
|
||||
const report = createValidationReport({
|
||||
id: "demo_validation",
|
||||
project,
|
||||
generatedAt: NOW,
|
||||
postDiagnostics: abb.report.map((issue) => ({
|
||||
severity: issue.severity,
|
||||
code: issue.code,
|
||||
message: issue.message
|
||||
})),
|
||||
pathValidation: {
|
||||
ok: true,
|
||||
reachable: true,
|
||||
cycleTime: 4.2,
|
||||
segmentReports: [],
|
||||
diagnostics: []
|
||||
}
|
||||
});
|
||||
|
||||
const pkg = exportCustomerDeliveryPackage({
|
||||
project,
|
||||
report,
|
||||
trace: { samples: ["t=0 movej home"] },
|
||||
brandPrograms: { [abb.filename]: abb.text },
|
||||
ioMap: { "io.do[1]": "vacuum" }
|
||||
});
|
||||
const serialized = serializeDeliveryPackage(pkg);
|
||||
|
||||
expect(pkg.manifest).toEqual({
|
||||
sourcePrograms: ["source/pick_op_grl.grl"],
|
||||
brandPrograms: ["post/DemoCell.mod"],
|
||||
reports: ["reports/demo_validation.html", "reports/demo_validation.json"],
|
||||
calibrationFiles: ["calibration/calibrations.json"],
|
||||
traceFiles: ["trace/trace.json"],
|
||||
ioMaps: ["io/io_map.json"]
|
||||
});
|
||||
expect(pkg.files.map((file) => file.path)).toEqual([
|
||||
"calibration/calibrations.json",
|
||||
"io/io_map.json",
|
||||
"post/DemoCell.mod",
|
||||
"project/project.json",
|
||||
"reports/demo_validation.html",
|
||||
"reports/demo_validation.json",
|
||||
"source/pick_op_grl.grl",
|
||||
"trace/trace.json"
|
||||
]);
|
||||
expect(serialized).toContain("\"format\": \"customer-delivery/0.1\"");
|
||||
expect(serialized).toContain("fnv1a32:");
|
||||
expect(calibrationSummary(project.calibrations)).toEqual({ tcp: 1 });
|
||||
});
|
||||
});
|
||||
88
kdl-wasm/web/tests/runtime/ioRuntime.test.ts
Normal file
88
kdl-wasm/web/tests/runtime/ioRuntime.test.ts
Normal file
@@ -0,0 +1,88 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { IoImageRuntime, ioKey } from "../../src/runtime/index.js";
|
||||
import type { IoReference, PulseInstruction, WaitInstruction } from "../../src/grl/ir/index.js";
|
||||
|
||||
const DO1: IoReference = { domain: "do", index: 1, raw: "io.do[1]" };
|
||||
const DI1: IoReference = { domain: "di", index: 1, raw: "io.di[1]" };
|
||||
const DI2: IoReference = { domain: "di", index: 2, raw: "io.di[2]" };
|
||||
const AI1: IoReference = { domain: "ai", index: 1, raw: "io.ai[1]" };
|
||||
|
||||
describe("virtual IO image, waits, pulses, edges, and scripts", () => {
|
||||
it("enforces write permissions and records IO events", () => {
|
||||
const io = new IoImageRuntime({
|
||||
permissions: [
|
||||
{ writer: "program", domains: ["do"], access: "write" },
|
||||
{ writer: "script", domains: ["di", "ai"], access: "write" }
|
||||
]
|
||||
});
|
||||
|
||||
expect(io.write(DO1, true, "program")).toMatchObject({ kind: "write", value: true });
|
||||
expect(io.write(DI1, true, "program")).toBeUndefined();
|
||||
expect(io.snapshot()).toMatchObject({
|
||||
image: { [ioKey(DO1)]: true },
|
||||
diagnostics: [expect.objectContaining({ code: "VC_IO_PERMISSION_DENIED" })]
|
||||
});
|
||||
});
|
||||
|
||||
it("evaluates waits, timeouts, on_timeout hold-stop, and edge conditions", () => {
|
||||
const io = new IoImageRuntime({
|
||||
permissions: [{ writer: "script", domains: ["di", "ai"], access: "write" }]
|
||||
});
|
||||
const wait: WaitInstruction = {
|
||||
kind: "WAIT",
|
||||
condition: "all ( io . di [ 1 ] == true , io . di [ 2 ] == false )",
|
||||
timeout: 1,
|
||||
onTimeout: { kind: "call", value: "recover" }
|
||||
};
|
||||
|
||||
expect(io.evaluateWait(wait, 0)).toMatchObject({ status: "waiting" });
|
||||
expect(io.evaluateWait(wait, 1)).toMatchObject({
|
||||
status: "hold-stop",
|
||||
onTimeout: { kind: "call", value: "recover" },
|
||||
diagnostic: { code: "VC_WAIT_TIMEOUT" }
|
||||
});
|
||||
|
||||
io.write(DI1, true, "script");
|
||||
io.write(DI2, false, "script");
|
||||
expect(io.evaluateWait(wait, 0)).toMatchObject({ status: "satisfied" });
|
||||
expect(io.matchesCondition("rising ( io . di [ 1 ] )")).toBe(true);
|
||||
io.write(DI1, false, "script");
|
||||
expect(io.matchesCondition("falling ( io . di [ 1 ] )")).toBe(true);
|
||||
io.write(AI1, 5, "script");
|
||||
expect(io.matchesCondition("changed ( io . ai [ 1 ] )")).toBe(true);
|
||||
});
|
||||
|
||||
it("auto-resets pulses and executes delayed IO scripts", () => {
|
||||
const io = new IoImageRuntime({
|
||||
permissions: [
|
||||
{ writer: "program", domains: ["do"], access: "write" },
|
||||
{ writer: "script", domains: ["di"], access: "write" }
|
||||
]
|
||||
});
|
||||
const pulse: PulseInstruction = {
|
||||
kind: "PULSE",
|
||||
target: DO1,
|
||||
duration: 0.2,
|
||||
trace: [
|
||||
{ time: 0, action: "set", target: DO1, value: true },
|
||||
{ time: 0.2, action: "reset", target: DO1, value: false }
|
||||
]
|
||||
};
|
||||
|
||||
io.executePulse(pulse);
|
||||
expect(io.read(DO1)).toBe(true);
|
||||
io.advance(0.2);
|
||||
expect(io.read(DO1)).toBe(false);
|
||||
|
||||
io.addScript("part-arrival", [{ delay: 0.5, target: DI1, value: true }]);
|
||||
io.advance(0.49);
|
||||
expect(io.read(DI1)).toBeUndefined();
|
||||
io.advance(0.01);
|
||||
expect(io.read(DI1)).toBe(true);
|
||||
expect(io.snapshot().events.map((event) => event.kind)).toEqual([
|
||||
"pulse_set",
|
||||
"pulse_reset",
|
||||
"script"
|
||||
]);
|
||||
});
|
||||
});
|
||||
157
kdl-wasm/web/tests/runtime/motionQueue.test.ts
Normal file
157
kdl-wasm/web/tests/runtime/motionQueue.test.ts
Normal file
@@ -0,0 +1,157 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { SemanticProgramIr } from "../../src/grl/ir/index.js";
|
||||
import type { PathPlanRequest, PathPlanResult, TrajectoryPoint } from "../../src/kdl/types.js";
|
||||
import { KdlRuntimeBridge, MotionQueue, type MotionPlanner } from "../../src/runtime/index.js";
|
||||
|
||||
const POSE = {
|
||||
position: [0, 0, 0] as [number, number, number],
|
||||
quaternion: [0, 0, 0, 1] as [number, number, number, number]
|
||||
};
|
||||
|
||||
function point(index: number, time: number, joints: number[], segmentId: string): TrajectoryPoint {
|
||||
return {
|
||||
index,
|
||||
time,
|
||||
dt: index === 0 ? 0 : time,
|
||||
s: time,
|
||||
sd: 1,
|
||||
sdd: 0,
|
||||
joints,
|
||||
jointVelocity: joints.map(() => 0),
|
||||
jointAcceleration: joints.map(() => 0),
|
||||
flange: POSE,
|
||||
tcp: POSE,
|
||||
motion: "MOVEJ",
|
||||
segmentId,
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
|
||||
function pathResult(request: PathPlanRequest): PathPlanResult {
|
||||
return {
|
||||
ok: true,
|
||||
duration: 1,
|
||||
segments: [],
|
||||
points: [point(0, 0, request.startJoints, request.segments[0]?.id ?? "p0"), point(1, 1, [1], request.segments[0]?.id ?? "p0")],
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
|
||||
function program(): SemanticProgramIr {
|
||||
return {
|
||||
moduleName: "Main",
|
||||
symbols: [],
|
||||
semanticChecks: [],
|
||||
diagnostics: [],
|
||||
sourceMap: [
|
||||
{ kind: "path_point", id: "p0", pathId: "pick_path", pointId: "p0", sourceMap: { line: 10 } },
|
||||
{ kind: "operation_action", id: "pick_op_start_action_0", operationId: "pick_op", sourceMap: { line: 20 } }
|
||||
],
|
||||
procedures: [
|
||||
{
|
||||
name: "main",
|
||||
instructions: [
|
||||
{ kind: "RUN_OPERATION", operationId: "pick_op", sourceMap: { line: 30 } }
|
||||
]
|
||||
}
|
||||
],
|
||||
paths: [
|
||||
{
|
||||
pathId: "pick_path",
|
||||
request: {
|
||||
pathId: "pick_path",
|
||||
startJoints: [0],
|
||||
sampleTime: 0.1,
|
||||
segments: [
|
||||
{
|
||||
id: "p0",
|
||||
motion: "MOVEJ",
|
||||
target: { joints: [1] },
|
||||
speed: { kind: "joint_abs", velocity: 1 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { line: 10 }
|
||||
}
|
||||
]
|
||||
},
|
||||
motions: [
|
||||
{
|
||||
kind: "MOVEJ",
|
||||
id: "p0",
|
||||
pathId: "pick_path",
|
||||
pointId: "p0",
|
||||
target: { joints: [1] },
|
||||
speed: { kind: "joint_abs", velocity: 1 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { line: 10 },
|
||||
source: { brand: { vendor: "ABB", file: "cell.mod", line: 42 } }
|
||||
}
|
||||
],
|
||||
events: []
|
||||
}
|
||||
],
|
||||
operations: [
|
||||
{
|
||||
operationId: "pick_op",
|
||||
kind: "handling",
|
||||
pathId: "pick_path",
|
||||
process: {},
|
||||
startActions: [
|
||||
{
|
||||
kind: "ACTION",
|
||||
actionKind: "start_action",
|
||||
operationId: "pick_op",
|
||||
statement: "io.do[1] = true",
|
||||
sourceMap: { line: 20 }
|
||||
}
|
||||
],
|
||||
endActions: [
|
||||
{
|
||||
kind: "ACTION",
|
||||
actionKind: "end_action",
|
||||
operationId: "pick_op",
|
||||
statement: "io.do[1] = false",
|
||||
sourceMap: { line: 22 }
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
kdlBridge: {
|
||||
motionRequests: [],
|
||||
pathRequests: []
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
describe("motion queue and KDL runtime bridge", () => {
|
||||
it("preserves operation/path/action sources and samples planned paths by virtual time", async () => {
|
||||
const plannerCalls: PathPlanRequest[] = [];
|
||||
const planner: MotionPlanner = {
|
||||
planPath: (_handle, request) => {
|
||||
plannerCalls.push(request);
|
||||
return pathResult(request);
|
||||
}
|
||||
};
|
||||
const queue = new MotionQueue({ startJoints: [0], sampleTime: 0.1, planner, robotHandle: 7 });
|
||||
const ir = program();
|
||||
const bridge = new KdlRuntimeBridge(ir, queue);
|
||||
|
||||
const items = bridge.enqueueInstruction(ir.procedures[0]!.instructions[0]!);
|
||||
expect(items.map((item) => item.kind)).toEqual(["action", "path", "action"]);
|
||||
expect(items[0]).toMatchObject({ source: { operationId: "pick_op", sourceMap: { line: 20 } } });
|
||||
expect(items[1]).toMatchObject({ source: { pathId: "pick_path", operationId: "pick_op" } });
|
||||
|
||||
await queue.planAll();
|
||||
expect(plannerCalls).toHaveLength(1);
|
||||
expect(plannerCalls[0]).toMatchObject({ pathId: "pick_path", startJoints: [0] });
|
||||
|
||||
expect(queue.advance(0.5)).toMatchObject({
|
||||
itemId: items[1]!.id,
|
||||
localTime: 0.5,
|
||||
point: { joints: [1] },
|
||||
source: { pathId: "pick_path", operationId: "pick_op" }
|
||||
});
|
||||
expect(queue.snapshot().items[1]).toMatchObject({
|
||||
planned: { ok: true, duration: 1 }
|
||||
});
|
||||
});
|
||||
});
|
||||
195
kdl-wasm/web/tests/workbench/debugWorkbench.test.ts
Normal file
195
kdl-wasm/web/tests/workbench/debugWorkbench.test.ts
Normal file
@@ -0,0 +1,195 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import type { SemanticProgramIr } from "../../src/grl/ir/index.js";
|
||||
import type { PathPlanResult, TrajectoryPoint } from "../../src/kdl/types.js";
|
||||
import { IrExecutionRuntime } from "../../src/controller/index.js";
|
||||
import { IoImageRuntime, MotionQueue, type MotionPlanner } from "../../src/runtime/index.js";
|
||||
import { DebugFacade, WorkbenchFacade } from "../../src/workbench/index.js";
|
||||
|
||||
const POSE = {
|
||||
position: [0, 0, 0] as [number, number, number],
|
||||
quaternion: [0, 0, 0, 1] as [number, number, number, number]
|
||||
};
|
||||
|
||||
function point(index: number, time: number, joints: number[]): TrajectoryPoint {
|
||||
return {
|
||||
index,
|
||||
time,
|
||||
dt: time,
|
||||
s: time,
|
||||
sd: 1,
|
||||
sdd: 0,
|
||||
joints,
|
||||
jointVelocity: [0],
|
||||
jointAcceleration: [0],
|
||||
flange: POSE,
|
||||
tcp: POSE,
|
||||
motion: "MOVEJ",
|
||||
diagnostics: []
|
||||
};
|
||||
}
|
||||
|
||||
function program(): SemanticProgramIr {
|
||||
return {
|
||||
moduleName: "Main",
|
||||
symbols: [{ kind: "path", name: "path1" }, { kind: "operation", name: "op1" }],
|
||||
semanticChecks: [],
|
||||
diagnostics: [],
|
||||
sourceMap: [
|
||||
{ kind: "RUN_PATH", id: "run_path", procedureId: "main", pathId: "path1", sourceMap: { file: "main.grl", line: 5 } },
|
||||
{ kind: "path_point", id: "p1", pathId: "path1", pointId: "p1", sourceMap: { file: "main.grl", line: 10 } },
|
||||
{ kind: "operation_action", id: "op1_start_action_0", operationId: "op1", sourceMap: { file: "main.grl", line: 20 } }
|
||||
],
|
||||
procedures: [
|
||||
{
|
||||
name: "main",
|
||||
sourceMap: { file: "main.grl", line: 1 },
|
||||
instructions: [
|
||||
{ kind: "RAW_STATEMENT", text: "part_ready = true", sourceMap: { file: "main.grl", line: 4 } },
|
||||
{ kind: "RUN_PATH", pathId: "path1", sourceMap: { file: "main.grl", line: 5 } }
|
||||
]
|
||||
}
|
||||
],
|
||||
paths: [
|
||||
{
|
||||
pathId: "path1",
|
||||
request: {
|
||||
pathId: "path1",
|
||||
startJoints: [0],
|
||||
sampleTime: 0.1,
|
||||
segments: [
|
||||
{
|
||||
id: "p1",
|
||||
motion: "MOVEJ",
|
||||
target: { joints: [1] },
|
||||
speed: { kind: "joint_abs", velocity: 1 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { file: "main.grl", line: 10 }
|
||||
}
|
||||
]
|
||||
},
|
||||
motions: [
|
||||
{
|
||||
kind: "MOVEJ",
|
||||
id: "p1",
|
||||
pathId: "path1",
|
||||
pointId: "p1",
|
||||
target: { joints: [1] },
|
||||
speed: { kind: "joint_abs", velocity: 1 },
|
||||
zone: { kind: "fine" },
|
||||
sourceMap: { file: "main.grl", line: 10 },
|
||||
source: { brand: { vendor: "KUKA", file: "src.src", line: 99 } }
|
||||
}
|
||||
],
|
||||
events: []
|
||||
}
|
||||
],
|
||||
operations: [
|
||||
{
|
||||
operationId: "op1",
|
||||
kind: "handling",
|
||||
pathId: "path1",
|
||||
process: {},
|
||||
startActions: [],
|
||||
endActions: []
|
||||
}
|
||||
],
|
||||
kdlBridge: { motionRequests: [], pathRequests: [] }
|
||||
};
|
||||
}
|
||||
|
||||
describe("debug and workbench facades", () => {
|
||||
it("models breakpoints, motion breakpoints, watches, replay, and cross-source lookup", async () => {
|
||||
const ir = program();
|
||||
const runtime = new IrExecutionRuntime();
|
||||
runtime.load(ir);
|
||||
runtime.step();
|
||||
|
||||
const planner: MotionPlanner = {
|
||||
planPath: (_handle, request): PathPlanResult => ({
|
||||
ok: true,
|
||||
duration: 1,
|
||||
segments: [],
|
||||
points: [point(0, 0, request.startJoints), point(1, 1, [1])],
|
||||
diagnostics: []
|
||||
})
|
||||
};
|
||||
const queue = new MotionQueue({ startJoints: [0], sampleTime: 0.1, planner });
|
||||
const item = queue.enqueueRunPath({ kind: "RUN_PATH", pathId: "path1", sourceMap: { file: "main.grl", line: 5 } }, ir.paths[0]!);
|
||||
await queue.planAll();
|
||||
queue.advance(1);
|
||||
|
||||
const io = new IoImageRuntime();
|
||||
const debug = new DebugFacade({ runtime, motionQueue: queue, io, program: ir });
|
||||
debug.addBreakpoint({ id: "bp-run-path", procedure: "main", source: { line: 5 }, enabled: true });
|
||||
debug.addMotionBreakpoint({ id: "mb-path", pathId: "path1", enabled: true });
|
||||
const watch = debug.addWatch("watch-ready", "part_ready");
|
||||
|
||||
expect(watch).toMatchObject({ value: true, source: { sourceMap: { line: 5 } } });
|
||||
expect(debug.checkMotionBreakpoint(item)).toMatchObject({ id: "mb-path" });
|
||||
expect(debug.locatePathPoint("path1", "p1")).toMatchObject({ sourceMap: { line: 10 } });
|
||||
expect(debug.locateOperation("op1")).toMatchObject([{ sourceMap: { line: 20 } }]);
|
||||
expect(debug.playbackAt(1)).toMatchObject({ point: { joints: [1] } });
|
||||
expect(debug.replayTrace()).toEqual(expect.arrayContaining([expect.objectContaining({ kind: "load" })]));
|
||||
expect(debug.snapshot()).toMatchObject({
|
||||
breakpoints: [expect.objectContaining({ id: "bp-run-path" })],
|
||||
motionBreakpoints: [expect.objectContaining({ id: "mb-path" })],
|
||||
watches: [expect.objectContaining({ id: "watch-ready", value: true })]
|
||||
});
|
||||
});
|
||||
|
||||
it("builds object tree, editor, teach pendant, IO panel, and report entries", async () => {
|
||||
const ir = program();
|
||||
const runtime = new IrExecutionRuntime();
|
||||
runtime.load(ir);
|
||||
runtime.step();
|
||||
|
||||
const queue = new MotionQueue({
|
||||
startJoints: [0],
|
||||
sampleTime: 0.1,
|
||||
planner: {
|
||||
planPath: (_handle, request): PathPlanResult => ({
|
||||
ok: true,
|
||||
duration: 1,
|
||||
segments: [],
|
||||
points: [point(0, 0, request.startJoints), point(1, 1, [1])],
|
||||
diagnostics: []
|
||||
})
|
||||
}
|
||||
});
|
||||
queue.enqueueRunPath({ kind: "RUN_PATH", pathId: "path1" }, ir.paths[0]!);
|
||||
await queue.planAll();
|
||||
queue.advance(1);
|
||||
|
||||
const io = new IoImageRuntime();
|
||||
io.write({ domain: "do", index: 1, raw: "io.do[1]" }, true);
|
||||
|
||||
const workbench = new WorkbenchFacade();
|
||||
const snapshot = workbench.snapshot({
|
||||
program: ir,
|
||||
runtime: runtime.snapshot(),
|
||||
io: io.snapshot(),
|
||||
motion: queue.snapshot(),
|
||||
controllerState: "paused"
|
||||
});
|
||||
|
||||
expect(snapshot.objectTree[0]).toMatchObject({
|
||||
kind: "station",
|
||||
children: [
|
||||
expect.objectContaining({ id: "programs" }),
|
||||
expect.objectContaining({ id: "paths" }),
|
||||
expect.objectContaining({ id: "operations" }),
|
||||
expect.objectContaining({ id: "reports" })
|
||||
]
|
||||
});
|
||||
expect(snapshot.editor).toMatchObject({ activeFile: "main.grl", cursor: { line: 5 } });
|
||||
expect(snapshot.teachPendant).toMatchObject({ state: "paused", procedure: "main", joints: [1] });
|
||||
expect(snapshot.ioPanel.image).toMatchObject({ "io.do[1]": true });
|
||||
expect(snapshot.reports.map((report) => report.kind)).toEqual([
|
||||
"reachability",
|
||||
"cycle_time",
|
||||
"io_wait",
|
||||
"post",
|
||||
"import"
|
||||
]);
|
||||
});
|
||||
});
|
||||
140
kdl-wasm/web/tests/workspace/storage.test.ts
Normal file
140
kdl-wasm/web/tests/workspace/storage.test.ts
Normal file
@@ -0,0 +1,140 @@
|
||||
import { describe, expect, it } from "vitest";
|
||||
import { sampleOlpProject } from "../../src/olp/index.js";
|
||||
import {
|
||||
MemoryWorkspaceStorage,
|
||||
detectWorkspaceDamage,
|
||||
exportWorkspaceBundle,
|
||||
importWorkspaceBundle,
|
||||
initializeWorkspace,
|
||||
migrateManifest,
|
||||
readJson,
|
||||
readManifest,
|
||||
readProjectModel,
|
||||
restoreWorkspace,
|
||||
snapshotWorkspace,
|
||||
writeJson,
|
||||
writeProjectModel
|
||||
} from "../../src/workspace/index.js";
|
||||
|
||||
const NOW = "2026-06-27T00:00:00.000Z";
|
||||
|
||||
describe("Workspace storage", () => {
|
||||
it("creates project manifest and conventional project layout", async () => {
|
||||
const storage = new MemoryWorkspaceStorage();
|
||||
const manifest = await initializeWorkspace(storage, {
|
||||
projectId: "demo_cell",
|
||||
name: "Demo Cell",
|
||||
model: sampleOlpProject(),
|
||||
now: NOW
|
||||
});
|
||||
|
||||
expect(manifest.entrypoints.model).toBe("model/olp-project.json");
|
||||
expect(await storage.list()).toEqual(["model/olp-project.json", "project.json"]);
|
||||
expect(await readManifest(storage)).toEqual(manifest);
|
||||
expect((await readProjectModel(storage)).project.id).toBe("demo_cell");
|
||||
});
|
||||
|
||||
it("supports text/json read-write-delete with OPFS-like async storage", async () => {
|
||||
const storage = new MemoryWorkspaceStorage();
|
||||
await storage.writeText("reports/readme.txt", "hello");
|
||||
await writeJson(storage, "imports/report.json", { ok: true, count: 2 });
|
||||
|
||||
expect(await storage.readText("reports/readme.txt")).toBe("hello");
|
||||
expect(await readJson(storage, "imports/report.json")).toEqual({ ok: true, count: 2 });
|
||||
|
||||
await storage.delete("reports/readme.txt");
|
||||
expect(await storage.exists("reports/readme.txt")).toBe(false);
|
||||
});
|
||||
|
||||
it("snapshots and restores an equivalent workspace", async () => {
|
||||
const source = new MemoryWorkspaceStorage();
|
||||
await initializeWorkspace(source, {
|
||||
projectId: "demo_cell",
|
||||
name: "Demo Cell",
|
||||
model: sampleOlpProject(),
|
||||
now: NOW
|
||||
});
|
||||
await source.writeText("programs/main.grl", "language grl 0.1\n");
|
||||
const snapshot = await snapshotWorkspace(source);
|
||||
|
||||
const target = new MemoryWorkspaceStorage({ "old.txt": "delete me" });
|
||||
await restoreWorkspace(target, snapshot);
|
||||
|
||||
expect(await target.list()).toEqual(await source.list());
|
||||
expect(await snapshotWorkspace(target)).toEqual(snapshot);
|
||||
});
|
||||
|
||||
it("exports and imports a stable JSON bundle with checksum", async () => {
|
||||
const source = new MemoryWorkspaceStorage();
|
||||
await initializeWorkspace(source, {
|
||||
projectId: "demo_cell",
|
||||
name: "Demo Cell",
|
||||
model: sampleOlpProject(),
|
||||
now: NOW
|
||||
});
|
||||
await source.writeText("programs/main.grl", "language grl 0.1\n");
|
||||
const bundle = await exportWorkspaceBundle(source);
|
||||
|
||||
const target = new MemoryWorkspaceStorage();
|
||||
const report = await importWorkspaceBundle(target, bundle);
|
||||
|
||||
expect(report).toEqual({ ok: true, damages: [] });
|
||||
expect(await snapshotWorkspace(target)).toEqual(await snapshotWorkspace(source));
|
||||
});
|
||||
|
||||
it("migrates legacy manifests and detects damaged workspaces", async () => {
|
||||
expect(migrateManifest({
|
||||
schemaVersion: 0,
|
||||
projectId: "legacy",
|
||||
name: "Legacy",
|
||||
createdAt: NOW,
|
||||
updatedAt: NOW,
|
||||
modelPath: "legacy/model.json"
|
||||
})).toMatchObject({
|
||||
schemaVersion: 1,
|
||||
projectId: "legacy",
|
||||
entrypoints: { model: "legacy/model.json" }
|
||||
});
|
||||
|
||||
const storage = new MemoryWorkspaceStorage();
|
||||
await initializeWorkspace(storage, {
|
||||
projectId: "demo_cell",
|
||||
name: "Demo Cell",
|
||||
model: sampleOlpProject(),
|
||||
now: NOW
|
||||
});
|
||||
const checksum = (await snapshotWorkspace(storage)).checksum;
|
||||
const model = await readProjectModel(storage);
|
||||
model.operations[0]!.pathId = "missing_path";
|
||||
await writeProjectModel(storage, model, NOW);
|
||||
|
||||
expect(await detectWorkspaceDamage(storage, checksum)).toMatchObject({
|
||||
ok: false,
|
||||
damages: [
|
||||
expect.objectContaining({ code: "WORKSPACE_MODEL_INVALID" }),
|
||||
expect.objectContaining({ code: "WORKSPACE_CHECKSUM_MISMATCH" })
|
||||
]
|
||||
});
|
||||
});
|
||||
|
||||
it("rejects tampered bundles before import", async () => {
|
||||
const source = new MemoryWorkspaceStorage();
|
||||
await initializeWorkspace(source, {
|
||||
projectId: "demo_cell",
|
||||
name: "Demo Cell",
|
||||
model: sampleOlpProject(),
|
||||
now: NOW
|
||||
});
|
||||
const tampered = JSON.parse(await exportWorkspaceBundle(source)) as {
|
||||
files: Record<string, string>;
|
||||
};
|
||||
tampered.files["project.json"] = "{}";
|
||||
|
||||
const target = new MemoryWorkspaceStorage();
|
||||
expect(await importWorkspaceBundle(target, JSON.stringify(tampered))).toMatchObject({
|
||||
ok: false,
|
||||
damages: [expect.objectContaining({ code: "WORKSPACE_CHECKSUM_MISMATCH" })]
|
||||
});
|
||||
expect(await target.list()).toEqual([]);
|
||||
});
|
||||
});
|
||||
Reference in New Issue
Block a user