@viamrobotics/motion-tools 1.9.1 → 1.11.0

package/dist/components/xr/CameraFeed.svelte

@@ -1,16 +1,23 @@
 <script lang="ts">
 	import { T, useTask } from '@threlte/core'
 	import { createStreamClient } from '@viamrobotics/svelte-sdk'
-	import BentPlaneGeometry from '../BentPlaneGeometry.svelte'
-	import { useHeadset } from '@threlte/xr'
-	import { Euler, Group, Mesh, Vector3, Quaternion, VideoTexture } from 'three'
+	import { VideoTexture } from 'three'
 	import { usePartID } from '../../hooks/usePartID.svelte'
+	import BentPlaneGeometry from './BentPlaneGeometry.svelte'
 
 	interface CameraFeedProps {
 		resourceName: string
+		offset?: { x?: number; y?: number; z?: number }
+		scale?: number
+		enableProfiling?: boolean
 	}
 
-	let { resourceName }: CameraFeedProps = $props()
+	let {
+		resourceName,
+		offset = {},
+		scale = 0.7,
+		enableProfiling = false,
+	}: CameraFeedProps = $props()
 
 	const partID = usePartID()
 	const streamClient = createStreamClient(
@@ -21,63 +28,200 @@
 	let video = document.createElement('video')
 	let aspect = $state(1)
 	let ready = $state(false)
+	let texture = $state<VideoTexture | null>(null)
 
-	video.addEventListener('canplaythrough', () => {
-		aspect = video.videoWidth / video.videoHeight
-		video.play()
-	})
+	// ===== LATENCY PROFILING =====
+	interface LatencyMetrics {
+		streamConnectTime?: number
+		videoReadyTime?: number
+		firstFrameTime?: number
+		captureToPresent?: number // Camera capture → browser decode (from metadata)
+		presentToRender?: number // Browser decode → Three.js texture update
+		totalLatency?: number // End-to-end
+		fps?: number
+	}
+	let metrics = $state<LatencyMetrics>({})
+	let frameCount = $state(0)
+	let lastFrameTime = 0
+	let fpsFrames: number[] = []
+	let videoFrameCallbackId: number | null = null
+
+	// Critical: video must autoplay and be muted for streams to work
+	video.autoplay = true
+	video.muted = true
+	video.playsInline = true
+
+	// Low-latency settings for teleoperation
+	// @ts-expect-error - latencyHint is not in standard types but supported by browsers
+	video.latencyHint = 0 // Minimize latency
+	video.disableRemotePlayback = true
 
 	$effect.pre(() => {
-		video.srcObject = streamClient.mediaStream
-		ready = true
-	})
+		const mediaStream = streamClient.mediaStream
+		if (!mediaStream) {
+			ready = false
+			texture?.dispose()
+			texture = null
+			return
+		}
+
+		// PROFILING: Stream connected
+		const streamConnectTime = performance.now()
+		if (enableProfiling) {
+			metrics.streamConnectTime = streamConnectTime
+		}
+
+		video.srcObject = mediaStream
 
-	const headset = useHeadset()
+		// Wait for video to be ready before creating texture
+		const onReady = () => {
+			const videoReadyTime = performance.now()
+			aspect = video.videoWidth / video.videoHeight
 
-	let group = new Group()
-	let mesh = new Mesh()
-	let euler = new Euler()
-	let quaternion = new Quaternion()
-	let direction = new Vector3()
+			if (!texture) {
+				texture = new VideoTexture(video)
+			}
 
-	const { start, stop } = useTask(
-		(delta) => {
-			group.position.lerp(headset.position, delta * 5)
+			// Force play to ensure stream is active
+			video.play().catch((e) => console.warn('Video play failed:', e))
+			ready = true
 
-			headset.getWorldDirection(direction)
-			euler.set(0, Math.atan2(direction.x, direction.z), 0)
-			quaternion.setFromEuler(euler)
-			group.quaternion.slerp(quaternion, delta * 5)
+			// PROFILING: Video ready
+			if (enableProfiling) {
+				metrics.videoReadyTime = videoReadyTime
+				const setupLatency = videoReadyTime - streamConnectTime
+				console.log(
+					`[🎥 ${resourceName}] Ready: ${video.videoWidth}x${video.videoHeight} (setup: ${setupLatency.toFixed(0)}ms)`
+				)
+			}
 
-			mesh.lookAt(headset.position)
-		},
-		{
-			autoStart: false,
+			// Start frame-by-frame profiling using requestVideoFrameCallback
+			startFrameProfiling()
+		}
+
+		const onMetadata = () => {
+			if (video.readyState >= video.HAVE_METADATA) {
+				onReady()
+			}
 		}
-	)
 
-	$effect(() => {
-		if (ready) {
-			start()
+		if (video.readyState >= video.HAVE_METADATA) {
+			onReady()
 		} else {
-			stop()
+			video.addEventListener('loadedmetadata', onMetadata, { once: true })
+		}
+
+		// Cleanup when component unmounts
+		return () => {
+			ready = false
+			video.pause()
+			video.srcObject = null
+			texture?.dispose()
+			texture = null
+			stopFrameProfiling()
+			// Don't stop tracks - let the streamClient manage them
 		}
 	})
 
-	const texture = new VideoTexture(video)
+	// Frame-by-frame profiling using requestVideoFrameCallback
+	function startFrameProfiling() {
+		if (!enableProfiling) {
+			// If profiling disabled, use simple useTask approach
+			return
+		}
+
+		stopFrameProfiling() // Clear any existing callback
+
+		const updateFrame = (now: number, metadata: VideoFrameCallbackMetadata) => {
+			if (!texture || !ready) {
+				stopFrameProfiling()
+				return
+			}
+
+			frameCount++
+
+			// Update texture
+			texture.needsUpdate = true
+
+			// Calculate latency metrics from metadata
+			if (metadata) {
+				// All times in metadata are in microseconds, need to convert to ms
+				// 'now' parameter is DOMHighResTimeStamp in milliseconds
+				const captureTime = metadata.captureTime || metadata.mediaTime
+				const presentationTime = metadata.presentationTime || metadata.expectedDisplayTime
+
+				if (captureTime) {
+					// The times might be in different epochs, so we can only reliably calculate
+					// the difference between capture and presentation
+					if (presentationTime) {
+						// Encoding + Network + Decoding time
+						const captureToPresentMs = (presentationTime - captureTime) / 1000
+						metrics.captureToPresent = captureToPresentMs
+
+						// Time since video element presented the frame to when we render it
+						// This should be very small (< 16ms ideally)
+						const presentMsRelative = presentationTime / 1000
+						const timeSincePresentation = now - presentMsRelative
+
+						// Only use this if the time domains seem aligned (value is reasonable)
+						if (Math.abs(timeSincePresentation) < 1000) {
+							metrics.presentToRender = timeSincePresentation
+							metrics.totalLatency = captureToPresentMs + timeSincePresentation
+						} else {
+							// Time domains don't align - just use capture to present as approximation
+							metrics.presentToRender = undefined
+							metrics.totalLatency = captureToPresentMs
+						}
+					}
+				}
+			}
+
+			// Calculate FPS
+			if (lastFrameTime > 0) {
+				const frameDelta = now - lastFrameTime
+				fpsFrames.push(1000 / frameDelta)
+				if (fpsFrames.length > 30) fpsFrames.shift() // Keep last 30 frames
+				metrics.fps = fpsFrames.reduce((a, b) => a + b, 0) / fpsFrames.length
+			}
+			lastFrameTime = now
+
+			// Log key metrics every 60 frames
+			if (enableProfiling && frameCount % 60 === 0) {
+				const latency = metrics.totalLatency ? `${metrics.totalLatency.toFixed(1)}ms` : 'N/A'
+				const fps = metrics.fps ? `${metrics.fps.toFixed(1)}fps` : 'N/A'
+				const resolution = metadata ? `${metadata.width}x${metadata.height}` : 'N/A'
+				console.log(`[🎥 ${resourceName}] ${resolution} @ ${fps} | Latency: ${latency}`)
+			}
+
+			// Schedule next frame
+			videoFrameCallbackId = video.requestVideoFrameCallback(updateFrame)
+		}
+
+		// Start the callback loop
+		videoFrameCallbackId = video.requestVideoFrameCallback(updateFrame)
+	}
+
+	function stopFrameProfiling() {
+		if (videoFrameCallbackId !== null) {
+			video.cancelVideoFrameCallback(videoFrameCallbackId)
+			videoFrameCallbackId = null
+		}
+	}
+
+	// Fallback: If profiling is disabled, use simple useTask
+	useTask(() => {
+		if (!enableProfiling && texture && ready) {
+			texture.needsUpdate = true
+		}
+	})
 </script>
 
-{#if ready}
-	<T is={group}>
-		<T.Group>
-			<T
-				is={mesh}
-				position={[0, 0, -1.5]}
-				scale={0.7}
-			>
-				<BentPlaneGeometry args={[0.1, aspect, 1, 20, 20]} />
-				<T.MeshBasicMaterial map={texture} />
-			</T>
-		</T.Group>
-	</T>
+{#if ready && texture}
+	<T.Mesh
+		position={[offset.x ?? 0, offset.y ?? 0, offset.z ?? -1.5]}
+		{scale}
+	>
+		<BentPlaneGeometry args={[0.1, aspect, 1, 20, 20]} />
+		<T.MeshBasicMaterial map={texture} />
+	</T.Mesh>
 {/if}

package/dist/components/xr/CameraFeed.svelte.d.ts

@@ -1,5 +1,12 @@
 interface CameraFeedProps {
     resourceName: string;
+    offset?: {
+        x?: number;
+        y?: number;
+        z?: number;
+    };
+    scale?: number;
+    enableProfiling?: boolean;
 }
 declare const CameraFeed: import("svelte").Component<CameraFeedProps, {}, "">;
 type CameraFeed = ReturnType<typeof CameraFeed>;

package/dist/components/xr/Controllers.svelte

@@ -1,45 +1,26 @@
 <script lang="ts">
 	import { Controller } from '@threlte/xr'
-	// import { useGamepad } from '@threlte/extras'
-
-	// import { BaseClient } from '@viamrobotics/sdk'
-
 	import { RigidBody } from '@threlte/rapier'
 	import HandCollider from './HandCollider.svelte'
-	// import { usePartID } from '../../hooks/usePartID.svelte'
-	// import { useResourceNames, useRobotClient } from '@viamrobotics/svelte-sdk'
-
-	// const gamepadLeft = useGamepad({ xr: true, hand: 'left' })
-
-	// const partID = usePartID()
-	// const resources = useResourceNames(() => partID.current)
-	// const robotClient = useRobotClient(() => partID.current)
-	// const resource = $derived(resources.current.find((r) => r.subtype === 'base'))
-	// const baseClient = $derived(
-	// 	robotClient.current && resource ? new BaseClient(robotClient.current, resource.name) : undefined
-	// )
-
-	// const linear = { x: 0, y: 0, z: 0 }
-	// const angular = { x: 0, y: 0, z: 0 }
-
-	// gamepadLeft.squeeze.on('change', (event) => {
-	// 	linear.y = -event.value
-	// 	baseClient?.setPower(linear, angular)
-	// })
-
-	// gamepadLeft.trigger.on('change', (event) => {
-	// 	if (typeof event.value === 'number') {
-	// 		linear.y = event.value
-	// 		baseClient?.setPower(linear, angular)
-	// 	}
-	// })
-
-	// gamepadLeft.thumbstick.on('change', (event) => {
-	// 	if (typeof event.value === 'object') {
-	// 		angular.z = event.value.x
-	// 		baseClient?.setPower(linear, angular)
-	// 	}
-	// })
+	import ArmTeleop from './ArmTeleop.svelte'
+	import { useSettings } from '../../hooks/useSettings.svelte'
+
+	const settings = useSettings()
+
+	// Get controller config from settings
+	const config = $derived(settings.current.xrController)
+
+	// Left controller configuration
+	const leftArmName = $derived(config.left.armName)
+	const leftGripperName = $derived(config.left.gripperName)
+	const leftScaleFactor = $derived(config.left.scaleFactor)
+	const leftRotationEnabled = $derived(config.left.rotationEnabled)
+
+	// Right controller configuration
+	const rightArmName = $derived(config.right.armName)
+	const rightGripperName = $derived(config.right.gripperName)
+	const rightScaleFactor = $derived(config.right.scaleFactor)
+	const rightRotationEnabled = $derived(config.right.rotationEnabled)
 </script>
 
 <Controller left>
@@ -57,3 +38,29 @@
 		</RigidBody>
 	{/snippet}
 </Controller>
+
+<!-- Left Controller Arm Teleop -->
+{#if leftArmName}
+	{#key `${leftArmName}-${leftGripperName}-${leftScaleFactor}-${leftRotationEnabled}`}
+		<ArmTeleop
+			armName={leftArmName}
+			gripperName={leftGripperName}
+			scaleFactor={leftScaleFactor}
+			rotationEnabled={leftRotationEnabled}
+			hand="left"
+		/>
+	{/key}
+{/if}
+
+<!-- Right Controller Arm Teleop -->
+{#if rightArmName}
+	{#key `${rightArmName}-${rightGripperName}-${rightScaleFactor}-${rightRotationEnabled}`}
+		<ArmTeleop
+			armName={rightArmName}
+			gripperName={rightGripperName}
+			scaleFactor={rightScaleFactor}
+			rotationEnabled={rightRotationEnabled}
+			hand="right"
+		/>
+	{/key}
+{/if}

package/dist/components/xr/Controllers.svelte.d.ts

@@ -1,18 +1,3 @@
-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 Controllers: $$__sveltets_2_IsomorphicComponent<Record<string, never>, {
-    [evt: string]: CustomEvent<any>;
-}, {}, {}, string>;
-type Controllers = InstanceType<typeof Controllers>;
+declare const Controllers: import("svelte").Component<Record<string, never>, {}, "">;
+type Controllers = ReturnType<typeof Controllers>;
 export default Controllers;

package/dist/components/xr/Hands.svelte

@@ -1,14 +1,12 @@
 <script lang="ts">
 	import { Hand } from '@threlte/xr'
 
-	console.log('hands')
-
 	const onpinchstart = () => {
-		console.log('start')
+		// Pinch started
 	}
 
 	const onpinchend = () => {
-		console.log('end')
+		// Pinch ended
 	}
 </script>
 

package/dist/components/xr/JointLimitsWidget.svelte

@@ -0,0 +1,209 @@
+<script lang="ts">
+	import { T } from '@threlte/core'
+	import { CanvasTexture, PlaneGeometry } from 'three'
+	import { useArmClient } from '../../hooks/useArmClient.svelte'
+	import { useArmKinematics } from '../../hooks/useArmKinematics.svelte'
+
+	interface JointLimitsWidgetProps {
+		armName: string
+		offset?: { x?: number; y?: number; z?: number }
+		scale?: number
+		rotationY?: number
+	}
+
+	let {
+		armName,
+		offset = {},
+		scale = 0.6,
+		rotationY = -15 * (Math.PI / 180),
+	}: JointLimitsWidgetProps = $props()
+
+	const armClient = useArmClient()
+	const armKinematics = useArmKinematics()
+
+	interface JointLimitData {
+		jointId: string
+		currentPosition: number
+		min: number
+		max: number
+		percentage: number
+		status: 'safe' | 'caution' | 'danger'
+	}
+
+	// Get joint limits and current positions for this arm
+	const jointLimits = $derived(armKinematics.kinematics[armName])
+	const currentPositions = $derived(armClient.currentPositions[armName])
+
+	// Combine limits and positions into display data
+	const jointData = $derived.by((): JointLimitData[] | undefined => {
+		if (!jointLimits || !currentPositions) return undefined
+
+		return jointLimits.map((limit, index) => {
+			const current = currentPositions[index] ?? 0
+			const range = limit.max - limit.min
+			const percentage = range !== 0 ? ((current - limit.min) / range) * 100 : 50
+
+			let status: 'safe' | 'caution' | 'danger'
+			if (percentage < 10 || percentage > 90) {
+				status = 'danger'
+			} else if (percentage < 20 || percentage > 80) {
+				status = 'caution'
+			} else {
+				status = 'safe'
+			}
+
+			return {
+				jointId: limit.id,
+				currentPosition: current,
+				min: limit.min,
+				max: limit.max,
+				percentage,
+				status,
+			}
+		})
+	})
+
+	// Canvas setup
+	const CANVAS_WIDTH = 800
+	const HEADER_HEIGHT = 80
+	const ROW_HEIGHT = 120
+	let canvasHeight = $derived(HEADER_HEIGHT + (jointData?.length ?? 0) * ROW_HEIGHT)
+
+	let canvas: HTMLCanvasElement | undefined = $state()
+	let texture: CanvasTexture | undefined = $state()
+	let geometry: PlaneGeometry | undefined = $state()
+
+	// Initialize canvas
+	$effect(() => {
+		if (!canvas && jointData && jointData.length > 0) {
+			canvas = document.createElement('canvas')
+			canvas.width = CANVAS_WIDTH
+			canvas.height = canvasHeight
+			texture = new CanvasTexture(canvas)
+
+			// Calculate aspect ratio for plane geometry
+			const aspect = CANVAS_WIDTH / canvasHeight
+			// Width in 3D space, height based on aspect
+			geometry = new PlaneGeometry(1.2, 1.2 / aspect)
+		}
+	})
+
+	// Update canvas height when number of joints changes
+	$effect(() => {
+		if (canvas && jointData) {
+			const newHeight = HEADER_HEIGHT + jointData.length * ROW_HEIGHT
+			if (canvas.height !== newHeight) {
+				canvas.height = newHeight
+
+				// Update geometry for new aspect ratio
+				const aspect = CANVAS_WIDTH / newHeight
+				geometry?.dispose()
+				geometry = new PlaneGeometry(1.2, 1.2 / aspect)
+			}
+		}
+	})
+
+	// Render header with arm name
+	function renderHeader(ctx: CanvasRenderingContext2D, width: number) {
+		// Header background
+		ctx.fillStyle = '#0a0a0a'
+		ctx.fillRect(0, 0, width, HEADER_HEIGHT)
+
+		// Arm name
+		ctx.fillStyle = '#ffffff'
+		ctx.font = 'bold 36px monospace'
+		ctx.textBaseline = 'middle'
+		ctx.fillText(armName, 20, HEADER_HEIGHT / 2)
+
+		// Separator line
+		ctx.strokeStyle = '#444444'
+		ctx.lineWidth = 4
+		ctx.beginPath()
+		ctx.moveTo(0, HEADER_HEIGHT)
+		ctx.lineTo(width, HEADER_HEIGHT)
+		ctx.stroke()
+	}
+
+	// Render joint data to canvas
+	function renderJointLimits(
+		ctx: CanvasRenderingContext2D,
+		joints: JointLimitData[],
+		width: number,
+		height: number
+	) {
+		const rowHeight = (height - HEADER_HEIGHT) / joints.length
+
+		joints.forEach((joint, index) => {
+			const y = HEADER_HEIGHT + index * rowHeight
+
+			// Background row
+			ctx.fillStyle = index % 2 === 0 ? '#1a1a1a' : '#222222'
+			ctx.fillRect(0, y, width, rowHeight)
+
+			// Joint label
+			ctx.fillStyle = '#ffffff'
+			ctx.font = 'bold 32px monospace'
+			ctx.textBaseline = 'middle'
+			ctx.fillText(joint.jointId, 20, y + rowHeight / 2)
+
+			// Progress bar dimensions
+			const barX = 240
+			const barY = y + (rowHeight - 60) / 2
+			const barWidth = 360
+			const barHeight = 60
+
+			// Progress bar background
+			ctx.fillStyle = '#333333'
+			ctx.fillRect(barX, barY, barWidth, barHeight)
+
+			// Progress bar fill (colored by status)
+			const fillWidth = barWidth * (joint.percentage / 100)
+			ctx.fillStyle =
+				joint.status === 'danger' ? '#ff4444' : joint.status === 'caution' ? '#ffaa00' : '#44ff44'
+			ctx.fillRect(barX, barY, fillWidth, barHeight)
+
+			// Progress bar border
+			ctx.strokeStyle = '#666666'
+			ctx.lineWidth = 4
+			ctx.strokeRect(barX, barY, barWidth, barHeight)
+
+			// Current value text
+			ctx.fillStyle = '#ffffff'
+			ctx.font = '28px monospace'
+			ctx.fillText(`${joint.currentPosition.toFixed(1)}°`, barX + barWidth + 20, y + rowHeight / 2)
+		})
+	}
+
+	// Update canvas when joint data changes
+	$effect(() => {
+		if (canvas && jointData && jointData.length > 0) {
+			const ctx = canvas.getContext('2d')
+			if (!ctx) return
+
+			// Clear canvas
+			ctx.clearRect(0, 0, canvas.width, canvas.height)
+
+			// Render header with arm name
+			renderHeader(ctx, canvas.width)
+
+			// Render joint limits
+			renderJointLimits(ctx, jointData, canvas.width, canvas.height)
+
+			// Mark texture for update
+			if (texture) {
+				texture.needsUpdate = true
+			}
+		}
+	})
+</script>
+
+{#if texture && geometry && jointData && jointData.length > 0}
+	<T.Mesh
+		position={[offset.x ?? 0, offset.y ?? 1.5, offset.z ?? -2.5]}
+		rotation.y={rotationY}
+		{scale}
+	>
+		<T is={geometry} />
+		<T.MeshBasicMaterial map={texture} />
+	</T.Mesh>
+{/if}

package/dist/components/xr/JointLimitsWidget.svelte.d.ts

@@ -0,0 +1,13 @@
+interface JointLimitsWidgetProps {
+    armName: string;
+    offset?: {
+        x?: number;
+        y?: number;
+        z?: number;
+    };
+    scale?: number;
+    rotationY?: number;
+}
+declare const JointLimitsWidget: import("svelte").Component<JointLimitsWidgetProps, {}, "">;
+type JointLimitsWidget = ReturnType<typeof JointLimitsWidget>;
+export default JointLimitsWidget;

package/dist/components/xr/OriginMarker.svelte

@@ -36,7 +36,6 @@
 	const right = useController('right')
 
 	const leftPad = useGamepad({ xr: true, hand: 'left' })
-	const rightPad = useGamepad({ xr: true, hand: 'right' })
 
 	leftPad.trigger.on('down', () => {
 		const grip = $left?.grip
@@ -51,26 +50,13 @@
 	})
 	leftPad.trigger.on('up', () => (dragging = false))
 
-	rightPad.trigger.on('down', () => {
-		const grip = $right?.grip
-
-		if (!grip) {
-			return
-		}
-
-		rotating = true
-		rotateDown.copy($right?.grip.position)
-		currentDistance = euler.z
-	})
-	rightPad.trigger.on('up', () => (rotating = false))
-
 	const dragTask = useTask(
 		() => {
 			if (!$left || !rigidBody) return
 
 			position.copy($left.grip.position).sub(offset)
 
-			origin.set(position)
+			origin.set([position.x, position.y, position.z])
 
 			rigidBody.setNextKinematicTranslation({ x: position.x, y: position.y, z: position.z })
 		},