Inverse kinematics: from a pose in space to six joint angles

11.1 The problem

Forward kinematics is a function: joint angles in, exactly one pose out. Inverse kinematics is a relation: a pose in, and no solutions, several, or infinitely many out.

No solutions happens when the pose is outside the workspace, or inside it but unreachable with the required orientation, or blocked by joint limits. Several solutions is the normal case — a six-axis arm of this type generally has eight distinct joint configurations that put the tool in exactly the same place with exactly the same orientation. Infinitely many happens at singularities.

The solver has to handle all three cases explicitly. An IK routine that returns one answer with no indication of whether it is valid will eventually drive the arm into itself.

11.2 Kinematic decoupling: the key insight

A spherical wrist is worth building because it splits the six-dimensional problem into two three-dimensional ones that can be solved independently.

The axes of joints 4, 5, and 6 all intersect at a single point, the wrist centre. Rotating any of those three joints rotates the tool about that point but does not move the point. Therefore the position of the wrist centre depends only on joints 1, 2, and 3.

So: given a desired tool pose, first compute where the wrist centre must be. That depends only on the desired position and orientation, both of which you know. Then solve joints 1, 2, and 3 to place the wrist centre there — a pure positioning problem in three unknowns with a geometric solution. Then, knowing joints 1–3, compute what rotation the wrist must supply to make up the difference between what the arm gives you and what you asked for, and solve joints 4, 5, and 6 for that rotation — a pure orientation problem in three unknowns.

Two three-DOF problems instead of one six-DOF problem, both with closed-form solutions — which is why almost every industrial six-axis arm has a spherical wrist.

11.3 Step one: locate the wrist centre

The wrist centre sits at a fixed offset from the flange, expressed in the flange's own frame. For the PAROL6 DH table above, I computed that offset numerically and it comes out as a constant, as it must:

$$\begin{aligned} p_{wc} &= p_{\text{flange}} + R_{\text{flange}} \cdot \begin{bmatrix} a_7 & 0 & -a_6 \end{bmatrix}^{\mathsf{T}} \\ &= p_{\text{flange}} + R_{\text{flange}} \cdot \begin{bmatrix} 45.25 & 0 & -62.80 \end{bmatrix}^{\mathsf{T}} \end{aligned}$$

I verified this at many arbitrary joint configurations and the offset vector expressed in frame 6 is exactly (45.25, 0, −62.80) mm every time.

If you are targeting a TCP rather than a flange, first convert: $T_{\text{base}}^{\text{flange}} = T_{\text{base}}^{\text{tool}} \cdot \big(T_{\text{flange}}^{\text{tool}}\big)^{-1}$, then apply the above.

def wrist_centre(R, p):
    return p + R @ np.array([a7, 0.0, -a6])

That line is the whole of the decoupling step, and everything downstream depends on it.

11.4 Step two: joints 1, 2, and 3

Now you have a point, and you need the first three joints to put the wrist centre there.

Joint 1. There is a subtlety here that is easy to get wrong, and I got it wrong on my first pass through this derivation before the numerical check caught it. The $a_2 = 23.42$ mm offset at the shoulder looks like it should be a lateral offset — the kind that shifts the arm's plane sideways off the base axis and forces a correction term into $\theta_1$. It is not. Working through the DH table, the origin of frame 1 sits at $(a_2\cos\theta_1,\; a_2\sin\theta_1,\; a_1)$, which is displaced radially outward in the direction the arm is already pointing, not sideways. The arm's plane of motion therefore still contains the base's vertical axis.

Which means joint 1 is simply:

$$\theta_1 = \operatorname{atan2}(y_{wc},\, x_{wc})$$

with no correction term at all. The second solution is $\theta_1 + \pi$, the "shoulder flipped" configuration in which the arm reaches back over its own base. On the PAROL6, joint 1 is limited to ±123.05°, so many flipped solutions fall outside the joint limits and get discarded — which is normal and expected.

It is a good argument for checking numerically rather than trusting a derivation that looks right: the wrong version produces plausible angles that do not reach the target.

Joints 2 and 3 are then a planar two-link problem in the vertical plane containing the arm. Convert the wrist centre into that plane's coordinates:

$$\begin{aligned} u &= \sqrt{x_{wc}^2 + y_{wc}^2} - a_2 &&\text{horizontal distance from the shoulder axis} \\ v &= a_1 - z_{wc} &&\text{note the sign — the frame-1 y axis points downward} \end{aligned}$$

For the flipped-shoulder branch, use $u = -\sqrt{x_{wc}^2 + y_{wc}^2} - a_2$ instead.

The two links are the upper arm, $a_3 = 180.00$ mm, and the forearm. The forearm is not a straight link — there is a 43.50 mm offset at the elbow and a 176.35 mm length beyond it — so its effective length is the hypotenuse, and it carries a fixed built-in bend:

$$\begin{aligned} L &= \sqrt{a_4^2 + a_5^2} = \sqrt{43.50^2 + 176.35^2} = 181.636 \text{ mm} \\ \varphi &= \operatorname{atan2}(a_4, a_5) = \operatorname{atan2}(43.50, 176.35) = 0.24184 \text{ rad} = 13.856° \end{aligned}$$

