ENG-654 · Continuation of Lecture 03

Inverse Kinematics of Wrist-Partitioned 6R Robots

Solve position with the arm, orientation with the wrist, and recover all eight branches

Running example: custom_6R_new.urdf. Transfer example: the PUMA 560.

The central idea

Why a spherical wrist simplifies inverse kinematics

The common intersection of axes 4–6 is the partition point $O_w$.

4arm solutions
place $O_w$
× 2wrist flips
match $R_d$
= 8complete IKs
match $T_d$
\[T_0^6(\mathbf q)=\underbrace{T_0^3(q_1,q_2,q_3)}_{\text{arm}}\underbrace{T_3^6(q_4,q_5,q_6)}_{\text{spherical wrist}}\]
The position solve is the 3R IK from Lecture 03; the new work is extracting and solving the residual wrist rotation.

Meet the running example

Six joint axes form an arm and a spherical wrist

Drag all six sliders. The red point is the spherical-wrist center.

1
$z$
2
$y$
3
$z$
4
$x$
5
$z$
6
$x$
Operational frame: we use link_6. The legacy URDF tool0 is attached to link 3, so it cannot represent the 6R tool pose.

Recognizing a spherical wrist

Concurrency belongs to axis lines—not to URDF origins

A revolute axis is an infinite line, not merely the point stored in the URDF joint origin.

axis 4$x$-line through $(4.5,1.25,1.75)$
axis 5$z$-line through $(5.5,1.25,1.75)$
axis 6same $x$-line, stored at $(7,1.25,1.75)$
\[\boxed{O_w=(5.5,1.25,1.75)\ \mathrm m\quad\text{at home}}\]
Rotating joints 4–6 changes orientation but leaves $O_w$ fixed.

Building the kinematic model

Construct each D–H frame from neighboring joint axes

Use standard D–H: $A_i=R_z(\vartheta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i)$.

1

Axes 1–2: $a_1=1$, $d_1=1$, $\alpha_1=-90°$.

2

Axes 2–3: $a_2=2$, $d_2=1.25$, $\alpha_2=+90°$.

3

Axes 3–4 intersect after $d_3=.75$, hence $a_3=0$.

4

Along axis 4, the 3–4 intersection to $O_w$ is $d_4=1.5+1.0=2.5$.

5

Axes 4–5–6 meet at $O_w$, so $a_4=a_5=0$ and $d_5=0$.

6

The link-6 origin lies $d_6=1.5$ along axis 6 from $O_w$.

The resulting model

The D–H table captures lengths, twists, and angle conventions

$i$$a_i$ [m]$\alpha_i$$d_i$ [m]$\vartheta_i=q_i+\delta_i$
11.00−90°1.00$q_1$
22.00+90°1.25$q_2$
30+90°0.75$q_3+90°$
40+90°2.50$q_4+180°$
50+90°0$q_5+180°$
60+90°1.50$q_6+180°$
Lengths come from axis-line geometry; offsets come from the selected D–H $x_i$ directions at URDF home.

Coordinate conventions

One physical home pose can have two coordinate descriptions

\[\boldsymbol\delta=(0,0,90^\circ,180^\circ,180^\circ,180^\circ)\]
joint 3$+90°$ aligns the chosen intersecting-axis frame
joints 4–6$+180°$ accounts for opposite common-normal directions
\[\boxed{T_0^{6,U}=\left(\prod_{i=1}^{6}A_i(q_i+\delta_i)\right)R_z(90^\circ)}\]

The constant terminal rotation maps the D–H frame to the URDF link_6 orientation without moving its origin.

The corrected D–H product agrees with the URDF product-of-exponentials model for arbitrary joint vectors.

Preparing a numerical example

Choose a regular target before solving inverse kinematics

\[\mathbf q_d=(-60^\circ,20^\circ,120^\circ,35^\circ,-50^\circ,70^\circ)\]
arm$(-60°,20°,120°)$
wrist$(35°,-50°,70°)$
regular wrist$\sin q_5\neq0$

What the solver must reproduce

Inverse kinematics must recover the entire pose

Every IK branch must reproduce all sixteen entries, not only the translation.

\[T_d=\begin{bmatrix} .782996& .589593& .198234&5.112668\\ .384659&-.208506&-.899201& .585998\\ -.488830& .780324&-.390052& .715009\\ 0&0&0&1 \end{bmatrix}=\begin{bmatrix}R_d&p_d\\0&1\end{bmatrix}.\]
$R_d$desired tool orientation
$p_d$desired link_6 origin
check$\det R_d=1$, $R_d^TR_d=I$

Separating position from orientation

