Let \( \mathbf{q} \in \mathbb{R}^n \) denote the joint variable vector and let \( \mathbf{x} \in \mathbb{R}^m \) denote a task-space representation of the end-effector (for this lesson, mainly position in 2D or 3D and, for decoupled problems, orientation given from the previous lesson). The forward kinematics map is

\[ \mathbf{x} = f(\mathbf{q}), \quad f : \mathbb{R}^n \to \mathbb{R}^m. \]

The inverse kinematics problem is: given a desired task-space value \( \mathbf{x}_d \), find all joint vectors \( \mathbf{q} \) such that

\[ f(\mathbf{q}) = \mathbf{x}_d. \]

In general, the set of solutions \( f^{-1}(\mathbf{x}_d) = \{ \mathbf{q} \mid f(\mathbf{q}) = \mathbf{x}_d \} \) can be empty, finite, or infinite. Geometric IK attacks this problem by directly analyzing the geometry of the manipulator in the workspace, often using:

• Circle and sphere intersections (for link lengths and distances).
• The law of cosines and law of sines in triangles formed by links and the target point.
• Planar projections of 3D problems into appropriately chosen planes.
• Decoupling strategies, such as solving first for a wrist center position and then for orientation (spherical wrist decoupling).

Geometric IK is most effective when the manipulator has a “structured” geometry (e.g., planar 2R/3R arms, anthropomorphic 6R arms with a spherical wrist). Compared to purely algebraic symbolic IK, geometric methods often yield more insight into configuration multiplicity, workspace boundaries, and singular configurations.

flowchart TD
  S["Desired pose (x_d, R_d)"] --> D["If wrist is spherical: compute wrist center position"]
  D --> P["Reduce to position IK for first 2 or 3 joints"]
  P --> T["Construct triangles / circles in appropriate plane"]
  T --> Q["Apply law of cosines and atan2 identities"]
  Q --> B["Enumerate branches (elbow up/down, shoulder options)"]
  B --> V["Check joint limits and workspace feasibility"]
  V --> O["Return all feasible IK solutions"]
        

Consider a planar 2R arm with link lengths \( \ell_1 > 0 \) and \( \ell_2 > 0 \), joint angles \( q_1 \) and \( q_2 \) (measured in the plane), and base frame at the shoulder joint. Its forward kinematics for the end-effector position \( (x,y) \) are

\[ x = \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2), \quad y = \ell_1 \sin q_1 + \ell_2 \sin(q_1 + q_2). \]

Let the desired end-effector position be \( \mathbf{p}_d = (x_d,y_d) \). Define the radial distance from the origin:

\[ d = \sqrt{x_d^2 + y_d^2}. \]

Geometrically, we have a triangle with sides \( \ell_1 \), \( \ell_2 \), and \( d \). The interior angle at the elbow (between links \( \ell_1 \) and \( \ell_2 \)) is related to the joint angle \( q_2 \) depending on the sign convention. A standard convention is that the elbow interior angle equals \( \pi - q_2 \). Applying the law of cosines to the side opposite to \( \pi - q_2 \) (the side of length \( d \)) gives

\[ d^2 = \ell_1^2 + \ell_2^2 - 2 \ell_1 \ell_2 \cos(\pi - q_2). \]

Using the identity \( \cos(\pi - q_2) = - \cos q_2 \), we obtain

\[ d^2 = \ell_1^2 + \ell_2^2 + 2 \ell_1 \ell_2 \cos q_2 \quad \Longrightarrow \quad \cos q_2 = \frac{d^2 - \ell_1^2 - \ell_2^2}{2 \ell_1 \ell_2}. \]

Denote \( c_2 = \cos q_2 \). Because \( \sin^2 q_2 + \cos^2 q_2 = 1 \), we have \( \sin q_2 = \pm \sqrt{1 - c_2^2} \). Thus we obtain the two elbow configurations:

\[ q_2^{(\pm)} = \arctan2\!\big(\pm \sqrt{1 - c_2^2},\; c_2\big). \]

The sign choice corresponds to “elbow-up” vs. “elbow-down” configurations (the exact mapping depends on the coordinate system; but they are geometrically distinct).

Next, to recover \( q_1 \), let \( \varphi = \arctan2(y_d, x_d) \) be the polar angle of the target point from the base. Consider the projection of link \( \ell_2 \) onto the line from the base to the end-effector. With \( q_2 \) fixed, the coordinates of the elbow in a frame aligned with \( \mathbf{p}_d \) give the relation

\[ \begin{aligned} k_1 &= \ell_1 + \ell_2 \cos q_2, \\ k_2 &= \ell_2 \sin q_2. \end{aligned} \]

