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

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.
@@ -6,10 +6,6 @@ import { bc as Pose$1, t as Nova } from "../../Nova-DdbUm145.mjs";
6
6
  * wire-format `PoseData` type (own `position`/`orientation` properties only,
7
7
  * no enumerable methods), so they can be passed directly back into API calls
8
8
  * that expect a pose.
9
- *
10
- * Poses returned from `NovaAPIClient`/`Nova` are automatically upgraded to
11
- * `Pose` instances; construct one directly only when you have a pose from
12
- * elsewhere (e.g. a websocket message).
13
9
  */
14
10
  declare class Pose implements Pose$1 {
15
11
  readonly position: number[];
@@ -29,7 +25,7 @@ declare class Pose implements Pose$1 {
29
25
  /** Apply this pose's transform to a point, returning the transformed point. */
30
26
  transformPoint(point: number[]): number[];
31
27
  /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
32
- equals(other: Pose$1, epsilon?: number): boolean;
28
+ isApprox(other: Pose$1, epsilon?: number): boolean;
33
29
  toJSON(): Pose$1;
34
30
  }
35
31
  //#endregion
@@ -73,35 +69,52 @@ declare function augmentMath(nova: Nova): NovaWithMath;
73
69
  //#endregion
74
70
  //#region src/lib/experimental/math/Quaternion.d.ts
75
71
  /**
76
- * Internal quaternion helpers used to compose/invert the axis-angle
77
- * `orientation` vectors used by the NOVA API's `Pose` type. Rotation vectors
78
- * are converted to quaternions for composition (numerically stable, unlike
79
- * composing axis-angle vectors directly) and converted back at the boundary.
72
+ * A unit quaternion representing a 3D rotation. Mirrors the subset of
73
+ * `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that
74
+ * `Pose` composition needs: Hamilton product, conjugate/inverse, vector
75
+ * rotation, and axis-angle rotation-vector conversion. Instances are
76
+ * immutable - every method returns a new `Quaternion`.
80
77
  */
81
- type Quaternion = {
78
+ type QuaternionData = {
82
79
  w: number;
83
80
  x: number;
84
81
  y: number;
85
82
  z: number;
86
83
  };
87
- /** Convert an axis-angle rotation vector [rx, ry, rz] to a unit quaternion. */
88
- declare function axisAngleToQuaternion(r: number[]): Quaternion;
89
- /**
90
- * Convert a unit quaternion back to an axis-angle rotation vector [rx, ry, rz],
91
- * matching wb-robotix's `Quaternion::toRotationVector()` (ported from
92
- * QuaternionBasePlugin.h): using `atan2` rather than `acos` keeps the angle
93
- * canonically within [0, pi] regardless of which of the two antipodal unit
94
- * quaternions (q or -q) represents the rotation, rather than acos's [0, 2*pi]
95
- * range - important since composed poses need to match the same canonical
96
- * rotation vector the robot controller itself would report for a pose.
97
- */
98
- declare function quaternionToAxisAngle(q: Quaternion): number[];
99
- /** Hamilton product: composes rotations so that applying `multiplyQuaternions(a, b)` to a vector equals applying `b` then `a`. */
100
- declare function multiplyQuaternions(a: Quaternion, b: Quaternion): Quaternion;
101
- /** Conjugate of a unit quaternion equals its inverse. */
102
- declare function conjugateQuaternion(q: Quaternion): Quaternion;
103
- /** Rotate a 3D vector by a unit quaternion. */
104
- declare function rotateVectorByQuaternion(q: Quaternion, v: number[]): number[];
84
+ declare class Quaternion implements QuaternionData {
85
+ readonly w: number;
86
+ readonly x: number;
87
+ readonly y: number;
88
+ readonly z: number;
89
+ constructor(w?: number, x?: number, y?: number, z?: number);
90
+ static identity(): Quaternion;
91
+ /**
92
+ * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =
93
+ * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported
94
+ * from wb-robotix's `QuaternionBasePlugin.h`).
95
+ */
96
+ static fromRotationVector(rotationVector: number[]): Quaternion;
97
+ /**
98
+ * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),
99
+ * matching `Eigen::Quaternion::toRotationVector()` (ported from
100
+ * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`
101
+ * keeps the angle canonically within [0, pi] regardless of which of the
102
+ * two antipodal unit quaternions (this or its negation) represents the
103
+ * rotation, rather than acos's [0, 2*pi] range - important since composed
104
+ * poses need to match the same canonical rotation vector the robot
105
+ * controller itself would report for a pose.
106
+ */
107
+ toRotationVector(): number[];
108
+ /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */
109
+ multiply(other: QuaternionData): Quaternion;
110
+ /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
111
+ conjugate(): Quaternion;
112
+ /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
113
+ inverse(): Quaternion;
114
+ /** Rotate a 3D vector by this quaternion. */
115
+ rotateVector(v: number[]): number[];
116
+ toJSON(): QuaternionData;
117
+ }
105
118
  //#endregion
106
- export { type DeepPoseAugmented, type NovaWithMath, Pose, type Pose$1 as PoseData, type Quaternion, augmentMath, augmentPoses, axisAngleToQuaternion, conjugateQuaternion, multiplyQuaternions, quaternionToAxisAngle, rotateVectorByQuaternion };
119
+ export { type DeepPoseAugmented, type NovaWithMath, Pose, type Pose$1 as PoseData, Quaternion, type QuaternionData, augmentMath, augmentPoses };
107
120
  //# sourceMappingURL=index.d.mts.map
@@ -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":";;;;;;;;;;;;;cAkCa,gBAAgB;WAClB;WACA;EAGP,YAAA,qBACA;SAMK,KAAK,MAAM,SAAW;SAUtB,YAAY;;;;;;;EAUnB,SAAS,OAAO,SAAW;;EAc3B,WAAW;;EAeX,eAAe;;EAMf,OAAO,OAAO,QAAU;EAcxB,UAAU;;;;KC/GP,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;;;;;;;;;KC1J7B;EAAe;EAAW;EAAW;EAAW;;;iBAK5C,sBAAsB,cAAc;;;;;;;;;;iBAuBpC,sBAAsB,GAAG;;iBAWzB,oBAAoB,GAAG,YAAY,GAAG,aAAa;;iBAUnD,oBAAoB,GAAG,aAAa;;iBAKpC,yBAAyB,GAAG,YAAY"}
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,83 +1,97 @@
1
1
  //#region src/lib/experimental/math/Quaternion.ts
2
2
  const EPSILON = 1e-12;
3
- /** Convert an axis-angle rotation vector [rx, ry, rz] to a unit quaternion. */
4
- function axisAngleToQuaternion(r) {
5
- const rx = r[0] ?? 0;
6
- const ry = r[1] ?? 0;
7
- const rz = r[2] ?? 0;
8
- const angle = Math.sqrt(rx * rx + ry * ry + rz * rz);
9
- if (angle < EPSILON) return {
10
- w: 1,
11
- x: 0,
12
- y: 0,
13
- z: 0
14
- };
15
- const half = angle / 2;
16
- const s = Math.sin(half) / angle;
17
- return {
18
- w: Math.cos(half),
19
- x: rx * s,
20
- y: ry * s,
21
- z: rz * s
22
- };
23
- }
24
- /**
25
- * Convert a unit quaternion back to an axis-angle rotation vector [rx, ry, rz],
26
- * matching wb-robotix's `Quaternion::toRotationVector()` (ported from
27
- * QuaternionBasePlugin.h): using `atan2` rather than `acos` keeps the angle
28
- * canonically within [0, pi] regardless of which of the two antipodal unit
29
- * quaternions (q or -q) represents the rotation, rather than acos's [0, 2*pi]
30
- * range - important since composed poses need to match the same canonical
31
- * rotation vector the robot controller itself would report for a pose.
32
- */
33
- function quaternionToAxisAngle(q) {
34
- const vecNorm = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
35
- if (vecNorm < EPSILON) return [
36
- 0,
37
- 0,
38
- 0
39
- ];
40
- const angle = 2 * Math.atan2(vecNorm, Math.abs(q.w));
41
- const scale = (q.w >= 0 ? angle : -angle) / vecNorm;
42
- return [
43
- q.x * scale,
44
- q.y * scale,
45
- q.z * scale
46
- ];
47
- }
48
- /** Hamilton product: composes rotations so that applying `multiplyQuaternions(a, b)` to a vector equals applying `b` then `a`. */
49
- function multiplyQuaternions(a, b) {
50
- return {
51
- w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
52
- x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
53
- y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
54
- z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w
55
- };
56
- }
57
- /** Conjugate of a unit quaternion equals its inverse. */
58
- function conjugateQuaternion(q) {
59
- return {
60
- w: q.w,
61
- x: -q.x,
62
- y: -q.y,
63
- z: -q.z
64
- };
65
- }
66
- /** Rotate a 3D vector by a unit quaternion. */
67
- function rotateVectorByQuaternion(q, v) {
68
- const vx = v[0] ?? 0;
69
- const vy = v[1] ?? 0;
70
- const vz = v[2] ?? 0;
71
- const { w, x, y, z } = q;
72
- const tx = 2 * (y * vz - z * vy);
73
- const ty = 2 * (z * vx - x * vz);
74
- const tz = 2 * (x * vy - y * vx);
75
- return [
76
- vx + w * tx + (y * tz - z * ty),
77
- vy + w * ty + (z * tx - x * tz),
78
- vz + w * tz + (x * ty - y * tx)
79
- ];
80
- }
3
+ var Quaternion = class Quaternion {
4
+ w;
5
+ x;
6
+ y;
7
+ z;
8
+ constructor(w = 1, x = 0, y = 0, z = 0) {
9
+ this.w = w;
10
+ this.x = x;
11
+ this.y = y;
12
+ this.z = z;
13
+ }
14
+ static identity() {
15
+ return new Quaternion();
16
+ }
17
+ /**
18
+ * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =
19
+ * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported
20
+ * from wb-robotix's `QuaternionBasePlugin.h`).
21
+ */
22
+ static fromRotationVector(rotationVector) {
23
+ const rx = rotationVector[0] ?? 0;
24
+ const ry = rotationVector[1] ?? 0;
25
+ const rz = rotationVector[2] ?? 0;
26
+ const angle = Math.sqrt(rx * rx + ry * ry + rz * rz);
27
+ if (angle < EPSILON) return Quaternion.identity();
28
+ const half = angle / 2;
29
+ const s = Math.sin(half) / angle;
30
+ return new Quaternion(Math.cos(half), rx * s, ry * s, rz * s);
31
+ }
32
+ /**
33
+ * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),
34
+ * matching `Eigen::Quaternion::toRotationVector()` (ported from
35
+ * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`
36
+ * keeps the angle canonically within [0, pi] regardless of which of the
37
+ * two antipodal unit quaternions (this or its negation) represents the
38
+ * rotation, rather than acos's [0, 2*pi] range - important since composed
39
+ * poses need to match the same canonical rotation vector the robot
40
+ * controller itself would report for a pose.
41
+ */
42
+ toRotationVector() {
43
+ const vecNorm = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
44
+ if (vecNorm < EPSILON) return [
45
+ 0,
46
+ 0,
47
+ 0
48
+ ];
49
+ const angle = 2 * Math.atan2(vecNorm, Math.abs(this.w));
50
+ const scale = (this.w >= 0 ? angle : -angle) / vecNorm;
51
+ return [
52
+ this.x * scale,
53
+ this.y * scale,
54
+ this.z * scale
55
+ ];
56
+ }
57
+ /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */
58
+ multiply(other) {
59
+ const { w: aw, x: ax, y: ay, z: az } = this;
60
+ const { w: bw, x: bx, y: by, z: bz } = other;
61
+ 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);
62
+ }
63
+ /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
64
+ conjugate() {
65
+ return new Quaternion(this.w, -this.x, -this.y, -this.z);
66
+ }
67
+ /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
68
+ inverse() {
69
+ return this.conjugate();
70
+ }
71
+ /** Rotate a 3D vector by this quaternion. */
72
+ rotateVector(v) {
73
+ const vx = v[0] ?? 0;
74
+ const vy = v[1] ?? 0;
75
+ const vz = v[2] ?? 0;
76
+ const { w, x, y, z } = this;
77
+ const tx = 2 * (y * vz - z * vy);
78
+ const ty = 2 * (z * vx - x * vz);
79
+ const tz = 2 * (x * vy - y * vx);
80
+ return [
81
+ vx + w * tx + (y * tz - z * ty),
82
+ vy + w * ty + (z * tx - x * tz),
83
+ vz + w * tz + (x * ty - y * tx)
84
+ ];
85
+ }
86
+ toJSON() {
87
+ return {
88
+ w: this.w,
89
+ x: this.x,
90
+ y: this.y,
91
+ z: this.z
92
+ };
93
+ }
94
+ };
81
95
  //#endregion
82
96
  //#region src/lib/experimental/math/Pose.ts
83
97
  const ZERO_VECTOR = [
@@ -105,10 +119,6 @@ function negateVector(a) {
105
119
  * wire-format `PoseData` type (own `position`/`orientation` properties only,
106
120
  * no enumerable methods), so they can be passed directly back into API calls
107
121
  * that expect a pose.
108
- *
109
- * Poses returned from `NovaAPIClient`/`Nova` are automatically upgraded to
110
- * `Pose` instances; construct one directly only when you have a pose from
111
- * elsewhere (e.g. a websocket message).
112
122
  */
113
123
  var Pose = class Pose {
114
124
  position;
@@ -131,25 +141,25 @@ var Pose = class Pose {
131
141
  * applying `other` first, then `this`.
132
142
  */
133
143
  multiply(other) {
134
- const q1 = axisAngleToQuaternion(this.orientation);
135
- const q2 = axisAngleToQuaternion(other.orientation ?? ZERO_VECTOR);
136
- const position = addVectors(this.position, rotateVectorByQuaternion(q1, other.position ?? ZERO_VECTOR));
137
- const orientation = quaternionToAxisAngle(multiplyQuaternions(q1, q2));
144
+ const q1 = Quaternion.fromRotationVector(this.orientation);
145
+ const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR);
146
+ const position = addVectors(this.position, q1.rotateVector(other.position ?? ZERO_VECTOR));
147
+ const orientation = q1.multiply(q2).toRotationVector();
138
148
  return new Pose(position, orientation);
139
149
  }
140
150
  /** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */
141
151
  inverse() {
142
- const position = rotateVectorByQuaternion(conjugateQuaternion(axisAngleToQuaternion(this.orientation)), negateVector(this.position));
152
+ const position = Quaternion.fromRotationVector(this.orientation).inverse().rotateVector(negateVector(this.position));
143
153
  const orientation = negateVector(this.orientation);
144
154
  return new Pose(position, orientation);
145
155
  }
146
156
  /** Apply this pose's transform to a point, returning the transformed point. */
147
157
  transformPoint(point) {
148
- const q = axisAngleToQuaternion(this.orientation);
149
- return addVectors(this.position, rotateVectorByQuaternion(q, point));
158
+ const q = Quaternion.fromRotationVector(this.orientation);
159
+ return addVectors(this.position, q.rotateVector(point));
150
160
  }
151
161
  /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
152
- equals(other, epsilon = 1e-9) {
162
+ isApprox(other, epsilon = 1e-9) {
153
163
  const otherPosition = other.position ?? ZERO_VECTOR;
154
164
  const otherOrientation = other.orientation ?? ZERO_VECTOR;
155
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);
@@ -221,6 +231,6 @@ function augmentMath(nova) {
221
231
  } });
222
232
  }
223
233
  //#endregion
224
- export { Pose, augmentMath, augmentPoses, axisAngleToQuaternion, conjugateQuaternion, multiplyQuaternions, quaternionToAxisAngle, rotateVectorByQuaternion };
234
+ export { Pose, Quaternion, augmentMath, augmentPoses };
225
235
 
226
236
  //# sourceMappingURL=index.mjs.map
@@ -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 * Internal quaternion helpers used to compose/invert the axis-angle\n * `orientation` vectors used by the NOVA API's `Pose` type. Rotation vectors\n * are converted to quaternions for composition (numerically stable, unlike\n * composing axis-angle vectors directly) and converted back at the boundary.\n */\n\nexport type Quaternion = { w: number; x: number; y: number; z: number }\n\nconst EPSILON = 1e-12\n\n/** Convert an axis-angle rotation vector [rx, ry, rz] to a unit quaternion. */\nexport function axisAngleToQuaternion(r: number[]): Quaternion {\n const rx = r[0] ?? 0\n const ry = r[1] ?? 0\n const rz = r[2] ?? 0\n\n const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)\n if (angle < EPSILON) {\n return { w: 1, x: 0, y: 0, z: 0 }\n }\n const half = angle / 2\n const s = Math.sin(half) / angle\n return { w: Math.cos(half), x: rx * s, y: ry * s, z: rz * s }\n}\n\n/**\n * Convert a unit quaternion back to an axis-angle rotation vector [rx, ry, rz],\n * matching wb-robotix's `Quaternion::toRotationVector()` (ported from\n * QuaternionBasePlugin.h): using `atan2` rather than `acos` keeps the angle\n * canonically within [0, pi] regardless of which of the two antipodal unit\n * quaternions (q or -q) represents the rotation, rather than acos's [0, 2*pi]\n * range - important since composed poses need to match the same canonical\n * rotation vector the robot controller itself would report for a pose.\n */\nexport function quaternionToAxisAngle(q: Quaternion): number[] {\n const vecNorm = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z)\n if (vecNorm < EPSILON) {\n return [0, 0, 0]\n }\n const angle = 2 * Math.atan2(vecNorm, Math.abs(q.w))\n const scale = (q.w >= 0 ? angle : -angle) / vecNorm\n return [q.x * scale, q.y * scale, q.z * scale]\n}\n\n/** Hamilton product: composes rotations so that applying `multiplyQuaternions(a, b)` to a vector equals applying `b` then `a`. */\nexport function multiplyQuaternions(a: Quaternion, b: Quaternion): Quaternion {\n return {\n w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,\n x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,\n y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,\n z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,\n }\n}\n\n/** Conjugate of a unit quaternion equals its inverse. */\nexport function conjugateQuaternion(q: Quaternion): Quaternion {\n return { w: q.w, x: -q.x, y: -q.y, z: -q.z }\n}\n\n/** Rotate a 3D vector by a unit quaternion. */\nexport function rotateVectorByQuaternion(q: Quaternion, 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 } = q\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","import type { Pose as PoseData } from \"@wandelbots/nova-api/v2\"\nimport {\n axisAngleToQuaternion,\n conjugateQuaternion,\n multiplyQuaternions,\n quaternionToAxisAngle,\n rotateVectorByQuaternion,\n} 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 *\n * Poses returned from `NovaAPIClient`/`Nova` are automatically upgraded to\n * `Pose` instances; construct one directly only when you have a pose from\n * elsewhere (e.g. a websocket message).\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 = axisAngleToQuaternion(this.orientation)\n const q2 = axisAngleToQuaternion(other.orientation ?? ZERO_VECTOR)\n\n const position = addVectors(\n this.position,\n rotateVectorByQuaternion(q1, other.position ?? ZERO_VECTOR),\n )\n const orientation = quaternionToAxisAngle(multiplyQuaternions(q1, q2))\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 q = axisAngleToQuaternion(this.orientation)\n const qInverse = conjugateQuaternion(q)\n\n const position = rotateVectorByQuaternion(\n qInverse,\n negateVector(this.position),\n )\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 = axisAngleToQuaternion(this.orientation)\n return addVectors(this.position, rotateVectorByQuaternion(q, point))\n }\n\n /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */\n equals(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":";AASA,MAAM,UAAU;;AAGhB,SAAgB,sBAAsB,GAAyB;CAC7D,MAAM,KAAK,EAAE,MAAM;CACnB,MAAM,KAAK,EAAE,MAAM;CACnB,MAAM,KAAK,EAAE,MAAM;CAEnB,MAAM,QAAQ,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,KAAK,EAAE;CACnD,IAAI,QAAQ,SACV,OAAO;EAAE,GAAG;EAAG,GAAG;EAAG,GAAG;EAAG,GAAG;CAAE;CAElC,MAAM,OAAO,QAAQ;CACrB,MAAM,IAAI,KAAK,IAAI,IAAI,IAAI;CAC3B,OAAO;EAAE,GAAG,KAAK,IAAI,IAAI;EAAG,GAAG,KAAK;EAAG,GAAG,KAAK;EAAG,GAAG,KAAK;CAAE;AAC9D;;;;;;;;;;AAWA,SAAgB,sBAAsB,GAAyB;CAC7D,MAAM,UAAU,KAAK,KAAK,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,CAAC;CAC3D,IAAI,UAAU,SACZ,OAAO;EAAC;EAAG;EAAG;CAAC;CAEjB,MAAM,QAAQ,IAAI,KAAK,MAAM,SAAS,KAAK,IAAI,EAAE,CAAC,CAAC;CACnD,MAAM,SAAS,EAAE,KAAK,IAAI,QAAQ,CAAC,SAAS;CAC5C,OAAO;EAAC,EAAE,IAAI;EAAO,EAAE,IAAI;EAAO,EAAE,IAAI;CAAK;AAC/C;;AAGA,SAAgB,oBAAoB,GAAe,GAA2B;CAC5E,OAAO;EACL,GAAG,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE;EAC/C,GAAG,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE;EAC/C,GAAG,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE;EAC/C,GAAG,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE,IAAI,EAAE;CACjD;AACF;;AAGA,SAAgB,oBAAoB,GAA2B;CAC7D,OAAO;EAAE,GAAG,EAAE;EAAG,GAAG,CAAC,EAAE;EAAG,GAAG,CAAC,EAAE;EAAG,GAAG,CAAC,EAAE;CAAE;AAC7C;;AAGA,SAAgB,yBAAyB,GAAe,GAAuB;CAC7E,MAAM,KAAK,EAAE,MAAM;CACnB,MAAM,KAAK,EAAE,MAAM;CACnB,MAAM,KAAK,EAAE,MAAM;CACnB,MAAM,EAAE,GAAG,GAAG,GAAG,MAAM;CAGvB,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;CAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;CAC7B,MAAM,KAAK,KAAK,IAAI,KAAK,IAAI;CAG7B,OAAO;EACL,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;EAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;EAC5B,KAAK,IAAI,MAAM,IAAI,KAAK,IAAI;CAC9B;AACF;;;ACrEA,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;;;;;;;;;;;;AAaA,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,sBAAsB,KAAK,WAAW;EACjD,MAAM,KAAK,sBAAsB,MAAM,eAAe,WAAW;EAEjE,MAAM,WAAW,WACf,KAAK,UACL,yBAAyB,IAAI,MAAM,YAAY,WAAW,CAC5D;EACA,MAAM,cAAc,sBAAsB,oBAAoB,IAAI,EAAE,CAAC;EAErE,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,UAAgB;EAId,MAAM,WAAW,yBAFA,oBADP,sBAAsB,KAAK,WACA,CAGnC,GACA,aAAa,KAAK,QAAQ,CAC5B;EAEA,MAAM,cAAc,aAAa,KAAK,WAAW;EAEjD,OAAO,IAAI,KAAK,UAAU,WAAW;CACvC;;CAGA,eAAe,OAA2B;EACxC,MAAM,IAAI,sBAAsB,KAAK,WAAW;EAChD,OAAO,WAAW,KAAK,UAAU,yBAAyB,GAAG,KAAK,CAAC;CACrE;;CAGA,OAAO,OAAiB,UAAU,MAAe;EAC/C,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;;;ACnEA,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/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"}
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.6592c26",
4
+ "version": "4.3.0-pr.318.c6d70a6",
5
5
  "description": "Official JS client for the Wandelbots API",
6
6
  "sideEffects": false,
7
7
  "packageManager": "pnpm@11.9.0+sha512.bd682d5d03fe525ef7c9fd6780c6884d1e756ac4c9c9fe00c538782824310dcf90e3ddc4f53835f06dfaebd5085e41855e0bcbb3b60de2ac5bbab89e5036f03b",
@@ -14,11 +14,5 @@ export type {
14
14
  NovaWithMath,
15
15
  } from "../../lib/experimental/math/augmentMath.ts"
16
16
  export { Pose } from "../../lib/experimental/math/Pose.ts"
17
- export {
18
- axisAngleToQuaternion,
19
- conjugateQuaternion,
20
- multiplyQuaternions,
21
- quaternionToAxisAngle,
22
- rotateVectorByQuaternion,
23
- } from "../../lib/experimental/math/Quaternion.ts"
24
- export type { Quaternion } from "../../lib/experimental/math/Quaternion.ts"
17
+ export { Quaternion } from "../../lib/experimental/math/Quaternion.ts"
18
+ export type { QuaternionData } from "../../lib/experimental/math/Quaternion.ts"
@@ -1,11 +1,5 @@
1
1
  import type { Pose as PoseData } from "@wandelbots/nova-api/v2"
2
- import {
3
- axisAngleToQuaternion,
4
- conjugateQuaternion,
5
- multiplyQuaternions,
6
- quaternionToAxisAngle,
7
- rotateVectorByQuaternion,
8
- } from "./Quaternion.ts"
2
+ import { Quaternion } from "./Quaternion.ts"
9
3
 
10
4
  const ZERO_VECTOR = [0, 0, 0]
11
5
 
@@ -27,10 +21,6 @@ function negateVector(a: number[]): number[] {
27
21
  * wire-format `PoseData` type (own `position`/`orientation` properties only,
28
22
  * no enumerable methods), so they can be passed directly back into API calls
29
23
  * that expect a pose.
30
- *
31
- * Poses returned from `NovaAPIClient`/`Nova` are automatically upgraded to
32
- * `Pose` instances; construct one directly only when you have a pose from
33
- * elsewhere (e.g. a websocket message).
34
24
  */
35
25
  export class Pose implements PoseData {
36
26
  readonly position: number[]
@@ -65,27 +55,23 @@ export class Pose implements PoseData {
65
55
  * applying `other` first, then `this`.
66
56
  */
67
57
  multiply(other: PoseData): Pose {
68
- const q1 = axisAngleToQuaternion(this.orientation)
69
- const q2 = axisAngleToQuaternion(other.orientation ?? ZERO_VECTOR)
58
+ const q1 = Quaternion.fromRotationVector(this.orientation)
59
+ const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR)
70
60
 
71
61
  const position = addVectors(
72
62
  this.position,
73
- rotateVectorByQuaternion(q1, other.position ?? ZERO_VECTOR),
63
+ q1.rotateVector(other.position ?? ZERO_VECTOR),
74
64
  )
75
- const orientation = quaternionToAxisAngle(multiplyQuaternions(q1, q2))
65
+ const orientation = q1.multiply(q2).toRotationVector()
76
66
 
77
67
  return new Pose(position, orientation)
78
68
  }
79
69
 
80
70
  /** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */
81
71
  inverse(): Pose {
82
- const q = axisAngleToQuaternion(this.orientation)
83
- const qInverse = conjugateQuaternion(q)
72
+ const qInverse = Quaternion.fromRotationVector(this.orientation).inverse()
84
73
 
85
- const position = rotateVectorByQuaternion(
86
- qInverse,
87
- negateVector(this.position),
88
- )
74
+ const position = qInverse.rotateVector(negateVector(this.position))
89
75
  // Negating an axis-angle vector gives the inverse rotation directly.
90
76
  const orientation = negateVector(this.orientation)
91
77
 
@@ -94,12 +80,12 @@ export class Pose implements PoseData {
94
80
 
95
81
  /** Apply this pose's transform to a point, returning the transformed point. */
96
82
  transformPoint(point: number[]): number[] {
97
- const q = axisAngleToQuaternion(this.orientation)
98
- return addVectors(this.position, rotateVectorByQuaternion(q, point))
83
+ const q = Quaternion.fromRotationVector(this.orientation)
84
+ return addVectors(this.position, q.rotateVector(point))
99
85
  }
100
86
 
101
87
  /** Compare to another pose within a tolerance, since floating-point pose math rarely produces exact equality. */
102
- equals(other: PoseData, epsilon = 1e-9): boolean {
88
+ isApprox(other: PoseData, epsilon = 1e-9): boolean {
103
89
  const otherPosition = other.position ?? ZERO_VECTOR
104
90
  const otherOrientation = other.orientation ?? ZERO_VECTOR
105
91
 
@@ -1,79 +1,116 @@
1
1
  /**
2
- * Internal quaternion helpers used to compose/invert the axis-angle
3
- * `orientation` vectors used by the NOVA API's `Pose` type. Rotation vectors
4
- * are converted to quaternions for composition (numerically stable, unlike
5
- * composing axis-angle vectors directly) and converted back at the boundary.
2
+ * A unit quaternion representing a 3D rotation. Mirrors the subset of
3
+ * `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that
4
+ * `Pose` composition needs: Hamilton product, conjugate/inverse, vector
5
+ * rotation, and axis-angle rotation-vector conversion. Instances are
6
+ * immutable - every method returns a new `Quaternion`.
6
7
  */
7
8
 
8
- export type Quaternion = { w: number; x: number; y: number; z: number }
9
+ export type QuaternionData = { w: number; x: number; y: number; z: number }
9
10
 
10
11
  const EPSILON = 1e-12
11
12
 
12
- /** Convert an axis-angle rotation vector [rx, ry, rz] to a unit quaternion. */
13
- export function axisAngleToQuaternion(r: number[]): Quaternion {
14
- const rx = r[0] ?? 0
15
- const ry = r[1] ?? 0
16
- const rz = r[2] ?? 0
13
+ export class Quaternion implements QuaternionData {
14
+ readonly w: number
15
+ readonly x: number
16
+ readonly y: number
17
+ readonly z: number
17
18
 
18
- const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)
19
- if (angle < EPSILON) {
20
- return { w: 1, x: 0, y: 0, z: 0 }
19
+ constructor(w = 1, x = 0, y = 0, z = 0) {
20
+ this.w = w
21
+ this.x = x
22
+ this.y = y
23
+ this.z = z
21
24
  }
22
- const half = angle / 2
23
- const s = Math.sin(half) / angle
24
- return { w: Math.cos(half), x: rx * s, y: ry * s, z: rz * s }
25
- }
26
25
 
27
- /**
28
- * Convert a unit quaternion back to an axis-angle rotation vector [rx, ry, rz],
29
- * matching wb-robotix's `Quaternion::toRotationVector()` (ported from
30
- * QuaternionBasePlugin.h): using `atan2` rather than `acos` keeps the angle
31
- * canonically within [0, pi] regardless of which of the two antipodal unit
32
- * quaternions (q or -q) represents the rotation, rather than acos's [0, 2*pi]
33
- * range - important since composed poses need to match the same canonical
34
- * rotation vector the robot controller itself would report for a pose.
35
- */
36
- export function quaternionToAxisAngle(q: Quaternion): number[] {
37
- const vecNorm = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z)
38
- if (vecNorm < EPSILON) {
39
- return [0, 0, 0]
26
+ static identity(): Quaternion {
27
+ return new Quaternion()
40
28
  }
41
- const angle = 2 * Math.atan2(vecNorm, Math.abs(q.w))
42
- const scale = (q.w >= 0 ? angle : -angle) / vecNorm
43
- return [q.x * scale, q.y * scale, q.z * scale]
44
- }
45
29
 
46
- /** Hamilton product: composes rotations so that applying `multiplyQuaternions(a, b)` to a vector equals applying `b` then `a`. */
47
- export function multiplyQuaternions(a: Quaternion, b: Quaternion): Quaternion {
48
- return {
49
- w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
50
- x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
51
- y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
52
- z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
30
+ /**
31
+ * Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =
32
+ * angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported
33
+ * from wb-robotix's `QuaternionBasePlugin.h`).
34
+ */
35
+ static fromRotationVector(rotationVector: number[]): Quaternion {
36
+ const rx = rotationVector[0] ?? 0
37
+ const ry = rotationVector[1] ?? 0
38
+ const rz = rotationVector[2] ?? 0
39
+
40
+ const angle = Math.sqrt(rx * rx + ry * ry + rz * rz)
41
+ if (angle < EPSILON) {
42
+ return Quaternion.identity()
43
+ }
44
+ const half = angle / 2
45
+ const s = Math.sin(half) / angle
46
+ return new Quaternion(Math.cos(half), rx * s, ry * s, rz * s)
53
47
  }
54
- }
55
48
 
56
- /** Conjugate of a unit quaternion equals its inverse. */
57
- export function conjugateQuaternion(q: Quaternion): Quaternion {
58
- return { w: q.w, x: -q.x, y: -q.y, z: -q.z }
59
- }
49
+ /**
50
+ * Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),
51
+ * matching `Eigen::Quaternion::toRotationVector()` (ported from
52
+ * wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`
53
+ * keeps the angle canonically within [0, pi] regardless of which of the
54
+ * two antipodal unit quaternions (this or its negation) represents the
55
+ * rotation, rather than acos's [0, 2*pi] range - important since composed
56
+ * poses need to match the same canonical rotation vector the robot
57
+ * controller itself would report for a pose.
58
+ */
59
+ toRotationVector(): number[] {
60
+ const vecNorm = Math.sqrt(
61
+ this.x * this.x + this.y * this.y + this.z * this.z,
62
+ )
63
+ if (vecNorm < EPSILON) {
64
+ return [0, 0, 0]
65
+ }
66
+ const angle = 2 * Math.atan2(vecNorm, Math.abs(this.w))
67
+ const scale = (this.w >= 0 ? angle : -angle) / vecNorm
68
+ return [this.x * scale, this.y * scale, this.z * scale]
69
+ }
60
70
 
61
- /** Rotate a 3D vector by a unit quaternion. */
62
- export function rotateVectorByQuaternion(q: Quaternion, v: number[]): number[] {
63
- const vx = v[0] ?? 0
64
- const vy = v[1] ?? 0
65
- const vz = v[2] ?? 0
66
- const { w, x, y, z } = q
71
+ /** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */
72
+ multiply(other: QuaternionData): Quaternion {
73
+ const { w: aw, x: ax, y: ay, z: az } = this
74
+ const { w: bw, x: bx, y: by, z: bz } = other
75
+ return new Quaternion(
76
+ aw * bw - ax * bx - ay * by - az * bz,
77
+ aw * bx + ax * bw + ay * bz - az * by,
78
+ aw * by - ax * bz + ay * bw + az * bx,
79
+ aw * bz + ax * by - ay * bx + az * bw,
80
+ )
81
+ }
67
82
 
68
- // t = 2 * cross(q.xyz, v)
69
- const tx = 2 * (y * vz - z * vy)
70
- const ty = 2 * (z * vx - x * vz)
71
- const tz = 2 * (x * vy - y * vx)
83
+ /** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
84
+ conjugate(): Quaternion {
85
+ return new Quaternion(this.w, -this.x, -this.y, -this.z)
86
+ }
87
+
88
+ /** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
89
+ inverse(): Quaternion {
90
+ return this.conjugate()
91
+ }
92
+
93
+ /** Rotate a 3D vector by this quaternion. */
94
+ rotateVector(v: number[]): number[] {
95
+ const vx = v[0] ?? 0
96
+ const vy = v[1] ?? 0
97
+ const vz = v[2] ?? 0
98
+ const { w, x, y, z } = this
72
99
 
73
- // v' = v + w*t + cross(q.xyz, t)
74
- return [
75
- vx + w * tx + (y * tz - z * ty),
76
- vy + w * ty + (z * tx - x * tz),
77
- vz + w * tz + (x * ty - y * tx),
78
- ]
100
+ // t = 2 * cross(q.xyz, v)
101
+ const tx = 2 * (y * vz - z * vy)
102
+ const ty = 2 * (z * vx - x * vz)
103
+ const tz = 2 * (x * vy - y * vx)
104
+
105
+ // v' = v + w*t + cross(q.xyz, t)
106
+ return [
107
+ vx + w * tx + (y * tz - z * ty),
108
+ vy + w * ty + (z * tx - x * tz),
109
+ vz + w * tz + (x * ty - y * tx),
110
+ ]
111
+ }
112
+
113
+ toJSON(): QuaternionData {
114
+ return { w: this.w, x: this.x, y: this.y, z: this.z }
115
+ }
79
116
  }