@viamrobotics/motion-tools 0.1.0

package/dist/hooks/usePoses.svelte.js

@@ -0,0 +1,63 @@
+import { createQueries, queryOptions } from '@tanstack/svelte-query';
+import { MotionClient, PoseInFrame, ResourceName } from '@viamrobotics/sdk';
+import { createResourceClient, useResourceNames } from '@viamrobotics/svelte-sdk';
+import { getContext, setContext } from 'svelte';
+import { fromStore, toStore } from 'svelte/store';
+import { useRefreshRates } from './useRefreshRates.svelte';
+const key = Symbol('poses-context');
+export const providePoses = (partID) => {
+    const refreshRates = useRefreshRates();
+    const resources = useResourceNames(partID);
+    const components = $derived(resources.current.filter(({ type }) => type === 'component'));
+    const motionResources = useResourceNames(partID, 'motion');
+    const clients = $derived(motionResources.current.map((resource) => createResourceClient(MotionClient, partID, () => resource.name)));
+    if (!refreshRates.has('Poses')) {
+        refreshRates.set('Poses', 1000);
+    }
+    const options = $derived(clients.map((motionClient) => {
+        const interval = refreshRates.get('Poses');
+        return queryOptions({
+            enabled: interval !== -1 && motionClient.current !== undefined,
+            refetchInterval: interval === 0 ? false : interval,
+            queryKey: ['partID', partID(), motionClient.current?.name, 'getPose'],
+            queryFn: async () => {
+                const client = motionClient.current;
+                if (!client) {
+                    throw new Error('No client');
+                }
+                const promises = components.map((component) => {
+                    return client.getPose(component, 'world', []);
+                });
+                const results = await Promise.allSettled(promises);
+                return results
+                    .map((result, index) => ({ ...result, component: components[index] }))
+                    .filter((result) => result.status === 'fulfilled')
+                    .map((result) => ({ ...result.value, component: result.component }));
+            },
+        });
+    }));
+    const queries = fromStore(createQueries({
+        queries: toStore(() => options),
+        combine: (results) => {
+            const obj = {};
+            for (const service of results) {
+                let i = 0;
+                if (service.data) {
+                    for (const pose of service.data) {
+                        obj[components[i].name] = pose;
+                        i += 1;
+                    }
+                }
+            }
+            return obj;
+        },
+    }));
+    setContext(key, {
+        get current() {
+            return queries.current;
+        },
+    });
+};
+export const usePoses = () => {
+    return getContext(key);
+};

package/dist/hooks/useRefreshRates.svelte.d.ts

@@ -0,0 +1,5 @@
+import { SvelteMap } from 'svelte/reactivity';
+type Context = SvelteMap<string, number>;
+export declare const provideRefreshRates: () => SvelteMap<string, number>;
+export declare const useRefreshRates: () => Context;
+export {};

package/dist/hooks/useRefreshRates.svelte.js

@@ -0,0 +1,23 @@
+import { get, set } from 'idb-keyval';
+import { getContext, setContext } from 'svelte';
+import { SvelteMap } from 'svelte/reactivity';
+const key = Symbol('polling-rate-context');
+const idbKey = 'polling-rate';
+export const provideRefreshRates = () => {
+    const map = new SvelteMap();
+    get(idbKey).then((entries) => {
+        if (entries) {
+            for (const [key, value] of entries) {
+                map.set(key, value);
+            }
+        }
+    });
+    $effect(() => {
+        set(idbKey, [...map.entries()]);
+    });
+    setContext(key, map);
+    return map;
+};
+export const useRefreshRates = () => {
+    return getContext(key);
+};

package/dist/hooks/useSelection.svelte.d.ts

