Forward and inverse kinematics, without the fog

Every motor on a typical robot arm is a revolute joint: it rotates about a fixed axis. The tool or gripper at the far end is the last link in that chain. Kinematics answers two inverse questions about that chain. Once you see them clearly, the rest of the machinery — matrices, Jacobians, solvers — is bookkeeping.

The two questions

Forward kinematics (FK). Given the joint angles $q$, where is the end-effector in the base frame? The answer is a pose: position $(x,y,z)$ and orientation $R$. The map is unique. One set of angles produces one pose. You multiply transforms.

Inverse kinematics (IK). Given a desired tip pose $p^*$, which joint angles put the tip there? That map may have many solutions, one, or none — out of reach, or past joint limits.

Those two questions apply to any serial chain: a desk arm, a welding robot, a crane, even a 2D cartoon of sticks on paper. The dimension and the joint count change; the questions do not.

Warm-up: a 2-joint planar arm

Six-degree-of-freedom formulas look dense because they are $4\times 4$ matrices. On a flat table with two hinges, the same FK is high-school trigonometry. Do this once on paper; everything later is the same idea in 3D.

Put the shoulder at the origin. Link 1 has length $L_1$, link 2 has $L_2$. Joint 1 rotates by $\theta_1$, joint 2 by $\theta_2$ relative to link 1. The tip is:

Why add? The second link starts where the first ends. The first term is the elbow; the second is elbow to tip.

Worked numbers with $L_1 = 65\,\mathrm{mm}$, $L_2 = 116\,\mathrm{mm}$, $\theta_1 = 30^\circ$, $\theta_2 = 45^\circ$:

• $\cos 30^\circ = 0.8660$, $\sin 30^\circ = 0.5000$
• $\theta_1+\theta_2 = 75^\circ$, $\cos 75^\circ = 0.2588$, $\sin 75^\circ = 0.9659$
• $x = 65\cdot 0.8660 + 116\cdot 0.2588 = 86.31\,\mathrm{mm}$
• $y = 65\cdot 0.5000 + 116\cdot 0.9659 = 144.54\,\mathrm{mm}$

Drag the sliders below to see that formula live. The printed $(x,y)$ should match the drawing, and the $4\times 4$ readout is the same FK answer packed as a homogeneous transform.

T₀ᴱᴱ  ≈  (2-joint planar FK)
[    0.259   -0.966    0.000   86.315 ]
[    0.966    0.259    0.000  144.547 ]
[    0.000    0.000    1.000    0.000 ]
[    0.000    0.000    0.000    1.000 ]

Check: first link ends at (56.29, 32.50)

IK for this arm has a closed form from the law of cosines. Let $r^2 = x^2 + y^2$:

The $\pm$ is elbow-up versus elbow-down — two ways to put the tip at the same $(x,y)$. If $|\cos\theta_2| > 1$, the point is outside the reachable ring and IK fails honestly.

Plug in the numbers above and you recover $\theta_2 \approx \pm 45^\circ$; then $\theta_1$ follows from $\mathrm{atan2}$. That is already inverse kinematics: pose in, angles out.

Drag the green target (or use the $x,y$ sliders). The arm solves IK live. The grey dashed arm is the other elbow solution. Drag outside the rings and IK fails ($|\cos\theta_2| > 1$).

p* = (86.0, 145.0) mm
cos θ₂ = (x² + y² − L₁² − L₂²) / (2 L₁ L₂)
       = (86.0² + 145.0² − 4225 − 13456) / 15080
       = 10740.0 / 15080 = 0.7122   →  |cos| ≤ 1, two elbows
active elbow sign = +1 (up)   θ₂ = ±arccos(·)
θ₁ = atan2(y, x) − atan2(L₂ sinθ₂, L₁ + L₂ cosθ₂)
FK check of this q*:  tip = (86.00, 145.00)   |e| = 0.00 mm
other elbow: θ₁ = 88.2°, θ₂ = -44.6°  (grey dashed)

A longer 3D arm rarely has an equally simple cosine formula for every joint. The meaning does not change. You still ask “which angles put the tip here?” You answer with a matrix chain for FK and, when needed, a numerical solver for IK.

What a 4×4 homogeneous transform does

Section two already did forward kinematics in 2D: given $\theta_1,\theta_2$, add the sticks. A homogeneous transform is not a third kind of kinematics. It is the same idea rewritten so it still works when the chain leaves the page — pan, lift, roll, full 3D.

A rigid link has its own “here”: the shoulder’s here, the elbow’s here, the tip’s here. Those heres differ in two ways only:

• Where that piece sits — a translation
• Which way it faces — a rotation

A 4×4 homogeneous transform packs both into one object. Think of it as a sticky note on a link:

If you are standing at the previous joint, take this many millimetres in this direction, then spin this many degrees — now you are standing at me.

Why 4×4, not 3×3? A 3×3 can spin a point but cannot slide it. A 3-vector can slide but cannot spin. One multiply then means “move from this joint to the next.” Walking the whole arm is applying those notes in order — which in algebra is multiplying them.

The top-left $3\times 3$ ($R$) is orientation. The right column $t$ is the origin. The bottom row is always $(0,0,0,1)$ so slide and spin stay in one multiply.

