Puma 560

Degrees of Freedom
Redundancy
The Puma 560 can reach the same
position in the interior of its workspace
with eight different configurations.

𝜃4′ = 𝜃4 + 180∘
𝜃5′ = −𝜃5
𝜃6′ = 𝜃6 + 180∘
 IK Problem
Problem:
What are the joint angles (𝜃1 , … , 𝜃6 ) as a function of wrist
position and orientation (when 𝑇60 is given as numeric values)?

𝑟11 𝑟12 𝑟13 𝑝𝑥

𝑟21 𝑟22 𝑟23 𝑝𝑦
𝑇60 = 𝑇10 𝜃1 𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6 =
𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1

Forward kinematics Goal

12 equations, 6 unknowns
 Solution (General Technique)
• Solution (General Technique): Multiply each side of the direct
kinematics equation by an inverse transformation matrix to
separate out variables in search of a solvable equation.

• We will solve the inverse kinematics with the following equations

in order from top to bottom. Green indicates the variables solved
in each step; red indicates identity matrices.

• In the first step, we move all dependencies on 𝜃1 to the LHS.

𝐼
(𝑇10 𝜃1 )−1 𝑇60 = (𝑇10 𝜃1 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

(𝑇30 𝜃1 , 𝜃2 , 𝜃3 )−1 𝑇60 = (𝑇30 𝜃1 , 𝜃2 , 𝜃3 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

(𝑇40 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 )−1 𝑇60 = (𝑇40 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

(𝑇50 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5 )−1 𝑇60 = (𝑇50 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6
Intermediate Transformations
(𝑇10 𝜃1 )−1 𝑇60 = (𝑇10 𝜃1 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

𝑐1 −𝑠1 0 0 𝑐4 −𝑠4 0 𝑎3
𝑠 𝑐1 0 0 0 0 1 𝑑4
𝑇10 = 1 𝑇43 =
0 0 1 0 −𝑠4 −𝑐4 0 0
0 0 0 1 0 0 0 1

𝑐2 −𝑠2 0 0 𝑐5 −𝑠5 0 0
0 0 1 0 0 0 −1 0
𝑇21 = 𝑇54 =
−𝑠2 −𝑐2 0 0 −𝑠5 −𝑐5 0 0
0 0 0 1 0 0 0 1

𝑐3 −𝑠3 0 𝑎2 𝑐6 −𝑠6 0 0
𝑠 𝑐3 0 0 0 0 1 0
𝑇32 = 3 𝑇65 =
0 0 1 𝑑3 −𝑠6 −𝑐6 0 0
0 0 0 1 0 0 0 1
 First Equation
(𝑇10 𝜃1 )−1 𝑇60 = (𝑇10 𝜃1 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

𝐼 𝑇61

𝑐1 𝑠1 0 0 𝑟11 𝑟12 𝑟13 𝑝𝑥

−𝑠1 𝑐1 0 0 𝑟21 𝑟22 𝑟23 𝑝𝑦
LHS: (𝑇10 𝜃1 )−1 𝑇60 =
0 0 1 0 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1

⋅ ⋅ ⋅ 𝑝𝑥 16 𝑝𝑥 16 = 𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23
⋅ ⋅ ⋅ 𝑝𝑦 1 𝑝𝑦 1 = 𝑑3
RHS: 𝑇61 = 6 6
⋅ ⋅ ⋅ 𝑝𝑧 16 𝑝𝑧 16 = −𝑎2 𝑠2 − 𝑎3 𝑠23 − 𝑑4 𝑐23
0 0 0 1
 Solving for 𝜃1
• Equate the (2,4) elements of both sides:
−𝑠1 𝑝𝑥 + 𝑐1 𝑝𝑦 = 𝑑3

• Make a trigonometric substitution: 𝜙 = atan2(𝑝𝑦 , 𝑝𝑥 )

𝑝𝑥 = 𝜌 cos 𝜙
𝑝𝑦 = 𝜌 sin 𝜙 𝜌= 𝑝𝑥2 + 𝑝𝑦2

• Substituting back into the original equation:

−𝑠1 𝜌 cos 𝜙 + 𝑐1 𝜌 sin 𝜙 = 𝑑3

• Divide by 𝜌 and use the difference of angles formula:

−𝑠1 𝑐𝜙 + 𝑐1 𝑠𝜙 = sin 𝜙 − 𝜃1 = 𝑑3 /𝜌

• Find cos(𝜙 − 𝜃1 ) using sum of squares, and apply atan2:

2
𝑑3 𝑑3
𝜃1 = 𝜙 − atan2 ,± 1 − Note two possible
𝜌 𝜌 solutions
 Solving for 𝜃3
𝑐1 𝑠1 0 0 𝑟11 𝑟12 𝑟13 𝑝𝑥
−𝑠1 𝑐1 0 0 𝑟21 𝑟22 𝑟23 𝑝𝑦
LHS: (𝑇10 𝜃1 )−1 𝑇60 =
0 0 1 0 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1

⋅ ⋅ ⋅ 𝑝𝑥 16 𝑝𝑥 16 = 𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23
⋅ ⋅ ⋅ 𝑝𝑦 1 𝑝𝑦 1 = 𝑑3
RHS: 𝑇61 = 6 6
⋅ ⋅ ⋅ 𝑝𝑧 16 1
𝑝𝑧 6 = −𝑎2 𝑠2 − 𝑎3 𝑠23 − 𝑑4 𝑐23
0 0 0 1

𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦 = 𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23
−𝑠1 𝑝𝑥 + 𝑐1 𝑝𝑦 = 𝑑3
−𝑝𝑧 = 𝑎2 𝑠2 + 𝑎3 𝑠23 + 𝑑4 𝑐23
 Solving for 𝜃3
𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦 = 𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23
−𝑠1 𝑝𝑥 + 𝑐1 𝑝𝑦 = 𝑑3
−𝑝𝑧 = 𝑎2 𝑠2 + 𝑎3 𝑠23 + 𝑑4 𝑐23

• Square all three equations and add them:

𝑝𝑥2 + 𝑝𝑦2 + 𝑝𝑧2 − 𝑎22 − 𝑎32 − 𝑑32 − 𝑑42
𝑎3 𝑐3 − 𝑑4 𝑠3 = =𝐾
2𝑎2

• Everything on the RHS is known! Moreover, this equation is in

the same form as that for 𝜃1 , so we can solve it using a similar
trigonometric substitution followed by atan2:

𝜃3 = atan2 𝑎3 , 𝑑4 − atan2 𝐾, ± 𝑎32 + 𝑑42 − 𝐾 2

Note two possible
solutions
 Second Equation
(𝑇30 𝜃1 , 𝜃2 , 𝜃3 )−1 𝑇60 = (𝑇30 𝜃1 , 𝜃2 , 𝜃3 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

𝐼 𝑇63

𝑐1 𝑐23 𝑠1 𝑐23 −𝑠23 −𝑎2 𝑐3 𝑟11 𝑟12 𝑟13 𝑝𝑥

−𝑐1 𝑠23 −𝑠1 𝑠23 −𝑐23 𝑎2 𝑠3 𝑟21 𝑟22 𝑟23 𝑝𝑦
LHS: (𝑇30 𝜃1 )−1 𝑇60 =
−𝑠1 𝑐1 0 −𝑑3 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1

𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 −𝑐4 𝑐5 𝑠6 − 𝑠4 𝑐6 −𝑐4 𝑠5 𝑎3
𝑠5 𝑐6 −𝑠5 𝑠6 𝑐5 𝑑4
RHS: 𝑇63 =
−𝑠4 𝑐5 𝑐6 − 𝑐4 𝑠6 𝑠4 𝑐5 𝑠6 − 𝑐4 𝑐6 𝑠4 𝑠5 0
0 0 0 1

𝑐1 𝑐23 𝑝𝑥 + 𝑠1 𝑐23 𝑝𝑦 − 𝑠23 𝑝𝑧 − 𝑎2 𝑐3 = 𝑎3

−𝑐1 𝑠23 𝑝𝑥 − 𝑠1 𝑠23 𝑝𝑦 − 𝑐23 𝑝𝑧 + 𝑎2 𝑠3 = 𝑑4
 Solving for 𝜃2
𝑐1 𝑐23 𝑝𝑥 + 𝑠1 𝑐23 𝑝𝑦 − 𝑠23 𝑝𝑧 − 𝑎2 𝑐3 = 𝑎3
−𝑐1 𝑠23 𝑝𝑥 − 𝑠1 𝑠23 𝑝𝑦 − 𝑐23 𝑝𝑧 + 𝑎2 𝑠3 = 𝑑4

• Solve equations simultaneously for 𝑐23 and 𝑠23 :

𝑎2 𝑠3 − 𝑑4 𝑝𝑧 + (𝑎2 𝑐3 + 𝑎3 )(𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦 )
𝑐23 = 2
𝑝𝑧2 + 𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦
− 𝑎2 𝑐2 + 𝑎3 𝑝𝑧 + (𝑎2 𝑠3 − 𝑑4 )(𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦 )
𝑠23 = 2
𝑝𝑧2 + 𝑐1 𝑝𝑥 + 𝑠1 𝑝𝑦

• All quantities on RHS are known, so we can use atan2:

𝜃2 = atan2 𝑠23 , 𝑐23 − 𝜃3

Note four possible solutions
from combinations of 𝜃1 and 𝜃3
 Solving for 𝜃4
𝑐1 𝑐23 𝑠1 𝑐23 −𝑠23 −𝑎2 𝑐3 𝑟11 𝑟12 𝑟13 𝑝𝑥
−𝑐1 𝑠23 −𝑠1 𝑠23 −𝑐23 𝑎2 𝑠3 𝑟21 𝑟22 𝑟23 𝑝𝑦
LHS: (𝑇30 𝜃1 )−1 𝑇60 =
−𝑠1 𝑐1 0 −𝑑3 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1

𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 −𝑐4 𝑐5 𝑠6 − 𝑠4 𝑐6 −𝑐4 𝑠5 𝑎3
𝑠5 𝑐6 −𝑠5 𝑠6 𝑐5 𝑑4
RHS: 𝑇63 =
−𝑠4 𝑐5 𝑐6 − 𝑐4 𝑠6 𝑠4 𝑐5 𝑠6 − 𝑐4 𝑐6 𝑠4 𝑠5 0
0 0 0 1

𝑟13 𝑐1 𝑐23 + 𝑟23 𝑠1 𝑐23 − 𝑟33 𝑠23 = −𝑐4 𝑠5

−𝑟13 𝑠1 + 𝑟23 𝑐1 = 𝑠4 𝑠5

𝜃4 = atan2(−𝑟13 𝑠1 + 𝑟23 𝑐1 , −𝑟13 𝑐1 𝑐23 − 𝑟23 𝑠1 𝑐23 + 𝑟33 𝑠23 )

Can also take 𝜃4 + 180∘ if 𝑠5 is opposite sign
 Singular Configuration
• Our solution for 𝜃4 assumes that 𝜃5 ≠ 0.
• What happens if this is not the case?

• When 𝜃5 = 0, then the manipulator is in a singular configuration, where the

joint axes 4 and 6 line up.
• Both joints cause the same motion at the last link of the robot.
• In other words, we lose a degree of freedom.

• All that can be solved for in this case is the sum or difference of 𝜃4 and 𝜃6 .
• To avoid this, check whether both arguments of atan2 are near 0.
 Solving for 𝜃5
(𝑇40 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 )−1 𝑇60 = (𝑇40 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

𝐼 𝑇64
LHS:
𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 −𝑠23 𝑐4 −𝑎2 𝑐3 𝑐4 + 𝑑3 𝑠4 − 𝑎3 𝑐4 𝑟11 𝑟12 𝑟13 𝑝𝑥
−𝑐1 𝑐23 𝑠4 + 𝑠1 𝑐4 −𝑠1 𝑐23 𝑠4 − 𝑐1 𝑐4 𝑠23 𝑠4 𝑎2 𝑐3 𝑠4 + 𝑑3 𝑐4 + 𝑎3 𝑠4 𝑟21 𝑟22 𝑟23 𝑝𝑦
−𝑐1 𝑠23 −𝑠1 𝑠23 −𝑐23 𝑎2 𝑠3 − 𝑑4 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1
𝑐5 𝑐6 −𝑐5 𝑠6 −𝑠5 0
𝑠6 𝑐6 0 0
RHS: 𝑇64 =
𝑠5 𝑐6 −𝑠5 𝑠6 𝑐5 0
0 0 0 1

𝑟13 𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 + 𝑟23 𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 − 𝑟33 𝑠23 𝑐4 = −𝑠5

𝑟13 𝑐1 𝑠23 + 𝑟23 𝑠1 𝑠23 + 𝑟33 𝑐23 = −𝑐5

𝜃5 = atan2(𝑠5 , 𝑐5 )
 Solving for 𝜃6
(𝑇50 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5 )−1 𝑇60 = (𝑇50 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 , 𝜃5 )−1 𝑇10 (𝜃1 )𝑇21 𝜃2 𝑇32 𝜃3 𝑇43 𝜃4 𝑇54 𝜃5 𝑇65 𝜃6

• All variables on LHS are known. Pick two equations:

𝑟11 𝑠1 𝑐4 − 𝑐1 𝑐23 𝑠4 − 𝑟21 𝑐1 𝑐4 + 𝑠1 𝑐23 𝑠4 + 𝑟31 𝑠23 𝑠4 = 𝑠6

𝑟11 𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 𝑐5 − 𝑐1 𝑠23 𝑠5 + 𝑟21 𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 𝑐5 − 𝑠1 𝑠23 𝑠5 − 𝑟31 (𝑠23 𝑐4 𝑐5 + 𝑐23 𝑠5 ) = 𝑐6

𝜃6 = atan2(𝑠6 , 𝑐6 )
 Number of Solutions
• Recall that we get 2 separate solutions for 𝜃1 and 2 for 𝜃3 , for a total
of 4 through all combinations.
2
𝑑3 𝑑3
𝜃1 = atan2(𝑝𝑦 , 𝑝𝑥 ) − atan2 ,± 1 −
𝜌 𝜌

𝜃3 = atan2 𝑎3 , 𝑑4 − atan2 𝐾, ± 𝑎32 + 𝑑42 − 𝐾 2

• For each of the 4 solutions, the last three DOF can be flipped.
𝜃4′ = 𝜃4 + 180∘
𝜃5′ = −𝜃5
𝜃6′ = 𝜃6 + 180∘

• There are thus a total of 8 possible solutions.

 Number of Solutions
• After all eight solutions have been computed,
some or all of them may have to be discarded
because of joint limit violations.

• Of the remaining valid solutions, usually the

one closest to the present manipulator
configuration is chosen.