May 31, 2018

Kinematics of a UR5
Rasmus Skovgaard Andersen
Aalborg University

Contents
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
2 Forward Kinematics for UR5 . . . . . . . . . . . . . . . . . . . . . . . . . . 2
3 Inverse Kinematics for UR5 . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
3.1 Finding θ1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
3.2 Finding θ5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.3 Finding θ6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.4 Finding θ3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.5 Finding θ2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.6 Finding θ4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
4.1 Additional Material . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
5 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1 Introduction
This worksheet describes how to derive the forward and inverse kinematic equations
of a UR5 robot. The worksheet is inspired by [Hawkins, 2013], [Keating, 2017], and
[Kebria et al., 2016] but attempts to explain each step more thoroughly.

1.1 Notation
The worksheet follows the Denavit-Hartenberg notation used by [Craig, 2005], some-
times referred to as modified DH-parameters. Additionally, the following brief nota-
tions are used:
0 
P 6x
• 0 P 6 = 0 P 6y  is the origin of frame 6 seen from frame 0.
 
0
P 6z
0 
Ŷ 6x
0 0 
• Ŷ 6 = 
 Ŷ 6y  is a unit vector giving the direction of the y-axis of frame 6 seen

0
Ŷ 6z
from frame 0.

1
 • 06 T is a transformation from frame 6 to frame 0, meaning that 0 P = 06 T · 6 P .

2 Forward Kinematics for UR5

The forward kinematic (FK) equations calculates a transformation matrix 06 T based on
known joint angles θ1−6 . The transformation matrix is defined as:
" #
0 0
0 6R P6
6 T (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) =
0 1
"0 0 0
#
X̂ 6 Ŷ 6 Ẑ 6 0 P 6
=
0 0 0 1
0 0 0 
X̂ 6x Ŷ 6x Ẑ 6x 0 P 6x
0
 X̂ 6y 0 Ŷ 6y 0 Ẑ 6y 0 P 6y 

=0 0 0
 (1)
 X̂ 6z Ŷ 6z Ẑ 6z 0 P 6z 


0 0 0 1

0 0 0
, where the columns X̂ 6 , Ŷ 6 , and Ẑ 6 are unit vectors defining the axes of frame 6
in relation to frame 0.
We can split the transformation matrix is given as a chain of transformations; one
for each joint:

