276 lines
9.1 KiB
JavaScript
276 lines
9.1 KiB
JavaScript
import test from "node:test";
|
|
import assert from "node:assert/strict";
|
|
import { readFileSync } from "node:fs";
|
|
import { validatePackage } from "../src/platform/loader/SchemaValidator.js";
|
|
import { ObservationPipeline } from "../src/platform/policy/ObservationPipeline.js";
|
|
import { TermRegistry } from "../src/platform/policy/TermRegistry.js";
|
|
import { PolicyStateMachine } from "../src/platform/skills/PolicyStateMachine.js";
|
|
import { JointPdActuation } from "../src/platform/sim/actuation/JointPdActuation.js";
|
|
import { InputRouter } from "../src/platform/input/InputRouter.js";
|
|
import { OnnxPolicy } from "../src/platform/policy/controllers/OnnxPolicy.js";
|
|
const config = JSON.parse(
|
|
readFileSync(new URL("../public/robots/rp1/robot.json", import.meta.url)),
|
|
);
|
|
|
|
test("RP1 contract has 24 named actions, 243 term-major observations and fail-closed availability gates", () => {
|
|
validatePackage(config);
|
|
const names = config.joints.groups.policy;
|
|
assert.equal(names.length, 24);
|
|
assert.equal(config.actions[0].scale, 0.25);
|
|
assert.equal(config.sim.timestep * config.sim.decimation, 0.02);
|
|
assert.equal(config.observations.obs.history, 3);
|
|
assert.equal(config.observations.obs.layout, "term_major");
|
|
assert.deepEqual(
|
|
config.observations.obs.terms.map((t) => t.type),
|
|
[
|
|
"gyro",
|
|
"projected_gravity",
|
|
"command",
|
|
"joint_pos_rel",
|
|
"joint_vel",
|
|
"last_action",
|
|
],
|
|
);
|
|
assert.equal(
|
|
names.filter((n) => config.actuation.joints[n].kp === 120).length,
|
|
8,
|
|
);
|
|
const invalid = structuredClone(config);
|
|
invalid.skills[1].available = true;
|
|
delete invalid.skills[1].controller.policy;
|
|
invalid.input.bindings.find((b) => b.action === "skill:dance").disabled =
|
|
true;
|
|
assert.throws(() => validatePackage(invalid), /policy/);
|
|
invalid.skills[1].controller.policy = "policies/dance.onnx";
|
|
assert.throws(() => validatePackage(invalid), /disabled/);
|
|
invalid.input.bindings.find((b) => b.action === "skill:dance").disabled =
|
|
false;
|
|
validatePackage(invalid);
|
|
invalid.skills[1].owns = "missing";
|
|
assert.throws(() => validatePackage(invalid), /group/);
|
|
});
|
|
|
|
test("history is oldest→newest within each term, backfills first sample, debug doesn't advance, reset clears", () => {
|
|
const registry = new TermRegistry([
|
|
[
|
|
"value",
|
|
{
|
|
size: (t) => t.size,
|
|
write: (out, offset, ctx, op) =>
|
|
out.set(ctx.value.slice(0, op.term.size), offset),
|
|
},
|
|
],
|
|
]);
|
|
const ctx = { value: [1, 2] };
|
|
const specs = {
|
|
obs: {
|
|
history: 3,
|
|
layout: "term_major",
|
|
terms: [
|
|
{ type: "value", size: 2 },
|
|
{ type: "value", size: 1 },
|
|
],
|
|
},
|
|
};
|
|
const pipeline = new ObservationPipeline(specs, ctx, { registry });
|
|
assert.deepEqual([...pipeline.write().obs], [1, 2, 1, 2, 1, 2, 1, 1, 1]);
|
|
ctx.value = [3, 4];
|
|
assert.deepEqual([...pipeline.write().obs], [1, 2, 1, 2, 3, 4, 1, 1, 3]);
|
|
ctx.value = [5, 6];
|
|
assert.deepEqual(
|
|
[...pipeline.write({ advance: false }).obs],
|
|
[1, 2, 1, 2, 5, 6, 1, 1, 5],
|
|
);
|
|
assert.deepEqual([...pipeline.write().obs], [1, 2, 3, 4, 5, 6, 1, 3, 5]);
|
|
pipeline.reset();
|
|
pipeline.write({ advance: false });
|
|
ctx.value = [9, 10];
|
|
assert.deepEqual([...pipeline.write().obs], [9, 10, 9, 10, 9, 10, 9, 9, 9]);
|
|
ctx.value = [5, 6];
|
|
specs.obs.layout = "frame_major";
|
|
const frame = new ObservationPipeline(specs, ctx, { registry });
|
|
frame.write();
|
|
ctx.value = [7, 8];
|
|
assert.deepEqual([...frame.write().obs], [5, 6, 5, 5, 6, 5, 7, 8, 7]);
|
|
});
|
|
|
|
function stateMachine() {
|
|
const c = structuredClone(config);
|
|
c.skills[1].available = true;
|
|
c.skills[1].duration = 10;
|
|
c.skills[1].blend = 0.5;
|
|
c.skills[2].available = false;
|
|
c.joints.groups.policy = ["joint"];
|
|
return new PolicyStateMachine(c);
|
|
}
|
|
test("exclusive full-body FSM waits for actual stop, blends, times out and returns to walk", () => {
|
|
const fsm = stateMachine(),
|
|
targets = new Map([["joint", 1]]);
|
|
assert.equal(fsm.request("wave", 0), false);
|
|
assert.equal(fsm.request("dance", 0), true);
|
|
assert.equal(fsm.request("dance", 0), false);
|
|
fsm.step(0.5, [0, 0, 0], 0.8, 0, targets);
|
|
assert.equal(fsm.state, "settling");
|
|
fsm.step(1, [0, 0, 0], 0, 0, targets);
|
|
assert.equal(fsm.step(1.5, [0, 0, 0], 0, 0, targets), "dance");
|
|
targets.set("joint", 3);
|
|
fsm.blend(1.75, targets);
|
|
assert.equal(targets.get("joint"), 2);
|
|
fsm.step(2.1, [0, 0, 0], 0, 0, targets);
|
|
assert.equal(fsm.state, "playing");
|
|
assert.equal(fsm.step(11.6, [0, 0, 0], 0, 0, targets), "walk");
|
|
assert.equal(fsm.state, "returning");
|
|
fsm.step(12.2, [0, 0, 0], 0, 0, targets);
|
|
assert.equal(fsm.state, "walk");
|
|
fsm.request("dance", 13);
|
|
fsm.step(19, [0, 0, 0], 1, 0, targets);
|
|
assert.equal(fsm.state, "walk");
|
|
});
|
|
test("episode blending never overwrites independent overlay joint targets", () => {
|
|
const fsm = stateMachine(),
|
|
targets = new Map([
|
|
["joint", 1],
|
|
["head", 2],
|
|
]);
|
|
fsm.request("dance", 0);
|
|
fsm.step(0.1, [0, 0, 0], 0, 0, targets);
|
|
fsm.step(0.6, [0, 0, 0], 0, 0, targets);
|
|
targets.set("joint", 3);
|
|
targets.set("head", 7);
|
|
fsm.blend(0.85, targets);
|
|
assert.equal(targets.get("joint"), 2);
|
|
assert.equal(targets.get("head"), 7);
|
|
});
|
|
test("movement cancels pending skills and interrupts active skills with a return blend; reset clears", () => {
|
|
const fsm = stateMachine(),
|
|
targets = new Map();
|
|
fsm.request("dance", 0);
|
|
fsm.step(0.1, [0.4, 0, 0], 0, 0, targets);
|
|
assert.equal(fsm.state, "walk");
|
|
fsm.request("dance", 1);
|
|
fsm.step(1.1, [0, 0, 0], 0, 0, targets);
|
|
fsm.step(1.6, [0, 0, 0], 0, 0, targets);
|
|
assert.equal(fsm.active, "dance");
|
|
fsm.step(1.7, [0, 0, 0.6], 0, 0, targets);
|
|
assert.equal(fsm.state, "returning");
|
|
assert.equal(fsm.active, "walk");
|
|
fsm.reset();
|
|
assert.equal(fsm.state, "walk");
|
|
assert.equal(fsm.pending, null);
|
|
});
|
|
test("named per-joint gains and four-quadrant torque-speed envelope apply atomically", () => {
|
|
const sim = {
|
|
joints: new Map([
|
|
["a", { qpos: 0, dof: 0, actuator: 1 }],
|
|
["b", { qpos: 1, dof: 1, actuator: 0 }],
|
|
]),
|
|
data: { qpos: [0, 0], qvel: [10, -10], ctrl: new Float64Array(2) },
|
|
};
|
|
const targets = new Map([
|
|
["a", 2],
|
|
["b", -2],
|
|
]);
|
|
const pd = new JointPdActuation(sim, targets, {
|
|
kp: 1,
|
|
kd: 0,
|
|
torque_limit: 10,
|
|
joints: {
|
|
a: { kp: 20, velocity_limit: 10, keep_velocity: 5 },
|
|
b: { kp: 20, velocity_limit: 10, keep_velocity: 5 },
|
|
},
|
|
});
|
|
pd.apply();
|
|
assert.deepEqual([...sim.data.ctrl], [0, 0]);
|
|
sim.data.qvel = [-10, 10];
|
|
pd.apply();
|
|
assert.deepEqual([...sim.data.ctrl], [-10, 10]);
|
|
targets.set("b", NaN);
|
|
assert.throws(() => pd.apply(), /non-finite/);
|
|
assert.deepEqual([...sim.data.ctrl], [-10, 10]);
|
|
});
|
|
test("independent skills own histories/defaults and share only targets; failed smoke releases session", async () => {
|
|
let released = 0;
|
|
const session = {
|
|
inputNames: ["obs"],
|
|
outputNames: ["actions"],
|
|
inputMetadata: [
|
|
{ name: "obs", isTensor: true, type: "float32", shape: [1, 24] },
|
|
],
|
|
outputMetadata: [
|
|
{ name: "actions", isTensor: true, type: "float32", shape: [1, 24] },
|
|
],
|
|
run: async () => ({ actions: { data: new Float32Array(24).fill(1) } }),
|
|
release: async () => released++,
|
|
};
|
|
const ort = {
|
|
Tensor: class {
|
|
constructor(type, data) {
|
|
this.data = data;
|
|
}
|
|
},
|
|
InferenceSession: { create: async () => session },
|
|
};
|
|
const names = config.joints.groups.policy;
|
|
const shared = {
|
|
config,
|
|
policyNames: names,
|
|
defaults: new Map(names.map((n) => [n, 0])),
|
|
command: new Float32Array(3),
|
|
targets: new Map(),
|
|
sim: {
|
|
joints: new Map(names.map((n, i) => [n, { qpos: i }])),
|
|
data: { qpos: new Float64Array(24) },
|
|
},
|
|
};
|
|
const skill = {
|
|
id: "fixture",
|
|
defaults: Object.fromEntries(names.map((n) => [n, 0.5])),
|
|
observations: {
|
|
obs: { terms: [{ type: "joint_pos_rel", group: "policy" }] },
|
|
},
|
|
};
|
|
const policy = await OnnxPolicy.load(
|
|
ort,
|
|
new Uint8Array(),
|
|
config,
|
|
shared,
|
|
skill,
|
|
);
|
|
assert.equal(policy.ctx.targets, shared.targets);
|
|
assert.equal(shared.defaults.get(names[0]), 0);
|
|
assert.equal(policy.obs.write().obs[0], -0.5);
|
|
await policy.run();
|
|
assert.equal(shared.targets.get(names[0]), 0.75);
|
|
policy.reset();
|
|
assert.equal(policy.ctx.lastAction[0], 0);
|
|
await policy.dispose();
|
|
session.run = async () => ({
|
|
actions: { data: new Float32Array(24).fill(NaN) },
|
|
});
|
|
await assert.rejects(
|
|
() => OnnxPolicy.load(ort, new Uint8Array(), config, shared, skill),
|
|
/smoke/,
|
|
);
|
|
assert.equal(released, 2);
|
|
});
|
|
test("unavailable-skill keyboard bindings never dispatch", () => {
|
|
const router = new InputRouter({
|
|
bindings: config.input.bindings.map((b) => ({
|
|
...b,
|
|
disabled: b.action.startsWith("skill:") ? true : b.disabled,
|
|
})),
|
|
getVelocityLimits: () => config.commands.limits,
|
|
});
|
|
const target = new EventTarget(),
|
|
calls = [];
|
|
router.onAction = (action) => calls.push(action);
|
|
router.init(target);
|
|
for (const code of ["KeyJ", "KeyH", "Space"]) {
|
|
const e = new Event("keydown");
|
|
Object.assign(e, { code });
|
|
target.dispatchEvent(e);
|
|
}
|
|
assert.deepEqual(calls, ["reset"]);
|
|
router.dispose();
|
|
});
|