@wandelbots/nova-js 4.3.0-pr.318.c6d70a6 → 4.3.0

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.
Files changed (9) hide show
  1. package/dist/experimental/math/index.d.mts +62 -42
  2. package/dist/experimental/math/index.d.mts.map +1 -1
  3. package/dist/experimental/math/index.mjs +65 -11
  4. package/dist/experimental/math/index.mjs.map +1 -1
  5. package/package.json +1 -1
  6. package/src/experimental/math/index.ts +8 -8
  7. package/src/lib/experimental/math/Pose.ts +67 -10
  8. package/src/lib/experimental/math/Quaternion.ts +33 -2
  9. package/src/lib/experimental/math/{augmentMath.ts → withMath.ts} +3 -3
package/dist/experimental/math/index.d.mts CHANGED
@@ -24,49 +24,26 @@ declare class Pose implements Pose$1 {
24
24
  inverse(): Pose;
25
25
  /** Apply this pose's transform to a point, returning the transformed point. */
26
26
  transformPoint(point: number[]): number[];
27
- /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
28
- isApprox(other: Pose$1, epsilon?: number): boolean;
27
+ /**
28
+ * Compare to another pose using separate position/orientation tolerances,
29
+ * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
30
+ * position, and quaternion angular distance for orientation - not a naive
31
+ * per-component diff, since two rotation vectors can represent nearly
32
+ * identical rotations while differing componentwise near a
33
+ * canonicalization boundary (e.g. close to the +/-pi wraparound).
34
+ */
35
+ isApprox(other: Pose$1, deltaPosition?: number, deltaOrientation?: number): boolean;
36
+ /**
37
+ * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
38
+ * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
39
+ * wb-robotix's `Pose::toCartesian()`.
40
+ */
41
+ toCartesian(): number[];
42
+ /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
43
+ toString(precision?: number): string;
29
44
  toJSON(): Pose$1;
30
45
  }
31
46
  //#endregion
32
- //#region src/lib/experimental/math/augmentMath.d.ts
33
- type PoseKeys = keyof Pose$1;
34
- type IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never] ? [Exclude<PoseKeys, keyof T>] extends [never] ? true : false : false;
35
- type MaxDepth = [unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown];
36
- type AugmentPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : IsExactlyPoseData<T> extends true ? Pose : T extends readonly (infer U)[] ? DeepPoseAugmented<U, [...Depth, unknown]>[] : T extends object ? { [K in keyof T]: DeepPoseAugmented<T[K], [...Depth, unknown]>; } : T;
37
- /**
38
- * Type-level counterpart to `augmentPoses`: recursively replaces any
39
- * `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so
40
- * optional `Pose | undefined` fields keep the `| undefined`), so the static
41
- * type matches what's actually returned at runtime.
42
- */
43
- type DeepPoseAugmented<T, Depth extends readonly unknown[] = []> = T extends unknown ? AugmentPosesCore<T, Depth> : never;
44
- /**
45
- * Recursively walks a parsed API response/request body, upgrading any
46
- * `Pose`-shaped objects in place to `Pose` instances so they gain math
47
- * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
48
- * Used by `augmentMath` for `nova.api.*` responses; exported so it can also
49
- * be applied manually to e.g. websocket messages.
50
- */
51
- declare function augmentPoses<T>(value: T): DeepPoseAugmented<T>;
52
- type WithAugmentedPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : T extends ((...args: infer A) => Promise<infer R>) ? (...args: A) => Promise<DeepPoseAugmented<R>> : T extends object ? { [K in keyof T]: WithAugmentedPosesCore<T[K], [...Depth, unknown]>; } : T;
53
- /**
54
- * Recursively maps every method on `Nova["api"]` (and its nested API groups)
55
- * so calls are typed as returning `DeepPoseAugmented` results, matching what
56
- * `augmentMath` does at runtime.
57
- */
58
- type WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>;
59
- type NovaWithMath = Omit<Nova, "api"> & {
60
- readonly api: WithAugmentedPoses<Nova["api"]>;
61
- };
62
- /**
63
- * Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades
64
- * `Pose`-shaped fields in its response to `Pose` instances (with math
65
- * methods like `multiply`/`inverse`). Returns a new proxied view - does not
66
- * mutate the original `nova` instance.
67
- */
68
- declare function augmentMath(nova: Nova): NovaWithMath;
69
- //#endregion
70
47
  //#region src/lib/experimental/math/Quaternion.d.ts
71
48
  /**
72
49
  * A unit quaternion representing a 3D rotation. Mirrors the subset of
@@ -109,12 +86,55 @@ declare class Quaternion implements QuaternionData {
109
86
  multiply(other: QuaternionData): Quaternion;
110
87
  /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
111
88
  conjugate(): Quaternion;
112
- /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
89
+ /** Inverse: `conjugate()` scaled by `1 / squaredNorm()`, matching `Eigen::Quaternion::inverse()`. Equal to `conjugate()` for unit quaternions. */
113
90
  inverse(): Quaternion;
91
+ /**
92
+ * The angle (in rad, always within [0, pi]) of the rotation that takes
93
+ * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
94
+ */
95
+ angularDistance(other: QuaternionData): number;
114
96
  /** Rotate a 3D vector by this quaternion. */
115
97
  rotateVector(v: number[]): number[];
116
98
  toJSON(): QuaternionData;
117
99
  }
118
100
  //#endregion
119
- export { type DeepPoseAugmented, type NovaWithMath, Pose, type Pose$1 as PoseData, Quaternion, type QuaternionData, augmentMath, augmentPoses };
101
+ //#region src/lib/experimental/math/withMath.d.ts
102
+ type PoseKeys = keyof Pose$1;
103
+ type IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never] ? [Exclude<PoseKeys, keyof T>] extends [never] ? true : false : false;
104
+ type MaxDepth = [unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown];
105
+ type AugmentPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : IsExactlyPoseData<T> extends true ? Pose : T extends readonly (infer U)[] ? DeepPoseAugmented<U, [...Depth, unknown]>[] : T extends object ? { [K in keyof T]: DeepPoseAugmented<T[K], [...Depth, unknown]>; } : T;
106
+ /**
107
+ * Type-level counterpart to `augmentPoses`: recursively replaces any
108
+ * `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so
109
+ * optional `Pose | undefined` fields keep the `| undefined`), so the static
110
+ * type matches what's actually returned at runtime.
111
+ */
112
+ type DeepPoseAugmented<T, Depth extends readonly unknown[] = []> = T extends unknown ? AugmentPosesCore<T, Depth> : never;
113
+ /**
114
+ * Recursively walks a parsed API response/request body, upgrading any
115
+ * `Pose`-shaped objects in place to `Pose` instances so they gain math
116
+ * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
117
+ * Used by `withMath` for `nova.api.*` responses; exported so it can also
118
+ * be applied manually to e.g. websocket messages.
119
+ */
120
+ declare function augmentPoses<T>(value: T): DeepPoseAugmented<T>;
121
+ type WithAugmentedPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : T extends ((...args: infer A) => Promise<infer R>) ? (...args: A) => Promise<DeepPoseAugmented<R>> : T extends object ? { [K in keyof T]: WithAugmentedPosesCore<T[K], [...Depth, unknown]>; } : T;
122
+ /**
123
+ * Recursively maps every method on `Nova["api"]` (and its nested API groups)
124
+ * so calls are typed as returning `DeepPoseAugmented` results, matching what
125
+ * `withMath` does at runtime.
126
+ */
127
+ type WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>;
128
+ type NovaWithMath = Omit<Nova, "api"> & {
129
+ readonly api: WithAugmentedPoses<Nova["api"]>;
130
+ };
131
+ /**
132
+ * Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades
133
+ * `Pose`-shaped fields in its response to `Pose` instances (with math
134
+ * methods like `multiply`/`inverse`). Returns a new proxied view - does not
135
+ * mutate the original `nova` instance.
136
+ */
137
+ declare function withMath(nova: Nova): NovaWithMath;
138
+ //#endregion
139
+ export { type DeepPoseAugmented, type NovaWithMath, Pose, type Pose$1 as PoseData, Quaternion, type QuaternionData, augmentPoses, withMath };
120
140
  //# sourceMappingURL=index.d.mts.map
