diff --git a/src/components/mujoco-framework-next/checkers/PositionDeltaChecker.js b/src/components/mujoco-framework-next/checkers/PositionDeltaChecker.js new file mode 100644 index 0000000..da06450 --- /dev/null +++ b/src/components/mujoco-framework-next/checkers/PositionDeltaChecker.js @@ -0,0 +1,263 @@ +/** + * PositionDeltaChecker + * + * Checks whether a body's position has changed by at least a given + * threshold along one or more axes relative to a captured reference + * position. Used by Hub tasks to detect coarse-grained motion such as + * "the lid was rotated past 30 degrees" or "the object was moved at + * least 2 cm to the left of where it started". + * + * Task config schema (matches Hub `checker_config` for `PositionDeltaChecker`): + * + * { + * "type": "PositionDeltaChecker", + * "sampleBodyName": "akita_black_bowl_1_main", + * "axes": ["x", "y"], // which axes to consider + * "minDeltaX": 0.018, // body must have moved +18mm on X + * "maxDeltaX": -0.018, // OR must have moved -18mm on X + * "minDeltaY": 0.018, // same for Y + * "maxDeltaY": -0.018, + * "captureRuntimeInitial": true, // baseline = first-seen body position + * // false (default) = use task's initial_state + * "initialPosition": [x, y, z] // optional explicit baseline; if absent + * // and !captureRuntimeInitial, the position + * // at construction time is used + * } + * + * The matching Python implementation is in + * AxisDataCleaning/util/validate_offline_trajectory.py::check_position_delta + * and MUST stay in sync with this class. + * + * Semantics: + * + * - If `captureRuntimeInitial` is true, the first observed body position + * (after the checker is created) is captured as the baseline. This is + * useful when the task's "initial position" in the MJCF does not match + * the position the body actually settles into after physics warmup. + * - If `captureRuntimeInitial` is false, the baseline is `initialPosition` + * if provided, otherwise the position at construction time. + * - The check passes if, for ANY of the configured axes, the current + * position is >= minDelta (positive direction) OR <= maxDelta (negative + * direction). An axis with neither min nor max bound is ignored. + * - The check does NOT require ALL axes to move; OR semantics by default. + * If you need AND semantics, wrap in a CompositeChecker. + */ + +export class PositionDeltaChecker { + constructor(mujoco, model, data, options = {}) { + this.mujoco = mujoco; + this.model = model; + this.data = data; + + this.bodyName = options.sample_body || options.sampleBodyName; + if (!this.bodyName) { + throw new Error( + '[PositionDeltaChecker] sample_body (or sampleBodyName) is required', + ); + } + + this.axes = this.parseAxes(options.axes || ['x', 'y']); + this.captureRuntimeInitial = Boolean( + options.capture_runtime_initial ?? options.captureRuntimeInitial ?? false, + ); + + // Per-axis bounds. Either or both may be set per axis; missing means + // "no constraint on that direction for that axis". + this.minDeltas = { + x: this._num(options.min_delta_x, options.minDeltaX), + y: this._num(options.min_delta_y, options.minDeltaY), + z: this._num(options.min_delta_z, options.minDeltaZ), + }; + this.maxDeltas = { + x: this._num(options.max_delta_x, options.maxDeltaX), + y: this._num(options.max_delta_y, options.maxDeltaY), + z: this._num(options.max_delta_z, options.maxDeltaZ), + }; + + this.initialPosition = null; + this._runtimeBaseline = null; + if (Array.isArray(options.initialPosition) && options.initialPosition.length === 3) { + this.initialPosition = [ + Number(options.initialPosition[0]), + Number(options.initialPosition[1]), + Number(options.initialPosition[2]), + ]; + } + + this.textDecoder = new TextDecoder('utf-8'); + this.namesBuffer = new Uint8Array(model.names); + this.bodyAddress = this.resolveBodyAddress(this.bodyName); + if (this.bodyAddress < 0) { + console.warn(`[PositionDeltaChecker] body "${this.bodyName}" not found`); + } + + console.log( + `[PositionDeltaChecker] Initialized: body="${this.bodyName}" ` + + `axes=[${this.axes.join(',')}] captureRuntimeInitial=${this.captureRuntimeInitial} ` + + `bounds=${JSON.stringify(this._boundsSummary())}`, + ); + } + + _num(...candidates) { + for (const v of candidates) { + if (v === undefined || v === null) continue; + const n = Number(v); + if (Number.isFinite(n)) return n; + } + return null; + } + + _boundsSummary() { + const out = {}; + for (const ax of this.axes) { + const lo = this.maxDeltas[ax]; + const hi = this.minDeltas[ax]; + if (lo !== null || hi !== null) { + out[ax] = {}; + if (hi !== null) out[ax].min = hi; + if (lo !== null) out[ax].max = lo; + } + } + return out; + } + + parseAxes(axes) { + if (!Array.isArray(axes)) { + throw new Error( + `[PositionDeltaChecker] axes must be an array, got ${typeof axes}`, + ); + } + const out = []; + for (const ax of axes) { + const a = String(ax).toLowerCase(); + if (!['x', 'y', 'z'].includes(a)) { + throw new Error(`[PositionDeltaChecker] unknown axis "${ax}"`); + } + if (!out.includes(a)) out.push(a); + } + if (out.length === 0) { + throw new Error('[PositionDeltaChecker] axes must contain at least one of x, y, z'); + } + return out; + } + + resolveBodyAddress(bodyName) { + for (let b = 0; b < this.model.nbody; b++) { + const nameStart = this.model.name_bodyadr[b]; + let nameEnd = nameStart; + while ( + nameEnd < this.namesBuffer.length && + this.namesBuffer[nameEnd] !== 0 + ) { + nameEnd++; + } + const name = this.textDecoder.decode( + this.namesBuffer.subarray(nameStart, nameEnd), + ); + if (name === bodyName) return b; + } + return -1; + } + + getCurrentPosition() { + if (this.bodyAddress < 0 || this.bodyAddress >= this.model.nbody) return null; + const base = 3 * this.bodyAddress; + return [ + this.data.xpos[base + 0], + this.data.xpos[base + 1], + this.data.xpos[base + 2], + ]; + } + + /** + * Returns the current baseline: runtime-captured if available, else the + * initialPosition given at construction, else the current position + * (which means delta = 0 and the check will pass if any bound is 0). + */ + baseline() { + if (this._runtimeBaseline) return this._runtimeBaseline; + if (this.initialPosition) return this.initialPosition; + return this.getCurrentPosition() || [0, 0, 0]; + } + + /** + * Whether the body has moved past one of the configured bounds. + */ + check() { + const cur = this.getCurrentPosition(); + if (!cur) return false; + let base = this._runtimeBaseline; + if (!base && this.captureRuntimeInitial) { + base = [cur[0], cur[1], cur[2]]; + this._runtimeBaseline = base; + } else if (!base) { + base = this.baseline(); + } + for (let i = 0; i < this.axes.length; i++) { + const ax = this.axes[i]; + const delta = cur[i] - base[i]; + const hi = this.minDeltas[ax]; + const lo = this.maxDeltas[ax]; + if (hi !== null && delta >= hi) return true; + if (lo !== null && delta <= lo) return true; + } + return false; + } + + /** + * Current per-axis delta, useful for the debug UI. + */ + getStatus() { + const cur = this.getCurrentPosition(); + const base = this.baseline(); + const deltas = { x: 0, y: 0, z: 0 }; + for (let i = 0; i < 3; i++) { + if (cur && base) deltas[['x','y','z'][i]] = cur[i] - base[i]; + } + return { + success: this.check(), + body: this.bodyName, + deltas, + baseline: base, + current: cur, + bounds: this._boundsSummary(), + message: this.check() + ? `Movement detected for ${this.bodyName}` + : `${this.bodyName} has not moved past any configured bound`, + }; + } + + getProgress() { + const cur = this.getCurrentPosition(); + const base = this.baseline(); + const goals = []; + for (const ax of this.axes) { + const idx = { x: 0, y: 1, z: 2 }[ax]; + const delta = cur && base ? cur[idx] - base[idx] : 0; + const hi = this.minDeltas[ax]; + const lo = this.maxDeltas[ax]; + const passed = (hi !== null && delta >= hi) || (lo !== null && delta <= lo); + goals.push({ + key: `${this.bodyName}.${ax}`, + label: `${this.bodyName} Δ${ax}`, + ok: passed, + current: delta, + threshold: hi !== null ? hi : lo ?? 0, + unit: 'm', + }); + } + return { type: 'PositionDeltaChecker', goals }; + } + + reset() { + this._runtimeBaseline = null; + } + + updateModelData(model, data) { + this.model = model; + this.data = data; + this.namesBuffer = new Uint8Array(model.names); + this.bodyAddress = this.resolveBodyAddress(this.bodyName); + this._runtimeBaseline = null; + } +} diff --git a/src/components/mujoco-framework-next/checkers/index.js b/src/components/mujoco-framework-next/checkers/index.js index eb8a153..4807716 100644 --- a/src/components/mujoco-framework-next/checkers/index.js +++ b/src/components/mujoco-framework-next/checkers/index.js @@ -7,6 +7,7 @@ import { GripperOpenChecker } from './GripperOpenChecker.js'; import { BoxJointPositionChecker } from './BoxJointPositionChecker.js'; import { CompositeChecker } from './CompositeChecker.js'; +import { PositionDeltaChecker } from './PositionDeltaChecker.js'; /** * Registry mapping checker type names to their classes */ @@ -14,10 +15,12 @@ export const CHECKER_REGISTRY = { GripperOpenChecker, BoxJointPositionChecker, CompositeChecker, + PositionDeltaChecker, // Aliases for task config compatibility joint_position: BoxJointPositionChecker, composite: CompositeChecker, gripper_open: GripperOpenChecker, + position_delta: PositionDeltaChecker, }; /**