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

package/dist/experimental/math/index.d.mts

@@ -24,8 +24,23 @@ declare class Pose implements Pose$1 {
   inverse(): Pose;
   /** Apply this pose's transform to a point, returning the transformed point. */
   transformPoint(point: number[]): number[];
-  /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
-  isApprox(other: Pose$1, epsilon?: number): boolean;
+  /**
+   * Compare to another pose using separate position/orientation tolerances,
+   * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
+   * position, and quaternion angular distance for orientation - not a naive
+   * per-component diff, since two rotation vectors can represent nearly
+   * identical rotations while differing componentwise near a
+   * canonicalization boundary (e.g. close to the +/-pi wraparound).
+   */
+  isApprox(other: Pose$1, deltaPosition?: number, deltaOrientation?: number): boolean;
+  /**
+   * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
+   * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
+   * wb-robotix's `Pose::toCartesian()`.
+   */
+  toCartesian(): number[];
+  /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
+  toString(precision?: number): string;
   toJSON(): Pose$1;
 }
 //#endregion
@@ -111,6 +126,11 @@ declare class Quaternion implements QuaternionData {
   conjugate(): Quaternion;
   /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
   inverse(): Quaternion;
+  /**
+   * The angle (in rad, always within [0, pi]) of the rotation that takes
+   * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
+   */
+  angularDistance(other: QuaternionData): number;
   /** Rotate a 3D vector by this quaternion. */
   rotateVector(v: number[]): number[];
   toJSON(): QuaternionData;

package/dist/experimental/math/index.d.mts.map

@@ -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"}
+{"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":";;;;;;;;;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;;;;KC1JP,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;;;;;EAQX,gBAAgB,OAAO;;EAgBvB,aAAa;EAmBb,UAAU"}

package/dist/experimental/math/index.mjs

@@ -68,6 +68,20 @@ var Quaternion = class Quaternion {
 	inverse() {
 		return this.conjugate();
 	}
+	/**
+	* The angle (in rad, always within [0, pi]) of the rotation that takes
+	* `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
+	*/
+	angularDistance(other) {
+		const relative = this.multiply({
+			w: other.w,
+			x: -other.x,
+			y: -other.y,
+			z: -other.z
+		});
+		const vecNorm = Math.sqrt(relative.x * relative.x + relative.y * relative.y + relative.z * relative.z);
+		return 2 * Math.atan2(vecNorm, Math.abs(relative.w));
+	}
 	/** Rotate a 3D vector by this quaternion. */
 	rotateVector(v) {
 		const vx = v[0] ?? 0;
@@ -99,6 +113,7 @@ const ZERO_VECTOR = [
 	0,
 	0
 ];
+const TOLERANCE_MILLI = .001;
 function addVectors(a, b) {
 	return [
 		(a[0] ?? 0) + (b[0] ?? 0),
@@ -113,6 +128,9 @@ function negateVector(a) {
 		-(a[2] ?? 0)
 	];
 }
+function isFiniteVector3(v) {
+	return v.length === 3 && v.every((n) => Number.isFinite(n));
+}
 /**
 * A `Pose` (position + axis-angle orientation) with methods for composing
 * and inverting transforms. Instances are plain-data compatible with the
@@ -124,8 +142,10 @@ var Pose = class Pose {
 	position;
 	orientation;
 	constructor(position = ZERO_VECTOR, orientation = ZERO_VECTOR) {
-		this.position = position;
-		this.orientation = orientation;
+		if (!isFiniteVector3(position)) throw new Error(`Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`);
+		if (!isFiniteVector3(orientation)) throw new Error(`Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`);
+		this.position = [...position];
+		this.orientation = [...orientation];
 	}
 	static from(pose) {
 		if (pose instanceof Pose) return pose;
@@ -158,11 +178,42 @@ var Pose = class Pose {
 		const q = Quaternion.fromRotationVector(this.orientation);
 		return addVectors(this.position, q.rotateVector(point));
 	}
-	/** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
-	isApprox(other, epsilon = 1e-9) {
+	/**
+	* Compare to another pose using separate position/orientation tolerances,
+	* matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
+	* position, and quaternion angular distance for orientation - not a naive
+	* per-component diff, since two rotation vectors can represent nearly
+	* identical rotations while differing componentwise near a
+	* canonicalization boundary (e.g. close to the +/-pi wraparound).
+	*/
+	isApprox(other, deltaPosition = TOLERANCE_MILLI, deltaOrientation = TOLERANCE_MILLI) {
 		const otherPosition = other.position ?? ZERO_VECTOR;
 		const otherOrientation = other.orientation ?? ZERO_VECTOR;
-		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);
+		const positionDistance = Math.sqrt(this.position.reduce((sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2, 0));
+		const orientationDistance = Quaternion.fromRotationVector(this.orientation).angularDistance(Quaternion.fromRotationVector(otherOrientation));
+		return positionDistance <= deltaPosition && orientationDistance <= deltaOrientation;
+	}
+	/**
+	* Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
+	* (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
+	* wb-robotix's `Pose::toCartesian()`.
+	*/
+	toCartesian() {
+		const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation);
+		const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y));
+		const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))));
+		const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z));
+		return [
+			...this.position,
+			roll,
+			pitch,
+			yaw
+		];
+	}
+	/** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
+	toString(precision = 6) {
+		const fmt = (v) => Number(v.toPrecision(precision));
+		return `[${this.position.map(fmt).join(", ")}][${this.orientation.map(fmt).join(", ")}]`;
 	}
 	toJSON() {
 		return {

package/dist/experimental/math/index.mjs.map

@@ -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"}
+{"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  /**\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 `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;;;;;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;;;ACnIA,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,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"}

package/package.json

@@ -1,7 +1,7 @@
 {
   "name": "@wandelbots/nova-js",
   "type": "module",
-  "version": "4.3.0-pr.318.c6d70a6",
+  "version": "4.3.0-pr.318.e48e5ca",
   "description": "Official JS client for the Wandelbots API",
   "sideEffects": false,
   "packageManager": "pnpm@11.9.0+sha512.bd682d5d03fe525ef7c9fd6780c6884d1e756ac4c9c9fe00c538782824310dcf90e3ddc4f53835f06dfaebd5085e41855e0bcbb3b60de2ac5bbab89e5036f03b",

package/src/lib/experimental/math/Pose.ts

@@ -3,6 +3,9 @@ import { Quaternion } from "./Quaternion.ts"
 
 const ZERO_VECTOR = [0, 0, 0]
 
+// Matches wb-robotix's Constants.h TOLERANCE_MILLI, the default isApprox tolerance.
+const TOLERANCE_MILLI = 1e-3
+
 function addVectors(a: number[], b: number[]): number[] {
   return [
     (a[0] ?? 0) + (b[0] ?? 0),
@@ -15,6 +18,10 @@ function negateVector(a: number[]): number[] {
   return [-(a[0] ?? 0), -(a[1] ?? 0), -(a[2] ?? 0)]
 }
 
+function isFiniteVector3(v: number[]): boolean {
+  return v.length === 3 && v.every((n) => Number.isFinite(n))
+}
+
 /**
  * A `Pose` (position + axis-angle orientation) with methods for composing
  * and inverting transforms. Instances are plain-data compatible with the
@@ -30,8 +37,20 @@ export class Pose implements PoseData {
     position: number[] = ZERO_VECTOR,
     orientation: number[] = ZERO_VECTOR,
   ) {
-    this.position = position
-    this.orientation = orientation
+    if (!isFiniteVector3(position)) {
+      throw new Error(
+        `Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`,
+      )
+    }
+    if (!isFiniteVector3(orientation)) {
+      throw new Error(
+        `Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`,
+      )
+    }
+    // Copy defensively so mutating the caller's arrays afterward can't
+    // change this (supposedly immutable) instance's state.
+    this.position = [...position]
+    this.orientation = [...orientation]
   }
 
   static from(pose: PoseData): Pose {
@@ -84,21 +103,59 @@ export class Pose implements PoseData {
     return addVectors(this.position, q.rotateVector(point))
   }
 
-  /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
-  isApprox(other: PoseData, epsilon = 1e-9): boolean {
+  /**
+   * Compare to another pose using separate position/orientation tolerances,
+   * matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
+   * position, and quaternion angular distance for orientation - not a naive
+   * per-component diff, since two rotation vectors can represent nearly
+   * identical rotations while differing componentwise near a
+   * canonicalization boundary (e.g. close to the +/-pi wraparound).
+   */
+  isApprox(
+    other: PoseData,
+    deltaPosition = TOLERANCE_MILLI,
+    deltaOrientation = TOLERANCE_MILLI,
+  ): boolean {
     const otherPosition = other.position ?? ZERO_VECTOR
     const otherOrientation = other.orientation ?? ZERO_VECTOR
 
+    const positionDistance = Math.sqrt(
+      this.position.reduce(
+        (sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2,
+        0,
+      ),
+    )
+    const orientationDistance = Quaternion.fromRotationVector(
+      this.orientation,
+    ).angularDistance(Quaternion.fromRotationVector(otherOrientation))
+
     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,
-      )
+      positionDistance <= deltaPosition &&
+      orientationDistance <= deltaOrientation
     )
   }
 