Apply $T$ to a point written as a 4-vector:

Compose frames by multiplying. If $T_A^B$ is “$B$ as seen from $A$” and $T_B^C$ is “$C$ as seen from $B$,” then:

Order matters. Parent-then-child is the physical chain: base, then joint 1, then joint 2, and so on. Six motors does not mean a 6×6 pose matrix. Each joint still has its own $4\times 4$; the product is still $4\times 4$, because the tip still has one position and one orientation in 3D space.

One joint’s matrix, then the chain

Each revolute joint $i$ contributes one matrix $A_i(q_i)$. In the Denavit–Hartenberg (DH) convention, that note is four tiny moves — fixed link geometry times the live motor angle:

Rule of thumb: if the letter has a joint index $i$ and depends on one angle, it is an $A$. If it has a from–to pair and is a product, it is a $T$.

Walk from the base (frame 0) out to the end-effector by multiplying one $A_i$ at a time:

That product is the tip pose. You do not invert anything to read it. Copy the right-hand column for position $p_{\mathrm{EE}} = t$, and the top-left $3\times 3$ for orientation $R_{\mathrm{EE}}$.

Why not one giant formula for $(x,y,z)$ like the 2-joint warm-up? You can, after multiplying all $A_i$. It becomes a mess of nested sines. Keeping small crates is shorter, matches how robot descriptions (URDF and friends) store the chain, and is what kinematics libraries do.

What FK is for in practice

Once you can compute $p_{\mathrm{EE}}(q)$ from measured or recorded joints, you can ask ordinary engineering questions without solving IK:

1. Height. Is the tip near the object height at the grasp frame?
2. Target alignment. Is the tip over the intended drop or weld point?
3. Reachability. Is $\|p_{\mathrm{EE}}\|$ within the sum of the long links?
4. Clearance. After a pick, did $z$ rise above the table — or did the tip drag sideways?

Those checks use joints you already have. Finding new joints that would reach a chosen $p^*$ is the inverse problem.

Inverse kinematics as a search

FK is unique. IK is the awkward reverse. Many angle combinations can share one tip pose. Sometimes none work.

In one sentence: find $q^*$ such that FK lands near the chosen point $p^*$, without breaking motor limits.

Jaw open/close (or any DOF that does not move the tool frame) is usually a separate map, not part of this position puzzle.

IK can fail honestly: target beyond maximum reach, behind the pedestal, or past a joint limit. That is a layout or task problem, not a bug in the formula. A good solver must say “no” rather than invent a pose that would snap a servo.

Numerical IK: nudge, measure, repeat

For more than a couple of planar joints, you usually do not magically invert FK. You nudge the motors a little, ask FK where you landed, repeat until the tip is close enough to $p^*$. That loop is numerical IK.

The error is the leftover miss:

The Jacobian $J(q)$ is a table of “what if I twitch one motor?” Each column is one servo: a tiny turn of that motor moves the tip in this Cartesian direction. For position-only IK on $n$ motors, $J$ is $3\times n$. It grows with joint count; the pose matrix $T_0^{\mathrm{EE}}$ does not.

On the 2-joint warm-up the same table is small enough to write out:

You want a small joint change $\Delta q$ such that $J\,\Delta q \approx e$. When $J$ is wide (more knobs than directions), many $\Delta q$ could work. A common compromise is damped least-squares:

$\lambda$ is a small cushion. If $\lambda = 0$ and the arm is stretched (a singularity), $\Delta q$ can explode. Think of $\lambda$ as “do not panic when the cheat sheet is unclear.”

Each iteration:

1. Start from current $q$
2. FK: where is the tip?
3. Form the miss $e$
4. Compute $\Delta q = J^{+} e$
5. Set $q \leftarrow q + \Delta q$, clip to joint limits
6. Stop when $\|e\|$ is tiny, or give up after too many tries

Watch that loop on the same 2-joint arm. Drag the target, then Step once to apply one $\Delta q = J^{+} e$, or Solve until the miss is under $2\,\mathrm{mm}$. This is the same recipe a 6-DOF solver uses; here $J$ is $2\times 2$ so every number stays readable.

q starts at (15°, 25°). Each step: e = p* − FK(q),  Δq = J⁺ e.

The starting guess matters. From a folded posture the solver may pick the other elbow configuration or get stuck. When the real arm is in the loop, seed from measured joints; when replaying without feedback, seed from the previous solution.

After $q^*$ exists, motion control interpolates from today’s joints to that goal. That interpolation is neither FK nor IK — it is the sequel to IK. Learning policies that copy joint trajectories from demonstration skip the online IK path entirely; they still benefit from FK as a check that the demonstrated joints made geometric sense.

Symbol cheat-sheet

Takeaways

1. FK multiplies joint transforms and reads the tip. Unique, cheap, always available once you have $q$.
2. IK searches for angles that realise a pose. Multiple, one, or zero answers — and failure is information.
3. The $4\times 4$ is a packing crate for “slide + spin.” Six joints still yield one $4\times 4$ tip pose; the Jacobian is what grows with joint count.
4. Start with the 2-joint arm on paper. Every longer chain is the same two questions with more sticky notes.

Angles in, pose out. Pose in, angles out. Everything else is how carefully you ask those two questions in three dimensions.