For a manipulator with joint configuration vector \( \boldsymbol{\theta} \in \mathbb{R}^n \) and end-effector pose \( \mathbf{T}(\boldsymbol{\theta}) \in SE(3) \), the inverse kinematics problem asks for all joint vectors that satisfy

\[ \mathbf{T}(\boldsymbol{\theta}) = \begin{bmatrix} \mathbf{R}(\boldsymbol{\theta}) & \mathbf{p}(\boldsymbol{\theta}) \\ \mathbf{0}^T & 1 \end{bmatrix} = \mathbf{T}_d = \begin{bmatrix} \mathbf{R}_d & \mathbf{p}_d \\ \mathbf{0}^T & 1 \end{bmatrix}. \]

An analytical IK solution expresses each joint angle as an explicit algebraic function of the desired pose: \( \theta_i = f_i(\mathbf{R}_d, \mathbf{p}_d) \), typically using trigonometric and algebraic manipulations. Closed-form IK is feasible only for special structures (e.g., planar arms, spherical wrists), but when it exists, it is:

• Exact (up to floating-point error),
• Fast (no iteration required),
• Explicit about solution multiplicity and singularities.

For many industrial 6R arms, the kinematic design admits decoupling of position and orientation. Let joint indices \(1,2,3\) form the positioning arm and \(4,5,6\) form a spherical wrist whose axes intersect at a single point. Then the IK problem is solved in two steps:

1. Compute the wrist center position \( \mathbf{p}_w \) from \( \mathbf{T}_d \).
2. Solve a 3R positioning problem for joints \(1,2,3\), then a 3R orientation problem for joints \(4,5,6\).

flowchart TD
  A["Desired pose T_d (R_d, p_d)"] --> B["Compute wrist center p_w = p_d - R_d * o_6"]
  B --> C["Solve 3R position IK for joints 1..3"]
  C --> D["Compute R_3_6 = R_3_0^T * R_d"]
  D --> E["Solve 3R orientation IK for joints 4..6"]
  E --> F["Assemble all joint solutions theta = [theta1..theta6] with branches"]
        

Before tackling 6R arms, we first derive a complete analytical IK solution for the planar 2R arm, which appears as a substructure in many manipulators and illustrates the trigonometric techniques used in more complex geometries.

Consider a planar 2R manipulator with link lengths \( l_1, l_2 > 0 \) and joint angles \( \theta_1, \theta_2 \) measured from the standard \(x\)-axis. The end-effector position is

\[ \mathbf{p}(\theta_1,\theta_2) = \begin{bmatrix} x \\ y \end{bmatrix} = \begin{bmatrix} l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2) \\ l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2) \end{bmatrix}. \]

Given a desired planar target \( \mathbf{p}_d = [x_d \; y_d]^T \), IK asks for all \( (\theta_1,\theta_2) \) such that \( \mathbf{p}(\theta_1,\theta_2) = \mathbf{p}_d \). Define the distance from the base to the target

\[ r^2 = x_d^2 + y_d^2. \]

From elementary geometry, the arm can reach \( \mathbf{p}_d \) iff the target lies inside the annulus defined by link lengths:

\[ (l_1 - l_2)^2 \le r^2 \le (l_1 + l_2)^2. \]

To derive an explicit solution, we apply the law of cosines to the triangle formed by \( l_1, l_2 \), and \( r \). The interior angle opposite side \( l_2 \) is \( \pi - \theta_2 \), so

\[ r^2 = l_1^2 + l_2^2 - 2 l_1 l_2 \cos(\pi - \theta_2) = l_1^2 + l_2^2 + 2l_1 l_2 \cos\theta_2. \]

Solving for \( \cos\theta_2 \) yields

\[ \cos\theta_2 = \frac{r^2 - l_1^2 - l_2^2}{2 l_1 l_2}. \]

Real solutions require \( \left|\cos\theta_2\right| \le 1 \), which is equivalent to the reachability condition above. There are generally two solutions for \( \theta_2 \):

\[ \theta_2^{(\pm)} = \operatorname{atan2}\!\Big(\pm\sqrt{1-\cos^2\theta_2},\, \cos\theta_2\Big), \]

