@viamrobotics/motion-tools 0.14.7 → 0.14.9

package/dist/hooks/useArmClient.svelte.js

@@ -4,24 +4,23 @@ import { getContext, setContext } from 'svelte';
 const key = Symbol('arm-client-context');
 export const provideArmClient = (partID) => {
     const arms = useResourceNames(partID, 'arm');
-    const clients = $state({});
     const options = { refetchInterval: 500 };
-    const jointPositionsQueries = $state({});
     const names = $derived(arms.current.map((arm) => arm.name));
-    const currentPositions = $derived(Object.fromEntries(Object.entries(jointPositionsQueries).map(([name, query]) => [name, query.data?.values])));
-    $effect(() => {
-        for (const arm of arms.current) {
-            const client = createResourceClient(ArmClient, partID, () => arm.name);
-            if (client.current && !clients[arm.name])
-                clients[arm.name] = { current: client.current };
-        }
-    });
-    $effect(() => {
-        for (const client of Object.values(clients)) {
+    const clients = $derived(arms.current.map((arm) => createResourceClient(ArmClient, partID, () => arm.name)));
+    const jointPositionsQueries = $derived.by(() => {
+        const results = {};
+        for (const client of clients) {
+            if (!client.current)
+                continue;
             const query = createResourceQuery(client, 'getJointPositions', options);
-            jointPositionsQueries[client.current.name] = query.current;
+            results[client.current.name] = query;
         }
+        return results;
     });
+    const currentPositions = $derived(Object.fromEntries(Object.entries(jointPositionsQueries).map(([name, query]) => [
+        name,
+        query.current.data?.values,
+    ])));
     setContext(key, {
         get names() {
             return names;

package/dist/transform.js

@@ -3,6 +3,8 @@ import { Euler, MathUtils, Matrix4, Quaternion, Vector3 } from 'three';
 const quaternion = new Quaternion();
 const euler = new Euler();
 const ov = new OrientationVector();
+const translation = new Vector3();
+const scale = new Vector3();
 export const createPose = (pose) => {
     return {
         x: pose?.x ?? 0,
@@ -90,32 +92,23 @@ export const scaleToDimensions = (scale, geometry) => {
     }
 };
 export const poseToMatrix = (pose) => {
+    ov.set(pose.oX, pose.oY, pose.oZ, MathUtils.degToRad(pose.theta));
+    ov.toQuaternion(quaternion);
     const matrix = new Matrix4();
-    const poseQuaternion = new Quaternion().setFromAxisAngle(new Vector3(pose.oX, pose.oY, pose.oZ), pose.theta * (Math.PI / 180));
-    matrix.makeRotationFromQuaternion(poseQuaternion);
-    matrix.setPosition(new Vector3(pose.x, pose.y, pose.z));
+    matrix.makeRotationFromQuaternion(quaternion);
+    matrix.setPosition(pose.x, pose.y, pose.z);
     return matrix;
 };
 export const matrixToPose = (matrix) => {
     const pose = createPose();
-    const translation = new Vector3();
-    const quaternion = new Quaternion();
-    matrix.decompose(translation, quaternion, new Vector3());
+    matrix.decompose(translation, quaternion, scale);
     pose.x = translation.x;
     pose.y = translation.y;
     pose.z = translation.z;
-    const s = Math.sqrt(1 - quaternion.w * quaternion.w);
-    if (s < 0.000001) {
-        pose.oX = 0;
-        pose.oY = 0;
-        pose.oZ = 1;
-        pose.theta = 0;
-    }
-    else {
-        pose.oX = quaternion.x / s;
-        pose.oY = quaternion.y / s;
-        pose.oZ = quaternion.z / s;
-        pose.theta = Math.acos(quaternion.w) * 2 * (180 / Math.PI);
-    }
+    ov.setFromQuaternion(quaternion);
+    pose.oX = ov.x;
+    pose.oY = ov.y;
+    pose.oZ = ov.z;
+    pose.theta = MathUtils.radToDeg(ov.th);
     return pose;
 };

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/motion-tools",
-  "version": "0.14.7",
+  "version": "0.14.9",
   "description": "Motion visualization with Viam",
   "license": "Apache-2.0",
   "type": "module",