@@ -0,0 +1,40 @@
+import type { Object3D } from 'three';
+import type { WorldObject } from '../WorldObject';
+type UUID = string;
+interface SelectionContext {
+    readonly current: UUID | undefined;
+    set(value?: UUID): void;
+}
+interface FocusContext {
+    readonly current: UUID | undefined;
+    set(value?: UUID): void;
+}
+export declare const provideSelection: () => {
+    selection: {
+        readonly current: string | undefined;
+        set(value?: UUID): void;
+    };
+    focus: {
+        readonly current: string | undefined;
+        set(value?: UUID): void;
+    };
+    hover: {
+        readonly current: string | undefined;
+        set(value?: UUID): void;
+    };
+};
+export declare const useSelected: () => SelectionContext;
+export declare const useFocused: () => FocusContext;
+export declare const useFocusedObject: () => {
+    current: WorldObject | undefined;
+};
+export declare const useSelectedObject: () => {
+    current: WorldObject | undefined;
+};
+export declare const useFocusedObject3d: () => {
+    current: Object3D | undefined;
+};
+export declare const useSelectedObject3d: () => {
+    current: Object3D | undefined;
+};
+export {};

package/dist/hooks/useSelection.svelte.js

@@ -0,0 +1,92 @@
+import { useThrelte } from '@threlte/core';
+import { getContext, setContext } from 'svelte';
+import { useObjects } from './useObjects.svelte';
+const hoverKey = Symbol('hover-context');
+const selectionKey = Symbol('selection-context');
+const focusKey = Symbol('focus-context');
+const selectedObjectKey = Symbol('selected-frame-context');
+const focusedObjectKey = Symbol('focused-frame-context');
+export const provideSelection = () => {
+    let selected = $state();
+    let focused = $state();
+    let hovered = $state();
+    const selectionContext = {
+        get current() {
+            return selected;
+        },
+        set(value) {
+            selected = value;
+        },
+    };
+    setContext(selectionKey, selectionContext);
+    const focusContext = {
+        get current() {
+            return focused;
+        },
+        set(value) {
+            focused = value;
+        },
+    };
+    setContext(focusKey, focusContext);
+    const hoverContext = {
+        get current() {
+            return hovered;
+        },
+        set(value) {
+            hovered = value;
+        },
+    };
+    setContext(hoverKey, hoverContext);
+    const objects = useObjects();
+    const selectedObject = $derived(objects.current.find((object) => object.uuid === selected));
+    const selectedObjectContext = {
+        get current() {
+            return selectedObject;
+        },
+    };
+    setContext(selectedObjectKey, selectedObjectContext);
+    const focusedFrame = $derived(objects.current.find((object) => object.uuid === focused));
+    const focusedFrameContext = {
+        get current() {
+            return focusedFrame;
+        },
+    };
+    setContext(focusedObjectKey, focusedFrameContext);
+    return {
+        selection: selectionContext,
+        focus: focusContext,
+        hover: hoverContext,
+    };
+};
+export const useSelected = () => {
+    return getContext(selectionKey);
+};
+export const useFocused = () => {
+    return getContext(focusKey);
+};
+export const useFocusedObject = () => {
+    return getContext(focusedObjectKey);
+};
+export const useSelectedObject = () => {
+    return getContext(selectedObjectKey);
+};
+export const useFocusedObject3d = () => {
+    const focusedObject = useFocusedObject();
+    const { scene } = useThrelte();
+    const object = $derived(focusedObject.current ? scene.getObjectByName(focusedObject.current.name)?.clone() : undefined);
+    return {
+        get current() {
+            return object;
+        },
+    };
+};
+export const useSelectedObject3d = () => {
+    const selectedObject = useSelectedObject();
+    const { scene } = useThrelte();
+    const object = $derived(selectedObject.current ? scene.getObjectByName(selectedObject.current.name) : undefined);
+    return {
+        get current() {
+            return object;
+        },
+    };
+};

package/dist/hooks/useShapes.svelte.d.ts