0
6 T (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = 01 T (θ1 ) 12 T (θ2 ) 23 T (θ3 ) 34 T (θ4 ) 45 T (θ5 ) 56 T (θ6 ) (2)

The kinematic structure of the UR5 robot in zero position is shown in Figure 1:

−a 2
y2 z
1

x1,2 z2 −a 3 d4
y3 y4 z4
shoulder y6
z3 z6
y1
x3 x4
d1 elbow
z0 x6 tool
x0 d5 y5

base x5
y0 wrist d6

z5
(a) Frames describing the UR5 robot. (b) Visualization

Figure 1: The UR5 robot in zero position.

2
The DH-parameters are according to [Craig, 2005] specified as:

ai = distance from Zi to Zi+1 measured along Xi

αi = angle from Zi to Zi+1 measured about Xi
di = distance from Xi−1 to Xi measured along Zi
θi = angle from Xi−1 to Xi measured about Zi (3)

For the UR5, the DH parameters are:

i αi−1 ai−1 di θi
1 0 0 d1 θ1
2 α1 = 90◦ 0 0 θ2
3 0 a2 0 θ3
4 0 a3 d4 θ4
5 α4 = 90◦ 0 d5 θ5
6 α5 = −90◦ 0 d6 θ6

Table 1: Modified Denavit-Harteberg parameters (DH-parameters) of a UR5 robot, correspond-

ing to the frames in Figure 1. The parameters θi are variables and the remaining parameters
are constants.

The DH-parameters can be used to write the transformations for each link. The
general transformation between link i − 1 and i is given by1 :
 
cos θi − sin θi 0 ai−1
 
i-1
sin θi cos(αi−1 ) cos θi cos(αi−1 ) − sin(αi−1 ) − sin(αi−1 )di 
iT = 
  (4)
 sin θ i sin(α i−1 ) cos θ i sin(αi−1 ) cos(α i−1 ) cos(α i−1 )di


0 0 0 1

It is straightforward to write the transformation matrix for each link of the UR5
robot by inserting the DH-parameters from Table 1 in Equation (4). The complete
transformation from base to end-effector can then be derived by multiplication of
all 6 transformation matrices, as shown in Equation (2). The result is analytical
expressions for all 12 parameters in the transformation matrix 06 T . The complete
analytic equations can be found in [Hawkins, 2013].

3 Inverse Kinematics for UR5

The inverse kinematic (IK) equations calculates the joint angles θ1−6 based on a de-
sired position and orientation of the final frame, specified as the transformation 06 T .
In the solution, we restrict all angles as (θ1 , ..., θ6 ) ∈ [0; 2π[.
1
Equation 3.6 in [Craig, 2005]

3
3.1 Finding θ1
To find θ1 , we first determine the location of frame 5 (the wrist frame) in relation
to the base frame; 0 P 5 . As illustrated in Figure 2, 0 P 5 can be found by translating
backwards from frame 6 to frame 5 along z6 . Remember that both 06 T and d6 are
known.
y6
z6
x6

z0 0
x0 T
6 y5
x5
d6
y0 0
P5

z5

Figure 2: Finding the origin of frame 5.

The translation 0 P 5 can be written as:

0 0
P 5 = 0 P 6 − d6 · Ẑ 6 ⇔
 
0
 
0 0  0 
 
P 5 = 6T  (5)
−d6 

1

To derive θ1 , we examine the robot seen from above in Figure 3 (looking down into z0 ):

4
 y1
x1
0
P5 x x0 x1
θ1 x0
φ1 θ1
0
z2
P5 y
φ2
z3 y0 0
P5 xy y0
0
P5 xy

d4
z4
z5
(a) (b)

Figure 3: Robot (until frame 5) seen from above. The robot is shown as grey lines. Note that
z5 should actually point into the page if all angles are 0. Here θ4 6= 0 in order to better illustrate
how θ1 can be isolated.

Our approach to finding θ1 is to consider the wrist, P 5 , seen from frame 0 and
frame 1, respectively. Intuitively, the rotation from frame 0-to-1, θ1 , should equal the
difference between the rotations from 0-to-5 and 1-to-5. Formalizing and inserting
symbols from Figure 3 give:

v0→1 = v0→5 − v1→5 ⇔

v0→1 = v0→5 + v5→1 ⇔
 π
θ1 = φ 1 + φ 2 + (6)
2

The angle φ1 can be found by examining the triangle with sides 0 P 5x and 0 P 5y :

φ1 = atan2(0 P 5y , 0 P 5x ) (7)

The angle φ2 is found by examining the rightmost triangle with φ2 as one of the angles.
Two of the sides have lengths | 0 P 5xy | and d4 :

d4
cos(φ2 ) = 0
⇒
| P 5xy |
!
d4
φ2 = ±acos 0
⇔
| P 5xy |
 
d4
φ2 = ±acos  q  (8)
0 2 0 2
P 5x + P 5y

5
The desired angle θ1 can now be found simply as:
π
θ1 = φ 1 + φ 2 + ⇔
2  

0
 d4 + π
θ1 = atan2 P 5y , 0 P 5x ± acos  q (9)
0 2 2 2
P 5x + 0 P 5y

The two solutions correspond to the shoulder being “left” or “right”.

3.2 Finding θ5
Figure 4 shows the robot from above again; this time with frame 6 included.

y1 y1
x1 θ1
x0
θ1
− 1 P6 y

z3 y0
y0 − 1 P6 y

x4
θ5
d4 z4 x5
z5 x4 0
P6 y
0
θ5
d 6 cos ( θ5 ) P6 y
x5
P6
z6 0
P6 x P 0
P6 x
(a) (b)

Figure 4: Robot (including frame 6) seen from above.

Our approach to finding θ5 is to notice that 1 P 6y (the y-component of 1 P 6 ) only

depends on θ5 . In the figure, we can trace y1 backwards to see that 1 P 6y is given by:

− 1 P 6y = d4 + d6 cos θ5 (10)

The component 1 P 6y can also be expressed by looking at 1 P 6 as a rotation of 0 P 6

6
around z1 :

0
P 6 = 01 R · 1 P 6 ⇔
1
P 6 = 01 R> · 0 P 6 ⇔
1   > 0 
P 6x cos(θ1 ) − sin(θ1 ) 0 P 6x
1
 P 6y  =  sin(θ1 ) cos(θ1 ) 0 0 P 6y  ⇔
    
1 0
P 6z 0 0 1 P 6z
1    0 
P 6x cos(θ1 ) sin(θ1 ) 0 P 6x
1
 P 6y  = − sin(θ1 ) cos(θ1 ) 0 0 P 6y  ⇒
   
1 0
P 6z 0 0 1 P 6z
1
P 6y = 0 P 6x · (− sin θ1 ) + 0 P 6y · cos θ1 (11)

By combining Equation (10) and (11), we can eliminate 1 P 6y and express θ5 only using
known values:

−d4 − d6 cos θ5 = 0 P 6x (− sin θ1 ) + 0 P 6y cos θ1 ⇔

0
P 6x sin θ1 − 0 P 6y cos θ1 − d4
cos θ5 = ⇔
d6
!
0
P 6x sin θ1 − 0 P 6y cos θ1 − d4
θ5 = ±acos (12)
d6

Again there are two solutions. These correspond to the wrist being “up” or “down”,
respectively. This can be interpreted intuitively: The joint sum (θ2 + θ3 + θ4 ) can cause
the end-effector to be located in the same position, but with the wrist flipped. The
orientation can then be “corrected” by θ6 .
Also note that a solution is defined as long as the value inside acos has a magni-
tude not greater than 1; equivalent to | 1 P 6y − d4 | ≤ |d6 |.

3.3 Finding θ6
6
To determine θ6 we examine y1 seen from frame 6; Ŷ 1 . This axis will (ignoring trans-
6
lations) always be parallel to Ẑ 2,3,4 , as can be seen from Figure 5b. Therefore, it
6
will only depend on θ5 and θ6 . It turns out that − Ŷ 1 can in fact be described using
spherical coordinates, where azimuth is −θ6 and the polar angle is θ5 ; see Figure 5a.

7
 z6
z1 z2
z3 z4
y4 y6
θ5 x1 z6
‒y1=z4 x4
x6 ‒θ6
y1
y6
y5
x6 ‒θ6 x5
θ5
z5
6
(a) Polar angles for − Ŷ 1 (b) Reference view of the relevant frames

6
Figure 5: The axis − Ŷ 1 expressed in spherical coordinates with azimuth −θ6 and polar angle
6
θ5 . For simplicity, Ŷ 1 is denoted y1 in the Figure.

6
Converting − Ŷ 1 from spherical to Cartesian coordinates gives:
 
sin θ5 cos(−θ6 )
6
− Ŷ 1 =  sin θ5 sin(−θ6 )  ⇔
 

cos θ5
 
− sin θ5 cos θ6
6
Ŷ 1 =  sin θ5 sin θ6  (13)
 

− cos θ5

In Equation (13) we could isolate θ6 and have an expression of θ6 in relation to 61 T .

6
We want an expression of θ6 all the way from 60 T . To get this, we identify that Ŷ 1 is
given as a rotation of θ1 in the x, y-plane of frame 0 (very similar to Equation (11)):
6 6 6
Ŷ 1 = X̂ 0 · (− sin θ1 ) + Ŷ 0 · cos θ1 ⇔
 6 6

− X̂ 0x · sin θ1 + Ŷ 0x · cos θ1
6  6 6 
Ŷ 1 = 
− X̂ 0y · sin θ1 + Ŷ 0y · cos θ1 
 (14)
6 6
− X̂ 0z · sin θ1 + Ŷ 0z · cos θ1

Equating the first two entries of (13) and (14) give:

6 6 )
− sin θ5 cos θ6 = − X̂ 0x · sin θ1 + Ŷ 0x · cos θ1
6 6 ⇔ (15)
sin θ5 sin θ6 = − X̂ 0y · sin θ1 + Ŷ 0y · cos θ1
 6 6

 cos θ = X̂ 0x ·sin θ1 − Ŷ 0x ·cos θ1 
6 sin θ5
6 6 ⇒
 sin θ = − X̂ 0y ·sin θ1 + Ŷ 0y ·cos θ1 
6 sin θ5
6 6 6 6
!
− X̂ 0y · sin θ1 + Ŷ 0y · cos θ1 X̂ 0x · sin θ1 − Ŷ 0x · cos θ1
θ6 = atan2 , (16)
sin θ5 sin θ5

This solution is undetermined if the denominator sin θ5 = 0. In this case, the joint
axes 2, 3, 4 and 6 are aligned (as in Figure 5b). This is “too many” degrees of freedom.

8
The axes 2, 3, and 4 can on their own rotate the end-effector (frame 6) around z6
without moving it, and the 6’th joint therefore becomes redundant. In this case, θ6
can simply be set to an arbitrary value.
If both of the numerators in Equation (16) are 0, the solution is also undetermined.
If this is the case, sin θ5 must also be 0, and the situation is thus the same. This can
be seen by examining both sides in Equation (15).

3.4 Finding θ3
We examine the remaining three joints (2, 3, and 4). Notice that their joint axes are all
parallel. Together they constitute a planar 3R-manipulator, as illustrated in Figure 6.

z1
z1 y2
x2 y2 x2
− 1 P4 x
θ2 1
− P4 x
φ1 x1
x1 θ2 x3 x3 −1 P4 z
y3 1
− P4 z
φ2
−a 2
elbow
φ3 θ3
1
P4 xz 1
P4 xz
−a 3
x4 x4
y4

(a) (b)

Figure 6: Joint 2, 3, and 4 together constitutes a 3R planar manipulator.

We can constrict ourselves to look at 14 T (frame 4 in relation to frame 1) because

0 4 5
1 T , 5 T , and 6 T at this point are known. This transformation is illustrated in the
x, z-plane of frame 1 in Figure 6a. From the figure it is clear that the length of the
translation | 1 P 4xz | is determined only by θ3 , or similarly by φ3 . The angle φ3 can be
found by using the law of cosine:

(−a2 )2 + (−a3 )2 − | 1 P 4xz |2 a2 2 + a3 2 − | 1 P 4xz |2

cos φ3 = = (17)
2(−a2 )(−a3 ) 2a2 a3

The relationship between cos φ3 and cos θ3 is:

cos θ3 = cos(π − φ3 ) = − cos(φ3 ) (18)

9
Combining (17) and (18) give:

a2 2 + a3 2 − | 1 P 4xz |2
cos θ3 = − ⇔
2a2 a3
 1
| P 4xz |2 − a2 2 − a3 2

θ3 = ±acos (19)
2a2 a3

Note that solutions exist for θ3 if the argument of acos is within [−1; 1]. It can
be shown that this is equivalent to | 1 P 4xz | ∈ [|a2 − a3 |; |a2 + a3 |]. In most cases where
solutions exist, there will exist two different solutions. These correspond to “elbow
up” and “elbow down”.

3.5 Finding θ2
The angle θ2 can be found as φ1 − φ2 . Each of these can be found by inspecting Figure
6a and using atan2 and sine relations:

φ1 = atan2(− 1 P 4z , − 1 P 4x ) (20)
sin φ2 sin φ3
= 1 ⇔
−a3 | P 4xz |
 
−a3 sin φ3
φ2 = asin (21)
| 1 P 4xz |

We can replace φ3 with θ3 by noticing that sin φ3 = sin (180◦ − θ3 ) = sin θ3 . Combining
the equations now give:
 
1 1 −a3 sin θ3
θ2 = φ1 − φ2 = atan2(− P 4z , − P 4x ) − asin (22)
| 1 P 4xz |

3.6 Finding θ4
The last remaining angle θ4 is defined as the angle from X3 to X4 measured about Z4
(c.f. Equation (3) in page 3). It can thus easily be derived from the last remaining
3
transformation matrix, 34 T , using its first column X̂ 4 :
3 3
θ4 = atan2( X̂ 4y , X̂ 4x ) (23)

4 Discussion
To sum up, a total of 8 solutions exist in general for the general inverse kinematic
problem of the UR5: 2θ1 × 2θ5 × 1θ6 × 2θ3 × 1θ2 × 1θ4 .

4.1 Additional Material

Each of the sources used as inspiration for this document contain more information in
specific subjects. Most notably: [Hawkins, 2013], [Keating, 2017], and [Kebria et al., 2016]

• Full FK solution is included in [Hawkins, 2013].

10
• Dynamics is briefly covered in [Kebria et al., 2016].

• An extra axis is added in [Hawkins, 2013].

• Multiple IK solutions are visualized in [Keating, 2017]

11
5 References
[Craig, 2005] Craig, J. J. (2005). Introduction to robotics: mechanics and control, vol-
ume 3. Pearson/Prentice Hall Upper Saddle River, NJ, USA:.

[Hawkins, 2013] Hawkins, K. P. (2013). Analytic inverse kinematics for the univer-
sal robots ur-5/ur-10 arms. Technical report, Georgia Institute of Technology.
Available at: https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_-
kin_tech_report_1.pdf.

[Keating, 2017] Keating, R. (2017). Analytic inverse kinematics for the univer-
sal robots ur-5/ur-10 arms. Technical report, John Hopkins. Available at:
https://www.slideshare.net/RyanKeating13/ur5-ik.

[Kebria et al., 2016] Kebria, P. M., Al-Wais, S., Abdi, H., and Nahavandi, S. (2016).
Kinematic and dynamic modelling of ur5 manipulator. In Systems, Man, and Cyber-
netics (SMC), 2016 IEEE International Conference on, pages 004229–004234. IEEE.

12