@viamrobotics/motion-tools 0.14.9 → 0.14.11
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
|
@@ -15,13 +15,23 @@
|
|
|
15
15
|
import Label from './Label.svelte'
|
|
16
16
|
import WorldState from './WorldState.svelte'
|
|
17
17
|
import { determinePose } from '../WorldObject.svelte'
|
|
18
|
-
|
|
18
|
+
import { useWeblabs } from '../hooks/useWeblabs.svelte'
|
|
19
|
+
import type { WorldObject } from '../WorldObject.svelte'
|
|
20
|
+
import type { Pose as ViamPose } from '@viamrobotics/sdk'
|
|
19
21
|
const points = usePointClouds()
|
|
20
22
|
const drawAPI = useDrawAPI()
|
|
21
23
|
const frames = useFrames()
|
|
22
24
|
const geometries = useGeometries()
|
|
23
25
|
const worldStates = useWorldStates()
|
|
24
26
|
const batchedArrow = useArrows()
|
|
27
|
+
const weblabs = useWeblabs()
|
|
28
|
+
|
|
29
|
+
const weblabedDeterminePose = (object: WorldObject, pose: ViamPose | undefined) => {
|
|
30
|
+
if (weblabs.isActive('MOTION_TOOLS_EDIT_FRAME')) {
|
|
31
|
+
return determinePose(object, pose)
|
|
32
|
+
}
|
|
33
|
+
return pose ?? object.pose
|
|
34
|
+
}
|
|
25
35
|
</script>
|
|
26
36
|
|
|
27
37
|
{#each frames.current as object (object.uuid)}
|
|
@@ -30,7 +40,7 @@
|
|
|
30
40
|
parent={object.referenceFrame}
|
|
31
41
|
>
|
|
32
42
|
{#snippet children({ pose })}
|
|
33
|
-
{@const framePose =
|
|
43
|
+
{@const framePose = weblabedDeterminePose(object, pose)}
|
|
34
44
|
<Portal id={object.referenceFrame}>
|
|
35
45
|
<Frame
|
|
36
46
|
uuid={object.uuid}
|
|
@@ -196,7 +196,7 @@ export class OrientationVector {
|
|
|
196
196
|
const cosTheta = MathUtils.clamp(cosThetaCand, -1, 1);
|
|
197
197
|
const theta = Math.acos(cosTheta);
|
|
198
198
|
if (theta > EPSILON) {
|
|
199
|
-
const newZImagUnit = vecF.copy(
|
|
199
|
+
const newZImagUnit = vecF.copy(newZimag).normalize();
|
|
200
200
|
const rotQuatUnit = quatD.setFromAxisAngle(newZImagUnit, -1 * theta);
|
|
201
201
|
const conj2 = quatE.copy(rotQuatUnit).conjugate();
|
|
202
202
|
const testZ = rotQuatUnit.multiplyQuaternions(rotQuatUnit.multiply(zAxis), conj2);
|