Now the standard two-link solution. Let $D^2 = u^2 + v^2$. The law of cosines gives the included angle $B$ between the two links:

$$\cos B = \frac{D^2 - a_3^2 - L^2}{2 \, a_3 L}$$

If the magnitude of that cosine exceeds 1, the point is out of reach — too far, or too close for the elbow to fold around — and you must return "no solution" rather than letting acos produce a NaN that propagates silently into your motor commands. Check it explicitly, every time.

Otherwise there are two solutions, the elbow-up and elbow-down configurations:

$$\begin{aligned} B &= \pm \arccos\left(\frac{D^2 - a_3^2 - L^2}{2 \, a_3 L}\right) \\ A &= \operatorname{atan2}(v, u) - \operatorname{atan2}\big(L\sin B,\; a_3 + L\cos B\big) \end{aligned}$$

And then the conversion from these geometric angles to the actual joint variables, which absorbs the DH $\theta$-offsets and the forearm's built-in bend:

$$\begin{aligned} \theta_2 &= A + \pi/2 \\ \theta_3 &= \pi/2 - \varphi - B \end{aligned}$$

Those two lines are where the sign errors live. I derived them by computing forward kinematics on a grid of $(\theta_2, \theta_3)$ values and fitting the relationship rather than by reasoning on paper, and I would do the same if you change the geometry — it takes ten minutes and it is correct by construction.

11.5 Step three: joints 4, 5, and 6

With $\theta_1$, $\theta_2$, and $\theta_3$ known, compute the rotation the arm has produced up to frame 3:

R_0_3 = fk([th1, th2, th3, 0, 0, 0])[1][2][:3, :3]

The wrist must supply whatever rotation gets you from there to the target:

R_3_6 = R_0_3.T @ R_target

Now, joints 4, 5, and 6 form a Z–Y–Z-like Euler sequence in frame 3, but the $\alpha$ values in the DH table ($-\pi/2$, $+\pi/2$, $\pi$) and the $+\pi$ offset on joint 6 mean the extraction is not the textbook ZYZ formula. Multiplying the three rotation matrices out symbolically gives:

$$R_3^6 = \begin{bmatrix} \cdot & \cdot & -\sin\theta_5 \cos\theta_4 \\ \cdot & \cdot & -\sin\theta_5 \sin\theta_4 \\ \sin\theta_5 \cos\theta_6 & \sin\theta_5 \sin\theta_6 & -\cos\theta_5 \end{bmatrix}$$

from which the extraction follows directly. Writing $r_{ij}$ for the elements of $R_3^6$, with rows and columns numbered from 1:

$$\begin{aligned} \theta_5 &= \operatorname{atan2}\left(\sqrt{r_{13}^2 + r_{23}^2},\; -r_{33}\right) \\ \theta_4 &= \operatorname{atan2}(-r_{23},\, -r_{13}) \\ \theta_6 &= \operatorname{atan2}(r_{32},\, r_{31}) \end{aligned}$$

I have verified these numerically against the forward kinematics over thousands of random configurations; they reproduce the original pose to within $2 \times 10^{-12}$mm. Note the minus signs — they come from the$\alpha = \pi$ rows in the DH table and they are not present in the standard textbook ZYZ formula. Apply the textbook version to this table and you get an answer that is wrong in a way that looks almost right.

The second solution — the wrist-flipped configuration — is:

$$\theta_4' = \theta_4 + \pi, \qquad \theta_5' = -\theta_5, \qquad \theta_6' = \theta_6 + \pi$$

which reaches the same orientation by rotating joint 5 the other way and spinning joints 4 and 6 half a turn each. I have verified this branch too; it is exact.

The wrist singularity. When $\sin\theta_5$ approaches zero, the axes of joints 4 and 6 become collinear. Both $r_{13}$ and $r_{23}$ go to zero, atan2(0, 0) is undefined, and the split between $\theta_4$ and $\theta_6$ is arbitrary — only their sum matters. Detect it by testing whether $\sqrt{r_{13}^2 + r_{23}^2}$ is below a small threshold, and when it is, hold $\theta_4$ at its current value and put all the rotation into $\theta_6$. That gives continuous motion rather than a flip.

11.5a A complete, verified implementation

The whole closed-form solver, tested end-to-end. Over 5,000 random configurations it produces every valid solution branch, and every returned solution reproduces the target pose to within $2.3 \times 10^{-13}$.

import numpy as np

a1, a2, a3, a4, a5, a6, a7 = 110.50, 23.42, 180.00, 43.50, 176.35, 62.80, 45.25
pi = np.pi
L   = np.hypot(a4, a5)        # 181.636 mm
PHI = np.arctan2(a4, a5)      # 0.24184 rad

def wrap(x):
    return (x + pi) % (2 * pi) - pi

