@viamrobotics/motion-tools 1.26.1 → 1.27.0

package/dist/FrameConfigUpdater.svelte.js

@@ -1,4 +1,6 @@
 import { hierarchy, traits } from './ecs';
+import { createPose, matrixToPose, poseToMatrix } from './transform';
+const tempPose = createPose();
 export class FrameConfigUpdater {
     updateFrame;
     removeFrame;
@@ -10,19 +12,22 @@ export class FrameConfigUpdater {
         const { x, y, z } = position;
         if (x === undefined && y === undefined && z === undefined)
             return;
-        const change = {};
+        const current = entity.get(traits.EditedMatrix);
+        if (!current)
+            return;
+        matrixToPose(current, tempPose);
         if (x !== undefined)
-            change.x = x;
+            tempPose.x = x;
         if (y !== undefined)
-            change.y = y;
+            tempPose.y = y;
         if (z !== undefined)
-            change.z = z;
-        entity.set(traits.EditedPose, change);
+            tempPose.z = z;
+        poseToMatrix(tempPose, current);
+        entity.changed(traits.EditedMatrix);
         const name = entity.get(traits.Name);
         const parent = hierarchy.getParentName(entity) ?? 'world';
-        const updatedPose = entity.get(traits.EditedPose);
-        if (name && updatedPose) {
-            this.updateFrame(name, parent, updatedPose);
+        if (name) {
+            this.updateFrame(name, parent, { ...tempPose });
         }
     };
     updateLocalOrientation = (entity, orientation) => {
@@ -30,27 +35,32 @@ export class FrameConfigUpdater {
         if (oX === undefined && oY === undefined && oZ === undefined && theta === undefined) {
             return;
         }
-        const change = {};
+        const current = entity.get(traits.EditedMatrix);
+        if (!current)
+            return;
+        matrixToPose(current, tempPose);
         if (oX !== undefined)
-            change.oX = oX;
+            tempPose.oX = oX;
         if (oY !== undefined)
-            change.oY = oY;
+            tempPose.oY = oY;
         if (oZ !== undefined)
-            change.oZ = oZ;
+            tempPose.oZ = oZ;
         if (theta !== undefined)
-            change.theta = theta;
-        entity.set(traits.EditedPose, change);
+            tempPose.theta = theta;
+        poseToMatrix(tempPose, current);
+        entity.changed(traits.EditedMatrix);
         const name = entity.get(traits.Name);
         const parent = hierarchy.getParentName(entity) ?? 'world';
-        const updatedPose = entity.get(traits.EditedPose);
-        if (name && updatedPose) {
-            this.updateFrame(name, parent, updatedPose);
+        if (name) {
+            this.updateFrame(name, parent, { ...tempPose });
         }
     };
     updateGeometry = (entity, geometry) => {
         const name = entity.get(traits.Name);
         const parent = hierarchy.getParentName(entity) ?? 'world';
-        const pose = entity.get(traits.EditedPose);
+        const matrix = entity.get(traits.EditedMatrix);
+        if (matrix)
+            matrixToPose(matrix, tempPose);
         if (geometry?.type === 'box') {
             const { x, y, z } = geometry;
             if (x === undefined && y === undefined && z === undefined)
@@ -64,8 +74,8 @@ export class FrameConfigUpdater {
                 change.z = z;
             entity.set(traits.Box, change);
             const box = entity.get(traits.Box);
-            if (name && box && pose) {
-                this.updateFrame(name, parent, pose, { type: 'box', ...box });
+            if (name && box && matrix) {
+                this.updateFrame(name, parent, { ...tempPose }, { type: 'box', ...box });
             }
         }
         else if (geometry?.type === 'sphere') {
@@ -74,8 +84,8 @@ export class FrameConfigUpdater {
                 return;
             entity.set(traits.Sphere, { r });
             const sphere = entity.get(traits.Sphere);
-            if (name && sphere && pose) {
-                this.updateFrame(name, parent, pose, { type: 'sphere', ...sphere });
+            if (name && sphere && matrix) {
+                this.updateFrame(name, parent, { ...tempPose }, { type: 'sphere', ...sphere });
             }
         }
         else if (geometry?.type === 'capsule') {
@@ -89,16 +99,17 @@ export class FrameConfigUpdater {
                 change.l = l;
             entity.set(traits.Capsule, change);
             const capsule = entity.get(traits.Capsule);
-            if (name && capsule && pose) {
-                this.updateFrame(name, parent, pose, { type: 'capsule', ...capsule });
+            if (name && capsule && matrix) {
+                this.updateFrame(name, parent, { ...tempPose }, { type: 'capsule', ...capsule });
             }
         }
     };
     setFrameParent = (entity, parentName) => {
         const name = entity.get(traits.Name);
-        const pose = entity.get(traits.EditedPose);
-        if (name && pose) {
-            this.updateFrame(name, parentName, pose);
+        const matrix = entity.get(traits.EditedMatrix);
+        if (name && matrix) {
+            matrixToPose(matrix, tempPose);
+            this.updateFrame(name, parentName, { ...tempPose });
         }
     };
     deleteFrame = (entity) => {
@@ -110,9 +121,11 @@ export class FrameConfigUpdater {
     setGeometryType = (entity, type) => {
         const name = entity.get(traits.Name);
         const parent = hierarchy.getParentName(entity) ?? 'world';
-        const pose = entity.get(traits.EditedPose);
-        if (!name || !pose)
+        const matrix = entity.get(traits.EditedMatrix);
+        if (!name || !matrix)
             return;
+        matrixToPose(matrix, tempPose);
+        const pose = { ...tempPose };
         if (type === 'none') {
             this.updateFrame(name, parent, pose, { type: 'none' });
         }

package/dist/assert.d.ts

@@ -0,0 +1,13 @@
+export declare class AssertionError extends Error {
+    constructor(message: string);
+}
+/**
+ * Assert that a value is defined.
+ *
+ * @example
+ * const stringify = (value: number | undefined): number => {
+ *  assertExists(value)
+ *  return `${value}` // TS now knows that value is of type `number`
+ * }
+ */
+export declare const assertExists: <T>(value: T, message: string) => asserts value is NonNullable<T>;

package/dist/assert.js

@@ -0,0 +1,20 @@
+export class AssertionError extends Error {
+    constructor(message) {
+        super(message);
+        this.name = 'AssertionError';
+    }
+}
+/**
+ * Assert that a value is defined.
+ *
+ * @example
+ * const stringify = (value: number | undefined): number => {
+ *  assertExists(value)
+ *  return `${value}` // TS now knows that value is of type `number`
+ * }
+ */
+export const assertExists = (value, message) => {
+    if (value === null || value === undefined) {
+        throw new AssertionError(message);
+    }
+};

package/dist/buf/common/v1/common_pb.d.ts

@@ -855,6 +855,25 @@ export declare class Get3DModelsResponse extends Message<Get3DModelsResponse> {
     static fromJsonString(jsonString: string, options?: Partial<JsonReadOptions>): Get3DModelsResponse;
     static equals(a: Get3DModelsResponse | PlainMessage<Get3DModelsResponse> | undefined, b: Get3DModelsResponse | PlainMessage<Get3DModelsResponse> | undefined): boolean;
 }
+/**
+ * @generated from message viam.common.v1.GetWorldPoseResponse
+ */
+export declare class GetWorldPoseResponse extends Message<GetWorldPoseResponse> {
+    /**
+     * Pose of the component in the world reference frame
+     *
+     * @generated from field: viam.common.v1.Pose pose = 1;
+     */
+    pose?: Pose;
+    constructor(data?: PartialMessage<GetWorldPoseResponse>);
+    static readonly runtime: typeof proto3;
+    static readonly typeName = "viam.common.v1.GetWorldPoseResponse";
+    static readonly fields: FieldList;
+    static fromBinary(bytes: Uint8Array, options?: Partial<BinaryReadOptions>): GetWorldPoseResponse;
+    static fromJson(jsonValue: JsonValue, options?: Partial<JsonReadOptions>): GetWorldPoseResponse;
+    static fromJsonString(jsonString: string, options?: Partial<JsonReadOptions>): GetWorldPoseResponse;
+    static equals(a: GetWorldPoseResponse | PlainMessage<GetWorldPoseResponse> | undefined, b: GetWorldPoseResponse | PlainMessage<GetWorldPoseResponse> | undefined): boolean;
+}
 /**
  * @generated from message viam.common.v1.GetReadingsRequest
  */

package/dist/buf/common/v1/common_pb.js

@@ -1260,6 +1260,38 @@ export class Get3DModelsResponse extends Message {
         return proto3.util.equals(Get3DModelsResponse, a, b);
     }
 }
+/**
+ * @generated from message viam.common.v1.GetWorldPoseResponse
+ */
+export class GetWorldPoseResponse extends Message {
+    /**
+     * Pose of the component in the world reference frame
+     *
+     * @generated from field: viam.common.v1.Pose pose = 1;
+     */
+    pose;
+    constructor(data) {
+        super();
+        proto3.util.initPartial(data, this);
+    }
+    static runtime = proto3;
+    static typeName = "viam.common.v1.GetWorldPoseResponse";
+    static fields = proto3.util.newFieldList(() => [
+        { no: 1, name: "pose", kind: "message", T: Pose },
+    ]);
+    static fromBinary(bytes, options) {
+        return new GetWorldPoseResponse().fromBinary(bytes, options);
+    }
+    static fromJson(jsonValue, options) {
+        return new GetWorldPoseResponse().fromJson(jsonValue, options);
+    }
+    static fromJsonString(jsonString, options) {
+        return new GetWorldPoseResponse().fromJsonString(jsonString, options);
+    }
+    static equals(a, b) {
+        return proto3.util.equals(GetWorldPoseResponse, a, b);
+    }
+}
 /**
  * @generated from message viam.common.v1.GetReadingsRequest
  */

package/dist/components/BatchedArrows.svelte

@@ -2,103 +2,101 @@
 	import type { Entity } from 'koota'
 
 	import { T } from '@threlte/core'
-	import { Portal } from '@threlte/extras'
-	import { Color, Vector3 } from 'three'
+	import { Color, Quaternion, Vector3 } from 'three'
 
-	import { hierarchy, traits, useWorld } from '../ecs'
+	import { traits, useWorld } from '../ecs'
 	import { BatchedArrow } from '../three/BatchedArrow'
+	import { OrientationVector } from '../three/OrientationVector'
 
-	const arrowBatchMap = $state<Record<string, BatchedArrow>>({
-		world: new BatchedArrow(),
-	})
-	const batchEntries = $derived(Object.entries(arrowBatchMap))
+	const batched = new BatchedArrow()
 
 	const world = useWorld()
 
 	const direction = new Vector3()
 	const origin = new Vector3()
 	const color = new Color()
+	const tempQuat = new Quaternion()
+	const tempScale = new Vector3()
+	const tempOv = new OrientationVector()
+
+	/**
+	 * Decompose the entity's `WorldMatrix` directly into the arrow's world
+	 * origin (translation) and direction (OV components from the rotation).
+	 */
+	const decompose = (entity: Entity): boolean => {
+		const worldMatrix = entity.get(traits.WorldMatrix)
+		if (!worldMatrix) return false
+		worldMatrix.decompose(origin, tempQuat, tempScale)
+		tempOv.setFromQuaternion(tempQuat)
+		direction.set(tempOv.x, tempOv.y, tempOv.z)
+		return true
+	}
 
 	const onAdd = (entity: Entity) => {
-		const parent = hierarchy.getParentName(entity) ?? 'world'
-
-		arrowBatchMap[parent] ??= new BatchedArrow()
-		const batched = arrowBatchMap[parent]
-
-		const pose = entity.get(traits.Pose)
 		const colorRGB = entity.get(traits.Color)
 
+		if (!decompose(entity)) {
+			direction.set(0, 0, 0)
+			origin.set(0, 0, 0)
+		}
+
 		const instanceID = batched.addArrow(
-			direction.set(pose?.oX ?? 0, pose?.oY ?? 0, pose?.oZ ?? 0),
-			origin.set(pose?.x ?? 0, pose?.y ?? 0, pose?.z ?? 0).multiplyScalar(0.001),
+			direction,
+			origin,
 			colorRGB ? color.set(colorRGB.r, colorRGB.g, colorRGB.b) : color.set('yellow')
 		)
 
 		entity.add(traits.Instance({ instanceID, meshID: batched.mesh.id }))
 	}
 
-	const onPoseChange = (entity: Entity) => {
+	const onWorldMatrixChange = (entity: Entity) => {
 		if (!entity.has(traits.Arrow)) return
 
-		const parent = hierarchy.getParentName(entity) ?? 'world'
-		const batch = arrowBatchMap[parent]
 		const instanceID = entity.get(traits.Instance)?.instanceID
-		const pose = entity.get(traits.Pose)
-
-		if (instanceID && instanceID !== -1 && pose) {
-			batch?.updateArrow(
-				instanceID,
-				direction.set(pose.oX, pose.oY, pose.oZ),
-				origin.set(pose.x, pose.y, pose.z).multiplyScalar(0.001)
-			)
+
+		if (instanceID && instanceID !== -1 && decompose(entity)) {
+			batched.updateArrow(instanceID, direction, origin)
 		}
 	}
 
 	const onColorChange = (entity: Entity) => {
 		if (!entity.has(traits.Arrow)) return
 
-		const parent = hierarchy.getParentName(entity) ?? 'world'
-		const batch = arrowBatchMap[parent]
 		const instanceID = entity.get(traits.Instance)?.instanceID
 		const colorRGB = entity.get(traits.Color)
 
 		if (instanceID && instanceID !== -1 && colorRGB) {
 			color.set(colorRGB.r, colorRGB.g, colorRGB.b)
-			batch.mesh.setColorAt(instanceID, color)
+			batched.mesh.setColorAt(instanceID, color)
 		}
 	}
 
 	const onInstanceRemove = (entity: Entity) => {
 		const instance = entity.get(traits.Instance)
-
-		for (const [, batch] of batchEntries) {
-			if (batch.mesh.id === instance?.meshID) {
-				batch.removeArrow(instance.instanceID)
-			}
+		if (instance && instance.meshID === batched.mesh.id) {
+			batched.removeArrow(instance.instanceID)
 		}
 	}
 
 	$effect(() => {
 		const unsubAdd = world.onAdd(traits.Arrow, onAdd)
 		const unsubRemove = world.onRemove(traits.Instance, onInstanceRemove)
-		const unsubPoseChange = world.onChange(traits.Pose, onPoseChange)
+		const unsubMatrixAdd = world.onAdd(traits.WorldMatrix, onWorldMatrixChange)
+		const unsubMatrixChange = world.onChange(traits.WorldMatrix, onWorldMatrixChange)
 		const unsubColorChange = world.onChange(traits.Color, onColorChange)
 
 		return () => {
 			unsubAdd()
 			unsubRemove()
-			unsubPoseChange()
+			unsubMatrixAdd()
+			unsubMatrixChange()
 			unsubColorChange()
 		}
 	})
 </script>
 
-{#each batchEntries as [parent, batch] (parent)}
-	<Portal id={parent}>
-		<T
-			is={batch.mesh}
-			dispose={false}
-			bvh={{ enabled: false }}
-		/>
-	</Portal>
-{/each}
+<T
+	is={batched.mesh}
+	dispose={false}
+	bvh={{ enabled: false }}
+/>

package/dist/components/Entities/Arrows/Arrows.svelte

@@ -1,14 +1,13 @@
 <script lang="ts">
 	import type { Entity } from 'koota'
 
-	import { T } from '@threlte/core'
-	import { Portal } from '@threlte/extras'
+	import { T, useThrelte } from '@threlte/core'
 
 	import type { InstancedArrows } from '../../../three/InstancedArrows/InstancedArrows'
 
 	import AxesHelper from '../../AxesHelper.svelte'
 	import { useEntityEvents } from '../hooks/useEntityEvents.svelte'
-	import { traits, useParentName, useTrait } from '../../../ecs'
+	import { traits, useTrait } from '../../../ecs'
 	import { useFocusedEntity, useSelectedEntity } from '../../../hooks/useSelection.svelte'
 	import { meshBoundsRaycast, raycast } from '../../../three/InstancedArrows/raycast'
 
@@ -19,7 +18,8 @@
 
 	let { entity, arrows }: Props = $props()
 
-	const parent = useParentName(() => entity)
+	const { invalidate } = useThrelte()
+	const worldMatrix = useTrait(() => entity, traits.WorldMatrix)
 	const invisible = useTrait(() => entity, traits.Invisible)
 	const showAxesHelper = useTrait(() => entity, traits.ShowAxesHelper)
 
@@ -35,32 +35,38 @@
 		}
 		return meshBoundsRaycast
 	})
+
+	$effect.pre(() => {
+		arrows.matrixAutoUpdate = false
+		if (!worldMatrix.current) return
+		arrows.matrix.copy(worldMatrix.current)
+		arrows.updateMatrixWorld()
+		invalidate()
+	})
 </script>
 
-<Portal id={parent.current}>
+<T
+	is={arrows}
+	name={entity}
+	{...events}
+	raycast={raycastFunction}
+	visible={invisible.current !== true}
+>
 	<T
-		is={arrows}
-		name={entity}
-		{...events}
-		raycast={raycastFunction}
-		visible={invisible.current !== true}
-	>
-		<T
-			is={arrows.headMesh}
-			bvh={{ enabled: false }}
-			raycast={() => null}
-		/>
-		<T
-			is={arrows.shaftMesh}
-			bvh={{ enabled: false }}
-			raycast={() => null}
+		is={arrows.headMesh}
+		bvh={{ enabled: false }}
+		raycast={() => null}
+	/>
+	<T
+		is={arrows.shaftMesh}
+		bvh={{ enabled: false }}
+		raycast={() => null}
+	/>
+	{#if showAxesHelper.current}
+		<AxesHelper
+			name={entity}
+			width={3}
+			length={0.1}
 		/>
-		{#if showAxesHelper.current}
-			<AxesHelper
-				name={entity}
-				width={3}
-				length={0.1}
-			/>
-		{/if}
-	</T>
-</Portal>
+	{/if}
+</T>

package/dist/components/Entities/Entities.svelte

@@ -64,14 +64,9 @@
 
 {#each machineFramesEntities.current as entity (entity)}
 	<Pose {entity}>
-		{#snippet children({ pose })}
-			<Frame
-				{pose}
-				{entity}
-			>
-				<Label text={entity.get(traits.Name)} />
-			</Frame>
-		{/snippet}
+		<Frame {entity}>
+			<Label text={entity.get(traits.Name)} />
+		</Frame>
 	</Pose>
 {/each}
 

package/dist/components/Entities/Frame.svelte

@@ -10,39 +10,34 @@ Renders a Viam Frame object
 </script>
 
 <script lang="ts">
-	import type { Pose } from '@viamrobotics/sdk'
 	import type { Entity } from 'koota'
 	import type { Snippet } from 'svelte'
 
 	import { T, useThrelte } from '@threlte/core'
-	import { Portal, PortalTarget } from '@threlte/extras'
 	import { Group, type Object3D } from 'three'
 
 	import { asColor } from '../../buffer'
 	import { colors, resourceColors } from '../../color'
-	import { traits, useParentName, useTrait } from '../../ecs'
+	import { traits, useTrait } from '../../ecs'
 	import { useResourceByName } from '../../hooks/useResourceByName.svelte'
-	import { poseToObject3d } from '../../transform'
 
 	import { useEntityEvents } from './hooks/useEntityEvents.svelte'
 	import Mesh from './Mesh.svelte'
 
 	interface Props {
 		entity: Entity
-		pose?: Pose
 		children?: Snippet<[{ ref: Object3D }]>
 	}
 
-	let { entity, pose, children }: Props = $props()
+	let { entity, children }: Props = $props()
 
 	const { invalidate } = useThrelte()
 	const resourceByName = useResourceByName()
 
 	const name = useTrait(() => entity, traits.Name)
-	const parent = useParentName(() => entity)
 	const entityColors = useTrait(() => entity, traits.Colors)
 	const entityColor = useTrait(() => entity, traits.Color)
-	const entityPose = useTrait(() => entity, traits.Pose)
+	const worldMatrix = useTrait(() => entity, traits.WorldMatrix)
 	const center = useTrait(() => entity, traits.Center)
 	const invisible = useTrait(() => entity, traits.Invisible)
 
@@ -68,32 +63,36 @@ Renders a Viam Frame object
 	})
 
 	const group = new Group()
+	group.matrixAutoUpdate = false
 
-	const resolvedPose = $derived(pose ?? entityPose.current)
 	$effect.pre(() => {
-		if (resolvedPose) {
-			poseToObject3d(resolvedPose, group)
-			invalidate()
-		}
+		if (!worldMatrix.current) return
+
+		group.matrix.copy(worldMatrix.current)
+
+		/**
+		 * Keep position/quaternion/scale in sync with matrix so TransformControls
+		 * (which reads/writes those fields) sees the entity's actual transform on
+		 * drag start. Without this, the gizmo applies its drag delta against an
+		 * identity baseline and the frame snaps to identity on first onChange.
+		 */
+		group.matrix.decompose(group.position, group.quaternion, group.scale)
+
+		group.updateMatrixWorld()
+		invalidate()
 	})
 </script>
 
-<Portal id={parent.current}>
-	<T
-		is={group}
-		visible={invisible.current !== true}
-	>
-		<Mesh
-			{entity}
-			{color}
-			{...events}
-			center={center.current}
-		/>
-
-		{#if name.current}
-			<PortalTarget id={name.current} />
-		{/if}
-
-		{@render children?.({ ref: group })}
-	</T>
-</Portal>
+<T
+	is={group}
+	visible={invisible.current !== true}
+>
+	<Mesh
+		{entity}
+		{color}
+		{...events}
+		center={center.current}
+	/>
+
+	{@render children?.({ ref: group })}
+</T>

package/dist/components/Entities/Frame.svelte.d.ts

@@ -1,10 +1,8 @@
-import type { Pose } from '@viamrobotics/sdk';
 import type { Entity } from 'koota';
 import type { Snippet } from 'svelte';
 import { type Object3D } from 'three';
 interface Props {
     entity: Entity;
-    pose?: Pose;
     children?: Snippet<[{
         ref: Object3D;
     }]>;