Six-Axis Robot Arm (cutaway)

A six-axis industrial arm of the 3 kg, 580 mm class in cutaway. The joint layout is a published Denavit-Hartenberg table, and forwardKinematics() places every link from six joint angles. The housings are a display model: a boolean cut opens four of them to show illustrative servo drives with strain-wave reducers. The screws are M4 socket head cap screws to ISO 4762, and the cut shows two of them in their threaded holes.
Code
This is the full program. The playground runs it in your browser without a change.
import {
addChild,
addJoint,
applyMatrix,
box,
compound,
createAssemblyNode,
cut,
cutAll,
cylinder,
edgeFinder,
ellipse,
extrude,
face,
fillet,
forwardKinematics,
fuseAll,
getBounds,
jointsFromDH,
line,
loft,
polygon,
revolve,
rotate,
sphere,
threePointArc,
translate,
unwrap,
wire,
} from 'brepjs/quick';
import { color } from 'brepjs/playground';
// Six-axis industrial arm of the 3 kg, 580 mm class. The joint layout is the published
// Denavit-Hartenberg table of that class; the housings are a display model. Units are millimetres.
// Z is up, the arm reaches toward +X, and every link is modelled at the zero pose.
const D1 = 290; // floor to axis 2
const A2 = 270; // axis 2 to axis 3
const A3 = 70; // axis 3 up to axis 4
const D4 = 302; // axis 3 forward to the wrist centre
const D6 = 72; // wrist centre to the tool flange
const DH = [
{ name: 'link1', a: 0, alpha: -90, d: D1, theta: 0, min: -165, max: 165 },
{ name: 'link2', a: A2, alpha: 0, d: 0, theta: -90, min: -110, max: 110 },
{ name: 'link3', a: A3, alpha: -90, d: 0, theta: 0, min: -110, max: 70 },
{ name: 'link4', a: 0, alpha: 90, d: D4, theta: 0, min: -160, max: 160 },
{ name: 'link5', a: 0, alpha: -90, d: 0, theta: 0, min: -120, max: 120 },
{ name: 'link6', a: 0, alpha: 0, d: D6, theta: 180, min: -400, max: 400 },
];
// ---- published figures: socket head cap screw M4 x 10 (ISO 4762), coarse thread (ISO 261 and ISO 724),
// clearance hole of the medium series (ISO 273), tap drill (ISO 2306)
const M4 = { major: 4, minor: 3.242, pitch: 0.7, head: 7, headHeight: 4, socket: 3, socketDepth: 2, length: 10, clearance: 4.5, tapDrill: 3.3 };
type P2 = [number, number];
type P3 = [number, number, number];
type Shape = ReturnType<typeof cylinder>;
type Tone = 'cast' | 'cover' | 'ring' | 'steel' | 'dark';
interface Part {
link: number; // 0 = base, 1..6 = the link that joint n moves
tone: Tone;
shape: Shape;
}
// Kernel shapes live in WASM memory. Every shape goes through own(). freeing() runs a build and then frees them all.
const owned: { delete(): void }[] = [];
const own = <T extends { delete(): void }>(s: T): T => {
owned.push(s);
return s;
};
const freeing = <T,>(build: () => T): T => { try { return build(); } finally { owned.splice(0).forEach((s) => s.delete()); } }; // also when the build throws
const X: P3 = [1, 0, 0];
const Y: P3 = [0, 1, 0];
const Z: P3 = [0, 0, 1];
const along = (o: P3, dir: P3, t: number): P3 => [o[0] + dir[0] * t, o[1] + dir[1] * t, o[2] + dir[2] * t];
// A cylinder on the line through o along dir, from station t0 to t1.
const rod = (o: P3, dir: P3, r: number, t0: number, t1: number): Shape =>
own(cylinder(r, t1 - t0, { at: along(o, dir, t0), axis: dir }));
const block = (x0: number, x1: number, y0: number, y1: number, z0: number, z1: number): Shape =>
own(translate(own(box(x1 - x0, y1 - y0, z1 - z0)), [x0, y0, z0]) as Shape);
// A [station, radius] profile revolved about the line through o along dir.
const lathe = (o: P3, dir: P3, pts: P2[]): Shape => {
const out: P3 = dir[2] === 1 ? X : Z; // radial direction of the profile plane
const profile = own(unwrap(polygon(pts.map(([t, r]) => along(along(o, dir, t), out, r)))));
return own(unwrap(revolve(profile, { axis: dir, at: o }))) as Shape;
};
// The same profile revolved only from one angle to another, in degrees from the profile plane.
const wedge = (o: P3, dir: P3, pts: P2[], from: number, to: number): Shape => {
const out: P3 = dir[2] === 1 ? X : Z;
const profile = own(unwrap(polygon(pts.map(([t, r]) => along(along(o, dir, t), out, r)))));
const swept = own(unwrap(revolve(profile, { axis: dir, at: o, angle: ((to - from) * Math.PI) / 180 })));
return own(rotate(swept, from, { axis: dir, at: o })) as Shape;
};
// Two circles in the XZ plane joined by their outer tangents, extruded from y0 to y1.
const belt = (c1: P2, r1: number, c2: P2, r2: number, y0: number, y1: number): Shape => {
const d = Math.hypot(c2[0] - c1[0], c2[1] - c1[1]);
const u: P2 = [(c2[0] - c1[0]) / d, (c2[1] - c1[1]) / d];
const s = (r1 - r2) / d;
const c = Math.sqrt(1 - s * s);
const pt = (o: P2, r: number, a: number, b: number): P3 => [o[0] + r * (u[0] * a - u[1] * b), y0, o[1] + r * (u[1] * a + u[0] * b)];
const edges = [
own(line(pt(c1, r1, s, c), pt(c2, r2, s, c))),
own(threePointArc(pt(c2, r2, s, c), pt(c2, r2, 1, 0), pt(c2, r2, s, -c))),
own(line(pt(c2, r2, s, -c), pt(c1, r1, s, -c))),
own(threePointArc(pt(c1, r1, s, -c), pt(c1, r1, -1, 0), pt(c1, r1, s, c))),
];
const loop = own(unwrap(wire(edges)));
const profile = own(unwrap(face(loop as never)));
return own(unwrap(extrude(profile, [0, y1 - y0, 0]))) as Shape;
};
// Round the rim edges of a prism: the edges that lie in a plane across axis.
const round = (s: Shape, axis: 'x' | 'y' | 'z', r: number): Shape => {
const flat = (e: Parameters<typeof getBounds>[0]) => {
const b = getBounds(e);
const span = axis === 'x' ? b.xMax - b.xMin : axis === 'y' ? b.yMax - b.yMin : b.zMax - b.zMin;
return span < 1e-6;
};
return own(unwrap(fillet(s, edgeFinder().when(flat).findAll(s), r))) as Shape;
};
const fused = (parts: Shape[]): Shape => own(unwrap(fuseAll(parts))) as Shape;
const corners = (s: Shape, dir: 'X' | 'Z', r: number): Shape =>
own(unwrap(fillet(s, edgeFinder().inDirection(dir).findAll(s), r))) as Shape;
// Turn a shape built along +Z onto the line through o along dir, at station t. dir is a world axis.
const place = (s: Shape, o: P3, dir: P3, t = 0): Shape => {
const turn = dir[0] !== 0 ? { deg: 90 * dir[0], axis: Y } : dir[1] !== 0 ? { deg: -90 * dir[1], axis: X } : { deg: dir[2] > 0 ? 0 : 180, axis: X };
return own(translate(own(rotate(s, turn.deg, { axis: turn.axis })), along(o, dir, t)) as Shape);
};
// One M4 cap screw along +Z: the head sits on z = 0 and the shank goes toward -Z. The shank is the thread root.
const capScrew = (): Shape => {
const r = M4.socket / Math.sqrt(3);
const hex = Array.from({ length: 6 }, (_, i): P3 => [r * Math.cos((i * Math.PI) / 3), r * Math.sin((i * Math.PI) / 3), M4.headHeight - M4.socketDepth]);
const socket = own(unwrap(extrude(own(unwrap(polygon(hex))), M4.socketDepth + 1)));
const body = lathe([0, 0, 0], Z, [[-M4.length, 0], [-M4.length, M4.minor / 2], [0, M4.minor / 2], [0, M4.head / 2], [M4.headHeight, M4.head / 2], [M4.headHeight, 0]]);
return own(unwrap(cut(body, socket))) as Shape;
};
// A helical thread tooth about +Z, from z = from to z = to. Its crest passes the +X side at z = ref.
// tip > core is the thread of a screw, tip < core the thread in a hole. The loft is smooth: no facets.
const helixTooth = (core: number, tip: number, from: number, to: number, ref: number): Shape => {
const outward = tip > core;
const root = outward ? core / 0.998 : core * 0.998; // a hair clear of the core or the bore
const crest = outward ? tip : tip / 0.998;
const half = 0.42 * M4.pitch;
const n = Math.max(2, Math.round(((to - from) / M4.pitch) * 12)); // 12 sections in each turn
const sections = Array.from({ length: n + 1 }, (_, j) => {
const z = from + ((to - from) * j) / n;
const a = 2 * Math.PI * ((z - ref) / M4.pitch);
const at = (rr: number, dz: number): P3 => [rr * Math.cos(a), rr * Math.sin(a), z + dz];
const pts = [at(root, -half), at(crest, 0), at(root, half)];
return own(unwrap(wire(pts.map((p, i) => own(line(p, pts[(i + 1) % 3]!))))));
});
return own(unwrap(loft(sections, { ruled: false }))) as Shape;
};
// count seats on a circle of radius r about the line through o along dir, at station t.
const circle = (o: P3, dir: P3, t: number, r: number, count: number): P3[] => {
const u: P3 = dir[2] !== 0 ? X : Z;
const v: P3 = [dir[1] * u[2] - dir[2] * u[1], dir[2] * u[0] - dir[0] * u[2], dir[0] * u[1] - dir[1] * u[0]];
return Array.from({ length: count }, (_, i) => {
const a = (2 * Math.PI * (i + 0.5)) / count;
return along(along(along(o, dir, t), u, r * Math.cos(a)), v, r * Math.sin(a));
});
};
// One servo axis along +Z, output face at z = 0: crossed bearing, strain-wave reducer, motor, brake, encoder.
// R is the bearing radius and L the length. The proportions are illustrative.
const drive = (R: number, L: number): [Tone, Shape][] => {
const O: P3 = [0, 0, 0];
const tube = (r0: number, r1: number, z0: number, z1: number) => lathe(O, Z, [[z0, r0], [z0, r1], [z1, r1], [z1, r0]]);
const balls = (rx: number, ry: number, r: number, z: number, n: number) =>
Array.from({ length: n }, (_, i) => own(sphere(r, { at: [rx * Math.cos((2 * Math.PI * i) / n), ry * Math.sin((2 * Math.PI * i) / n), z] })) as Shape);
// Wave generator: an exact elliptical plug and its bearing balls. The cup stays round in this model.
const oval = own(unwrap(wire([own(unwrap(ellipse(0.6 * R, 0.56 * R, { at: [0, 0, 0.42 * R] })))])));
const plug = own(unwrap(cut(own(unwrap(extrude(own(unwrap(face(oval as never))), 0.14 * R))), rod(O, Z, 0.1 * R, 0.4 * R, 0.6 * R)))) as Shape;
const z0 = 0.92 * R;
const z1 = L - 0.3 * R;
const fins = 6;
const stator: P2[] = [[z0, 0.14 * R]];
for (let i = 0; i < fins; i++) {
const a = z0 + ((z1 - z0) * i) / fins;
const w = (z1 - z0) / fins;
stator.push([a, 0.58 * R], [a + w * 0.6, 0.58 * R], [a + w * 0.6, 0.5 * R], [a + w, 0.5 * R]);
}
stator.push([z1, 0.14 * R]);
const steel = (shape: Shape): [Tone, Shape] => ['steel', shape];
return [
steel(tube(0, 0.5 * R, 0, 0.1 * R)),
['dark', tube(0.78 * R, 0.95 * R, 0.1 * R, 0.32 * R)],
['dark', tube(0.5 * R, 0.66 * R, 0.1 * R, 0.32 * R)],
...balls(0.72 * R, 0.72 * R, 0.055 * R, 0.21 * R, 20).map(steel),
['dark', tube(0.74 * R, 0.95 * R, 0.36 * R, 0.6 * R)],
['ring', tube(0.66 * R, 0.71 * R, 0.36 * R, 0.8 * R)],
['ring', tube(0.1 * R, 0.71 * R, 0.8 * R, 0.84 * R)],
['cover', plug],
...balls(0.63 * R, 0.59 * R, 0.03 * R, 0.49 * R, 22).map(steel),
steel(rod(O, Z, 0.1 * R, 0.1 * R, L - 0.12 * R)),
['dark', lathe(O, Z, stator)],
steel(tube(0.1 * R, 0.4 * R, L - 0.27 * R, L - 0.13 * R)),
['ring', rod(O, Z, 0.3 * R, L - 0.12 * R, L)],
];
};
function armParts(grip: number, cutaway: boolean): Part[] {
const O: P3 = [0, 0, 0];
const J2: P3 = [0, 0, D1];
const J3: P3 = [0, 0, D1 + A2];
const J4: P3 = [0, 0, D1 + A2 + A3]; // a point on axis 4, which runs along X
const W: P3 = [D4, 0, J4[2]]; // wrist centre: axes 4, 5 and 6 meet here
const down: P3 = [0, 0, -1];
const back: P3 = [-1, 0, 0];
const right: P3 = [0, -1, 0];
const parts: Part[] = [];
const add = (link: number, tone: Tone, ...shapes: Shape[]) => shapes.forEach((shape) => parts.push({ link, tone, shape }));
const minus = (s: Shape, tool: Shape): Shape => own(unwrap(cut(s, tool))) as Shape;
// A real boolean opens a housing: one quarter comes off along the joint axis.
const open = (s: Shape, window: Shape): Shape => (cutaway ? minus(s, window) : s);
const fit = (link: number, o: P3, dir: P3, t: number, R: number, L: number) => drive(R, L).forEach(([tone, shape]) => add(link, tone, place(shape, o, dir, t)));
// Screws. A seat is the point under a screw head. Stations of a hole are measured from the seat, into the part.
const screw = capScrew();
const drill = (seats: P3[], dir: P3, d: number, t0: number, t1: number): Shape[] => seats.map((p) => rod(p, dir, d / 2, t0, t1));
const screws = (seats: P3[], dir: P3): Shape[] => {
const turned = place(screw, O, dir);
return seats.map((p) => own(translate(turned, p)) as Shape);
};
// A hidden thread is not modelled: the shank is the root cylinder, in a hole of the tap drill size.
// The two screws that the cutaway shows from the side have the true helix, in a hole with the mating thread.
const PLAY = 0.08; // radial play between the two threads
const thread = cutaway ? helixTooth(M4.minor / 2, M4.major / 2, 0.3 - M4.length, -0.5, 0) : undefined;
const halfTurn = cutaway ? helixTooth(M4.major / 2 + PLAY, M4.minor / 2 + PLAY, M4.pitch, 1.5 * M4.pitch, M4.pitch / 2) : undefined;
// A motor cap: a can and a lid, hollow inside. Six screws go through the lid into lugs on the can wall.
// On an opened cap one screw goes with the quarter that comes off, and the cut plane passes through the next one.
const LID = 6;
const LUG = 9;
const cap = (link: number, o: P3, dir: P3, t: number, r: number, len: number, ring: number, window?: Shape, spin = 0): Shape => {
const seats = circle(o, dir, t + len + LID, ring, 6);
const half = (Math.asin(5.5 / ring) * 180) / Math.PI;
const section: P2[] = [[t + len - LUG, ring - 4.5], [t + len - LUG, r - 7], [t + len, r - 7], [t + len, ring - 4.5]];
const tapped = (d: number, at: P3) => rod(at, dir, d / 2, -LID - LUG - 1, 1 - LID);
const lug = minus(wedge(o, dir, section, 30 - half, 30 + half), tapped(M4.tapDrill, seats[0]!));
const hidden = screws(seats, dir);
seats.forEach((seat, i) => {
if (cutaway && window && i === 5) return;
if (cutaway && window && i === 4) {
// The half lug that stays, the whole screw, and the half turns of the thread in the hole.
const put = (s: Shape) => own(translate(place(own(rotate(s, spin)) as Shape, O, dir), seat)) as Shape;
add(link, 'cover', minus(wedge(o, dir, section, 270 - half, 270), tapped(M4.major + 2 * PLAY, seat)));
add(link, 'cover', ...Array.from({ length: 10 }, (_, k) => put(own(translate(halfTurn!, [0, 0, -(k + 11) * M4.pitch])) as Shape)));
add(link, 'steel', put(screw), put(thread!));
return;
}
add(link, 'cover', own(rotate(lug, 60 * i, { axis: dir, at: o })) as Shape);
add(link, 'steel', hidden[i]!);
});
const shell = fused([rod(o, dir, r, t, t + len), rod(o, dir, r - 8, t + len, t + len + LID)]);
const body = own(unwrap(cutAll(shell, [rod(o, dir, r - 7, t - 1, t + len), ...drill(seats, dir, M4.clearance, -LID - 1, 1)]))) as Shape;
return window ? open(body, window) : body;
};
// A flat cover: screws through the cover into blind holes in the casting under it.
const coverHoles = (seats: P3[], dir: P3) => drill(seats, dir, M4.clearance, -8, 1);
const tapHoles = (seats: P3[], dir: P3, thickness: number) => drill(seats, dir, M4.tapDrill, -M4.length - 2, 1 - thickness);
// ---- base: 180 x 180 foot, pedestal, rear connector box
const foot = corners(block(-90, 90, -90, 90, 0, 16), 'Z', 24);
const pedestal = lathe(O, Z, [[16, 72], [64, 72], [80, 66], [150, 66], [150, 0], [16, 0]]);
const rearBox = corners(block(-90, -50, -44, 44, 16, 92), 'Z', 8);
const anchorHoles = [[66, 66], [-66, 66], [-66, -66], [66, -66]].map(([x, y]) => rod([x!, y!, 0], Z, 5.5, -1, 17));
const window0 = block(0, 100, -100, 0, 16.5, 157);
add(0, 'cast', open(own(unwrap(cutAll(fused([foot, pedestal, rearBox]), [...anchorHoles, rod(O, Z, 58, 24, 151)]))) as Shape, window0));
add(0, 'dark', rod([-90, 20, 62], back, 12, 0, 12), rod([-90, -20, 62], back, 12, 0, 12));
add(0, 'steel', ...[-24, -8, 8, 24].map((y) => rod([-90, y, 34], back, 3.5, 0, 6)));
add(0, 'ring', open(lathe(O, Z, [[150, 60], [150, 69], [156, 69], [156, 60]]), window0));
fit(0, [0, 0, 150], down, 0, 54, 122);
// ---- link 1: turret and shoulder housing, turns about Z
const drum = lathe(O, Z, [[156, 68], [178, 68], [192, 58], [192, 0], [156, 0]]);
const shoulder = minus(round(belt([0, 215], 60, [0, D1], 62, -50, 50), 'y', 10), block(-120, 120, -60, 60, 100, 178));
const window1 = block(0, 130, -100, 49, D1, D1 + 130);
add(1, 'cast', open(minus(fused([drum, shoulder, rod([-58, 0, 0], Z, 30, 178, 252)]), fused([rod(J2, right, 55, -51, 44), rod(J2, right, 39, 43, 51)])), window1));
add(1, 'cover', cap(1, J2, right, 50, 46, 30, 30, window1));
add(1, 'ring', rod(J2, Y, 52, 50, 52));
fit(1, J2, right, -48, 52, 124);
// ---- link 2: upper arm, a slab beside the housings
const slab = round(belt([0, D1], 60, [0, J3[2]], 52, 52, 112), 'y', 8);
const seatsJ2 = circle(J2, Y, 117, 35, 8);
const seatsJ3 = circle(J3, Y, 117, 30, 8);
const seats2 = [...seatsJ2, ...seatsJ3];
add(2, 'cast', own(unwrap(cutAll(slab, [belt([0, D1 + 72], 24, [0, J3[2] - 66], 20, 108, 113), ...tapHoles(seats2, Y, 5)]))) as Shape);
add(2, 'cover', own(unwrap(cutAll(rod(J2, Y, 42, 112, 117), coverHoles(seatsJ2, Y)))) as Shape, own(unwrap(cutAll(rod(J3, Y, 36, 112, 117), coverHoles(seatsJ3, Y)))) as Shape);
add(2, 'cover', belt([0, D1 + 72], 21, [0, J3[2] - 66], 17, 108, 109.5));
add(2, 'steel', ...screws(seats2, Y));
add(2, 'ring', rod(J3, Y, 44, 50, 52));
// ---- link 3: elbow housing, carries the drives of axes 3 and 4
const elbow = round(belt([0, J3[2]], 54, [0, J4[2]], 50, -50, 50), 'y', 8);
const barrel4 = lathe(J4, X, [[-74, 45], [-60, 50], [60, 50], [80, 47], [80, 0], [-74, 0]]);
const hollow3 = fused([rod(J3, right, 47, -51, 44), rod(J3, right, 33, 43, 51), rod(J4, back, 44, -81, 68), rod(J4, back, 35, 67, 75)]);
const window3 = block(-120, 81.5, -100, 0, J4[2], J4[2] + 80);
add(3, 'cast', open(minus(fused([elbow, barrel4]), hollow3), window3));
add(3, 'cover', cap(3, J4, back, 74, 42, 28, 27, window3, -90), cap(3, J3, right, 50, 40, 28, 25));
add(3, 'ring', rod(J4, X, 46, 80, 83));
fit(3, J3, right, -48, 44, 118);
fit(3, J4, back, -78, 42, 176);
// ---- link 4: forearm tube and wrist fork, turns about X; the two wrist motors sit inside
const tube = lathe(J4, X, [[83, 45], [96, 45], [112, 37], [240, 32], [240, 0], [83, 0]]);
const bore = lathe(J4, X, [[86, 0], [86, 39], [98, 39], [112, 31], [232, 26], [232, 0]]);
const seats4 = circle(W, Y, 48, 15, 4);
const cheeks = [27, -45].map((y) => round(belt([240, W[2]], 32, [D4, W[2]], 28, y, y + 18), 'y', 3));
const tappedCheek = own(unwrap(cutAll(cheeks[0]!, tapHoles(seats4, Y, 3)))) as Shape;
add(4, 'cast', fused([open(minus(tube, bore), block(84, 236, -100, 0, W[2], W[2] + 80)), tappedCheek, cheeks[1]!]));
for (const y of [13, -13]) {
add(4, 'dark', rod([0, y, W[2]], X, 11, 126, 200));
add(4, 'steel', rod([0, y, W[2]], X, 8.5, 200, 224));
add(4, 'ring', rod([0, y, W[2]], X, 8, 116, 126));
}
// Timing belt and pulleys of the wrist drive, under a cover on the right cheek.
const beltLoop = minus(belt([250, W[2]], 13.5, [D4, W[2]], 17.5, -50, -46), belt([250, W[2]], 12, [D4, W[2]], 16, -51, -45));
add(4, 'dark', beltLoop);
add(4, 'steel', rod([250, 0, W[2]], right, 12, 45, 51), rod(W, right, 16, 45, 51));
if (!cutaway) add(4, 'cover', minus(belt([250, W[2]], 19, [D4, W[2]], 23, -53, -45), belt([250, W[2]], 17, [D4, W[2]], 21, -51.5, -44)));
add(4, 'cover', own(unwrap(cutAll(rod(W, Y, 21, 45, 48), coverHoles(seats4, Y)))) as Shape);
add(4, 'steel', ...screws(seats4, Y));
add(4, 'ring', rod(W, Y, 26, 25, 27), rod(W, Y, 26, -27, -25));
// ---- link 5: wrist, bends about Y
add(5, 'cast', fused([rod(W, Y, 27, -25, 25), rod(W, X, 25, 0, 48)]));
add(5, 'ring', rod(W, X, 23, 48, 50));
// ---- link 6: tool flange with a 31.5 mm bolt circle, turns about X
const disc = lathe(W, X, [[50, 24], [56, 24], [56, 20], [D6, 20], [D6, 0], [50, 0]]);
const onCircle = (a: number): P3 => [W[0], 15.75 * Math.cos(a), W[2] + 15.75 * Math.sin(a)];
const flangeHoles = [0, 1, 2, 3].map((i) => rod(onCircle((i * Math.PI) / 2), X, 2.1, D6 - 9, D6 + 1));
flangeHoles.push(rod(onCircle(Math.PI / 4), X, 2.5, D6 - 6, D6 + 1), rod(W, X, 6, D6 - 5, D6 + 1));
add(6, 'steel', own(unwrap(cutAll(disc, flangeHoles))) as Shape);
// ---- tool: a two-finger gripper on the flange
const x0 = D4 + D6;
add(6, 'dark', rod(W, X, 20, D6, D6 + 8), corners(block(x0 + 8, x0 + 46, -34, 34, W[2] - 15, W[2] + 15), 'X', 4));
add(6, 'steel', block(x0 + 46, x0 + 92, grip / 2, grip / 2 + 8, W[2] - 8, W[2] + 8), block(x0 + 46, x0 + 92, -grip / 2 - 8, -grip / 2, W[2] - 8, W[2] + 8));
return parts;
}
type Pose = { position: P3; rotation: [number, number, number, number] };
const matrixOf = ([w, x, y, z]: Pose['rotation']): number[] => [
1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y),
2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x),
2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y),
];
// The rigid motion that carries a link from its zero pose to its posed frame: R = Rb * Ra^T, t = pb - R * pa.
const motion = (from: Pose, to: Pose) => {
const a = matrixOf(from.rotation);
const b = matrixOf(to.rotation);
const R = Array.from({ length: 9 }, (_, i) => {
const [row, col] = [Math.floor(i / 3), i % 3];
return b[row * 3]! * a[col * 3]! + b[row * 3 + 1]! * a[col * 3 + 1]! + b[row * 3 + 2]! * a[col * 3 + 2]!;
});
const p = from.position;
const t: P3 = [0, 1, 2].map((r) => to.position[r]! - (R[r * 3]! * p[0] + R[r * 3 + 1]! * p[1] + R[r * 3 + 2]! * p[2])) as P3;
return { linear: R as never, translation: t };
};
interface ArmParams {
pose?: number[]; // the six joint angles in degrees; each is held inside its range
grip?: number; // finger opening in millimetres, 0 to 52
cutaway?: boolean; // open the base, shoulder, elbow and forearm to show the drives
}
const robotArm = ({ pose = [-35, 28, 12, 0, 50, 0], grip = 24, cutaway = true }: ArmParams = {}) => freeing(() => {
const parts = armParts(Math.min(52, Math.max(0, grip)), cutaway);
// The kinematics API turns the DH table into a joint chain and solves the pose of every link.
let chain = addChild(createAssemblyNode('arm'), createAssemblyNode('base'));
const joints = jointsFromDH(DH);
for (const joint of joints) chain = addChild(chain, createAssemblyNode(joint.child));
for (const joint of joints) chain = addJoint(chain, joint);
const solve = (q: number[]) => forwardKinematics(chain, Object.fromEntries(DH.map((row, i) => [row.name, q[i] ?? 0])));
const zero = solve([]);
const posed = solve(pose);
const tones: Tone[] = ['cast', 'cover', 'ring', 'steel', 'dark'];
const arm = Object.fromEntries(
tones.map((tone) => {
const moved = DH.map((_, link) => link).concat(DH.length).flatMap((link) => {
const shapes = parts.filter((p) => p.link === link && p.tone === tone).map((p) => p.shape);
if (shapes.length === 0) return [];
const body = own(compound(shapes)) as Shape;
if (link === 0) return [body];
const name = DH[link - 1]!.name;
return [own(unwrap(applyMatrix(body, motion(zero.get(name) as Pose, posed.get(name) as Pose)))) as Shape];
});
return [tone, compound(moved) as Shape];
}),
) as Record<Tone, Shape>;
return { ...arm, flange: (posed.get('link6') as Pose).position };
});
// Turn the open side toward the camera, for the first view and for the Iso view.
const arm = robotArm();
const show = (s: Shape, css: string) => {
const turned = rotate(s, 125);
s.delete();
return color(turned, css);
};
console.log('tool flange at', arm.flange.map((v) => v.toFixed(1)).join(', '), 'mm in the base frame');
export default [
show(arm.cast, '#f5f2ea'),
show(arm.cover, '#ff5a1f'),
show(arm.ring, '#12b5b0'),
show(arm.steel, '#a9b1ba'),
show(arm.dark, '#48515c'),
];Functions used
addChild addJoint applyMatrix box compound createAssemblyNode cut cutAll cylinder edgeFinder ellipse extrude face fillet forwardKinematics fuseAll getBounds jointsFromDH line loft polygon revolve rotate sphere threePointArc translate unwrap wire color