Part 3: Solving the "Where Am I?" Problem (IK)

Welcome to Part 3 of "Robotics Zero to Hero." Forward Kinematics (Part 2) maps joint values to a tip pose. But in the real world a robot is handed a goal: "Grab the cup at $(x,y,z)$." It must compute the joint values that achieve that pose. This is Inverse Kinematics (IK) — and it is where low-dimensional and high-dimensional robotics part ways most dramatically.

The Math: Inverting a Nonlinear Map

We want $\mathbf q$ such that $FK(\mathbf q) = \mathbf x_{target}$, i.e. we seek a root of

For a 2-link planar arm this has a clean trigonometric (closed-form) solution. For a general 6-DOF arm, a closed-form solution exists only under special geometric conditions — the classic being a spherical wrist (three axes intersecting at a point, the Pieper condition). Break that condition, or add joints, and analytical IK evaporates. Then we go numerical.

The Jacobian

The Jacobian $J(\mathbf q) \in \mathbb{R}^{m\times n}$ ($m$ task dimensions, $n$ joints) linearizes FK by relating joint velocities to end-effector velocity:

To step the tip toward the goal by $\Delta\mathbf x$ we want the joint step $\Delta\mathbf q$ solving $\Delta\mathbf x = J\,\Delta\mathbf q$. When $J$ is square and invertible, $\Delta\mathbf q = J^{-1}\Delta\mathbf x$. It almost never is.

When $J$ is not invertible: pseudoinverse, damping, transpose

For non-square or rank-deficient $J$ we use one of several estimators, each with a distinct failure mode:

• Moore–Penrose pseudoinverse $J^\dagger = J^\top(JJ^\top)^{-1}$ gives the minimum-norm joint step — but blows up near singularities where $JJ^\top$ becomes ill-conditioned.
• Damped Least Squares (DLS / Levenberg–Marquardt): $\;\Delta\mathbf q = J^\top(JJ^\top + \lambda^2 I)^{-1}\Delta\mathbf x$. The damping $\lambda$ trades accuracy for stability, taming singularities at the cost of small steady-state error.
• Jacobian transpose $\Delta\mathbf q = \alpha J^\top \Delta\mathbf x$ requires no matrix inverse and behaves like gradient descent on $\tfrac12\|\mathbf e\|^2$ — cheap and robust, but slow to converge.

Singularities and manipulability

A singularity is a configuration where $J$ loses rank: the arm can no longer move instantaneously in some task direction. We quantify proximity with Yoshikawa's manipulability index

which collapses to $0$ at a singularity (think of an arm stretched fully straight — it cannot extend further). Production controllers monitor $w$ or the condition number $\kappa(J)$ and switch on damping as they approach trouble.

Python Implementation: Numerical IK Solver

Rather than hand-deriving the Jacobian, a robust route is to pose IK as nonlinear least squares and let an optimizer handle it.

Loading Interactive Python Environment...

Note this 3-link arm reaching a 2D target is already redundant: 3 joints, 2 constraints. The optimizer returns one of infinitely many solutions — which one depends on the initial guess. That redundancy is a curse for a 3-link toy and an existential challenge for a tentacle.

The competing IK families and where they break

High-Dimensional vs. Low-Dimensional: Redundancy as the Dividing Line

For a low-DOF arm ($n = m$, e.g. a 6-DOF arm reaching a 6-D pose), IK has a finite number of solutions — typically up to 8 for a 6R arm (elbow up/down, wrist flip, etc.). You enumerate them and pick one.

For a high-DOF (redundant, $n > m$) robot, the solution set is a continuous $(n-m)$-dimensional manifold — infinitely many configurations reach the same point. This is simultaneously a gift (dexterity, obstacle avoidance, fault tolerance) and a curse (you must choose). The standard tool is null-space projection:

The first term achieves the task; the second term lives in the null space of $J$ and reshapes the arm without moving the tip — used to push toward $\dot{\mathbf q}_0 = \nabla H(\mathbf q)$, the gradient of a secondary objective $H$ (stay away from joint limits, maximize manipulability, dodge an obstacle). This is how a redundant robot scratches its elbow while keeping its hand still.

As $n \to \infty$ (a continuum arm), even this breaks down: the null space is infinite-dimensional, the Jacobian becomes an operator, and we must regularize with physically meaningful priors (minimum bending energy, smoothness). This is precisely why learning-based and energy-based methods (Part 7) increasingly replace classical IK for soft robots.

Focus on the Octopus: Reachability and Hyper-Redundancy

For a rigid robot, IK returns joint angles. For our metallic continuum octopus, IK is exponentially harder because a tentacle is hyper-redundant — theoretically infinite DOF — so there are infinitely many ways to reach any point.

We resolve this redundancy by adding secondary constraints to the cost, exactly as in the null-space formulation above: minimize total bending energy, avoid obstacles, or orient the suction cups in a chosen direction. Concretely, we solve

Before deploying control we also run reachability analysis: executing the IK solver across a dense 3D grid (millions of queries) to map the exact workspace volume the tentacle can reach without violating its bending limits. For a 6-DOF arm this map is a tidy shell; for a tentacle it is a thick, soft-edged cloud whose boundary depends on the secondary objectives we chose — a vivid reminder that in high dimensions, the question "can you reach it?" has no answer until you say what else you care about.

In Part 4 we add mass, force, and acceleration, moving from kinematics to Dynamics.

Further reading: Buss (2004), "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares"; Nakamura (1991), "Advanced Robotics: Redundancy and Optimization"; Diffusion Policy (arXiv:2303.04137) for learning multimodal solutions where classical IK struggles.