May 31, 2018

Kinematics of a UR5

Rasmus Skovgaard AndersenAalborg University

Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 Forward Kinematics for UR5 . . . . . . . . . . . . . . . . . . . . . . . . . . 23 Inverse Kinematics for UR5 . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3.1 Finding θ1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43.2 Finding θ5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.3 Finding θ6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.4 Finding θ3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.5 Finding θ2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.6 Finding θ4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104.1 Additional Material . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

5 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1 Introduction

This worksheet describes how to derive the forward and inverse kinematic equationsof 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:

• 0P 6 =

0P 6x

0P 6y

0P 6z

is the origin of frame 6 seen from frame 0.

• 0Y 6 =

0Y 6x

0Y 6y

0Y 6z

is a unit vector giving the direction of the y-axis of frame 6 seen

from frame 0.

1

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

6T ·6P .

2 Forward Kinematics for UR5

The forward kinematic (FK) equations calculates a transformation matrix 06T based on

known joint angles θ1−6. The transformation matrix is defined as:

06T (θ1, θ2, θ3, θ4, θ5, θ6) =

[06R

0P 6

0 1

]

=

[0X6

0Y 6

0Z6

0P 6

0 0 0 1

]

=

0X6x

0Y 6x

0Z6x

0P 6x0X6y

0Y 6y

0Z6y

0P 6y0X6z

0Y 6z

0Z6z

0P 6z

0 0 0 1

(1)

, where the columns0X6,

0Y 6, and

0Z6 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:

06T (θ1, θ2, θ3, θ4, θ5, θ6) =

01T (θ1)

12T (θ2)

23T (θ3)

34T (θ4)

45T (θ5)

56T (θ6) (2)

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

z0

x0

y0

x1,2

x4

x5

d1

z2

y2

y1

z1

d5

d6

y3

y4

y5

z3

z5

z4y

3

x6

y6z6

tool

d4

x3

base

wrist

elbow

shoulder

−a2

−a3