The angle between the radial line and the first link is \( \psi = \arctan2(k_2, k_1) \). Geometrically, the shoulder angle is

\[ q_1 = \varphi - \psi \;=\; \arctan2(y_d, x_d) - \arctan2\big(\ell_2 \sin q_2,\; \ell_1 + \ell_2 \cos q_2\big). \]

Combining these formulas, we obtain up to two IK solutions: \( (q_1^{(+)}, q_2^{(+)}) \) and \( (q_1^{(-)}, q_2^{(-)}) \), corresponding to the two elbow geometries.

Uniqueness at the workspace boundary. When \( |c_2| = 1 \), we have \( \sin q_2 = 0 \) and the arm is fully stretched or folded. In that case, \( q_2 \) is effectively unique and the triangle degenerates; the arm loses one geometric branch. These boundary configurations are also kinematic singularities for the 2R arm (to be revisited in the singularities chapter).

The distances \(\ell_1\), \(\ell_2\), and \(d\) must form a valid triangle. By the triangle inequality, the necessary and sufficient condition is

\[ |\ell_1 - \ell_2| \le d \le \ell_1 + \ell_2. \]

In terms of the workspace, this means the reachable set of the 2R arm is an annulus in the plane: all points with distance from the base between the inner radius \( |\ell_1 - \ell_2| \) and the outer radius \( \ell_1 + \ell_2 \). Points:

• Inside the inner circle, where \( d < |\ell_1 - \ell_2| \), cannot be reached because the links are “too long” to fold sufficiently.
• Outside the outer circle, where \( d > \ell_1 + \ell_2 \), cannot be reached because the links are “too short” even when fully stretched.
• On the boundaries \( d = |\ell_1 - \ell_2| \) or \( d = \ell_1 + \ell_2 \), the arm is fully folded or stretched; IK has a unique solution (up to any symmetries in the joint limits).
• Strictly inside the annulus, there are exactly two geometric solutions (assuming joint limits do not remove one).

flowchart TD
  S["Compute d = sqrt(x_d^2 + y_d^2)"] --> C1["Check: d smaller than |l1 - l2|?"]
  C1 -->|yes| R1["Unreachable: \ninside inner workspace hole"]
  C1 -->|no| C2["Check: \nd greater than l1 + l2?"]
  C2 -->|yes| R2["Unreachable: \noutside workspace boundary"]
  C2 -->|no| R3["Reachable: \none or two geometric IK solutions"]
        

This workspace picture is fundamental: many IK algorithms first determine whether a target is in the reachable set before attempting to compute angles. For more complex manipulators, similar geometric constraints appear but with more complicated shapes in 3D.

Many industrial 6R manipulators (e.g., anthropomorphic arms) are designed so that the last three joint axes intersect at a single point, forming a spherical wrist. Under this structure, the pose of the end-effector can be decoupled into:

1. Position of the wrist center.
2. Orientation of the hand (controlled by the wrist joints).

Let the desired end-effector pose be represented by a homogeneous transform \( {}^0\mathbf{T}_d \in \mathrm{SE}(3) \), with rotation part \( {}^0\mathbf{R}_d \) and position part \( {}^0\mathbf{p}_d \). Suppose the hand frame origin is at a distance \( d_6 \) along its \( z \)-axis from the wrist center. Then the desired wrist center position is

\[ {}^0\mathbf{p}_{\mathrm{wc}} = {}^0\mathbf{p}_d - d_6 \, {}^0\mathbf{R}_d \mathbf{\hat{z}}, \]

where \( \mathbf{\hat{z}} \) is the unit vector along the end-effector local \( z \)-axis expressed in that frame. This is a purely geometric relation: the hand is offset from the wrist center along a known direction by a known distance.

Once \( {}^0\mathbf{p}_{\mathrm{wc}} \) is known, the IK for the first three joints reduces to a position-only problem: find \((q_1, q_2, q_3)\) such that the origin of the wrist frame coincides with the wrist center. In many standard arms, joints 2 and 3 form an effectively planar 2R structure in some plane, so a variant of the previous 2R derivation applies with appropriate projection and effective link lengths.

After solving the position part, the remaining three joints (the spherical wrist) solve the orientation equation

\[ {}^3\mathbf{R}_6(q_4,q_5,q_6) \;=\; ({}^0\mathbf{R}_3(q_1,q_2,q_3))^\top {}^0\mathbf{R}_d, \]

where \( {}^0\mathbf{R}_3 \) is the rotation of the frame attached at the wrist center relative to the base. This orientation subproblem is treated analytically in the previous lesson and typically reduces to solving three coupled trigonometric equations for \( q_4, q_5, q_6 \) using geometric reasoning about Euler angle factorizations.

We now implement the geometric 2R IK in Python. We provide a pure-numeric implementation and briefly mention how robotics libraries can wrap this for larger systems.