@@ -0,0 +1,17 @@
+import { BatchedArrow } from '../three/BatchedArrow';
+import { WorldObject, type PointsGeometry } from '../WorldObject';
+type ConnectionStatus = 'connecting' | 'open' | 'closed';
+interface Context {
+    points: WorldObject<PointsGeometry>[];
+    meshes: WorldObject[];
+    poses: WorldObject[];
+    nurbs: WorldObject[];
+    models: WorldObject[];
+    connectionStatus: ConnectionStatus;
+    object3ds: {
+        batchedArrow: BatchedArrow;
+    };
+}
+export declare const provideShapes: () => void;
+export declare const useShapes: () => Context;
+export {};

package/dist/hooks/useShapes.svelte.js

@@ -0,0 +1,264 @@
+import { getContext, setContext } from 'svelte';
+import { Vector3, Vector4 } from 'three';
+import { NURBSCurve } from 'three/addons/curves/NURBSCurve.js';
+import { parsePCD } from '../loaders/pcd';
+import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
+import { BatchedArrow } from '../three/BatchedArrow';
+import { WorldObject } from '../WorldObject';
+const key = Symbol('websocket-context-key');
+const tryParse = (json) => {
+    try {
+        return JSON.parse(json);
+    }
+    catch (error) {
+        console.warn('Failed to parse JSON:', error);
+        return;
+    }
+};
+export const provideShapes = () => {
+    let pointsIndex = 0;
+    let geometryIndex = 0;
+    let poseIndex = 0;
+    let reconnectDelay = 200;
+    const maxReconnectDelay = 5_000;
+    const { BACKEND_IP, BUN_SERVER_PORT } = globalThis;
+    let ws;
+    const points = $state([]);
+    const meshes = $state([]);
+    const poses = $state([]);
+    const nurbs = $state([]);
+    const models = $state([]);
+    let connectionStatus = $state('connecting');
+    const loader = new GLTFLoader();
+    const addPcd = async (data) => {
+        const buffer = await data.arrayBuffer();
+        const { positions, colors } = await parsePCD(new Uint8Array(buffer));
+        points.push(new WorldObject(`points ${++pointsIndex}`, undefined, undefined, {
+            case: 'points',
+            value: new Float32Array(positions),
+        }, colors ? { colors: new Float32Array(colors) } : undefined));
+    };
+    // eslint-disable-next-line @typescript-eslint/no-explicit-any
+    const addGeometry = (data, color, parent) => {
+        let geometry;
+        if ('mesh' in data) {
+            geometry = {
+                case: 'mesh',
+                value: { contentType: '', mesh: data.mesh.mesh },
+            };
+        }
+        else if ('box' in data) {
+            geometry = { case: 'box', value: data.box };
+        }
+        else if ('sphere' in data) {
+            geometry = { case: 'sphere', value: data.sphere };
+        }
+        else if ('capsule' in data) {
+            geometry = { case: 'capsule', value: data.capsule };
+        }
+        else {
+            geometry = { case: undefined, value: undefined };
+        }
+        const object = new WorldObject(data.label ?? ++geometryIndex, data.center, parent, geometry, {
+            color,
+        });
+        meshes.push(object);
+    };
+    // eslint-disable-next-line @typescript-eslint/no-explicit-any
+    const addNurbs = (data, color) => {
+        const controlPoints = data.ControlPts.map((point) => new Vector4(point.x / 1000, point.y / 1000, point.z / 1000));
+        const curve = new NURBSCurve(data.Degree, data.Knots, controlPoints);
+        const object = new WorldObject(data.name, data.pose, data.parent, { case: 'line', value: new Float32Array() }, { color, points: curve.getPoints(200) });
+        nurbs.push(object);
+    };
+    const direction = new Vector3();
+    const origin = new Vector3();
+    const vec3 = new Vector3();
+    const batchedArrow = new BatchedArrow();
+    // eslint-disable-next-line @typescript-eslint/no-explicit-any
+    const addPoses = (nextPoses, colors, arrowHeadAtPose) => {
+        for (let i = 0, l = nextPoses.length; i < l; i += 1) {
+            const pose = nextPoses[i];
+            const length = 0.1;
+            direction.set(pose.oX ?? 0, pose.oY ?? 0, pose.oZ ?? 0);
+            origin.set((pose.x ?? 0) / 1000, (pose.y ?? 0) / 1000, (pose.z ?? 0) / 1000);
+            if (arrowHeadAtPose) {
+                // Compute the base position so the arrow ends at the origin
+                origin.sub(vec3.copy(direction).multiplyScalar(length));
+            }
+            const arrowId = batchedArrow.addArrow(direction, origin, length, colors[i]);
+            poses.push(new WorldObject(`pose ${++poseIndex}`, pose, undefined, undefined, {
+                getBoundingBoxAt(box3) {
+                    return batchedArrow.getBoundingBoxAt(arrowId, box3);
+                },
+                batched: {
+                    id: arrowId,
+                    name: batchedArrow.object3d.name,
+                },
+            }));
+        }
+    };
+    // eslint-disable-next-line @typescript-eslint/no-explicit-any
+    const addGeometries = (geometries, colors, parent) => {
+        let i = 0;
+        for (const geometry of geometries) {
+            addGeometry(geometry, colors[i], parent);
+            i += 1;
+        }
+    };
+    const remove = (names) => {
+        let index = -1;
+        for (const name of names) {
+            index = points.findIndex((p) => p.name === name);
+            if (index !== -1) {
+                points.splice(index, 1);
+                continue;
+            }
+            index = meshes.findIndex((m) => m.name === name);
+            if (index !== -1) {
+                meshes.splice(index, 1);
+                continue;
+            }
+            index = poses.findIndex((p) => p.name === name);
+            if (index !== -1) {
+                const id = poses[index].metadata.batched?.id;
+                if (id) {
+                    batchedArrow.removeArrow(id);
+                    poses.splice(index, 1);
+                    continue;
+                }
+            }
+            index = nurbs.findIndex((n) => n.name === name);
+            if (index !== -1) {
+                nurbs.splice(index, 1);
+                continue;
+            }
+            index = models.findIndex((m) => m.name === name);
+            if (index !== -1) {
+                models.splice(index, 1);
+                continue;
+            }
+        }
+    };
+    const removeAll = () => {
+        points.splice(0, points.length);
+        meshes.splice(0, meshes.length);
+        nurbs.splice(0, nurbs.length);
+        models.splice(0, models.length);
+        poses.splice(0, poses.length);
+        batchedArrow.clear();
+        pointsIndex = 0;
+        geometryIndex = 0;
+        poseIndex = 0;
+    };
+    let metadata = undefined;
+    const handleMetadata = (data) => {
+        const json = tryParse(data);
+        if ('ext' in json) {
+            metadata = json;
+            return true;
+        }
+        return false;
+    };
+    const loadGLTF = async (data) => {
+        const buffer = await data.arrayBuffer();
+        const blob = new Blob([buffer], { type: 'model/gltf-binary' });
+        const url = URL.createObjectURL(blob);
+        const gltf = await loader.loadAsync(url);
+        models.push(new WorldObject(gltf.scene.name, undefined, undefined, undefined, { gltf }));
+        URL.revokeObjectURL(url);
+    };
+    const scheduleReconnect = () => {
+        setTimeout(() => {
+            reconnectDelay = Math.min(reconnectDelay * 2, maxReconnectDelay);
+            console.log(`Reconnecting in ${reconnectDelay / 1000} seconds...`);
+            connect();
+        }, reconnectDelay);
+    };
+    const onOpen = () => {
+        connectionStatus = 'open';
+        reconnectDelay = 1000;
+        console.log(`Connected to websocket server at ${BACKEND_IP}:${BUN_SERVER_PORT}`);
+    };
+    const onClose = () => {
+        connectionStatus = 'closed';
+        console.log('Disconnected from websocket server');
+        scheduleReconnect();
+    };
+    const onError = (event) => {
+        console.log('Websocket error', JSON.stringify(event));
+        ws.close();
+    };
+    const onMessage = (event) => {
+        if (typeof event.data === 'string') {
+            if (handleMetadata(event.data)) {
+                return;
+            }
+        }
+        if (typeof event.data === 'object' && 'arrayBuffer' in event.data) {
+            if (metadata?.ext === 'glb') {
+                loadGLTF(event.data);
+            }
+            else if (metadata?.ext === 'pcd') {
+                addPcd(event.data);
+            }
+            return;
+        }
+        const data = tryParse(event.data);
+        if (!data)
+            return;
+        if ('geometries' in data) {
+            return addGeometries(data.geometries, data.colors, data.parent);
+        }
+        if ('remove' in data) {
+            return remove(data.names);
+        }
+        if ('removeAll' in data) {
+            return removeAll();
+        }
+        if ('Knots' in data) {
+            return addNurbs(data, data.Color);
+        }
+        if ('poses' in data) {
+            return addPoses(data.poses, data.colors, data.arrowHeadAtPose);
+        }
+        if ('geometry' in data) {
+            addGeometry(data.geometry, data.color);
+        }
+    };
+    const connect = () => {
+        const protocol = location.protocol === 'https:' ? 'wss' : 'ws';
+        ws = new WebSocket(`${protocol}://${BACKEND_IP}:${BUN_SERVER_PORT}/ws`);
+        ws.onclose = onClose;
+        ws.onerror = onError;
+        ws.onopen = onOpen;
+        ws.onmessage = onMessage;
+    };
+    connect();
+    setContext(key, {
+        get points() {
+            return points;
+        },
+        get meshes() {
+            return meshes;
+        },
+        get poses() {
+            return poses;
+        },
+        get nurbs() {
+            return nurbs;
+        },
+        get models() {
+            return models;
+        },
+        get connectionStatus() {
+            return connectionStatus;
+        },
+        object3ds: {
+            batchedArrow,
+        },
+    });
+};
+export const useShapes = () => {
+    return getContext(key);
+};