where the \(+\) branch corresponds to the elbow-up configuration and the \(-\) branch to the elbow-down configuration.

To solve for \( \theta_1 \), consider the projection of the second link onto the line from the base to the target. Define the auxiliary angles

\[ \phi = \operatorname{atan2}(y_d, x_d), \qquad \psi^{(\pm)} = \operatorname{atan2}\!\Big( l_2 \sin\theta_2^{(\pm)},\, l_1 + l_2\cos\theta_2^{(\pm)} \Big). \]

Then the two corresponding solutions for the first joint angle are

\[ \theta_1^{(\pm)} = \phi - \psi^{(\pm)}. \]

Altogether, the analytical IK for the planar 2R arm yields up to two distinct solutions \( (\theta_1^{(+)},\theta_2^{(+)}) \) and \( (\theta_1^{(-)},\theta_2^{(-)}) \), which merge into a single solution when the arm is fully stretched or fully folded (singular configurations).

Many industrial arms use three proximal revolute joints to position a wrist center \( \mathbf{p}_w \in \mathbb{R}^3 \) in space. Common examples include RRR configurations with either all three axes mutually orthogonal, or two axes parallel and one orthogonal.

Suppose the Denavit–Hartenberg (DH) parameters for joints \(1,2,3\) are known and the forward kinematics from base frame \( \{0\} \) to frame \( \{3\} \) is

\[ \mathbf{T}_{0}^{3}(\theta_1,\theta_2,\theta_3) = \begin{bmatrix} \mathbf{R}_{0}^{3}(\theta_1,\theta_2,\theta_3) & \mathbf{p}_{0}^{3}(\theta_1,\theta_2,\theta_3) \\ \mathbf{0}^T & 1 \end{bmatrix}. \]

For decoupled 6R arms, the IK for the first three joints is solved from \( \mathbf{p}_{0}^{3}(\theta_1,\theta_2,\theta_3) = \mathbf{p}_w \), where \( \mathbf{p}_w \) is computed from the desired end-effector pose (see Section 4). For many designs, \( \mathbf{p}_{0}^{3} \) can be expressed in a form that reduces to a 2R planar problem in some projection.

As a canonical example, consider a positioning structure where joint 1 rotates about the global \(z\)-axis, and joints 2 and 3 actuate an arm in a plane that is vertical when viewed from the side. Let

\[ \mathbf{p}_w = \begin{bmatrix} x_w \\ y_w \\ z_w \end{bmatrix}, \qquad \rho = \sqrt{x_w^2 + y_w^2}. \]

Then \( \theta_1 \) is determined by projecting onto the \(x\)-\(y\) plane:

\[ \theta_1 = \operatorname{atan2}(y_w, x_w), \]

while joints \( \theta_2, \theta_3 \) solve a planar 2R problem in the plane defined by \( \rho \) and \( z_w \). That is, with effective link lengths \( L_2, L_3 \) and the target point \( (\rho, z_w) \), we reuse the 2R formulas from Section 2:

\[ \cos\theta_3 = \frac{\rho^2 + z_w^2 - L_2^2 - L_3^2}{2 L_2 L_3}, \qquad \theta_2 = \operatorname{atan2}(z_w, \rho) - \operatorname{atan2}\!\Big( L_3 \sin\theta_3,\, L_2 + L_3 \cos\theta_3 \Big), \]

with the same \( \pm \) branching as in the planar 2R case. This illustrates a recurring pattern: spatial IK often reduces to planar subproblems when the mechanism exhibits structural symmetries.

A spherical wrist means that the last three joint axes intersect at a single point (the wrist center) and are mutually orthogonal (or otherwise arranged so that they span all 3D rotations). For such manipulators, we define:

• \( \mathbf{o}_6 \): vector from the wrist center to the tool frame origin, expressed in frame \(\{6\} \),
• \( \mathbf{R}_d \), \( \mathbf{p}_d \): desired end-effector rotation and position.

The wrist center in base coordinates is

\[ \mathbf{p}_w = \mathbf{p}_d - \mathbf{R}_d \mathbf{o}_6. \]

