300 lines
9.1 KiB
JavaScript
300 lines
9.1 KiB
JavaScript
import { test } from "node:test";
|
|
import assert from "node:assert/strict";
|
|
import { Matrix4 } from "three";
|
|
import { modelResourceUrl } from "../src/scene/assembly-plan.ts";
|
|
import {
|
|
resolveAssembly,
|
|
interfaceCategory,
|
|
componentCategory,
|
|
componentPorts,
|
|
} from "../src/scene/automatic-assembly.ts";
|
|
|
|
const identity = () => new Matrix4();
|
|
function part(id, category, root = "ROOT", frames = []) {
|
|
return {
|
|
item: {
|
|
instanceId: id,
|
|
component: {
|
|
componentName: id,
|
|
category,
|
|
urdfInfo: { rootLink: root, frames },
|
|
},
|
|
},
|
|
rootLink: root,
|
|
ports: new Map(),
|
|
frames: new Map([[root, identity()]]),
|
|
urdfFrames: frames,
|
|
};
|
|
}
|
|
function assembly(status = "ready") {
|
|
return { status, rootInstanceId: "torso", connections: [], issues: [] };
|
|
}
|
|
const equalMatrix = (a, b) =>
|
|
a.elements.forEach((value, index) =>
|
|
assert.ok(Math.abs(value - b.elements[index]) < 1e-10),
|
|
);
|
|
|
|
test("frame indexes and interfaceChild determine the child placement", () => {
|
|
const parentInterface = new Matrix4().makeTranslation(0.1, 0.2, 0.3);
|
|
const childInterface = new Matrix4().makeTranslation(-0.2, 0.1, 0.05);
|
|
const parent = part("torso", "TORSO_0", "BASE", [
|
|
{ name: "J_ARM", child: "LINK_ARM_R" },
|
|
]);
|
|
parent.frames.set("LINK_ARM_R", parentInterface);
|
|
const child = part("arm", "ARM_R", "ARM_ROOT", [
|
|
{ name: "INCOMING", child: "LINK_ARM_R" },
|
|
]);
|
|
child.frames.set("LINK_ARM_R", childInterface);
|
|
const plan = assembly();
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: "J_ARM",
|
|
parentLink: "BASE",
|
|
childInstanceId: "arm",
|
|
childFrame: "INCOMING",
|
|
childRootLink: "ARM_ROOT",
|
|
interfaceChild: "LINK_ARM_R",
|
|
parentFrameIndex: 0,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly([parent, child], plan);
|
|
equalMatrix(
|
|
result.world.get("arm"),
|
|
parentInterface.clone().multiply(childInterface.clone().invert()),
|
|
);
|
|
assert.deepEqual(
|
|
result.connections.map(({ parentFrame, childFrame }) => [
|
|
parentFrame,
|
|
childFrame,
|
|
]),
|
|
[["J_ARM", "INCOMING"]],
|
|
);
|
|
});
|
|
|
|
test("canonical backend interface names resolve raw URDF Link ids by frame index", () => {
|
|
const parentInterface = new Matrix4().makeTranslation(0.1, 0.2, 0.3);
|
|
const childInterface = new Matrix4().makeTranslation(-0.2, 0.1, 0.05);
|
|
const parent = part("torso", "TORSO_0", "BASE", [
|
|
{ name: "J_LEG", child: "LINK_LEG_L" },
|
|
]);
|
|
parent.frames = new Map([
|
|
["BASE", identity()],
|
|
["LINK#2_LEG_L", parentInterface],
|
|
]);
|
|
parent.frameLinks = new Map([[0, "LINK#2_LEG_L"]]);
|
|
const child = part("leg", "LEG_L", "LEG_ROOT", [
|
|
{ name: "INCOMING", child: "LINK_LEG_L" },
|
|
]);
|
|
child.frames = new Map([
|
|
["LEG_ROOT", identity()],
|
|
["LINK#4_LEG_L", childInterface],
|
|
]);
|
|
child.frameLinks = new Map([[0, "LINK#4_LEG_L"]]);
|
|
const plan = assembly();
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: "J_LEG",
|
|
parentLink: "BASE",
|
|
childInstanceId: "leg",
|
|
childFrame: "INCOMING",
|
|
childRootLink: "LEG_ROOT",
|
|
interfaceChild: "LINK_LEG_L",
|
|
parentFrameIndex: 0,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly([parent, child], plan);
|
|
equalMatrix(
|
|
result.world.get("leg"),
|
|
parentInterface.clone().multiply(childInterface.clone().invert()),
|
|
);
|
|
assert.equal(result.connections.length, 1);
|
|
assert.deepEqual(result.warnings, []);
|
|
});
|
|
|
|
test("a synthetic root interface frame resolves to the real root Link", () => {
|
|
const torso = part("torso", "TORSO_0", "LINK_TORSO_0", [
|
|
{ name: "J_HEAD", child: "LINK_HEAD_0" },
|
|
{
|
|
name: "__root_interface__",
|
|
kind: "interface",
|
|
parent: null,
|
|
child: "LINK_HEAD_0",
|
|
interfaceType: "HEAD_0",
|
|
isRoot: true,
|
|
synthetic: true,
|
|
xyz: [0, 0, 0],
|
|
rpy: [0, 0, 0],
|
|
},
|
|
]);
|
|
torso.frames.set("LINK_HEAD_0", new Matrix4().makeTranslation(0, 0, 0.4));
|
|
const head = part("head", "HEAD_0", "LINK_HEAD_0", [
|
|
{
|
|
name: "__root_interface__",
|
|
kind: "interface",
|
|
parent: null,
|
|
child: "LINK_HEAD_0",
|
|
interfaceType: "HEAD_0",
|
|
isRoot: true,
|
|
synthetic: true,
|
|
xyz: [0, 0, 0],
|
|
rpy: [0, 0, 0],
|
|
},
|
|
]);
|
|
const plan = assembly();
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: "__root_interface__",
|
|
parentLink: "LINK_HEAD_0",
|
|
childInstanceId: "head",
|
|
childFrame: "__root_interface__",
|
|
childRootLink: "LINK_HEAD_0",
|
|
interfaceChild: "LINK_HEAD_0",
|
|
parentFrameIndex: 1,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly([torso, head], plan);
|
|
equalMatrix(
|
|
result.world.get("head"),
|
|
new Matrix4().makeTranslation(0, 0, 0.4),
|
|
);
|
|
assert.equal(result.connections[0].parentFrame, "__root_interface__");
|
|
assert.equal(result.connections[0].childFrame, "__root_interface__");
|
|
equalMatrix(
|
|
result.connections[0].parentInterface,
|
|
torso.frames.get("LINK_HEAD_0"),
|
|
);
|
|
equalMatrix(
|
|
result.connections[0].childInterface,
|
|
head.frames.get("LINK_HEAD_0"),
|
|
);
|
|
});
|
|
|
|
test("blocked assembly does not infer or create connections", () => {
|
|
const plan = assembly("blocked");
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: null,
|
|
parentLink: null,
|
|
childInstanceId: "head",
|
|
childFrame: null,
|
|
childRootLink: null,
|
|
interfaceChild: "LINK_HEAD_0",
|
|
parentFrameIndex: 0,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly(
|
|
[part("torso", "TORSO_0"), part("head", "HEAD_0")],
|
|
plan,
|
|
);
|
|
assert.equal(result.connections.length, 0);
|
|
assert.equal(result.world.size, 1);
|
|
});
|
|
|
|
test("backend-approved connections ignore mismatched diagnostic interface names", () => {
|
|
const torso = part("torso", "TORSO_0", "BASE", [
|
|
{ name: "J_HEAD", child: "LINK_HEAD_0" },
|
|
]);
|
|
torso.frames.set("LINK_HEAD_0", identity());
|
|
const head = part("head", "HEAD_0", "HEAD_ROOT", [
|
|
{ name: "INCOMING", child: "LINK_HEAD_0" },
|
|
]);
|
|
head.frames.set("LINK_HEAD_0", identity());
|
|
const plan = assembly();
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: "J_HEAD",
|
|
parentLink: "BASE",
|
|
childInstanceId: "head",
|
|
childFrame: "INCOMING",
|
|
childRootLink: "HEAD_ROOT",
|
|
interfaceChild: "LINK_ARM_R",
|
|
parentFrameIndex: 0,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly([torso, head], plan);
|
|
assert.equal(result.world.has("head"), true);
|
|
assert.equal(result.connections.length, 1);
|
|
assert.deepEqual(result.warnings, []);
|
|
});
|
|
|
|
test("invalid frame indexes are still rejected because no assembly pose can be calculated", () => {
|
|
const torso = part("torso", "TORSO_0", "BASE", [
|
|
{ name: "J_HEAD", child: "LINK_HEAD_0" },
|
|
]);
|
|
const head = part("head", "HEAD_0", "HEAD_ROOT", [
|
|
{ name: "INCOMING", child: "LINK_HEAD_0" },
|
|
]);
|
|
const plan = assembly();
|
|
plan.connections.push({
|
|
parentInstanceId: "torso",
|
|
parentFrame: "J_HEAD",
|
|
parentLink: "BASE",
|
|
childInstanceId: "head",
|
|
childFrame: "INCOMING",
|
|
childRootLink: "HEAD_ROOT",
|
|
parentFrameIndex: 3,
|
|
childFrameIndex: 0,
|
|
});
|
|
const result = resolveAssembly([torso, head], plan);
|
|
assert.equal(result.world.has("head"), false);
|
|
assert.match(result.warnings[0], /frame 下标无效/);
|
|
});
|
|
|
|
test("a component without a backend connection remains available separately", () => {
|
|
const result = resolveAssembly(
|
|
[part("torso", "TORSO_0"), part("head", "HEAD_0")],
|
|
assembly(),
|
|
);
|
|
assert.equal(result.world.size, 1);
|
|
assert.match(result.warnings[0], /未提供连接/);
|
|
});
|
|
|
|
test("interface and component classification preserve explicit metadata", () => {
|
|
assert.equal(interfaceCategory("LINK_ARM_R"), "ARM_R");
|
|
assert.equal(interfaceCategory(" LINK_HEAD_0 "), "HEAD_0");
|
|
assert.equal(interfaceCategory("LINK#4_LEG_L"), "LEG_L");
|
|
assert.equal(interfaceCategory("Link-head"), undefined);
|
|
assert.equal(
|
|
componentPorts(
|
|
["BASE", "LINK_HEAD_0"],
|
|
[{ child: "LINK_HEAD_0", interfaceType: "HEAD_0" }],
|
|
).get("LINK_HEAD_0"),
|
|
"HEAD_0",
|
|
);
|
|
assert.equal(componentPorts(["LINK_HEAD_0 "]).get("LINK_HEAD_0 "), "HEAD_0");
|
|
const spiderLinks = [
|
|
...Array.from({ length: 4 }, (_, index) => `LINK#${index + 1}_LEG_L`),
|
|
...Array.from({ length: 4 }, (_, index) => `LINK#${index + 1}_LEG_R`),
|
|
];
|
|
const spiderPorts = componentPorts(spiderLinks);
|
|
assert.equal(spiderPorts.size, 8);
|
|
assert.deepEqual(
|
|
[...spiderPorts.values()],
|
|
["LEG_L", "LEG_L", "LEG_L", "LEG_L", "LEG_R", "LEG_R", "LEG_R", "LEG_R"],
|
|
);
|
|
assert.equal(
|
|
componentCategory({
|
|
urdfMainPath: "urdf/%E8%B6%B3_L_%E5%9E%8B%E5%8F%B7.urdf",
|
|
component: { category: "LEG_L" },
|
|
}),
|
|
"LEG_L",
|
|
);
|
|
});
|
|
|
|
test("model assets use the direct storage URL and preserve signed URLs", () => {
|
|
assert.equal(
|
|
modelResourceUrl(
|
|
"https://dev-cadworld-admin.robotquan.com/admin-api/component-assets/head/%E5%A4%B4.STL?v=2",
|
|
"http://127.0.0.1:8091",
|
|
),
|
|
"https://linkerbot.oss-cn-beijing.aliyuncs.com/head/%E5%A4%B4.STL?v=2",
|
|
);
|
|
assert.equal(
|
|
modelResourceUrl(
|
|
"https://assets.example/head.stl",
|
|
"http://127.0.0.1:8091",
|
|
),
|
|
"https://assets.example/head.stl",
|
|
);
|
|
});
|