Algebraic · Pieper
Use standard D–H parameters, eliminate \(\theta_1\), eliminate \(\theta_2\), and solve one quartic in \(\theta_3\).
ENG-654 · Session 2 · Lecture 1
Inverse Kinematics of 3R Robots
The same inverse problem, solved once by elimination and once by geometry
We will derive Pieper’s quartic from D–H parameters, then solve an intersecting-axis 3R chain with Paden–Kahan circle subproblems.
Running example · Meet the robot
Kinematic geometry of the custom 3R robot
The lines connect consecutive D–H axes. Their signed lengths are the geometry used by both forward and inverse kinematics.
The narrative
Algebraic and geometric structure in 3R inverse kinematics
Both methods enumerate branches analytically; they differ in what they exploit.
↔
Geometric · Paden–Kahan
View a revolute joint as a circle, then solve circle–point and circle–circle constraints.
Learning goal: recognize when elimination is generic—and when robot geometry gives a shorter route.
Sources:
D. L. Pieper, The Kinematics of Manipulators under Computer Control, PhD thesis, Stanford University, 1968;
W. Kahan, Lectures on Computational Aspects of Geometry, UC Berkeley, 1983.
B. E. Paden, Kinematics and Control of Robot Manipulators, PhD thesis, UC Berkeley, 1986;
R. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to Robotic Manipulation. Boca Raton, FL: CRC Press, 1994.
Problem setup
The 3R position inverse-kinematics problem
Given a reachable end-effector position \(\mathbf p_d=[x\;y\;z_e]^T\), find every \(\boldsymbol\theta=[\theta_1\;\theta_2\;\theta_3]^T\) that produces it.
\[\mathbf p(\theta_1,\theta_2,\theta_3)=\mathbf p_d\]
\[
\mathbf q=(-35^\circ,-10^\circ,-170^\circ)
\Rightarrow
\mathbf p_d=\begin{bmatrix}1.7018\\0.0164\\1.8294\end{bmatrix}\mathrm m
\]
InputD–H geometry and \(\mathbf p_d\)
Outputall real joint triples within limits
Branchesone position may have several IK solutions
Algebraic method · D–H model
Standard D–H convention for the 3R chain
We use standard D–H transforms. This fixes the meaning of every parameter in the derivation.
\[{}^{i-1}T_i=R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i)\]
| joint \(i\) | \(a_i\) | \(\alpha_i\) | \(d_i\) | variable |
|---|---|---|---|---|
| 1 | \(a_1\) | \(\alpha_1\) | \(d_1\) | \(\theta_1\) |
| 2 | \(a_2\) | \(\alpha_2\) | \(d_2\) | \(\theta_2\) |
| 3 | \(a_3\) | \(\alpha_3\) | \(d_3\) | \(\theta_3\) |
Position only: \(\alpha_3\) changes the terminal frame orientation but not the origin \(O_3\), so it does not enter \(\mathbf p_{O_3}\).
Apply it · custom_3R D–H model
D–H parameters of the custom 3R robot
Placing \(F_0\) at the world origin and absorbing the vertical offset into \(d_1\) reproduces the model geometry exactly.
Algebraic method · Strategy
Ordered elimination of joint variables
A compact geometric representation exposes invariants that remove \(\theta_1\) and \(\theta_2\) before solving for \(\theta_3\).
Translate by \(d_1\), then keep radial distance and height—quantities unchanged by rotation about \(z_0\).
Represent its action as a 2D rotation and use preservation of planar length.
Use \(t=\tan(\theta_3/2)\) to convert the trigonometric closure into a polynomial.
Guiding question: which scalar quantities remain unchanged when a point is rotated?
Algebraic method · Package joints 2 and 3
A composite vector for the third-joint dependence
The vector \(\mathbf u(\theta_3)\) collects every occurrence of \(\theta_3\), with components that are linear in \(c_3\) and \(s_3\).
Before rotation by \(\theta_2\), define
\[
\mathbf u(\theta_3)=
\begin{bmatrix}a_2\\0\\d_2\end{bmatrix}
+R_x(\alpha_2)
\begin{bmatrix}a_3c_3\\a_3s_3\\d_3\end{bmatrix}.
\]
\[
\begin{aligned}
u_x&=a_2+a_3c_3,\\
u_y&=c_{\alpha2}a_3s_3-s_{\alpha2}d_3,\\
u_z&=d_2+s_{\alpha2}a_3s_3+c_{\alpha2}d_3.
\end{aligned}
\]
Each component is at most linear in \(s_3,c_3\).
Apply it · custom_3R \(\mathbf u(\theta_3)\)
Geometric variation of \(\mathbf u(\theta_3)\)
The home vector and the global target’s \(\theta_3=-170^\circ\) vector isolate the geometric change produced before the \(\theta_2\) rotation.
Algebraic method · Eliminate \(\theta_1\)
Azimuth-invariant target coordinates
The custom robot’s forward kinematics makes the first elimination visible: \(\theta_1\) rotates the horizontal coordinates but cannot change height or horizontal radius.
Substituting the D–H values from custom_3R_new.urdf, define
\[
F_1=2+1.5c_3,\qquad F_2=0.75,\qquad C=1.25+1.5s_3,
\]
\[
\begin{aligned}
x&=c_1(1+F_1c_2+F_2s_2)-s_1C,\\
y&=s_1(1+F_1c_2+F_2s_2)+c_1C,\\
z&:=z_e-d_1=F_2c_2-F_1s_2.
\end{aligned}
\]
\[
\rho^2:=x^2+y^2,
\qquad R:=\rho^2+z^2=\|[x\;y\;z]^T\|^2,
\]
\[
\frac{\partial R}{\partial\theta_1}=0,
\qquad \frac{\partial z}{\partial\theta_1}=0,
\qquad \frac{\partial\rho^2}{\partial\theta_1}=0.
\]
Notation follows Salunkhe et al., Mechanism and Machine Theory 171 (2022), Sec. 2.1; the paper’s \(z\) is measured after removing \(d_1\).
The norm \(R=\rho^2+z^2\), the shifted height \(z\), and \(\rho^2=x^2+y^2\) are independent of \(\theta_1\).
Algebraic method · Coefficient form
The invariant equations are linear in \(c_2,s_2\)
The paper packages every \(\theta_3\)-dependent term into \(F_i(\theta_3)\), leaving two equations with explicit second-joint coefficients.
Algebraic method · Radial and height equations
Two coefficient equations determine \(\theta_2\)
Rearranging the two invariant equations isolates two orthogonal linear combinations of \(c_2,s_2\).
Algebraic method · Eliminate \(\theta_2\)
Squaring and adding removes the second joint
The coefficient rows are orthogonal, so the squared sum cancels the cross terms and uses only \(c_2^2+s_2^2=1\).
1
Square \(F_1c_2+F_2s_2=E\).
2
Define \(z_m=z/\sin\alpha_1\), then square \(F_1s_2-F_2c_2=z_m\).
3
Add: the \(2F_1F_2c_2s_2\) terms cancel.
\[
\begin{aligned}
E^2+z_m^2
&=(F_1^2+F_2^2)(c_2^2+s_2^2)\\
&=F_1^2+F_2^2,
\end{aligned}
\]
\[
\boxed{
z_m=\frac{z}{\sin\alpha_1},
\qquad E^2+z_m^2=F_1^2+F_2^2.}
\]
This is the paper’s elimination step: \(F_i=F_i(\theta_3)\), so the closure contains only \(\theta_3\).
Custom 3R · Numerical closure
A scalar equation for the third joint
Substituting the target invariant \(R\), shifted height \(z\), and the custom robot’s \(F_i\) leaves a scalar constraint in \(c_3,s_3\)
Algebraic method · Back-substitute \(\theta_2\)
Recovering the second joint from the coefficient equations
Once a real \(\theta_3\) is known, the same two coefficient equations recover \(c_2,s_2\) without another nonlinear solve.
\[
\begin{bmatrix}F_1&F_2\\-F_2&F_1\end{bmatrix}^{T}
\begin{bmatrix}F_1&F_2\\-F_2&F_1\end{bmatrix}
=(F_1^2+F_2^2)I,
\]
\[
\boxed{\begin{aligned}
c_2&=\frac{F_1E-F_2z_m}{F_1^2+F_2^2},\\
s_2&=\frac{F_2E+F_1z_m}{F_1^2+F_2^2},\\
\theta_2&=\operatorname{atan2}(s_2,c_2).
\end{aligned}}
\]
Closure
tests whether \(E^2+z_m^2=F_1^2+F_2^2\)
Recovery
applies the scaled transpose of the coefficient matrix
Apply it · custom_3R radial equation
Radial distance in the target plane
The circle shows \(\rho^2=x^2+y^2\); the paper combines it with shifted height to form \(R=\rho^2+z^2\).
Algebraic method · Conic closure
The squared-sum equation is a conic
Using \(c_3^2+s_3^2=1\), each \(F_i(\theta_3)\) is constant or affine in \(c_3,s_3\); the closure is therefore quadratic.
\[
E(c_3,s_3)=\frac{R-F_3(c_3,s_3)}{2a_1},
\qquad z_m=\frac{z}{\sin\alpha_1},
\]
\[
F(c_3,s_3)=E^2+z_m^2-F_1^2-F_2^2.
\]
\[
\boxed{A_{xx}c_3^2+2A_{xy}c_3s_3+A_{yy}s_3^2+2B_xc_3+2B_ys_3+C_0=0.}
\]
Paper’s construction
\(R,z\) enter only through the conic coefficients
Circle identity
\(c_3^2+s_3^2=1\) supplies the second equation
The algebra now contains only \(c_3,s_3\); intersecting this conic with the unit circle gives every real \(\theta_3\).
Algebraic method · Geometric interpretation
A conic–circle formulation for \(\theta_3\)
Substituting the custom robot’s \(F_i\), target invariant \(R\), and shifted height \(z\) gives the numerical conic.
Algebraic method · Interactive target invariants
Target invariants reshape the third-joint conic
Changing the paper’s invariants \(R=\rho^2+z^2\) and \(z\) changes the conic while the trigonometric unit circle remains fixed.
Algebraic method · Why degree four?
Quartic closure in tangent-half-angle coordinates
\(\mathbf u\) is linear in \(s_3,c_3\), while \(\|\mathbf u\|^2\) also reduces to a linear expression using \(s_3^2+c_3^2=1\). Thus \(F\) is at most quadratic in \(s_3,c_3\).
Trig closure
\(F(s_3,c_3)=0\)
degree \(\le2\)
→
Rational parameter
\(t=\tan(\theta_3/2)\)
\(s_3=\frac{2t}{1+t^2}\)
\(c_3=\frac{1-t^2}{1+t^2}\)
→
Polynomial
\(b_4t^4+b_3t^3+b_2t^2+b_1t+b_0=0\)
at most four roots
Map every real root back with \(\theta_3=2\operatorname{atan}(t)\). Check \(\theta_3=\pi\) separately: it corresponds to \(t=\infty\).
Apply it · custom_3R numerical quartic
Four real branches of the numerical IK problem
The quartic has four real roots, and back-substitution maps each root to a distinct joint configuration at the same target position.
Algebraic method · Closed form
A generic 3R position IK is algebraically closed-form
Polynomials through degree four have formulas in radicals. Since generic 3R position IK reduces to a quartic, it is analytically solvable without iteration.
Closed form gives
- a finite algebraic procedure;
- enumeration of all candidates;
- no initial guess.
It does not imply
- every target is reachable;
- every root is real;
- radical formulas are numerically best.
In practice: form the quartic, use a stable polynomial root solver, then validate candidates with forward kinematics.
Algebraic method · Backpropagation 1/2
Back-substitution for \(\theta_2\)
For one accepted \(\theta_3\), the coefficient vector \((F_1,-F_2)\) rotates directly into the target-invariant vector \((E,z_e)\).
\[\begin{bmatrix}E\\z_e\end{bmatrix}=R(\theta_2)\begin{bmatrix}F_1\\-F_2\end{bmatrix}\]
Subtract their polar angles.
\[\boxed{\theta_2=\operatorname{atan2}(z_e,E)-\operatorname{atan2}(-F_2,F_1)}\]
Wrap to the chosen joint interval.
The closure guarantees the two vectors have equal length; the oriented-angle difference therefore recovers \(\theta_2\).
Apply it · custom_3R backpropagation
Numerical recovery of \(\theta_2\)
The joint angle is the oriented difference between the paper’s coefficient vector \((F_1,-F_2)\) and the target-invariant vector \((E,z_e)\).
Algebraic method · Backpropagation 2/2
Back-substitution for \(\theta_1\)
Before the first rotation, the horizontal vector is \((a_1+E,C)\). Joint 1 rotates it into the measured target projection \((x,y)\).
\[\begin{bmatrix}x\\y\end{bmatrix}=R(\theta_1)\begin{bmatrix}a_1+E\\C\end{bmatrix}\]
\[\boxed{\theta_1=\operatorname{atan2}(y,x)-\operatorname{atan2}(C,a_1+E)}\]
Normalize and enforce joint limits.
For every real \(\theta_3\): recover \(\theta_2\), then \(\theta_1\), then check \(\|\mathbf p(\boldsymbol\theta)-\mathbf p_d\|\).
Apply it · custom_3R backpropagation
Numerical recovery of \(\theta_1\)
The first-joint angle is the azimuth correction between the pre-rotation vector and the target projection.
Algebraic method · Feasibility and special structure
Feasibility tests and the intersecting-axis limit
The coefficient formulation identifies singular recovery cases explicitly; intersecting first and second axes require a separate geometric reduction.
\(a_1=0\)
The common-normal distance between axes 1 and 2 vanishes: the axes intersect.
\[E=\frac{R-F_3}{2a_1}\quad\text{is undefined.}\]
Interpretation: the paper treats this as a special structure; the shared point enables a shorter geometric decomposition.
\(F_1^2+F_2^2=0\)
The coefficient matrix for recovering \(c_2,s_2\) loses rank.
\[\det\!\begin{bmatrix}F_1&F_2\\F_2&-F_1\end{bmatrix}=-(F_1^2+F_2^2).\]
Conclusion: \(\theta_2\) is not identifiable from these two invariant equations.
URDF sequence · a₁ → 0
Intersecting axes in the limit \(a_1\to0\)
The sequence custom_3R_new → 800 → 600 → 400 → 0 renders the corresponding URDF geometry while the common-normal distance \(a_1\) contracts to zero.
Algebraic method · Algorithm
The ordered-elimination algorithm
The analytical procedure reduces position IK to one quartic solve, two angle recoveries, and forward-kinematics validation.
1
Compute shifted \(z=z_e-d_1\), \(\rho^2=x^2+y^2\), and the paper’s invariant \(R=\rho^2+z^2\).
2
Express \(R\) and \(z\) using \(F_i(\theta_3)\), then set \(E=(R-F_3)/(2a_1)\).
3
Set \(z_e=z/\sin\alpha_1\), then enforce \(E^2+z_e^2=F_1^2+F_2^2\) and solve the conic–circle system or quartic.
4
Recover \(\theta_2\) from \((F_1,-F_2)\to(E,z_e)\), then recover \(\theta_1\) from \((a_1+E,C)\to(x,y)\).
5
Apply joint limits and validate every candidate by forward kinematics.
Numerical replay · custom_3R
Numerical reconstruction of one IK branch
Back-substitution recovers \(\theta_2\) and \(\theta_1\) from a selected real root \(\theta_3\), reproducing the target to numerical precision.
Method 2 · Geometry
Paden–Kahan Subproblems
A revolute joint draws a circle. IK asks where that circle meets another constraint.
Geometric method · Circle primitive
Circular orbits of revolute motion
Let \(\boldsymbol\omega\) be a unit axis through the origin and decompose a point into axial and perpendicular parts.
\[
\mathbf p_\parallel=(\boldsymbol\omega^T\mathbf p)\boldsymbol\omega,
\qquad \mathbf p_\perp=\mathbf p-\mathbf p_\parallel
\]
center\(\mathbf p_\parallel\)
radius\(r=\|\mathbf p_\perp\|\)
plane\(\boldsymbol\omega^T\mathbf x=\boldsymbol\omega^T\mathbf p\)
Rotation preserves axial coordinate, distance to the axis, and Euclidean length.
Geometric method · Circle–circle distance
Circle intersection and geometric feasibility
For coplanar circles with radii \(r_1,r_2\) and center distance \(d\), feasibility is determined by three lengths.
\(d>r_1+r_2\)
too far apart
\(d=r_1+r_2\)
external tangent
\(|r_1-r_2|<d<r_1+r_2\)
two intersections
\(d<|r_1-r_2|\)
one inside the other
Geometric method · PK1 statement
Paden–Kahan subproblem 1: point registration
Given unit axis \(\boldsymbol\omega\) and points \(\mathbf p,\mathbf q\), find \(\theta\) such that \(R(\boldsymbol\omega,\theta)\mathbf p=\mathbf q\).
PK1
\[R(\boldsymbol\omega,\theta)\mathbf p=\mathbf q\]
Exact feasibility requires
\[
\boldsymbol\omega^T\mathbf p=\boldsymbol\omega^T\mathbf q,
\qquad \|\mathbf p_\perp\|=\|\mathbf q_\perp\|.
\]
If feasible and the orbit radius is nonzero, PK1 has one solution modulo \(2\pi\).
Geometric method · PK1 solution
The oriented-angle solution of PK1
Project both points perpendicular to the axis; then compute cosine and signed sine of the required angle.
\[
\mathbf p_\perp=(I-\boldsymbol\omega\boldsymbol\omega^T)\mathbf p,\qquad
\mathbf q_\perp=(I-\boldsymbol\omega\boldsymbol\omega^T)\mathbf q
\]
\[
\cos\theta=\frac{\mathbf p_\perp^T\mathbf q_\perp}{\|\mathbf p_\perp\|\|\mathbf q_\perp\|},\quad
\sin\theta=\frac{\boldsymbol\omega^T(\mathbf p_\perp\times\mathbf q_\perp)}{\|\mathbf p_\perp\|\|\mathbf q_\perp\|}.
\]
\[
\boxed{\theta=\operatorname{atan2}\!\left(
\boldsymbol\omega^T(\mathbf p_\perp\times\mathbf q_\perp),
\mathbf p_\perp^T\mathbf q_\perp\right)}
\]
\(\operatorname{atan2}\)(sine-like, cosine-like) preserves direction and all four quadrants.
If \(\|\mathbf p_\perp\|=0\), the point lies on the axis and does not move: the angle is arbitrary or the constraint is infeasible.
Geometric method · PK3 statement
Paden–Kahan subproblem 3: a distance constraint
Given \(\boldsymbol\omega,\mathbf p,\mathbf q,\delta\), find every angle for which the rotated point lies on a sphere centered at \(\mathbf q\).
PK3
\[\|R(\boldsymbol\omega,\theta)\mathbf p-\mathbf q\|=\delta\]
The orbit is one circle. The distance constraint is a sphere. Slice the sphere by the orbit plane to obtain a second circle.
Geometric method · PK3 reduction
Planar reduction of the PK3 constraint
Axial separation consumes part of the allowed distance; the remainder becomes an in-plane circle radius.
\[
\mathbf c=(\boldsymbol\omega^T\mathbf p)\boldsymbol\omega,\quad
\mathbf p_\perp=\mathbf p-\mathbf c,\quad r=\|\mathbf p_\perp\|
\]
\[
h=\boldsymbol\omega^T(\mathbf q-\mathbf c),\quad
\mathbf q_\perp=(I-\boldsymbol\omega\boldsymbol\omega^T)(\mathbf q-\mathbf c),\quad
d=\|\mathbf q_\perp\|.
\]
Cross-section radius:
\[\boxed{\delta_\perp=\sqrt{\delta^2-h^2}}\]
First check: \(\delta^2-h^2\ge0\).
Second check: \(|r-\delta_\perp|\le d\le r+\delta_\perp\).
A 3D circle–sphere intersection is now an ordinary 2D circle–circle intersection.
Geometric method · PK3 solution
Two PK3 branches from the law of cosines
Let \(\gamma\) rotate \(\mathbf p_\perp\) toward the second circle’s center. Let \(\phi\) be the triangle’s remaining angle.
\[
\gamma=\operatorname{atan2}\!\left(
\boldsymbol\omega^T(\mathbf p_\perp\times\mathbf q_\perp),
\mathbf p_\perp^T\mathbf q_\perp\right)
\]
\[
\phi=\arccos\!\left(\frac{r^2+d^2-\delta_\perp^2}{2rd}\right)
\]
\[\boxed{\theta=\gamma\pm\phi}\]
two\(0<\phi<\pi\)
one\(\phi=0\) or \(\pi\): tangent
noneacos argument outside \([-1,1]\)
Clamp within floating-point tolerance before \(\arccos\); reject larger feasibility violations.
Geometric method · Connect to D–H
The intersecting axes of custom_3R_new_0
The URDF places joint 1 on the vertical line through \((0,0,0.5)\) and joint 2 on the \(y\)-directed line through \((0,0,1)\). They meet at \(\mathbf c=(0,0,1)\,\mathrm m\), so \(a_1=0\).
\[
e^{\widehat{\xi}_1\theta_1}\mathbf c=\mathbf c,\qquad
e^{\widehat{\xi}_2\theta_2}\mathbf c=\mathbf c
\]
The black joint-1 axis and red joint-2 axis both pass through the blue point \(\mathbf c\). Therefore both rotations fix \(\mathbf c\).
The algebraic degeneracy becomes a geometric advantage.
Geometric 3R IK · Setup
Robot-specific POE data and position target
This is position IK: only \(\mathbf p_d\) is prescribed. Tool orientation is deliberately left free, allowing every position branch to survive.
\[
\begin{aligned}
\boldsymbol\omega_1&=\mathbf e_z,&\mathbf q_1&=(0,0,0.5),\\
\boldsymbol\omega_2&=\mathbf e_y,&\mathbf q_2&=\mathbf c=(0,0,1),\\
\boldsymbol\omega_3&=\mathbf e_z,&\mathbf q_3&=(2,1.25,1).
\end{aligned}
\]
\[\mathbf p=\mathbf p_{tool,0}=(3.5,1.25,1.75)\ \mathrm m\]
shared point \(\mathbf c\)fixed by joints 1 and 2
tool point \(\mathbf p\)moved first by joint 3
Numerical target used in the derivation:
\[\mathbf p_d=\begin{bmatrix}.882624\\.589970\\1.829387\end{bmatrix}\mathrm m\]
Position equation
\[e^{\widehat\xi_1\theta_1}e^{\widehat\xi_2\theta_2}e^{\widehat\xi_3\theta_3}\mathbf p=\mathbf p_d\]
This target was generated from \((-35^\circ,-10^\circ,-170^\circ)\); the solve below recovers it without using those angles.
Expected multiplicity: PK3 gives two \(\theta_3\) candidates; PK2 gives two \((\theta_1,\theta_2)\) pairs for each candidate—up to four position solutions.
Geometric 3R IK · PK3 construction
Build the sphere and the joint-3 orbit
Before moving the robot, convert the target position into the two geometric objects used by PK3.
1
The desired tool position sets a sphere centered at the shared point:
\[\delta=\|\mathbf p_d-\mathbf c\|=1.3472\ \mathrm m\]
\[\mathcal S=\{\mathbf x:\|\mathbf x-\mathbf c\|=\delta\}\]
2
The home tool point is \(1.5\,\mathrm m\) from the third axis:
\[\mathbf p=(3.5,1.25,1.75),\quad \rho_3=1.5\ \mathrm m\]
\[\mathcal C_3=\{e^{\widehat\xi_3\theta_3}\mathbf p:\theta_3\in[-\pi,\pi]\}\]
PK3 question
\[\text{Where does }\mathcal C_3\text{ intersect }\mathcal S?\qquad \|e^{\widehat\xi_3\theta_3}\mathbf p-\mathbf c\|=\delta\]
The sphere encodes the target distance; the circle encodes every position joint 3 can give the home tool point.
Geometric 3R IK · Step 1 · Animated construction
Rotate \(\mathbf p\) to a circle–sphere intersection
The animation follows the solve order: create the sphere, reveal the robot at its home tool point \(\mathbf p\), then rotate joint 3 until \(\mathbf p\) reaches an intersection.
1
Sphere: draw \(\|\mathbf x-\mathbf c\|=\delta\), where \(\delta=1.3472\,\mathrm m\).
2
Start: show the robot at \(\theta_3=0\), with the tool at \(\mathbf p\).
3
Rotate: move \(\mathbf p\) along \(\mathcal C_3\) until it lies on the sphere.
PK3 scalar equation
\[e^{\widehat\xi_3\theta_3}\mathbf p-\mathbf c=\begin{bmatrix}2+1.5c_3\\1.25+1.5s_3\\.75\end{bmatrix}\]
\[\delta^2=8.375+6c_3+3.75s_3\]
\[\boxed{\theta_3=-170.000^\circ\ \text{or}\ -125.989^\circ}\]
At an intersection, the first two rotations can reorient the vector about \(\mathbf c\) without changing its length. This is why PK3 determines \(\theta_3\) first.
Geometric 3R IK · Step 2
Each \(\theta_3\) candidate leaves a PK2 problem
Position IK does not prescribe orientation. After selecting \(\theta_3\), solve the two remaining rotations together rather than constructing a full-pose residual transform.
\[
\begin{aligned}
\mathbf u_0&=\mathbf p-\mathbf r_3=(1.5,0,.75),\\
\mathbf a&=\mathbf r_3-\mathbf c=(2,1.25,0),\\
\mathbf t(\theta_3)&=\mathbf a+R(\boldsymbol\omega_3,\theta_3)\mathbf u_0.
\end{aligned}
\]
\[
\mathbf q:=\mathbf p_d-\mathbf c
=\begin{bmatrix}.882624\\.589970\\.829387\end{bmatrix}
\]
PK2
\[\boxed{\mathbf q=R(\boldsymbol\omega_1,\theta_1)R(\boldsymbol\omega_2,\theta_2)\mathbf t(\theta_3)}\]
For each of the two PK3 values of \(\theta_3\), PK2 can return two pairs \((\theta_1,\theta_2)\).
Geometric 3R IK · PK2 construction
PK2 introduces two intermediate vectors
Let \(\mathbf x=R(\boldsymbol\omega_2,\theta_2)\mathbf t\). On the common sphere centered at \(\mathbf c\), intersect the joint-2 orbit through \(\mathbf t\) with the inverse joint-1 orbit through \(\mathbf p_d\).
\[
\eta=\boldsymbol\omega_2^T\mathbf t,\qquad
\zeta=\boldsymbol\omega_1^T\mathbf q
\]
\[
\gamma=\sqrt{\|\mathbf t\|^2-\eta^2-\zeta^2}
\]
\[
\boxed{\mathbf x_\pm=\zeta\boldsymbol\omega_1+\eta\boldsymbol\omega_2
\pm\gamma(\boldsymbol\omega_1\times\boldsymbol\omega_2)}
\]
blue \(\mathcal C_2\)sphere ∩ plane normal to \(\boldsymbol\omega_2\), passing through \(\mathbf t\)
orange \(\mathcal C_1\)sphere ∩ plane normal to \(\boldsymbol\omega_1\), passing through \(\mathbf q\)
The two red circle–circle intersections are \(\mathbf x_\pm\). Then solve \(R(\boldsymbol\omega_2,\theta_2^\pm)\mathbf t=\mathbf x_\pm\) and \(R(\boldsymbol\omega_1,\theta_1^\pm)\mathbf x_\pm=\mathbf q\) with PK1.
Geometric 3R IK · PK2 branch A
Two solutions for \(\theta_3=-125.989^\circ\)
Compute \(\mathbf t\), form \(\mathbf x_\pm\), then apply the two PK1 registrations inside PK2.
\[
\mathbf t=\begin{bmatrix}1.11855\\.03631\\.75000\end{bmatrix},\quad
(\eta,\zeta,\gamma)=(.03631,.82939,1.06102)
\]
\[
\mathbf x_+=\begin{bmatrix}-1.06102\\.03631\\.82939\end{bmatrix},\qquad
\mathbf x_-=\begin{bmatrix}1.06102\\.03631\\.82939\end{bmatrix}
\]
\(\mathbf x_+\)\((\theta_1,\theta_2)=(-144.280^\circ,-108.143^\circ)\)
\(\mathbf x_-\)\((\theta_1,\theta_2)=(31.800^\circ,-4.172^\circ)\)
Both configurations place the tool at the same \(\mathbf p_d\); their orientations are different and unconstrained.
Geometric 3R IK · PK2 branch B
Two solutions for \(\theta_3=-170.000^\circ\)
The second PK3 intersection produces another pair of intermediate vectors and therefore another two PK2 branches.
\[
\mathbf t=\begin{bmatrix}.52279\\.98953\\.75000\end{bmatrix},\quad
(\eta,\zeta,\gamma)=(.98953,.82939,.38461)
\]
\[
\mathbf x_+=\begin{bmatrix}-.38461\\.98953\\.82939\end{bmatrix},\qquad
\mathbf x_-=\begin{bmatrix}.38461\\.98953\\.82939\end{bmatrix}
\]
\(\mathbf x_+\)\((\theta_1,\theta_2)=(-77.480^\circ,-59.757^\circ)\)
\(\mathbf x_-\)\((\theta_1,\theta_2)=(-35.000^\circ,-10.000^\circ)\)
This branch contains the original generating configuration and a second configuration with the same tool position.
Geometric 3R IK · Branches
Four position-IK solutions for custom_3R_new_0
Both PK3 intersections are valid. Each feeds PK2, whose two intermediate vectors produce two more branches.
two \(\theta_3\) values
two orbit–sphere intersections
→
two \(\mathbf x_\pm\) vectors
two elbow branches
→
\(2\times2=4\)
all reach \(\mathbf p_d\)
| solution | \(\theta_1\) | \(\theta_2\) | \(\theta_3\) |
|---|---|---|---|
| 1 | \(-144.280^\circ\) | \(-108.143^\circ\) | \(-125.989^\circ\) |
| 2 | \(31.800^\circ\) | \(-4.172^\circ\) | \(-125.989^\circ\) |
| 3 | \(-77.480^\circ\) | \(-59.757^\circ\) | \(-170.000^\circ\) |
| 4 | \(-35.000^\circ\) | \(-10.000^\circ\) | \(-170.000^\circ\) |
Forward kinematics of every row gives \(\mathbf p=(.882624,.589970,1.829387)\,\mathrm m\) within numerical tolerance.
Synthesis
Algebraic and geometric IK: a comparison
Both approaches eliminate rotations through invariants, but they expose solution structure in different mathematical representations.
D–H ordered elimination
- generic \(a_1\neq0,\ s_{\alpha1}\neq0\);
- invariants eliminate \(\theta_1,\theta_2\);
- a quartic enumerates \(\theta_3\);
- backpropagate with \(\operatorname{atan2}\).
Paden–Kahan decomposition
- exploits \(a_1=0\), intersecting axes;
- fixed points cancel joint motions;
- circle intersections expose feasibility;
- PK3 → PK2, with two branches at each stage.
Common principle: eliminate a rotation with something it cannot change—distance, axial projection, or a point on its axis.
Takeaways
Structural principles of analytical 3R inverse kinematics
Radial invariance, norm preservation, angle back-substitution, and fixed-point geometry provide the main analytical tools.
1
Shift \(z=z_e-d_1\), then use \(R=x^2+y^2+z^2\) to eliminate \(\theta_1\).
2
Write \(R\) and \(z\) as coefficient equations in \(c_2,s_2\); squaring and adding eliminates \(\theta_2\).
3
Backpropagation subtracts polar angles with \(\operatorname{atan2}\).
4
For \(a_1=0\), position IK gives PK3 → PK2 and up to four branches.
Further reading: R. Murray, Z. Li & S. Sastry, A Mathematical Introduction to Robotic Manipulation, §3.3.
Advanced formulation · geometric preview
Inverse kinematics as
circle–torus intersection
The IK model through conformal geometric algebra lens.
e^{\theta_1S_1}e^{\theta_2S_2}e^{\theta_3S_3}P=P_d
\[
\quad\Longleftrightarrow\quad
X\in C_B\cap C_A(\theta_2)
\]
Geometric construction adapted from Nayak & Salunkhe, Inverse kinematic solution for generic 3R positional robots using Conformal Geometric Algebra.
CGA view · the two circles
Start with one fixed circle and one moving circle
Both are ordinary Euclidean circles; CGA makes the incidence and rigid-motion operations algebraically uniform.
fixed \(C_B\)passes through \(p_d\), is centered on axis 1, and lies in a plane normal to \(\boldsymbol\omega_1\)
home \(C_A\)passes through the home tool point \(p\), is centered on axis 3, and lies in a plane normal to \(\boldsymbol\omega_3\)
\[
C_B=\operatorname{Orb}_{\omega_1}(p_d),\qquad
C_A=\operatorname{Orb}_{\omega_3}(p)
\]
The target circle contains every point that joint 1 can rotate into \(p_d\); the home circle contains every point joint 3 can reach from \(p\).
CGA view · pose the IK problem
Rotate the home circle about axis 2
Joint 2 carries the entire joint-3 orbit. Watch only \(C_A\): a red sphere appears whenever it intersects the fixed target circle.
\[
C_A(\theta_2)=\mathcal R_2(\theta_2)\,C_A\,\widetilde{\mathcal R}_2(\theta_2)
\]
\[
\boxed{X(\theta_2)=C_A(\theta_2)\vee C_B}
\]
The meet \(\vee\) returns their candidate point pair. A real null point satisfies \(X\cdot X=0\).
No torus is drawn yet: this slide isolates the moving circle and marks each real circle–circle intersection as it occurs.
CGA view · choose a branch
Choose an intersection, then rotate \(C_A\) to it
Raising \(p_d\) by \(1\,\mathrm m\) leaves two real branches. Use the circle selector in the visualization to replay either one.
| branch | \(\theta_2\) |
|---|---|
| 1 | \(-46.888^\circ\) |
| 2 | \(-132.273^\circ\) |
\[
X_i=R_1(-\theta_{1i})p_d
=R_2(\theta_{2i})R_3(\theta_{3i})p
\]
Selecting a branch resets \(C_A\) to home and animates it to the corresponding point \(x_i\in C_B\).
CGA view · recover \(\theta_3\)
Undo joint 2, then measure an arc on \(C_A\)
Start from either selected IK circle already positioned at its intersection with \(C_B\).
1
Take its intersection point \(x\in C_B\cap C_A(\theta_2)\).
2
Reverse the circle rotation: \(a=R_2(-\theta_2)x\in C_A\).
3
Solve \(R_3(\theta_3)p=a\): branch 1 gives \(\theta_3=-97.515^\circ\); branch 2 gives \(-39.698^\circ\).
\[R_3(\theta_3)p=R_2(-\theta_2)x\]
The circle and its intersection point move together under \(-\theta_2\); only after they reach home does the joint-3 rotation begin.
CGA view · complete one branch
Reach the intersection, then travel along \(C_B\)
Forward motion makes the geometric factorization visible in the same order as the product of rotations.
\[
p\xrightarrow{\ \theta_3\ }R_3p
\xrightarrow{\ \theta_2\ }x
\xrightarrow{\ \theta_1\ }p_d
\]
joint 3moves \(p\) around the home circle \(C_A\)
joint 2carries that point and circle to the intersection \(x\)
joint 1keeps the point on \(C_B\) and rotates \(x\) to \(p_d\)
\[R_1(8.561^\circ)R_2(-46.888^\circ)R_3(-97.515^\circ)p=p_d\]
The final joint-1 motion remains on the fixed orange circle by construction, so it ends exactly at \(p_d\).
CGA view · interactive construction
Choose \(p_d\), then build an IK branch by dragging
Use the six sliders in order. The target controls rebuild \(C_B\); the joint controls reproduce the circle construction.
1
Set \(p_x,p_y,p_z\) to place \(p_d\) and rebuild \(C_B\).
2
Drag \(\theta_2\). Within \(4^\circ\) of a real solution, the circle snaps to the intersection.
3
Drag \(\theta_2\) back toward zero; the latched point travels with \(C_A\).
4
Adjust \(\theta_3\) to meet that point, then \(\theta_1\) to reach \(p_d\).
Some target positions have fewer real branches—or none. The status line reports the current count.
The sliders expose the same factorization as the analytical solve: circle intersection → reverse joint 2 → joint 3 arc → joint 1 arc.
CGA view · global solution picture
The swept circle is a torus; crossings are IK branches
Let \(\theta_2\) span one revolution. For the raised target, \(C_B\) cuts the swept surface at two real branch points.
\[
\begin{aligned}
\mathcal T&=\bigcup_{\theta_2\in[-\pi,\pi)}C_A(\theta_2),\\
C_B\cap\mathcal T&=\{X_1,X_2\}.
\end{aligned}
\]
what CGA addspoints, spheres, planes, circles, meets, and rotors share one algebraic language
scopethe full derivation and quartic expansion are intentionally deferred
For Euclidean 3-space, the paper uses the five-dimensional conformal algebra \(\mathcal G_{4,1}\), i.e. signature \((4,1,0)\).
The point of this preview: the algebra may be advanced, but its geometry is beautifully direct—IK is an intersection problem.