import math
from typing import List, Tuple

def ik_2r(l1: float, l2: float,
          x_d: float, y_d: float,
          tol: float = 1e-9) -> List[Tuple[float, float]]:
    """
    Geometric IK for a planar 2R manipulator.

    Args:
        l1, l2 : link lengths (> 0)
        x_d, y_d : desired end-effector position in base frame
        tol : numerical tolerance on workspace checks

    Returns:
        List of (q1, q2) solutions in radians.
    Raises:
        ValueError if the target is unreachable.
    """
    d_sq = x_d * x_d + y_d * y_d
    d = math.sqrt(d_sq)

    # Workspace feasibility via triangle inequality
    if d > l1 + l2 + tol or d < abs(l1 - l2) - tol:
        raise ValueError("Target is outside the reachable workspace")

    # Clamp cosine to [-1, 1] to avoid numerical errors
    c2 = (d_sq - l1 * l1 - l2 * l2) / (2.0 * l1 * l2)
    c2 = max(-1.0, min(1.0, c2))

    # Two possible signs for s2 = sin(q2)
    disc = max(0.0, 1.0 - c2 * c2)
    s2_candidates = [math.sqrt(disc), -math.sqrt(disc)]

    solutions: List[Tuple[float, float]] = []

    for s2 in s2_candidates:
        q2 = math.atan2(s2, c2)

        k1 = l1 + l2 * c2
        k2 = l2 * s2

        # atan2 takes (y, x)
        phi = math.atan2(y_d, x_d)
        psi = math.atan2(k2, k1)
        q1 = phi - psi

        solutions.append((q1, q2))

    return solutions

if __name__ == "__main__":
    l1, l2 = 1.0, 0.6
    x_d, y_d = 1.0, 0.4

    sols = ik_2r(l1, l2, x_d, y_d)
    for i, (q1, q2) in enumerate(sols):
        print(f"Solution {i}: q1 = {q1:.3f} rad, q2 = {q2:.3f} rad")
      

In Python, robotics libraries such as roboticstoolbox-python can encapsulate these geometric computations and also provide numerical IK (ikine, damped least squares, etc.). For this lesson, we deliberately construct the geometric formulas by hand to expose the underlying structure.

In C++, geometric IK can be implemented using standard library functions and a linear algebra package such as Eigen. Dedicated robotics libraries (e.g., Orocos KDL, Pinocchio) build on similar geometric primitives while supporting arbitrary kinematic chains.

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

struct Solution2R {
    double q1;
    double q2;
};

std::vector<Solution2R> ik2R(double l1, double l2,
                             double x_d, double y_d,
                             double tol = 1e-9)
{
    std::vector<Solution2R> sols;

    double d_sq = x_d * x_d + y_d * y_d;
    double d = std::sqrt(d_sq);

    if (d > l1 + l2 + tol || d < std::fabs(l1 - l2) - tol) {
        // unreachable
        return sols;
    }

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

    double disc = std::max(0.0, 1.0 - c2 * c2);
    double s2_candidates[2] = { std::sqrt(disc), -std::sqrt(disc) };

    for (double s2 : s2_candidates) {
        double q2 = std::atan2(s2, c2);

        double k1 = l1 + l2 * c2;
        double k2 = l2 * s2;

        double phi = std::atan2(y_d, x_d);
        double psi = std::atan2(k2, k1);
        double q1 = phi - psi;

        sols.push_back({q1, q2});
    }
    return sols;
}

int main()
{
    double l1 = 1.0, l2 = 0.6;
    double x_d = 1.0, y_d = 0.4;

    auto sols = ik2R(l1, l2, x_d, y_d);
    if (sols.empty()) {
        std::cout << "Target unreachable\n";
    } else {
        for (std::size_t i = 0; i < sols.size(); ++i) {
            std::cout << "Solution " << i
                      << ": q1 = " << sols[i].q1
                      << ", q2 = " << sols[i].q2 << "\n";
        }
    }
    return 0;
}
      

For more complex arms, C++ libraries like Orocos KDL offer generalized solvers that embed this kind of geometric reasoning for specific joint configurations (e.g. anthropomorphic arms with spherical wrists).

Java does not have a standard robotics library, but a 2R IK solver can be implemented using java.lang.Math. Linear algebra packages like EJML or Apache Commons Math can be useful for higher-DOF robots.

public class IK2R {

    public static class Solution {
        public final double q1;
        public final double q2;
        public Solution(double q1, double q2) {
            this.q1 = q1;
            this.q2 = q2;
        }
    }

