[Robotics] Inverse Kinematics
Introduction
In the forward kinematics problem, we are given a vector of joint variables $\boldsymbol{\theta} = (\theta_1, \theta_2, \ldots, \theta_n)^\top$ and we compute the end-effector pose
\[\mathbf{T}(\boldsymbol{\theta}) \in SE(3).\]The inverse kinematics (IK) problem reverses this map: given a desired end-effector pose $\mathbf{X} \in SE(3)$, find all $\boldsymbol{\theta}$ such that
\[\mathbf{T}(\boldsymbol{\theta}) = \mathbf{X}.\]This formulation is what makes a robot useful in practice. A user almost never thinks in joint coordinates — instead one specifies “place the gripper at this pose,” and the controller is responsible for synthesizing joint angles that realize that command. Despite its innocuous-looking statement, inverse kinematics is substantially more difficult than forward kinematics, for several intertwined reasons:
- The map $\mathbf{T}(\boldsymbol{\theta})$ is nonlinear in the joint variables. The componentwise equations involve products of sines and cosines, and unlike a linear system there is no general algorithm guaranteed to find all roots of a system of nonlinear transcendental equations.
- A solution may not exist. The desired pose can lie outside the workspace of the manipulator.
- When solutions exist, there may be multiple of them — sometimes infinitely many — and any practical algorithm has to decide which one to return.
- The behavior of solution methods degrades near singularities, where small Cartesian displacements demand arbitrarily large joint motions.
Given a serial manipulator with forward kinematics $\mathbf{T} : \mathbb{R}^n \rightarrow SE(3)$ and a desired end-effector configuration $\mathbf{X} \in SE(3)$, find every joint vector $\boldsymbol{\theta} \in \mathbb{R}^n$ (subject to joint limits) such that $$ \mathbf{T}(\boldsymbol{\theta}) = \mathbf{X}. $$ A manipulator is said to be **solvable** if an algorithm exists that, in finite time, returns all such $\boldsymbol{\theta}$.
This post organizes the topic in the order in which one usually approaches it in practice: first we discuss when solutions exist (workspace) and how many to expect (multiplicity); then we cover closed-form analytic methods (algebraic, geometric, and Pieper’s decoupling for wrist-partitioned arms); and finally we examine numerical methods (Newton-Raphson, Jacobian iteration, and damped least squares).
Solvability of the IK Problem
Workspaces
Before we can solve $\mathbf{T}(\boldsymbol{\theta}) = \mathbf{X}$, we must ask whether a solution can exist at all. The set of poses that the end-effector can attain is called the workspace of the manipulator. It is usually decomposed as follows.
The **reachable workspace** of a manipulator is the set of points in $\mathbb{R}^3$ that the origin of the end-effector frame can reach with at least one orientation. The **dexterous workspace** is the set of points the end-effector can reach with **every** possible orientation.
The dexterous workspace is always a subset of the reachable workspace, and is usually strictly smaller. As a small example, consider a planar 2R manipulator with link lengths $L_1$ and $L_2$ ($L_1 \ge L_2$). Squaring and adding the position equations one obtains
\[x^2 + y^2 = L_1^2 + L_2^2 + 2 L_1 L_2 \cos \theta_2,\]so the reachable workspace in the $xy$-plane is the annulus
\[\big\{\, (x, y) : (L_1 - L_2)^2 \le x^2 + y^2 \le (L_1 + L_2)^2 \,\big\}.\]The dexterous workspace is empty for $L_1 \neq L_2$ and is the single point ${(0,0)}$ when $L_1 = L_2$, attained only when the arm is folded back on itself.
For a manipulator with $n < 6$ degrees of freedom, the reachable workspace is a subset of an $n$-dimensional subspace of $SE(3)$. Demanding a general goal frame is then immediately rejectable: only goals that lie inside that subspace can be reached. In such underactuated cases one often projects the desired pose onto the subspace, then solves IK against the projection.
A useful sufficient (and obvious) observation:
If the desired wrist (or tool) frame lies in the workspace of the manipulator, then at least one inverse kinematics solution exists.
Multiplicity of Solutions
Even when a solution exists, it is rarely unique. The classical example is the planar 3R manipulator with revolute joints: the same end-effector position and orientation $(x, y, \phi)$ in the interior of its dexterous workspace can be reached by two different joint configurations, related by reflecting the elbow across the line from the shoulder to the wrist. These are the familiar elbow-up and elbow-down solutions.
The picture becomes richer for general spatial 6-DOF arms. The PUMA 560, for instance, can reach a typical goal pose in eight distinct ways. Four of these correspond to combinations of “lefty/righty” shoulder and “elbow-up/elbow-down” configurations; for each of those four, the wrist can be flipped to obtain another valid joint vector by
\[\theta_4' = \theta_4 + 180^\circ,\quad \theta_5' = -\theta_5,\quad \theta_6' = \theta_6 + 180^\circ.\]In general, the maximum number of solutions grows with the number of nonzero link-length parameters $a_i$ in the Denavit-Hartenberg description. For a fully general spatial 6R manipulator with all $a_i \neq 0$, there can be up to sixteen distinct closed-form solutions. The following table summarizes Craig’s classification:
| Nonzero parameters | Max number of solutions |
|---|---|
| $a_1 = a_3 = a_5 = 0$ | $\le 4$ |
| $a_3 = a_5 = 0$ | $\le 8$ |
| $a_3 = 0$ | $\le 16$ |
| All $a_i \neq 0$ | $\le 16$ |
Multiplicity is not a bug; it is structural. A manipulator with $n > 6$ joints is called kinematically redundant, and for any reachable pose admits an $(n - 6)$-dimensional continuum of solutions. The selection problem (which solution to return) is then a separate algorithmic question — typically resolved by choosing the configuration closest in joint space to the current one, or by optimizing a secondary objective such as distance from joint limits or from obstacles.
A manipulator admits a **closed-form** (analytic) inverse kinematics solution if there exists an algorithm that returns all joint vectors satisfying $\mathbf{T}(\boldsymbol{\theta}) = \mathbf{X}$ using a finite sequence of arithmetic operations, root-finding for polynomials of degree at most four, and elementary inverse trigonometric functions. Otherwise one must resort to **numerical** (iterative) inverse kinematics.
A central theoretical fact, due to Pieper, is that all revolute/prismatic 6-DOF serial chains in which three consecutive joint axes intersect at a single point admit a closed-form solution. Almost every industrial 6-DOF arm has been designed to satisfy this condition (a spherical wrist), precisely so that real-time IK can be computed analytically.
Closed-Form Solutions
For manipulators that admit closed-form IK, we have two complementary toolkits: an algebraic approach that manipulates the kinematic equations directly, and a geometric approach that exploits planar sub-triangles in the arm’s structure. In practice the two often merge — geometric reasoning leads to algebraic identities and vice versa — and a useful solution typically blends both.
Algebraic Solution: Planar 3R Manipulator
Consider the planar 3R manipulator with link lengths $L_1$, $L_2$ and a third (orientation) link. Following the Denavit-Hartenberg construction, the forward kinematics in the base frame is
\[{}^{B}_{W}\mathbf{T}(\boldsymbol{\theta}) = \begin{bmatrix} c_{123} & -s_{123} & 0 & L_1 c_1 + L_2 c_{12} \\ s_{123} & c_{123} & 0 & L_1 s_1 + L_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},\]where we abbreviate $c_i = \cos \theta_i$, $s_i = \sin \theta_i$, $c_{ij} = \cos(\theta_i + \theta_j)$, $s_{ij} = \sin(\theta_i + \theta_j)$, and similarly $c_{123} = \cos(\theta_1 + \theta_2 + \theta_3)$. A planar goal is specified by three scalars $(x, y, \phi)$ where $\phi$ is the orientation of the last link relative to the base $+\hat{X}$ axis. Equating the goal frame with the forward kinematics gives four scalar equations:
\[\begin{aligned} c_\phi &= c_{123}, & s_\phi &= s_{123}, \\ x &= L_1 c_1 + L_2 c_{12}, & y &= L_1 s_1 + L_2 s_{12}. \end{aligned}\]Step 1: solve for $\theta_2$. Squaring and adding the position equations,
\[x^2 + y^2 = L_1^2 + L_2^2 + 2 L_1 L_2 c_2,\]so that
\[c_2 = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2 L_1 L_2}.\]For the goal to be reachable, the right-hand side must lie in $[-1, 1]$; this is an explicit reachability check. Then
\[s_2 = \pm \sqrt{1 - c_2^2},\]and we recover $\theta_2$ by the four-quadrant arctangent
\[\theta_2 = \mathrm{Atan2}(s_2, c_2).\]The choice of sign in $s_2$ selects between the two geometric solutions:
The two signs $s_2 = +\sqrt{1 - c_2^2}$ and $s_2 = -\sqrt{1 - c_2^2}$ correspond to the **elbow-up** and **elbow-down** configurations of the planar arm. Both reach the same end-effector pose, but with the intermediate joint reflected across the chord from shoulder to wrist.
Step 2: solve for $\theta_1$. Having fixed $\theta_2$, expand $c_{12} = c_1 c_2 - s_1 s_2$ and $s_{12} = s_1 c_2 + c_1 s_2$. Substituting,
\[x = k_1 c_1 - k_2 s_1, \qquad y = k_1 s_1 + k_2 c_1,\]where $k_1 = L_1 + L_2 c_2$ and $k_2 = L_2 s_2$. Introducing $r = \sqrt{k_1^2 + k_2^2}$ and $\gamma = \mathrm{Atan2}(k_2, k_1)$, we can write $k_1 = r \cos\gamma$, $k_2 = r \sin\gamma$, and the system collapses to
\[\cos(\gamma + \theta_1) = \frac{x}{r}, \qquad \sin(\gamma + \theta_1) = \frac{y}{r},\]yielding
\[\theta_1 = \mathrm{Atan2}(y, x) - \mathrm{Atan2}(k_2, k_1).\]The two choices of $\theta_2$ propagate into two values of $k_2$, hence two values of $\theta_1$.
Step 3: solve for $\theta_3$. Finally, the orientation equations $c_\phi = c_{123}$ and $s_\phi = s_{123}$ give
\[\theta_1 + \theta_2 + \theta_3 = \mathrm{Atan2}(s_\phi, c_\phi) = \phi,\]so $\theta_3 = \phi - \theta_1 - \theta_2$. This completes the closed-form solution: two distinct $\boldsymbol{\theta}$ for each reachable $(x, y, \phi)$ in the interior of the workspace.
Geometric Solution
The same problem can be approached purely from plane geometry. Consider the triangle formed by the two links $L_1, L_2$ and the line segment from the base origin to the wrist origin of length $\sqrt{x^2 + y^2}$. By the law of cosines, the interior angle at the elbow joint is
\[\beta = \cos^{-1}\!\left( \frac{L_1^2 + L_2^2 - (x^2 + y^2)}{2 L_1 L_2} \right),\]restricted to $[0, \pi]$. We then choose $\theta_2 = \pm(\pi - \beta)$, with the sign selecting elbow-up/elbow-down. Two further angles fix $\theta_1$:
\[\gamma = \mathrm{Atan2}(y, x), \quad \alpha = \cos^{-1}\!\left( \frac{x^2 + y^2 + L_1^2 - L_2^2}{2 L_1 \sqrt{x^2 + y^2}} \right),\]so that the “righty” solution is $\theta_1 = \gamma - \alpha$ and the “lefty” solution is $\theta_1 = \gamma + \alpha$. The third joint is again $\theta_3 = \phi - \theta_1 - \theta_2$. We obtain the same two solutions as the algebraic method, just viewed from a different angle.
Reduction to a Polynomial via the Half-Angle Substitution
A frequently used algebraic trick reduces a transcendental equation in one variable to a polynomial. Setting $u = \tan(\theta / 2)$, the half-angle identities give
\[\cos \theta = \frac{1 - u^2}{1 + u^2}, \qquad \sin \theta = \frac{2u}{1 + u^2}.\]For instance, the linear-in-$\sin/\cos$ equation $a \cos \theta + b \sin \theta = c$ becomes, after multiplying through by $1 + u^2$ and collecting,
\[(a + c)\, u^2 - 2b\, u + (c - a) = 0,\]whose solutions are
\[u = \frac{b \pm \sqrt{b^2 + a^2 - c^2}}{a + c}, \qquad \theta = 2 \tan^{-1} u.\]Whenever the closed-form solution of a manipulator demands solving a transcendental equation in a single joint variable, this substitution is the canonical trick. Polynomials up to degree four admit closed-form roots, so any IK problem that reduces to such a polynomial qualifies as closed-form solvable.
Kinematic Decoupling: Pieper’s Solution
For a general spatial 6R arm, no analytic algorithm is known in full generality; one must in principle solve a system whose root structure is governed by a polynomial of high degree (up to sixteen real roots). The breakthrough due to Pieper is the observation that if three consecutive joint axes intersect at a single point — for example, the last three joint axes of an arm with a spherical wrist — then the IK problem decouples cleanly into two subproblems of three joints each.
Consider a 6-DOF serial manipulator with revolute joints, in which the last three joint axes intersect at a common point $\mathbf{p}_w$ (the **wrist center**). Then, given a desired end-effector pose $\mathbf{X} = (\mathbf{R}, \mathbf{p}) \in SE(3)$: 1. The wrist center is determined by the desired position and orientation alone: $$ \mathbf{p}_w = \mathbf{p} - d_6 \mathbf{R} \hat{\mathbf{z}}, $$ where $d_6$ is the offset from the wrist center to the end-effector along the last link's axis. 2. The first three joint variables $(\theta_1, \theta_2, \theta_3)$ are obtained as the inverse-position solution that places the wrist center at $\mathbf{p}_w$. This problem reduces (after some algebra) to a polynomial of degree at most four in $\tan(\theta_3 / 2)$. 3. The last three joint variables $(\theta_4, \theta_5, \theta_6)$ are then obtained as the inverse-orientation solution that places the end-effector orientation at $\mathbf{R}$. Concretely, having computed the orientation $\mathbf{R}_3^0$ of the wrist base from $(\theta_1, \theta_2, \theta_3)$, the residual orientation is $$ \mathbf{R}_3^0{}^\top \mathbf{R} = \mathbf{R}_4^3(\theta_4) \mathbf{R}_5^4(\theta_5) \mathbf{R}_6^5(\theta_6), $$ which is solvable as a ZYZ (or analogous) Euler-angle problem.
The decoupling pivots on a clean fact: if axes 4, 5, 6 meet at $\mathbf{p}_w$, then the position of $\mathbf{p}_w$ does not depend on $\theta_4, \theta_5, \theta_6$ at all. Hence the position subproblem isolates the first three joints, and the orientation subproblem isolates the last three. Each subproblem is a 3-variable IK, which is far easier than the coupled 6-variable system.
Position subproblem in detail. Writing $\mathbf{p}_w = (x, y, z)^\top$ in base coordinates, and following Pieper’s derivation, one obtains a system of two equations
\[r = (k_1 c_2 + k_2 s_2)\, 2 a_1 + k_3, \qquad z = (k_1 s_2 - k_2 c_2)\, s\alpha_1 + k_4,\]where $r = x^2 + y^2 + z^2$ and $k_1, k_2, k_3, k_4$ depend only on $\theta_3$. Eliminating $c_2, s_2$ yields
\[\frac{(r - k_3)^2}{4 a_1^2} + \frac{(z - k_4)^2}{s^2\alpha_1} = k_1^2 + k_2^2,\]which after the half-angle substitution becomes a polynomial of degree at most four in $\tan(\theta_3 / 2)$. Once $\theta_3$ is known, $\theta_2$ follows from the original two equations, and $\theta_1$ from the projection of $\mathbf{p}_w$ onto the $\hat{x}_0$-$\hat{y}_0$ plane via $\theta_1 = \mathrm{Atan2}(y, x)$ (with a second solution $\theta_1 = \mathrm{Atan2}(y, x) + \pi$).
Orientation subproblem in detail. Having found $(\theta_1, \theta_2, \theta_3)$, we compute the rotation $\mathbf{R}_3^0$ at the wrist base and form
\[\mathbf{R}_6^3 = \big(\mathbf{R}_3^0\big)^\top \mathbf{R}.\]Since the last three joints form a spherical wrist, $\mathbf{R}_6^3$ can be parameterized as a product of three rotations about orthogonal axes — most commonly the ZYZ Euler form $\mathrm{Rot}(\hat{z}, \theta_4) \mathrm{Rot}(\hat{y}, \theta_5) \mathrm{Rot}(\hat{z}, \theta_6)$. Solving for the three Euler angles is a standard exercise and yields exactly two solutions related by
\[(\theta_4, \theta_5, \theta_6) \longleftrightarrow (\theta_4 + \pi, -\theta_5, \theta_6 + \pi).\]This wrist flip doubles the number of orientation solutions for each position solution, and is exactly the source of the “extra” four PUMA solutions noted earlier.
Numerical Methods
For manipulators outside Pieper’s class — including most 7-DOF redundant arms, manipulators with non-orthogonal axes, and any arm whose nominal kinematic parameters are perturbed by calibration — closed-form IK is unavailable or inaccurate. We then resort to iterative root-finding.
Newton-Raphson on the Kinematics Function
Recall the scalar Newton-Raphson iteration for solving $g(\theta) = 0$: linearize the residual at the current guess $\theta^k$,
\[g(\theta) \approx g(\theta^k) + \frac{\partial g}{\partial \theta}(\theta^k)(\theta - \theta^k),\]set the right-hand side to zero, and solve for the update
\[\theta^{k+1} = \theta^k - \left( \frac{\partial g}{\partial \theta}(\theta^k) \right)^{-1} g(\theta^k).\]The same formula generalizes to multi-dimensional $\mathbf{g} : \mathbb{R}^n \to \mathbb{R}^n$ with $\partial \mathbf{g} / \partial \boldsymbol{\theta}$ replaced by the $n \times n$ Jacobian matrix.
Apply this to inverse kinematics. Let $\mathbf{x}_d$ denote the desired end-effector coordinates and $\mathbf{f}(\boldsymbol{\theta})$ the forward kinematics expressed in the same coordinates. Define the residual
\[\mathbf{g}(\boldsymbol{\theta}) = \mathbf{x}_d - \mathbf{f}(\boldsymbol{\theta}).\]The Jacobian of $\mathbf{g}$ with respect to $\boldsymbol{\theta}$ is $-\mathbf{J}(\boldsymbol{\theta})$, the (negative) coordinate Jacobian of the forward kinematics. Newton-Raphson then reads
\[\mathbf{J}(\boldsymbol{\theta}^k)\, \Delta \boldsymbol{\theta} = \mathbf{x}_d - \mathbf{f}(\boldsymbol{\theta}^k), \qquad \boldsymbol{\theta}^{k+1} = \boldsymbol{\theta}^k + \Delta \boldsymbol{\theta}.\]Provided that the initial guess $\boldsymbol{\theta}^0$ is sufficiently close to a true solution $\boldsymbol{\theta}_d$, the iterates converge quadratically to $\boldsymbol{\theta}_d$.
For nonlinear $\mathbf{f}$, each isolated root has its own **basin of attraction** in the space of initial guesses. A Newton iteration started inside the basin of $\boldsymbol{\theta}_d$ converges to $\boldsymbol{\theta}_d$; started outside, it may converge to a different solution, oscillate, or diverge entirely.
SE(3) Residuals and the Log-Map Formulation
When the desired pose is given as $\mathbf{T}_{sd} \in SE(3)$ rather than as a coordinate vector, naively subtracting transformation matrices does not produce a meaningful residual. The correct generalization, following the Lynch and Park exposition, replaces coordinate subtraction with the matrix logarithm.
Suppose the current configuration of the body frame is $\mathbf{T}_{sb}(\boldsymbol{\theta}^k)$. The relative transformation that must be applied, expressed in the body frame, is
\[\mathbf{T}_{bd}(\boldsymbol{\theta}^k) = \mathbf{T}_{sb}^{-1}(\boldsymbol{\theta}^k)\, \mathbf{T}_{sd}.\]This element of $SE(3)$ can be represented as the exponential of a body twist over unit time,
\[[\boldsymbol{\mathcal{V}}_b] = \log \mathbf{T}_{bd}(\boldsymbol{\theta}^k), \qquad \boldsymbol{\mathcal{V}}_b = (\boldsymbol{\omega}_b, \mathbf{v}_b) \in \mathbb{R}^6.\]The Newton step then uses the body Jacobian $\mathbf{J}_b(\boldsymbol{\theta}) \in \mathbb{R}^{6 \times n}$:
\[\boldsymbol{\theta}^{k+1} = \boldsymbol{\theta}^k + \mathbf{J}_b^\dagger(\boldsymbol{\theta}^k)\, \boldsymbol{\mathcal{V}}_b,\]where $\mathbf{J}_b^\dagger$ is the Moore-Penrose pseudoinverse (right inverse when $\mathbf{J}_b$ is fat with $n > 6$, ordinary inverse when $n = 6$, left inverse when $n < 6$).
An entirely equivalent space-frame formulation is
\[\boldsymbol{\mathcal{V}}_s = [\mathrm{Ad}_{\mathbf{T}_{sb}}] \boldsymbol{\mathcal{V}}_b, \qquad \boldsymbol{\theta}^{k+1} = \boldsymbol{\theta}^k + \mathbf{J}_s^\dagger(\boldsymbol{\theta}^k)\, \boldsymbol{\mathcal{V}}_s,\]using the space Jacobian and the spatial twist.
Body-Jacobian Newton-Raphson Algorithm
- Initialize. Choose initial guess $\boldsymbol{\theta}^0$; set $i = 0$.
- Compute residual. Form $\mathbf{T}_{sb}(\boldsymbol{\theta}^i)$ via forward kinematics, then
- Check convergence. If $| \boldsymbol{\omega}b | < \epsilon\omega$ and $| \mathbf{v}_b | < \epsilon_v$, return $\boldsymbol{\theta}^i$.
- Update. Compute the body Jacobian $\mathbf{J}_b(\boldsymbol{\theta}^i)$ and set
- Increment $i$ and return to step 2.
A Worked Example: 2R Planar Arm
Following the Lynch-Park presentation, consider a planar 2R arm with both links of unit length, target $(x_d, y_d) = (0.366, 1.366)$ m corresponding to $(\theta_1^d, \theta_2^d) = (30^\circ, 90^\circ)$, and initial guess $\boldsymbol{\theta}^0 = (0^\circ, 30^\circ)$. With tolerances $\epsilon_\omega = 0.001$ rad and $\epsilon_v = 10^{-4}$ m, the body-Jacobian Newton-Raphson iterates as follows:
| $i$ | $(\theta_1, \theta_2)$ | $(x, y)$ | $|\boldsymbol{\omega}_b|$ | $|\mathbf{v}_b|$ |
|---|---|---|---|---|
| 0 | $(0.00^\circ, 30.00^\circ)$ | $(1.866, 0.500)$ | $1.571$ | $1.924$ |
| 1 | $(34.23^\circ, 79.18^\circ)$ | $(0.429, 1.480)$ | $0.115$ | $0.131$ |
| 2 | $(29.98^\circ, 90.22^\circ)$ | $(0.363, 1.364)$ | $0.004$ | $0.004$ |
| 3 | $(30.00^\circ, 90.00^\circ)$ | $(0.366, 1.366)$ | $0.000$ | $0.000$ |
The iteration converges within tolerance after three steps, exhibiting the characteristic quadratic local convergence of Newton’s method.
Damped Least Squares
The Newton-Raphson update relies on inverting the Jacobian. At — or even near — a singularity, $\mathbf{J}(\boldsymbol{\theta})$ loses rank, and $\mathbf{J}^\dagger$ either does not exist or has enormous norm. The consequence is that even a tiny Cartesian residual can demand huge joint updates, the iteration over-corrects, and convergence fails. Joint-velocity solutions also suffer in physical execution, since the commanded joint speeds can exceed actuator limits.
The cure is to regularize the inversion. The damped least squares method (also called the Levenberg-Marquardt method in the optimization community) replaces the unregularized pseudoinverse with
\[\mathbf{J}^\dagger_{\mathrm{DLS}} = \mathbf{J}^\top \left( \mathbf{J} \mathbf{J}^\top + \lambda^2 \mathbf{I} \right)^{-1},\]for a damping factor $\lambda > 0$. The resulting joint update
\[\Delta \boldsymbol{\theta} = \mathbf{J}^\top \left( \mathbf{J} \mathbf{J}^\top + \lambda^2 \mathbf{I} \right)^{-1} \mathbf{e},\]where $\mathbf{e}$ is the Cartesian residual, is the unique minimizer of the augmented cost
\[\Phi(\Delta \boldsymbol{\theta}) = \| \mathbf{J}\, \Delta \boldsymbol{\theta} - \mathbf{e} \|^2 + \lambda^2 \| \Delta \boldsymbol{\theta} \|^2.\]The damping term $\lambda^2 | \Delta \boldsymbol{\theta} |^2$ penalizes large joint changes, trading off exact tracking of $\mathbf{e}$ against the magnitude of $\Delta \boldsymbol{\theta}$.
For singular value $\sigma_i$ of $\mathbf{J}$, the damped pseudoinverse has effective singular value $\sigma_i / (\sigma_i^2 + \lambda^2)$, in contrast to $1 / \sigma_i$ for the unregularized inverse. Hence: - When $\sigma_i \gg \lambda$, the damped inverse is essentially the ordinary inverse: $\sigma_i / (\sigma_i^2 + \lambda^2) \approx 1/\sigma_i$. - When $\sigma_i \ll \lambda$, contributions in that direction are suppressed: $\sigma_i / (\sigma_i^2 + \lambda^2) \approx \sigma_i / \lambda^2 \to 0$. The damping factor $\lambda$ is therefore a soft threshold below which singular directions are ignored.
A practical refinement is to use a state-dependent damping factor — small far from singularities, large near them — for example $\lambda^2 = \lambda_0^2 (1 - \sigma_{\min}(\mathbf{J}) / \sigma_0)^2$ when $\sigma_{\min}(\mathbf{J}) < \sigma_0$, and $\lambda = 0$ otherwise. This way one pays no precision cost in well-conditioned regions and gains numerical stability near singularities.
Pseudocode for Jacobian Iteration
The following skeleton consolidates the ideas above into a single iterative IK routine:
1
2
3
4
5
6
7
8
9
10
11
12
function IK(T_sd, theta0, max_iter, eps_w, eps_v, lambda):
theta = theta0
for i = 1, ..., max_iter:
T_sb = forward_kinematics(theta)
V_b = log_SE3(inverse(T_sb) @ T_sd)
if norm(V_b[0:3]) < eps_w and norm(V_b[3:6]) < eps_v:
return theta, success=True
J_b = body_jacobian(theta)
# Damped least squares update
dtheta = J_b.T @ inv(J_b @ J_b.T + lambda**2 * I_6) @ V_b
theta = theta + dtheta
return theta, success=False
In the limit $\lambda \to 0$ this reduces to the plain pseudoinverse update $\Delta \boldsymbol{\theta} = \mathbf{J}_b^\dagger \boldsymbol{\mathcal{V}}_b$. In an industrial implementation one would also clamp $\boldsymbol{\theta}$ to joint limits at each step and possibly scale $\boldsymbol{\mathcal{V}}_b$ if its norm is too large, to avoid taking destabilizingly long steps.
Practical Considerations
Initial Guesses
All iterative IK methods inherit the basin-of-attraction issue. The most reliable strategy in a control loop is to seed each call with the previous joint vector: between successive control cycles the desired pose changes by a small amount, so the previous solution is close to the current one. For an open-loop one-shot IK query, one can:
- Try multiple random initial guesses, accept the first that converges and is within joint limits.
- If a closed-form approximate solution exists (e.g. the nominal Pieper solution for an arm whose true axes only slightly deviate from intersecting), use it as the initial guess and let numerical refinement absorb the calibration errors.
Joint Limits and Self-Collisions
Industrial joints have hard physical bounds, and a kinematically valid joint vector can still be illegal because it violates a joint-limit constraint or causes a self-collision. After computing a candidate $\boldsymbol{\theta}$ one must:
- Verify $\theta_i^{\min} \le \theta_i \le \theta_i^{\max}$ for every joint.
- Optionally, perform forward-kinematics collision-check against the robot’s own geometry and the environment.
If a configuration is rejected, one can attempt to flip the wrist or swap elbow-up for elbow-down (or, in the numerical setting, restart from a different initial guess) and re-check.
Redundancy Resolution
When $n > 6$, the pseudoinverse update $\Delta \boldsymbol{\theta} = \mathbf{J}^\dagger \mathbf{e}$ already selects a particular solution: the one of minimum $| \Delta \boldsymbol{\theta} |$. To exploit the extra degrees of freedom for a secondary objective $h(\boldsymbol{\theta})$ — for example, distance from joint limits, manipulability, or distance from obstacles — one augments the update by a null-space projection
\[\Delta \boldsymbol{\theta} = \mathbf{J}^\dagger \mathbf{e} + (\mathbf{I} - \mathbf{J}^\dagger \mathbf{J})\, \mathbf{z},\]where $\mathbf{z}$ is any $n$-vector — typically $-\nabla h(\boldsymbol{\theta})$ to descend $h$ — and the projector $\mathbf{I} - \mathbf{J}^\dagger \mathbf{J}$ guarantees that the null-space motion does not perturb the end-effector pose to first order.
A weighted-pseudoinverse variant uses the configuration-dependent mass matrix $\mathbf{M}(\boldsymbol{\theta})$,
\[\dot{\boldsymbol{\theta}} = \mathbf{M}^{-1} \mathbf{J}^\top (\mathbf{J} \mathbf{M}^{-1} \mathbf{J}^\top)^{-1} \boldsymbol{\mathcal{V}}_d,\]minimizing kinetic energy $\frac{1}{2} \dot{\boldsymbol{\theta}}^\top \mathbf{M} \dot{\boldsymbol{\theta}}$ subject to $\mathbf{J} \dot{\boldsymbol{\theta}} = \boldsymbol{\mathcal{V}}_d$.
Closed vs. Open-Loop Trajectories
Numerical IK applied independently at each sample of a desired Cartesian trajectory does not guarantee that the resulting joint trajectory is closed even when the Cartesian trajectory is closed. If the desired $\mathbf{T}{sd}(t)$ satisfies $\mathbf{T}{sd}(0) = \mathbf{T}_{sd}(t_f)$, the iteratively recovered $\boldsymbol{\theta}(t)$ may have $\boldsymbol{\theta}(0) \neq \boldsymbol{\theta}(t_f)$ because the manipulator may have drifted to a different basin of attraction along the way. When joint-space closure is required, additional cyclicity conditions must be enforced.
Summary
We close with a side-by-side comparison of the two main solution paradigms.
| Closed-form | Numerical | |
|---|---|---|
| Returns all solutions | Yes | No (returns one, biased by initial guess) |
| Speed | Constant time | Iterative; depends on initial guess |
| Generality | Restricted to special geometries (e.g. Pieper) | Any differentiable forward kinematics |
| Behavior at singularities | Often well-defined (degenerate but explicit) | Requires damping (DLS) |
| Robustness to calibration error | Sensitive (assumes ideal geometry) | Naturally absorbs small perturbations |
The state of the art in industrial practice is hybrid: use a closed-form solution (when available) to generate candidate seeds for a damped least-squares iteration that polishes them against the true, calibrated kinematics. The closed form provides global coverage of solution branches; the numerical refinement provides accuracy and graceful behavior in non-ideal regions of the workspace.
Reference
- John J. Craig, Introduction to Robotics: Mechanics and Control, 3rd ed., Pearson Prentice Hall, 2005. (Chapter 4: Inverse Manipulator Kinematics)
- Kevin M. Lynch and Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017. (Chapter 6: Inverse Kinematics)
- Mark W. Spong, Seth Hutchinson, and M. Vidyasagar, Robot Modeling and Control, Wiley, 2005. (Chapter 3: Forward and Inverse Kinematics)
- D. L. Pieper, The Kinematics of Manipulators under Computer Control, PhD thesis, Stanford University, 1968.
- A. A. Maciejewski and C. A. Klein, “Numerical Filtering for the Operation of Robotic Manipulators through Kinematically Singular Configurations,” Journal of Robotic Systems, vol. 5, no. 6, pp. 527-552, 1988.
- C. W. Wampler, “Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods,” IEEE Transactions on Systems, Man, and Cybernetics, vol. 16, no. 1, pp. 93-101, 1986.
Leave a comment