(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 parametersare constants.

The DH-parameters can be used to write the transformations for each link. Thegeneral transformation between link i− 1 and i is given by1:

i-1iT =

cos θi − sin θi 0 ai−1

sin θi cos(αi−1) cos θi cos(αi−1) − sin(αi−1) − sin(αi−1)di

sin θi sin(αi−1) cos θi sin(αi−1) cos(αi−1) cos(αi−1)di

0 0 0 1

(4)

It is straightforward to write the transformation matrix for each link of the UR5robot by inserting the DH-parameters from Table 1 in Equation (4). The completetransformation from base to end-effector can then be derived by multiplication ofall 6 transformation matrices, as shown in Equation (2). The result is analyticalexpressions for all 12 parameters in the transformation matrix 0

6T . The completeanalytic 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 0

6T .In the solution, we restrict all angles as (θ1, ..., θ6) ∈ [0; 2π[.

1Equation 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 relationto the base frame; 0P 5. As illustrated in Figure 2, 0P 5 can be found by translatingbackwards from frame 6 to frame 5 along z6. Remember that both 0

6T and d6 areknown.

x5

d6

y5

z5

z0x

0

y0

x6

y6 z

6

T60

P50

Figure 2: Finding the origin of frame 5.

The translation 0P 5 can be written as:

0P 5 =0P 6 − d6 ·

0Z6 ⇔

0P 5 =06T

0

0

−d61

(5)

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

4

d4

x0

y0

P5 xy0

x1

z2

z3

z4

z5

φ1

φ2

θ1

P5 y0

P5 x0

y1

(a)

P5 xy0

x1

y0

x0 θ

1

(b)

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

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

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

v0→1 = v0→5 + v5→1 ⇔

θ1 = φ1 +(φ2 +

π

2

)(6)

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

φ1 = atan2(0P 5y,0P 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 | 0P 5xy| and d4:

cos(φ2) =d4

| 0P 5xy|⇒

φ2 = ±acos

(d4

| 0P 5xy|

)⇔

φ2 = ±acos

d4√0P 5x

2+ 0P 5y

2

(8)

5

The desired angle θ1 can now be found simply as:

θ1 = φ1 + φ2 +π

2⇔

θ1 = atan2(

0P 5y,0P 5x

)± acos

d4√0P 5x

2+ 0P 5y

2

+π

2(9)

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.

d6 cos (θ5)

d4

x0

y0

x1

z3

z4z

5

θ1

z6

x4

x5

y1

θ5

P6

P6 y0

P6 x0

− P6 y1

(a)

y1

y0

θ1

P P6 x0

P6 y0

− P6 y1

θ5

x5

x4

(b)

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

Our approach to finding θ5 is to notice that 1P 6y (the y-component of 1P 6) onlydepends on θ5. In the figure, we can trace y1 backwards to see that 1P 6y is given by:

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

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

6

around z1:

0P 6 =01R ·

1P 6 ⇔1P 6 =

01R> · 0P 6 ⇔

1P 6x

1P 6y

1P 6z

=

cos(θ1) − sin(θ1) 0

sin(θ1) cos(θ1) 0

0 0 1

>

0P 6x

0P 6y

0P 6z

⇔

1P 6x

1P 6y

1P 6z

=

cos(θ1) sin(θ1) 0

− sin(θ1) cos(θ1) 0

0 0 1

0P 6x

0P 6y

0P 6z

⇒1P 6y =

0P 6x · (− sin θ1) +0P 6y · cos θ1 (11)

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

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

cos θ5 =0P 6x sin θ1 − 0P 6y cos θ1 − d4

d6⇔

θ5 = ±acos

(0P 6x sin θ1 − 0P 6y cos θ1 − d4

d6

)(12)

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 causethe end-effector to be located in the same position, but with the wrist flipped. Theorientation 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 | 1P 6y − d4| ≤ |d6|.

3.3 Finding θ6

To determine θ6 we examine y1 seen from frame 6;6Y 1. This axis will (ignoring trans-

lations) always be parallel to6Z2,3,4, as can be seen from Figure 5b. Therefore, it

will only depend on θ5 and θ6. It turns out that − 6Y 1 can in fact be described using

spherical coordinates, where azimuth is −θ6 and the polar angle is θ5; see Figure 5a.

7

x6

y6

z6

‒y1=z

4

‒θ6

θ5

(a) Polar angles for − 6Y 1

x1

x4

y1

z1

y4

y5

z5

z4

x6

y6 z

6

x5

θ5

‒θ6

z2 z

3

(b) Reference view of the relevant frames

Figure 5: The axis −6Y 1 expressed in spherical coordinates with azimuth −θ6 and polar angle

θ5. For simplicity,6Y 1 is denoted y1 in the Figure.

Converting − 6Y 1 from spherical to Cartesian coordinates gives:

− 6Y 1 =

sin θ5 cos(−θ6)sin θ5 sin(−θ6)cos θ5

⇔6Y 1 =

− sin θ5 cos θ6

sin θ5 sin θ6

− cos θ5

(13)

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

We want an expression of θ6 all the way from 60T . To get this, we identify that

6Y 1 is

given as a rotation of θ1 in the x, y-plane of frame 0 (very similar to Equation (11)):

6Y 1 =

6X0 · (− sin θ1) +

6Y 0 · cos θ1 ⇔

6Y 1 =

− 6

X0x · sin θ1 +6Y 0x · cos θ1

− 6X0y · sin θ1 +

6Y 0y · cos θ1

− 6X0z · sin θ1 +

6Y 0z · cos θ1

(14)

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

− sin θ5 cos θ6= −6X0x · sin θ1 +

6Y 0x · cos θ1

sin θ5 sin θ6= −6X0y · sin θ1 +

6Y 0y · cos θ1

}⇔ (15) cos θ6=

6X0x·sin θ1−

6Y 0x·cos θ1

sin θ5

sin θ6=− 6

X0y ·sin θ1+6Y 0y ·cos θ1

sin θ5

⇒θ6 = atan2

(− 6

X0y · sin θ1 +6Y 0y · cos θ1

sin θ5,

6X0x · sin θ1 −

6Y 0x · cos θ1

sin θ5

)(16)

This solution is undetermined if the denominator sin θ5 = 0. In this case, the jointaxes 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, θ6can 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 canbe 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 allparallel. Together they constitute a planar 3R-manipulator, as illustrated in Figure 6.

x4

x3

elbow

x2

θ2

θ3

− P4 z1

− P4 x1

P4 xz1

φ2

φ3

x1

z1

y2

θ2

y3

y4

φ1

−a2

−a3

(a)

− P4 z1

− P4 x1

x2

z1

P4 xz1

x1

x3

x4

y2

(b)

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

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

01T , 4

5T , and 56T 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 thetranslation | 1P 4xz| is determined only by θ3, or similarly by φ3. The angle φ3 can befound by using the law of cosine:

cosφ3 =(−a2)2 + (−a3)2 − | 1P 4xz|2

2(−a2)(−a3)=a2

2 + a32 − | 1P 4xz|2

2a2a3(17)

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

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

9

Combining (17) and (18) give:

cos θ3 = −a2

2 + a32 − | 1P 4xz|2

2a2a3⇔

θ3 = ±acos(| 1P 4xz|2 − a22 − a32

2a2a3

)(19)

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

3.5 Finding θ2

The angle θ2 can be found as φ1−φ2. Each of these can be found by inspecting Figure6a and using atan2 and sine relations:

φ1 = atan2(− 1P 4z,− 1P 4x) (20)

sinφ2−a3

=sinφ3

| 1P 4xz|⇔

φ2 = asin(−a3 sinφ3| 1P 4xz|

)(21)

We can replace φ3 with θ3 by noticing that sinφ3 = sin (180◦ − θ3) = sin θ3. Combiningthe equations now give:

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

)(22)

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 remainingtransformation matrix, 3

4T , using its first column3X4:

θ4 = atan2(3X4y,

3X4x) (23)

4 Discussion

To sum up, a total of 8 solutions exist in general for the general inverse kinematicproblem 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 inspecific 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