Orientation tells us where the wrist center must be

\[\boxed{p_w=p_d-d_6R_de_x},\qquad e_x=\begin{bmatrix}1\\0\\0\end{bmatrix},\ d_6=1.5.\]
\[p_w=\begin{bmatrix}5.112668\\.585998\\.715009\end{bmatrix}-1.5\begin{bmatrix}.782996\\.384659\\-.488830\end{bmatrix} =\boxed{\begin{bmatrix}3.938174\\.009009\\1.448254\end{bmatrix}\mathrm m}.\]
Because $R_de_x$ is the joint-6 axis direction, the wrist variables disappear from the position equation.

A geometric reading of the equation

Walk backward from the tool frame to the wrist center

1

Read $p_d$ and $R_d$ from $T_d$.

2

Take the first column $R_de_x$.

3

Move backward $1.5\,\mathrm m$ to obtain $p_w$.

Returning to Lecture 03

The first three joints now solve a familiar position problem

Replace the Lecture-03 tool point by $O_w$; the first three axes are unchanged.

01

Target

$p_w=(3.938174,.009009,1.448254)$

→
02

3R position IK

Apply Lecture-03 quartic elimination and back-substitution.

→
03

Branches

Retain every real $(q_1,q_2,q_3)$.

\[p_w=T_0^3(q_1,q_2,q_3)\begin{bmatrix}5.5&1.25&1.75&1\end{bmatrix}^{T}_{\!\text{home}}\]

Enumerating position solutions

The wrist center admits four arm configurations

arm$q_1$$q_2$$q_3$$z_3$ orientation
A117.108°6.318°−73.729°$(.105,.032,.994)$
A2−151.329°−164.628°14.632°$(.233,.127,-.964)$
A3−107.813°−149.154°87.017°$(.157,.488,-.859)$
A4−60.000°20.000°120.000°$(.171,-.296,.940)$
The wrist point is identical, but $R_0^3$ differs; every arm row therefore needs its own wrist solve.

What differs between arm branches

Position agreement does not imply orientation agreement

position$p_w$ fixes every triad origin
orientation$R_0^3$ fixes the remaining wrist rotation
\[R_3^6=(R_0^3)^TR_d\]

Isolating the wrist motion

Each arm branch leaves a different rotation for the wrist

\[\boxed{M_k=R_{3,k}^{6}=(R_{0,k}^{3})^TR_d}\]
1

Evaluate $R_{0,k}^{3}$ using arm row $k$.

2

Premultiply $R_d$ by $(R_{0,k}^{3})^T$.

3

Decompose $M_k$ using the wrist’s $x-z-x$ sequence.

Reading wrist angles from a matrix

The spherical wrist reduces to an $x$–$z$–$x$ rotation

$R_3^6=R_x(q_4)R_z(q_5)R_x(q_6)$.

\[M=\begin{bmatrix} c_5&-s_5c_6&s_5s_6\\ s_5c_4&-s_4s_6+c_4c_5c_6&-s_4c_6-c_4c_5s_6\\ s_4s_5&s_4c_5c_6+c_4s_6&-s_4c_5s_6+c_4c_6 \end{bmatrix}.\]
middle angle$m_{11}=c_5$
first angle$(m_{21},m_{31})=s_5(c_4,s_4)$
last angle$(-m_{12},m_{13})=s_5(c_6,s_6)$

The wrist’s discrete ambiguity

A regular spherical wrist has two equivalent flips

\[\begin{aligned} s_5&=\pm\sqrt{m_{12}^2+m_{13}^2},\\ q_5&=\operatorname{atan2}(s_5,m_{11}),\\ q_4&=\operatorname{atan2}(m_{31}/s_5,m_{21}/s_5),\\ q_6&=\operatorname{atan2}(m_{13}/s_5,-m_{12}/s_5). \end{aligned}\]
\[(q_4,q_5,q_6)\leftrightarrow(q_4+\pi,-q_5,q_6+\pi)\]
Singularity: if $s_5=0$, $q_4$ and $q_6$ are not individually identifiable.

Completing one arm solution

Either wrist flip completes the same desired pose

A4 · flip 1

\[(-145^\circ,50^\circ,-110^\circ)\]

A4 · flip 2

\[(35^\circ,-50^\circ,70^\circ)\]

The complete solution tree

Four arm branches and two wrist flips produce eight IKs