package/dist/experimental/math/index.d.mts.map CHANGED
@@ -1 +1 @@
1
- {"version":3,"file":"index.d.mts","names":[],"sources":["../../../src/lib/experimental/math/Pose.ts","../../../src/lib/experimental/math/augmentMath.ts","../../../src/lib/experimental/math/Quaternion.ts"],"mappings":";;;;;;;;;cAwBa,gBAAgB;WAClB;WACA;EAGP,YAAA,qBACA;SAMK,KAAK,MAAM,SAAW;SAUtB,YAAY;;;;;;;EAUnB,SAAS,OAAO,SAAW;;EAc3B,WAAW;;EAWX,eAAe;;EAMf,SAAS,OAAO,QAAU;EAc1B,UAAU;;;;KCjGP,iBAAiB;KAEjB,kBAAkB,MAAM,cAAc,GAAG,8BACzC,QAAQ,gBAAgB;KAOxB;KAaA,iBACH,GACA,oCACE,wBAAwB,qBACxB,IACA,kBAAkB,kBAChB,OACA,0BAA0B,OACxB,kBAAkB,OAAO,qBACzB,sBACK,WAAW,IAAI,kBAAkB,EAAE,QAAQ,sBAC9C;;;;;;;KAQE,kBACV,GACA,yCACE,oBAAoB,iBAAiB,GAAG;;;;;;;;iBA2D5B,aAAa,GAAG,OAAO,IAAI,kBAAkB;KAKxD,uBACH,GACA,oCACE,wBAAwB,qBACxB,IACA,eAAc,YAAY,MAAM,cAAc,UACxC,MAAM,MAAM,QAAQ,kBAAkB,MAC1C,sBACK,WAAW,IAAI,uBAAuB,EAAE,QAAQ,sBACnD;;;;;;KAOH,mBAAmB,KAAK,uBAAuB;KAExC,eAAe,KAAK;WACrB,KAAK,mBAAmB;;;;;;;;iBA6BnB,YAAY,MAAM,OAAO;;;;;;;;;;KCzJ7B;EAAmB;EAAW;EAAW;EAAW;;cAInD,sBAAsB;WACxB;WACA;WACA;WACA;EAEG,YAAA,YAAO,YAAO,YAAO;SAO1B,YAAY;;;;;;SASZ,mBAAmB,2BAA2B;;;;;;;;;;;EAwBrD;;EAaA,SAAS,OAAO,iBAAiB;;EAYjC,aAAa;;EAKb,WAAW;;EAKX,aAAa;EAmBb,UAAU"}
1
+ {"version":3,"file":"index.d.mts","names":[],"sources":["../../../src/lib/experimental/math/Pose.ts","../../../src/lib/experimental/math/Quaternion.ts","../../../src/lib/experimental/math/withMath.ts"],"mappings":";;;;;;;;;cA+Ba,gBAAgB;WAClB;WACA;EAGP,YAAA,qBACA;SAkBK,KAAK,MAAM,SAAW;SAUtB,YAAY;;;;;;;EAUnB,SAAS,OAAO,SAAW;;EAc3B,WAAW;;EAWX,eAAe;;;;;;;;;EAaf,SACE,OAAO,QACP,wBACA;;;;;;EA0BF;;EAWA,SAAS;EAKT,UAAU;;;;;;;;;;;KCtJA;EAAmB;EAAW;EAAW;EAAW;;cAInD,sBAAsB;WACxB;WACA;WACA;WACA;EAEG,YAAA,YAAO,YAAO,YAAO;SAO1B,YAAY;;;;;;SASZ,mBAAmB,2BAA2B;;;;;;;;;;;EAwBrD;;EAaA,SAAS,OAAO,iBAAiB;;EAYjC,aAAa;;EAKb,WAAW;;;;;EAoBX,gBAAgB,OAAO;;EAgBvB,aAAa;EAmBb,UAAU;;;;KC3IP,iBAAiB;KAEjB,kBAAkB,MAAM,cAAc,GAAG,8BACzC,QAAQ,gBAAgB;KAOxB;KAaA,iBACH,GACA,oCACE,wBAAwB,qBACxB,IACA,kBAAkB,kBAChB,OACA,0BAA0B,OACxB,kBAAkB,OAAO,qBACzB,sBACK,WAAW,IAAI,kBAAkB,EAAE,QAAQ,sBAC9C;;;;;;;KAQE,kBACV,GACA,yCACE,oBAAoB,iBAAiB,GAAG;;;;;;;;iBA2D5B,aAAa,GAAG,OAAO,IAAI,kBAAkB;KAKxD,uBACH,GACA,oCACE,wBAAwB,qBACxB,IACA,eAAc,YAAY,MAAM,cAAc,UACxC,MAAM,MAAM,QAAQ,kBAAkB,MAC1C,sBACK,WAAW,IAAI,uBAAuB,EAAE,QAAQ,sBACnD;;;;;;KAOH,mBAAmB,KAAK,uBAAuB;KAExC,eAAe,KAAK;WACrB,KAAK,mBAAmB;;;;;;;;iBA6BnB,SAAS,MAAM,OAAO"}
package/dist/experimental/math/index.mjs CHANGED
@@ -64,9 +64,26 @@ var Quaternion = class Quaternion {
64
64
  conjugate() {
65
65
  return new Quaternion(this.w, -this.x, -this.y, -this.z);
66
66
  }
67
- /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
67
+ /** Inverse: `conjugate()` scaled by `1 / squaredNorm()`, matching `Eigen::Quaternion::inverse()`. Equal to `conjugate()` for unit quaternions. */
68
68
  inverse() {
69
- return this.conjugate();
69
+ const squaredNorm = this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z;
70
+ if (squaredNorm < EPSILON) return new Quaternion(0, 0, 0, 0);
71
+ const conjugate = this.conjugate();
72
+ return new Quaternion(conjugate.w / squaredNorm, conjugate.x / squaredNorm, conjugate.y / squaredNorm, conjugate.z / squaredNorm);
73
+ }
74
+ /**
75
+ * The angle (in rad, always within [0, pi]) of the rotation that takes
76
+ * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
77
+ */
78
+ angularDistance(other) {
79
+ const relative = this.multiply({
80
+ w: other.w,
81
+ x: -other.x,
82
+ y: -other.y,
83
+ z: -other.z
84
+ });
85
+ const vecNorm = Math.sqrt(relative.x * relative.x + relative.y * relative.y + relative.z * relative.z);
86
+ return 2 * Math.atan2(vecNorm, Math.abs(relative.w));
70
87
  }
71
88
  /** Rotate a 3D vector by this quaternion. */
72
89
  rotateVector(v) {
@@ -99,6 +116,7 @@ const ZERO_VECTOR = [
99
116
  0,
100
117
  0
101
118
  ];
119
+ const TOLERANCE_MILLI = .001;
102
120
  function addVectors(a, b) {
103
121
  return [
104
122
  (a[0] ?? 0) + (b[0] ?? 0),
@@ -113,6 +131,9 @@ function negateVector(a) {
113
131
  -(a[2] ?? 0)
114
132
  ];
115
133
  }
134
+ function isFiniteVector3(v) {
135
+ return v.length === 3 && v.every((n) => Number.isFinite(n));
136
+ }
116
137
  /**
117
138
  * A `Pose` (position + axis-angle orientation) with methods for composing
118
139
  * and inverting transforms. Instances are plain-data compatible with the
@@ -124,8 +145,10 @@ var Pose = class Pose {
124
145
  position;
125
146
  orientation;
126
147
  constructor(position = ZERO_VECTOR, orientation = ZERO_VECTOR) {
127
- this.position = position;
128
- this.orientation = orientation;
148
+ if (!isFiniteVector3(position)) throw new Error(`Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`);
149
+ if (!isFiniteVector3(orientation)) throw new Error(`Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`);
150
+ this.position = [...position];
151
+ this.orientation = [...orientation];
129
152
  }
130
153
  static from(pose) {
131
154
  if (pose instanceof Pose) return pose;
@@ -158,11 +181,42 @@ var Pose = class Pose {
158
181
  const q = Quaternion.fromRotationVector(this.orientation);
159
182
  return addVectors(this.position, q.rotateVector(point));
160
183
  }
161
- /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
162
- isApprox(other, epsilon = 1e-9) {
184
+ /**
185
+ * Compare to another pose using separate position/orientation tolerances,
186
+ * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
187
+ * position, and quaternion angular distance for orientation - not a naive
188
+ * per-component diff, since two rotation vectors can represent nearly
189
+ * identical rotations while differing componentwise near a
190
+ * canonicalization boundary (e.g. close to the +/-pi wraparound).
191
+ */
192
+ isApprox(other, deltaPosition = TOLERANCE_MILLI, deltaOrientation = TOLERANCE_MILLI) {
163
193
  const otherPosition = other.position ?? ZERO_VECTOR;
164
194
  const otherOrientation = other.orientation ?? ZERO_VECTOR;
165
- return this.position.every((v, i) => Math.abs(v - (otherPosition[i] ?? 0)) <= epsilon) && this.orientation.every((v, i) => Math.abs(v - (otherOrientation[i] ?? 0)) <= epsilon);
195
+ const positionDistance = Math.sqrt(this.position.reduce((sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2, 0));
196
+ const orientationDistance = Quaternion.fromRotationVector(this.orientation).angularDistance(Quaternion.fromRotationVector(otherOrientation));
197
+ return positionDistance <= deltaPosition && orientationDistance <= deltaOrientation;
198
+ }
199
+ /**
200
+ * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
201
+ * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
202
+ * wb-robotix's `Pose::toCartesian()`.
203
+ */
204
+ toCartesian() {
205
+ const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation);
206
+ const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y));
207
+ const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))));
208
+ const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z));
209
+ return [
210
+ ...this.position,
211
+ roll,
212
+ pitch,
213
+ yaw
214
+ ];
215
+ }
216
+ /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
217
+ toString(precision = 6) {
218
+ const fmt = (v) => Number(v.toPrecision(precision));
219
+ return `[${this.position.map(fmt).join(", ")}][${this.orientation.map(fmt).join(", ")}]`;
166
220
  }
