diff --git a/src/app.css b/src/app.css index 3d205fa1e..f87df212a 100644 --- a/src/app.css +++ b/src/app.css @@ -67,3 +67,8 @@ select { .motion-tools-table tbody th { @apply border-light font-roboto-mono text-default h-[40px] gap-2 border px-1.5 text-center text-xs font-normal; } + +/* Push toasts above the motion plan scrubber bar (fixed bottom-4, ~44px tall). */ +body.has-scrubber [aria-label='Toasts'] { + padding-bottom: 5rem; +} diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte new file mode 100644 index 000000000..2baf7c65f --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte @@ -0,0 +1,27 @@ + + + + diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte new file mode 100644 index 000000000..fcd60d19a --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte @@ -0,0 +1,176 @@ + + + + {#if ctx.totalSteps > 0} +
+ + + + + + + seek(Number((e.currentTarget as HTMLInputElement).value))} + /> + + + + + + {ctx.currentStep + 1} / {ctx.totalSteps} + + +
+ {/if} + + + diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte new file mode 100644 index 000000000..c78c8ac86 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte @@ -0,0 +1,155 @@ + + + +
+ (isOpen = !isOpen)} + /> +
+ + + + +
+ {#if ctx.plans.length === 0} +
+ Use the button below to upload a plan JSON file +
+ {/if} + + {#each ctx.plans as plan, i (plan.name)} + {@const isActive = ctx.activePlanIndex === i} +
(isActive ? ctx.clearActivePlan() : ctx.selectPlan(i))} + onkeydown={(e) => + e.key === 'Enter' && (isActive ? ctx.clearActivePlan() : ctx.selectPlan(i))} + > + + {#if isActive} + + {:else} + + {/if} + + {plan.name} + + +
+ + {#if plan.status === 'error'} +
{plan.error}
+ {/if} + {#if plan.status === 'no-trajectory'} +
No trajectory — nothing to replay
+ {/if} + {/each} + +
+ {#if extraSource} + {@render extraSource(ctx.addPlan)} + {/if} + + +
+
+ + diff --git a/src/lib/plugins/MotionPlanReplayer/__tests__/build-frame-descriptors.spec.ts b/src/lib/plugins/MotionPlanReplayer/__tests__/build-frame-descriptors.spec.ts new file mode 100644 index 000000000..25e50b11a --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/__tests__/build-frame-descriptors.spec.ts @@ -0,0 +1,500 @@ +import { describe, expect, it } from 'vitest' + +import type { ParsedPlan } from '../parse-plan' + +import { buildFrameDescriptors } from '../build-frame-descriptors' + +const plan = (frames: ParsedPlan['frames'], parents: ParsedPlan['parents']): ParsedPlan => ({ + frames, + parents, + trajectory: [], + goals: [], +}) + +describe('buildFrameDescriptors', () => { + it('produces a static descriptor for a named static frame', () => { + const p = plan( + { + 'arm:link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 267 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + }, + { 'arm:link': 'world' } + ) + const descriptors = buildFrameDescriptors(p) + expect(descriptors).toHaveLength(1) + const d = descriptors[0]! + expect(d.kind).toBe('static') + if (d.kind === 'static') { + expect(d.name).toBe('arm:link') + expect(d.parent).toBe('world') + expect(d.localPose.z).toBeCloseTo(267) + expect(d.geometry).toBeNull() + } + }) + + it('joint frames emit joint descriptors; link frames emit static descriptors parented to their joint', () => { + // arm chain: arm:base (static) → arm:waist (joint, Z) → arm:base_top (static link) + // arm:base_top → arm:shoulder (joint, Y) → arm:upper_arm (static link) + const p = plan( + { + arm: { + frame_type: 'model', + frame: { name: 'arm', model: { joints: [{ id: 'waist' }, { id: 'shoulder' }] } }, + }, + 'arm:waist': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'rotational', + frame: { axis: { X: 0, Y: 0, Z: 1 } }, + }, + }, + }, + 'arm:shoulder': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'rotational', + frame: { axis: { X: 0, Y: 1, Z: 0 } }, + }, + }, + }, + 'arm:base_top': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 267 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + 'arm:upper_arm': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 53.5, Y: 0, Z: 284.5 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + }, + { + 'arm:waist': 'arm:base', + 'arm:shoulder': 'arm:base_top', + 'arm:base_top': 'arm:waist', + 'arm:upper_arm': 'arm:shoulder', + } + ) + const descriptors = buildFrameDescriptors(p) + + // Joint frames appear as joint descriptors + const waist = descriptors.find((d) => d.name === 'arm:waist')! + expect(waist).toBeDefined() + expect(waist.kind).toBe('joint') + if (waist.kind === 'joint') { + expect(waist.parent).toBe('arm:base') + expect(waist.componentName).toBe('arm') + expect(waist.jointIndex).toBe(0) + expect(waist.axis).toEqual({ X: 0, Y: 0, Z: 1 }) + } + + const shoulder = descriptors.find((d) => d.name === 'arm:shoulder')! + expect(shoulder).toBeDefined() + expect(shoulder.kind).toBe('joint') + if (shoulder.kind === 'joint') { + expect(shoulder.parent).toBe('arm:base_top') + expect(shoulder.componentName).toBe('arm') + expect(shoulder.jointIndex).toBe(1) + expect(shoulder.axis).toEqual({ X: 0, Y: 1, Z: 0 }) + } + + // Link frames appear as static descriptors parented directly to their joint + const baseTop = descriptors.find((d) => d.name === 'arm:base_top')! + expect(baseTop).toBeDefined() + expect(baseTop.kind).toBe('static') + if (baseTop.kind === 'static') { + expect(baseTop.parent).toBe('arm:waist') // parented to the joint, not the joint's parent + expect(baseTop.localPose.z).toBeCloseTo(267) + } + + const upperArm = descriptors.find((d) => d.name === 'arm:upper_arm')! + expect(upperArm).toBeDefined() + expect(upperArm.kind).toBe('static') + if (upperArm.kind === 'static') { + expect(upperArm.parent).toBe('arm:shoulder') + expect(upperArm.localPose.x).toBeCloseTo(53.5) + expect(upperArm.localPose.z).toBeCloseTo(284.5) + } + }) + + it('prefers primary_output_frame over last-joint static child when both exist', () => { + const p = plan( + { + arm: { + frame_type: 'model', + frame: { + name: 'arm', + model: { + joints: [{ id: 'waist' }, { id: 'gripper_rot' }], + primary_output_frame: 'gripper_mount', + links: [{ id: 'extra_link' }, { id: 'gripper_mount' }], + }, + }, + }, + 'arm:gripper_rot': { + frame_type: 'named', + frame: { + inner_frame: { frame_type: 'rotational', frame: { axis: { X: 0, Y: 0, Z: 1 } } }, + }, + }, + 'arm:extra_link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + 'arm:gripper_mount': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + camera_origin: { + frame_type: 'static', + frame: { + translation: { X: 10, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + { + 'arm:extra_link': 'arm:gripper_rot', + 'arm:gripper_mount': 'arm:extra_link', + camera_origin: 'arm', + } + ) + const camera = buildFrameDescriptors(p).find((d) => d.name === 'camera_origin')! + expect(camera.parent).toBe('arm:gripper_mount') + }) + + it('remaps frames parented to a model frame to the terminal static frame', () => { + // camera is parented to the model frame 'arm' (never an ECS entity). + // It should be reparented to 'arm:gripper_mount' (static frame after the last joint). + const p = plan( + { + arm: { + frame_type: 'model', + frame: { name: 'arm', model: { joints: [{ id: 'waist' }, { id: 'gripper_rot' }] } }, + }, + 'arm:gripper_rot': { + frame_type: 'named', + frame: { + inner_frame: { frame_type: 'rotational', frame: { axis: { X: 0, Y: 0, Z: 1 } } }, + }, + }, + 'arm:gripper_mount': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + }, + camera_origin: { + frame_type: 'static', + frame: { + translation: { X: 10, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + }, + }, + }, + { + 'arm:gripper_mount': 'arm:gripper_rot', + camera_origin: 'arm', // model frame — should be remapped + } + ) + const descriptors = buildFrameDescriptors(p) + const camera = descriptors.find((d) => d.name === 'camera_origin')! + expect(camera).toBeDefined() + expect(camera.parent).toBe('arm:gripper_mount') // remapped from 'arm' + }) + + it('skips model frames', () => { + const p = plan( + { arm: { frame_type: 'model', frame: { name: 'arm', model: { joints: [] } } } }, + {} + ) + expect(buildFrameDescriptors(p)).toHaveLength(0) + }) + + it('parses capsule geometry from a static frame', () => { + const p = plan( + { + 'arm:link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + geometry: { + type: 'capsule', + r: 50, + l: 320, + translation: { X: 0, Y: 0, Z: 160 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + Label: 'link', + }, + }, + }, + }, + }, + }, + { 'arm:link': 'world' } + ) + const descriptors = buildFrameDescriptors(p) + const d = descriptors[0]! + if (d.kind === 'static') { + expect(d.geometry).not.toBeNull() + expect(d.geometry!.geometryType.case).toBe('capsule') + if (d.geometry!.geometryType.case === 'capsule') { + expect(d.geometry!.geometryType.value.radiusMm).toBe(50) + expect(d.geometry!.geometryType.value.lengthMm).toBe(320) + } + expect(d.geometry!.center!.z).toBeCloseTo(160) + } + }) + + it('subtracts frame translation from parent-frame geometry center (xArm6 base_top)', () => { + const p = plan( + { + 'arm:base_top': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 267 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + geometry: { + type: 'capsule', + r: 50, + l: 320, + translation: { X: 0, Y: 0, Z: 160 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + Label: 'base_top', + }, + }, + }, + }, + }, + }, + { 'arm:base_top': 'arm:waist' } + ) + const d = buildFrameDescriptors(p)[0]! + if (d.kind === 'static') { + expect(d.localPose.z).toBeCloseTo(267) + // geo z=160 and frame z=267 both in parent (waist) → local center 107mm below frame + expect(d.geometry!.center!.z).toBeCloseTo(-107) + } + }) + + it('rotates parent-frame geometry offset into link-local coords (xArm850 link_2)', () => { + const p = plan( + { + 'left-arm:link_2': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 390, Y: 0, Z: 0 }, + orientation: { + type: 'quaternion', + value: { + W: -2.5973434669646147e-6, + X: -0.7071054825064661, + Y: 0.7071080798547033, + Z: 2.597353007557415e-6, + }, + }, + geometry: { + type: 'box', + x: 500.07, + y: 119.987, + z: 161.805, + translation: { X: 189.857, Y: 0, Z: 31.0907 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + Label: 'link_2', + }, + }, + }, + }, + }, + }, + { 'left-arm:link_2': 'left-arm:joint_2' } + ) + const d = buildFrameDescriptors(p)[0]! + if (d.kind === 'static') { + expect(d.localPose.x).toBeCloseTo(390) + // R_frame⁻¹ * (geo − frame): not a naive component-wise subtract + expect(d.geometry!.center!.x).toBeCloseTo(0, 1) + expect(d.geometry!.center!.y).toBeCloseTo(200.143, 1) + expect(d.geometry!.center!.z).toBeCloseTo(-31.0907, 1) + } + }) + + it('parses euler_angles orientation on a static frame (not identity)', () => { + const p = plan( + { + 'arm:link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + // 90 degree yaw about Z + orientation: { + type: 'euler_angles', + value: { roll: 0, pitch: 0, yaw: Math.PI / 2 }, + }, + }, + }, + }, + }, + }, + { 'arm:link': 'world' } + ) + const d = buildFrameDescriptors(p)[0]! + if (d.kind === 'static') { + // Identity would be oX:0 oY:0 oZ:1 theta:0 — assert it's not identity and + // specifically a 90 degree rotation about Z. + expect(d.localPose.theta).toBeCloseTo(90) + expect(d.localPose.oZ).toBeCloseTo(1) + } + }) + + it('parses euler_angles on link frame and rotates geometry orient into local coords', () => { + // link_1 from salad: 90° fixed rotation with geometry offset in parent frame + const p = plan( + { + 'left-arm:link_1': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { + type: 'euler_angles', + value: { roll: 1.5708, pitch: -1.5708, yaw: 0 }, + }, + geometry: { + type: 'box', + x: 120.619, + y: 185.133, + z: 238.5, + translation: { X: 0, Y: 32.5667, Z: -59.25 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + Label: 'link_1', + }, + }, + }, + }, + }, + }, + { 'left-arm:link_1': 'left-arm:joint_1' } + ) + const d = buildFrameDescriptors(p)[0]! + if (d.kind === 'static') { + expect(d.localPose.theta).not.toBeCloseTo(0) + // geometry center offset is rotated into link-local, not left in parent Y/Z + expect(d.geometry!.center!.x).toBeCloseTo(-59.25, 1) + expect(d.geometry!.center!.y).toBeCloseTo(0, 1) + expect(d.geometry!.center!.z).toBeCloseTo(-32.5667, 1) + } + }) + + it('parses ov_degrees orientation on a static frame', () => { + const p = plan( + { + 'arm:link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'ov_degrees', value: { x: 0, y: 0, z: 1, th: 90 } }, + }, + }, + }, + }, + }, + { 'arm:link': 'world' } + ) + const d = buildFrameDescriptors(p)[0]! + if (d.kind === 'static') { + expect(d.localPose.theta).toBeCloseTo(90) + expect(d.localPose.oZ).toBeCloseTo(1) + } + }) + + it('returns null geometry for unrecognized geometry type', () => { + const p = plan( + { + 'arm:link': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 0 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + geometry: { type: '', r: 1, l: 0 }, + }, + }, + }, + }, + }, + { 'arm:link': 'world' } + ) + const descriptors = buildFrameDescriptors(p) + const d = descriptors[0]! + if (d.kind === 'static') expect(d.geometry).toBeNull() + }) +}) diff --git a/src/lib/plugins/MotionPlanReplayer/__tests__/parse-plan.spec.ts b/src/lib/plugins/MotionPlanReplayer/__tests__/parse-plan.spec.ts new file mode 100644 index 000000000..69f5b4341 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/__tests__/parse-plan.spec.ts @@ -0,0 +1,54 @@ +import { describe, expect, it } from 'vitest' + +import { parsePlan, PlanParseError } from '../parse-plan' + +const MINIMAL_FRAME_SYSTEM = { + frames: { + 'left-arm': { + frame_type: 'model', + frame: { name: 'left-arm', model: { joints: [{ id: 'waist' }] } }, + }, + 'left-arm:waist': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'rotational', + frame: { axis: { X: 0, Y: 0, Z: 1 } }, + }, + }, + }, + }, + parents: { 'left-arm:waist': 'world' }, +} + +const REQUEST_OBJ = { frame_system: MINIMAL_FRAME_SYSTEM, goals: [], start_state: {} } +const RESULT_OBJ = { trajectory: [{ 'left-arm': [0.5] }] } + +describe('parsePlan', () => { + it('parses a single-object plan (no trajectory)', () => { + const plan = parsePlan(JSON.stringify(REQUEST_OBJ)) + expect(Object.keys(plan.frames)).toContain('left-arm:waist') + expect(plan.trajectory).toHaveLength(0) + }) + + it('parses two concatenated JSON objects', () => { + const content = JSON.stringify(REQUEST_OBJ) + JSON.stringify(RESULT_OBJ) + const plan = parsePlan(content) + expect(plan.trajectory).toHaveLength(1) + expect(plan.trajectory[0]!['left-arm']).toEqual([0.5]) + }) + + it('throws PlanParseError when frame_system is absent', () => { + expect(() => parsePlan(JSON.stringify({ path: [], trajectory: [] }))).toThrow(PlanParseError) + }) + + it('throws PlanParseError on invalid JSON', () => { + expect(() => parsePlan('{ not valid json')).toThrow(PlanParseError) + }) + + it('throws PlanParseError when a chunk has wrong types', () => { + expect(() => + parsePlan(JSON.stringify({ frame_system: MINIMAL_FRAME_SYSTEM, trajectory: 'bad' })) + ).toThrow(PlanParseError) + }) +}) diff --git a/src/lib/plugins/MotionPlanReplayer/__tests__/plan-to-snapshots.spec.ts b/src/lib/plugins/MotionPlanReplayer/__tests__/plan-to-snapshots.spec.ts new file mode 100644 index 000000000..195085b06 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/__tests__/plan-to-snapshots.spec.ts @@ -0,0 +1,123 @@ +import { describe, expect, it } from 'vitest' + +import { parsePlan } from '../parse-plan' +import { parsedPlanToSnapshots } from '../plan-to-snapshots' + +// arm chain: waist (joint, Z-axis) → base (link, z=100mm, capsule geometry) +const REQUEST = { + frame_system: { + frames: { + arm: { + frame_type: 'model', + frame: { name: 'arm', model: { joints: [{ id: 'waist' }] } }, + }, + 'arm:waist': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'rotational', + frame: { axis: { X: 0, Y: 0, Z: 1 } }, + }, + }, + }, + 'arm:base': { + frame_type: 'named', + frame: { + inner_frame: { + frame_type: 'static', + frame: { + translation: { X: 0, Y: 0, Z: 100 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + geometry: { + type: 'capsule', + r: 30, + l: 100, + translation: { X: 0, Y: 0, Z: 50 }, + orientation: { type: 'quaternion', value: { W: 1, X: 0, Y: 0, Z: 0 } }, + Label: 'base', + }, + }, + }, + }, + }, + }, + parents: { 'arm:waist': 'world', 'arm:base': 'arm:waist' }, + }, + goals: [], + start_state: {}, +} + +const RESULT = { trajectory: [{ arm: [0] }, { arm: [1.5708] }] } +const CONTENT = JSON.stringify(REQUEST) + JSON.stringify(RESULT) + +const snapshotsFromContent = (content: string) => parsedPlanToSnapshots(parsePlan(content)) + +describe('parsedPlanToSnapshots', () => { + it('returns one Snapshot per trajectory step', () => { + const snapshots = snapshotsFromContent(CONTENT) + expect(snapshots).toHaveLength(2) + }) + + it('joint and link both appear as transforms', () => { + const snapshots = snapshotsFromContent(CONTENT) + // waist (joint) and base (link) each emit a transform + expect(snapshots[0]!.transforms).toHaveLength(2) + const names = snapshots[0]!.transforms.map((t) => t.referenceFrame) + expect(names).toContain('arm:waist') + expect(names).toContain('arm:base') + }) + + it('joint pose changes per step; link local pose stays constant', () => { + const snapshots = snapshotsFromContent(CONTENT) + const waist0 = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:waist')! + const waist1 = snapshots[1]!.transforms.find((t) => t.referenceFrame === 'arm:waist')! + const base0 = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + const base1 = snapshots[1]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + + // joint theta changes: step 0 → 0°, step 1 → 90° + expect(waist0.poseInObserverFrame!.pose!.theta).toBeCloseTo(0, 1) + expect(waist1.poseInObserverFrame!.pose!.theta).toBeCloseTo(90, 1) + + // link local pose is constant — WorldMatrix handles world-space composition + expect(base0.poseInObserverFrame!.pose!.z).toBeCloseTo(100, 1) + expect(base1.poseInObserverFrame!.pose!.z).toBeCloseTo(100, 1) + expect(base0.poseInObserverFrame!.pose!.theta).toBeCloseTo(0, 1) + expect(base1.poseInObserverFrame!.pose!.theta).toBeCloseTo(0, 1) + }) + + it('link is parented to its joint', () => { + const snapshots = snapshotsFromContent(CONTENT) + const base = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + expect(base.poseInObserverFrame!.referenceFrame).toBe('arm:waist') + }) + + it('joint is parented to its frame_system parent', () => { + const snapshots = snapshotsFromContent(CONTENT) + const waist = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:waist')! + expect(waist.poseInObserverFrame!.referenceFrame).toBe('world') + }) + + it('joint has no physicalObject', () => { + const snapshots = snapshotsFromContent(CONTENT) + const waist = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:waist')! + expect(waist.physicalObject).toBeUndefined() + }) + + it('link has same UUID across steps', () => { + const snapshots = snapshotsFromContent(CONTENT) + const base0 = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + const base1 = snapshots[1]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + expect(base0.uuid).toStrictEqual(base1.uuid) + }) + + it('link has physicalObject when geometry is present', () => { + const snapshots = snapshotsFromContent(CONTENT) + const base = snapshots[0]!.transforms.find((t) => t.referenceFrame === 'arm:base')! + expect(base.physicalObject).toBeDefined() + expect(base.physicalObject!.geometryType.case).toBe('capsule') + }) + + it('returns empty array for plan with no trajectory', () => { + expect(parsedPlanToSnapshots(parsePlan(JSON.stringify(REQUEST)))).toHaveLength(0) + }) +}) diff --git a/src/lib/plugins/MotionPlanReplayer/__tests__/relations.spec.ts b/src/lib/plugins/MotionPlanReplayer/__tests__/relations.spec.ts new file mode 100644 index 000000000..0bb52ec11 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/__tests__/relations.spec.ts @@ -0,0 +1,38 @@ +import { createWorld, type World } from 'koota' +import { afterEach, describe, expect, it } from 'vitest' + +import { relations, traits } from '$lib/ecs' + +import { PartOfPlan } from '../relations' + +describe('PartOfPlan', () => { + let world: World + afterEach(() => world?.destroy()) + + it('destroys every member when the plan root is destroyed', () => { + world = createWorld() + const planRoot = world.spawn(traits.Name('plan')) + const member = world.spawn(PartOfPlan(planRoot)) + const nestedParent = world.spawn(PartOfPlan(planRoot)) + const nestedChild = world.spawn(relations.ChildOf(nestedParent), PartOfPlan(planRoot)) + + planRoot.destroy() + + expect(planRoot.isAlive()).toBe(false) + expect(member.isAlive()).toBe(false) + expect(nestedParent.isAlive()).toBe(false) + expect(nestedChild.isAlive()).toBe(false) + }) + + it('leaves entities outside the plan alone', () => { + world = createWorld() + const planRoot = world.spawn(traits.Name('plan')) + const member = world.spawn(PartOfPlan(planRoot)) + const unrelated = world.spawn(traits.Name('unrelated')) + + planRoot.destroy() + + expect(member.isAlive()).toBe(false) + expect(unrelated.isAlive()).toBe(true) + }) +}) diff --git a/src/lib/plugins/MotionPlanReplayer/build-frame-descriptors.ts b/src/lib/plugins/MotionPlanReplayer/build-frame-descriptors.ts new file mode 100644 index 000000000..764bcdf1b --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/build-frame-descriptors.ts @@ -0,0 +1,334 @@ +import { Euler, MathUtils, Quaternion, Vector3 } from 'three' + +import { + Capsule, + Geometry, + Pose, + RectangularPrism, + Sphere, + Vector3 as ViamVector3, +} from '$lib/buf/common/v1/common_pb' +import { OrientationVector } from '$lib/three/OrientationVector' +import { quaternionToPose } from '$lib/transform' + +import type { ParsedPlan } from './parse-plan' + +import { planUuid } from './plan-uuid' + +/** A rigid link with a fixed local transform — no joint involved. */ +export interface StaticFrameDescriptor { + kind: 'static' + name: string + parent: string + localPose: Pose + geometry: Geometry | null + uuid: Uint8Array +} + +/** A revolute joint frame. Its pose at each step is a pure rotation around `axis` by the trajectory angle. */ +export interface JointFrameDescriptor { + kind: 'joint' + name: string + parent: string + axis: { X: number; Y: number; Z: number } + componentName: string + jointIndex: number + uuid: Uint8Array +} + +export type FrameDescriptor = StaticFrameDescriptor | JointFrameDescriptor + +// Shared scratch objects — safe in single-threaded JS +const tmpQ = new Quaternion() +const tmpQFrame = new Quaternion() +const tmpQGeo = new Quaternion() +const tmpQInv = new Quaternion() +const tmpQLocal = new Quaternion() +const tmpE = new Euler() +const tmpV = new Vector3() +const tmpOv = new OrientationVector() + +type QuatJson = { W: number; X: number; Y: number; Z: number } +type EulerJson = { roll: number; pitch: number; yaw: number } +type OvJson = { x: number; y: number; z: number; th: number } +type OrientJson = + | { type: 'quaternion'; value: QuatJson } + | { type: 'euler_angles'; value: EulerJson } + | { type: 'ov_degrees'; value: OvJson } + | { type: 'ov_radians'; value: OvJson } + | undefined +type Vec3Json = { X: number; Y: number; Z: number } | undefined + +const hasOrientJson = (orientation: OrientJson): orientation is NonNullable => + orientation?.type === 'quaternion' || + orientation?.type === 'euler_angles' || + orientation?.type === 'ov_degrees' || + orientation?.type === 'ov_radians' + +/** Write orientation JSON into `out`. RDK euler_angles use Tait–Bryan Z-Y′-X″ (ZYX). */ +const quatFromJson = (orientation: OrientJson, out: Quaternion): Quaternion => { + if (orientation?.type === 'quaternion' && orientation.value) { + const v = orientation.value + // Three.js Quaternion order: (x, y, z, w) — W is LAST + return out.set(v.X, v.Y, v.Z, v.W) + } + if (orientation?.type === 'euler_angles' && orientation.value) { + const v = orientation.value + tmpE.set(v.roll, v.pitch, v.yaw, 'ZYX') + return out.setFromEuler(tmpE) + } + if (orientation?.type === 'ov_radians' && orientation.value) { + const v = orientation.value + return tmpOv.set(v.x, v.y, v.z, v.th).toQuaternion(out) + } + if (orientation?.type === 'ov_degrees' && orientation.value) { + const v = orientation.value + const th = MathUtils.degToRad(v.th ?? 0) + return tmpOv.set(v.x, v.y, v.z, th).toQuaternion(out) + } + return out.set(0, 0, 0, 1) +} + +const poseFromFrame = (translation: Vec3Json, orientation: OrientJson): Pose => { + const pose = new Pose({ + x: translation?.X ?? 0, + y: translation?.Y ?? 0, + z: translation?.Z ?? 0, + }) + quaternionToPose(quatFromJson(orientation, tmpQ), pose) + return pose +} + +type FramePoseJson = { translation?: Vec3Json; orientation?: OrientJson } + +/** + * Convert a geometry center pose from parent-frame coordinates into the link + * frame's local coordinates. + * + * Both the link frame and geometry center are expressed in the same parent + * frame. The link frame pose places the frame origin; the geometry describes + * the physical link body (length/shape) with its center offset in parent space. + */ +const geometryCenterInFrame = ( + geoTrans: Vec3Json, + geoOrient: OrientJson, + framePose: FramePoseJson +): Pose => { + quatFromJson(framePose.orientation, tmpQFrame) + tmpQInv.copy(tmpQFrame).invert() + + tmpV + .set( + (geoTrans?.X ?? 0) - (framePose.translation?.X ?? 0), + (geoTrans?.Y ?? 0) - (framePose.translation?.Y ?? 0), + (geoTrans?.Z ?? 0) - (framePose.translation?.Z ?? 0) + ) + .applyQuaternion(tmpQInv) + + const center = new Pose({ x: tmpV.x, y: tmpV.y, z: tmpV.z }) + + if (hasOrientJson(geoOrient)) { + quatFromJson(geoOrient, tmpQGeo) + tmpQLocal.copy(tmpQInv).multiply(tmpQGeo) + quaternionToPose(tmpQLocal, center) + } + + return center +} + +/** + * Parse a geometry from raw JSON, returning a proto Geometry. + * + * For named arm link frames, pass `framePose` so the geometry center (which is + * in parent-frame coordinates alongside the link frame) is converted to local + * coordinates via R_frame⁻¹. + * + * For tail_geometry_static / static frames, omit `framePose` — geometry is + * already in local coordinates. + */ +const parseGeometry = (geom: unknown, framePose?: FramePoseJson): Geometry | null => { + if (!geom || typeof geom !== 'object') return null + const g = geom as Record + const type = g.type as string + if (type !== 'box' && type !== 'sphere' && type !== 'capsule') return null + + const trans = g.translation as Vec3Json + const orient = g.orientation as OrientJson + const center = framePose + ? geometryCenterInFrame(trans, orient, framePose) + : (() => { + const local = new Pose({ + x: trans?.X ?? 0, + y: trans?.Y ?? 0, + z: trans?.Z ?? 0, + }) + if (hasOrientJson(orient)) { + quaternionToPose(quatFromJson(orient, tmpQ), local) + } + return local + })() + + const label = (g.Label ?? g.label ?? '') as string + + if (type === 'sphere') { + return new Geometry({ + center, + geometryType: { case: 'sphere', value: new Sphere({ radiusMm: (g.r as number) ?? 0 }) }, + label, + }) + } + if (type === 'capsule') { + return new Geometry({ + center, + geometryType: { + case: 'capsule', + value: new Capsule({ radiusMm: (g.r as number) ?? 0, lengthMm: (g.l as number) ?? 0 }), + }, + label, + }) + } + return new Geometry({ + center, + geometryType: { + case: 'box', + value: new RectangularPrism({ + dimsMm: new ViamVector3({ + x: (g.x as number) ?? 0, + y: (g.y as number) ?? 0, + z: (g.z as number) ?? 0, + }), + }), + }, + label, + }) +} + +export const buildFrameDescriptors = (plan: ParsedPlan): FrameDescriptor[] => { + const { frames, parents } = plan + + // Pass 1: map component name → ordered joint frame names (from "model" frames). + // This gives us the index each joint occupies in the trajectory array. + const jointMap = new Map() + for (const [frameName, entry] of Object.entries(frames)) { + if (entry.frame_type !== 'model') continue + const model = (entry.frame as Record).model as + | Record + | undefined + const joints = model?.joints as Array<{ id: string }> | undefined + if (!joints) continue + jointMap.set( + frameName, + joints.map((j) => `${frameName}:${j.id}`) + ) + } + + // Pass 1b: map model frame name → its end-effector static frame name. + // External components (cameras, remote grippers) have parent = model frame name in the + // JSON, but the model frame itself is never an ECS entity. Reparent them to the terminal + // static frame so they appear at the arm's end-effector instead of floating at origin. + // + // Prefer primary_output_frame from model metadata; fall back to the last entry in + // model.links (always the end-effector in Viam's kinematic format). If neither is + // present, use the static frame parented to the last joint. + const modelTerminalMap = new Map() + for (const [frameName, entry] of Object.entries(frames)) { + if (entry.frame_type !== 'model') continue + const model = (entry.frame as Record).model as + | Record + | undefined + const primaryOutput = model?.primary_output_frame as string | undefined + const links = model?.links as Array<{ id: string }> | undefined + const endEffectorId = primaryOutput ?? links?.at(-1)?.id + if (endEffectorId) { + modelTerminalMap.set(frameName, `${frameName}:${endEffectorId}`) + continue + } + const jointNames = jointMap.get(frameName) + const lastJoint = jointNames?.at(-1) + if (!lastJoint) continue + for (const [childFrame, parentName] of Object.entries(parents)) { + if (parentName === lastJoint) { + modelTerminalMap.set(frameName, childFrame) + break + } + } + } + + // Pass 2: emit one descriptor per frame. Joint frames become JointFrameDescriptors + // (their pose is computed from the trajectory at each step). All other frames + // become StaticFrameDescriptors with a fixed local pose. + const descriptors: FrameDescriptor[] = [] + + for (const [frameName, entry] of Object.entries(frames)) { + const rawParent = parents[frameName] ?? 'world' + const parent = modelTerminalMap.get(rawParent) ?? rawParent + + switch (entry.frame_type) { + case 'model': { + continue + } + + case 'named': { + const outer = entry.frame as Record + const inner = outer.inner_frame as Record + + if (inner.frame_type === 'rotational') { + const innerData = inner.frame as Record + + let componentName = '' + let jointIndex = -1 + for (const [comp, names] of jointMap) { + const idx = names.indexOf(frameName) + if (idx !== -1) { + componentName = comp + jointIndex = idx + break + } + } + if (!componentName) continue + + descriptors.push({ + kind: 'joint', + name: frameName, + parent, + axis: innerData.axis as { X: number; Y: number; Z: number }, + componentName, + jointIndex, + uuid: planUuid(), + }) + } else if (inner.frame_type === 'static') { + const innerData = inner.frame as Record + const framePose: FramePoseJson = { + translation: innerData.translation as Vec3Json, + orientation: innerData.orientation as OrientJson, + } + descriptors.push({ + kind: 'static', + name: frameName, + parent, + localPose: poseFromFrame(framePose.translation, framePose.orientation), + geometry: parseGeometry(innerData.geometry, framePose), + uuid: planUuid(), + }) + } + break + } + + case 'tail_geometry_static': + case 'static': { + const frame = entry.frame as Record + descriptors.push({ + kind: 'static', + name: frameName, + parent, + localPose: poseFromFrame(frame.translation as Vec3Json, frame.orientation as OrientJson), + geometry: parseGeometry(frame.geometry), + uuid: planUuid(), + }) + break + } + } + } + + return descriptors +} diff --git a/src/lib/plugins/MotionPlanReplayer/parse-plan.ts b/src/lib/plugins/MotionPlanReplayer/parse-plan.ts new file mode 100644 index 000000000..530e48792 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/parse-plan.ts @@ -0,0 +1,136 @@ +import { z } from 'zod' + +const RawFrameSchema = z.object({ + frame_type: z.string(), + frame: z.unknown(), +}) + +const FrameSystemSchema = z.object({ + frames: z.record(z.string(), RawFrameSchema), + parents: z.record(z.string(), z.string()), +}) + +const PlanChunkSchema = z.object({ + frame_system: FrameSystemSchema.optional(), + goals: z.array(z.unknown()).optional(), + trajectory: z.array(z.record(z.string(), z.array(z.number()))).optional(), +}) + +export type RawFrame = z.infer + +export type ParsedPlan = { + frames: Record + parents: Record + trajectory: Array> + goals: unknown[] +} + +export class PlanParseError extends Error { + constructor(message: string) { + super(message) + this.name = 'PlanParseError' + } +} + +const skipWhitespace = (text: string, start: number): number => { + let i = start + while (i < text.length && /\s/.test(text[i]!)) i++ + return i +} + +const readJSONObject = (text: string, start: number): { raw: string; end: number } | null => { + let i = skipWhitespace(text, start) + if (i >= text.length || text[i] !== '{') return null + let depth = 0 + let inString = false + let escaped = false + for (; i < text.length; i++) { + const ch = text[i]! + if (inString) { + if (escaped) { + escaped = false + continue + } + if (ch === '\\') { + escaped = true + continue + } + if (ch === '"') inString = false + continue + } + if (ch === '"') { + inString = true + continue + } + if (ch === '{') { + depth++ + continue + } + if (ch === '}') { + depth-- + if (depth === 0) return { raw: text.slice(start, i + 1), end: i + 1 } + } + } + return null +} + +const splitJsonObjects = (content: string): string[] => { + const chunks: string[] = [] + let index = 0 + for (let i = 0; i < 8; i++) { + index = skipWhitespace(content, index) + if (index >= content.length) break + const next = readJSONObject(content, index) + if (!next) break + chunks.push(next.raw) + index = next.end + } + return chunks +} + +const parseChunk = (raw: string): z.infer => { + let parsed: unknown + try { + parsed = JSON.parse(raw) + } catch { + throw new PlanParseError('plan JSON contains invalid JSON') + } + + const result = PlanChunkSchema.safeParse(parsed) + if (!result.success) { + throw new PlanParseError('plan JSON does not match expected motion plan format') + } + return result.data +} + +export const parsePlan = (content: string): ParsedPlan => { + const chunks = splitJsonObjects(content) + if (chunks.length === 0) { + throw new PlanParseError('plan JSON contains invalid JSON') + } + + let frames: Record = {} + let parents: Record = {} + let trajectory: Array> = [] + let goals: unknown[] = [] + let foundFrameSystem = false + + for (const chunk of chunks) { + const obj = parseChunk(chunk) + + if (obj.frame_system) { + frames = obj.frame_system.frames + parents = obj.frame_system.parents + goals = obj.goals ?? [] + foundFrameSystem = true + } + + if (obj.trajectory) { + trajectory = obj.trajectory + } + } + + if (!foundFrameSystem) throw new PlanParseError('plan is missing frame_system') + + return { frames, parents, trajectory, goals } +} diff --git a/src/lib/plugins/MotionPlanReplayer/plan-dropper.ts b/src/lib/plugins/MotionPlanReplayer/plan-dropper.ts new file mode 100644 index 000000000..e478c3dae --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/plan-dropper.ts @@ -0,0 +1,35 @@ +import type { Snapshot } from '$lib/buf/draw/v1/snapshot_pb' + +import { FileDropperError } from '$lib/components/FileDrop/file-dropper' + +import { parsePlan, PlanParseError } from './parse-plan' +import { parsedPlanToSnapshots } from './plan-to-snapshots' + +const truncate = (s: string, max = 40): string => (s.length > max ? `${s.slice(0, max - 1)}…` : s) + +type PlanDropResult = + | { success: true; name: string; content: string; snapshots: Snapshot[]; stepCount: number } + | { success: false; error: FileDropperError } + +export const planDropper = async ({ + name, + content, +}: { + name: string + content: string | ArrayBuffer | null | undefined +}): Promise => { + const label = truncate(name) + + if (typeof content !== 'string') { + return { success: false, error: new FileDropperError(`"${label}" failed to load.`) } + } + + try { + const plan = parsePlan(content) + const snapshots = parsedPlanToSnapshots(plan) + return { success: true, name, content, snapshots, stepCount: snapshots.length } + } catch (error) { + const message = error instanceof PlanParseError ? error.message : `"${label}" failed to parse.` + return { success: false, error: new FileDropperError(message) } + } +} diff --git a/src/lib/plugins/MotionPlanReplayer/plan-to-snapshots.ts b/src/lib/plugins/MotionPlanReplayer/plan-to-snapshots.ts new file mode 100644 index 000000000..4be0e2a71 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/plan-to-snapshots.ts @@ -0,0 +1,69 @@ +import { Quaternion, Vector3 } from 'three' + +import { Pose, PoseInFrame, Transform } from '$lib/buf/common/v1/common_pb' +import { Snapshot } from '$lib/buf/draw/v1/snapshot_pb' +import { quaternionToPose } from '$lib/transform' + +import type { ParsedPlan } from './parse-plan' + +import { + buildFrameDescriptors, + type FrameDescriptor, + type JointFrameDescriptor, +} from './build-frame-descriptors' +import { planUuid } from './plan-uuid' + +// Shared scratch objects — safe in single-threaded JS +const tmpQ = new Quaternion() +const tmpVec = new Vector3() + +const computeJointPose = (descriptor: JointFrameDescriptor, angleRad: number): Pose => { + tmpQ.setFromAxisAngle( + tmpVec.set(descriptor.axis.X, descriptor.axis.Y, descriptor.axis.Z).normalize(), + angleRad + ) + const pose = new Pose() + quaternionToPose(tmpQ, pose) + return pose +} + +const descriptorToTransform = ( + descriptor: FrameDescriptor, + stepInputs: Record +): Transform => { + if (descriptor.kind === 'static') { + return new Transform({ + referenceFrame: descriptor.name, + poseInObserverFrame: new PoseInFrame({ + referenceFrame: descriptor.parent, + pose: descriptor.localPose, + }), + physicalObject: descriptor.geometry ?? undefined, + uuid: descriptor.uuid, + }) + } + + const angleRad = stepInputs[descriptor.componentName]?.[descriptor.jointIndex] ?? 0 + return new Transform({ + referenceFrame: descriptor.name, + poseInObserverFrame: new PoseInFrame({ + referenceFrame: descriptor.parent, + pose: computeJointPose(descriptor, angleRad), + }), + uuid: descriptor.uuid, + }) +} + +const planToSnapshots = ( + descriptors: FrameDescriptor[], + trajectory: Array> +): Snapshot[] => + trajectory.map((stepInputs) => { + const transforms = descriptors.map((d) => descriptorToTransform(d, stepInputs)) + return new Snapshot({ transforms, uuid: planUuid() }) + }) + +export const parsedPlanToSnapshots = (plan: ParsedPlan): Snapshot[] => { + const descriptors = buildFrameDescriptors(plan) + return planToSnapshots(descriptors, plan.trajectory) +} diff --git a/src/lib/plugins/MotionPlanReplayer/plan-uuid.ts b/src/lib/plugins/MotionPlanReplayer/plan-uuid.ts new file mode 100644 index 000000000..5ee8d3fb4 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/plan-uuid.ts @@ -0,0 +1,7 @@ +import { UuidTool } from 'uuid-tool' + +export const planUuid = (): Uint8Array => { + const bytes = new Uint8Array(new ArrayBuffer(16)) + bytes.set(UuidTool.toBytes(crypto.randomUUID())) + return bytes +} diff --git a/src/lib/plugins/MotionPlanReplayer/relations.ts b/src/lib/plugins/MotionPlanReplayer/relations.ts new file mode 100644 index 000000000..3706a04fc --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/relations.ts @@ -0,0 +1,10 @@ +import { relation } from 'koota' + +/** + * Members are the source, the plan root is the target. Destroying the plan + * root cascades to destroy every member — the opposite direction from + * Selection's `PointsCapturedBy` (autoDestroy: 'target'), since here we want + * "destroy the group destroys its members", not "destroy a member destroys + * the group". + */ +export const PartOfPlan = relation({ autoDestroy: 'source' }) diff --git a/src/lib/plugins/MotionPlanReplayer/useMotionPlanReplayer.svelte.ts b/src/lib/plugins/MotionPlanReplayer/useMotionPlanReplayer.svelte.ts new file mode 100644 index 000000000..0e72fb890 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/useMotionPlanReplayer.svelte.ts @@ -0,0 +1,214 @@ +import type { Entity } from 'koota' + +import { getContext, setContext } from 'svelte' + +import type { Snapshot } from '$lib/buf/draw/v1/snapshot_pb' + +import { relations, traits, useWorld } from '$lib/ecs' +import { useRelationships } from '$lib/hooks/useRelationships.svelte' +import { reconcileSnapshotEntities, type SnapshotEntity } from '$lib/snapshot' + +import { parsePlan, PlanParseError } from './parse-plan' +import { parsedPlanToSnapshots } from './plan-to-snapshots' +import * as planRelations from './relations' + +const PLAN_COLOR = { r: 0, g: 0.47, b: 1 } +const PLAN_OPACITY = 0.6 + +export interface PlanEntry { + name: string + content: string +} + +// Only primitives here — proto objects (Snapshot[]) live outside $state to avoid Svelte 5 deep proxy +interface PlanState { + name: string + content: string + status: 'idle' | 'ready' | 'error' | 'no-trajectory' + error: string | null + stepCount: number +} + +interface MotionPlanReplayerContext { + readonly plans: PlanState[] + readonly activePlanIndex: number | null + readonly currentStep: number + readonly totalSteps: number + addPlan: (name: string, content: string, precomputedSnapshots?: Snapshot[]) => void + removePlan: (index: number) => void + selectPlan: (index: number) => void + setStep: (step: number) => void + clearActivePlan: () => void +} + +const KEY = Symbol('motion-plan-replayer') + +export const provideMotionPlanReplayer = (initialPlans?: PlanEntry[]) => { + const world = useWorld() + const relationships = useRelationships() + + // Proto objects stored here — never inside $state to avoid Svelte 5 deep proxy + const snapshotStore = new Map() + + let plans = $state( + (initialPlans ?? []).map((e) => ({ + name: e.name, + content: e.content, + status: 'idle' as const, + error: null, + stepCount: 0, + })) + ) + let activePlanIndex = $state(null) + let currentStep = $state(0) + let entityMap = $state.raw(new Map()) + let planEntity: Entity | undefined + + const totalSteps = $derived( + activePlanIndex === null ? 0 : (plans[activePlanIndex]?.stepCount ?? 0) + ) + + const clearActivePlan = () => { + if (planEntity && world.has(planEntity)) planEntity.destroy() + planEntity = undefined + entityMap = new Map() + currentStep = 0 + activePlanIndex = null + } + + const tagPartOfPlan = (entity: Entity): void => { + if (!planEntity) return + entity.add(planRelations.PartOfPlan(planEntity)) + for (const child of world.query(relations.ChildOf(entity))) { + tagPartOfPlan(child) + } + } + + const applyStep = (snapshots: Snapshot[], step: number) => { + const snap = snapshots[step]! + + const result = reconcileSnapshotEntities(world, snap, entityMap) + + for (const spawned of result.spawned) { + relationships.apply(spawned.entity, spawned.relationships) + const uuid = spawned.entity.get(traits.UUID) + if (uuid) relationships.flush(uuid) + tagPartOfPlan(spawned.entity) + } + entityMap = result.current + + if (planEntity) { + for (const entity of world.query(planRelations.PartOfPlan(planEntity))) { + if (!entity.isAlive()) continue + if (entity.has(traits.ReferenceFrame)) continue + if (entity.has(traits.Color)) { + entity.set(traits.Color, PLAN_COLOR) + } else { + entity.add(traits.Color(PLAN_COLOR)) + } + entity.set(traits.Opacity, PLAN_OPACITY) + } + } + + currentStep = step + } + + const setStep = (step: number) => { + if (activePlanIndex === null) return + const snapshots = snapshotStore.get(activePlanIndex) + if (!snapshots || snapshots.length === 0) return + applyStep(snapshots, Math.max(0, Math.min(snapshots.length - 1, step))) + } + + const loadPlan = (index: number): void => { + const planState = plans[index] + if (!planState) return + + const stored = snapshotStore.get(index) + if (stored) { + activePlanIndex = index + currentStep = 0 + if (!planEntity) planEntity = world.spawn(traits.Name(planState.name)) + applyStep(stored, 0) + return + } + + try { + const plan = parsePlan(planState.content) + const snapshots = parsedPlanToSnapshots(plan) + if (snapshots.length === 0) { + plans[index] = { ...planState, status: 'no-trajectory', stepCount: 0 } + activePlanIndex = index + return + } + snapshotStore.set(index, snapshots) + plans[index] = { ...planState, status: 'ready', stepCount: snapshots.length, error: null } + activePlanIndex = index + currentStep = 0 + if (!planEntity) planEntity = world.spawn(traits.Name(planState.name)) + applyStep(snapshots, 0) + } catch (error) { + const msg = error instanceof PlanParseError ? error.message : 'Failed to parse plan.' + console.warn('[MotionPlanReplayer] loadPlan error:', msg) + plans[index] = { ...planState, status: 'error', error: msg } + } + } + + const selectPlan = (index: number): void => { + if (activePlanIndex !== null && activePlanIndex !== index) clearActivePlan() + loadPlan(index) + } + + const addPlan = (name: string, content: string, precomputedSnapshots?: Snapshot[]) => { + const index = plans.length + if (precomputedSnapshots && precomputedSnapshots.length > 0) { + snapshotStore.set(index, precomputedSnapshots) + } + plans = [ + ...plans, + { + name, + content, + status: precomputedSnapshots && precomputedSnapshots.length > 0 ? 'ready' : 'idle', + error: null, + stepCount: precomputedSnapshots?.length ?? 0, + }, + ] + selectPlan(index) + } + + const removePlan = (index: number) => { + if (activePlanIndex === index) clearActivePlan() + snapshotStore.delete(index) + plans = plans.filter((_, i) => i !== index) + if (activePlanIndex !== null && activePlanIndex > index) { + activePlanIndex = activePlanIndex - 1 + } + } + + const context: MotionPlanReplayerContext = { + get plans() { + return plans + }, + get activePlanIndex() { + return activePlanIndex + }, + get currentStep() { + return currentStep + }, + get totalSteps() { + return totalSteps + }, + addPlan, + removePlan, + selectPlan, + setStep, + clearActivePlan, + } + + setContext(KEY, context) + return context +} + +export const useMotionPlanReplayer = (): MotionPlanReplayerContext => + getContext(KEY) diff --git a/src/lib/plugins/index.ts b/src/lib/plugins/index.ts index 4a36171da..dfd4af532 100644 --- a/src/lib/plugins/index.ts +++ b/src/lib/plugins/index.ts @@ -30,3 +30,7 @@ export type { FrameDelta } from './LLMSceneBuilder/frameDeltaAdapter' // XR export { default as XR } from './XR/XR.svelte' + +// MotionPlanReplayer +export { default as MotionPlanReplayer } from './MotionPlanReplayer/MotionPlanReplayer.svelte' +export type { PlanEntry } from './MotionPlanReplayer/useMotionPlanReplayer.svelte' diff --git a/src/routes/+layout.svelte b/src/routes/+layout.svelte index 49132423d..a33434ddb 100644 --- a/src/routes/+layout.svelte +++ b/src/routes/+layout.svelte @@ -7,7 +7,7 @@ import { Visualizer } from '$lib' import { backendIP, websocketPort } from '$lib/defines' - import { DrawService, Focus, Logs, MeasureTool, XR } from '$lib/plugins' + import { DrawService, Focus, Logs, MeasureTool, MotionPlanReplayer, XR } from '$lib/plugins' import MachineConnectionProvider from './lib/components/MachineConnectionProvider.svelte' import Machines from './lib/components/Machines.svelte' @@ -80,7 +80,7 @@ - +