Step 1 (Position IK). Use \( \mathbf{p}_w \) to solve for joints \( \theta_1, \theta_2, \theta_3 \) via the 3R positioning IK from Section 3, obtaining a finite set of candidate triplets \( (\theta_1^{(k)},\theta_2^{(k)},\theta_3^{(k)}) \).

Step 2 (Orientation IK). For each candidate positioning solution, compute the resulting rotation \( \mathbf{R}_{0}^{3}(\theta_1^{(k)},\theta_2^{(k)},\theta_3^{(k)}) \) and define the wrist rotation:

\[ \mathbf{R}_{3}^{6}(\theta_4,\theta_5,\theta_6) = \mathbf{R}_{0}^{3}(\theta_1^{(k)},\theta_2^{(k)},\theta_3^{(k)})^T \mathbf{R}_d. \]

Because joints \(4,5,6\) form a 3R wrist, \( \mathbf{R}_{3}^{6} \) can be interpreted as the rotation parameterized by the three wrist angles. For example, in a common \(z\)-\(y\)-\(z\) wrist parameterization,

\[ \mathbf{R}_{3}^{6}(\theta_4,\theta_5,\theta_6) = \mathbf{R}_z(\theta_4)\mathbf{R}_y(\theta_5)\mathbf{R}_z(\theta_6), \]

where each element of \( \mathbf{R}_{3}^{6} \) is a known trigonometric function of the unknowns \( \theta_4, \theta_5, \theta_6 \). Matching this product with the numerically computed matrix \( \mathbf{R}_{3}^{6} \) yields scalar equations. A standard sequence of manipulations (analogous to Euler angle extraction) gives:

\[ \theta_5 = \operatorname{atan2}\!\Big( \sqrt{(r_{13})^2 + (r_{23})^2},\, r_{33} \Big), \]

\[ \theta_4 = \operatorname{atan2}(r_{23}, r_{13}), \qquad \theta_6 = \operatorname{atan2}(r_{32}, -r_{31}), \]

where \( r_{ij} \) are entries of the numerically known matrix \( \mathbf{R}_{3}^{6} \). Depending on the wrist parameterization and sign conventions, alternative extraction formulas may be used but follow the same pattern: one joint angle comes from an off-diagonal block magnitude, the others from atan2 of suitable entries.

Altogether, combining the branches from the 3R positioning IK (typically up to four solutions) with the branches from the wrist orientation IK (typically up to two solutions) yields up to \(8\) distinct analytical IK solutions for a generic 6R arm with a spherical wrist, ignoring joint limits and excluding singularities.

Analytical IK exposes the structure of multiple solutions as combinations of discrete choices. Even non-redundant arms (with 6 DOF) typically admit several branches:

• Shoulder configurations (e.g., left vs. right),
• Elbow configurations (e.g., up vs. down),
• Wrist configurations (e.g., flipped vs. non-flipped).

For the planar 2R arm, the two branches are distinguished by the sign of \( \sin\theta_2 \). For spatial arms, similar signs appear in square roots or atan2 operations. Conceptually, each sign choice corresponds to a discrete branch in a solution tree:

flowchart TD
  ROOT["Solve position IK (3R)"] --> S1["theta1 choice (e.g., left/right)"]
  S1 --> E1["theta2, theta3 elbow-up"]
  S1 --> E2["theta2, theta3 elbow-down"]
  E1 --> W1["wrist orientation \nbranch 1"]
  E1 --> W2["wrist orientation \nbranch 2"]
  E2 --> W3["wrist orientation \nbranch 1"]
  E2 --> W4["wrist orientation \nbranch 2"]
        

Singularities appear when these branches merge or become undefined. For instance, the planar 2R arm is singular when \( \sin\theta_2 = 0 \), i.e., \( \theta_2 = 0 \) or \( \theta_2 = \pi \), corresponding to collinear links. In spherical wrists, singularities occur when the wrist loses one rotational DOF, e.g., when \( \theta_5 = 0 \) and the \(z\)-axes of joints 4 and 6 align.