167
221
  toJSON() {
168
222
  return {
@@ -172,7 +226,7 @@ var Pose = class Pose {
172
226
  }
173
227
  };
174
228
  //#endregion
175
- //#region src/lib/experimental/math/augmentMath.ts
229
+ //#region src/lib/experimental/math/withMath.ts
176
230
  function isPlainObject(value) {
177
231
  return typeof value === "object" && value !== null && !Array.isArray(value) && !(value instanceof Pose);
178
232
  }
@@ -200,7 +254,7 @@ function augmentPosesInPlace(value) {
200
254
  * Recursively walks a parsed API response/request body, upgrading any
201
255
  * `Pose`-shaped objects in place to `Pose` instances so they gain math
202
256
  * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
203
- * Used by `augmentMath` for `nova.api.*` responses; exported so it can also
257
+ * Used by `withMath` for `nova.api.*` responses; exported so it can also
204
258
  * be applied manually to e.g. websocket messages.
205
259
  */
206
260
  function augmentPoses(value) {
@@ -224,13 +278,13 @@ function wrapWithPoseAugmentation(target) {
224
278
  * methods like `multiply`/`inverse`). Returns a new proxied view - does not
225
279
  * mutate the original `nova` instance.
226
280
  */
227
- function augmentMath(nova) {
281
+ function withMath(nova) {
228
282
  return new Proxy(nova, { get(target, prop) {
229
283
  if (prop === "api") return wrapWithPoseAugmentation(target.api);
230
284
  return Reflect.get(target, prop, target);
231
285
  } });
232
286
  }
233
287
  //#endregion
234
- export { Pose, Quaternion, augmentMath, augmentPoses };
288
+ export { Pose, Quaternion, augmentPoses, withMath };
235
289
 
236
290
  //# sourceMappingURL=index.mjs.map
package/dist/experimental/math/index.mjs.map CHANGED
@@ -1 +1 @@
1
- {"version":3,"file":"index.mjs","names":[],"sources":["../../../src/lib/experimental/math/Quaternion.ts","../../../src/lib/experimental/math/Pose.ts","../../../src/lib/experimental/math/augmentMath.ts"],"sourcesContent":["/**\n * A unit quaternion representing a 3D rotation. Mirrors the subset of\n * `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that\n * `Pose` composition needs: Hamilton product, conjugate/inverse, vector\n * rotation, and axis-angle rotation-vector conversion. Instances are\n * immutable - every method returns a new `Quaternion`.\n */\n\nexport type QuaternionData = { w: number; x: number; y: number; z: number }\n\nconst EPSILON = 1e-12\n\nexport class Quaternion implements QuaternionData {\n readonly w: number\n readonly x: number\n readonly y: number\n readonly z: number\n\n constructor(w = 1, x = 0, y = 0, z = 0) {\n this.w = w\n this.x = x\n this.y = y\n this.z = z\n }\n\n static identity(): Quaternion {\n return new Quaternion()\n }\n\n /**\n * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =\n * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported\n * from wb-robotix's `QuaternionBasePlugin.h`).\n */\n static fromRotationVector(rotationVector: number[]): Quaternion {\n const rx = rotationVector[0] ?? 0\n const ry = rotationVector[1] ?? 0\n const rz = rotationVector[2] ?? 0\n\n const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)\n if (angle < EPSILON) {\n return Quaternion.identity()\n }\n const half = angle / 2\n const s = Math.sin(half) / angle\n return new Quaternion(Math.cos(half), rx * s, ry * s, rz * s)\n }\n\n /**\n * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),\n * matching `Eigen::Quaternion::toRotationVector()` (ported from\n * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`\n * keeps the angle canonically within [0, pi] regardless of which of the\n * two antipodal unit quaternions (this or its negation) represents the\n * rotation, rather than acos's [0, 2*pi] range - important since composed\n * poses need to match the same canonical rotation vector the robot\n * controller itself would report for a pose.\n */\n toRotationVector(): number[] {\n const vecNorm = Math.sqrt(\n this.x * this.x + this.y * this.y + this.z * this.z,\n )\n if (vecNorm < EPSILON) {\n return [0, 0, 0]\n }\n const angle = 2 * Math.atan2(vecNorm, Math.abs(this.w))\n const scale = (this.w >= 0 ? angle : -angle) / vecNorm\n return [this.x * scale, this.y * scale, this.z * scale]\n }\n\n /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */\n multiply(other: QuaternionData): Quaternion {\n const { w: aw, x: ax, y: ay, z: az } = this\n const { w: bw, x: bx, y: by, z: bz } = other\n return new Quaternion(\n aw * bw - ax * bx - ay * by - az * bz,\n aw * bx + ax * bw + ay * bz - az * by,\n aw * by - ax * bz + ay * bw + az * bx,\n aw * bz + ax * by - ay * bx + az * bw,\n )\n }\n\n /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */\n conjugate(): Quaternion {\n return new Quaternion(this.w, -this.x, -this.y, -this.z)\n }\n\n /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */\n inverse(): Quaternion {\n return this.conjugate()\n }\n\n /** Rotate a 3D vector by this quaternion. */\n rotateVector(v: number[]): number[] {\n const vx = v[0] ?? 0\n const vy = v[1] ?? 0\n const vz = v[2] ?? 0\n const { w, x, y, z } = this\n\n // t = 2 * cross(q.xyz, v)\n const tx = 2 * (y * vz - z * vy)\n const ty = 2 * (z * vx - x * vz)\n const tz = 2 * (x * vy - y * vx)\n\n // v' = v + w*t + cross(q.xyz, t)\n return [\n vx + w * tx + (y * tz - z * ty),\n vy + w * ty + (z * tx - x * tz),\n vz + w * tz + (x * ty - y * tx),\n ]\n }\n\n toJSON(): QuaternionData {\n return { w: this.w, x: this.x, y: this.y, z: this.z }\n }\n}\n","import type { Pose as PoseData } from \"@wandelbots/nova-api/v2\"\nimport { Quaternion } from \"./Quaternion.ts\"\n\nconst ZERO_VECTOR = [0, 0, 0]\n\nfunction addVectors(a: number[], b: number[]): number[] {\n return [\n (a[0] ?? 0) + (b[0] ?? 0),\n (a[1] ?? 0) + (b[1] ?? 0),\n (a[2] ?? 0) + (b[2] ?? 0),\n ]\n}\n\nfunction negateVector(a: number[]): number[] {\n return [-(a[0] ?? 0), -(a[1] ?? 0), -(a[2] ?? 0)]\n}\n\n/**\n * A `Pose` (position + axis-angle orientation) with methods for composing\n * and inverting transforms. Instances are plain-data compatible with the\n * wire-format `PoseData` type (own `position`/`orientation` properties only,\n * no enumerable methods), so they can be passed directly back into API calls\n * that expect a pose.\n */\nexport class Pose implements PoseData {\n readonly position: number[]\n readonly orientation: number[]\n\n constructor(\n position: number[] = ZERO_VECTOR,\n orientation: number[] = ZERO_VECTOR,\n ) {\n this.position = position\n this.orientation = orientation\n }\n\n static from(pose: PoseData): Pose {\n if (pose instanceof Pose) {\n return pose\n }\n return new Pose(\n pose.position ?? ZERO_VECTOR,\n pose.orientation ?? ZERO_VECTOR,\n )\n }\n\n static identity(): Pose {\n return new Pose()\n }\n\n /**\n * Compose this pose with `other`, treating `other` as being expressed in\n * this pose's coordinate frame. Equivalent to the homogeneous transform\n * product `this * other`: applying the result to a point is the same as\n * applying `other` first, then `this`.\n */\n multiply(other: PoseData): Pose {\n const q1 = Quaternion.fromRotationVector(this.orientation)\n const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR)\n\n const position = addVectors(\n this.position,\n q1.rotateVector(other.position ?? ZERO_VECTOR),\n )\n const orientation = q1.multiply(q2).toRotationVector()\n\n return new Pose(position, orientation)\n }\n\n /** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */\n inverse(): Pose {\n const qInverse = Quaternion.fromRotationVector(this.orientation).inverse()\n\n const position = qInverse.rotateVector(negateVector(this.position))\n // Negating an axis-angle vector gives the inverse rotation directly.\n const orientation = negateVector(this.orientation)\n\n return new Pose(position, orientation)\n }\n\n /** Apply this pose's transform to a point, returning the transformed point. */\n transformPoint(point: number[]): number[] {\n const q = Quaternion.fromRotationVector(this.orientation)\n return addVectors(this.position, q.rotateVector(point))\n }\n\n /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */\n isApprox(other: PoseData, epsilon = 1e-9): boolean {\n const otherPosition = other.position ?? ZERO_VECTOR\n const otherOrientation = other.orientation ?? ZERO_VECTOR\n\n return (\n this.position.every(\n (v, i) => Math.abs(v - (otherPosition[i] ?? 0)) <= epsilon,\n ) &&\n this.orientation.every(\n (v, i) => Math.abs(v - (otherOrientation[i] ?? 0)) <= epsilon,\n )\n )\n }\n\n toJSON(): PoseData {\n return { position: this.position, orientation: this.orientation }\n }\n}\n","import type { Pose as PoseData } from \"@wandelbots/nova-api/v2\"\nimport type { Nova } from \"../../Nova.ts\"\nimport { Pose } from \"./Pose.ts\"\n\ntype PoseKeys = keyof PoseData\n\ntype IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never]\n ? [Exclude<PoseKeys, keyof T>] extends [never]\n ? true\n : false\n : false\n\n// Depth cap avoids \"type instantiation is excessively deep\" on nova-api's\n// large (and possibly cyclic, e.g. compound colliders) generated type graph.\ntype MaxDepth = [\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n]\n\ntype AugmentPosesCore<\n T,\n Depth extends readonly unknown[],\n> = Depth[\"length\"] extends MaxDepth[\"length\"]\n ? T\n : IsExactlyPoseData<T> extends true\n ? Pose\n : T extends readonly (infer U)[]\n ? DeepPoseAugmented<U, [...Depth, unknown]>[]\n : T extends object\n ? { [K in keyof T]: DeepPoseAugmented<T[K], [...Depth, unknown]> }\n : T\n\n/**\n * Type-level counterpart to `augmentPoses`: recursively replaces any\n * `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so\n * optional `Pose | undefined` fields keep the `| undefined`), so the static\n * type matches what's actually returned at runtime.\n */\nexport type DeepPoseAugmented<\n T,\n Depth extends readonly unknown[] = [],\n> = T extends unknown ? AugmentPosesCore<T, Depth> : never\n\nfunction isPlainObject(value: unknown): value is Record<string, unknown> {\n return (\n typeof value === \"object\" &&\n value !== null &&\n !Array.isArray(value) &&\n !(value instanceof Pose)\n )\n}\n\nfunction isNumberTriple(value: unknown): value is number[] {\n return (\n Array.isArray(value) &&\n value.length === 3 &&\n value.every((n) => typeof n === \"number\")\n )\n}\n\n/**\n * Structural check for the wire shape of `Pose` (`{ position, orientation }`,\n * each a 3-number array, and nothing else). This API has no other type with\n * this exact shape, so it reliably identifies `Pose` values without needing\n * per-endpoint knowledge of which fields are poses.\n */\nfunction isPoseShape(value: Record<string, unknown>): boolean {\n return (\n Object.keys(value).length === 2 &&\n isNumberTriple(value.position) &&\n isNumberTriple(value.orientation)\n )\n}\n\nfunction augmentPosesInPlace(value: unknown): void {\n if (Array.isArray(value)) {\n for (const item of value) {\n augmentPosesInPlace(item)\n }\n return\n }\n\n if (isPlainObject(value)) {\n if (isPoseShape(value)) {\n Object.setPrototypeOf(value, Pose.prototype)\n } else {\n for (const key of Object.keys(value)) {\n augmentPosesInPlace(value[key])\n }\n }\n }\n}\n\n/**\n * Recursively walks a parsed API response/request body, upgrading any\n * `Pose`-shaped objects in place to `Pose` instances so they gain math\n * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.\n * Used by `augmentMath` for `nova.api.*` responses; exported so it can also\n * be applied manually to e.g. websocket messages.\n */\nexport function augmentPoses<T>(value: T): DeepPoseAugmented<T> {\n augmentPosesInPlace(value)\n return value as DeepPoseAugmented<T>\n}\n\ntype WithAugmentedPosesCore<\n T,\n Depth extends readonly unknown[],\n> = Depth[\"length\"] extends MaxDepth[\"length\"]\n ? T\n : T extends (...args: infer A) => Promise<infer R>\n ? (...args: A) => Promise<DeepPoseAugmented<R>>\n : T extends object\n ? { [K in keyof T]: WithAugmentedPosesCore<T[K], [...Depth, unknown]> }\n : T\n\n/**\n * Recursively maps every method on `Nova[\"api\"]` (and its nested API groups)\n * so calls are typed as returning `DeepPoseAugmented` results, matching what\n * `augmentMath` does at runtime.\n */\ntype WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>\n\nexport type NovaWithMath = Omit<Nova, \"api\"> & {\n readonly api: WithAugmentedPoses<Nova[\"api\"]>\n}\n\nfunction wrapWithPoseAugmentation<T extends object>(target: T): T {\n return new Proxy(target, {\n get(t, prop) {\n const value = Reflect.get(t, prop, t)\n if (typeof value === \"function\") {\n return (...args: unknown[]) => {\n const result = (value as (...a: unknown[]) => unknown).apply(t, args)\n return result instanceof Promise\n ? result.then((data) => augmentPoses(data))\n : result\n }\n }\n if (value !== null && typeof value === \"object\") {\n return wrapWithPoseAugmentation(value)\n }\n return value\n },\n }) as T\n}\n\n/**\n * Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades\n * `Pose`-shaped fields in its response to `Pose` instances (with math\n * methods like `multiply`/`inverse`). Returns a new proxied view - does not\n * mutate the original `nova` instance.\n */\nexport function augmentMath(nova: Nova): NovaWithMath {\n const proxy = new Proxy(nova, {\n get(target, prop) {\n if (prop === \"api\") {\n return wrapWithPoseAugmentation(target.api)\n }\n // receiver is `target`, not the proxy, so `this` inside Nova's own\n // methods stays bound to the original (un-proxied) instance.\n return Reflect.get(target, prop, target)\n },\n })\n\n return proxy as unknown as NovaWithMath\n}\n"],"mappings":";AAUA,MAAM,UAAU;AAEhB,IAAa,aAAb,MAAa,WAAqC;CAChD;CACA;CACA;CACA;CAEA,YAAY,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG;EACtC,KAAK,IAAI;EACT,KAAK,IAAI;EACT,KAAK,IAAI;EACT,KAAK,IAAI;CACX;CAEA,OAAO,WAAuB;EAC5B,OAAO,IAAI,WAAW;CACxB;;;;;;CAOA,OAAO,mBAAmB,gBAAsC;EAC9D,MAAM,KAAK,eAAe,MAAM;EAChC,MAAM,KAAK,eAAe,MAAM;EAChC,MAAM,KAAK,eAAe,MAAM;EAEhC,MAAM,QAAQ,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,EAAE;EACnD,IAAI,QAAQ,SACV,OAAO,WAAW,SAAS;EAE7B,MAAM,OAAO,QAAQ;EACrB,MAAM,IAAI,KAAK,IAAI,IAAI,IAAI;EAC3B,OAAO,IAAI,WAAW,KAAK,IAAI,IAAI,GAAG,KAAK,GAAG,KAAK,GAAG,KAAK,CAAC;CAC9D;;;;;;;;;;;CAYA,mBAA6B;EAC3B,MAAM,UAAU,KAAK,KACnB,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,CACpD;EACA,IAAI,UAAU,SACZ,OAAO;GAAC;GAAG;GAAG;EAAC;EAEjB,MAAM,QAAQ,IAAI,KAAK,MAAM,SAAS,KAAK,IAAI,KAAK,CAAC,CAAC;EACtD,MAAM,SAAS,KAAK,KAAK,IAAI,QAAQ,CAAC,SAAS;EAC/C,OAAO;GAAC,KAAK,IAAI;GAAO,KAAK,IAAI;GAAO,KAAK,IAAI;EAAK;CACxD;;CAGA,SAAS,OAAmC;EAC1C,MAAM,EAAE,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,OAAO;EACvC,MAAM,EAAE,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,OAAO;EACvC,OAAO,IAAI,WACT,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,EACrC;CACF;;CAGA,YAAwB;EACtB,OAAO,IAAI,WAAW,KAAK,GAAG,CAAC,KAAK,GAAG,CAAC,KAAK,GAAG,CAAC,KAAK,CAAC;CACzD;;CAGA,UAAsB;EACpB,OAAO,KAAK,UAAU;CACxB;;CAGA,aAAa,GAAuB;EAClC,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,EAAE,GAAG,GAAG,GAAG,MAAM;EAGvB,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAG7B,OAAO;GACL,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;GAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;GAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;EAC9B;CACF;CAEA,SAAyB;EACvB,OAAO;GAAE,GAAG,KAAK;GAAG,GAAG,KAAK;GAAG,GAAG,KAAK;GAAG,GAAG,KAAK;EAAE;CACtD;AACF;;;AChHA,MAAM,cAAc;CAAC;CAAG;CAAG;AAAC;AAE5B,SAAS,WAAW,GAAa,GAAuB;CACtD,OAAO;GACJ,EAAE,MAAM,MAAM,EAAE,MAAM;GACtB,EAAE,MAAM,MAAM,EAAE,MAAM;GACtB,EAAE,MAAM,MAAM,EAAE,MAAM;CACzB;AACF;AAEA,SAAS,aAAa,GAAuB;CAC3C,OAAO;EAAC,EAAE,EAAE,MAAM;EAAI,EAAE,EAAE,MAAM;EAAI,EAAE,EAAE,MAAM;CAAE;AAClD;;;;;;;;AASA,IAAa,OAAb,MAAa,KAAyB;CACpC;CACA;CAEA,YACE,WAAqB,aACrB,cAAwB,aACxB;EACA,KAAK,WAAW;EAChB,KAAK,cAAc;CACrB;CAEA,OAAO,KAAK,MAAsB;EAChC,IAAI,gBAAgB,MAClB,OAAO;EAET,OAAO,IAAI,KACT,KAAK,YAAY,aACjB,KAAK,eAAe,WACtB;CACF;CAEA,OAAO,WAAiB;EACtB,OAAO,IAAI,KAAK;CAClB;;;;;;;CAQA,SAAS,OAAuB;EAC9B,MAAM,KAAK,WAAW,mBAAmB,KAAK,WAAW;EACzD,MAAM,KAAK,WAAW,mBAAmB,MAAM,eAAe,WAAW;EAEzE,MAAM,WAAW,WACf,KAAK,UACL,GAAG,aAAa,MAAM,YAAY,WAAW,CAC/C;EACA,MAAM,cAAc,GAAG,SAAS,EAAE,CAAC,CAAC,iBAAiB;EAErD,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,UAAgB;EAGd,MAAM,WAFW,WAAW,mBAAmB,KAAK,WAAW,CAAC,CAAC,QAEzC,CAAC,CAAC,aAAa,aAAa,KAAK,QAAQ,CAAC;EAElE,MAAM,cAAc,aAAa,KAAK,WAAW;EAEjD,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,eAAe,OAA2B;EACxC,MAAM,IAAI,WAAW,mBAAmB,KAAK,WAAW;EACxD,OAAO,WAAW,KAAK,UAAU,EAAE,aAAa,KAAK,CAAC;CACxD;;CAGA,SAAS,OAAiB,UAAU,MAAe;EACjD,MAAM,gBAAgB,MAAM,YAAY;EACxC,MAAM,mBAAmB,MAAM,eAAe;EAE9C,OACE,KAAK,SAAS,OACX,GAAG,MAAM,KAAK,IAAI,KAAK,cAAc,MAAM,EAAE,KAAK,OACrD,KACA,KAAK,YAAY,OACd,GAAG,MAAM,KAAK,IAAI,KAAK,iBAAiB,MAAM,EAAE,KAAK,OACxD;CAEJ;CAEA,SAAmB;EACjB,OAAO;GAAE,UAAU,KAAK;GAAU,aAAa,KAAK;EAAY;CAClE;AACF;;;ACrDA,SAAS,cAAc,OAAkD;CACvE,OACE,OAAO,UAAU,YACjB,UAAU,QACV,CAAC,MAAM,QAAQ,KAAK,KACpB,EAAE,iBAAiB;AAEvB;AAEA,SAAS,eAAe,OAAmC;CACzD,OACE,MAAM,QAAQ,KAAK,KACnB,MAAM,WAAW,KACjB,MAAM,OAAO,MAAM,OAAO,MAAM,QAAQ;AAE5C;;;;;;;AAQA,SAAS,YAAY,OAAyC;CAC5D,OACE,OAAO,KAAK,KAAK,CAAC,CAAC,WAAW,KAC9B,eAAe,MAAM,QAAQ,KAC7B,eAAe,MAAM,WAAW;AAEpC;AAEA,SAAS,oBAAoB,OAAsB;CACjD,IAAI,MAAM,QAAQ,KAAK,GAAG;EACxB,KAAK,MAAM,QAAQ,OACjB,oBAAoB,IAAI;EAE1B;CACF;CAEA,IAAI,cAAc,KAAK,GACrB,IAAI,YAAY,KAAK,GACnB,OAAO,eAAe,OAAO,KAAK,SAAS;MAE3C,KAAK,MAAM,OAAO,OAAO,KAAK,KAAK,GACjC,oBAAoB,MAAM,IAAI;AAItC;;;;;;;;AASA,SAAgB,aAAgB,OAAgC;CAC9D,oBAAoB,KAAK;CACzB,OAAO;AACT;AAwBA,SAAS,yBAA2C,QAAc;CAChE,OAAO,IAAI,MAAM,QAAQ,EACvB,IAAI,GAAG,MAAM;EACX,MAAM,QAAQ,QAAQ,IAAI,GAAG,MAAM,CAAC;EACpC,IAAI,OAAO,UAAU,YACnB,QAAQ,GAAG,SAAoB;GAC7B,MAAM,SAAU,MAAuC,MAAM,GAAG,IAAI;GACpE,OAAO,kBAAkB,UACrB,OAAO,MAAM,SAAS,aAAa,IAAI,CAAC,IACxC;EACN;EAEF,IAAI,UAAU,QAAQ,OAAO,UAAU,UACrC,OAAO,yBAAyB,KAAK;EAEvC,OAAO;CACT,EACF,CAAC;AACH;;;;;;;AAQA,SAAgB,YAAY,MAA0B;CAYpD,OAAO,IAXW,MAAM,MAAM,EAC5B,IAAI,QAAQ,MAAM;EAChB,IAAI,SAAS,OACX,OAAO,yBAAyB,OAAO,GAAG;EAI5C,OAAO,QAAQ,IAAI,QAAQ,MAAM,MAAM;CACzC,EACF,CAEW;AACb"}
1
+ {"version":3,"file":"index.mjs","names":[],"sources":["../../../src/lib/experimental/math/Quaternion.ts","../../../src/lib/experimental/math/Pose.ts","../../../src/lib/experimental/math/withMath.ts"],"sourcesContent":["/**\n * A unit quaternion representing a 3D rotation. Mirrors the subset of\n * `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that\n * `Pose` composition needs: Hamilton product, conjugate/inverse, vector\n * rotation, and axis-angle rotation-vector conversion. Instances are\n * immutable - every method returns a new `Quaternion`.\n */\n\nexport type QuaternionData = { w: number; x: number; y: number; z: number }\n\nconst EPSILON = 1e-12\n\nexport class Quaternion implements QuaternionData {\n readonly w: number\n readonly x: number\n readonly y: number\n readonly z: number\n\n constructor(w = 1, x = 0, y = 0, z = 0) {\n this.w = w\n this.x = x\n this.y = y\n this.z = z\n }\n\n static identity(): Quaternion {\n return new Quaternion()\n }\n\n /**\n * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =\n * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported\n * from wb-robotix's `QuaternionBasePlugin.h`).\n */\n static fromRotationVector(rotationVector: number[]): Quaternion {\n const rx = rotationVector[0] ?? 0\n const ry = rotationVector[1] ?? 0\n const rz = rotationVector[2] ?? 0\n\n const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)\n if (angle < EPSILON) {\n return Quaternion.identity()\n }\n const half = angle / 2\n const s = Math.sin(half) / angle\n return new Quaternion(Math.cos(half), rx * s, ry * s, rz * s)\n }\n\n /**\n * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),\n * matching `Eigen::Quaternion::toRotationVector()` (ported from\n * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`\n * keeps the angle canonically within [0, pi] regardless of which of the\n * two antipodal unit quaternions (this or its negation) represents the\n * rotation, rather than acos's [0, 2*pi] range - important since composed\n * poses need to match the same canonical rotation vector the robot\n * controller itself would report for a pose.\n */\n toRotationVector(): number[] {\n const vecNorm = Math.sqrt(\n this.x * this.x + this.y * this.y + this.z * this.z,\n )\n if (vecNorm < EPSILON) {\n return [0, 0, 0]\n }\n const angle = 2 * Math.atan2(vecNorm, Math.abs(this.w))\n const scale = (this.w >= 0 ? angle : -angle) / vecNorm\n return [this.x * scale, this.y * scale, this.z * scale]\n }\n\n /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */\n multiply(other: QuaternionData): Quaternion {\n const { w: aw, x: ax, y: ay, z: az } = this\n const { w: bw, x: bx, y: by, z: bz } = other\n return new Quaternion(\n aw * bw - ax * bx - ay * by - az * bz,\n aw * bx + ax * bw + ay * bz - az * by,\n aw * by - ax * bz + ay * bw + az * bx,\n aw * bz + ax * by - ay * bx + az * bw,\n )\n }\n\n /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */\n conjugate(): Quaternion {\n return new Quaternion(this.w, -this.x, -this.y, -this.z)\n }\n\n /** Inverse: `conjugate()` scaled by `1 / squaredNorm()`, matching `Eigen::Quaternion::inverse()`. Equal to `conjugate()` for unit quaternions. */\n inverse(): Quaternion {\n const squaredNorm =\n this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z\n // Degenerate (zero) quaternion has no inverse - flag it the same way Eigen does.\n if (squaredNorm < EPSILON) {\n return new Quaternion(0, 0, 0, 0)\n }\n const conjugate = this.conjugate()\n return new Quaternion(\n conjugate.w / squaredNorm,\n conjugate.x / squaredNorm,\n conjugate.y / squaredNorm,\n conjugate.z / squaredNorm,\n )\n }\n\n /**\n * The angle (in rad, always within [0, pi]) of the rotation that takes\n * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.\n */\n angularDistance(other: QuaternionData): number {\n const relative = this.multiply({\n w: other.w,\n x: -other.x,\n y: -other.y,\n z: -other.z,\n })\n const vecNorm = Math.sqrt(\n relative.x * relative.x +\n relative.y * relative.y +\n relative.z * relative.z,\n )\n return 2 * Math.atan2(vecNorm, Math.abs(relative.w))\n }\n\n /** Rotate a 3D vector by this quaternion. */\n rotateVector(v: number[]): number[] {\n const vx = v[0] ?? 0\n const vy = v[1] ?? 0\n const vz = v[2] ?? 0\n const { w, x, y, z } = this\n\n // t = 2 * cross(q.xyz, v)\n const tx = 2 * (y * vz - z * vy)\n const ty = 2 * (z * vx - x * vz)\n const tz = 2 * (x * vy - y * vx)\n\n // v' = v + w*t + cross(q.xyz, t)\n return [\n vx + w * tx + (y * tz - z * ty),\n vy + w * ty + (z * tx - x * tz),\n vz + w * tz + (x * ty - y * tx),\n ]\n }\n\n toJSON(): QuaternionData {\n return { w: this.w, x: this.x, y: this.y, z: this.z }\n }\n}\n","import type { Pose as PoseData } from \"@wandelbots/nova-api/v2\"\nimport { Quaternion } from \"./Quaternion.ts\"\n\nconst ZERO_VECTOR = [0, 0, 0]\n\n// Matches wb-robotix's Constants.h TOLERANCE_MILLI, the default isApprox tolerance.\nconst TOLERANCE_MILLI = 1e-3\n\nfunction addVectors(a: number[], b: number[]): number[] {\n return [\n (a[0] ?? 0) + (b[0] ?? 0),\n (a[1] ?? 0) + (b[1] ?? 0),\n (a[2] ?? 0) + (b[2] ?? 0),\n ]\n}\n\nfunction negateVector(a: number[]): number[] {\n return [-(a[0] ?? 0), -(a[1] ?? 0), -(a[2] ?? 0)]\n}\n\nfunction isFiniteVector3(v: number[]): boolean {\n return v.length === 3 && v.every((n) => Number.isFinite(n))\n}\n\n/**\n * A `Pose` (position + axis-angle orientation) with methods for composing\n * and inverting transforms. Instances are plain-data compatible with the\n * wire-format `PoseData` type (own `position`/`orientation` properties only,\n * no enumerable methods), so they can be passed directly back into API calls\n * that expect a pose.\n */\nexport class Pose implements PoseData {\n readonly position: number[]\n readonly orientation: number[]\n\n constructor(\n position: number[] = ZERO_VECTOR,\n orientation: number[] = ZERO_VECTOR,\n ) {\n if (!isFiniteVector3(position)) {\n throw new Error(\n `Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`,\n )\n }\n if (!isFiniteVector3(orientation)) {\n throw new Error(\n `Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`,\n )\n }\n // Copy defensively so mutating the caller's arrays afterward can't\n // change this (supposedly immutable) instance's state.\n this.position = [...position]\n this.orientation = [...orientation]\n }\n\n static from(pose: PoseData): Pose {\n if (pose instanceof Pose) {\n return pose\n }\n return new Pose(\n pose.position ?? ZERO_VECTOR,\n pose.orientation ?? ZERO_VECTOR,\n )\n }\n\n static identity(): Pose {\n return new Pose()\n }\n\n /**\n * Compose this pose with `other`, treating `other` as being expressed in\n * this pose's coordinate frame. Equivalent to the homogeneous transform\n * product `this * other`: applying the result to a point is the same as\n * applying `other` first, then `this`.\n */\n multiply(other: PoseData): Pose {\n const q1 = Quaternion.fromRotationVector(this.orientation)\n const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR)\n\n const position = addVectors(\n this.position,\n q1.rotateVector(other.position ?? ZERO_VECTOR),\n )\n const orientation = q1.multiply(q2).toRotationVector()\n\n return new Pose(position, orientation)\n }\n\n /** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */\n inverse(): Pose {\n const qInverse = Quaternion.fromRotationVector(this.orientation).inverse()\n\n const position = qInverse.rotateVector(negateVector(this.position))\n // Negating an axis-angle vector gives the inverse rotation directly.\n const orientation = negateVector(this.orientation)\n\n return new Pose(position, orientation)\n }\n\n /** Apply this pose's transform to a point, returning the transformed point. */\n transformPoint(point: number[]): number[] {\n const q = Quaternion.fromRotationVector(this.orientation)\n return addVectors(this.position, q.rotateVector(point))\n }\n\n /**\n * Compare to another pose using separate position/orientation tolerances,\n * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for\n * position, and quaternion angular distance for orientation - not a naive\n * per-component diff, since two rotation vectors can represent nearly\n * identical rotations while differing componentwise near a\n * canonicalization boundary (e.g. close to the +/-pi wraparound).\n */\n isApprox(\n other: PoseData,\n deltaPosition = TOLERANCE_MILLI,\n deltaOrientation = TOLERANCE_MILLI,\n ): boolean {\n const otherPosition = other.position ?? ZERO_VECTOR\n const otherOrientation = other.orientation ?? ZERO_VECTOR\n\n const positionDistance = Math.sqrt(\n this.position.reduce(\n (sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2,\n 0,\n ),\n )\n const orientationDistance = Quaternion.fromRotationVector(\n this.orientation,\n ).angularDistance(Quaternion.fromRotationVector(otherOrientation))\n\n return (\n positionDistance <= deltaPosition &&\n orientationDistance <= deltaOrientation\n )\n }\n\n /**\n * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector\n * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching\n * wb-robotix's `Pose::toCartesian()`.\n */\n toCartesian(): number[] {\n const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation)\n\n const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y))\n const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))))\n const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z))\n\n return [...this.position, roll, pitch, yaw]\n }\n\n /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */\n toString(precision = 6): string {\n const fmt = (v: number) => Number(v.toPrecision(precision))\n return `[${this.position.map(fmt).join(\", \")}][${this.orientation.map(fmt).join(\", \")}]`\n }\n\n toJSON(): PoseData {\n return { position: this.position, orientation: this.orientation }\n }\n}\n","import type { Pose as PoseData } from \"@wandelbots/nova-api/v2\"\nimport type { Nova } from \"../../Nova.ts\"\nimport { Pose } from \"./Pose.ts\"\n\ntype PoseKeys = keyof PoseData\n\ntype IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never]\n ? [Exclude<PoseKeys, keyof T>] extends [never]\n ? true\n : false\n : false\n\n// Depth cap avoids \"type instantiation is excessively deep\" on nova-api's\n// large (and possibly cyclic, e.g. compound colliders) generated type graph.\ntype MaxDepth = [\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n unknown,\n]\n\ntype AugmentPosesCore<\n T,\n Depth extends readonly unknown[],\n> = Depth[\"length\"] extends MaxDepth[\"length\"]\n ? T\n : IsExactlyPoseData<T> extends true\n ? Pose\n : T extends readonly (infer U)[]\n ? DeepPoseAugmented<U, [...Depth, unknown]>[]\n : T extends object\n ? { [K in keyof T]: DeepPoseAugmented<T[K], [...Depth, unknown]> }\n : T\n\n/**\n * Type-level counterpart to `augmentPoses`: recursively replaces any\n * `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so\n * optional `Pose | undefined` fields keep the `| undefined`), so the static\n * type matches what's actually returned at runtime.\n */\nexport type DeepPoseAugmented<\n T,\n Depth extends readonly unknown[] = [],\n> = T extends unknown ? AugmentPosesCore<T, Depth> : never\n\nfunction isPlainObject(value: unknown): value is Record<string, unknown> {\n return (\n typeof value === \"object\" &&\n value !== null &&\n !Array.isArray(value) &&\n !(value instanceof Pose)\n )\n}\n\nfunction isNumberTriple(value: unknown): value is number[] {\n return (\n Array.isArray(value) &&\n value.length === 3 &&\n value.every((n) => typeof n === \"number\")\n )\n}\n\n/**\n * Structural check for the wire shape of `Pose` (`{ position, orientation }`,\n * each a 3-number array, and nothing else). This API has no other type with\n * this exact shape, so it reliably identifies `Pose` values without needing\n * per-endpoint knowledge of which fields are poses.\n */\nfunction isPoseShape(value: Record<string, unknown>): boolean {\n return (\n Object.keys(value).length === 2 &&\n isNumberTriple(value.position) &&\n isNumberTriple(value.orientation)\n )\n}\n\nfunction augmentPosesInPlace(value: unknown): void {\n if (Array.isArray(value)) {\n for (const item of value) {\n augmentPosesInPlace(item)\n }\n return\n }\n\n if (isPlainObject(value)) {\n if (isPoseShape(value)) {\n Object.setPrototypeOf(value, Pose.prototype)\n } else {\n for (const key of Object.keys(value)) {\n augmentPosesInPlace(value[key])\n }\n }\n }\n}\n\n/**\n * Recursively walks a parsed API response/request body, upgrading any\n * `Pose`-shaped objects in place to `Pose` instances so they gain math\n * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.\n * Used by `withMath` for `nova.api.*` responses; exported so it can also\n * be applied manually to e.g. websocket messages.\n */\nexport function augmentPoses<T>(value: T): DeepPoseAugmented<T> {\n augmentPosesInPlace(value)\n return value as DeepPoseAugmented<T>\n}\n\ntype WithAugmentedPosesCore<\n T,\n Depth extends readonly unknown[],\n> = Depth[\"length\"] extends MaxDepth[\"length\"]\n ? T\n : T extends (...args: infer A) => Promise<infer R>\n ? (...args: A) => Promise<DeepPoseAugmented<R>>\n : T extends object\n ? { [K in keyof T]: WithAugmentedPosesCore<T[K], [...Depth, unknown]> }\n : T\n\n/**\n * Recursively maps every method on `Nova[\"api\"]` (and its nested API groups)\n * so calls are typed as returning `DeepPoseAugmented` results, matching what\n * `withMath` does at runtime.\n */\ntype WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>\n\nexport type NovaWithMath = Omit<Nova, \"api\"> & {\n readonly api: WithAugmentedPoses<Nova[\"api\"]>\n}\n\nfunction wrapWithPoseAugmentation<T extends object>(target: T): T {\n return new Proxy(target, {\n get(t, prop) {\n const value = Reflect.get(t, prop, t)\n if (typeof value === \"function\") {\n return (...args: unknown[]) => {\n const result = (value as (...a: unknown[]) => unknown).apply(t, args)\n return result instanceof Promise\n ? result.then((data) => augmentPoses(data))\n : result\n }\n }\n if (value !== null && typeof value === \"object\") {\n return wrapWithPoseAugmentation(value)\n }\n return value\n },\n }) as T\n}\n\n/**\n * Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades\n * `Pose`-shaped fields in its response to `Pose` instances (with math\n * methods like `multiply`/`inverse`). Returns a new proxied view - does not\n * mutate the original `nova` instance.\n */\nexport function withMath(nova: Nova): NovaWithMath {\n const proxy = new Proxy(nova, {\n get(target, prop) {\n if (prop === \"api\") {\n return wrapWithPoseAugmentation(target.api)\n }\n // receiver is `target`, not the proxy, so `this` inside Nova's own\n // methods stays bound to the original (un-proxied) instance.\n return Reflect.get(target, prop, target)\n },\n })\n\n return proxy as unknown as NovaWithMath\n}\n"],"mappings":";AAUA,MAAM,UAAU;AAEhB,IAAa,aAAb,MAAa,WAAqC;CAChD;CACA;CACA;CACA;CAEA,YAAY,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG;EACtC,KAAK,IAAI;EACT,KAAK,IAAI;EACT,KAAK,IAAI;EACT,KAAK,IAAI;CACX;CAEA,OAAO,WAAuB;EAC5B,OAAO,IAAI,WAAW;CACxB;;;;;;CAOA,OAAO,mBAAmB,gBAAsC;EAC9D,MAAM,KAAK,eAAe,MAAM;EAChC,MAAM,KAAK,eAAe,MAAM;EAChC,MAAM,KAAK,eAAe,MAAM;EAEhC,MAAM,QAAQ,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,EAAE;EACnD,IAAI,QAAQ,SACV,OAAO,WAAW,SAAS;EAE7B,MAAM,OAAO,QAAQ;EACrB,MAAM,IAAI,KAAK,IAAI,IAAI,IAAI;EAC3B,OAAO,IAAI,WAAW,KAAK,IAAI,IAAI,GAAG,KAAK,GAAG,KAAK,GAAG,KAAK,CAAC;CAC9D;;;;;;;;;;;CAYA,mBAA6B;EAC3B,MAAM,UAAU,KAAK,KACnB,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,CACpD;EACA,IAAI,UAAU,SACZ,OAAO;GAAC;GAAG;GAAG;EAAC;EAEjB,MAAM,QAAQ,IAAI,KAAK,MAAM,SAAS,KAAK,IAAI,KAAK,CAAC,CAAC;EACtD,MAAM,SAAS,KAAK,KAAK,IAAI,QAAQ,CAAC,SAAS;EAC/C,OAAO;GAAC,KAAK,IAAI;GAAO,KAAK,IAAI;GAAO,KAAK,IAAI;EAAK;CACxD;;CAGA,SAAS,OAAmC;EAC1C,MAAM,EAAE,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,OAAO;EACvC,MAAM,EAAE,GAAG,IAAI,GAAG,IAAI,GAAG,IAAI,GAAG,OAAO;EACvC,OAAO,IAAI,WACT,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,IACnC,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,EACrC;CACF;;CAGA,YAAwB;EACtB,OAAO,IAAI,WAAW,KAAK,GAAG,CAAC,KAAK,GAAG,CAAC,KAAK,GAAG,CAAC,KAAK,CAAC;CACzD;;CAGA,UAAsB;EACpB,MAAM,cACJ,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK,IAAI,KAAK;EAEtE,IAAI,cAAc,SAChB,OAAO,IAAI,WAAW,GAAG,GAAG,GAAG,CAAC;EAElC,MAAM,YAAY,KAAK,UAAU;EACjC,OAAO,IAAI,WACT,UAAU,IAAI,aACd,UAAU,IAAI,aACd,UAAU,IAAI,aACd,UAAU,IAAI,WAChB;CACF;;;;;CAMA,gBAAgB,OAA+B;EAC7C,MAAM,WAAW,KAAK,SAAS;GAC7B,GAAG,MAAM;GACT,GAAG,CAAC,MAAM;GACV,GAAG,CAAC,MAAM;GACV,GAAG,CAAC,MAAM;EACZ,CAAC;EACD,MAAM,UAAU,KAAK,KACnB,SAAS,IAAI,SAAS,IACpB,SAAS,IAAI,SAAS,IACtB,SAAS,IAAI,SAAS,CAC1B;EACA,OAAO,IAAI,KAAK,MAAM,SAAS,KAAK,IAAI,SAAS,CAAC,CAAC;CACrD;;CAGA,aAAa,GAAuB;EAClC,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,KAAK,EAAE,MAAM;EACnB,MAAM,EAAE,GAAG,GAAG,GAAG,MAAM;EAGvB,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;EAG7B,OAAO;GACL,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;GAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;GAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;EAC9B;CACF;CAEA,SAAyB;EACvB,OAAO;GAAE,GAAG,KAAK;GAAG,GAAG,KAAK;GAAG,GAAG,KAAK;GAAG,GAAG,KAAK;EAAE;CACtD;AACF;;;AC/IA,MAAM,cAAc;CAAC;CAAG;CAAG;AAAC;AAG5B,MAAM,kBAAkB;AAExB,SAAS,WAAW,GAAa,GAAuB;CACtD,OAAO;GACJ,EAAE,MAAM,MAAM,EAAE,MAAM;GACtB,EAAE,MAAM,MAAM,EAAE,MAAM;GACtB,EAAE,MAAM,MAAM,EAAE,MAAM;CACzB;AACF;AAEA,SAAS,aAAa,GAAuB;CAC3C,OAAO;EAAC,EAAE,EAAE,MAAM;EAAI,EAAE,EAAE,MAAM;EAAI,EAAE,EAAE,MAAM;CAAE;AAClD;AAEA,SAAS,gBAAgB,GAAsB;CAC7C,OAAO,EAAE,WAAW,KAAK,EAAE,OAAO,MAAM,OAAO,SAAS,CAAC,CAAC;AAC5D;;;;;;;;AASA,IAAa,OAAb,MAAa,KAAyB;CACpC;CACA;CAEA,YACE,WAAqB,aACrB,cAAwB,aACxB;EACA,IAAI,CAAC,gBAAgB,QAAQ,GAC3B,MAAM,IAAI,MACR,wEAAwE,KAAK,UAAU,QAAQ,GACjG;EAEF,IAAI,CAAC,gBAAgB,WAAW,GAC9B,MAAM,IAAI,MACR,2EAA2E,KAAK,UAAU,WAAW,GACvG;EAIF,KAAK,WAAW,CAAC,GAAG,QAAQ;EAC5B,KAAK,cAAc,CAAC,GAAG,WAAW;CACpC;CAEA,OAAO,KAAK,MAAsB;EAChC,IAAI,gBAAgB,MAClB,OAAO;EAET,OAAO,IAAI,KACT,KAAK,YAAY,aACjB,KAAK,eAAe,WACtB;CACF;CAEA,OAAO,WAAiB;EACtB,OAAO,IAAI,KAAK;CAClB;;;;;;;CAQA,SAAS,OAAuB;EAC9B,MAAM,KAAK,WAAW,mBAAmB,KAAK,WAAW;EACzD,MAAM,KAAK,WAAW,mBAAmB,MAAM,eAAe,WAAW;EAEzE,MAAM,WAAW,WACf,KAAK,UACL,GAAG,aAAa,MAAM,YAAY,WAAW,CAC/C;EACA,MAAM,cAAc,GAAG,SAAS,EAAE,CAAC,CAAC,iBAAiB;EAErD,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,UAAgB;EAGd,MAAM,WAFW,WAAW,mBAAmB,KAAK,WAAW,CAAC,CAAC,QAEzC,CAAC,CAAC,aAAa,aAAa,KAAK,QAAQ,CAAC;EAElE,MAAM,cAAc,aAAa,KAAK,WAAW;EAEjD,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,eAAe,OAA2B;EACxC,MAAM,IAAI,WAAW,mBAAmB,KAAK,WAAW;EACxD,OAAO,WAAW,KAAK,UAAU,EAAE,aAAa,KAAK,CAAC;CACxD;;;;;;;;;CAUA,SACE,OACA,gBAAgB,iBAChB,mBAAmB,iBACV;EACT,MAAM,gBAAgB,MAAM,YAAY;EACxC,MAAM,mBAAmB,MAAM,eAAe;EAE9C,MAAM,mBAAmB,KAAK,KAC5B,KAAK,SAAS,QACX,KAAK,GAAG,MAAM,OAAO,KAAK,cAAc,MAAM,OAAO,GACtD,CACF,CACF;EACA,MAAM,sBAAsB,WAAW,mBACrC,KAAK,WACP,CAAC,CAAC,gBAAgB,WAAW,mBAAmB,gBAAgB,CAAC;EAEjE,OACE,oBAAoB,iBACpB,uBAAuB;CAE3B;;;;;;CAOA,cAAwB;EACtB,MAAM,EAAE,GAAG,GAAG,GAAG,MAAM,WAAW,mBAAmB,KAAK,WAAW;EAErE,MAAM,OAAO,KAAK,MAAM,KAAK,IAAI,IAAI,IAAI,IAAI,IAAI,KAAK,IAAI,IAAI,IAAI,EAAE;EACpE,MAAM,QAAQ,KAAK,KAAK,KAAK,IAAI,GAAG,KAAK,IAAI,IAAI,KAAK,IAAI,IAAI,IAAI,EAAE,CAAC,CAAC;EACtE,MAAM,MAAM,KAAK,MAAM,KAAK,IAAI,IAAI,IAAI,IAAI,IAAI,KAAK,IAAI,IAAI,IAAI,EAAE;EAEnE,OAAO;GAAC,GAAG,KAAK;GAAU;GAAM;GAAO;EAAG;CAC5C;;CAGA,SAAS,YAAY,GAAW;EAC9B,MAAM,OAAO,MAAc,OAAO,EAAE,YAAY,SAAS,CAAC;EAC1D,OAAO,IAAI,KAAK,SAAS,IAAI,GAAG,CAAC,CAAC,KAAK,IAAI,EAAE,IAAI,KAAK,YAAY,IAAI,GAAG,CAAC,CAAC,KAAK,IAAI,EAAE;CACxF;CAEA,SAAmB;EACjB,OAAO;GAAE,UAAU,KAAK;GAAU,aAAa,KAAK;EAAY;CAClE;AACF;;;AC9GA,SAAS,cAAc,OAAkD;CACvE,OACE,OAAO,UAAU,YACjB,UAAU,QACV,CAAC,MAAM,QAAQ,KAAK,KACpB,EAAE,iBAAiB;AAEvB;AAEA,SAAS,eAAe,OAAmC;CACzD,OACE,MAAM,QAAQ,KAAK,KACnB,MAAM,WAAW,KACjB,MAAM,OAAO,MAAM,OAAO,MAAM,QAAQ;AAE5C;;;;;;;AAQA,SAAS,YAAY,OAAyC;CAC5D,OACE,OAAO,KAAK,KAAK,CAAC,CAAC,WAAW,KAC9B,eAAe,MAAM,QAAQ,KAC7B,eAAe,MAAM,WAAW;AAEpC;AAEA,SAAS,oBAAoB,OAAsB;CACjD,IAAI,MAAM,QAAQ,KAAK,GAAG;EACxB,KAAK,MAAM,QAAQ,OACjB,oBAAoB,IAAI;EAE1B;CACF;CAEA,IAAI,cAAc,KAAK,GACrB,IAAI,YAAY,KAAK,GACnB,OAAO,eAAe,OAAO,KAAK,SAAS;MAE3C,KAAK,MAAM,OAAO,OAAO,KAAK,KAAK,GACjC,oBAAoB,MAAM,IAAI;AAItC;;;;;;;;AASA,SAAgB,aAAgB,OAAgC;CAC9D,oBAAoB,KAAK;CACzB,OAAO;AACT;AAwBA,SAAS,yBAA2C,QAAc;CAChE,OAAO,IAAI,MAAM,QAAQ,EACvB,IAAI,GAAG,MAAM;EACX,MAAM,QAAQ,QAAQ,IAAI,GAAG,MAAM,CAAC;EACpC,IAAI,OAAO,UAAU,YACnB,QAAQ,GAAG,SAAoB;GAC7B,MAAM,SAAU,MAAuC,MAAM,GAAG,IAAI;GACpE,OAAO,kBAAkB,UACrB,OAAO,MAAM,SAAS,aAAa,IAAI,CAAC,IACxC;EACN;EAEF,IAAI,UAAU,QAAQ,OAAO,UAAU,UACrC,OAAO,yBAAyB,KAAK;EAEvC,OAAO;CACT,EACF,CAAC;AACH;;;;;;;AAQA,SAAgB,SAAS,MAA0B;CAYjD,OAAO,IAXW,MAAM,MAAM,EAC5B,IAAI,QAAQ,MAAM;EAChB,IAAI,SAAS,OACX,OAAO,yBAAyB,OAAO,GAAG;EAI5C,OAAO,QAAQ,IAAI,QAAQ,MAAM,MAAM;CACzC,EACF,CAEW;AACb"}
package/package.json CHANGED
@@ -1,7 +1,7 @@
1
1
  {
2
2
  "name": "@wandelbots/nova-js",
3
3
  "type": "module",
4
- "version": "4.3.0-pr.318.c6d70a6",
4
+ "version": "4.3.0",
5
5
  "description": "Official JS client for the Wandelbots API",
6
6
  "sideEffects": false,
7
7
  "packageManager": "pnpm@11.9.0+sha512.bd682d5d03fe525ef7c9fd6780c6884d1e756ac4c9c9fe00c538782824310dcf90e3ddc4f53835f06dfaebd5085e41855e0bcbb3b60de2ac5bbab89e5036f03b",
package/src/experimental/math/index.ts CHANGED
@@ -1,18 +1,18 @@
1
1
  /**
2
- * Experimental pose math helpers (`Pose`, `augmentMath`) for working with
3
- * position + axis-angle orientation values from the NOVA API.
2
+ * Experimental pose math helpers (`Pose`, `Quaternion`, `withMath`) for
3
+ * working with position + axis-angle orientation values from the NOVA API.
4
4
  *
5
5
  * This API is experimental and may change without a major version bump.
6
6
  */
7
7
  export type { Pose as PoseData } from "@wandelbots/nova-api/v2"
8
+ export { Pose } from "../../lib/experimental/math/Pose.ts"
9
+ export { Quaternion } from "../../lib/experimental/math/Quaternion.ts"
10
+ export type { QuaternionData } from "../../lib/experimental/math/Quaternion.ts"
8
11
  export {
9
- augmentMath,
10
12
  augmentPoses,
11
- } from "../../lib/experimental/math/augmentMath.ts"
13
+ withMath,
14
+ } from "../../lib/experimental/math/withMath.ts"
12
15
  export type {
13
16
  DeepPoseAugmented,
14
17
  NovaWithMath,
15
- } from "../../lib/experimental/math/augmentMath.ts"
16
- export { Pose } from "../../lib/experimental/math/Pose.ts"
17
- export { Quaternion } from "../../lib/experimental/math/Quaternion.ts"
18
- export type { QuaternionData } from "../../lib/experimental/math/Quaternion.ts"
18
+ } from "../../lib/experimental/math/withMath.ts"
package/src/lib/experimental/math/Pose.ts CHANGED
@@ -3,6 +3,9 @@ import { Quaternion } from "./Quaternion.ts"
3
3
 
4
4
  const ZERO_VECTOR = [0, 0, 0]
5
5
 
6
+ // Matches wb-robotix's Constants.h TOLERANCE_MILLI, the default isApprox tolerance.
7
+ const TOLERANCE_MILLI = 1e-3
8
+
6
9
  function addVectors(a: number[], b: number[]): number[] {
7
10
  return [
8
11
  (a[0] ?? 0) + (b[0] ?? 0),
@@ -15,6 +18,10 @@ function negateVector(a: number[]): number[] {
15
18
  return [-(a[0] ?? 0), -(a[1] ?? 0), -(a[2] ?? 0)]
16
19
  }
17
20
 
21
+ function isFiniteVector3(v: number[]): boolean {
22
+ return v.length === 3 && v.every((n) => Number.isFinite(n))
23
+ }
24
+
18
25
  /**
19
26
  * A `Pose` (position + axis-angle orientation) with methods for composing
20
27
  * and inverting transforms. Instances are plain-data compatible with the
@@ -30,8 +37,20 @@ export class Pose implements PoseData {
30
37
  position: number[] = ZERO_VECTOR,
31
38
  orientation: number[] = ZERO_VECTOR,
32
39
  ) {
33
- this.position = position
34
- this.orientation = orientation
40
+ if (!isFiniteVector3(position)) {
41
+ throw new Error(
42
+ `Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`,
43
+ )
44
+ }
45
+ if (!isFiniteVector3(orientation)) {
46
+ throw new Error(
47
+ `Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`,
48
+ )
49
+ }
50
+ // Copy defensively so mutating the caller's arrays afterward can't
51
+ // change this (supposedly immutable) instance's state.
52
+ this.position = [...position]
53
+ this.orientation = [...orientation]
35
54
  }
36
55
 
37
56
  static from(pose: PoseData): Pose {
@@ -84,21 +103,59 @@ export class Pose implements PoseData {
84
103
  return addVectors(this.position, q.rotateVector(point))
85
104
  }
86
105
 
87
- /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
88
- isApprox(other: PoseData, epsilon = 1e-9): boolean {
106
+ /**
107
+ * Compare to another pose using separate position/orientation tolerances,
108
+ * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
109
+ * position, and quaternion angular distance for orientation - not a naive
110
+ * per-component diff, since two rotation vectors can represent nearly
111
+ * identical rotations while differing componentwise near a
112
+ * canonicalization boundary (e.g. close to the +/-pi wraparound).
113
+ */
114
+ isApprox(
115
+ other: PoseData,
116
+ deltaPosition = TOLERANCE_MILLI,
117
+ deltaOrientation = TOLERANCE_MILLI,
118
+ ): boolean {
89
119
  const otherPosition = other.position ?? ZERO_VECTOR
90
120
  const otherOrientation = other.orientation ?? ZERO_VECTOR
91
121
 
122
+ const positionDistance = Math.sqrt(
123
+ this.position.reduce(
124
+ (sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2,
125
+ 0,
126
+ ),
127
+ )
128
+ const orientationDistance = Quaternion.fromRotationVector(
129
+ this.orientation,
130
+ ).angularDistance(Quaternion.fromRotationVector(otherOrientation))
131
+
92
132
  return (
93
- this.position.every(
94
- (v, i) => Math.abs(v - (otherPosition[i] ?? 0)) <= epsilon,
95
- ) &&
96
- this.orientation.every(
97
- (v, i) => Math.abs(v - (otherOrientation[i] ?? 0)) <= epsilon,
98
- )
133
+ positionDistance <= deltaPosition &&
134
+ orientationDistance <= deltaOrientation
99
135
  )
100
136
  }
101
137
 
138
+ /**
139
+ * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
140
+ * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
141
+ * wb-robotix's `Pose::toCartesian()`.
142
+ */
143
+ toCartesian(): number[] {
144
+ const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation)
145
+
146
+ const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y))
147
+ const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))))
148
+ const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z))
149
+
150
+ return [...this.position, roll, pitch, yaw]
151
+ }
152
+
153
+ /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
154
+ toString(precision = 6): string {
155
+ const fmt = (v: number) => Number(v.toPrecision(precision))
156
+ return `[${this.position.map(fmt).join(", ")}][${this.orientation.map(fmt).join(", ")}]`
157
+ }
158
+
102
159
  toJSON(): PoseData {
103
160
  return { position: this.position, orientation: this.orientation }
104
161
  }
