From 641222d9bf5d33a346f01658f757c6d1a1bafdac Mon Sep 17 00:00:00 2001 From: Andrii Shylenko <14119286+w1ne@users.noreply.github.com> Date: Tue, 25 Aug 2026 16:23:04 +0200 Subject: [PATCH] fix(assembly): treat a joint origin as a pivot, not a translation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Parts are modeled in assembly coordinates, so a joint's `origin` is a PIVOT POINT. forwardKinematics composed T(o) . M with no T(-o), so the origin leaked in as a bare translation and every joint with a non-origin pivot displaced its child by the pivot vector — visible at pose 0, where the child must sit exactly where it was modeled. A hinge with pivot [5,20,16] posed at elbow=0 exported its arm at [10,35,28]..[60,45,36] instead of the modeled [5,15,12]..[55,25,20]. The Y figure alone is decisive: the joint spins about Y, and a Y rotation cannot change a Y coordinate, yet y moved 15..25 -> 35..45. Conjugate the motion by the origin — T(o) . M . T(-o) — for all four joint kinds. This is the convention the mate solver already uses; see composeChildTransform() in src/modeling/mates/solver.ts, which conjugates the joint frame by the parent and child connector origins the same way. Transform.rotationAroundPivot() in se3.ts was already the correct primitive. Every pre-existing `.revolute(...)` test used `origin: [0, 0, 0]`, where T(o) is identity and the defect is invisible, and the one FK test asserted only `toBeDefined()`. The new tests all use a non-zero pivot and assert positions: all 7 fail without this change. --- .../capture/forwardKinematics.test.ts | 110 ++++++++++++++++++ src/modeling/capture/forwardKinematics.ts | 26 +++-- 2 files changed, 128 insertions(+), 8 deletions(-) create mode 100644 src/modeling/capture/forwardKinematics.test.ts diff --git a/src/modeling/capture/forwardKinematics.test.ts b/src/modeling/capture/forwardKinematics.test.ts new file mode 100644 index 000000000..bae410f41 --- /dev/null +++ b/src/modeling/capture/forwardKinematics.test.ts @@ -0,0 +1,110 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2026 Andrii Shylenko and kernelCAD contributors +// +// Regression: the joint origin is a PIVOT POINT, not a parent->child frame +// offset. Parts are modeled in assembly coordinates, so a joint must rotate +// its child ABOUT the origin — T(o) . M . T(-o) — and must leave the child +// exactly where it was modeled at pose 0. +// +// The original code composed T(o) . M with no T(-o), so the origin leaked in +// as a bare translation and every non-origin pivot displaced the child by the +// pivot vector. Every pre-existing `.revolute(...)` test used +// `origin: [0, 0, 0]`, where T(o) is identity and the defect is invisible. +// +// These tests all use a NON-ZERO pivot on purpose. + +import { describe, it, expect } from 'vitest'; +import { CaptureSession } from './captureSession'; +import { createApi } from '../api'; +import type { Vec3 } from '../../shared/runtime/se3'; + +const PIVOT: Vec3 = [5, 20, 16]; + +function closeTo(actual: Vec3, expected: Vec3, digits = 9): void { + for (let i = 0; i < 3; i++) expect(actual[i]).toBeCloseTo(expected[i], digits); +} + +/** Hinge with the arm modeled resting on the base, pivot away from origin. */ +function hinge() { + const session = new CaptureSession(); + const kcad = createApi({ session }); + const arm = kcad.assembly('hinge'); + const base = arm.part('base', kcad.box(60, 40, 10)); + const link = arm.part('arm', kcad.box(50, 10, 8).translate(5, 15, 12)); + return { arm, base, link, kcad }; +} + +describe('forwardKinematics — joint origin is a pivot, not an offset', () => { + it('revolute at pose 0 leaves the child exactly where it was modeled', () => { + const { arm, base, link } = hinge(); + arm.revolute('elbow', base, link, { axis: [0, -1, 0], origin: PIVOT, limitsDeg: [0, 90] }); + + const t = arm.solve({ elbow: 0 }).transform('arm'); + + // Identity — not a translation by the pivot. + closeTo(t.point([5, 15, 12]), [5, 15, 12]); + closeTo(t.point([55, 25, 20]), [55, 25, 20]); + }); + + it('revolute holds the pivot point fixed under rotation', () => { + const { arm, base, link } = hinge(); + arm.revolute('elbow', base, link, { axis: [0, -1, 0], origin: PIVOT, limitsDeg: [0, 90] }); + + for (const deg of [15, 30, 45, 90]) { + const t = arm.solve({ elbow: deg }).transform('arm'); + // A point ON the axis must not move — that is what "pivot" means. + closeTo(t.point(PIVOT), PIVOT); + } + }); + + it('rotation about Y never changes a Y coordinate', () => { + const { arm, base, link } = hinge(); + arm.revolute('elbow', base, link, { axis: [0, -1, 0], origin: PIVOT, limitsDeg: [0, 90] }); + + const t = arm.solve({ elbow: 30 }).transform('arm'); + for (const p of [[5, 15, 12], [55, 25, 20], [30, 20, 16]] as Vec3[]) { + expect(t.point(p)[1]).toBeCloseTo(p[1], 9); + } + }); + + it('revolute rotates the child by the requested angle about the pivot', () => { + const { arm, base, link } = hinge(); + arm.revolute('elbow', base, link, { axis: [0, -1, 0], origin: PIVOT, limitsDeg: [0, 90] }); + + // 90 deg about -Y takes +X to +Z. The arm tip is 50 mm out along +X from + // the pivot, so it must land 50 mm above the pivot. + const t = arm.solve({ elbow: 90 }).transform('arm'); + closeTo(t.point([55, 20, 16]), [5, 20, 66]); + }); + + it('prismatic translates by the stroke only — the pivot adds nothing', () => { + const { arm, base, link } = hinge(); + arm.prismatic('slide', base, link, { axis: [0, 0, 1], origin: PIVOT, limitsMm: [0, 40] }); + + closeTo(arm.solve({ slide: 0 }).transform('arm').point([5, 15, 12]), [5, 15, 12]); + closeTo(arm.solve({ slide: 25 }).transform('arm').point([5, 15, 12]), [5, 15, 37]); + }); + + it('ball joint at rest does not move the child', () => { + const { arm, base, link } = hinge(); + arm.ball('socket', base, link, { origin: PIVOT }); + + closeTo(arm.solve({}).transform('arm').point([5, 15, 12]), [5, 15, 12]); + closeTo(arm.solve({ socket: [0, 30, 0] }).transform('arm').point(PIVOT), PIVOT); + }); + + it('a chain of joints composes without accumulating pivot offsets', () => { + const session = new CaptureSession(); + const kcad = createApi({ session }); + const arm = kcad.assembly('chain'); + const a = arm.part('a', kcad.box(10, 10, 10)); + const b = arm.part('b', kcad.box(10, 10, 10).translate(0, 0, 10)); + const c = arm.part('c', kcad.box(10, 10, 10).translate(0, 0, 20)); + arm.revolute('j1', a, b, { axis: [0, 0, 1], origin: [0, 0, 10] }); + arm.revolute('j2', b, c, { axis: [0, 0, 1], origin: [0, 0, 20] }); + + const solved = arm.solve({ j1: 0, j2: 0 }); + closeTo(solved.transform('b').point([0, 0, 10]), [0, 0, 10]); + closeTo(solved.transform('c').point([0, 0, 20]), [0, 0, 20]); + }); +}); diff --git a/src/modeling/capture/forwardKinematics.ts b/src/modeling/capture/forwardKinematics.ts index ba4f044a5..05b7ae1b0 100644 --- a/src/modeling/capture/forwardKinematics.ts +++ b/src/modeling/capture/forwardKinematics.ts @@ -110,13 +110,20 @@ export function forwardKinematics( ); } - let jointLocalT: Transform; + // Joint motion expressed about the joint origin. Parts are modeled in + // assembly coordinates, so the origin is a PIVOT POINT, not a + // parent->child frame offset: the motion must be conjugated by it + // (T(o) . M . T(-o)), otherwise the origin leaks in as a translation and + // the child is displaced by the pivot vector even at pose 0. + // Same convention as the mate solver — see composeChildTransform() in + // src/modeling/mates/solver.ts, where parent and child connector origins + // conjugate the joint frame the same way. + let motionT: Transform; switch (parentJ.kind) { case 'revolute': { const deg = (poses[parentJ.name] as number | undefined) ?? 0; const ax = parentJ.axis as Se3Vec3; - jointLocalT = Transform.translation(parentJ.origin[0], parentJ.origin[1], parentJ.origin[2]) - .compose(Transform.rotationAxisAngleDeg(ax, deg)); + motionT = Transform.rotationAxisAngleDeg(ax, deg); break; } case 'prismatic': { @@ -126,21 +133,24 @@ export function forwardKinematics( const dx = (ax[0] / len) * stroke; const dy = (ax[1] / len) * stroke; const dz = (ax[2] / len) * stroke; - jointLocalT = Transform.translation(parentJ.origin[0], parentJ.origin[1], parentJ.origin[2]) - .compose(Transform.translation(dx, dy, dz)); + motionT = Transform.translation(dx, dy, dz); break; } case 'fixed': { - jointLocalT = Transform.translation(parentJ.origin[0], parentJ.origin[1], parentJ.origin[2]); + motionT = Transform.identity(); break; } case 'ball': { const euler = (poses[parentJ.name] as [number, number, number] | undefined) ?? [0, 0, 0]; - jointLocalT = Transform.translation(parentJ.origin[0], parentJ.origin[1], parentJ.origin[2]) - .compose(Transform.eulerXYZDeg(euler[0], euler[1], euler[2])); + motionT = Transform.eulerXYZDeg(euler[0], euler[1], euler[2]); break; } } + + const [ox, oy, oz] = parentJ.origin; + const jointLocalT = Transform.translation(ox, oy, oz) + .compose(motionT) + .compose(Transform.translation(-ox, -oy, -oz)); worldT.set(part.id, parentT.compose(jointLocalT)); }