M.E. 530.

646
UR5 Inverse Kinematics

Ryan Keating
Johns Hopkins University

Introduction
Figure 1 below illustrates a common assignment of Denavit-Hartenberg convention to the
UR5 robot (shown with all joint angles at 0).

d4
a2 a3
y4
x4

z4
y1 y2 y3
z1 y6
z2 z3
z6
x1 x2 x3 x6

d1 y5

d5
z5 d6
z0 x5

x0
y0

Figure 1: D-H Convention Frame Assignment

Joint a α d θ
1 0 π/2 0.089159 θ1
2 −0.425 0 0 θ2
3 −0.39225 0 0 θ3
4 0 π/2 0.10915 θ4
5 0 −π/2 0.09465 θ5
6 0 0 0.0823 θ6

Table 1: D-H Parameters

1
As with any 6-DOF robot, the homogeneous transformation from the base frame to the
gripper can be defined as follows:

T60 (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = T10 (θ1 ) T21 (θ2 ) T32 (θ3 ) T43 (θ4 ) T54 (θ5 ) T65 (θ6 ) (1)

Also remember that a homogenous transformation Tji has the following form:

i
 
x x y x zx (P j )x
P~ji
 
Rji  xy yy zy (Pji )y 
Tji = =
 xz yz zz (Pji )z 
 (2)
0 1
0 0 0 1

where P~ji is the translation from frame i to frame j and each column of Rji is the projection
of one of the axes of frame j onto the axes of frame i (i.e. [xx , xy , xz ]T ≡ xij ).
.[?]

Inverse Kinematics
The first step to solving the inverse kinematics is to find θ1 . To do so, we must first find the
location of the 5th coordinate frame with respect to the base frame, P50 . As illustrated in
Figure 2 below, we can do so by translating by d6 in the negative z direction from the 6th
frame.

z6

y6

x6
z5
d6

z0

y0

x0

Figure 2: Finding the Origin of the 5th Frame

2
This is equivalent to the following operation:
   
0 0
0   0
P~50 = T60 
   
 −d6  −  0 (3)


1 1

With the location of the 5th frame, we can draw an overhead view of the robot:

z5

d4

z3

z2

y0
z1

θ1 ψ
x0

x1

Figure 3: Finding θ1
π
From Figure 3, we can see that θ1 = ψ + φ + 2
where

ψ = atan2 (P50 )y , (P50 )x

(4)

  !
d4 d4
φ = ± arccos = ± arccos (5)
(P50 )xy
p
(P5 )x + (P50 )y 2
0 2

The two solutions for θ1 correspond to the shoulder being either “left” or “right,”. Note that
Equation 5 has a solution in all cases except that d4 > (P50 )xy . You can see from Figure 3

3
this happens when the origin of the 3rd frame is close to the z axis of frame 0. This forms an
unreachable cylinder in the otherwise spherical workspace of the UR5 (as shown in Figure 4
taken from the UR5 manual).

Figure 4: The Workspace of the UR5

Knowing θ1 , we can now solve for θ5 . Once again we draw an overhead view of the robot,
but this time we consider location of the 6th frame with respect to the 1st (Figure 5.
We can see that (P61 )z = d6 cos(θ5 ) + d4 , where (P61 )z = (P60 )x sin(θ1 ) − (P60 )y cos(θ1 ). Solving
for θ5 ,

(P61 )z − d4
 
θ5 = ± arccos (6)
d6

Once again, there are two solutions. These solutions correspond to the wrist being “down”
and “up.”
Figure 6 illustrates that, ignoring translations between frames, z1 can be represented with
respect to frame 6 as a unit vector defined with spherical coordinates. We can find the x
and y components of of this vector by projecting it onto the x-y plane and then onto the x
or y axes.
Next we find transformation from frame 6 to frame 1,

T16 = ((T10 )−1 T60 )−1 (7)

Remembering the structure the of the first three columns of the homogenous transformation
T16 (see Equation 2), we can form the following equalities:

− sin(θ6 ) sin(θ5 ) = zy (8)

cos(θ6 ) sin(θ5 ) = zx (9)

4
 z6

d6c5

z5
y4

x5
θ5

x4 d4

z3

z2

y0
z1

θ1
x0

x1

Figure 5: Finding θ5

Solving for θ6 ,
 
−zy zx
θ6 = atan2 , (10)
sin(θ5 ) sin(θ5 )

Equation 10 shows that θ6 is not well-defined when sin(θ5 ) = 0 or when zx , zy = 0. We can

see from Figure 6 that these conditions are actually the same. In this configuration joints 2,
3, 4, and 6 are parallel. As a result, there four degrees of freedom to determine the position
and rotation of the end-effector in the plane, resulting in an infinite number of solutions. In
this case, a desired value for q6 can be chosen to reduce the number of degrees of freedom
to three.
We can now consider the three remaining joints as forming a planar 3R manipulator. First
we will find the location of frame 3 with respect to frame 1. This is done as follows:

T41 = T61 T46 = T61 (T54 T65 )−1 (11)

5
 y4,z1
z5,z6,y4 z5,z6 z5,z6
y4
θ5 θ5
c5

z4 y5,y6 z4 y5,y6 y6

x5,x6 x6 -θ6 c6s5

x4,x5,x6 θ5
x4
-s6s5

Figure 6: Finding θ6

  
0 0
−d4   0
P~31 = T41 
   
 0 − 0 (12)


1 1

Now we can draw the plane containing frames 1-3, as shown in Figure 7. We can see
that

||P~31 ||2 − a22 − a23

cos(ξ) = (13)
2a2 a3
with use of the law of cosines.

cos(ξ) = − cos(π − ξ)
= − cos(−θ3 ) (14)
= cos(θ3 )

Combining 13 and 14 !
||P~31 ||2 − a22 − a23
θ3 = ± arccos (15)
2a2 a3

Equation 15 has a solution as long as the argument to arccos is ∈ [−1, 1]. We can see that
large values of ||P~31 || will cause this the argument to exceed 1. The physical interpretation
here is that the robot can only reach so far out in any direction (creating the spherical bound
to the workspace as seen in 4).
Figure 7 also shows that
θ2 = −(δ − ) (16)

where δ = atan2((P31 )y , −(P31 )x ) and  can be found via law of sines:

sin(ξ) sin()
= (17)
||P~1 ||
3
a3

6
Combining 16 and 17
!
a3 sin(θ3 )
θ2 = − atan2((P31 )y , −(P31 )x ) + arcsin (18)
||P~1 ||
3

x3

a3

y2 z2

-θ3

x2
y1
z1 a2
δ
-θ2
x1

Figure 7: Finding θ2 and θ3

Notice that there are two solutions for θ2 and θ3 . These solutions are known as “elbow up”
and “elbow down.”
The final step to solving the inverse kinematics is to solve for θ4 . First we want to find
T43 :

T43 = T13 T41 = (T21 T32 )−1 T41 (19)

Using the first column of T43 ,

θ4 = atan2(xy , xx ) (20)

Below are figures which show the eight solutions for one end-effector position/orientation.

This solution was adapted from an existing solution written by Kelsey P. Hawkins

7
 (a) Shoulder Left, Elbow Up, Wrist Down (b) Shoulder Left, Elbow Up, Wrist Up

(c) Shoulder Left, Elbow Down, Wrist Down (d) Shoulder Left, Elbow Down, Wrist Up

(e) Shoulder Right, Elbow Down, Wrist Up (f) Shoulder Right, Elbow Down, Wrist Down

(g) Shoulder Right, Elbow Up, Wrist Up (h) Shoulder Right, Elbow Up, Wrist Down