package/src/lib/experimental/math/Quaternion.ts CHANGED
@@ -85,9 +85,40 @@ export class Quaternion implements QuaternionData {
85
85
  return new Quaternion(this.w, -this.x, -this.y, -this.z)
86
86
  }
87
87
 
88
- /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
88
+ /** Inverse: `conjugate()` scaled by `1 / squaredNorm()`, matching `Eigen::Quaternion::inverse()`. Equal to `conjugate()` for unit quaternions. */
89
89
  inverse(): Quaternion {
90
- return this.conjugate()
90
+ const squaredNorm =
91
+ this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z
92
+ // Degenerate (zero) quaternion has no inverse - flag it the same way Eigen does.
93
+ if (squaredNorm < EPSILON) {
94
+ return new Quaternion(0, 0, 0, 0)
95
+ }
96
+ const conjugate = this.conjugate()
97
+ return new Quaternion(
98
+ conjugate.w / squaredNorm,
99
+ conjugate.x / squaredNorm,
100
+ conjugate.y / squaredNorm,
101
+ conjugate.z / squaredNorm,
102
+ )
103
+ }
104
+
105
+ /**
106
+ * The angle (in rad, always within [0, pi]) of the rotation that takes
107
+ * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
108
+ */
109
+ angularDistance(other: QuaternionData): number {
110
+ const relative = this.multiply({
111
+ w: other.w,
112
+ x: -other.x,
113
+ y: -other.y,
114
+ z: -other.z,
115
+ })
116
+ const vecNorm = Math.sqrt(
117
+ relative.x * relative.x +
118
+ relative.y * relative.y +
119
+ relative.z * relative.z,
120
+ )
121
+ return 2 * Math.atan2(vecNorm, Math.abs(relative.w))
91
122
  }