    public static java.util.List<Solution> solve(double l1, double l2,
                                                   double x_d, double y_d,
                                                   double tol) {
        java.util.List<Solution> sols = new java.util.ArrayList<>();

        double d_sq = x_d * x_d + y_d * y_d;
        double d = Math.sqrt(d_sq);

        if (d > l1 + l2 + tol || d < Math.abs(l1 - l2) - tol) {
            return sols; // unreachable
        }

        double c2 = (d_sq - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
        c2 = Math.max(-1.0, Math.min(1.0, c2));

        double disc = Math.max(0.0, 1.0 - c2 * c2);
        double[] s2Candidates = { Math.sqrt(disc), -Math.sqrt(disc) };

        double phi = Math.atan2(y_d, x_d);

        for (double s2 : s2Candidates) {
            double q2 = Math.atan2(s2, c2);
            double k1 = l1 + l2 * c2;
            double k2 = l2 * s2;
            double psi = Math.atan2(k2, k1);

            double q1 = phi - psi;
            sols.add(new Solution(q1, q2));
        }
        return sols;
    }

    public static void main(String[] args) {
        double l1 = 1.0, l2 = 0.6;
        double x_d = 1.0, y_d = 0.4;

        java.util.List<Solution> sols = solve(l1, l2, x_d, y_d, 1e-9);
        if (sols.isEmpty()) {
            System.out.println("Target unreachable");
        } else {
            for (int i = 0; i < sols.size(); ++i) {
                Solution s = sols.get(i);
                System.out.printf("Solution %d: q1 = %.3f, q2 = %.3f%n",
                                  i, s.q1, s.q2);
            }
        }
    }
}
      

The structure mirrors the mathematics: workspace check via triangle inequality, then law of cosines for \( q_2 \) and angle subtraction for \( q_1 \).

MATLAB is widely used in control engineering and robotics. We implement the 2R IK as a function that can also be called from a Simulink MATLAB Function block.

function sols = ik2R(l1, l2, x_d, y_d, tol)
%IK2R Geometric inverse kinematics for planar 2R manipulator.
%   sols is a 2xN matrix containing [q1; q2] solutions (radians).

if nargin < 5
    tol = 1e-9;
end

d_sq = x_d.^2 + y_d.^2;
d = sqrt(d_sq);

if (d > l1 + l2 + tol) || (d < abs(l1 - l2) - tol)
    sols = []; % unreachable
    return;
end

c2 = (d_sq - l1^2 - l2^2) / (2 * l1 * l2);
c2 = max(-1.0, min(1.0, c2));

disc = max(0.0, 1.0 - c2^2);
s2_candidates = [sqrt(disc), -sqrt(disc)];

phi = atan2(y_d, x_d);
sols = zeros(2, 0);

for s2 = s2_candidates
    q2 = atan2(s2, c2);
    k1 = l1 + l2 * c2;
    k2 = l2 * s2;
    psi = atan2(k2, k1);
    q1 = phi - psi;

    sols(:, end+1) = [q1; q2];
end
end
      

In Simulink, a MATLAB Function block can call ik2R to compute joint references from a desired planar end-effector position. For more complex robots, the Robotics System Toolbox provides rigidBodyTree and inverseKinematics objects which internally may combine geometric and numerical approaches.

Wolfram Mathematica can derive IK formulas symbolically and evaluate them numerically. Below we encode the 2R kinematics and let Solve and Reduce find the solutions:

(* Link lengths and desired position *)
l1 = 1.0; l2 = 0.6;
xd = 1.0; yd = 0.4;

(* Unknown joint variables *)
Clear[q1, q2];

eqns = {
  xd == l1 Cos[q1] + l2 Cos[q1 + q2],
  yd == l1 Sin[q1] + l2 Sin[q1 + q2]
};

(* Symbolic solution (may give multiple branches) *)
solSym = FullSimplify[
  Solve[eqns, {q1, q2}, Reals]
];

(* Numeric solutions directly *)
solNum = N[
  Solve[eqns /. {xd -> xd, yd -> yd}, {q1, q2}, Reals]
];

solNum
      

Mathematica can also be used to verify the analytic formulas derived in Section 2 by substituting the expressions for \( q_1 \) and \( q_2 \) and simplifying the resulting equations back to identities.

In this lesson we developed geometric inverse kinematics techniques, starting from the planar 2R arm and extending to the position part of 6R arms with spherical wrists. We derived closed-form IK formulas using triangle geometry and the law of cosines, analyzed the reachable workspace of a 2R arm as an annulus, and interpreted the multiplicity of IK solutions as arising from geometric reflections (elbow-up vs. elbow-down). We also discussed geometric decoupling for spherical-wrist manipulators and provided implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These geometric techniques form the basis for more general analytical IK methods and provide intuitive understanding that complements numerical IK algorithms introduced in the next lesson.