The document discusses the inverse kinematics of the UR5 robot using the Denavit-Hartenberg convention, detailing the parameters and transformations necessary to calculate joint angles. It outlines the step-by-step method to determine the angles θ1 to θ4, providing equations for efficiency and identifying specific conditions where solutions may not exist. Eight distinct solutions for configuring the end-effector's position and orientation are also presented.

1. 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).
y0
x0
z0
x1
y0
y1 y2 y3
y4
y5
y6z1 z2
x2 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 θ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

2. As with any 6-DOF robot, the homogeneous transformation from the base frame to the
gripper can be defined as follows:
T0
6 (θ1, θ2, θ3, θ4, θ5, θ6) = T0
1 (θ1) T1
2 (θ2) T2
3 (θ3) T3
4 (θ4) T4
5 (θ5) T5
6 (θ6) (1)
Also remember that a homogenous transformation Ti
j has the following form:
Ti
j =
Ri
j Pi
j
0 1
=




xx yx zx (Pi
j )x
xy yy zy (Pi
j )y
xz yz zz (Pi
j )z
0 0 0 1



 (2)
where Pi
j is the translation from frame i to frame j and each column of Ri
j is the projection
of one of the axes of frame j onto the axes of frame i (i.e. [xx, xy, xz]T
≡ xi
j).
.[?]
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, P0
5 . As illustrated in
Figure 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
z5
d6
Figure 2: Finding the Origin of the 5th
Frame
2

3. This is equivalent to the following operation:
P0
5 = T0
6




0
0
−d6
1



 −




0
0
0
1



 (3)
With the location of the 5th
frame, we can draw an overhead view of the robot:
x0
x1
y0
z1
z2
z3
z5
θ1
φ
ψ
d4
Figure 3: Finding θ1
From Figure 3, we can see that θ1 = ψ + φ + π
2
where
ψ = atan2 (P0
5 )y, (P0
5 )x (4)
φ = ± arccos
d4
(P0
5 )xy
= ± arccos
d4
(P0
5 )x
2 + (P0
5 )y
2
(5)
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 > (P0
5 )xy. You can see from Figure 3
3

4. 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 (P1
6 )z = d6 cos(θ5)+d4, where (P1
6 )z = (P0
6 )x sin(θ1)−(P0
6 )y cos(θ1). Solving
for θ5,
θ5 = ± arccos
(P1
6 )z − d4
d6
(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 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,
T6
1 = ((T0
1 )−1
T0
6 )−1
(7)
Remembering the structure the of the first three columns of the homogenous transformation
T6
1 (see Equation 2), we can form the following equalities:
− sin(θ6) sin(θ5) = zy (8)
cos(θ6) sin(θ5) = zx (9)
4

5. x0
x1
y0
z1
θ1
z2
z3
z5
z6
x4
x5
y4
θ5
d6c5
d4
Figure 5: Finding θ5
Solving for θ6,
θ6 = atan2
−zy
sin(θ5)
,
zx
sin(θ5)
(10)
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:
T1
4 = T1
6 T6
4 = T1
6 (T4
5 T5
6 )−1
(11)
5

6. 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
P1
3 = T1
4




0
−d4
0
1



 −




0
0
0
1



 (12)
Now we can draw the plane containing frames 1-3, as shown in Figure 7. We can see
that
cos(ξ) =
||P1
3 ||2
− a2
2 − a2
3
2a2a3
(13)
with use of the law of cosines.
cos(ξ) = − cos(π − ξ)
= − cos(−θ3)
= cos(θ3)
(14)
Combining 13 and 14
θ3 = ± arccos
||P1
3 ||2
− a2
2 − a2
3
2a2a3
(15)
Equation 15 has a solution as long as the argument to arccos is ∈ [−1, 1]. We can see that
large values of ||P1
3 || 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((P1
3 )y, −(P1
3 )x) and can be found via law of sines:
sin(ξ)
||P1
3 ||
=
sin( )
a3
(17)
6

7. Combining 16 and 17
θ2 = − atan2((P1
3 )y, −(P1
3 )x) + arcsin
a3 sin(θ3)
||P1
3 ||
(18)
x1
x2
x3
y1
z1
y2 z2
δ
-θ2
-θ3
a2
a3
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
T3
4 :
T3
4 = T3
1 T1
4 = (T1
2 T2
3 )−1
T1
4 (19)
Using the first column of T3
4 ,
θ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

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
8