@viamrobotics/motion-tools 1.32.0 → 1.33.1

package/dist/plugins/XR/OriginMarker.svelte

@@ -0,0 +1,341 @@
+<script lang="ts">
+	import { useTask, useThrelte } from '@threlte/core'
+	import { useGamepad } from '@threlte/extras'
+	import { Hand, useController, useHand, useHeadset, useXR } from '@threlte/xr'
+	import { useDebounce } from 'runed'
+	import { Euler, Quaternion, Vector3 } from 'three'
+
+	import { usePartID } from '../../hooks/usePartID.svelte'
+
+	import { useAnchors } from './useAnchors.svelte'
+	import { useOrigin } from './useOrigin.svelte'
+
+	const origin = useOrigin()
+	const anchors = useAnchors()
+	const partID = usePartID()
+	const headset = useHeadset()
+
+	const storageKey = $derived(`xr-origin-anchor:${partID.current}`)
+
+	const DEFAULT_ORIGIN: [number, number, number] = [-1, -1, 0]
+	const COMMIT_DEBOUNCE_MS = 500
+
+	const leftPad = useGamepad({ xr: true, hand: 'left' })
+	const rightPad = useGamepad({ xr: true, hand: 'right' })
+	const leftController = useController('left')
+	const rightController = useController('right')
+
+	const THUMBSTICK_SPEED = 0.05
+
+	// Head-relative translation basis. Recomputed per thumbstick tick so
+	// stick-forward always moves content away from the viewer, regardless of
+	// how the scene was rotated or which way the user is physically facing.
+	const headForward = new Vector3()
+	const headRight = new Vector3()
+
+	// The anchor that currently represents the persisted origin. Kept so we can
+	// delete it when a new calibration is committed.
+	let persistedAnchor: XRAnchor | undefined
+
+	// The restored anchor we're waiting to localize on session start. Once the
+	// device reports it as tracked we snap origin to its pose and clear this.
+	let pendingRestore = $state.raw<XRAnchor | undefined>(undefined)
+
+	const restoreQuat = new Quaternion()
+	const restoreEuler = new Euler(0, 0, 0, 'ZYX')
+
+	const commitVec = new Vector3()
+	const commitQuat = new Quaternion()
+
+	const commit = useDebounce(async () => {
+		// origin.position/rotation define the composed XR reference space's
+		// offset from zUp, so an anchor at identity in the current (composed)
+		// space IS the anchor at origin's pose in zUp. commitVec/commitQuat
+		// are left at their default-constructed zero/identity values.
+		const anchor = await anchors.createAnchor(commitVec, commitQuat)
+		if (!anchor) return
+
+		const uuid = await anchors.persist(anchor)
+		if (!uuid) {
+			anchor.delete()
+			return
+		}
+
+		const prev = localStorage.getItem(storageKey)
+		if (prev && prev !== uuid) {
+			anchors.remove(prev).catch(() => {})
+		}
+
+		if (persistedAnchor && persistedAnchor !== anchor) {
+			persistedAnchor.delete()
+		}
+
+		persistedAnchor = anchor
+		localStorage.setItem(storageKey, uuid)
+	}, COMMIT_DEBOUNCE_MS)
+
+	origin.registerCommit(() => commit())
+
+	leftPad.thumbstick.on('change', ({ value }) => {
+		// While the grip is held, the left controller drives fine rotation;
+		// ignore the thumbstick so the two inputs don't fight each other.
+		if (leftPad.squeeze.pressed || typeof value === 'number') return
+
+		const { x: vx, y: vy } = value
+		const [x, y, z] = origin.position
+
+		origin.set([x, y, z + vy * THUMBSTICK_SPEED], origin.rotation + vx * THUMBSTICK_SPEED)
+		commit()
+	})
+
+	rightPad.thumbstick.on('change', ({ value }) => {
+		if (rightPad.squeeze.pressed || typeof value === 'number') return
+
+		const { x: vx, y: vy } = value
+		const [x, y, z] = origin.position
+
+		headset.getWorldDirection(headForward)
+
+		// Flatten onto the XY (ground) plane so pitch doesn't bleed into horizontal motion.
+		headForward.z = 0
+		if (headForward.lengthSq() < 1e-6) {
+			// Viewer gaze was purely vertical (or pose not yet reported) — fall back to scene +Y.
+			headForward.set(0, 1, 0)
+		} else {
+			headForward.normalize()
+		}
+		// headForward is in the composed XR reference space; rotate into zUp
+		// so the stick direction maps to the user's physical gaze regardless
+		// of the current origin rotation.
+		origin.toZUpDir(headForward)
+		headRight.set(headForward.y, -headForward.x, 0)
+
+		const deltaX = headRight.x * vx * THUMBSTICK_SPEED + headForward.x * vy * THUMBSTICK_SPEED
+		const deltaY = headRight.y * vx * THUMBSTICK_SPEED + headForward.y * vy * THUMBSTICK_SPEED
+
+		origin.set([x + deltaX, y + deltaY, z], origin.rotation)
+		commit()
+	})
+
+	// Fine calibration: hold the grip, then move/rotate the controller to nudge
+	// the origin 1:1 with the controller delta. Extract world-Z yaw from the
+	// controller quaternion for rotation so pitch/roll don't leak in.
+	const quatYaw = (q: Quaternion) =>
+		Math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
+
+	const fineTranslateStart = new Vector3()
+	const fineTranslateOriginStart = new Vector3()
+	const fineTranslateCurrent = new Vector3()
+	let fineTranslating = $state(false)
+
+	rightPad.squeeze.on('change', () => {
+		const ray = rightController.current?.targetRay
+		if (rightPad.squeeze.pressed && ray) {
+			// Save start position in zUp so the delta stays stable as the
+			// composed reference space recomposes each tick on origin changes.
+			origin.toZUpPos(fineTranslateStart, ray.position)
+			const [ox, oy, oz] = origin.position
+			fineTranslateOriginStart.set(ox, oy, oz)
+			fineTranslating = true
+		} else {
+			fineTranslating = false
+		}
+	})
+
+	useTask(
+		() => {
+			const ray = rightController.current?.targetRay
+			if (!ray) return
+			origin.toZUpPos(fineTranslateCurrent, ray.position)
+			origin.set(
+				[
+					fineTranslateOriginStart.x + fineTranslateCurrent.x - fineTranslateStart.x,
+					fineTranslateOriginStart.y + fineTranslateCurrent.y - fineTranslateStart.y,
+					fineTranslateOriginStart.z + fineTranslateCurrent.z - fineTranslateStart.z,
+				],
+				origin.rotation
+			)
+			commit()
+		},
+		{ running: () => fineTranslating }
+	)
+
+	let fineRotateStartYaw = 0
+	let fineRotateOriginStart = 0
+	let fineRotating = $state(false)
+
+	leftPad.squeeze.on('change', () => {
+		const ray = leftController.current?.targetRay
+		if (leftPad.squeeze.pressed && ray) {
+			// Controller yaw in composed = yaw_zUp − origin.rotation; convert
+			// to zUp so the delta stays stable while origin.rotation updates.
+			fineRotateStartYaw = quatYaw(ray.quaternion) + origin.rotation
+			fineRotateOriginStart = origin.rotation
+			fineRotating = true
+		} else {
+			fineRotating = false
+		}
+	})
+
+	useTask(
+		() => {
+			const ray = leftController.current?.targetRay
+			if (!ray) return
+			const yawZup = quatYaw(ray.quaternion) + origin.rotation
+			origin.set(origin.position, fineRotateOriginStart + yawZup - fineRotateStartYaw)
+			commit()
+		},
+		{ running: () => fineRotating }
+	)
+
+	let startLeftPinchTranslation = new Vector3()
+	let leftPinchTranslation = new Vector3()
+	let startRightPinchTranslation = new Vector3()
+	let rightPinchCurrent = new Vector3()
+	let startRightPinchRotation = 0
+
+	const leftHand = useHand('left')
+	const rightHand = useHand('right')
+
+	let translating = $state(false)
+	let rotating = $state(false)
+
+	const { renderer } = useThrelte()
+	const { isPresenting } = useXR()
+
+	// Session start: try to restore a persisted anchor for this part. If none
+	// exists or restore fails, fall back to a default offset so the origin is
+	// visible in front of the user.
+	$effect(() => {
+		if (!$isPresenting) {
+			pendingRestore = undefined
+			persistedAnchor = undefined
+			return
+		}
+
+		const uuid = localStorage.getItem(storageKey)
+		if (!uuid) {
+			origin.set(DEFAULT_ORIGIN, 0)
+			return
+		}
+
+		let cancelled = false
+		anchors
+			.restore(uuid)
+			.then((anchor) => {
+				if (cancelled) return
+				if (anchor) {
+					persistedAnchor = anchor
+					pendingRestore = anchor
+				} else {
+					localStorage.removeItem(storageKey)
+					origin.set(DEFAULT_ORIGIN, 0)
+				}
+			})
+			.catch(() => {
+				if (cancelled) return
+				localStorage.removeItem(storageKey)
+				origin.set(DEFAULT_ORIGIN, 0)
+			})
+
+		return () => {
+			cancelled = true
+		}
+	})
+
+	// Once the restored anchor has localized, snap origin to its pose. This
+	// runs only while a restore is pending so it won't fight user input.
+	useTask(
+		() => {
+			const anchor = pendingRestore
+			if (!anchor) return
+
+			const pose = anchors.getAnchorPose(anchor)
+			if (!pose) return
+
+			const { position: p, orientation: o } = pose.transform
+			restoreQuat.set(o.x, o.y, o.z, o.w)
+			restoreEuler.setFromQuaternion(restoreQuat, 'ZYX')
+			origin.set([p.x, p.y, p.z], restoreEuler.z)
+			pendingRestore = undefined
+		},
+		{
+			running: () => pendingRestore !== undefined,
+		}
+	)
+
+	$effect(() => {
+		if (!$isPresenting) {
+			return
+		}
+		renderer.xr.getHand(0).addEventListener('pinchstart', () => {
+			const p = leftHand.current?.targetRay.position
+			if (p) {
+				translating = true
+				// Pinch start position in zUp; delta is cumulative from here.
+				origin.toZUpPos(startLeftPinchTranslation, p)
+			}
+		})
+	})
+
+	useTask(
+		() => {
+			const p = leftHand.current?.targetRay.position
+			if (p && translating) {
+				origin.toZUpPos(leftPinchTranslation, p)
+				origin.set(
+					[
+						leftPinchTranslation.x - startLeftPinchTranslation.x,
+						leftPinchTranslation.y - startLeftPinchTranslation.y,
+						leftPinchTranslation.z - startLeftPinchTranslation.z,
+					],
+					origin.rotation
+				)
+				commit()
+			}
+		},
+		{
+			running: () => translating,
+		}
+	)
+
+	useTask(
+		() => {
+			const p = rightHand.current?.targetRay.position
+			if (p && rotating) {
+				origin.toZUpPos(rightPinchCurrent, p)
+				const deltaX = rightPinchCurrent.x - startRightPinchTranslation.x
+				origin.set(origin.position, startRightPinchRotation + deltaX)
+				commit()
+			}
+		},
+		{
+			running: () => rotating,
+		}
+	)
+</script>
+
+<Hand
+	left
+	onpinchstart={() => {
+		const p = leftHand.current?.targetRay.position
+		if (p) {
+			translating = true
+			origin.toZUpPos(startLeftPinchTranslation, p)
+		}
+	}}
+	onpinchend={() => (translating = false)}
+/>
+
+<Hand
+	right
+	onpinchstart={() => {
+		const p = rightHand.current?.targetRay.position
+		if (p) {
+			rotating = true
+			origin.toZUpPos(startRightPinchTranslation, p)
+			startRightPinchRotation = origin.rotation
+		}
+	}}
+	onpinchend={() => (rotating = false)}
+/>

