@wandelbots/nova-js 4.2.0 → 4.3.0-nova-api-dev.26-6-0-dev-137

package/package.json

@@ -1,7 +1,7 @@
 {
   "name": "@wandelbots/nova-js",
   "type": "module",
-  "version": "4.2.0",
+  "version": "4.3.0-nova-api-dev.26-6-0-dev-137",
   "description": "Official JS client for the Wandelbots API",
   "sideEffects": false,
   "packageManager": "pnpm@11.9.0+sha512.bd682d5d03fe525ef7c9fd6780c6884d1e756ac4c9c9fe00c538782824310dcf90e3ddc4f53835f06dfaebd5085e41855e0bcbb3b60de2ac5bbab89e5036f03b",
@@ -20,6 +20,10 @@
     "./experimental/nats": {
       "types": "./dist/experimental/nats/index.d.mts",
       "default": "./dist/experimental/nats/index.mjs"
+    },
+    "./experimental/math": {
+      "types": "./dist/experimental/math/index.d.mts",
+      "default": "./dist/experimental/math/index.mjs"
     }
   },
   "main": "./dist/index.mjs",
@@ -57,7 +61,7 @@
     "@biomejs/biome": "^2.5.6",
     "@types/node": "^25.9.5",
     "@types/ws": "^8.18.1",
-    "@wandelbots/nova-api": "26.5.2",
+    "@wandelbots/nova-api": "26.6.0-dev.137",
     "concurrently": "^10.0.4",
     "conventional-changelog-conventionalcommits": "^9.3.1",
     "jsdom": "^29.1.1",

package/src/experimental/math/index.ts

@@ -0,0 +1,18 @@
+/**
+ * Experimental pose math helpers (`Pose`, `Quaternion`, `withMath`) for
+ * working with position + axis-angle orientation values from the NOVA API.
+ *
+ * This API is experimental and may change without a major version bump.
+ */
+export type { Pose as PoseData } from "@wandelbots/nova-api/v2"
+export { Pose } from "../../lib/experimental/math/Pose.ts"
+export { Quaternion } from "../../lib/experimental/math/Quaternion.ts"
+export type { QuaternionData } from "../../lib/experimental/math/Quaternion.ts"
+export {
+  augmentPoses,
+  withMath,
+} from "../../lib/experimental/math/withMath.ts"
+export type {
+  DeepPoseAugmented,
+  NovaWithMath,
+} from "../../lib/experimental/math/withMath.ts"

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

