The IK Solver Returned Four Confident Answers, All 10 cm Wrong

A crash is a good bug. It arrives with a stack trace, it points at a line, and nobody ships a robot on top of it. The bug we want to write about is the other kind: code that answers, answers confidently, and answers wrong.

We spent a day reading our own kinematics and simulation code rather than adding to it. What follows is what we measured, including the parts where we were wrong about what we would find.

Four solutions, every one 10 cm off

Our analytic inverse-kinematics solver for UR-style six-axis arms returns up to eight joint configurations for a requested tool pose. We asked it for a pose 1 cm from the shoulder axis, at (0.01, 0, 0.5) with identity orientation, and it returned four solutions.

Every one of them was finite. Every one had a quaternion dot product of 1.000000 against the requested orientation — the tool would be pointing exactly where we asked. And every one placed the tool 0.099150 m away from the position we asked for.

That number is not noise. It is exactly |d₄| − p_xy: the arm's shoulder offset (0.10915 m on this model) minus the 1 cm radius we requested. The target sat inside a cylinder the wrist centre geometrically cannot enter, and the solver knew it. Inside solveTheta1, a branch guarded by Math.abs(ratio) > 1 carried the comment "Fall back to closest solution" and returned a base angle that cannot satisfy the constraint it was derived from. The downstream two-link step then solved the in-plane problem correctly and silently dropped the lateral residual.

Nothing in the returned structure said "this is approximate". A caller reading solutions[0].angles and commanding them moves the tool 10 cm from where it asked to be, with no error, no flag and no warning.

The branch immediately above it, for the same geometric situation — a wrist centre exactly on the z-axis — already returned an empty array. So the fix was to make the two siblings agree: an unreachable pose returns nothing. The test that now guards it asserts empty inside the cylinder and eight solutions that round-trip through forward kinematics at radius 0.11 m, just outside the boundary. Without that second half, a solver that had simply gone empty for everything would pass.

One rotation, three scalings, three different answers

The same day turned up a second shape, and this one spanned four files in two packages.

A rotation matrix built from a quaternion is only a rotation if that quaternion is a unit quaternion. The formula is the familiar 1 − 2(y² + z²) arrangement. None of our three analytic solvers normalised their input first, and the type carried no constraint that would have made them.

A quaternion's scale carries no orientation information, so one rotation written at three scales must produce one answer. We measured, on a 90-degree pitch at (0.4, 0, 0.4):

input	result
unit quaternion	8 solutions, all round-tripping
the same rotation × 2	0 solutions — a reachable pose reported unreachable
the same rotation × 0.5	8 solutions, none matching the requested pose
all components zero	8 solutions for a pose nobody asked for

The all-zero case is the one worth staring at. 1 − 2 × 0 is 1, so a quaternion with no orientation in it at all reads as the identity rotation, and the solver cheerfully answers for a pose the caller never requested.

Two more consequences, in two other files with the same formula:

The URDF exporter. It converts a joint's orientation quaternion to roll-pitch-yaw for <origin rpy="...">. sinp scales with the square of the norm while the 1 − 2(...) terms do not, so an off-unit quaternion shifts roll and yaw and drives pitch into the gimbal-lock clamp. A 30-degree rotation about Y exported as 30 degrees at unit scale, 90 degrees at ×2 (clamped), 7.2 degrees at ×0.5, and an entirely different orientation at ×10. That value goes into the robot description handed to ROS — a joint mounted at the wrong angle in a file that looks perfectly well-formed.

A collision envelope. The broad-phase stage wraps each link's oriented bounding box in an axis-aligned envelope using the same matrix, under a comment that read "Rotation matrix from a unit quaternion" with nothing enforcing it. On a unit cube at 45 degrees about Z, the correct envelope reaches √2 ≈ 1.4142. At ×2 we measured 3.0000 and at ×10 we measured 99.0 — wasteful but safe, since an envelope larger than the box only costs candidate pairs. At ×0.5 we measured 1.1036, which is smaller than the box it is supposed to contain. An under-covering broad-phase envelope lets a real collision through unreported. That is the direction that matters.

All four now normalise. Where the norm is zero there is no orientation to recover, so they decline rather than guess — in the collision path specifically, because dividing by zero would make every bound NaN, and a NaN envelope overlaps nothing and would report no collisions at all.

Why the tests could not see any of it

The test sitting over the 10 cm pose read, in full:

const solutions = ur5IK.solve(pose)
expect(Array.isArray(solutions)).toBe(true)

Array.isArray is true for an empty array, true for eight correct answers, and true for four answers that are 10 cm wrong. A test that cannot fail cannot report the defect sitting underneath it. We found the same shape in a handful of places, including a test whose name claimed four market figures came "from the committed pitch deck" while its body only checked that three labels existed — it never opened the deck.

But the interesting part is the ones we left alone. Two sibling solvers had the same Array.isArray assertion at genuinely uncertain workspace edges, with comments saying so: "May or may not have solutions at the edge // But shouldn't crash." There, the weak assertion is the honest one — it really is asserting "this returned without throwing". The difference was never the assertion. It was the input. A pose comfortably inside the workspace that stops solving is a regression; a pose on the boundary is a question the solver is allowed to answer either way.

The honest part: most of what we suspected was fine

We should report the base rate, because it is the thing that would be missing from a tidier version of this post.

Having found one bad clamp, we grepped for the pattern across both packages. Of thirteen clamp and rotation-formula sites, eight were already correct — preceded by an explicit range check that returns empty, so the clamp only absorbs floating-point noise, which is exactly what a clamp is for. One file already normalised its quaternion properly. We then opened five more censuses — unguarded divisions, malformed date parameters, missing optional-dependency handling, two others — and every genuine site we read was already guarded. One scan flagged 113 "unguarded divisions" of which the real count was zero; most of the hits were import paths our regex had mistaken for division.

A regex census is an upper bound on nothing. The number that means something is "n candidates, m read, k fixed" — and in our case k was often zero.

What we would take from it

Three things we are keeping:

A formula with a precondition is a guard that is not there. "Only correct for a unit quaternion" is a requirement; a comment saying so is not an implementation of it.

A clamp converts "no solution" into a confident wrong one. Math.min(1, x) before acos is right when x is 1 + 1e-10 from rounding and wrong when x is 3 because the request was impossible. Which case you are in is decided by what precedes the clamp, not by the clamp.

And name the consequence, not the wrong number. "An AABB that is 1.1036 instead of 1.4142" is a diff. "A broad-phase envelope that under-covers the box it is meant to contain, so a real collision goes unreported" is the reason to stop and fix it.

None of this says anything about whether our product works; it says that a day spent reading code we had already written found four places where it would have answered confidently and wrongly, and that most of the code around them was fine. Both halves of that are worth writing down.