IK$q_1$$q_2$$q_3$$q_4$$q_5$$q_6$
117.1086.318−73.729−23.20782.925118.503
217.1086.318−73.729156.793−82.925−61.497
3−151.329−164.62814.632−78.522−45.7849.391
4−151.329−164.62814.632101.47845.784−170.609
5−107.813−149.15487.017−72.063−50.137−32.928
6−107.813−149.15487.017107.93750.137147.072
7−60.00020.000120.000−145.00050.000−110.000
8−60.00020.000120.00035.000−50.00070.000
Forward kinematics of every row agrees with $T_d$ to better than $2\times10^{-7}$ in every matrix entry.

Verifying every branch

Eight joint configurations realize one end-effector pose

IK 1–2arm A1, two wrist flips
IK 3–4arm A2, two wrist flips
IK 5–6arm A3, two wrist flips
IK 7–8arm A4, two wrist flips

A reusable algorithm

Wrist partition organizes the solution into five steps

1

Compute $p_w=p_d-d_6R_de_x$.

2

Solve all 3R arm branches for $p_w$.

3

For each branch, compute $M=(R_0^3)^TR_d$.

4

Extract both $x-z-x$ wrist flips.

5

Enforce limits and verify $T_0^6=T_d$.

A second spherical-wrist robot

The PUMA 560 exposes the same position–orientation partition

Explore the uploaded STL model and watch the common wrist center.

armjoints 1–3 position $O_w$
wristaxes 4–6 intersect at $O_w$
toollink7, the child of joint 6
URDF repair: the optional fixed joint names a nonexistent link_7; the valid six-joint chain ends at link7.

Reading the PUMA geometry

Axis-line distances provide the constants for the arm solve

base height$0.6718\,\mathrm m$
axes 2–3$\sqrt{.4318^2+.0203^2}=.43228\,\mathrm m$
shoulder offset$0.1501\,\mathrm m$ along the parallel axes
forearm$0.4331\,\mathrm m$ to the wrist center
\[\boxed{p_w=p_d-0.0558R_de_z}\]

The final link lies along the tool $z$-axis. Subtracting it removes joints 4–6 from the position equation.

\[p_w=f_{\mathrm{arm}}(q_1,q_2,q_3)\]
The numerical constants differ from the custom robot, but the two-stage reasoning is identical.

A regular PUMA target

Forward kinematics creates the pose that inverse kinematics must recover

\[q_d=(30^\circ,-35^\circ,45^\circ,40^\circ,-50^\circ,60^\circ)\]
\[p_w=\begin{bmatrix}.436419\\.078646\\-.019019\end{bmatrix}\mathrm m\]
\[T_d=\begin{bmatrix} .112612&-.525432& .843351& .483478\\ -.939575&-.332458&-.081670& .074089\\ .323291&-.783194&-.531121&-.048656\\ 0&0&0&1 \end{bmatrix}.\]
This target is nonsingular and produces four mathematical arm branches before joint limits are applied.

Solving the PUMA arm

Shoulder and elbow choices generate four ways to reach $p_w$

Apply the Lecture-03 Paden–Kahan construction to the first three PUMA axes.

arm$q_1$$q_2$$q_3$geometric branch
P130.000°−35.000°45.000°right shoulder · elbow 1
P230.000°−77.351°129.617°right shoulder · elbow 2
P3170.431°−139.617°129.617°left shoulder · elbow 2
P4170.431°−97.266°45.000°left shoulder · elbow 1
\[R_{3,k}^{6}=(R_{0,k}^{3})^TR_d,\qquad k=1,\ldots,4\]

Completing the PUMA solutions

Each PUMA arm branch admits two spherical-wrist flips

The final axes form a $z$–$y$–$z$-type wrist under the URDF sign conventions.

IK$q_1$$q_2$$q_3$$q_4$$q_5$$q_6$
130.000−35.00045.000−140.00050.000−120.000
230.000−35.00045.00040.000−50.00060.000
330.000−77.351129.617−90.22629.499178.601
430.000−77.351129.61789.774−29.499−1.399
5170.431−139.617129.617−4.60947.947−111.487
6170.431−139.617129.617175.391−47.94768.513
7170.431−97.26645.000−31.5396.549−83.205
8170.431−97.26645.000148.461−6.54996.795
These are all mathematical branches modulo $360°$. The uploaded URDF limits retain only IK 2 for this target.

Checking the PUMA solution set

All eight mathematical branches reproduce the same pose

positionthe red $p_w$ is unchanged
orientationthe colored target triad is unchanged
limitsfilter only after enumeration

KUKA iiwa 7 · redundancy

A 7R arm has one internal degree of freedom at a fixed pose

7joint variables
− 6pose constraints
= 1redundancy parameter
\[T_0^7(q_1,\ldots,q_7)=T_d\quad\Longrightarrow\quad \dim\mathcal S\approx1\]
Instead of isolated IK points, a regular redundant robot has continuous self-motion curves in joint space.