@@ -0,0 +1,162 @@
+import type { Pose as PoseData } from "@wandelbots/nova-api/v2"
+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),
+    (a[1] ?? 0) + (b[1] ?? 0),
+    (a[2] ?? 0) + (b[2] ?? 0),
+  ]
+}
+
+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
+ * wire-format `PoseData` type (own `position`/`orientation` properties only,
+ * no enumerable methods), so they can be passed directly back into API calls
+ * that expect a pose.
+ */
+export class Pose implements PoseData {
+  readonly position: number[]
+  readonly orientation: number[]
+
+  constructor(
+    position: number[] = ZERO_VECTOR,
+    orientation: number[] = ZERO_VECTOR,
+  ) {
+    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 {
+    if (pose instanceof Pose) {
+      return pose
+    }
+    return new Pose(
+      pose.position ?? ZERO_VECTOR,
+      pose.orientation ?? ZERO_VECTOR,
+    )
+  }
+
+  static identity(): Pose {
+    return new Pose()
+  }
+
+  /**
+   * Compose this pose with `other`, treating `other` as being expressed in
+   * this pose's coordinate frame. Equivalent to the homogeneous transform
+   * product `this * other`: applying the result to a point is the same as
+   * applying `other` first, then `this`.
+   */
+  multiply(other: PoseData): Pose {
+    const q1 = Quaternion.fromRotationVector(this.orientation)
+    const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR)
+
+    const position = addVectors(
+      this.position,
+      q1.rotateVector(other.position ?? ZERO_VECTOR),
+    )
+    const orientation = q1.multiply(q2).toRotationVector()
+
+    return new Pose(position, orientation)
+  }
+
+  /** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */
+  inverse(): Pose {
+    const qInverse = Quaternion.fromRotationVector(this.orientation).inverse()
+
+    const position = qInverse.rotateVector(negateVector(this.position))
+    // Negating an axis-angle vector gives the inverse rotation directly.
+    const orientation = negateVector(this.orientation)
+
+    return new Pose(position, orientation)
+  }
+
+  /** Apply this pose's transform to a point, returning the transformed point. */
+  transformPoint(point: number[]): number[] {
+    const q = Quaternion.fromRotationVector(this.orientation)
+    return addVectors(this.position, q.rotateVector(point))
+  }
+
+  /**
+   * 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 (
+      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

@@ -0,0 +1,147 @@
+/**
+ * A unit quaternion representing a 3D rotation. Mirrors the subset of
+ * `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that
+ * `Pose` composition needs: Hamilton product, conjugate/inverse, vector
+ * rotation, and axis-angle rotation-vector conversion. Instances are
+ * immutable - every method returns a new `Quaternion`.
+ */
+
+export type QuaternionData = { w: number; x: number; y: number; z: number }
+
+const EPSILON = 1e-12
+
+export class Quaternion implements QuaternionData {
+  readonly w: number
+  readonly x: number
+  readonly y: number
+  readonly z: number
+
+  constructor(w = 1, x = 0, y = 0, z = 0) {
+    this.w = w
+    this.x = x
+    this.y = y
+    this.z = z
+  }
+
+  static identity(): Quaternion {
+    return new Quaternion()
+  }
+
+  /**
+   * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =
+   * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported
+   * from wb-robotix's `QuaternionBasePlugin.h`).
+   */
+  static fromRotationVector(rotationVector: number[]): Quaternion {
+    const rx = rotationVector[0] ?? 0
+    const ry = rotationVector[1] ?? 0
+    const rz = rotationVector[2] ?? 0
+
+    const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)
+    if (angle < EPSILON) {
+      return Quaternion.identity()
+    }
+    const half = angle / 2
+    const s = Math.sin(half) / angle
+    return new Quaternion(Math.cos(half), rx * s, ry * s, rz * s)
+  }
+
+  /**
+   * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),
+   * matching `Eigen::Quaternion::toRotationVector()` (ported from
+   * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`
+   * keeps the angle canonically within [0, pi] regardless of which of the
+   * two antipodal unit quaternions (this or its negation) represents the
+   * rotation, rather than acos's [0, 2*pi] range - important since composed
+   * poses need to match the same canonical rotation vector the robot
+   * controller itself would report for a pose.
+   */
+  toRotationVector(): number[] {
+    const vecNorm = Math.sqrt(
+      this.x * this.x + this.y * this.y + this.z * this.z,
+    )
+    if (vecNorm < EPSILON) {
+      return [0, 0, 0]
+    }
+    const angle = 2 * Math.atan2(vecNorm, Math.abs(this.w))
+    const scale = (this.w >= 0 ? angle : -angle) / vecNorm
+    return [this.x * scale, this.y * scale, this.z * scale]
+  }
+
+  /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */
+  multiply(other: QuaternionData): Quaternion {
+    const { w: aw, x: ax, y: ay, z: az } = this
+    const { w: bw, x: bx, y: by, z: bz } = other
+    return new Quaternion(
+      aw * bw - ax * bx - ay * by - az * bz,
+      aw * bx + ax * bw + ay * bz - az * by,
+      aw * by - ax * bz + ay * bw + az * bx,
+      aw * bz + ax * by - ay * bx + az * bw,
+    )
+  }
+
+  /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
+  conjugate(): Quaternion {
+    return new Quaternion(this.w, -this.x, -this.y, -this.z)
+  }
+
+  /** Inverse: `conjugate()` scaled by `1 / squaredNorm()`, matching `Eigen::Quaternion::inverse()`. Equal to `conjugate()` for unit quaternions. */
+  inverse(): Quaternion {
+    const squaredNorm =
+      this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z
+    // Degenerate (zero) quaternion has no inverse - flag it the same way Eigen does.
+    if (squaredNorm < EPSILON) {
+      return new Quaternion(0, 0, 0, 0)
+    }
+    const conjugate = this.conjugate()
+    return new Quaternion(
+      conjugate.w / squaredNorm,
+      conjugate.x / squaredNorm,
+      conjugate.y / squaredNorm,
+      conjugate.z / squaredNorm,
+    )
+  }
+
+  /**
+   * 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
+    const vy = v[1] ?? 0
+    const vz = v[2] ?? 0
+    const { w, x, y, z } = this
+
+    // t = 2 * cross(q.xyz, v)
+    const tx = 2 * (y * vz - z * vy)
+    const ty = 2 * (z * vx - x * vz)
+    const tz = 2 * (x * vy - y * vx)
+
+    // v' = v + w*t + cross(q.xyz, t)
+    return [
+      vx + w * tx + (y * tz - z * ty),
+      vy + w * ty + (z * tx - x * tz),
+      vz + w * tz + (x * ty - y * tx),
+    ]
+  }
+
+  toJSON(): QuaternionData {
+    return { w: this.w, x: this.x, y: this.y, z: this.z }
+  }
+}

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

@@ -0,0 +1,175 @@
+import type { Pose as PoseData } from "@wandelbots/nova-api/v2"
+import type { Nova } from "../../Nova.ts"
+import { Pose } from "./Pose.ts"
+
+type PoseKeys = keyof PoseData
+
+type IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never]
+  ? [Exclude<PoseKeys, keyof T>] extends [never]
+    ? true
+    : false
+  : false
+
+// Depth cap avoids "type instantiation is excessively deep" on nova-api's
+// large (and possibly cyclic, e.g. compound colliders) generated type graph.
+type MaxDepth = [
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+  unknown,
+]
+
+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
+
+/**
+ * Type-level counterpart to `augmentPoses`: recursively replaces any
+ * `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so
+ * optional `Pose | undefined` fields keep the `| undefined`), so the static
+ * type matches what's actually returned at runtime.
+ */
+export type DeepPoseAugmented<
+  T,
+  Depth extends readonly unknown[] = [],
+> = T extends unknown ? AugmentPosesCore<T, Depth> : never
+
+function isPlainObject(value: unknown): value is Record<string, unknown> {
+  return (
+    typeof value === "object" &&
+    value !== null &&
+    !Array.isArray(value) &&
+    !(value instanceof Pose)
+  )
+}
+
+function isNumberTriple(value: unknown): value is number[] {
+  return (
+    Array.isArray(value) &&
+    value.length === 3 &&
+    value.every((n) => typeof n === "number")
+  )
+}
+
+/**
+ * Structural check for the wire shape of `Pose` (`{ position, orientation }`,
+ * each a 3-number array, and nothing else). This API has no other type with
+ * this exact shape, so it reliably identifies `Pose` values without needing
+ * per-endpoint knowledge of which fields are poses.
+ */
+function isPoseShape(value: Record<string, unknown>): boolean {
+  return (
+    Object.keys(value).length === 2 &&
+    isNumberTriple(value.position) &&
+    isNumberTriple(value.orientation)
+  )
+}
+
+function augmentPosesInPlace(value: unknown): void {
+  if (Array.isArray(value)) {
+    for (const item of value) {
+      augmentPosesInPlace(item)
+    }
+    return
+  }
+
+  if (isPlainObject(value)) {
+    if (isPoseShape(value)) {
+      Object.setPrototypeOf(value, Pose.prototype)
+    } else {
+      for (const key of Object.keys(value)) {
+        augmentPosesInPlace(value[key])
+      }
+    }
+  }
+}
+
+/**
+ * Recursively walks a parsed API response/request body, upgrading any
+ * `Pose`-shaped objects in place to `Pose` instances so they gain math
+ * methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
+ * Used by `withMath` for `nova.api.*` responses; exported so it can also
+ * be applied manually to e.g. websocket messages.
+ */
+export function augmentPoses<T>(value: T): DeepPoseAugmented<T> {
+  augmentPosesInPlace(value)
+  return value as DeepPoseAugmented<T>
+}
+
+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
+
+/**
+ * Recursively maps every method on `Nova["api"]` (and its nested API groups)
+ * so calls are typed as returning `DeepPoseAugmented` results, matching what
+ * `withMath` does at runtime.
+ */
+type WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>
+
+export type NovaWithMath = Omit<Nova, "api"> & {
+  readonly api: WithAugmentedPoses<Nova["api"]>
+}
+
+function wrapWithPoseAugmentation<T extends object>(target: T): T {
+  return new Proxy(target, {
+    get(t, prop) {
+      const value = Reflect.get(t, prop, t)
+      if (typeof value === "function") {
+        return (...args: unknown[]) => {
+          const result = (value as (...a: unknown[]) => unknown).apply(t, args)
+          return result instanceof Promise
+            ? result.then((data) => augmentPoses(data))
+            : result
+        }
+      }
+      if (value !== null && typeof value === "object") {
+        return wrapWithPoseAugmentation(value)
+      }
+      return value
+    },
+  }) as T
+}
+
+/**
+ * Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades
+ * `Pose`-shaped fields in its response to `Pose` instances (with math
+ * methods like `multiply`/`inverse`). Returns a new proxied view - does not
+ * mutate the original `nova` instance.
+ */
+export function withMath(nova: Nova): NovaWithMath {
+  const proxy = new Proxy(nova, {
+    get(target, prop) {
+      if (prop === "api") {
+        return wrapWithPoseAugmentation(target.api)
+      }
+      // receiver is `target`, not the proxy, so `this` inside Nova's own
+      // methods stays bound to the original (un-proxied) instance.
+      return Reflect.get(target, prop, target)
+    },
+  })
+
+  return proxy as unknown as NovaWithMath
+}