fc5ee4f2c6
Add declarative robot packages, shared simulation lifecycle and Microban ONNX/PD walking with independent head control. Unify comic UI and chase camera; default robot packages to a 4.5m arena with real collision walls. Include regression tests, asset provenance/licenses and verification evidence.
384 lines
12 KiB
JavaScript
384 lines
12 KiB
JavaScript
import * as THREE from "three";
|
|
import { OrbitControls } from "three/addons/controls/OrbitControls.js";
|
|
import { loadRuntimes } from "./runtimes.js";
|
|
import { ControlScheduler } from "./ControlScheduler.js";
|
|
import { fetchChecked } from "../loader/PackageLoader.js";
|
|
import { readOnnxMetadata, resolveJointDefaults } from "../loader/metadata.js";
|
|
import { SimCore } from "../sim/SimCore.js";
|
|
import {
|
|
JointPdActuation,
|
|
NativeActuation,
|
|
} from "../sim/actuation/JointPdActuation.js";
|
|
import { ObservationPipeline } from "../policy/ObservationPipeline.js";
|
|
import { ActionPipeline } from "../policy/ActionPipeline.js";
|
|
import { OnnxController } from "../policy/controllers/OnnxController.js";
|
|
import { SkillManager } from "../skills/SkillManager.js";
|
|
import { InputRouter } from "../input/InputRouter.js";
|
|
import { RigRenderer } from "../render/RigRenderer.js";
|
|
import { ChaseCamera } from "../render/ChaseCamera.js";
|
|
|
|
export class RobotRuntime {
|
|
static async load(options) {
|
|
const { pkg, world } = options,
|
|
config = pkg.config,
|
|
{ mujoco, ort } = await loadRuntimes();
|
|
let sim, session, rig, worldPresentation;
|
|
try {
|
|
sim = await SimCore.load(mujoco, pkg, world);
|
|
const primary = config.skills.find((s) => s.kind === "primary");
|
|
if (primary.controller.type !== "onnx")
|
|
throw new Error("Primary controller must be ONNX");
|
|
const bytes = await fetchChecked(
|
|
pkg.asset(primary.controller.policy),
|
|
"bytes",
|
|
);
|
|
const metadata = readOnnxMetadata(bytes),
|
|
names = [...sim.joints.keys()];
|
|
const values =
|
|
config.joints.source === "onnx_metadata"
|
|
? resolveJointDefaults(metadata, names)
|
|
: names.map((n) => config.joints.defaults[n]);
|
|
const defaults = new Map(names.map((n, i) => [n, values[i]]));
|
|
for (const [name, value] of Object.entries(config.joints.defaults)) {
|
|
if (defaults.has(name) && Math.abs(defaults.get(name) - value) > 1e-6)
|
|
throw new Error(`joints.defaults.${name}: metadata conflict`);
|
|
defaults.set(name, value);
|
|
}
|
|
if ([...defaults.values()].some((v) => !Number.isFinite(v)))
|
|
throw new Error("joints.defaults: missing/invalid reference pose");
|
|
sim.reset(defaults);
|
|
session = await ort.InferenceSession.create(bytes, {
|
|
executionProviders: ["wasm"],
|
|
});
|
|
const ctx = {
|
|
sim,
|
|
config,
|
|
defaults,
|
|
policyNames: config.joints.groups[primary.owns],
|
|
lastAction: new Float32Array(config.joints.groups[primary.owns].length),
|
|
command: new Float32Array(config.commands.size),
|
|
targets: new Map(
|
|
names.map((n) => [n, sim.data.qpos[sim.joints.get(n).qpos]]),
|
|
),
|
|
};
|
|
const obs = new ObservationPipeline(config.observations, ctx),
|
|
actions = new ActionPipeline(config.actions, ctx);
|
|
const controller = new OnnxController(ort, session, obs, actions.size);
|
|
obs.write();
|
|
const smoke = await controller.run();
|
|
if (smoke.length !== actions.size || !smoke.every(Number.isFinite))
|
|
throw new Error("ONNX: standing-pose smoke test failed");
|
|
rig = new RigRenderer(sim.model, sim.data, { ...config.render, world });
|
|
worldPresentation = await options.createWorldPresentation?.({
|
|
sim,
|
|
rig,
|
|
world,
|
|
});
|
|
return new RobotRuntime({
|
|
...options,
|
|
config,
|
|
sim,
|
|
ctx,
|
|
obs,
|
|
actions,
|
|
controller,
|
|
rig,
|
|
worldPresentation,
|
|
});
|
|
} catch (error) {
|
|
worldPresentation?.dispose();
|
|
rig?.dispose();
|
|
await session?.release();
|
|
sim?.dispose();
|
|
throw error;
|
|
}
|
|
}
|
|
constructor(options) {
|
|
Object.assign(this, options);
|
|
this.onStatus ??= () => {};
|
|
this.onTelemetry ??= () => {};
|
|
this.fallen = false;
|
|
this.odo = 0;
|
|
this.lastPosition = new Float64Array(
|
|
this.sim.data.qpos.slice(this.sim.rootQpos, this.sim.rootQpos + 2),
|
|
);
|
|
this.disposed = false;
|
|
this.active = false;
|
|
this.positions = new Map();
|
|
this.skills = new SkillManager(this.config);
|
|
this.actuation =
|
|
this.config.actuation.type === "joint_pd"
|
|
? new JointPdActuation(
|
|
this.sim,
|
|
this.ctx.targets,
|
|
this.config.actuation,
|
|
)
|
|
: new NativeActuation(this.sim, this.ctx.targets);
|
|
this.input = new InputRouter({
|
|
bindings: this.config.input.bindings,
|
|
getVelocityLimits: () => this.config.commands.limits,
|
|
isEnabled: (action) =>
|
|
this.active &&
|
|
(action === "reset" ||
|
|
action === "chaseToggle" ||
|
|
(this.scheduler.running && !this.fallen)),
|
|
});
|
|
this.input.onAction = (action) => {
|
|
if (action === "reset")
|
|
this.reset().catch((error) =>
|
|
this.onStatus({ runtimeError: error.message }),
|
|
);
|
|
else if (action === "chaseToggle") this.restoreCamera();
|
|
else if (action.startsWith("toggle:")) this.toggle(action.slice(7));
|
|
};
|
|
this.tickCount = 0;
|
|
this.hzAt = performance.now();
|
|
this.ctrlHz = 0;
|
|
this.frames = 0;
|
|
this.fpsAt = this.hzAt;
|
|
this.telemetryAt = 0;
|
|
this.scheduler = new ControlScheduler(() => this.controlStep(), {
|
|
dt: this.config.sim.timestep * this.config.sim.decimation,
|
|
onError: (error) => {
|
|
this.input.clear();
|
|
this.onStatus({
|
|
runtimeError: error.message,
|
|
paused: true,
|
|
runtimePhase: "error",
|
|
});
|
|
},
|
|
});
|
|
this.scene.add(this.rig.root);
|
|
if (this.worldPresentation) this.scene.add(this.worldPresentation.root);
|
|
this.follow = new THREE.Vector3();
|
|
}
|
|
activate({ paused = false } = {}) {
|
|
if (this.disposed) throw new Error("Runtime disposed");
|
|
this.active = true;
|
|
this.controls = new OrbitControls(this.camera, this.renderer.domElement);
|
|
this.controls.enableDamping = true;
|
|
this.controls.minDistance = 0.2;
|
|
this.controls.maxDistance = 4;
|
|
this.controls.maxPolarAngle = Math.PI / 2 - 0.02;
|
|
this.chaseCamera = new ChaseCamera(
|
|
this.camera,
|
|
this.controls,
|
|
this.config.render.camera,
|
|
);
|
|
this.restoreCamera();
|
|
this.input.init();
|
|
if (!paused) this.resume();
|
|
else this.onStatus({ paused: true });
|
|
}
|
|
cameraPose() {
|
|
const q = this.sim.data.qpos,
|
|
a = this.sim.rootQpos,
|
|
quat = this.sim.data.xquat,
|
|
b = this.sim.root * 4;
|
|
this.follow.set(q[a], q[a + 2], -q[a + 1]);
|
|
return Math.atan2(
|
|
2 * (quat[b] * quat[b + 3] + quat[b + 1] * quat[b + 2]),
|
|
1 - 2 * (quat[b + 2] ** 2 + quat[b + 3] ** 2),
|
|
);
|
|
}
|
|
restoreCamera() {
|
|
const yaw = this.cameraPose();
|
|
this.chaseCamera.restore(this.follow, yaw);
|
|
}
|
|
status() {
|
|
return {
|
|
actuationLabel:
|
|
this.config.actuation.type === "joint_pd"
|
|
? "Simplified actuation · joint PD"
|
|
: "",
|
|
};
|
|
}
|
|
async deactivate() {
|
|
this.active = false;
|
|
await this.pause();
|
|
}
|
|
reactivate({ paused = false } = {}) {
|
|
this.active = true;
|
|
if (!paused) this.resume();
|
|
}
|
|
async pause() {
|
|
this.input.clear();
|
|
await this.scheduler.pause();
|
|
this.onStatus({ paused: true });
|
|
}
|
|
resume() {
|
|
if (this.disposed) return;
|
|
this.active = true;
|
|
this.input.clear();
|
|
this.scheduler.start();
|
|
this.onStatus({ paused: false });
|
|
}
|
|
reset() {
|
|
return (this.resetPending ??= this.#reset().finally(() => {
|
|
this.resetPending = null;
|
|
}));
|
|
}
|
|
async #reset() {
|
|
const running = this.scheduler.running;
|
|
await this.scheduler.pause();
|
|
this.input.clear();
|
|
this.ctx.command.fill(0);
|
|
this.ctx.lastAction.fill(0);
|
|
this.skills.reset();
|
|
this.sim.reset(this.ctx.defaults);
|
|
this.odo = 0;
|
|
this.actuation.resetStats?.();
|
|
this.lastPosition.set(
|
|
this.sim.data.qpos.subarray(this.sim.rootQpos, this.sim.rootQpos + 2),
|
|
);
|
|
for (const [name, joint] of this.sim.joints)
|
|
this.ctx.targets.set(name, this.sim.data.qpos[joint.qpos]);
|
|
this.fallen = false;
|
|
this.onStatus({ fallen: false });
|
|
this.rig.sync();
|
|
if (running) this.scheduler.start();
|
|
}
|
|
triggerAction(action) {
|
|
if (this.input.isEnabled(action)) this.input.onAction(action);
|
|
}
|
|
toggle(id) {
|
|
for (const [name, j] of this.sim.joints)
|
|
this.positions.set(name, this.sim.data.qpos[j.qpos]);
|
|
this.skills.toggle(id, this.sim.data.time, this.positions);
|
|
}
|
|
async controlStep() {
|
|
if (!this.fallen) {
|
|
this.ctx.command.set(this.input.command);
|
|
this.obs.write();
|
|
this.actions.apply(await this.controller.run());
|
|
this.skills.update(this.sim.data.time, this.ctx.targets);
|
|
}
|
|
for (let i = 0; i < this.config.sim.decimation; i++) {
|
|
this.actuation.apply();
|
|
this.sim.step();
|
|
}
|
|
const q = this.sim.data.qpos,
|
|
a = this.sim.rootQpos,
|
|
quat = this.sim.data.xquat,
|
|
b = this.sim.root * 4,
|
|
x = quat[b + 1],
|
|
y = quat[b + 2];
|
|
const gz = -(1 - 2 * (x * x + y * y));
|
|
if (
|
|
!q.every(Number.isFinite) ||
|
|
!this.sim.data.qvel.every(Number.isFinite) ||
|
|
!Number.isFinite(gz)
|
|
)
|
|
throw new Error("Physics produced non-finite state");
|
|
if (
|
|
!this.fallen &&
|
|
(gz > this.config.termination.tilt_z ||
|
|
q[a + 2] < this.config.termination.min_height)
|
|
) {
|
|
this.fallen = true;
|
|
this.input.clear();
|
|
for (const [name, j] of this.sim.joints)
|
|
this.ctx.targets.set(name, q[j.qpos]);
|
|
this.onStatus({ fallen: true });
|
|
}
|
|
this.odo += Math.hypot(
|
|
q[a] - this.lastPosition[0],
|
|
q[a + 1] - this.lastPosition[1],
|
|
);
|
|
this.lastPosition[0] = q[a];
|
|
this.lastPosition[1] = q[a + 1];
|
|
this.tickCount++;
|
|
const now = performance.now();
|
|
if (now - this.hzAt >= 1000) {
|
|
this.ctrlHz = Math.round((this.tickCount * 1000) / (now - this.hzAt));
|
|
this.tickCount = 0;
|
|
this.hzAt = now;
|
|
}
|
|
}
|
|
frame() {
|
|
if (this.disposed) return;
|
|
this.rig.sync();
|
|
if (this.chaseCamera) {
|
|
const yaw = this.cameraPose();
|
|
this.worldPresentation?.update(this.follow);
|
|
this.chaseCamera.update(this.follow, yaw, {
|
|
turning: Math.abs(this.input.command[2]) > 0.05,
|
|
});
|
|
}
|
|
const now = performance.now();
|
|
this.frames++;
|
|
if (now - this.telemetryAt > 250) {
|
|
const v = this.sim.data.qvel,
|
|
a = this.sim.rootDof;
|
|
this.onTelemetry({
|
|
fps: Math.round((this.frames * 1000) / (now - this.fpsAt)),
|
|
ctrlHz: this.ctrlHz,
|
|
speed: Math.hypot(v[a], v[a + 1]),
|
|
odo: this.odo,
|
|
peers: 0,
|
|
});
|
|
this.telemetryAt = now;
|
|
}
|
|
}
|
|
debug() {
|
|
const self = this;
|
|
return {
|
|
get model() {
|
|
return self.sim.model;
|
|
},
|
|
get data() {
|
|
return self.sim.data;
|
|
},
|
|
mujoco: this.sim.mujoco,
|
|
camera: this.camera,
|
|
controls: this.controls,
|
|
get chaseCam() {
|
|
return self.chaseCamera.enabled;
|
|
},
|
|
scene: this.scene,
|
|
world: this.world,
|
|
command: this.input.command,
|
|
lastAction: this.ctx.lastAction,
|
|
skills: this.skills,
|
|
buildObs: () => {
|
|
this.ctx.command.set(this.input.command);
|
|
return Object.values(this.obs.write())[0];
|
|
},
|
|
pause: () => this.pause(),
|
|
resume: () => this.resume(),
|
|
step: (n) => this.scheduler.step(n),
|
|
resetSim: () => this.reset(),
|
|
toggle: (id) => this.toggle(id),
|
|
get fallen() {
|
|
return self.fallen;
|
|
},
|
|
get actuationStats() {
|
|
const a = self.actuation;
|
|
return {
|
|
samples: a.samples ?? 0,
|
|
saturated: a.saturated ?? 0,
|
|
saturationRatio: a.samples ? a.saturated / a.samples : 0,
|
|
longestSaturatedSeconds:
|
|
(a.maxStreak ?? 0) * self.config.sim.timestep,
|
|
};
|
|
},
|
|
get running() {
|
|
return self.scheduler.running;
|
|
},
|
|
};
|
|
}
|
|
async dispose() {
|
|
if (this.disposed) return;
|
|
this.disposed = true;
|
|
this.active = false;
|
|
await this.scheduler.dispose();
|
|
this.input.dispose();
|
|
this.chaseCamera?.dispose();
|
|
this.controls?.dispose();
|
|
this.worldPresentation?.dispose();
|
|
this.rig.dispose();
|
|
await this.controller.dispose();
|
|
this.sim.dispose();
|
|
}
|
|
}
|