package/dist/plugins/XR/PendingEditsPanel.svelte

@@ -0,0 +1,60 @@
+<script lang="ts">
+	import { Text } from 'threlte-uikit'
+	import { Button, ButtonLabel, Panel } from 'threlte-uikit/horizon'
+
+	import { usePartConfig } from '../../hooks/usePartConfig.svelte'
+
+	import WristDisplay from './WristDisplay.svelte'
+
+	const partConfig = usePartConfig()
+</script>
+
+{#if partConfig.isDirty}
+	<WristDisplay position={[0, 0.005, 0.1]}>
+		<Panel
+			flexDirection="column"
+			padding={16}
+			gap={12}
+			backgroundColor="#111"
+			borderRadius={16}
+			minWidth={420}
+		>
+			<Text
+				text="Pending frame edits"
+				fontSize={18}
+				color="#ffffff"
+			/>
+			<Panel
+				flexDirection="row"
+				gap={8}
+			>
+				<Button
+					variant="tertiary"
+					size="sm"
+					onclick={() => partConfig.discardChanges()}
+				>
+					<ButtonLabel>
+						<Text
+							text="Discard"
+							fontSize={14}
+							color="#ffffff"
+						/>
+					</ButtonLabel>
+				</Button>
+				<Button
+					variant="primary"
+					size="sm"
+					onclick={() => partConfig.save()}
+				>
+					<ButtonLabel>
+						<Text
+							text="Save"
+							fontSize={14}
+							color="#ffffff"
+						/>
+					</ButtonLabel>
+				</Button>
+			</Panel>
+		</Panel>
+	</WristDisplay>
+{/if}

package/dist/plugins/XR/PendingEditsPanel.svelte.d.ts

@@ -0,0 +1,18 @@
+interface $$__sveltets_2_IsomorphicComponent<Props extends Record<string, any> = any, Events extends Record<string, any> = any, Slots extends Record<string, any> = any, Exports = {}, Bindings = string> {
+    new (options: import('svelte').ComponentConstructorOptions<Props>): import('svelte').SvelteComponent<Props, Events, Slots> & {
+        $$bindings?: Bindings;
+    } & Exports;
+    (internal: unknown, props: {
+        $$events?: Events;
+        $$slots?: Slots;
+    }): Exports & {
+        $set?: any;
+        $on?: any;
+    };
+    z_$$bindings?: Bindings;
+}
+declare const PendingEditsPanel: $$__sveltets_2_IsomorphicComponent<Record<string, never>, {
+    [evt: string]: CustomEvent<any>;
+}, {}, {}, string>;
+type PendingEditsPanel = InstanceType<typeof PendingEditsPanel>;
+export default PendingEditsPanel;

package/dist/plugins/XR/WristDisplay.svelte

@@ -0,0 +1,60 @@
+<script lang="ts">
+	import type { Snippet } from 'svelte'
+
+	import { T } from '@threlte/core'
+	import { useController, useHandJoint } from '@threlte/xr'
+	import { fromStore } from 'svelte/store'
+	import { Group, type Vector3Tuple } from 'three'
+	import { provideDefaultProperties } from 'threlte-uikit'
+
+	interface Props {
+		/** Offset from the wrist in the wrist-local frame, meters. */
+		position?: Vector3Tuple
+		/**
+		 * Rotation in the wrist-local frame. The default orients uikit content
+		 * (panels face their own +Z) onto the dorsal side of the wrist, so the
+		 * user sees it when turning their palm down, like a smartwatch.
+		 */
+		rotation?: Vector3Tuple
+		/** Uniform scale for the wrist group. Smaller than the HUD default
+		 *  because panels live at arm's length instead of ~1 m away. */
+		scale?: number
+		children?: Snippet
+	}
+
+	const {
+		position = [0, 0.005, 0.08],
+		rotation = [-Math.PI / 2, 0, 0],
+		scale = 0.03,
+		children,
+	}: Props = $props()
+
+	// Draw uikit content on top of the scene (real-world depth, selection OBB,
+	// etc.), matching the old HUD behavior.
+	provideDefaultProperties(() => ({
+		depthTest: false,
+		renderOrder: 999,
+	}))
+
+	const leftController = fromStore(useController('left'))
+	const leftWrist = fromStore(useHandJoint('left', 'wrist'))
+
+	// Prefer the hand wrist joint when hand tracking is active; fall back to
+	// the controller grip. Both are three.js Groups updated per frame by
+	// WebXR, so attaching as a child follows the wrist automatically.
+	const parent = $derived(leftWrist.current ?? leftController.current?.grip)
+
+	const group = new Group()
+</script>
+
+{#if parent}
+	<T
+		is={group}
+		attach={parent}
+		{position}
+		{rotation}
+		{scale}
+	>
+		{@render children?.()}
+	</T>
+{/if}

package/dist/plugins/XR/WristDisplay.svelte.d.ts

@@ -0,0 +1,19 @@
+import type { Snippet } from 'svelte';
+import { type Vector3Tuple } from 'three';
+interface Props {
+    /** Offset from the wrist in the wrist-local frame, meters. */
+    position?: Vector3Tuple;
+    /**
+     * Rotation in the wrist-local frame. The default orients uikit content
+     * (panels face their own +Z) onto the dorsal side of the wrist, so the
+     * user sees it when turning their palm down, like a smartwatch.
+     */
+    rotation?: Vector3Tuple;
+    /** Uniform scale for the wrist group. Smaller than the HUD default
+     *  because panels live at arm's length instead of ~1 m away. */
+    scale?: number;
+    children?: Snippet;
+}
+declare const WristDisplay: import("svelte").Component<Props, {}, "">;
+type WristDisplay = ReturnType<typeof WristDisplay>;
+export default WristDisplay;

package/dist/{components/xr → plugins/XR}/XR.svelte

@@ -8,24 +8,29 @@
 	import { useSettings } from '../../hooks/useSettings.svelte'
 
 	import CameraFeed from './CameraFeed.svelte'
+	import DebugPanel from './DebugPanel.svelte'
 	import FrameConfigureControllers from './frame-configure/Controllers.svelte'
 	import JointLimitsWidget from './JointLimitsWidget.svelte'
 	import OriginMarker from './OriginMarker.svelte'
+	import PendingEditsPanel from './PendingEditsPanel.svelte'
 	import TeleopControllers from './teleop/Controllers.svelte'
 	import { provideAnchors } from './useAnchors.svelte'
-	import { useOrigin } from './useOrigin.svelte'
+	import { provideOrigin } from './useOrigin.svelte'
+	import XRPlugins from './XRPlugins.svelte'
 	import XRToast from './XRToast.svelte'
 
 	const { ...rest } = $props()
 
-	const { isPresenting } = useXR()
-	const settings = useSettings()
-	const origin = useOrigin()
+	const origin = provideOrigin()
 	provideAnchors()
-	const enableXR = $derived(settings.current.enableXR)
 
+	const { renderer } = useThrelte()
+	const { isPresenting } = useXR()
+	const settings = useSettings()
 	const partID = usePartID()
 
+	const enableXR = $derived(settings.current.enableXR)
+
 	// Get all enabled camera widgets for the current part
 	const enabledCameras = $derived.by(() => {
 		const openWidgets = settings.current.openCameraWidgets
@@ -48,34 +53,57 @@
 	const leftArmName = $derived(controllerConfig.left.armName)
 	const rightArmName = $derived(controllerConfig.right.armName)
 
-	const { renderer } = useThrelte()
+	// Compose the XR reference space from:
+	//   1) a -π/2 rotation around X to switch from WebXR's Y-up to Viam's Z-up
+	//   2) the scene origin (position + yaw) so the origin's pose lives at the
+	//      composed space's identity. With this, controllers, camera, and scene
+	//      content all share one frame — no separate origin group needed.
+	let baseRefSpace: XRReferenceSpace | undefined
 
-	// Move into Viam's coordinate system. This basically accomplishes
-	// the same thing as setting z up in the Camera component.
 	$effect(() => {
-		if ($isPresenting) {
-			const q = new Quaternion().setFromAxisAngle({ x: 1, y: 0, z: 0 }, -Math.PI / 2)
-
-			// after the XR session has started and a reference space exists:
-			const baseRefSpace = renderer.xr.getReferenceSpace()
-			if (baseRefSpace) {
-				const rotatedRefSpace = baseRefSpace.getOffsetReferenceSpace(
-					new XRRigidTransform({ x: 0, y: 0, z: 0, w: 1 }, { x: q.x, y: q.y, z: q.z, w: q.w })
-				)
+		if (!$isPresenting) {
+			baseRefSpace = undefined
+			return
+		}
 
-				renderer.xr.setReferenceSpace(rotatedRefSpace)
-			}
+		if (!baseRefSpace) {
+			const current = renderer.xr.getReferenceSpace()
+			if (!current) return
+			baseRefSpace = current
 		}
+
+		const [ox, oy, oz] = origin.position
+		const oRot = origin.rotation
+
+		const zUpQ = new Quaternion().setFromAxisAngle({ x: 1, y: 0, z: 0 }, -Math.PI / 2)
+		const originQ = new Quaternion().setFromAxisAngle({ x: 0, y: 0, z: 1 }, oRot)
+
+		const composed = baseRefSpace
+			.getOffsetReferenceSpace(
+				new XRRigidTransform(
+					{ x: 0, y: 0, z: 0, w: 1 },
+					{ x: zUpQ.x, y: zUpQ.y, z: zUpQ.z, w: zUpQ.w }
+				)
+			)
+			.getOffsetReferenceSpace(
+				new XRRigidTransform(
+					{ x: ox, y: oy, z: oz },
+					{ x: originQ.x, y: originQ.y, z: originQ.z, w: originQ.w }
+				)
+			)
+
+		renderer.xr.setReferenceSpace(composed)
 	})
 </script>
 
+{#if $isPresenting}
+	<XRPlugins />
+{/if}
+
 {#if enableXR}
 	<XR
-		onsessionstart={() => {
-			origin.set([-1, -1, 0])
-		}}
 		onsessionend={() => {
-			origin.set([0, 0, 0])
+			origin.set([0, 0, 0], 0)
 		}}
 	>
 		<!-- Render camera feeds only when presenting to avoid conflicting with overlay Camera widgets -->
@@ -115,6 +143,8 @@
 		<!-- <XRConfigPanel offset={{ x: 0, y: 2.5, z: -2.5 }} scale={0.7} /> -->
 
 		<XRToast />
+		<DebugPanel />
+		<PendingEditsPanel />
 
 		{#if settings.current.xrMode === 'arm-teleop'}
 			<TeleopControllers />
@@ -127,6 +157,22 @@
 
 	<XRButton
 		mode="immersive-ar"
+		sessionInit={{
+			optionalFeatures: [
+				'local-floor',
+				'bounded-floor',
+				'anchors',
+				// Required for cross-session persistence — enables
+				// `anchor.requestPersistentHandle()` and
+				// `session.restorePersistentAnchor()` on Quest Browser.
+				'persistent-anchors',
+				'plane-detection',
+				'hand-tracking',
+				'layers',
+				'hit-test',
+			],
+		}}
+		style="color: #555; border-color: #ccc; backdrop-filter: blur(4px); background: rgba(255,255,255,0.5)"
 		{...rest}
 	/>
 {/if}

package/dist/plugins/XR/XRPlugins.svelte

@@ -0,0 +1,9 @@
+<script>
+	import { pointerControls, touchControls } from '@threlte/xr'
+
+	pointerControls('left')
+	pointerControls('right')
+
+	touchControls('left')
+	touchControls('right')
+</script>

package/dist/plugins/XR/XRPlugins.svelte.d.ts

@@ -0,0 +1,26 @@
+export default XRPlugins;
+type XRPlugins = SvelteComponent<{
+    [x: string]: never;
+}, {
+    [evt: string]: CustomEvent<any>;
+}, {}> & {
+    $$bindings?: string | undefined;
+};
+declare const XRPlugins: $$__sveltets_2_IsomorphicComponent<{
+    [x: string]: never;
+}, {
+    [evt: string]: CustomEvent<any>;
+}, {}, {}, string>;
+interface $$__sveltets_2_IsomorphicComponent<Props extends Record<string, any> = any, Events extends Record<string, any> = any, Slots extends Record<string, any> = any, Exports = {}, Bindings = string> {
+    new (options: import("svelte").ComponentConstructorOptions<Props>): import("svelte").SvelteComponent<Props, Events, Slots> & {
+        $$bindings?: Bindings;
+    } & Exports;
+    (internal: unknown, props: {
+        $$events?: Events;
+        $$slots?: Slots;
+    }): Exports & {
+        $set?: any;
+        $on?: any;
+    };
+    z_$$bindings?: Bindings;
+}