92
123
 
93
124
  /** Rotate a 3D vector by this quaternion. */
package/src/lib/experimental/math/{augmentMath.ts → withMath.ts} RENAMED
@@ -103,7 +103,7 @@ function augmentPosesInPlace(value: unknown): void {
103
103
  * Recursively walks a parsed API response/request body, upgrading any
104
104
  * `Pose`-shaped objects in place to `Pose` instances so they gain math
105
105
  * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
106
- * Used by `augmentMath` for `nova.api.*` responses; exported so it can also
106
+ * Used by `withMath` for `nova.api.*` responses; exported so it can also
107
107
  * be applied manually to e.g. websocket messages.
108
108
  */
109
109
  export function augmentPoses<T>(value: T): DeepPoseAugmented<T> {
@@ -125,7 +125,7 @@ type WithAugmentedPosesCore<
125
125
  /**
126
126
  * Recursively maps every method on `Nova["api"]` (and its nested API groups)
127
127
  * so calls are typed as returning `DeepPoseAugmented` results, matching what
128
- * `augmentMath` does at runtime.
128
+ * `withMath` does at runtime.
129
129
  */
130
130
  type WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>
131
131
 
@@ -159,7 +159,7 @@ function wrapWithPoseAugmentation<T extends object>(target: T): T {
159
159
  * methods like `multiply`/`inverse`). Returns a new proxied view - does not
160
160
  * mutate the original `nova` instance.
161
161
  */
162
- export function augmentMath(nova: Nova): NovaWithMath {
162
+ export function withMath(nova: Nova): NovaWithMath {
163
163
  const proxy = new Proxy(nova, {
164
164
  get(target, prop) {
165
165
  if (prop === "api") {