Files
frontend/Client/tests/assembly-motion.test.mjs

221 lines
7.2 KiB
JavaScript

import { test } from "node:test";
import assert from "node:assert/strict";
import { Group, Matrix4, Vector3, Euler } from "three";
import { connectComponentGroups } from "../src/scene/assembly-motion.ts";
import {
parseRotationAxis,
rotationAxisVector,
ROTATION_AXES,
} from "../../shared/rotation-axis.ts";
const equal = (a, b) =>
a.elements.forEach((x, i) =>
assert.ok(Math.abs(x - b.elements[i]) < 1e-10, `matrix[${i}]`),
);
const pose = (xyz, rpy) =>
new Matrix4()
.makeRotationFromEuler(new Euler(...rpy, "ZYX"))
.setPosition(...xyz);
test("rotationAxis is optional and uses explicit positive/negative local unit axes", () => {
for (const empty of ["", null, undefined, " "])
assert.equal(parseRotationAxis(empty), null);
assert.equal(parseRotationAxis(" Z "), "z");
assert.equal(parseRotationAxis("+X"), "x");
for (const axis of ROTATION_AXES)
assert.equal(new Vector3(...rotationAxisVector(axis)).length(), 1);
for (const value of ["xyz", "0 0 1", "r", 1, {}, "z;"])
assert.throws(() => parseRotationAxis(value), /旋转轴/);
});
for (const axis of ROTATION_AXES)
test(`${axis} rotates at the interface, preserves URDF local poses, and carries descendants`, () => {
const root = new Group(),
body = new Group(),
head = new Group(),
hand = new Group(),
originalJoint = new Group();
root.add(body, head, hand);
head.add(originalJoint);
originalJoint.position.set(0.1, 0.02, 0.03);
originalJoint.rotation.set(0.2, 0.3, 0.4);
originalJoint.updateMatrix();
const localBefore = originalJoint.matrix.clone();
const B = pose([0.3, -0.2, 0.1], [0.3, 0.2, 0.1]),
P = pose([0.2, 0.1, 0.4], [0.8, -0.6, 0.2]),
C = pose([-0.03, 0.02, 0.01], [0.1, 0.2, -0.3]),
D = pose([0.1, 0, 0.2], [0, 0.3, 0]);
body.matrixAutoUpdate = false;
body.matrix.copy(B);
const groups = new Map([
["body", body],
["head", head],
["hand", hand],
]);
const part = (id, rotationAxis, frames) => ({
item: { instanceId: id, component: { componentName: id, rotationAxis } },
frames: new Map(Object.entries(frames)),
urdfFrames: [],
});
const parts = [
part("body", null, { socket: P }),
part("head", axis, { mount: C, handSocket: D }),
part("hand", null, { mount: new Matrix4() }),
];
const connection = {
parent: "body",
child: "head",
parentFrame: "socket",
childFrame: "mount",
relative: P.clone().multiply(C.clone().invert()),
parentInterface: P,
childInterface: C,
};
const result = connectComponentGroups(groups, parts, [
connection,
{
parent: "head",
child: "hand",
parentFrame: "handSocket",
childFrame: "mount",
relative: D,
parentInterface: D,
childInterface: new Matrix4(),
},
]);
assert.equal(result.motions.length, 1);
assert.deepEqual(result.warnings, []);
const motion = result.motions[0];
root.updateMatrixWorld(true);
equal(head.matrixWorld, B.clone().multiply(P).multiply(C.clone().invert()));
for (const angle of [0.7, -1.2, 0, Math.PI * 3]) {
motion.setAngle(angle);
root.updateMatrixWorld(true);
const expected = B.clone()
.multiply(P)
.multiply(
new Matrix4().makeRotationAxis(
new Vector3(...rotationAxisVector(axis)),
angle,
),
)
.multiply(C.clone().invert());
equal(head.matrixWorld, expected);
equal(hand.matrixWorld, expected.clone().multiply(D));
equal(originalJoint.matrix, localBefore);
const actualPivot = new Vector3().setFromMatrixPosition(
head.matrixWorld.clone().multiply(C),
);
const fixedPivot = new Vector3().setFromMatrixPosition(
B.clone().multiply(P),
);
assert.ok(actualPivot.distanceTo(fixedPivot) < 1e-10);
}
assert.throws(() => motion.setAngle(NaN), /有效数值/);
});
test("missing or invalid axis keeps a visible fixed connection; orphan parts never get a guessed joint", () => {
for (const axis of [null, undefined, "invalid"]) {
const root = new Group(),
p = new Group(),
c = new Group(),
orphan = new Group();
root.add(p, c, orphan);
const relative = new Matrix4().makeTranslation(1, 2, 3);
const parts = [
{
item: { instanceId: "p", component: {} },
frames: new Map(),
urdfFrames: [],
},
{
item: {
instanceId: "c",
component: { componentName: "child", rotationAxis: axis },
},
frames: new Map(),
urdfFrames: [],
},
{
item: { instanceId: "orphan", component: { rotationAxis: "z" } },
frames: new Map(),
urdfFrames: [],
},
];
const result = connectComponentGroups(
new Map([
["p", p],
["c", c],
["orphan", orphan],
]),
parts,
[
{
parent: "p",
child: "c",
relative,
parentFrame: "socket",
childFrame: "mount",
parentInterface: relative,
childInterface: new Matrix4(),
},
],
);
root.updateMatrixWorld(true);
assert.equal(result.motions.length, 0);
equal(c.matrixWorld, relative);
assert.equal(orphan.parent, root);
assert.equal(c.visible, true);
assert.equal(result.warnings.length, axis === "invalid" ? 1 : 0);
}
});
test("runtime refresh propagates an internal parent URDF joint to a child URDF", () => {
const root = new Group();
const body = new Group();
const hand = new Group();
const internalJoint = new Group();
const bodyLink = new Group();
const handLink = new Group();
body.add(internalJoint);
internalJoint.add(bodyLink);
hand.add(handLink);
root.add(body, hand);
bodyLink.position.set(1, 0, 0);
handLink.position.set(0.5, 0, 0);
const parts = [
{
item: { instanceId: "body", component: { componentName: "body", rotationAxis: null } },
frames: new Map([["socket", new Matrix4().makeTranslation(1, 0, 0)]]),
linkObjects: new Map([["bodyLink", bodyLink]]),
urdfFrames: [],
},
{
item: { instanceId: "hand", component: { componentName: "hand", rotationAxis: null } },
frames: new Map([["mount", new Matrix4().makeTranslation(0.5, 0, 0)]]),
linkObjects: new Map([["handLink", handLink]]),
urdfFrames: [],
},
];
const result = connectComponentGroups(
new Map([["body", body], ["hand", hand]]),
parts,
[{
parent: "body",
child: "hand",
parentFrame: "socket",
childFrame: "mount",
parentLink: "bodyLink",
childLink: "handLink",
relative: new Matrix4().makeTranslation(0.5, 0, 0),
parentInterface: new Matrix4().makeTranslation(1, 0, 0),
childInterface: new Matrix4().makeTranslation(0.5, 0, 0),
}],
);
root.updateMatrixWorld(true);
const before = hand.matrixWorld.clone();
internalJoint.rotation.z = Math.PI / 2;
internalJoint.updateMatrix();
result.refresh();
root.updateMatrixWorld(true);
assert.notDeepEqual(hand.matrixWorld.elements, before.elements);
const position = new Vector3().setFromMatrixPosition(handLink.matrixWorld);
assert.ok(position.distanceTo(new Vector3(0, 1, 0)) < 1e-10);
});