M.E. 530.646UR5 Inverse Kinematics

Ryan Keating · 2014Updated by Noah J. Cowan · 2016

Johns Hopkins University

Introduction

Figure refFrames below illustrates a common assignment of Denavit-Hartenberg conventionto the UR5 robot (shown with all joint angles at 0).

y0

x0z0

x1

y0

y1 y2y3

y4

y5

y6z1 z2x2

x3

x4

x5

x6

d1

a2 a3

d4

d5 d6

z3

z4

z5

z6

Figure 1: D-H Convention Frame Assignment

Joint a α d θ1 0 π/2 0.089159 θ12 −0.425 0 0 θ23 −0.39225 0 0 θ34 0 π/2 0.10915 θ45 0 −π/2 0.09465 θ56 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 thegripper can be defined as follows:

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

1 (θ1) T12 (θ2) T

23 (θ3) T

34 (θ4) T

45 (θ5) T

56 (θ6) (1)

Also remember that a homogenous transformation T ij has the following form:

T ij =

[Rij

~P ij

0 1

]=

xx yx zx (P i

j )xxy yy zy (P i

j )yxz yz zz (P i

j )z0 0 0 1

(2)

where ~P ij is the translation from frame i to frame j and each column of Ri

j is the projectionof 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 thelocation of the 5th coordinate frame with respect to the base frame, P 0

5 . As illustrated inFigure 2 below, we can do so by translating by d6 in the negative z direction from the 6th

frame.

x0

x6

y0

y6

z0

z6

z5d6

Figure 2: Finding the Origin of the 5th Frame.

This is equivalent to the following operation:

~P 05 = T 0

6

00−d6

1

−

0001

(3)

2

x0x1

y0z1

z2

z3

z5

z4

θ1

φ

ψ

d4

Frame 5 origin

Figure 3: Finding θ1. Note that the rotations around the axes z1, z2, z3, and z4 do not change the factthat the origin of Frame 5 is in the plane that is parallel to the x1 axis, offset by d4 as shown.

The key to this observation is that T 60 is known because it is simply the desired transfor-

mation! That is, given the desired transformation, T 60 , we can calculate the vector from the

origin of Frame 0 to the origin of Frame 5.

With the location of the 5th frame, we can draw an overhead view of the robot as shown inFigure 3. From this, we can see that θ1 = ψ + φ+ π

2where

ψ = atan2((P 0

5 )y, (P05 )x)

(4)

φ = ± arccos

(d4

(P 05 )xy

)= ± arccos

(d4√

(P 05 )x 2 + (P 0

5 )y 2

)(5)

The two solutions for θ1 above correspond to the shoulder being either “left” or “right,”.Note that Equation 5 has a solution in all cases except that d4 > (P 0

5 )xy. You can see fromFigure 3 this happens when the origin of the 3rd frame is close to the z axis of frame 0. Thisforms an unreachable cylinder in the otherwise spherical workspace of the UR5 (as shown inFigure 4 taken from the UR5 manual).

3

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 (P 16 )z = d6 cos(θ5)+d4, where (P 1

6 )z = (P 06 )x sin(θ1)− (P 0

6 )y cos(θ1). Solvingfor θ5,

θ5 = ± arccos

((P 1

6 )z − d4d6

)(6)

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 withrespect to frame 6 as a unit vector defined with spherical coordinates. We can find the xand y components of of this vector by projecting it onto the x-y plane and then onto the xor y axes.

Next we find transformation from frame 6 to frame 1,

T 61 = ((T 0

1 )−1 T 06 )−1 (7)

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

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

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

4

x0

x1

y0z1

θ1

z2

z3

z5

z6

x4

x5

y4

θ5

d6c5

d4

Figure 5: Finding θ5

5

Solving for θ6,

θ6 = atan2

(−zy

sin(θ5),

zxsin(θ5)

)(10)

Equation 10 shows that θ6 is not well-defined when sin(θ5) = 0 or when zx, zy = 0. We cansee 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 positionand rotation of the end-effector in the plane, resulting in an infinite number of solutions. Inthis case, a desired value for q6 can be chosen to reduce the number of degrees of freedomto three.

x4,x5,x6

z4 y5,y6

z5,z6,y4

x5,x6

z4 y5,y6

z5,z6z5,z6 z5,z6

θ5

θ5 c5θ5

-θ6

y6

x6

y4

x4

y4,z1

c6s5

-s6s5

Figure 6: Finding θ6

We can now consider the three remaining joints as forming a planar 3R manipulator. Firstwe will find the location of frame 3 with respect to frame 1. This is done as follows:

T 14 = T 1

6 T64 = T 1

6 (T 45 T

56 )−1 (11)

~P 13 = T 1

4

0−d4

01

−

0001

(12)

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

cos(ξ) =|| ~P 1

3 ||2 − a22 − a232a2a3

(13)

6

with use of the law of cosines.

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

= cos(θ3)(14)

Combining 13 and 14

θ3 = ± arccos

(|| ~P 1

3 ||2 − a22 − a232a2a3

)(15)

Equation 15 has a solution as long as the argument to arccos is ∈ [−1, 1]. We can see that

large values of || ~P 13 || 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 boundto the workspace as seen in 4).

Figure 7 also shows thatθ2 = −(δ − ε) (16)

where δ = atan2((P 13 )y,−(P 1

3 )x) and ε can be found via law of sines:

sin(ξ)

|| ~P 13 ||

=sin(ε)

a3(17)

Combining 16 and 17

θ2 = − atan2((P 13 )y,−(P 1

3 )x) + arcsin

(a3 sin(θ3)

|| ~P 13 ||

)(18)

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 findT 34 :

T 34 = T 3

1 T14 = (T 1

2 T23 )−1T 1

4 (19)

Using the first column of T 34 ,

θ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

x1

x2

x3

y1z1

y2 z2

δ-θ2

-θ3

a2

a3

Figure 7: Finding θ2 and θ3

8

(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

9