package/dist/hooks/useStaticGeometries.svelte.d.ts

@@ -0,0 +1,9 @@
+import { WorldObject } from '../WorldObject';
+interface Context {
+    current: WorldObject[];
+    add: () => void;
+    remove: (name: string) => void;
+}
+export declare const provideStaticGeometries: () => void;
+export declare const useStaticGeometries: () => Context;
+export {};

package/dist/hooks/useStaticGeometries.svelte.js

@@ -0,0 +1,37 @@
+import { getContext, setContext } from 'svelte';
+import { get, set } from 'idb-keyval';
+import { Debounced } from 'runed';
+import { createGeometry } from '../transform';
+import { WorldObject } from '../WorldObject';
+const key = Symbol('static-geometries-context');
+export const provideStaticGeometries = () => {
+    let geometries = $state([]);
+    const debounced = new Debounced(() => geometries, 500);
+    get('static-geometries').then((response) => {
+        if (Array.isArray(response)) {
+            geometries = response;
+        }
+    });
+    $effect(() => {
+        set('static-geometries', $state.snapshot(debounced.current));
+    });
+    setContext(key, {
+        get current() {
+            return geometries;
+        },
+        add() {
+            const object = new WorldObject(`custom geometry ${geometries.length + 1}`, undefined, undefined, createGeometry({
+                case: 'box',
+                value: { dimsMm: { x: 100, y: 100, z: 100 } },
+            }).geometryType);
+            geometries.push(structuredClone(object));
+        },
+        remove(name) {
+            const index = geometries.findIndex((geo) => geo.name === name);
+            geometries.splice(index, 1);
+        },
+    });
+};
+export const useStaticGeometries = () => {
+    return getContext(key);
+};

