@wandelbots/nova-js 4.3.0-pr.318.6592c26 → 4.3.0-pr.318.66ea9ed
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/Nova-CVMwZBmE.d.mts +84 -0
- package/dist/Nova-CVMwZBmE.d.mts.map +1 -0
- package/dist/experimental/math/index.d.mts +64 -69
- package/dist/experimental/math/index.d.mts.map +1 -1
- package/dist/experimental/math/index.mjs +156 -154
- package/dist/experimental/math/index.mjs.map +1 -1
- package/dist/experimental/nats/index.d.mts +1 -1
- package/dist/{Nova-DdbUm145.d.mts → index-O_-XIJE-.d.mts} +2 -82
- package/dist/index-O_-XIJE-.d.mts.map +1 -0
- package/dist/v2/index.d.mts +2 -1
- package/package.json +1 -1
- package/src/experimental/math/index.ts +3 -17
- package/src/lib/experimental/math/Pose.ts +76 -33
- package/src/lib/experimental/math/Quaternion.ts +120 -64
- package/dist/Nova-DdbUm145.d.mts.map +0 -1
- package/src/lib/experimental/math/augmentMath.ts +0 -175
|
@@ -0,0 +1,84 @@
|
|
|
1
|
+
import { Du as TrajectoryCachingApi, Ju as TrajectoryPlanningApi, Lo as MotionGroupApi, Mc as ProgramApi, Pn as ControllerApi, Ru as TrajectoryExecutionApi, Td as VirtualControllerInputsOutputsApi, Zl as StoreCollisionComponentsApi, _d as VirtualControllerApi, au as StoreObjectApi, fd as VersionApi, fn as Configuration, h as ApplicationApi, io as LicenseApi, kl as SessionApi, ps as NOVACloudApi, pt as CellApi, qa as KinematicsApi, qo as MotionGroupModelsApi, sa as JoggingApi, tl as RobotConfigurationsApi, tu as StoreCollisionSetupsApi, uu as SystemApi, x as BUSInputsOutputsApi, xd as VirtualControllerBehaviorApi, zn as ControllerInputsOutputsApi } from "./index-O_-XIJE-.mjs";
|
|
2
|
+
import { n as MockNovaInstance, t as AutoReconnectingWebsocket } from "./AutoReconnectingWebsocket-NVyczzLi.mjs";
|
|
3
|
+
import { AxiosInstance } from "axios";
|
|
4
|
+
//#region src/lib/NovaAPIClient.d.ts
|
|
5
|
+
type UnwrapAxiosResponseReturn<T> = T extends ((...a: any[]) => any) ? (...a: Parameters<T>) => Promise<Awaited<ReturnType<T>> extends {
|
|
6
|
+
data: infer D;
|
|
7
|
+
} ? D : never> : never;
|
|
8
|
+
type WithUnwrappedAxiosResponse<T> = { [P in keyof T]: UnwrapAxiosResponseReturn<T[P]>; };
|
|
9
|
+
type NovaAPIClientOpts = Configuration & {
|
|
10
|
+
axiosInstance?: AxiosInstance;
|
|
11
|
+
mock?: boolean;
|
|
12
|
+
};
|
|
13
|
+
/**
|
|
14
|
+
* API client providing type-safe access to all the endpoints of a NOVA
|
|
15
|
+
* instance.
|
|
16
|
+
*/
|
|
17
|
+
declare class NovaAPIClient {
|
|
18
|
+
readonly opts: NovaAPIClientOpts;
|
|
19
|
+
readonly application: WithUnwrappedAxiosResponse<ApplicationApi>;
|
|
20
|
+
readonly busIOs: WithUnwrappedAxiosResponse<BUSInputsOutputsApi>;
|
|
21
|
+
readonly cell: WithUnwrappedAxiosResponse<CellApi>;
|
|
22
|
+
readonly controller: WithUnwrappedAxiosResponse<ControllerApi>;
|
|
23
|
+
readonly controllerIOs: WithUnwrappedAxiosResponse<ControllerInputsOutputsApi>;
|
|
24
|
+
readonly jogging: WithUnwrappedAxiosResponse<JoggingApi>;
|
|
25
|
+
readonly kinematics: WithUnwrappedAxiosResponse<KinematicsApi>;
|
|
26
|
+
readonly license: WithUnwrappedAxiosResponse<LicenseApi>;
|
|
27
|
+
readonly motionGroup: WithUnwrappedAxiosResponse<MotionGroupApi>;
|
|
28
|
+
readonly motionGroupModels: WithUnwrappedAxiosResponse<MotionGroupModelsApi>;
|
|
29
|
+
readonly novaCloud: WithUnwrappedAxiosResponse<NOVACloudApi>;
|
|
30
|
+
readonly program: WithUnwrappedAxiosResponse<ProgramApi>;
|
|
31
|
+
readonly robotConfigurations: WithUnwrappedAxiosResponse<RobotConfigurationsApi>;
|
|
32
|
+
readonly session: WithUnwrappedAxiosResponse<SessionApi>;
|
|
33
|
+
readonly storeCollisionComponents: WithUnwrappedAxiosResponse<StoreCollisionComponentsApi>;
|
|
34
|
+
readonly storeCollisionSetups: WithUnwrappedAxiosResponse<StoreCollisionSetupsApi>;
|
|
35
|
+
readonly storeObject: WithUnwrappedAxiosResponse<StoreObjectApi>;
|
|
36
|
+
readonly system: WithUnwrappedAxiosResponse<SystemApi>;
|
|
37
|
+
readonly trajectoryCaching: WithUnwrappedAxiosResponse<TrajectoryCachingApi>;
|
|
38
|
+
readonly trajectoryExecution: WithUnwrappedAxiosResponse<TrajectoryExecutionApi>;
|
|
39
|
+
readonly trajectoryPlanning: WithUnwrappedAxiosResponse<TrajectoryPlanningApi>;
|
|
40
|
+
readonly version: WithUnwrappedAxiosResponse<VersionApi>;
|
|
41
|
+
readonly virtualController: WithUnwrappedAxiosResponse<VirtualControllerApi>;
|
|
42
|
+
readonly virtualControllerBehavior: WithUnwrappedAxiosResponse<VirtualControllerBehaviorApi>;
|
|
43
|
+
readonly virtualControllerIOs: WithUnwrappedAxiosResponse<VirtualControllerInputsOutputsApi>;
|
|
44
|
+
constructor(opts: NovaAPIClientOpts);
|
|
45
|
+
}
|
|
46
|
+
//#endregion
|
|
47
|
+
//#region src/lib/Nova.d.ts
|
|
48
|
+
type NovaConfig = {
|
|
49
|
+
/**
|
|
50
|
+
* Url of the deployed NOVA instance to connect to
|
|
51
|
+
* e.g. https://saeattii.instance.wandelbots.io
|
|
52
|
+
*/
|
|
53
|
+
instanceUrl: string;
|
|
54
|
+
/**
|
|
55
|
+
* Access token for Bearer authentication.
|
|
56
|
+
* If running on a NOVA instance, this can be automatically retrieved from
|
|
57
|
+
* the current session when omitted.
|
|
58
|
+
*/
|
|
59
|
+
accessToken?: string;
|
|
60
|
+
} & Omit<Configuration, "isJsonMime" | "basePath">;
|
|
61
|
+
/**
|
|
62
|
+
*
|
|
63
|
+
* Client for connecting to a NOVA instance and controlling robots.
|
|
64
|
+
*/
|
|
65
|
+
declare class Nova {
|
|
66
|
+
readonly api: NovaAPIClient;
|
|
67
|
+
readonly config: NovaConfig;
|
|
68
|
+
readonly mock?: MockNovaInstance;
|
|
69
|
+
readonly instanceUrl: URL;
|
|
70
|
+
authPromise: Promise<string | null> | null;
|
|
71
|
+
accessToken: string | null;
|
|
72
|
+
constructor(config: NovaConfig);
|
|
73
|
+
renewAuthentication(): Promise<void>;
|
|
74
|
+
makeWebsocketURL(path: string): string;
|
|
75
|
+
/**
|
|
76
|
+
* Retrieve an AutoReconnectingWebsocket to the given path on the Nova instance.
|
|
77
|
+
* If you explicitly want to reconnect an existing websocket, call `reconnect`
|
|
78
|
+
* on the returned object.
|
|
79
|
+
*/
|
|
80
|
+
openReconnectingWebsocket(path: string): AutoReconnectingWebsocket;
|
|
81
|
+
}
|
|
82
|
+
//#endregion
|
|
83
|
+
export { NovaConfig as n, NovaAPIClient as r, Nova as t };
|
|
84
|
+
//# sourceMappingURL=Nova-CVMwZBmE.d.mts.map
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
{"version":3,"file":"Nova-CVMwZBmE.d.mts","names":[],"sources":["../src/lib/NovaAPIClient.ts","../src/lib/Nova.ts"],"mappings":";;;;KAmCK,0BAA0B,KAAK,eAAc,wBAEzC,GAAG,WAAW,OACd,QAAQ,QAAQ,WAAW;EAAc,YAAY;IAAM;KAG/D,2BAA2B,QAC7B,WAAW,IAAI,0BAA0B,EAAE;KAGzC,oBAAoB;EACvB,gBAAgB;EAChB;;;;;;cAuCW;WACF,MAAM;WAEN,aAAa,2BAA2B;WACxC,QAAQ,2BAA2B;WACnC,MAAM,2BAA2B;WACjC,YAAY,2BAA2B;WACvC,eAAe,2BAA2B;WAC1C,SAAS,2BAA2B;WACpC,YAAY,2BAA2B;WACvC,SAAS,2BAA2B;WACpC,aAAa,2BAA2B;WACxC,mBAAmB,2BAA2B;WAC9C,WAAW,2BAA2B;WACtC,SAAS,2BAA2B;WACpC,qBAAqB,2BAA2B;WAChD,SAAS,2BAA2B;WACpC,0BAA0B,2BAA2B;WACrD,sBAAsB,2BAA2B;WACjD,aAAa,2BAA2B;WACxC,QAAQ,2BAA2B;WACnC,mBAAmB,2BAA2B;WAC9C,qBAAqB,2BAA2B;WAChD,oBAAoB,2BAA2B;WAC/C,SAAS,2BAA2B;WACpC,mBAAmB,2BAA2B;WAC9C,2BAA2B,2BAA2B;WACtD,sBAAsB,2BAA2B;EAE9C,YAAA,MAAM;;;;KCvGR;;;;;EAKV;;;;;;EAOA;IACE,KAAK;;;;;cAMI;WACF,KAAK;WACL,QAAQ;WACR,OAAO;WACP,aAAa;EACtB,aAAa;EACb;EAEY,YAAA,QAAQ;EAqId,uBAAuB;EA4B7B,iBAAiB;;;;;;EAiBjB,0BAA0B,eAAY"}
|
|
@@ -1,4 +1,4 @@
|
|
|
1
|
-
import {
|
|
1
|
+
import { _c as Pose$1 } from "../../index-O_-XIJE-.mjs";
|
|
2
2
|
//#region src/lib/experimental/math/Pose.d.ts
|
|
3
3
|
/**
|
|
4
4
|
* A `Pose` (position + axis-angle orientation) with methods for composing
|
|
@@ -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[];
|
|
@@ -28,80 +24,79 @@ declare class Pose implements Pose$1 {
|
|
|
28
24
|
inverse(): Pose;
|
|
29
25
|
/** Apply this pose's transform to a point, returning the transformed point. */
|
|
30
26
|
transformPoint(point: number[]): number[];
|
|
31
|
-
/**
|
|
32
|
-
|
|
27
|
+
/**
|
|
28
|
+
* Compare to another pose using separate position/orientation tolerances,
|
|
29
|
+
* matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
|
|
30
|
+
* position, and quaternion angular distance for orientation - not a naive
|
|
31
|
+
* per-component diff, since two rotation vectors can represent nearly
|
|
32
|
+
* identical rotations while differing componentwise near a
|
|
33
|
+
* canonicalization boundary (e.g. close to the +/-pi wraparound).
|
|
34
|
+
*/
|
|
35
|
+
isApprox(other: Pose$1, deltaPosition?: number, deltaOrientation?: number): boolean;
|
|
36
|
+
/**
|
|
37
|
+
* Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
|
|
38
|
+
* (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
|
|
39
|
+
* wb-robotix's `Pose::toCartesian()`.
|
|
40
|
+
*/
|
|
41
|
+
toCartesian(): number[];
|
|
42
|
+
/** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
|
|
43
|
+
toString(precision?: number): string;
|
|
33
44
|
toJSON(): Pose$1;
|
|
34
45
|
}
|
|
35
46
|
//#endregion
|
|
36
|
-
//#region src/lib/experimental/math/augmentMath.d.ts
|
|
37
|
-
type PoseKeys = keyof Pose$1;
|
|
38
|
-
type IsExactlyPoseData<T> = [Exclude<keyof T, PoseKeys>] extends [never] ? [Exclude<PoseKeys, keyof T>] extends [never] ? true : false : false;
|
|
39
|
-
type MaxDepth = [unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown];
|
|
40
|
-
type AugmentPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : IsExactlyPoseData<T> extends true ? Pose : T extends readonly (infer U)[] ? DeepPoseAugmented<U, [...Depth, unknown]>[] : T extends object ? { [K in keyof T]: DeepPoseAugmented<T[K], [...Depth, unknown]>; } : T;
|
|
41
|
-
/**
|
|
42
|
-
* Type-level counterpart to `augmentPoses`: recursively replaces any
|
|
43
|
-
* `PoseData`-shaped field with `Pose` (distributing over unions, e.g. so
|
|
44
|
-
* optional `Pose | undefined` fields keep the `| undefined`), so the static
|
|
45
|
-
* type matches what's actually returned at runtime.
|
|
46
|
-
*/
|
|
47
|
-
type DeepPoseAugmented<T, Depth extends readonly unknown[] = []> = T extends unknown ? AugmentPosesCore<T, Depth> : never;
|
|
48
|
-
/**
|
|
49
|
-
* Recursively walks a parsed API response/request body, upgrading any
|
|
50
|
-
* `Pose`-shaped objects in place to `Pose` instances so they gain math
|
|
51
|
-
* methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
|
|
52
|
-
* Used by `augmentMath` for `nova.api.*` responses; exported so it can also
|
|
53
|
-
* be applied manually to e.g. websocket messages.
|
|
54
|
-
*/
|
|
55
|
-
declare function augmentPoses<T>(value: T): DeepPoseAugmented<T>;
|
|
56
|
-
type WithAugmentedPosesCore<T, Depth extends readonly unknown[]> = Depth["length"] extends MaxDepth["length"] ? T : T extends ((...args: infer A) => Promise<infer R>) ? (...args: A) => Promise<DeepPoseAugmented<R>> : T extends object ? { [K in keyof T]: WithAugmentedPosesCore<T[K], [...Depth, unknown]>; } : T;
|
|
57
|
-
/**
|
|
58
|
-
* Recursively maps every method on `Nova["api"]` (and its nested API groups)
|
|
59
|
-
* so calls are typed as returning `DeepPoseAugmented` results, matching what
|
|
60
|
-
* `augmentMath` does at runtime.
|
|
61
|
-
*/
|
|
62
|
-
type WithAugmentedPoses<T> = WithAugmentedPosesCore<T, []>;
|
|
63
|
-
type NovaWithMath = Omit<Nova, "api"> & {
|
|
64
|
-
readonly api: WithAugmentedPoses<Nova["api"]>;
|
|
65
|
-
};
|
|
66
|
-
/**
|
|
67
|
-
* Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades
|
|
68
|
-
* `Pose`-shaped fields in its response to `Pose` instances (with math
|
|
69
|
-
* methods like `multiply`/`inverse`). Returns a new proxied view - does not
|
|
70
|
-
* mutate the original `nova` instance.
|
|
71
|
-
*/
|
|
72
|
-
declare function augmentMath(nova: Nova): NovaWithMath;
|
|
73
|
-
//#endregion
|
|
74
47
|
//#region src/lib/experimental/math/Quaternion.d.ts
|
|
75
48
|
/**
|
|
76
|
-
*
|
|
77
|
-
* `
|
|
78
|
-
*
|
|
79
|
-
*
|
|
49
|
+
* A unit quaternion representing a 3D rotation. Mirrors the subset of
|
|
50
|
+
* `Eigen::Quaternion`'s API (see wb-robotix's `QuaternionBasePlugin.h`) that
|
|
51
|
+
* `Pose` composition needs: Hamilton product, conjugate/inverse, vector
|
|
52
|
+
* rotation, and axis-angle rotation-vector conversion. Instances are
|
|
53
|
+
* immutable - every method returns a new `Quaternion`.
|
|
80
54
|
*/
|
|
81
|
-
type
|
|
55
|
+
type QuaternionData = {
|
|
82
56
|
w: number;
|
|
83
57
|
x: number;
|
|
84
58
|
y: number;
|
|
85
59
|
z: number;
|
|
86
60
|
};
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
61
|
+
declare class Quaternion implements QuaternionData {
|
|
62
|
+
readonly w: number;
|
|
63
|
+
readonly x: number;
|
|
64
|
+
readonly y: number;
|
|
65
|
+
readonly z: number;
|
|
66
|
+
constructor(w?: number, x?: number, y?: number, z?: number);
|
|
67
|
+
static identity(): Quaternion;
|
|
68
|
+
/**
|
|
69
|
+
* Construct from an axis-angle rotation vector [rx, ry, rz] (magnitude =
|
|
70
|
+
* angle in rad), matching `Eigen::Quaternion::FromRotationVector` (ported
|
|
71
|
+
* from wb-robotix's `QuaternionBasePlugin.h`).
|
|
72
|
+
*/
|
|
73
|
+
static fromRotationVector(rotationVector: number[]): Quaternion;
|
|
74
|
+
/**
|
|
75
|
+
* Rotation vector [rx, ry, rz] representation (magnitude = angle in rad),
|
|
76
|
+
* matching `Eigen::Quaternion::toRotationVector()` (ported from
|
|
77
|
+
* wb-robotix's `QuaternionBasePlugin.h`): using `atan2` rather than `acos`
|
|
78
|
+
* keeps the angle canonically within [0, pi] regardless of which of the
|
|
79
|
+
* two antipodal unit quaternions (this or its negation) represents the
|
|
80
|
+
* rotation, rather than acos's [0, 2*pi] range - important since composed
|
|
81
|
+
* poses need to match the same canonical rotation vector the robot
|
|
82
|
+
* controller itself would report for a pose.
|
|
83
|
+
*/
|
|
84
|
+
toRotationVector(): number[];
|
|
85
|
+
/** Hamilton product: `a.multiply(b)` applied to a vector rotates by `b` first, then by `a`. */
|
|
86
|
+
multiply(other: QuaternionData): Quaternion;
|
|
87
|
+
/** Conjugate: negates the vector part. Equal to `inverse()` for unit quaternions. */
|
|
88
|
+
conjugate(): Quaternion;
|
|
89
|
+
/** Inverse rotation. Equivalent to `conjugate()` since a `Pose`'s orientation is always a unit quaternion. */
|
|
90
|
+
inverse(): Quaternion;
|
|
91
|
+
/**
|
|
92
|
+
* The angle (in rad, always within [0, pi]) of the rotation that takes
|
|
93
|
+
* `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
|
|
94
|
+
*/
|
|
95
|
+
angularDistance(other: QuaternionData): number;
|
|
96
|
+
/** Rotate a 3D vector by this quaternion. */
|
|
97
|
+
rotateVector(v: number[]): number[];
|
|
98
|
+
toJSON(): QuaternionData;
|
|
99
|
+
}
|
|
105
100
|
//#endregion
|
|
106
|
-
export {
|
|
101
|
+
export { Pose, type Pose$1 as PoseData, Quaternion, type QuaternionData };
|
|
107
102
|
//# 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/
|
|
1
|
+
{"version":3,"file":"index.d.mts","names":[],"sources":["../../../src/lib/experimental/math/Pose.ts","../../../src/lib/experimental/math/Quaternion.ts"],"mappings":";;;;;;;;;cA+Ba,gBAAgB;WAClB;WACA;EAGP,YAAA,qBACA;SAkBK,KAAK,MAAM,SAAW;SAUtB,YAAY;;;;;;;EAUnB,SAAS,OAAO,SAAW;;EAc3B,WAAW;;EAWX,eAAe;;;;;;;;;EAaf,SACE,OAAO,QACP,wBACA;;;;;;EA0BF;;EAWA,SAAS;EAKT,UAAU;;;;;;;;;;;KCtJA;EAAmB;EAAW;EAAW;EAAW;;cAInD,sBAAsB;WACxB;WACA;WACA;WACA;EAEG,YAAA,YAAO,YAAO,YAAO;SAO1B,YAAY;;;;;;SASZ,mBAAmB,2BAA2B;;;;;;;;;;;EAwBrD;;EAaA,SAAS,OAAO,iBAAiB;;EAYjC,aAAa;;EAKb,WAAW;;;;;EAQX,gBAAgB,OAAO;;EAgBvB,aAAa;EAmBb,UAAU"}
|
|
@@ -1,83 +1,111 @@
|
|
|
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
|
+
/**
|
|
72
|
+
* The angle (in rad, always within [0, pi]) of the rotation that takes
|
|
73
|
+
* `other` to `this`, matching Eigen's built-in `Quaternion::angularDistance()`.
|
|
74
|
+
*/
|
|
75
|
+
angularDistance(other) {
|
|
76
|
+
const relative = this.multiply({
|
|
77
|
+
w: other.w,
|
|
78
|
+
x: -other.x,
|
|
79
|
+
y: -other.y,
|
|
80
|
+
z: -other.z
|
|
81
|
+
});
|
|
82
|
+
const vecNorm = Math.sqrt(relative.x * relative.x + relative.y * relative.y + relative.z * relative.z);
|
|
83
|
+
return 2 * Math.atan2(vecNorm, Math.abs(relative.w));
|
|
84
|
+
}
|
|
85
|
+
/** Rotate a 3D vector by this quaternion. */
|
|
86
|
+
rotateVector(v) {
|
|
87
|
+
const vx = v[0] ?? 0;
|
|
88
|
+
const vy = v[1] ?? 0;
|
|
89
|
+
const vz = v[2] ?? 0;
|
|
90
|
+
const { w, x, y, z } = this;
|
|
91
|
+
const tx = 2 * (y * vz - z * vy);
|
|
92
|
+
const ty = 2 * (z * vx - x * vz);
|
|
93
|
+
const tz = 2 * (x * vy - y * vx);
|
|
94
|
+
return [
|
|
95
|
+
vx + w * tx + (y * tz - z * ty),
|
|
96
|
+
vy + w * ty + (z * tx - x * tz),
|
|
97
|
+
vz + w * tz + (x * ty - y * tx)
|
|
98
|
+
];
|
|
99
|
+
}
|
|
100
|
+
toJSON() {
|
|
101
|
+
return {
|
|
102
|
+
w: this.w,
|
|
103
|
+
x: this.x,
|
|
104
|
+
y: this.y,
|
|
105
|
+
z: this.z
|
|
106
|
+
};
|
|
107
|
+
}
|
|
108
|
+
};
|
|
81
109
|
//#endregion
|
|
82
110
|
//#region src/lib/experimental/math/Pose.ts
|
|
83
111
|
const ZERO_VECTOR = [
|
|
@@ -85,6 +113,7 @@ const ZERO_VECTOR = [
|
|
|
85
113
|
0,
|
|
86
114
|
0
|
|
87
115
|
];
|
|
116
|
+
const TOLERANCE_MILLI = .001;
|
|
88
117
|
function addVectors(a, b) {
|
|
89
118
|
return [
|
|
90
119
|
(a[0] ?? 0) + (b[0] ?? 0),
|
|
@@ -99,23 +128,24 @@ function negateVector(a) {
|
|
|
99
128
|
-(a[2] ?? 0)
|
|
100
129
|
];
|
|
101
130
|
}
|
|
131
|
+
function isFiniteVector3(v) {
|
|
132
|
+
return v.length === 3 && v.every((n) => Number.isFinite(n));
|
|
133
|
+
}
|
|
102
134
|
/**
|
|
103
135
|
* A `Pose` (position + axis-angle orientation) with methods for composing
|
|
104
136
|
* and inverting transforms. Instances are plain-data compatible with the
|
|
105
137
|
* wire-format `PoseData` type (own `position`/`orientation` properties only,
|
|
106
138
|
* no enumerable methods), so they can be passed directly back into API calls
|
|
107
139
|
* 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
140
|
*/
|
|
113
141
|
var Pose = class Pose {
|
|
114
142
|
position;
|
|
115
143
|
orientation;
|
|
116
144
|
constructor(position = ZERO_VECTOR, orientation = ZERO_VECTOR) {
|
|
117
|
-
|
|
118
|
-
|
|
145
|
+
if (!isFiniteVector3(position)) throw new Error(`Pose constructor: position must be an array of 3 finite numbers, got ${JSON.stringify(position)}`);
|
|
146
|
+
if (!isFiniteVector3(orientation)) throw new Error(`Pose constructor: orientation must be an array of 3 finite numbers, got ${JSON.stringify(orientation)}`);
|
|
147
|
+
this.position = [...position];
|
|
148
|
+
this.orientation = [...orientation];
|
|
119
149
|
}
|
|
120
150
|
static from(pose) {
|
|
121
151
|
if (pose instanceof Pose) return pose;
|
|
@@ -131,28 +161,59 @@ var Pose = class Pose {
|
|
|
131
161
|
* applying `other` first, then `this`.
|
|
132
162
|
*/
|
|
133
163
|
multiply(other) {
|
|
134
|
-
const q1 =
|
|
135
|
-
const q2 =
|
|
136
|
-
const position = addVectors(this.position,
|
|
137
|
-
const orientation =
|
|
164
|
+
const q1 = Quaternion.fromRotationVector(this.orientation);
|
|
165
|
+
const q2 = Quaternion.fromRotationVector(other.orientation ?? ZERO_VECTOR);
|
|
166
|
+
const position = addVectors(this.position, q1.rotateVector(other.position ?? ZERO_VECTOR));
|
|
167
|
+
const orientation = q1.multiply(q2).toRotationVector();
|
|
138
168
|
return new Pose(position, orientation);
|
|
139
169
|
}
|
|
140
170
|
/** The inverse transform, such that `pose.multiply(pose.inverse())` is the identity pose. */
|
|
141
171
|
inverse() {
|
|
142
|
-
const position =
|
|
172
|
+
const position = Quaternion.fromRotationVector(this.orientation).inverse().rotateVector(negateVector(this.position));
|
|
143
173
|
const orientation = negateVector(this.orientation);
|
|
144
174
|
return new Pose(position, orientation);
|
|
145
175
|
}
|
|
146
176
|
/** Apply this pose's transform to a point, returning the transformed point. */
|
|
147
177
|
transformPoint(point) {
|
|
148
|
-
const q =
|
|
149
|
-
return addVectors(this.position,
|
|
178
|
+
const q = Quaternion.fromRotationVector(this.orientation);
|
|
179
|
+
return addVectors(this.position, q.rotateVector(point));
|
|
150
180
|
}
|
|
151
|
-
/**
|
|
152
|
-
|
|
181
|
+
/**
|
|
182
|
+
* Compare to another pose using separate position/orientation tolerances,
|
|
183
|
+
* matching wb-robotix's `Pose::isApprox()`: Euclidean distance for
|
|
184
|
+
* position, and quaternion angular distance for orientation - not a naive
|
|
185
|
+
* per-component diff, since two rotation vectors can represent nearly
|
|
186
|
+
* identical rotations while differing componentwise near a
|
|
187
|
+
* canonicalization boundary (e.g. close to the +/-pi wraparound).
|
|
188
|
+
*/
|
|
189
|
+
isApprox(other, deltaPosition = TOLERANCE_MILLI, deltaOrientation = TOLERANCE_MILLI) {
|
|
153
190
|
const otherPosition = other.position ?? ZERO_VECTOR;
|
|
154
191
|
const otherOrientation = other.orientation ?? ZERO_VECTOR;
|
|
155
|
-
|
|
192
|
+
const positionDistance = Math.sqrt(this.position.reduce((sum, v, i) => sum + (v - (otherPosition[i] ?? 0)) ** 2, 0));
|
|
193
|
+
const orientationDistance = Quaternion.fromRotationVector(this.orientation).angularDistance(Quaternion.fromRotationVector(otherOrientation));
|
|
194
|
+
return positionDistance <= deltaPosition && orientationDistance <= deltaOrientation;
|
|
195
|
+
}
|
|
196
|
+
/**
|
|
197
|
+
* Position + orientation as a 6-element [x, y, z, roll, pitch, yaw] vector
|
|
198
|
+
* (Euler angles in rad, XYZ Tait-Bryan / Rx*Ry*Rz convention), matching
|
|
199
|
+
* wb-robotix's `Pose::toCartesian()`.
|
|
200
|
+
*/
|
|
201
|
+
toCartesian() {
|
|
202
|
+
const { w, x, y, z } = Quaternion.fromRotationVector(this.orientation);
|
|
203
|
+
const roll = Math.atan2(2 * (w * x - y * z), 1 - 2 * (x * x + y * y));
|
|
204
|
+
const pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (x * z + w * y))));
|
|
205
|
+
const yaw = Math.atan2(2 * (w * z - x * y), 1 - 2 * (y * y + z * z));
|
|
206
|
+
return [
|
|
207
|
+
...this.position,
|
|
208
|
+
roll,
|
|
209
|
+
pitch,
|
|
210
|
+
yaw
|
|
211
|
+
];
|
|
212
|
+
}
|
|
213
|
+
/** Human-readable `[x, y, z][rx, ry, rz]` representation, matching wb-robotix's `Pose::string()`. */
|
|
214
|
+
toString(precision = 6) {
|
|
215
|
+
const fmt = (v) => Number(v.toPrecision(precision));
|
|
216
|
+
return `[${this.position.map(fmt).join(", ")}][${this.orientation.map(fmt).join(", ")}]`;
|
|
156
217
|
}
|
|
157
218
|
toJSON() {
|
|
158
219
|
return {
|
|
@@ -162,65 +223,6 @@ var Pose = class Pose {
|
|
|
162
223
|
}
|
|
163
224
|
};
|
|
164
225
|
//#endregion
|
|
165
|
-
|
|
166
|
-
function isPlainObject(value) {
|
|
167
|
-
return typeof value === "object" && value !== null && !Array.isArray(value) && !(value instanceof Pose);
|
|
168
|
-
}
|
|
169
|
-
function isNumberTriple(value) {
|
|
170
|
-
return Array.isArray(value) && value.length === 3 && value.every((n) => typeof n === "number");
|
|
171
|
-
}
|
|
172
|
-
/**
|
|
173
|
-
* Structural check for the wire shape of `Pose` (`{ position, orientation }`,
|
|
174
|
-
* each a 3-number array, and nothing else). This API has no other type with
|
|
175
|
-
* this exact shape, so it reliably identifies `Pose` values without needing
|
|
176
|
-
* per-endpoint knowledge of which fields are poses.
|
|
177
|
-
*/
|
|
178
|
-
function isPoseShape(value) {
|
|
179
|
-
return Object.keys(value).length === 2 && isNumberTriple(value.position) && isNumberTriple(value.orientation);
|
|
180
|
-
}
|
|
181
|
-
function augmentPosesInPlace(value) {
|
|
182
|
-
if (Array.isArray(value)) {
|
|
183
|
-
for (const item of value) augmentPosesInPlace(item);
|
|
184
|
-
return;
|
|
185
|
-
}
|
|
186
|
-
if (isPlainObject(value)) if (isPoseShape(value)) Object.setPrototypeOf(value, Pose.prototype);
|
|
187
|
-
else for (const key of Object.keys(value)) augmentPosesInPlace(value[key]);
|
|
188
|
-
}
|
|
189
|
-
/**
|
|
190
|
-
* Recursively walks a parsed API response/request body, upgrading any
|
|
191
|
-
* `Pose`-shaped objects in place to `Pose` instances so they gain math
|
|
192
|
-
* methods (`multiply`, `inverse`, ...) while remaining JSON/wire-compatible.
|
|
193
|
-
* Used by `augmentMath` for `nova.api.*` responses; exported so it can also
|
|
194
|
-
* be applied manually to e.g. websocket messages.
|
|
195
|
-
*/
|
|
196
|
-
function augmentPoses(value) {
|
|
197
|
-
augmentPosesInPlace(value);
|
|
198
|
-
return value;
|
|
199
|
-
}
|
|
200
|
-
function wrapWithPoseAugmentation(target) {
|
|
201
|
-
return new Proxy(target, { get(t, prop) {
|
|
202
|
-
const value = Reflect.get(t, prop, t);
|
|
203
|
-
if (typeof value === "function") return (...args) => {
|
|
204
|
-
const result = value.apply(t, args);
|
|
205
|
-
return result instanceof Promise ? result.then((data) => augmentPoses(data)) : result;
|
|
206
|
-
};
|
|
207
|
-
if (value !== null && typeof value === "object") return wrapWithPoseAugmentation(value);
|
|
208
|
-
return value;
|
|
209
|
-
} });
|
|
210
|
-
}
|
|
211
|
-
/**
|
|
212
|
-
* Wraps a `Nova` instance so every `nova.api.*` call automatically upgrades
|
|
213
|
-
* `Pose`-shaped fields in its response to `Pose` instances (with math
|
|
214
|
-
* methods like `multiply`/`inverse`). Returns a new proxied view - does not
|
|
215
|
-
* mutate the original `nova` instance.
|
|
216
|
-
*/
|
|
217
|
-
function augmentMath(nova) {
|
|
218
|
-
return new Proxy(nova, { get(target, prop) {
|
|
219
|
-
if (prop === "api") return wrapWithPoseAugmentation(target.api);
|
|
220
|
-
return Reflect.get(target, prop, target);
|
|
221
|
-
} });
|
|
222
|
-
}
|
|
223
|
-
//#endregion
|
|
224
|
-
export { Pose, augmentMath, augmentPoses, axisAngleToQuaternion, conjugateQuaternion, multiplyQuaternions, quaternionToAxisAngle, rotateVectorByQuaternion };
|
|
226
|
+
export { Pose, Quaternion };
|
|
225
227
|
|
|
226
228
|
//# sourceMappingURL=index.mjs.map
|