Spaces:
Running
Running
Download src/lib/math.ts from cfahlgren1/jenga-arm: direct link, hf CLI and curl.
- Browser
- Download file 2.12 kB
-
https://huggingface.co/spaces/cfahlgren1/jenga-arm/resolve/main/src/lib/math.ts
- Command line
-
hf download hf://spaces/cfahlgren1/jenga-arm/src/lib/math.ts
-
curl -L -o math.ts https://huggingface.co/spaces/cfahlgren1/jenga-arm/resolve/main/src/lib/math.ts
2.12 kB
| // Small vector and quaternion helpers. Quaternions are [x, y, z, w]; transforms are [px, py, pz, qx, qy, qz, qw]. | |
| export type Vec3 = [number, number, number]; | |
| export type Quat = [number, number, number, number]; | |
| export type Xform = [number, number, number, number, number, number, number]; | |
| export const qmul = (a: Quat, b: Quat): Quat => [ | |
| a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1], | |
| a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0], | |
| a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3], | |
| a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2] | |
| ]; | |
| export const qconj = (q: Quat): Quat => [-q[0], -q[1], -q[2], q[3]]; | |
| export const qrot = (q: Quat, v: Vec3): Vec3 => { | |
| const [x, y, z, w] = q; | |
| const tx = 2 * (y * v[2] - z * v[1]); | |
| const ty = 2 * (z * v[0] - x * v[2]); | |
| const tz = 2 * (x * v[1] - y * v[0]); | |
| return [v[0] + w * tx + (y * tz - z * ty), v[1] + w * ty + (z * tx - x * tz), v[2] + w * tz + (x * ty - y * tx)]; | |
| }; | |
| export const qaxis = (axis: Vec3, angle: number): Quat => { | |
| const s = Math.sin(angle / 2); | |
| return [axis[0] * s, axis[1] * s, axis[2] * s, Math.cos(angle / 2)]; | |
| }; | |
| export const xmul = (a: Xform, b: Xform): Xform => { | |
| const qa: Quat = [a[3], a[4], a[5], a[6]]; | |
| const p = qrot(qa, [b[0], b[1], b[2]]); | |
| const q = qmul(qa, [b[3], b[4], b[5], b[6]]); | |
| return [a[0] + p[0], a[1] + p[1], a[2] + p[2], ...q]; | |
| }; | |
| export const xinv = (a: Xform): Xform => { | |
| const qi = qconj([a[3], a[4], a[5], a[6]]); | |
| const p = qrot(qi, [-a[0], -a[1], -a[2]]); | |
| return [p[0], p[1], p[2], ...qi]; | |
| }; | |
| /** Rotation vector (axis times angle) that takes orientation `from` to `to`, in world frame. */ | |
| export const rotError = (to: Quat, from: Quat): Vec3 => { | |
| let d = qmul(to, qconj(from)); | |
| if (d[3] < 0) d = [-d[0], -d[1], -d[2], -d[3]]; | |
| const s = Math.hypot(d[0], d[1], d[2]); | |
| if (s < 1e-9) return [0, 0, 0]; | |
| const angle = 2 * Math.atan2(s, d[3]); | |
| return [(d[0] / s) * angle, (d[1] / s) * angle, (d[2] / s) * angle]; | |
| }; | |
| /** Gripper pointing straight down, rotated by `yaw` about world Z. */ | |
| export const downQuat = (yaw: number): Quat => qmul(qaxis([0, 0, 1], yaw), [1, 0, 0, 0]); | |