package/dist/hooks/useVisibility.svelte.d.ts

@@ -0,0 +1,5 @@
+import { SvelteMap } from 'svelte/reactivity';
+type Context = SvelteMap<string, boolean>;
+export declare const provideVisibility: () => void;
+export declare const useVisibility: () => Context;
+export {};

package/dist/hooks/useVisibility.svelte.js

@@ -0,0 +1,22 @@
+import { get, set } from 'idb-keyval';
+import { getContext, setContext } from 'svelte';
+import { SvelteMap } from 'svelte/reactivity';
+const key = Symbol('object-visibility-context');
+const idbKey = 'object-visibility';
+export const provideVisibility = () => {
+    const map = new SvelteMap();
+    get(idbKey).then((entries) => {
+        if (entries) {
+            for (const [key, value] of entries) {
+                map.set(key, value);
+            }
+        }
+    });
+    $effect(() => {
+        set(idbKey, [...map.entries()]);
+    });
+    setContext(key, map);
+};
+export const useVisibility = () => {
+    return getContext(key);
+};

package/dist/hooks/xr/useAnchors.svelte.js

@@ -0,0 +1 @@
+"use strict";

package/dist/index.d.ts

@@ -0,0 +1 @@
+export { default as MotionTools } from './components/App.svelte';

