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
Top Related