Analytic formulas therefore not only provide exact solutions but also furnish explicit algebraic tests for singular configurations that will be used later in workspace and manipulability analysis.

In Python, analytical IK for a planar 2R arm can be implemented directly using numpy. For more complex robots, one may rely on robotics libraries such as the Robotics Toolbox for Python or ikpy, but here we illustrate a minimal from-scratch implementation.

import numpy as np

def ik_2r(x, y, l1, l2, tol=1e-9):
    """
    Analytical IK for a planar 2R arm.
    Returns a list of solution tuples (theta1, theta2).
    Angles are in radians.
    """
    r2 = x**2 + y**2
    # Reachability check
    if r2 > (l1 + l2)**2 + tol or r2 < (l1 - l2)**2 - tol:
        return []  # no real solutions

    # cos(theta2)
    c2 = (r2 - l1**2 - l2**2) / (2.0 * l1 * l2)
    # Numerical clipping for robustness
    c2 = np.clip(c2, -1.0, 1.0)
    # two possible s2 signs
    s2_candidates = [np.sqrt(1.0 - c2**2), -np.sqrt(1.0 - c2**2)]

    phi = np.arctan2(y, x)
    solutions = []
    for s2 in s2_candidates:
        theta2 = np.arctan2(s2, c2)
        # atan2 term for projection of l2
        k1 = l1 + l2 * c2
        k2 = l2 * s2
        psi = np.arctan2(k2, k1)
        theta1 = phi - psi
        solutions.append((theta1, theta2))
    return solutions

if __name__ == "__main__":
    l1, l2 = 0.5, 0.4
    x_d, y_d = 0.6, 0.3
    sols = ik_2r(x_d, y_d, l1, l2)
    for i, (t1, t2) in enumerate(sols):
        print(f"Solution {i}: theta1 = {t1:.4f}, theta2 = {t2:.4f}")
      

This function explicitly returns both elbow-up and elbow-down configurations when they exist. Extending this approach to 3R positioners and 6R arms primarily involves additional trigonometric manipulation and careful handling of branch choices for each joint.

In C++, analytical IK is often implemented in the context of robotics middleware such as ROS, with libraries like Orocos KDL or MoveIt. Below is a simple C++ function for 2R IK using only the standard library:

#include <cmath>
#include <vector>
#include <utility>

struct AnglePair {
    double theta1;
    double theta2;
};

std::vector<AnglePair> ik2R(double x, double y, double l1, double l2, double tol = 1e-9) {
    std::vector<AnglePair> sols;
    double r2 = x*x + y*y;
    if (r2 > (l1 + l2)*(l1 + l2) + tol || r2 < (l1 - l2)*(l1 - l2) - tol) {
        return sols; // empty
    }

    double c2 = (r2 - l1*l1 - l2*l2) / (2.0 * l1 * l2);
    if (c2 > 1.0) c2 = 1.0;
    if (c2 < -1.0) c2 = -1.0;

    double s2_abs = std::sqrt(std::max(0.0, 1.0 - c2*c2));
    double phi = std::atan2(y, x);

    for (double s2 : {s2_abs, -s2_abs}) {
        double theta2 = std::atan2(s2, c2);
        double k1 = l1 + l2 * c2;
        double k2 = l2 * s2;
        double psi = std::atan2(k2, k1);
        double theta1 = phi - psi;
        sols.push_back({theta1, theta2});
    }
    return sols;
}
      

For 6R manipulators, analytic solvers are often implemented as specialized C++ classes (e.g., PUMA or KUKA kinematics plugins in MoveIt), where the DH parameters are hard-coded and the derived formulas are applied branch by branch.

8.1 Java Implementation

Java is less common in low-level robotics but is used in higher-level simulation frameworks and teaching tools. The following code shows a simple 2R IK method using java.lang.Math:

import java.util.ArrayList;
import java.util.List;

public class IK2R {
    public static class AnglePair {
        public double theta1;
        public double theta2;
        public AnglePair(double t1, double t2) {
            this.theta1 = t1;
            this.theta2 = t2;
        }
    }