package/dist/index.js

@@ -0,0 +1 @@
+export { default as MotionTools } from './components/App.svelte';

package/dist/keybindings.d.ts

@@ -0,0 +1,12 @@
+export declare const Keybindings: {
+    MACHINES: string;
+    TREEVIEW: string;
+    ESCAPE: string;
+    UP: string;
+    DOWN: string;
+    ADD_GEOMETRY: string;
+    REMOVE_GEOMETRY: string;
+    TRANSLATE: string;
+    ROTATE: string;
+    SCALE: string;
+};

package/dist/keybindings.js

@@ -0,0 +1,12 @@
+export const Keybindings = {
+    MACHINES: 'm',
+    TREEVIEW: '`',
+    ESCAPE: 'Escape',
+    UP: 'ArrowUp',
+    DOWN: 'ArrowDown',
+    ADD_GEOMETRY: '=',
+    REMOVE_GEOMETRY: '-',
+    TRANSLATE: 't',
+    ROTATE: 'r',
+    SCALE: 's',
+};

package/dist/loaders/pcd/index.d.ts

@@ -0,0 +1,4 @@
+export declare const parsePCD: (array: Uint8Array<ArrayBufferLike>) => Promise<{
+    positions: ArrayBuffer;
+    colors: ArrayBuffer | undefined;
+}>;

package/dist/loaders/pcd/index.js

@@ -0,0 +1,13 @@
+// main.js
+const worker = new Worker(new URL('./worker', import.meta.url), { type: 'module' });
+export const parsePCD = async (array) => {
+    return new Promise((resolve, reject) => {
+        worker.onmessage = (event) => {
+            if (event.data.error) {
+                return reject(event.data.error);
+            }
+            resolve({ positions: event.data.positionArray, colors: event.data.colorArray });
+        };
+        worker.postMessage({ data: array }, [array.buffer]);
+    });
+};

package/dist/loaders/pcd/worker.d.ts

@@ -0,0 +1 @@
+export {};

package/dist/loaders/pcd/worker.js

@@ -0,0 +1,26 @@
+// worker.js
+import { PCDLoader } from 'three/examples/jsm/loaders/PCDLoader.js';
+const loader = new PCDLoader();
+self.onmessage = async (event) => {
+    const { data } = event.data;
+    if (!(data instanceof Uint8Array)) {
+        postMessage({ error: 'Invalid data format' });
+        return;
+    }
+    try {
+        const pcd = loader.parse(data.buffer);
+        if (pcd.geometry) {
+            const positionArray = pcd.geometry.attributes.position.array;
+            const colorArray = pcd.geometry.attributes.color ? pcd.geometry.attributes.color.array : null;
+            postMessage({ success: true, positionArray, colorArray }, colorArray
+                ? [positionArray.buffer, colorArray.buffer]
+                : [positionArray.buffer]);
+        }
+        else {
+            postMessage({ error: 'Failed to extract geometry' });
+        }
+    }
+    catch (error) {
+        postMessage({ error: error.message });
+    }
+};