Choosing a redundancy coordinate

Fixing $q_3$ turns the iiwa into an instantaneous 6R problem

\[q_3=\lambda\]

At a chosen value of $\lambda$, the remaining variables are

\[\bar q=(q_1,q_2,q_4,q_5,q_6,q_7)\in\mathbb R^6.\]
\[F_\lambda(\bar q)=T_0^7(q_1,q_2,\lambda,q_4,q_5,q_6,q_7)=T_d.\]

Six unknown joint angles now satisfy the six independent constraints of a full pose.

Solving this parameterized 6R problem analytically produces branches $\bar q_k(\lambda)$ of the 7R inverse kinematics.

The D–H meaning of fixing $q_3$

A joint variable becomes a geometric parameter of the reduced robot

Original iiwa · 7R

Seven revolute coordinates; every twist is fixed.

$i$$\theta_i$$d_i$$a_i$$\alpha_i$
1$q_1$$d_1$$a_1$$\alpha_1^0$
2$q_2$$d_2$$a_2$$\alpha_2^0$
3$q_3$$d_3$$a_3$$\alpha_3^0$
4$q_4$$d_4$$a_4$$\alpha_4^0$
5$q_5$$d_5$$a_5$$\alpha_5^0$
6$q_6$$d_6$$a_6$$\alpha_6^0$
7$q_7$$d_7$$a_7$$\alpha_7^0$
$q_3=\lambda$→$\bar\alpha_2=\lambda$

Instantaneous member · 6R

Six revolute coordinates; geometry is indexed by $\lambda$.

$j$iiwa axis$\bar\theta_j$$\bar d_j$$\bar a_j$$\bar\alpha_j$
11$q_1$$\bar d_1$$\bar a_1$$\bar\alpha_1$
22$q_2$$\bar d_2$$\bar a_2$$\boxed{q_3=\lambda}$
34$q_4$$\bar d_3$$\bar a_3$$\bar\alpha_3$
45$q_5$$\bar d_4$$\bar a_4$$\bar\alpha_4$
56$q_6$$\bar d_5$$\bar a_5$$\bar\alpha_5$
67$q_7$$\bar d_6$$\bar a_6$$\bar\alpha_6$
With the reduced-frame convention, locking joint 3 removes $q_3$ from the coordinate vector and places its chosen value in the geometry: $\bar\alpha_2=q_3$.

A family of instantaneous robots

The 7R iiwa is a union of 6R robots with different twist parameters

\[\mathcal R_{7R}=\bigcup_{\lambda\in[-360^\circ,360^\circ]}\mathcal R_{6R}(\bar\alpha_2=\lambda)\]
choose$q_3=\lambda$
freezejoint 3 becomes geometry
solvethe remaining six joints
vary$\lambda$ to recover the 7R family
The translucent postures are different 6R members. Their D–H tables differ in $\bar\alpha_2$, yet all reproduce the same $T_d$.

A parameterized iiwa solution

Changing $q_3$ moves the elbow while the tool pose stays fixed

\[q_d=(20,-35,30,50,-40,45,60)^\circ\]
free parameter$q_3\in[-180°,180°]$ in the displayed cycle
dependent joints$q_1,q_2,q_4,q_5,q_6,q_7$
invariantthe complete $T_d$, not only $p_d$
The visualization uses iiwa7_free_joints.urdf, whose seven revolute joints each have a $[-360°,360°]$ range.

What “arbitrary $q_3$” really means

The redundancy parameter is free only inside feasible intervals

1

Choose $q_3=\lambda$ within its mechanical range.

2

Solve the resulting instantaneous 6R IK and retain every real branch.

3

Continue each branch as $\lambda$ varies; this traces a self-motion curve.

4

Stop or change branch at workspace boundaries, singularities, collisions, or joint limits.

\[\mathcal S(T_d)=\bigcup_{\lambda\in I}\left\{(q_1,q_2,\lambda,q_4,q_5,q_6,q_7)_k\right\}\]

What to carry forward

Robot geometry determines the right inverse-kinematics decomposition

1

A spherical wrist separates a 6R pose problem into arm position and wrist orientation.

2

The custom robot and PUMA use the same logic with different axis geometry and tool directions.

3

Four arm branches and two wrist flips can create eight mathematical 6R solutions.

4

For a 7R iiwa, fixing one redundancy parameter exposes a family of instantaneous 6R solves.

5

Joint limits, singularities, and collisions determine which mathematical branches are usable.

Reference: J. J. Craig, Introduction to Robotics: Mechanics and Control, manipulator kinematics and inverse kinematics chapters.