210 lines
6.8 KiB
JavaScript
210 lines
6.8 KiB
JavaScript
import test from "node:test";
|
|
import assert from "node:assert/strict";
|
|
import { ObservationPipeline } from "../src/platform/policy/ObservationPipeline.js";
|
|
import { ActionPipeline } from "../src/platform/policy/ActionPipeline.js";
|
|
import {
|
|
JointPdActuation,
|
|
NativeActuation,
|
|
} from "../src/platform/sim/actuation/JointPdActuation.js";
|
|
import { SkillManager } from "../src/platform/skills/SkillManager.js";
|
|
import {
|
|
InputRouter,
|
|
isEditingTarget,
|
|
} from "../src/platform/input/InputRouter.js";
|
|
test("button focus protects native Space/Enter without swallowing movement keys", () => {
|
|
const target = { closest: (selector) => (selector === "button" ? {} : null) };
|
|
assert.equal(isEditingTarget(target, "Space"), true);
|
|
assert.equal(isEditingTarget(target, "KeyW"), false);
|
|
assert.equal(isEditingTarget({ closest: () => ({}) }, "KeyW"), true);
|
|
});
|
|
function context() {
|
|
return {
|
|
config: { joints: { groups: { policy: ["b", "a"] } }, skills: [] },
|
|
policyNames: ["b", "a"],
|
|
defaults: new Map([
|
|
["a", 1],
|
|
["b", 2],
|
|
]),
|
|
lastAction: new Float32Array([0.3, 0.4]),
|
|
command: new Float32Array([0.15, 0, -0.6]),
|
|
targets: new Map([
|
|
["a", 0],
|
|
["b", 0],
|
|
]),
|
|
sim: {
|
|
mujoco: {
|
|
mjtObj: { mjOBJ_SENSOR: { value: 1 }, mjOBJ_BODY: { value: 2 } },
|
|
mj_name2id: (_m, type, name) => (name === "missing" ? -1 : 0),
|
|
},
|
|
model: { sensor_adr: [0], sensor_dim: [3] },
|
|
data: {
|
|
qpos: new Float64Array([1.5, 2.25]),
|
|
qvel: new Float64Array([0.1, 0.2]),
|
|
sensordata: new Float64Array([0.01, 0.02, 0.03]),
|
|
xquat: new Float64Array([1, 0, 0, 0]),
|
|
ctrl: new Float64Array(2),
|
|
},
|
|
joints: new Map([
|
|
["a", { qpos: 0, dof: 0, actuator: 1 }],
|
|
["b", { qpos: 1, dof: 1, actuator: 0 }],
|
|
]),
|
|
},
|
|
};
|
|
}
|
|
test("observation order/frame/defaults/history use policy order, not model order", () => {
|
|
const ctx = context(),
|
|
p = new ObservationPipeline(
|
|
{
|
|
obs: {
|
|
terms: [
|
|
{ type: "gyro", sensor: "gyro" },
|
|
{ type: "projected_gravity", body: "root" },
|
|
{ type: "joint_pos_rel", group: "policy" },
|
|
{ type: "joint_vel", group: "policy" },
|
|
{ type: "last_action", group: "policy" },
|
|
{ type: "command", size: 3 },
|
|
],
|
|
},
|
|
},
|
|
ctx,
|
|
);
|
|
const first = p.write().obs;
|
|
assert.equal(first.length, 15);
|
|
assert.deepEqual(
|
|
Array.from(first),
|
|
Array.from(
|
|
new Float32Array([
|
|
0.01, 0.02, 0.03, 0, -0, -1, 0.25, 0.5, 0.2, 0.1, 0.3, 0.4, 0.15, 0,
|
|
-0.6,
|
|
]),
|
|
),
|
|
);
|
|
ctx.sim.data.xquat.set([Math.SQRT1_2, Math.SQRT1_2, 0, 0]);
|
|
p.write();
|
|
assert.ok(Math.abs(first[4] + 1) < 1e-6);
|
|
assert.ok(Math.abs(first[5]) < 1e-6);
|
|
assert.equal(p.write().obs, first);
|
|
assert.throws(
|
|
() =>
|
|
new ObservationPipeline(
|
|
{ obs: { terms: [{ type: "gyro", sensor: "missing" }] } },
|
|
ctx,
|
|
),
|
|
/missing/,
|
|
);
|
|
});
|
|
test("body-frame linear velocity reads the named sensor and validates its dimension", () => {
|
|
const ctx = context();
|
|
const specs = {
|
|
obs: { terms: [{ type: "linear_velocity", sensor: "velocity" }] },
|
|
};
|
|
ctx.sim.model.sensor_adr[0] = 3;
|
|
ctx.sim.data.sensordata = new Float64Array([99, 98, 97, 1, -2, 3]);
|
|
assert.deepEqual(
|
|
Array.from(new ObservationPipeline(specs, ctx).write().obs),
|
|
[1, -2, 3],
|
|
);
|
|
ctx.sim.model.sensor_dim[0] = 1;
|
|
assert.throws(() => new ObservationPipeline(specs, ctx), /dimension 3/);
|
|
assert.throws(
|
|
() =>
|
|
new ObservationPipeline(
|
|
{ obs: { terms: [{ type: "linear_velocity", sensor: "missing" }] } },
|
|
ctx,
|
|
),
|
|
/missing sensor/,
|
|
);
|
|
});
|
|
test("actions resolve named defaults and native ctrl order; invalid output is atomic", () => {
|
|
const ctx = context(),
|
|
p = new ActionPipeline([{ group: "policy", scale: 2 }], ctx);
|
|
p.apply(new Float32Array([0.25, -0.5]));
|
|
new NativeActuation(ctx.sim, ctx.targets).apply();
|
|
assert.deepEqual(Array.from(ctx.sim.data.ctrl), [2.5, 0]);
|
|
const before = [...ctx.targets];
|
|
assert.throws(() => p.apply(new Float32Array([0.1, NaN])), /non-finite/);
|
|
assert.deepEqual([...ctx.targets], before);
|
|
assert.throws(() => p.apply(new Float32Array(3)), /expected/);
|
|
});
|
|
test("PD computes torque per substep and saturates without writing angle to motor", () => {
|
|
const ctx = context();
|
|
ctx.targets.set("a", 2);
|
|
ctx.targets.set("b", 0);
|
|
const pd = new JointPdActuation(ctx.sim, ctx.targets, {
|
|
kp: 2,
|
|
kd: 0.1,
|
|
torque_limit: 0.65,
|
|
});
|
|
pd.apply();
|
|
assert.deepEqual(Array.from(ctx.sim.data.ctrl), [-0.65, 0.65]);
|
|
ctx.sim.data.qpos[0] = 2;
|
|
pd.apply();
|
|
assert.ok(Math.abs(ctx.sim.data.ctrl[1] + 0.01) < 1e-12);
|
|
ctx.targets.set("b", NaN);
|
|
assert.throws(() => pd.apply(), /non-finite/);
|
|
});
|
|
test("head overlay blends, oscillates, stops at zero without touching primary targets", () => {
|
|
const skills = new SkillManager({
|
|
joints: { groups: { policy: ["leg"], head: ["head"] } },
|
|
skills: [
|
|
{ id: "walk", kind: "primary", owns: "policy" },
|
|
{
|
|
id: "head",
|
|
kind: "overlay",
|
|
owns: "head",
|
|
controller: {
|
|
type: "oscillator",
|
|
frequency: 0.35,
|
|
amplitude: Math.PI / 4,
|
|
blend: 0.5,
|
|
},
|
|
},
|
|
],
|
|
});
|
|
const targets = new Map([["leg", 0.123]]);
|
|
skills.toggle("head", 0, new Map([["head", 0.2]]));
|
|
skills.update(0.25, targets);
|
|
assert.equal(targets.get("head"), 0.1);
|
|
skills.update(0.5, targets);
|
|
skills.update(1, targets);
|
|
assert.ok(targets.get("head") > 0.6);
|
|
assert.equal(targets.get("leg"), 0.123);
|
|
skills.toggle("head", 1, new Map([["head", 0.4]]));
|
|
skills.update(1.25, targets);
|
|
assert.ok(Math.abs(targets.get("head") - 0.2) < 1e-10);
|
|
skills.update(1.5, targets);
|
|
assert.equal(targets.get("head"), 0);
|
|
skills.reset();
|
|
assert.equal(skills.overlays.get("head").enabled, false);
|
|
});
|
|
test("input holds aliases independently, clears focus loss, and ignores repeat", () => {
|
|
const target = new EventTarget(),
|
|
actions = [],
|
|
router = new InputRouter({
|
|
bindings: [
|
|
{ action: "move_forward", codes: ["KeyW", "ArrowUp"] },
|
|
{ action: "reset", codes: ["Space"] },
|
|
],
|
|
getVelocityLimits: () => [0.15, -0.1, 0.6],
|
|
});
|
|
router.onAction = (a) => actions.push(a);
|
|
router.init(target);
|
|
const key = (type, code, repeat = false) => {
|
|
const e = new Event(type);
|
|
Object.assign(e, { code, repeat });
|
|
target.dispatchEvent(e);
|
|
};
|
|
key("keydown", "KeyW");
|
|
key("keydown", "ArrowUp");
|
|
key("keyup", "KeyW");
|
|
assert.ok(router.command[0] > 0.1);
|
|
target.dispatchEvent(new Event("blur"));
|
|
assert.equal(router.command[0], 0);
|
|
key("keydown", "Space", true);
|
|
key("keydown", "Space");
|
|
assert.deepEqual(actions, ["reset"]);
|
|
router.dispose();
|
|
key("keydown", "KeyW");
|
|
assert.equal(router.command[0], 0);
|
|
});
|