@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.
- package/dist/experimental/math/index.d.mts +42 -29
- package/dist/experimental/math/index.d.mts.map +1 -1
- package/dist/experimental/math/index.mjs +101 -91
- package/dist/experimental/math/index.mjs.map +1 -1
- package/package.json +1 -1
- package/src/experimental/math/index.ts +2 -8
- package/src/lib/experimental/math/Pose.ts +10 -24
- package/src/lib/experimental/math/Quaternion.ts +99 -62
|
@@ -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
|
-
|
|
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
|
-
*
|
|
77
|
-
* `
|
|
78
|
-
*
|
|
79
|
-
*
|
|
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
|
|
78
|
+
type QuaternionData = {
|
|
82
79
|
w: number;
|
|
83
80
|
x: number;
|
|
84
81
|
y: number;
|
|
85
82
|
z: number;
|
|
86
83
|
};
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
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
|
|
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":"
|
|
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
|
-
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
|
|
7
|
-
|
|
8
|
-
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
}
|
|
57
|
-
/**
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
w:
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
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 =
|
|
135
|
-
const q2 =
|
|
136
|
-
const position = addVectors(this.position,
|
|
137
|
-
const orientation =
|
|
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 =
|
|
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 =
|
|
149
|
-
return addVectors(this.position,
|
|
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
|
-
|
|
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
|
|
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.
|
|
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
|
-
|
|
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 =
|
|
69
|
-
const q2 =
|
|
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
|
-
|
|
63
|
+
q1.rotateVector(other.position ?? ZERO_VECTOR),
|
|
74
64
|
)
|
|
75
|
-
const orientation =
|
|
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
|
|
83
|
-
const qInverse = conjugateQuaternion(q)
|
|
72
|
+
const qInverse = Quaternion.fromRotationVector(this.orientation).inverse()
|
|
84
73
|
|
|
85
|
-
const position =
|
|
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 =
|
|
98
|
-
return addVectors(this.position,
|
|
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
|
-
|
|
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
|
-
*
|
|
3
|
-
* `
|
|
4
|
-
*
|
|
5
|
-
*
|
|
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
|
|
9
|
+
export type QuaternionData = { w: number; x: number; y: number; z: number }
|
|
9
10
|
|
|
10
11
|
const EPSILON = 1e-12
|
|
11
12
|
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
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
|
-
|
|
19
|
-
|
|
20
|
-
|
|
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
|
-
|
|
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
|
-
/**
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
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
|
-
/**
|
|
57
|
-
|
|
58
|
-
|
|
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
|
-
/**
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
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
|
-
|
|
69
|
-
|
|
70
|
-
|
|
71
|
-
|
|
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
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
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
|
}
|