ik

Inverse kinematics for a planar serial arm. C++17, no dependencies, one translation unit.

Two solvers on purpose

Closed form, two links only. Law of cosines, exact, returns both the elbow-up and the elbow-down answer, and reports out-of-reach and the dead zone at the base as distinct failures rather than silently clamping.

Damped least squares, any number of links. Iterative, handles redundant arms, respects joint limits, and stays stable through singularities.

The damping is the whole point. Near full extension the Jacobian loses rank, a plain pseudo-inverse asks for enormous joint rates, and a real arm snaps. Levenberg-Marquardt damping trades a little tracking accuracy for a bounded joint rate, which is what you want on hardware. The solver also reports manipulability, sqrt(det(J J^T)), which collapses to zero at a singularity and works as a warning light.

Because the arm is planar, J J^T is 2x2 and the inverse is done by hand. No matrix library.

Build and run

make test                                   # 10 checks
./ik --links 1.0,0.8,0.6 --target 1.4 0.5   # single solve
make trace                                  # CSV of a traced ellipse

Single solve output:

arm      3 links, reach 2.4000
target   (1.4000, 0.5000), radius 1.4866
dls      q = [-0.546178, 1.118185, 1.212358]
         converged in 5 iters, residual 1.386e-07
         manipulability 1.522218

Ask for a point at the very edge of the workspace and it says so instead of pretending:

./ik --links 1.0,1.0 --target 2.0 0.0
         stalled in 199 iters, residual 1.606e-05
         manipulability 0.008015   (singular, near full extension)

Tests

Ten checks, including the ones that usually get skipped:

• both closed-form branches land on the target to machine precision
• out of reach and the base dead zone are reported, not clamped
• the iterative solver agrees with the exact answer
• a redundant four-link arm converges
• manipulability collapses at full extension
• an unreachable target stalls at the boundary with a bounded, finite error
• joint limits are never violated
• seeding each solve from the previous pose keeps a traced path smooth, measured as the largest joint step over 200 points, which comes out at 0.015 rad

That last one matters more than it looks. Solving each point from a fixed seed makes the arm flip branches mid-path. Seeding from where it already is keeps the motion continuous.

Built With

• c++
• makefile