Inverse Kinematics: Closed-Form and Numerical Methods
IK has no general closed form — and the numerical methods that substitute for it are local, fragile, and must be treated as engineering tools not mathematical truths.
🎯 By the end you'll understand
How to derive geometric IK for a 2-link planar arm — including both the elbow-up and elbow-down solutions. Why spherical-wrist arms admit analytic IK that decouples position from orientation. How the Jacobian pseudoinverse turns IK into an iterative update rule. Why damped least squares exists and what lambda actually controls. How to detect IK failure — divergence, oscillation, workspace limits — and what to do when it happens.
KEY INSIGHTIK is the question FK can always answer backwards — but almost never analytically
Forward kinematics is a function. Inverse kinematics is finding its preimage. The preimage of a smooth nonlinear function is, in general, a mess.
Forward kinematics (FK) takes joint angles and computes end-effector pose. It is a closed-form composition of rotation matrices — smooth, unique, always computable. Inverse kinematics (IK) runs the question backwards: given a desired end-effector pose, find the joint angles that achieve it. This seems symmetric. It is not. FK maps a bounded Euclidean space (joint space) onto a Euclidean target (task space). IK inverts that map — and the map is nonlinear, potentially many-to-one, and undefined outside the workspace. The existence, uniqueness, and computability of an IK solution are each non-trivial questions that depend on the arm's geometry.
Existence: is the target inside the workspace? A target beyond reach has no IK solution regardless of method. Uniqueness: how many solutions exist? A 2-link arm typically has two (elbow-up, elbow-down); a 7-DOF arm has infinitely many (redundancy). Computability: can we find a solution in closed form, or must we iterate? Closed-form (analytic) IK is always preferred for real-time control — it is fast, repeatable, and returns all solutions simultaneously. Numerical IK is a fallback when geometry blocks analytic solutions; it is an approximation loop, not a solver. Treat them accordingly.
HOW IT WORKSGeometric IK: the 2-link planar arm
The 2-link planar arm is the simplest case that exhibits IK's key features. Two links of lengths L₁ and L₂, two revolute joints, end-effector at (x, y). FK gives:
x = L₁ cos(θ₁) + L₂ cos(θ₁ + θ₂) y = L₁ sin(θ₁) + L₂ sin(θ₁ + θ₂)
To invert this, use the law of cosines. The distance from the base to the target is d = √(x² + y²). The cosine rule applied to the triangle formed by the two links gives:
import numpy as np def geometric_ik_2link(x, y, L1, L2): """ Returns (theta1_up, theta2_up), (theta1_down, theta2_down) or None if target is outside workspace. """ d2 = x**2 + y**2 d = np.sqrt(d2) # Reachability check — law of cosines denominator cos_t2 = (d2 - L1**2 - L2**2) / (2 * L1 * L2) if abs(cos_t2) > 1.0: return None # outside workspace (too far or fully collapsed) sin_t2_pos = np.sqrt(1 - cos_t2**2) # elbow-up: θ₂ > 0 sin_t2_neg = -sin_t2_pos # elbow-down: θ₂ < 0 def solve(sin_t2): theta2 = np.arctan2(sin_t2, cos_t2) k1 = L1 + L2 * cos_t2 k2 = L2 * sin_t2 theta1 = np.arctan2(y, x) - np.arctan2(k2, k1) return theta1, theta2 return solve(sin_t2_pos), solve(sin_t2_neg)
The key step is arctan2(y, x) - arctan2(k2, k1). The first term is the angle to the target in the base frame. The second term is the angle correction needed for the second link to reach the target given theta2. This derivation is entirely geometric — no iteration, no matrices, O(1) to compute. It handles the full circle (atan2, not atan) and correctly returns both solutions.
CORE CONCEPTThe elbow-up/down choice: a decision, not a solution
Both elbow-up and elbow-down solutions are mathematically correct. Choosing between them is an engineering decision with real consequences:
| Criterion | Elbow-up (θ₂ > 0) | Elbow-down (θ₂ < 0) |
|---|---|---|
| Obstacle avoidance | Elbow raised — avoids surface below | Elbow lowered — avoids ceiling above |
| Singularity proximity | Closer to singularity at θ₂ = 0 if L₁ ≈ L₂ | Closer to singularity at θ₂ = ±π |
| Joint limit clearance | Depends on arm geometry | Depends on arm geometry |
| Continuity in path | Switching solutions mid-path causes a discontinuous joint jump — always avoid this | |
The right policy: choose a solution branch at the start of a task and commit to it for the entire trajectory. Switching branches during motion requires either a halt-and-replan or a coordinated sweep through the singular configuration (which is always dangerous). Production arms encode this choice as a "configuration" parameter — often a bit flag passed alongside the target pose — so the IK solver knows which branch to use.
Near the workspace interior where both solutions are close in joint space (θ₂ ≈ 0), numerical noise in the target pose can flip the preferred solution between IK calls. A simple guard: define a preference (elbow-up), and only switch to elbow-down if the elbow-up solution violates a joint limit. Never switch because the elbow-down solution is marginally "closer" in joint-angle Euclidean distance — that metric is meaningless for rotary joints across branches.
CORE CONCEPTAnalytic IK for spherical-wrist arms
Most industrial 6-DOF arms (KUKA KR series, ABB IRB series, UR series) use a spherical wrist — the last three joints have axes that meet at a single point. This geometry is not accidental: it enables a powerful decoupling trick that makes closed-form IK tractable.
Decouple position from orientation
Given the desired end-effector pose (position p, rotation R), compute the wrist center w:
# d6 = distance from wrist centre to end-effector (tool length) # z-axis of desired end-effector frame in base frame: z_ee = R[:, 2] # third column of rotation matrix w = p - d6 * z_ee # wrist centre position
Now joints 1–3 need only position w (a 3D point). The wrist joints 4–6 need only the remaining orientation error after joints 1–3 are set. The two problems are fully decoupled.
Solve for joints 1–3 geometrically
Project w onto the plane defined by link 1. Joint 1 is atan2(w_y, w_x) (directly from the wrist center's horizontal direction). Joints 2–3 are solved with the same 2-link geometric IK as before, applied in the vertical plane containing the arm.
Solve for joints 4–6 via Euler-angle extraction
Once joints 1–3 are set, compute the rotation R₀³ achieved by the first three joints. The remaining rotation needed from the wrist is:
R_wrist = R03.T @ R # rotation error for wrist to achieve # Extract ZYZ or ZYX Euler angles from R_wrist # → θ₄, θ₅, θ₆ (multiple solutions: ±θ₅)
The ZYZ Euler extraction has two solutions (±θ₅), giving a total of 2 × 2 = 4 solution branches for a spherical-wrist arm (not 8 — some configurations are degenerate).
The spherical wrist is not an aesthetic choice. Without it, the IK problem for a 6-DOF arm couples all six joint angles simultaneously — the system of six transcendental equations in six unknowns has no known general closed form. With a spherical wrist, the position problem (joints 1–3) and the orientation problem (joints 4–6) are separate, each tractable geometrically. If you are designing a robot arm and want analytic IK, you must design a spherical wrist in. The UR series (UR3e, UR5e, UR10e) does have a spherical wrist, enabling analytic IK via wrist-center decoupling. However, the UR series has a shoulder offset and elbow offset that require additional geometric handling compared to the idealized zero-offset case above — the DLS numerical approach remains useful for UR when working near singularities.
HOW IT WORKSJacobian pseudoinverse IK
Geometric and analytic IK require exploitable arm geometry. When that geometry is absent — irregular link lengths, non-spherical wrist, redundant DOF — numerical IK steps in. The most important class uses the Jacobian, which you derived in Lesson 2.4.
Recall: the Jacobian J maps joint velocities q̇ to end-effector velocities ẋ:
import numpy as np def ik_pseudoinverse(fk_fn, J_fn, q0, x_target, max_iter=50, tol=1e-3, step=0.5): """ Jacobian pseudoinverse IK. fk_fn(q) → x (current end-effector pose, 6-vector) J_fn(q) → J (6×n Jacobian at q) Returns q_sol or None if max_iter exceeded. """ q = q0.copy() for i in range(max_iter): x_cur = fk_fn(q) err = x_target - x_cur # task-space error (6-vec) if np.linalg.norm(err) < tol: return q # converged J = J_fn(q) Jinv = np.linalg.pinv(J) # Moore-Penrose pseudoinverse dq = Jinv @ err q += step * dq # step-size α prevents overshoot return None # ALWAYS handle this case
The pseudoinverse J⁺ = Jᵀ(JJᵀ)⁻¹ minimises the joint-space norm ‖Δq‖ among all solutions to J·Δq = Δx. For a redundant arm (more DOF than task dimensions), this picks the "smallest" joint motion — other null-space terms can be added to optimise secondary objectives like joint-limit clearance.
When the Jacobian is rank-deficient (or nearly so), the pseudoinverse amplifies tiny task-space errors into enormous joint-space commands. Near a wrist singularity, J⁺ may command a 10 rad/s joint velocity to correct a 0.001 m position error. The arm jerks, the trajectory is ruined, and if the commanded velocity exceeds the hardware limit, the real robot saturates. This is not a numerical edge case — it happens every time a path passes through a singular configuration. The fix is damped least squares.
HOW IT WORKSDamped least squares: trading accuracy for stability
Damped least squares (DLS), also called the Levenberg-Marquardt IK, modifies the pseudoinverse to regularise near-singular behaviour:
def dls_update(J, err, lam): """ Δq = Jᵀ (J Jᵀ + λ²I)⁻¹ · err lam: damping constant (lambda) — controls stability vs. speed tradeoff. """ n = J.shape[0] # task space dimension A = J @ J.T + (lam**2) * np.eye(n) dq = J.T @ np.linalg.solve(A, err) return dq def ik_dls(fk_fn, J_fn, q0, x_target, lam=0.05, max_iter=100, tol=1e-3): q = q0.copy() for i in range(max_iter): err = x_target - fk_fn(q) if np.linalg.norm(err) < tol: return q, i, 'converged' dq = dls_update(J_fn(q), err, lam) q += dq return q, max_iter, 'max_iter' # return best effort + status
The key insight: replacing JJᵀ with JJᵀ + λ²I bounds the singular values of the inverse from below. When J is full rank, λ² is negligible and the DLS solution approaches the pseudoinverse. When J is rank-deficient, the λ²I term prevents the inverse from blowing up — at the cost of not perfectly achieving the target (tracking error proportional to λ).
The tradeoff is explicit and controllable:
| λ value | Near singularity behaviour | At full rank | Use case |
|---|---|---|---|
| λ = 0 | Pseudoinverse blows up | Exact solution | Never use at singularities |
| λ = 0.01–0.05 | Moderate damping, small tracking error | Near-exact | Default production value |
| λ = 0.1–0.5 | Heavy damping, robot slows at singularity | Visible tracking lag | Safety-critical near-singular paths |
| λ = 1.0+ | Robot barely moves near singularity | Sluggish everywhere | Avoid — DLS is overloaded as a motion limiter |
A more sophisticated variant uses variable damping: λ is small (or zero) when the minimum singular value of J is large, and ramps up as the minimum singular value approaches zero. The manipulability measure w = √(det(JJᵀ)) from Lesson 2.5 directly tracks this — use it to schedule λ adaptively.
Numerical IK without explicit failure detection is a production hazard. Three criteria must be checked at every iteration: (1) Position tolerance: ‖x_current − x_target‖ < ε_pos (typically 0.1–1 mm). (2) Orientation tolerance: angle between current and target orientation < ε_rot (typically 0.1–1°). (3) Max iterations: if not converged after N iterations (50–200 is typical), declare failure and activate the fallback behavior. Never silently return the best-effort joint angles as if they were correct — always propagate the convergence status to the caller.
SEE ITIK solver: geometric + DLS side by side
Click anywhere in the workspace to set the end-effector target. The arm draws both elbow-up (blue) and elbow-down (gold) geometric solutions instantly. Use the DLS stepper to watch the Jacobian iteration converge from a random start — and adjust λ to see the stability/speed tradeoff. Drag the target outside the workspace boundary to reveal why workspace limits are a hard constraint, not a tuning parameter.
Notice three things in the widget. First, geometric IK gives both solutions instantly — no iteration. Second, DLS converges faster with small λ when far from singularities, but oscillates when the target is near the workspace boundary (where the Jacobian becomes rank-deficient). Third, the infeasible region (outside the workspace circle) produces no geometric solution at all — this is a hard boundary, not a numerical problem. Any IK method will fail here regardless of how many iterations you run.
FOLLOWBOTIK in practice: the arm pick-and-handoff task
FollowBot's 6-DOF arm runs two IK modes depending on the task phase:
Analytic IK for pre-defined poses
The home pose, the follow pose (arm tucked), and the handoff pose are all pre-defined Cartesian targets. For each, analytic IK (geometric for joints 1–3, Euler extraction for joints 4–6 via the spherical wrist) is computed once, at configuration time, and stored as a joint-angle trajectory. At runtime the arm executes the stored trajectory — no IK solver runs online. This is the right approach for any target that can be pre-computed: it is exact, fast, and reproducible.
DLS IK for online tracking
When the arm must track a moving object (the person's hand for a handoff), the target pose arrives from the perception system at ~15 Hz. DLS IK runs online in a ROS 2 timer callback at the same rate. The critical implementation detail: the DLS solver is warm-started from the current joint configuration (not from zero), so it needs far fewer iterations to converge because the previous target was close. λ is set to 0.05 nominally and increased to 0.15 if the manipulability measure w drops below a threshold (near-singular protection from Lesson 2.5).
Fallback: hold last pose on IK failure
If DLS fails to converge in max_iter iterations (which can happen if the person's hand moves faster than expected or enters a poor-manipulability region), the arm holds its last joint configuration and the behavior tree escalates to a "re-approach" action. The arm never sends a joint command unless IK returned a converged solution. This is the failure-mode contract — if the IK solver returns a "max_iter exceeded" status, the control system treats it as a sensor dropout, not as a valid command to execute.
import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import JointState import numpy as np class ArmIKNode(Node): def __init__(self): super().__init__('arm_ik') self.q_current = np.zeros(6) # warm-start state self.manipulability_thresh = 0.02 self.create_subscription( PoseStamped, '/arm/target_pose', self.on_target, 10 ) self.pub = self.create_publisher( JointState, '/arm/joint_commands', 10 ) def on_target(self, msg): x_target = pose_to_vec(msg) # 6-vector [pos; rpy] lam = self._pick_lambda(self.q_current) q_sol, n_iter, status = ik_dls( self.fk, self.jacobian, self.q_current, x_target, lam=lam, max_iter=50, tol=1e-3 ) if status != 'converged': self.get_logger().warn(f'IK failed ({status}, {n_iter} iter) — holding') return # HOLD — do not send bad command self.q_current = q_sol self.pub.publish(joints_msg(q_sol)) def _pick_lambda(self, q): # Adaptive damping: higher λ near singularities w = manipulability(self.jacobian(q)) return 0.05 if w > self.manipulability_thresh else 0.15
With this lesson, FollowBot's kinematic stack is complete. The diff-drive base has a forward kinematic model (Lesson 2.1) and its inverse (velocity IK = Lesson 2.7). The arm has FK (DH parameters, Lesson 2.3), a Jacobian (Lesson 2.4), singularity monitoring (Lesson 2.5), and now IK. The arm can receive a target pose from the perception system and compute joint commands. The remaining Stage 2 work is velocity control of the base — translating cmd_vel to actual wheel commands — covered in Lesson 2.7.
WEAKNESSWhere IK bites: three failure modes that look like other problems
Jacobian-based numerical IK is a local gradient method. It follows the error gradient in task space and can converge to a local minimum that is not the global minimum — or oscillate when the gradient points toward a singularity. This is especially common when the target is near the workspace boundary, where the Jacobian approaches rank deficiency and the gradient becomes nearly zero in directions the arm cannot reach. The standard symptom: the IK solver runs for all 100 iterations and returns a solution that is 2 cm off-target, with no convergence flag set. The fix is not to increase max_iter further — it is to check whether the target is inside the reachable workspace before calling the IK solver, and to have a fallback (approach the nearest reachable point instead).
The DLS update Δq = Jᵀ(JJᵀ + λ²I)⁻¹·Δx is a first-order linearisation valid only for small Δx. Applying it once with a large target error (e.g., the arm is fully retracted and the target is at full reach) produces a poor joint command because the linear approximation is wildly inaccurate. The solution is to scale the error: apply a fraction α < 1 of the raw update per iteration. Setting α = 1 (full Newton step) is almost always wrong for large errors. Setting α = 0.1–0.3 is safer. The widget makes this visible: watch the large-step case overshoot and oscillate versus the small-step case converge smoothly.
In a running system that calls geometric IK at every trajectory point, the preferred solution branch (elbow-up vs. elbow-down) can flip between successive calls if the evaluation criterion changes discontinuously (e.g., one branch suddenly hits a joint limit). The result is a sudden 180° elbow rotation — the arm physically cannot track this in one control cycle, the controller saturates, and the arm may trigger a hardware limit fault. The fix is branch locking: once a branch is chosen at the start of a task, continue using it even if the other branch appears marginally better, until the task ends or a deliberate re-configuration is commanded. This is not optional.
KEY INSIGHTHow it connects
- The Jacobian: Velocity Kinematics and the Spine of Robotics (2.4) — the Jacobian J is the engine of numerical IK. Jacobian pseudoinverse and DLS use J·Δq = Δx as the update equation. You cannot understand why DLS works, or why it fails at singularities, without the Jacobian derivation from 2.4.
- Singularity Analysis and Manipulability (2.5) — the manipulability measure w and the singular values of J directly determine where DLS needs heavier damping. The adaptive-λ strategy in the FollowBot arm node directly consumes the manipulability computation from 2.5.
- Velocity Control in Practice: cmd_vel to Wheel Commands (2.7) — the next lesson closes the kinematics loop for the mobile base: given a desired body velocity (from the path planner), what wheel velocity commands achieve it? This is the mobile-base analogue of IK — a closed-form inversion with a unique solution for differential-drive geometry.
IK is not a solved problem — it is a set of domain-specific engineering tradeoffs. Analytic IK is always preferred but requires exploitable geometry. Numerical IK is flexible but local, fragile at singularities, and requires explicit convergence checking. The DLS damping constant λ is not a free parameter to tune for better results — it is a stability-accuracy tradeoff whose correct value depends on the Jacobian's conditioning, which changes with configuration. Understanding this tradeoff is what separates roboticists who can deploy IK from those who copy it.
GLOSSARYKey terms introduced
- Inverse kinematics (IK)
- The problem of finding joint angles that produce a desired end-effector pose. The inverse of forward kinematics; may have zero, one, or multiple solutions depending on target location and arm geometry.
- Geometric IK
- A closed-form IK derivation using the law of cosines and atan2, applicable to arms with 2–3 links in a plane. Exact, O(1), and returns all solutions simultaneously. Requires geometric insight specific to the arm structure.
- Spherical wrist
- An arm configuration in which the last three joint axes intersect at a single point (the wrist centre). Enables decoupling of position IK (joints 1–3) from orientation IK (joints 4–6), making analytic IK tractable for 6-DOF arms.
- Jacobian pseudoinverse IK
- An iterative numerical IK method using the Moore-Penrose pseudoinverse of the Jacobian as an update rule: Δq = J⁺·Δx. Fast near the target, but amplifies small errors when J is rank-deficient (near singularities).
- Damped least squares (DLS)
- A regularised variant of Jacobian pseudoinverse IK: Δq = Jᵀ(JJᵀ + λ²I)⁻¹·Δx. The damping constant λ trades tracking accuracy for numerical stability near singularities.
- Damping constant (λ)
- The regularisation parameter in damped least squares IK. Small λ → fast convergence, unstable near singularities. Large λ → stable but slow; introduces systematic tracking error proportional to λ. Optimal value depends on the Jacobian's minimum singular value.
- Warm start
- Initialising an iterative IK solver from the current joint configuration rather than from zero. For online tracking tasks where successive targets are close, warm starting typically reduces DLS convergence to 2–5 iterations instead of 20–50.
- Solution branch
- One of the discrete IK solutions (e.g., elbow-up vs. elbow-down). Branch continuity — committing to a single branch throughout a trajectory — is required to avoid discontinuous joint motion.
ACTIVE RECALLCheck your understanding
Answer in your own words first, then reveal.
1A 2-link planar arm has L₁ = L₂ = 0.5 m and the target is at (0.8, 0.3). How many IK solutions exist, and how do you choose between them?›
First check reachability: d = √(0.8² + 0.3²) ≈ 0.854 m. The maximum reach is L₁ + L₂ = 1.0 m, and the minimum reach (fully folded) is |L₁ − L₂| = 0. Since 0 < 0.854 < 1.0, the target is inside the workspace and two solutions exist — elbow-up (θ₂ > 0) and elbow-down (θ₂ < 0). To choose: evaluate both against joint limits and obstacle constraints; if both are valid, prefer the branch that avoids passing through singular configurations (θ₂ = 0, fully extended) during the intended trajectory. Once chosen, commit to that branch for the entire trajectory — switching mid-path causes a discontinuous joint jump.
2Your DLS IK solver runs for 100 iterations on a reachable target and stops without meeting the position tolerance. List two likely causes and the diagnostic step for each.›
Cause 1: target near workspace boundary / singularity. The Jacobian is approaching rank deficiency; even with DLS the effective step toward the target is tiny and convergence stalls. Diagnostic: compute the manipulability measure w = √(det(JJᵀ)) at the current configuration. If w is very small (< 0.01 for typical arms), the target is kinematically inaccessible at this configuration even though it is geometrically reachable. Fix: reduce λ or use variable damping, or approach the target from a direction with higher manipulability. Cause 2: step size α too large — solver oscillates around the solution. Diagnostic: log the task-space error norm at each iteration. If it decreases for 5–10 steps then starts increasing (oscillating), α is too large and the linearisation is breaking down. Fix: reduce α (try 0.2 instead of 0.5) or implement line search.
3Why do 6-DOF arms with spherical wrists admit analytic IK, while 6-DOF arms without that geometry generally do not?›
Without a spherical wrist, the IK problem for a 6-DOF arm is a coupled system of 6 transcendental equations in 6 unknowns — each equation involves combinations of sine and cosine terms from all 6 joints simultaneously. No known algebraic technique reduces this to a solvable form in general. The spherical wrist introduces a geometric constraint (last 3 joint axes concurrent at a single point) that decouples the position problem from the orientation problem. The wrist centre position is determined by the desired end-effector position minus the tool offset along the z-axis. Joints 1–3 are solved for that position using 2-link geometric IK. Joints 4–6 are then solved for the remaining orientation using Euler-angle extraction from a 3×3 rotation matrix — a separate, tractable problem. Neither sub-problem involves all 6 joints simultaneously, so both are solvable in closed form.
4Why does increasing λ in DLS IK reduce oscillation near singularities, and what does it cost?›
Near a singularity, the smallest singular value σ_min of J approaches zero, which makes (JJᵀ)⁻¹ amplify errors by a factor of 1/σ_min² — producing enormous joint commands from tiny task-space errors. DLS replaces (JJᵀ)⁻¹ with (JJᵀ + λ²I)⁻¹, which bounds the amplification: the effective singular value is never smaller than √(σ_min² + λ²) ≈ λ when σ_min is small. This caps the joint command magnitude and prevents oscillation. The cost is a systematic tracking error: the DLS solution minimises ‖J·Δq − Δx‖² + λ²‖Δq‖², not ‖J·Δq − Δx‖² alone. The λ² term biases the solution toward small joint motion, which means the task-space error is not driven to zero — the solver converges to a point where the tracking error is proportional to λ. The larger λ, the more the arm slows down and stops short of the target near singularities.