def ik(R, p):
    """R: 3x3 target orientation. p: target flange position (mm).
       Returns a list of [th1..th6] in radians — up to 8 branches."""
    pw = p + R @ np.array([a7, 0.0, -a6])      # wrist centre
    x, y, z = pw
    r = np.hypot(x, y)
    out = []

    for th1, sgn in ((np.arctan2(y, x), 1.0),
                     (wrap(np.arctan2(y, x) + pi), -1.0)):
        u = sgn * r - a2
        v = a1 - z
        c = (u*u + v*v - a3*a3 - L*L) / (2 * a3 * L)
        if abs(c) > 1.0:
            continue                            # out of reach on this branch

        for s in (+1.0, -1.0):                  # elbow up / elbow down
            B  = s * np.arccos(np.clip(c, -1.0, 1.0))
            A  = np.arctan2(v, u) - np.arctan2(L*np.sin(B), a3 + L*np.cos(B))
            th2 = wrap(A + pi/2)
            th3 = wrap(pi/2 - PHI - B)

            R03 = forward_kinematics_frames([th1, th2, th3, 0, 0, 0])[2][:3, :3]
            R36 = R03.T @ R
            r13, r23, r33 = R36[0, 2], R36[1, 2], R36[2, 2]
            r31, r32      = R36[2, 0], R36[2, 1]
            sf = np.hypot(r13, r23)

            if sf < 1e-8:                       # wrist singularity
                th5 = 0.0 if -r33 > 0 else pi
                th4 = 0.0
                th6 = wrap(np.arctan2(R36[1, 0], R36[0, 0]))
                out.append([th1, th2, th3, th4, th5, th6])
                continue

            th5 = np.arctan2(sf, -r33)
            th4 = np.arctan2(-r23, -r13)
            th6 = np.arctan2(r32, r31)
            out.append([th1, th2, th3, th4, th5, th6])
            out.append([th1, th2, th3, wrap(th4 + pi), -th5, wrap(th6 + pi)])

    return out

forward_kinematics_frames is the function from Chapter 10.3, modified to return the list of accumulated transforms rather than just the last one.

A note on joint-limit conventions: the published PAROL6 limits (J2 from −145.01° to −3.38°, J3 from +107.87° to +287.87°) are expressed in the robot's own user-facing convention, which may differ by a constant offset or a sign from the DH joint variables above. Establish the mapping empirically — command a known angle, measure the physical joint — before you use the published limits as software limits.

11.6 The eight solutions, and choosing between them

Multiply out the choices: two for shoulder (forward or flipped), two for elbow (up or down), two for wrist (normal or flipped). Eight configurations, all reaching exactly the same tool pose.

A complete IK routine computes all eight, then filters and ranks them.

Filter by joint limits first. The PAROL6's limits are: J1 ±123.05°, J2 from −145.01° to −3.38°, J3 from 107.87° to 287.87°, J4 ±105.47°, J5 ±90°, and J6 continuous. Any solution with a joint outside its range is discarded outright.

Filter next by self-collision, if you have a collision model. More work to implement, and what stops the arm folding into itself.

Rank the survivors. The usual criterion is a weighted sum of joint motion from the current configuration:

$$\text{cost} = \sum_i w_i \left| \theta_i^{\text{new}} - \theta_i^{\text{current}} \right|$$

with larger weights on the big proximal joints, because moving J2 costs far more time and energy than moving J6. Pick the lowest-cost solution.

Add one more consideration: configuration continuity. Along a continuous path, you want the arm to stay in one configuration rather than jumping between them. A jump from elbow-up to elbow-down mid-path is geometrically valid and produces a large, fast, unexpected motion. Prefer solutions in the same configuration branch as the previous point, and allow a branch change only deliberately, at a controlled speed, as an explicit reconfiguration move.

11.7 The numerical alternative

Closed-form IK is fast and complete, but it only exists because of the spherical wrist and it must be re-derived if you change the geometry. The alternative is to solve numerically.

Start from a guess at the joint angles. Compute forward kinematics. Measure the error between the resulting pose and the target — three components of position error, three of orientation error. Use the Jacobian (next chapter) to work out which way to move the joints to reduce that error. Take a step. Repeat until converged.

The naïve version uses the Jacobian inverse and blows up near singularities, where the Jacobian becomes ill-conditioned and the computed joint step becomes enormous. The standard fix is damped least squares, also called the Levenberg–Marquardt method:

$$\Delta q = J^{\mathsf{T}} \left( J J^{\mathsf{T}} + \lambda^2 I \right)^{-1} e$$

The damping factor $\lambda$ limits the step size near singularities at the cost of some accuracy. Adaptive schemes increase $\lambda$ when the condition number is bad and reduce it when things are well-behaved.

Numerical IK is slower — tens to hundreds of iterations — but it handles arbitrary geometry, it naturally incorporates joint limits and secondary objectives, and it degrades gracefully. On a 600 MHz Cortex-M7 you can run it in real time without difficulty.

My recommendation: implement the closed-form solution, which is exact, fast, and gives you all eight configurations, then implement a numerical solver as a fallback and an independent check. Where the two disagree, one of them has a bug.