Inverse Kinematics: From Pose to Joint Angles

Inverse kinematics (IK) is the reverse of FK: given a desired end-effector pose $T_{desired}$, find joint angles $q$ such that $T_0^{EE}(q) = T_{desired}$. IK is fundamentally harder than FK:

• Multiple solutions — the same pose may be reachable by several joint configurations (elbow-up vs. elbow-down).
• No solution — the target may be outside the robot's workspace.
• Infinite solutions — a redundant robot ($n > 6$ DOF) has extra freedom; infinitely many $q$ reach the same pose.

Two broad approaches: analytical (closed-form, fast, exact, but arm-specific) and numerical (iterative, general, but slower and convergence-sensitive). Industrial robots almost always use analytical IK in the inner loop.

For a 2-link arm with link lengths $l_1, l_2$, target position $(x, y)$ (ignoring orientation), FK gives:

$x = l_1 c_1 + l_2 c_{12}, \qquad y = l_1 s_1 + l_2 s_{12}$

where $c_{12} = \cos(q_1+q_2)$, $s_{12} = \sin(q_1+q_2)$. Square and add:

$x^2 + y^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos q_2$

Solve for $q_2$:

$\cos q_2 = \frac{x^2 + y^2 - l_1^2 - l_2^2}{2 l_1 l_2}$

If the right-hand side has magnitude greater than 1, the target is unreachable. Otherwise $q_2 = \pm \arccos(\cdot)$ — two solutions, giving the two elbow configurations.

The sign of $q_2$ selects the configuration:

• $q_2 > 0$: elbow-up (joint 2 bends counter-clockwise, arm curves upward)
• $q_2 < 0$: elbow-down (joint 2 bends clockwise, arm curves downward)

Both configurations reach the same $(x, y)$. In practice, the robot controller picks one based on:

1. Joint limits (one solution may violate them).
2. Shortest-path continuity (stay near the current configuration).
3. Obstacle avoidance.

With $q_2$ known, $q_1$ follows from the FK equations:

$q_1 = \text{atan2}(y, x) - \text{atan2}(l_2 s_2, l_1 + l_2 c_2)$

This gives a unique $q_1$ for each sign of $q_2$, yielding exactly two (q_1, q_2) pairs for a reachable target.

Use atan2(y, x) instead of arctan(y/x) for three reasons:

1. Full-circle coverage. arctan returns angles in $(-\pi/2, \pi/2)$; atan2 returns $(-\pi, \pi]$, covering all four quadrants.
2. No division by zero. arctan(y/0) is undefined; atan2(y, 0) correctly returns $\pm \pi/2$.
3. Sign disambiguation. arctan(-1/-1) equals arctan(1/1), so you cannot tell the angle is in the third quadrant. atan2 uses both signs independently.

In NumPy: np.arctan2(y, x). The argument order is (y first, x second) — a classic gotcha. Robot IK code that uses np.arctan instead of np.arctan2 will produce wrong joint angles in half of configurations.

import numpy as np

def ik_2link(x, y, l1=1.0, l2=0.8):
    """
    Analytical IK for a 2-link planar arm.
    Returns (q1_up, q2_up), (q1_down, q2_down) or None if unreachable.
    """
    cos_q2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)
    if abs(cos_q2) > 1.0:
        return None  # target unreachable

    q2_up   =  np.arccos(cos_q2)   # elbow-up
    q2_down = -np.arccos(cos_q2)   # elbow-down

    def solve_q1(q2):
        return np.arctan2(y, x) - np.arctan2(l2*np.sin(q2), l1 + l2*np.cos(q2))

    return (solve_q1(q2_up), q2_up), (solve_q1(q2_down), q2_down)

# Target: x=1.2, y=0.5
sols = ik_2link(1.2, 0.5)
for label, (q1, q2) in zip(["elbow-up", "elbow-down"], sols):
    print(f"{label}: q1={np.degrees(q1):.1f} deg, q2={np.degrees(q2):.1f} deg")

Always verify: pass each $(q_1, q_2)$ back through FK and confirm the position matches $(x, y)$ within numerical tolerance.

Two failure modes every IK implementation must handle:

Workspace boundary singularity. When $|\cos q_2| = 1$, the arm is fully extended or fully folded. The elbow-up and elbow-down solutions merge into one; any small position error can flip the solution branch discontinuously. The robot jerks.

Internal singularity (kinematic singularity). At certain configurations the Jacobian loses rank — the end-effector instantaneously cannot move in some direction, no matter what the joints do. For the 2-link arm this only happens at full extension/retraction. For 6-DOF arms there are additional wrist singularities (three wrist axes aligned) and elbow singularities.

Unreachable pose. $\cos q_2 > 1 \Rightarrow$ target too far; $\cos q_2 < -1 \Rightarrow$ target inside the inner dead zone (less common). Always guard with if abs(cos_q2) > 1: return None.

For arms where closed-form solutions don't exist (or are too complex), use numerical IK:

1. Start from a seed configuration $q^{(0)}$.
2. Compute the current pose error $\Delta x = x_{desired} - x(q^{(k)})$.
3. Update: $q^{(k+1)} = q^{(k)} + J^+(q^{(k)}) \, \Delta x$

where $J^+$ is the pseudoinverse of the Jacobian. Repeat until $\|\Delta x\| < \epsilon$. Pitfalls:

• Convergence not guaranteed near singularities (where $J$ loses rank and $J^+$ blows up).
• Local minima in workspace: the iteration may stop at a wrong configuration.
• Joint limits must be enforced explicitly with clamping or projected-gradient steps.

The Jacobian $J(q)$ — what it is and how to build it — is the subject of the next lesson.

Steps from target position to joint angles:

With two candidate solutions in hand, the controller must choose one. Standard industrial practice:

def select_solution(sols, q_current, joint_limits=None):
    """Pick the IK solution closest to current config."""
    best, best_dist = None, float('inf')
    for (q1, q2) in sols:
        if joint_limits:
            (lo1, hi1), (lo2, hi2) = joint_limits
            if not (lo1 <= q1 <= hi1 and lo2 <= q2 <= hi2):
                continue
        dist = (q1 - q_current[0])**2 + (q2 - q_current[1])**2
        if dist < best_dist:
            best, best_dist = (q1, q2), dist
    return best

After selection, always verify by running FK on the chosen angles and checking that the position error is below your tolerance (1e-6 m is typical). If FK and IK disagree, you have an algebra bug — catch it here, not at the real robot.