+  /**
+   * Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
+   * (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
+   * wb-robotix's `Pose::toCartesian()`.
+   */
+  toCartesian(): number[] {
+    const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation)
+
+    const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y))
+    const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))))
+    const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z))
+
+    return [...this.position, roll, pitch, yaw]
+  }
+
+  /** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
+  toString(precision = 6): string {
+    const fmt = (v: number) => Number(v.toPrecision(precision))
+    return `[${this.position.map(fmt).join(", ")}][${this.orientation.map(fmt).join(", ")}]`
+  }
+
   toJSON(): PoseData {
     return { position: this.position, orientation: this.orientation }
   }

package/src/lib/experimental/math/Quaternion.ts

@@ -90,6 +90,25 @@ export class Quaternion implements QuaternionData {
     return this.conjugate()
   }
 
+  /**
+   * The angle (in rad, always within [0, pi]) of the rotation that takes
+   * `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
+   */
+  angularDistance(other: QuaternionData): number {
+    const relative = this.multiply({
+      w: other.w,
+      x: -other.x,
+      y: -other.y,
+      z: -other.z,
+    })
+    const vecNorm = Math.sqrt(
+      relative.x * relative.x +
+        relative.y * relative.y +
+        relative.z * relative.z,
+    )
+    return 2 * Math.atan2(vecNorm, Math.abs(relative.w))
+  }
+
   /** Rotate a 3D vector by this quaternion. */
   rotateVector(v: number[]): number[] {
     const vx = v[0] ?? 0