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(); } }