Inverse Kinematics I

Craig Carignan
Glen Henshaw
October 15, 2023

1 Overview
Inverse kinematics is the problem of starting with a desired end effector pose
0
N T and finding the joint angles q that cause the end effector to assume that
pose. It is often the case that you will have not a single desired pose but a
Cartesian curve or trajectory 0N T (t) and will want to find a trajectory in joint
space q(t) that causes the end effector to follow the trajectory.
Note that while describing translational trajectories is pretty straightfor-
ward, using cubic splines or something equivalent, describing trajectories in ro-
tation is a bit more complicated. It isn’t that hard to use Euler of fixed angles,
as long as you avoid the orientations where these descriptions become singular,
but the fact that they do become singular at certain orientations means that
4–value descriptions such as quaternions is normally how it’s done in practice.
The math required to describe trajectories using quaternions is a bit beyond
this course. For the following we’ll assume that you have a single desired end
effector pose.

Inverse kinematics
RN RM
q x

Joint Space Task Space

(dim N ) Forward kinematics (dim M )

Formally, the problem is

0
GIVEN: NT
FIND: q

where 
 6 independent equations
0 0
N R, pN ⇒ nonlinear
transcendental


1
2 Workspace
The workspace is the Cartesian position and orientation subspace spanned by
the manipulator, e.g. the set of all {x, y, z, ϕ, θ, ψ} such that ∃ q : B
T T (q) =
B
T T (x, y, z, ϕ, θ, ψ):

y
5 For N ≤ 6, workspace dim = N
For N > 6, workspace dim = 6
4

3
l2
2
l1
1

0
−5 −4 −3 −2 −1 0 1 2 3 4 5 x
−1 if l < l
2 1

−2

−3
Reachable Volume of workspace
−4
Workspace reachable by tooltip

−5
subset Volume of workspace
Dexterous reachable by tooltip in
Workspace any orientation

3 Closed–form Solutions
In some case you can find a closed–form solution for the inverse kinematics of a
manipulator.

3.1 Example: 3–link planar manipulator

For this particular manipulator we can find a close–form solution for θ1 , θ2 , θ3 .
In fact there are two ways to go about it, an algebraic approach and a geometric
approach. You can see the details of the geometric approach in section 4.4 of
the textbook. I’ll do the algebraic one here.
Set 03 T = 0W Tgoal ...

2
 y
5

0
0 1 2 3 x

Figure 1: Three–link planar manipulator

 
c123 −s123 0 l1 c1 + l2 c12
0
 s123 c123 0 l1 s1 + l2 s12 
3T = 
 
0 0 1 0 
0 0 0 1
and  
cϕ −sϕ 0 x
B s
 ϕ
 cϕ 0 y 
WT =  0

0 1 0 
0 0 0 1
So we can, by inspection, get four equations that have to be solved for θ1 , θ2 , θ3 :

cϕ = c123 (1)
sϕ = s123 (2)
x = l1 c1 + l2 c12 (3)
y = l1 s1 + l2 s12 (4)

We can square 3 and 4 and add them:

x2 + y 2 = l12 + l22 + 2l1 l2 c2

and solve for c2 :

x2 + y 2 − l12 − l22
c2 =
2l1 l2
p
You can then get s2 = ± 1 − c22 and then θ2 = atan2(s2 , c2 ).
We can then solve for θ1 . This is a bit involved, but it’s a pattern that occurs
a lot in inverse kinematics.

3
 First, substitute k1 = l1 + l2 c2 and k2 = l2 s2 into Eqs. 1 and 2. This makes
the structure of the equations a bit clearer:

x = k1 c1 − k2 s1 (5)
y = k1 s1 + k2 c1 (6)

Now we can use a change of variables. Specifically, we’re going to change from
Cartesian to polar coordinates, as shown in the diagram:

1
y
k1

r
k2
γ
0
x
0 1
p
Define r = k12 + k22 and γ = atan2(k2 , k1 ). Then

k1 = r cos γ (7)
k2 = r sin γ (8)

We can now write Eqs. 5 and 6 as

x
= cos γ cos θ1 − sin γ sin θ1
r
y
= cos γ sin θ1 + sin γ cos θ1
r
or
x
cos γ + θ1 = (9)
r
y
sin γ + θ1 = (10)
r
We can then find
y x
γ + θ1 = atan2( , ) = atan2(y, x)
r r
Therefore
θ1 = atan2(y, x) − atan2(k2 , k1 )
And then using Eqs. 1 and 2 you can solve for θ3 :

θ1 + θ2 + θ3 = atan2(sϕ , cϕ ) = ϕ → θ3 = ϕ − θ1 − θ2 (11)

Note that there are in fact two solutions to the inverse kinematics problem
in this case, as illustrated in the diagram below:

4
 y
5

0
−1 0 1 2 3 4 x

And you canp also see this by virtue of the fact that in the derivation above
we chose s2 = ± 1 − c22 . You get to choose the sign — one choice corresponds
to the elbow–up configuration and the other to the elbow–down configuration.
Closed–form solutions are great when you can find them. They are numer-
ically stable (they always return exactly the same result, to the limit of the
precision of the computer you are using) and can be calculated quickly. Un-
fortunately, in general there is no straightforward way to find a closed–form
expression for the kinematics of a given robotic manipulator. There isn’t even
a guarantee that a closed–form solution exists.

3.2 Pieper’s Method

There is, however, one important case in which a closed–form solution can be
found: when
• you have a 6 DOF arm; and
• three consecutive axes intersect at a point (usually the first three or the
last three).
This requirement means that the problem can be decomposed into two successive
3–DOF problems. By far the most common case is a revolute manipulator where
the last three rotational axes intersect; this is sometimes known as a spherical
wrist. It implies that the point of intersection is at the origin of all three frames.
This technique is called “Pieper’s Method”.
The key observation is that 0 p4 = 0 p5 = 0 p6 .
Steps:

• Solve for 0 p4 :
0
0
pT = 0 p4 + 04 R ↗
4
p6 + 06 R 6 pT
|{z} |{z}
= 0T R given

5
• Solve for θ1 , θ2 , and θ3 :
0
p4 = 01 T (θ1 ) 12 T (θ2 ) 23 T (θ3 ) 3 p4
|{z}
given

Note that this gives three equations in three unknowns.

• Solve for θ4 , θ5 , and θ6 :
0 0 3 6
TR = 3R 6R T R
|{z} |{z} |{z}
for for =I
θ1 −θ3 θ4 −θ6

3
6R = 03 R(θ1 , θ2 , θ3 )T 0T R
Note that this also gives three equations in three unknowns.