221 lines
7.2 KiB
JavaScript
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);
|
|
});
|