    public static List<AnglePair> solve(double x, double y, double l1, double l2, double tol) {
        List<AnglePair> sols = new ArrayList<>();
        double r2 = x*x + y*y;
        if (r2 > (l1 + l2)*(l1 + l2) + tol || r2 < (l1 - l2)*(l1 - l2) - tol) {
            return sols;
        }
        double c2 = (r2 - l1*l1 - l2*l2) / (2.0 * l1 * l2);
        c2 = Math.max(-1.0, Math.min(1.0, c2));
        double s2Abs = Math.sqrt(Math.max(0.0, 1.0 - c2*c2));
        double phi = Math.atan2(y, x);

        double[] s2Options = new double[]{ s2Abs, -s2Abs };
        for (double s2 : s2Options) {
            double theta2 = Math.atan2(s2, c2);
            double k1 = l1 + l2 * c2;
            double k2 = l2 * s2;
            double psi = Math.atan2(k2, k1);
            double theta1 = phi - psi;
            sols.add(new AnglePair(theta1, theta2));
        }
        return sols;
    }
}
      

8.2 MATLAB Function and Simulink Usage

MATLAB provides a Robotics System Toolbox that includes general-purpose IK solvers, but analytical 2R IK is easily implemented directly and reused in Simulink via a MATLAB Function block:

function [theta1_solutions, theta2_solutions] = ik2R(x, y, l1, l2)
%IK2R Analytical IK for a planar 2R arm.
%   Returns two-column vectors of theta1 and theta2 (in radians).

r2 = x.^2 + y.^2;
theta1_solutions = [];
theta2_solutions = [];

if r2 > (l1 + l2)^2 || r2 < (l1 - l2)^2
    return; % no real solution
end

c2 = (r2 - l1^2 - l2^2) / (2*l1*l2);
c2 = max(-1,min(1,c2));
s2_abs = sqrt(max(0,1 - c2^2));

phi = atan2(y, x);

for s2 = [s2_abs, -s2_abs]
    theta2 = atan2(s2, c2);
    k1 = l1 + l2*c2;
    k2 = l2*s2;
    psi = atan2(k2, k1);
    theta1 = phi - psi;
    theta1_solutions(end+1,1) = theta1; %#ok<AGROW>
    theta2_solutions(end+1,1) = theta2; %#ok<AGROW>
end
      

In Simulink, this function can be wrapped in a MATLAB Function block that receives \(x, y\) and outputs the joint angles, allowing integration with dynamics blocks or trajectory generators covered in later chapters.

8.3 Wolfram Mathematica Implementation

Mathematica is well suited to symbolic derivation and verification of analytical IK formulas. The following code uses Solve to find symbolic solutions and then defines a numerical function:

(* Symbolic setup *)
Clear[x, y, l1, l2, theta1, theta2];
eqs = {
  x == l1 Cos[theta1] + l2 Cos[theta1 + theta2],
  y == l1 Sin[theta1] + l2 Sin[theta1 + theta2]
};

solSym = Solve[eqs, {theta1, theta2}, Reals];

(* Numeric wrapper *)
ik2R[xNum_, yNum_, l1Num_, l2Num_] := Module[
  {subs, sols},
  subs = {x -> xNum, y -> yNum, l1 -> l1Num, l2 -> l2Num};
  sols = ({theta1, theta2} /. solSym) /. subs;
  N[sols]
];

(* Example usage *)
ik2R[0.6, 0.3, 0.5, 0.4]
      

Symbolic computation can also be used to derive and simplify analytic IK for specific 6R manipulators, although the resulting expressions are often quite long and must be carefully implemented to ensure numerical stability.

In this lesson we developed analytical IK for common manipulator geometries. Starting from the planar 2R arm, we derived explicit trigonometric formulas for the joint angles and identified reachability and multiplicity of solutions. We extended the same geometric reasoning to 3R positioning structures and showed how spherical wrists enable a clean decoupling of position and orientation in 6R manipulators, yielding up to eight closed-form solutions in the generic case.

Implementation examples in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrated how these formulas can be translated into code suitable for simulation and control. The explicit expressions derived here will form the basis for the geometric IK techniques in the next lesson and will provide ground truth for evaluating numerical IK algorithms introduced later in the chapter.