@faicad/faijs 0.16.0 → 0.17.0

package/dist/api/assembly/solvers/joints-kinematics.js

@@ -0,0 +1,643 @@
+/**
+ * Kinematic joints + IK — core 自有实现（§5.6.3 移植：vendored jointFns + ikFns 逐字对齐）。
+ *
+ * 移植来源：packages/brepjs/src/operations/{jointFns,ikFns}.ts（纯算法零 kernel 依赖，
+ * 仅用 quat 与 assembly-tree；Phase 5 随 brepjs 包删除前已做 parity 对拍）。
+ */
+import { quatFromAxisAngle, quatRotate, quatMultiply } from './quat.js';
+import { walkAssembly } from './assembly-tree.js';
+const DEG2RAD = Math.PI / 180;
+// ---------------------------------------------------------------------------
+// Construction
+// ---------------------------------------------------------------------------
+function clamp(value, min, max) {
+    return Math.min(max, Math.max(min, value));
+}
+function unit(v) {
+    const l = Math.hypot(v[0], v[1], v[2]) || 1;
+    return [v[0] / l, v[1] / l, v[2] / l];
+}
+function cross(a, b) {
+    return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
+}
+/** A unit vector perpendicular to `v` (for the unspecified-reference case). */
+function anyPerpendicular(v) {
+    const ref = Math.abs(v[0]) < 0.9 ? [1, 0, 0] : [0, 1, 0];
+    return unit(cross(v, ref));
+}
+/** Build one DOF, normalizing an inverted range and clamping the initial value. */
+function makeDof(kind, axis, opts, defMin, defMax) {
+    // Normalize the range so an inverted (min > max) input can't break the
+    // "value is always within [min, max]" invariant.
+    const a = opts.min ?? defMin;
+    const b = opts.max ?? defMax;
+    const min = Math.min(a, b);
+    const max = Math.max(a, b);
+    return { kind, axis: unit(axis), min, max, value: clamp(opts.value ?? 0, min, max) };
+}
+/** Assemble a Joint from its DOFs, mirroring the primary DOF into the top level. */
+function buildJoint(type, parent, child, axis, dofs) {
+    const primary = dofs[0] ?? { kind: 'rotation', axis: [0, 0, 1], min: 0, max: 0, value: 0 };
+    return {
+        type,
+        parent,
+        child,
+        axis: { origin: axis.origin, direction: unit(axis.direction) },
+        min: primary.min,
+        max: primary.max,
+        value: primary.value,
+        dofs,
+    };
+}
+/** A revolute (hinge) joint — the child rotates about `axis` by `value` degrees.
+ * @param parent - Name of the reference (parent) body.
+ * @param child - Name of the body moved by the joint.
+ * @param axis - Joint axis: anchor point and direction.
+ * @param opts - Optional range bounds and initial value.
+ * @returns The newly created revolute joint.
+ */
+export function revoluteJoint(parent, child, axis, opts = {}) {
+    return buildJoint('revolute', parent, child, axis, [
+        makeDof('rotation', axis.direction, opts, -180, 180),
+    ]);
+}
+/**
+ * A prismatic (slider) joint — the child translates along `axis` by `value`
+ * units. Only `axis.direction` is used; `axis.origin` is ignored (a pure
+ * translation has no anchor point), unlike a revolute joint which rotates about
+ * the axis line through `origin`.
+ * @param parent - Name of the reference (parent) body.
+ * @param child - Name of the body moved by the joint.
+ * @param axis - Slide axis; only `direction` is used.
+ * @param opts - Optional range bounds and initial value.
+ * @returns The newly created prismatic joint.
+ */
+export function prismaticJoint(parent, child, axis, opts = {}) {
+    return buildJoint('prismatic', parent, child, axis, [
+        makeDof('translation', axis.direction, opts, 0, 100),
+    ]);
+}
+/**
+ * A cylindrical joint — the child both rotates about and slides along a single
+ * `axis` (2 DOF). DOF order: `[rotation, translation]`. The two motions share
+ * the axis, so they commute; rotation pivots about `axis.origin`.
+ * @param parent - Name of the reference (parent) body.
+ * @param child - Name of the body moved by the joint.
+ * @param axis - Joint axis: anchor point and direction.
+ * @param opts - Optional per-DOF ranges and initial values.
+ * @returns The newly created cylindrical joint.
+ */
+export function cylindricalJoint(parent, child, axis, opts = {}) {
+    return buildJoint('cylindrical', parent, child, axis, [
+        makeDof('rotation', axis.direction, opts.rotation ?? {}, -180, 180),
+        makeDof('translation', axis.direction, opts.translation ?? {}, 0, 100),
+    ]);
+}
+/**
+ * A planar joint — the child translates within a plane and rotates about its
+ * normal (3 DOF). `plane.direction` is the normal; `plane.origin` the rotation
+ * anchor. DOF order: `[u-translation, v-translation, rotation]`, where the
+ * translations are applied in the plane frame (independent of the rotation).
+ * @param parent - Name of the reference (parent) body.
+ * @param child - Name of the body moved by the joint.
+ * @param plane - Plane definition; `direction` is the normal, `origin` the anchor.
+ * @param opts - Optional per-DOF ranges, initial values, and in-plane axes.
+ * @returns The newly created planar joint.
+ */
+export function planarJoint(parent, child, plane, opts = {}) {
+    const normal = unit(plane.direction);
+    // Project a requested u-direction onto the plane; fall back to an arbitrary
+    // in-plane axis. v completes a right-handed in-plane basis.
+    let u;
+    if (opts.uDirection) {
+        const d = opts.uDirection;
+        const proj = d[0] * normal[0] + d[1] * normal[1] + d[2] * normal[2];
+        const inPlane = [
+            d[0] - proj * normal[0],
+            d[1] - proj * normal[1],
+            d[2] - proj * normal[2],
+        ];
+        u =
+            Math.hypot(inPlane[0], inPlane[1], inPlane[2]) < 1e-9
+                ? anyPerpendicular(normal)
+                : unit(inPlane);
+    }
+    else {
+        u = anyPerpendicular(normal);
+    }
+    const v = unit(cross(normal, u));
+    return buildJoint('planar', parent, child, { origin: plane.origin, direction: normal }, [
+        makeDof('translation', u, opts.u ?? {}, -100, 100),
+        makeDof('translation', v, opts.v ?? {}, -100, 100),
+        makeDof('rotation', normal, opts.rotation ?? {}, -180, 180),
+    ]);
+}
+/**
+ * A spherical (ball) joint — the child rotates freely about a pivot point
+ * (3 DOF). DOF order: `[x, y, z]` rotations about the local axes through
+ * `pivot`, composed as `Rx · Ry · Rz`.
+ * @param parent - Name of the reference (parent) body.
+ * @param child - Name of the body moved by the joint.
+ * @param pivot - Pivot point all rotations pass through.
+ * @param opts - Optional per-axis ranges and initial values.
+ * @returns The newly created spherical joint.
+ */
+export function sphericalJoint(parent, child, pivot, opts = {}) {
+    return buildJoint('spherical', parent, child, { origin: pivot, direction: [0, 0, 1] }, [
+        makeDof('rotation', [1, 0, 0], opts.x ?? {}, -180, 180),
+        makeDof('rotation', [0, 1, 0], opts.y ?? {}, -180, 180),
+        makeDof('rotation', [0, 0, 1], opts.z ?? {}, -180, 180),
+    ]);
+}
+/**
+ * Return a copy of `joint` with per-DOF values set (each clamped to its range).
+ * Values are positional, matching `joint.dofs`; omitted entries keep their
+ * stored value. The primary mirror (`value`) is kept in sync with `dofs[0]`.
+ * @param joint - The joint to update.
+ * @param values - Positional per-DOF values; omitted entries keep their value.
+ * @returns A new joint with the values set.
+ */
+export function setJointValues(joint, values) {
+    const dofs = joint.dofs.map((d, i) => {
+        const v = values[i];
+        return v === undefined ? d : { ...d, value: clamp(v, d.min, d.max) };
+    });
+    const primary = dofs[0];
+    return primary ? { ...joint, dofs, value: primary.value } : { ...joint, dofs };
+}
+/** Return a copy of `joint` with its primary DOF set (clamped to range).
+ * @param joint - The joint to update.
+ * @param value - New primary-DOF value.
+ * @returns A new joint with the primary value set.
+ */
+export function setJointValue(joint, value) {
+    return setJointValues(joint, [value]);
+}
+// ---------------------------------------------------------------------------
+// Kinematics
+// ---------------------------------------------------------------------------
+/** The local rigid transform contributed by a single DOF at `value`. */
+function dofPose(origin, dof, value) {
+    if (dof.kind === 'translation') {
+        // `origin` is intentionally unused: a pure translation has no pivot.
+        return {
+            position: [dof.axis[0] * value, dof.axis[1] * value, dof.axis[2] * value],
+            rotation: [1, 0, 0, 0],
+        };
+    }
+    // Rotation about the axis line through `origin`: p ↦ R·p + (origin − R·origin).
+    const rotation = quatFromAxisAngle(dof.axis, value * DEG2RAD);
+    const ro = quatRotate(rotation, origin);
+    return { position: [origin[0] - ro[0], origin[1] - ro[1], origin[2] - ro[2]], rotation };
+}
+/**
+ * The child's local rigid transform (relative to the parent) for given DOF
+ * values. Defaults to each DOF's stored value. A single `number` overrides only
+ * the primary DOF (single-DOF ergonomics); an array overrides positionally,
+ * with omitted entries keeping their stored value. Each value is clamped to its
+ * DOF range.
+ *
+ * DOFs are folded in array order via frame composition. For same-anchor
+ * rotations (e.g. spherical) this composes to a single rotation about the pivot;
+ * for a cylindrical axis the rotation and slide commute.
+ * @param joint - The joint to evaluate.
+ * @param value - Primary-DOF override (number) or positional per-DOF overrides.
+ * @returns The child's local pose relative to the parent.
+ */
+export function jointTransform(joint, value = joint.value) {
+    const overrides = Array.isArray(value) ? value : undefined;
+    const primary = overrides ? undefined : value;
+    const origin = joint.axis.origin;
+    let pose = IDENTITY_POSE;
+    for (let i = 0; i < joint.dofs.length; i++) {
+        const dof = joint.dofs[i];
+        if (!dof)
+            continue;
+        const raw = overrides
+            ? (overrides[i] ?? dof.value)
+            : i === 0
+                ? (primary ?? dof.value)
+                : dof.value;
+        pose = composePose(pose, dofPose(origin, dof, clamp(raw, dof.min, dof.max)));
+    }
+    return pose;
+}
+// ---------------------------------------------------------------------------
+// Assembly integration
+// ---------------------------------------------------------------------------
+/** Attach a joint to an assembly node. Returns a new node (immutable).
+ * @param assembly - The assembly node to attach the joint to.
+ * @param joint - The joint to attach.
+ * @returns A new node with the joint added.
+ */
+export function addJoint(assembly, joint) {
+    const existing = (assembly.joints ?? []);
+    return { ...assembly, joints: [...existing, joint] };
+}
+// ---------------------------------------------------------------------------
+// Forward kinematics
+// ---------------------------------------------------------------------------
+const IDENTITY_POSE = { position: [0, 0, 0], rotation: [1, 0, 0, 0] };
+/** Compose two poses: the result applies `b` in `a`'s frame (`a ∘ b`). */
+function composePose(a, b) {
+    const rb = quatRotate(a.rotation, b.position);
+    return {
+        position: [a.position[0] + rb[0], a.position[1] + rb[1], a.position[2] + rb[2]],
+        rotation: quatMultiply(a.rotation, b.rotation),
+    };
+}
+/** Collect every joint attached anywhere in the assembly tree. */
+function collectJoints(assembly) {
+    const joints = [];
+    walkAssembly(assembly, (node) => {
+        if (node.joints)
+            joints.push(...node.joints);
+    });
+    return joints;
+}
+/**
+ * Forward kinematics: set joint values and propagate world poses down the
+ * kinematic chain. Each joint's axis is interpreted in its **parent's** frame,
+ * so a child's world pose is `parentWorld ∘ jointTransform(joint, value)`.
+ *
+ * Bodies not driven by a joint (chain roots) start at the origin. `jointValues`
+ * overrides a joint's stored value, keyed by the **child** node name; omitted
+ * joints use `joint.value`. Resolution is topological (reuses the Phase-0
+ * ordering), so chains of any depth compose. Returns a world pose for every node.
+ * @param assembly - The assembly to solve.
+ * @param jointValues - Optional value overrides keyed by child node name.
+ * @returns A world pose for every node in the assembly.
+ */
+export function forwardKinematics(assembly, jointValues = {}) {
+    // Single pass: gather joints and node names together.
+    const joints = [];
+    const names = new Set();
+    walkAssembly(assembly, (node) => {
+        names.add(node.name);
+        if (node.joints)
+            joints.push(...node.joints);
+    });
+    const byChild = new Map();
+    for (const j of joints) {
+        byChild.set(j.child, j);
+        names.add(j.parent);
+        names.add(j.child);
+    }
+    const poses = new Map();
+    // Roots: any node not driven by a joint sits at the origin.
+    for (const name of names)
+        if (!byChild.has(name))
+            poses.set(name, IDENTITY_POSE);
+    // Propagate down the joint graph: a joint resolves once its parent is placed.
+    const pending = [...joints];
+    let progress = true;
+    while (progress && pending.length > 0) {
+        progress = false;
+        for (let i = pending.length - 1; i >= 0; i--) {
+            const j = pending[i];
+            if (!j)
+                continue;
+            const parentPose = poses.get(j.parent);
+            if (!parentPose)
+                continue; // parent not placed yet — defer
+            pending.splice(i, 1);
+            progress = true;
+            if (poses.has(j.child))
+                continue; // already placed (duplicate/cycle) — skip
+            const value = jointValues[j.child] ?? j.value;
+            const local = j.offset
+                ? composePose(jointTransform(j, value), j.offset)
+                : jointTransform(j, value);
+            poses.set(j.child, composePose(parentPose, local));
+        }
+    }
+    // A joint whose parent never resolved (cycle/dangling) still gets an entry so
+    // the map covers every node.
+    for (const j of pending)
+        if (!poses.has(j.child))
+            poses.set(j.child, IDENTITY_POSE);
+    return poses;
+}
+/**
+ * Open-chain mobility — the number of independent degrees of freedom, summing
+ * each joint's DOF count (revolute/prismatic 1, cylindrical 2, planar/spherical
+ * 3). For a serial chain this equals the total DOF. (Closed-loop
+ * Grübler/Kutzbach analysis is future work.)
+ * @param assembly - The assembly to analyze.
+ * @returns Total number of independent degrees of freedom.
+ */
+export function mechanismDOF(assembly) {
+    return collectJoints(assembly).reduce((sum, j) => sum + j.dofs.length, 0);
+}
+// ---------------------------------------------------------------------------
+// Small vector / quaternion helpers
+// ---------------------------------------------------------------------------
+const IK_IDENTITY_POSE = { position: [0, 0, 0], rotation: [1, 0, 0, 0] };
+function applyPose(pose, p) {
+    const r = quatRotate(pose.rotation, p);
+    return [r[0] + pose.position[0], r[1] + pose.position[1], r[2] + pose.position[2]];
+}
+function quatConjugate(q) {
+    return [q[0], -q[1], -q[2], -q[3]];
+}
+/**
+ * The rotation vector (axis · angle) taking orientation `from` to `to`, i.e. the
+ * angular error that drives `from` toward `to`. Returns the zero vector when the
+ * orientations coincide.
+ */
+function rotationError(from, to) {
+    const d = quatMultiply(to, quatConjugate(from));
+    let [w, x, y, z] = d;
+    const norm = Math.hypot(w, x, y, z) || 1;
+    w /= norm;
+    x /= norm;
+    y /= norm;
+    z /= norm;
+    if (w < 0) {
+        // Shortest path: q and -q represent the same rotation.
+        w = -w;
+        x = -x;
+        y = -y;
+        z = -z;
+    }
+    const s = Math.hypot(x, y, z);
+    if (s < 1e-12)
+        return [0, 0, 0];
+    const angle = 2 * Math.atan2(s, w);
+    const k = angle / s;
+    return [x * k, y * k, z * k];
+}
+// ---------------------------------------------------------------------------
+// Chain extraction
+// ---------------------------------------------------------------------------
+/** Joints from the root down to `endEffector`, in root→leaf order. */
+function chainTo(assembly, endEffector) {
+    const joints = [];
+    walkAssembly(assembly, (n) => {
+        if (n.joints)
+            joints.push(...n.joints);
+    });
+    const byChild = new Map();
+    for (const j of joints)
+        byChild.set(j.child, j);
+    const chain = [];
+    const seen = new Set();
+    let cur = endEffector;
+    while (cur && byChild.has(cur) && !seen.has(cur)) {
+        seen.add(cur);
+        const j = byChild.get(cur);
+        if (!j)
+            break;
+        chain.push(j);
+        cur = j.parent;
+    }
+    return chain.reverse();
+}
+// ---------------------------------------------------------------------------
+// Linear algebra (small dense systems, Float64Array to avoid index-undefined)
+// ---------------------------------------------------------------------------
+/** Read a Float64Array element as a definite number (dense matrices are full). */
+function el(a, i) {
+    return a[i] ?? 0;
+}
+/** Solve `A x = b` for an `n×n` system by Gauss-Jordan with partial pivoting. */
+function solveLinear(A, b, n) {
+    const w = n + 1;
+    const M = new Float64Array(n * w);
+    for (let r = 0; r < n; r++) {
+        for (let c = 0; c < n; c++)
+            M[r * w + c] = el(A, r * n + c);
+        M[r * w + n] = el(b, r);
+    }
+    for (let col = 0; col < n; col++) {
+        let piv = col;
+        for (let r = col + 1; r < n; r++) {
+            if (Math.abs(el(M, r * w + col)) > Math.abs(el(M, piv * w + col)))
+                piv = r;
+        }
+        if (Math.abs(el(M, piv * w + col)) < 1e-12)
+            return null;
+        if (piv !== col) {
+            for (let k = col; k < w; k++) {
+                const tmp = el(M, col * w + k);
+                M[col * w + k] = el(M, piv * w + k);
+                M[piv * w + k] = tmp;
+            }
+        }
+        const d = el(M, col * w + col);
+        for (let k = col; k < w; k++)
+            M[col * w + k] = el(M, col * w + k) / d;
+        for (let r = 0; r < n; r++) {
+            if (r === col)
+                continue;
+            const f = el(M, r * w + col);
+            if (f === 0)
+                continue;
+            for (let k = col; k < w; k++)
+                M[r * w + k] = el(M, r * w + k) - f * el(M, col * w + k);
+        }
+    }
+    const x = new Float64Array(n);
+    for (let r = 0; r < n; r++)
+        x[r] = el(M, r * w + n);
+    return x;
+}
+/** Flatten a chain's DOFs into a parameter vector with bounds, applying the seed. */
+function flattenChain(chain, seed) {
+    const segments = [];
+    const q = [];
+    const lo = [];
+    const hi = [];
+    for (const j of chain) {
+        const s = seed?.[j.child];
+        segments.push({ child: j.child, count: j.dofs.length });
+        j.dofs.forEach((dof, i) => {
+            const seeded = Array.isArray(s) ? s[i] : i === 0 ? s : undefined;
+            const v = seeded ?? dof.value;
+            q.push(Math.min(dof.max, Math.max(dof.min, v)));
+            lo.push(dof.min);
+            hi.push(dof.max);
+        });
+    }
+    return {
+        segments,
+        q: Float64Array.from(q),
+        lo: Float64Array.from(lo),
+        hi: Float64Array.from(hi),
+    };
+}
+/** Slice a parameter vector back into per-joint value arrays keyed by child. */
+function overridesOf(segments, q) {
+    const out = {};
+    let i = 0;
+    for (const seg of segments) {
+        const vals = [];
+        for (let k = 0; k < seg.count; k++)
+            vals.push(el(q, i++));
+        out[seg.child] = vals;
+    }
+    return out;
+}
+/** Residual twist `e` (target − current) for a pose; returns its norm. */
+function residualTwist(out, pose, target, tip, m) {
+    const pos = applyPose(pose, tip);
+    out[0] = target.position[0] - pos[0];
+    out[1] = target.position[1] - pos[1];
+    out[2] = target.position[2] - pos[2];
+    if (m === 6 && target.rotation) {
+        const r = rotationError(pose.rotation, target.rotation);
+        out[3] = r[0];
+        out[4] = r[1];
+        out[5] = r[2];
+    }
+    let s = 0;
+    for (let i = 0; i < m; i++)
+        s += el(out, i) ** 2;
+    return Math.sqrt(s);
+}
+/**
+ * Finite-difference Jacobian: column `j` is the end-effector twist from δq[j].
+ * The probe steps *inward* from a bound — `forwardKinematics` clamps each DOF to
+ * its range, so a forward `+eps` at the upper limit would yield a zero column and
+ * trap the solver at the ceiling. Stepping `-eps` there (and dividing by the
+ * signed step) keeps every column a true one-sided derivative.
+ */
+function fillJacobian(J, q, n, m, lo, hi, base, tip, eps, tipPose) {
+    const basePos = applyPose(base, tip);
+    for (let j = 0; j < n; j++) {
+        const saved = el(q, j);
+        // Step away from whichever bound we're against so the perturbation isn't
+        // clamped to a no-op; default forward.
+        const h = saved + eps > el(hi, j) && saved - eps >= el(lo, j) ? -eps : eps;
+        q[j] = saved + h;
+        const p2 = tipPose(q);
+        q[j] = saved;
+        const pos2 = applyPose(p2, tip);
+        J[j] = (pos2[0] - basePos[0]) / h;
+        J[n + j] = (pos2[1] - basePos[1]) / h;
+        J[2 * n + j] = (pos2[2] - basePos[2]) / h;
+        if (m === 6) {
+            const dr = rotationError(base.rotation, p2.rotation);
+            J[3 * n + j] = dr[0] / h;
+            J[4 * n + j] = dr[1] / h;
+            J[5 * n + j] = dr[2] / h;
+        }
+    }
+}
+/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
+function dlsStep(J, e, n, m, lambda) {
+    const A = new Float64Array(m * m);
+    const lam2 = lambda * lambda;
+    for (let r = 0; r < m; r++) {
+        for (let c = 0; c < m; c++) {
+            let s = 0;
+            for (let k = 0; k < n; k++)
+                s += el(J, r * n + k) * el(J, c * n + k);
+            A[r * m + c] = s + (r === c ? lam2 : 0);
+        }
+    }
+    const y = solveLinear(A, e, m);
+    if (!y)
+        return null;
+    const dq = new Float64Array(n);
+    for (let j = 0; j < n; j++) {
+        let v = 0;
+        for (let r = 0; r < m; r++)
+            v += el(J, r * n + j) * el(y, r);
+        dq[j] = v;
+    }
+    return dq;
+}
+/**
+ * Solve for the joint values that place `endEffector` (offset by `tip`) at
+ * `target`, by damped-least-squares descent on a numerical Jacobian. Joint
+ * ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
+ *
+ * Returns the solved per-DOF values keyed by child node (ready to pass to
+ * `forwardKinematics`), whether it converged, the iteration count, and the final
+ * residual norm. An end-effector with no driving joints, or an unreachable
+ * target, returns `converged: false` with the best configuration found.
+ * @param assembly - The assembly containing the kinematic chain.
+ * @param endEffector - Name of the end-effector node.
+ * @param target - Target world pose for the end-effector.
+ * @param options - Optional solver settings (iterations, tolerance, damping, seed, tip).
+ * @returns The solved joint values and convergence diagnostics.
+ */
+export function inverseKinematics(assembly, endEffector, target, options = {}) {
+    const maxIterations = options.maxIterations ?? 200;
+    const tolerance = options.tolerance ?? 1e-5;
+    const lambda = options.damping ?? 0.05;
+    const tip = options.tip ?? [0, 0, 0];
+    const m = target.rotation !== undefined ? 6 : 3;
+    const eps = 1e-6;
+    const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
+    const n = q.length;
+    const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IK_IDENTITY_POSE;
+    const e = new Float64Array(m);
+    const J = new Float64Array(m * n);
+    let pose = tipPose(q);
+    let err = residualTwist(e, pose, target, tip, m);
+    let iter = 0;
+    for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
+        fillJacobian(J, q, n, m, lo, hi, pose, tip, eps, tipPose);
+        const dq = dlsStep(J, e, n, m, lambda);
+        if (!dq)
+            break;
+        for (let j = 0; j < n; j++) {
+            const next = el(q, j) + el(dq, j);
+            q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
+        }
+        pose = tipPose(q);
+        err = residualTwist(e, pose, target, tip, m);
+    }
+    return {
+        values: overridesOf(segments, q),
+        converged: err <= tolerance,
+        iterations: iter,
+        error: err,
+    };
+}
+/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
+function valuesOf(joint, spec) {
+    return joint.dofs.map((dof, i) => {
+        const raw = Array.isArray(spec) ? spec[i] : i === 0 ? spec : undefined;
+        const v = raw ?? dof.value;
+        return Math.min(dof.max, Math.max(dof.min, v));
+    });
+}
+/**
+ * Sample a straight-line path in joint space from `from` to `to` over `steps`
+ * segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
+ * sample carries the interpolated per-DOF values (clamped to range) and the
+ * forward-kinematics poses of every node. Joints absent from `from`/`to` hold
+ * their stored value at both ends.
+ * @param assembly - The assembly to pose at each sample.
+ * @param from - Starting joint values keyed by child node name.
+ * @param to - Ending joint values keyed by child node name.
+ * @param steps - Number of path segments (samples are `steps + 1`).
+ * @returns The trajectory samples with interpolated values and poses.
+ */
+export function jointTrajectory(assembly, from, to, steps) {
+    const joints = [];
+    walkAssembly(assembly, (n) => {
+        if (n.joints)
+            joints.push(...n.joints);
+    });
+    const ends = joints.map((j) => ({
+        child: j.child,
+        a: valuesOf(j, from[j.child]),
+        b: valuesOf(j, to[j.child]),
+    }));
+    const count = Math.max(1, Math.floor(steps));
+    const samples = [];
+    for (let s = 0; s <= count; s++) {
+        const t = s / count;
+        const values = {};
+        for (const end of ends) {
+            values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
+        }
+        samples.push({ t, values, poses: forwardKinematics(assembly, values) });
+    }
+    return samples;
+}
+//# sourceMappingURL=joints-kinematics.js.map