Faculty of Computing, Engineering and Technology
Module Name: MSc Robotics and Automation
Forward Kinematics
Forward kinematics is the mathematics behind calculating an end point position (and
orientation) from knowing the joint lengths and angles.
Single link kinematics
Figure 1 Simple two dimension kinematics with a single link
The forward kinematics of a simple link with link length L and moving through angle Ө
are:
sin
cos
Lz
Lx
Example 1
If the link length, L=50, and the angle, Ө=45˚, the endpoint (x,z) would be (35,35).
Two link kinematics
Figure 2 Rotation of a link
z
θ
1
L
2
L
1
θ
2
End point
x
z
L
End point
Ө
x
Referring to figure 2, the geometric solution for two links can be solved as:
21211
21211
sinsin
coscos
LLz
LLx
The 3D graph
From now on, we are going to use 3D coordinates on the 3D coordinate graph. It is
essential that you get this graph the right way round. We are going to flatten our xy graph
used above and form another axis, the zaxis to define the height. Look at the graph
below, the yaxis is going away from you, into the page.
Figure 3 The 3D graph
Rotation
There are three possible rotations for an object, that is, every possible orientation of an
object can be described as a component of three distinct rotations. A rotation about the x
axis, about the yaxis, and about the zaxis. Imagine that each of these axes act as an axle
running through the centre of the object, all perpendicular to each other, and the object is
allowed to rotate about each one. The effect of rotating a block about the yaxis would be
as in Figure 4.
Figure 4 Effect of rotation about the yaxis
y y
x
z
y
Direction of rotation
How do we determine which way is a positive rotation, e.g. 10˚ and negative, e.g. 10˚?
We can use the righthand screw rule. If we imagine we are screwing a screw so it
translates in the direction the yaxis is pointing, we would have to rotate it clockwise (or
from the zaxis to the xaxis).
Figure 5 The righthand screw convention
So, rotating from the zaxis to the xaxis is a positive angle, but rotating from the xaxis
to the zaxis is the opposite direction from your normal screw and is a negative angle.
Likewise, rotating from the xaxis to the yaxis (about z) is a positive angle and so is
rotating from the yaxis to the zaxis.
Looking at the page with the yaxis pointing away from you, a rotation of a link having
length ’r’ and rotating about the yaxis by a value, θ, would be shown as in Figure 6.
Figure 6 Rotation of a link
x
z
y
r
z
θ
x
Homogenous Matrices
We are going to repeat the forward kinematics for this link using matrices. The
advantages of this method are that the system can work easily in 3D space, and it is
easily adapted for a multilink robot arm. The matrices we will use will be 2D 4 x 4
(think of a 2D array in programming). The general method of multiplying a 2 x 2 matrix
is as below:
dhcfdgce
bhafbgae
hg
fe
dc
ba
Example 2
Multiply two matrices to get the result:
5043
2219
87
65
43
21
We will use 4 x 4 matrices we will use consist of a rotation part and a translation part as
shown below, these are known as ‘homogenous matrices’:
The link motion is going to be described by two matrices that are multiplied together, one
to represent a rotation (about y) and another to represent a translation along x.
We will define the link direction along the xaxis because if the link had a rotation of
zero degrees it would lie along the xaxis. Refer to appendix 1 for the list of the three
translation matrices, and the three rotation matrices.
The rotation of θ about the yaxis matrix is defined as:
1000
00
0010
00
CS
SC
where Cθ = cos θ, and Sθ = sin θ
The translation along the xaxis by ‘x’ matrix is defined as:
Rotation
part of
matrix
Translation
0 0 0 1
Rotation about y X Translation along x
1000
0100
0010
001 x
Example 3Calculating the end point position
Going back to example 1, where the link length is 50, and rotated by 45˚, the resulting
matrix would be:
1000
357.007.0
0010
357.007.0
1000
0100
0010
50001
1000
07.007.0
0010
07.007.0
so, the link would be at the position 35 along the xaxis, and 35 along the zaxis. (the
value along the yaxis is still zero as there has been no rotation about z).
End point rotation
The rotation about the x, y and z axes can be calculated from the rotation part of the
resulting matrix, this is the 3 x 3 matrix defining rotation:
333231
232221
131211
rrr
rrr
rrr
)Ycos(r,)Ycos(rAtan2X
)Ycos(r,)Ycos(rAtan2Z
rr,rAtan2Y
3332
1121
21
2
11
2
31
where Atan2 computes tan
1
(y/x) but uses the signs of both x and y to determine the
quadrant of the angle from 0°to 360°. Atan2 is in the form of Atan2 (y value, x value).
For example, Atan2(2.0,2.0) =135˚;wheras Atan2(2.0,2.0)=45˚. We are computing
angles in the full range of 0 to 360˚.
In example 3 above, the 3 x 3 rotation matrix is:
7.007.0
010
7.007.0
And so the rotation is:
Y=315˚ (or 45˚)
Z=0
X=0
x
y
z
Defining multiple links
In order to really see the benefit of using matrices, a two link example is introduced.
Referring to figure 2, notice that, when both links are at rest (zero angles or rotation for
both), the links lie along the xaxis. Translation will therefore be along x.
The rotation of both links is about the yaxis with link 1 rotating in a negative direction
and link 2 rotating in a positive direction. Note that the angle of rotation of link 1 is with
respect to the current orientation of link 1 (shown with the dashed line). Notice how the
local coordinate frame moves and rotates along with each link. Each link not only
describes the position of the new frame origin but also its rotation.
The resulting endpoint will be found from multiplying the matrices in this order:
or
1000
0100
0010
001
1000
00
0010
00
1000
0100
0010
001
1000
00
0010
00
2
22
221
11
11
L
CS
SCL
CS
SC
Example using MATLAB
In figure 6 above, if θ
1
=45°, and θ
2
=30°, whilst L1=50 and L2=40.
The matrices would be:
1000
0100
0010
40001
1000
0866.005.0
0010
05.00866.0
1000
0100
0010
50001
1000
0707.00707.0
0010
0707.00707.0
Letting each matrix be a variable a,b,c,d, we need to find a x b x c x d.
The matrix is entered with spaces between the columns, and a semicolon separating each
row, as in:
%Forward kinematics for two link arm
a=[0.707 0 0.707 0;0 1 0 0;0.707 0 0.707 0;0 0 0 1];
b=[1 0 0 50;0 1 0 0;0 0 1 0;0 0 0 1];
c=[0.866 0 0.5 0;0 1 0 0;0.5 0 0.866 0;0 0 0 1];
d=[1 0 0 40;0 1 0 0;0 0 1 0;0 0 0 1];
and the answer obtained from:
r=a*b*c*d
Rotation about
y by θ
1
Translation
along x by L
1
X
Rotation
about y by θ
2
X
Translation
along x by L
2
X
which is:
r =
0.9658 0 0.2588 73.9805
0 1.0000 0 0
0.2588 0 0.9658 45.7005
0 0 0 1.0000
Thus, the end point is at x=73.9805, y=0, z=45.7005.
The end point rotation (about Y) can be calculated thus:
22
09658.0,2588.0Atan2 Y
=15° or 345°
or, using MATLAB
The 4x4 matrix, ‘r’, is calculated for the end point, the orientation can be calculated using
MATLAB as:
%Find orientation of final point
Y=atan2(r(3,1),sqrt(r(1,1)^2+r(2,1)^2));
Z=atan2(r(2,1)/cos(Y),r(1,1)/cos(Y));
X=atan2(r(3,2)/cos(Y),r(3,3)/cos(Y));
%Convert to degrees
todeg=180/pi;
Xd=X*todeg
Yd=Y*todeg
Zd=Z*todeg
Another example using MATLAB
I have made a variable, ‘t’ equal to the angle in radians. Then I have defined ‘ct’ and ‘st’
to be the cos and sin of this angle respectively.
>> t=

45/180*pi;
>> ct=cos(t);
>> st=sin(t);
>> roty=[ct 0 st 0;0 1 0 0;st 0 ct 0;0 0 0 1];
>> transx=[1 0 0 50;0 1 0 0;0 0 1 0;0 0 0 1];
>> link=roty*transx
link =
0.7071 0 0.7071 35.3553
0 1.0000 0 0
0.7071 0 0.7071 35.3553
0 0 0 1.0000
Appendix 1 Rotation and translation (homogenous) matrices
Here is a list of rotation and translation matrices useful for defining different links of a
robot:
Rotation about Z:
1000
0100
00
00
:
CS
SC
i
Translation along X:
1000
0100
0010
001
:
x
x
Rotation about X:
1000
00
00
0001
:
CS
SC
i
Translation along Y:
1000
0100
010
0001
:
y
y
Rotation about Y:
1000
00
0010
00
:
CS
SC
i
Translation along Z:
1000
100
0010
0001
:
z
z
Where
cosC
, and
sinS
DH Matrices
These are used to simplify the rotation and translation matrices as explained above. There
are just four parameters and represent translations and rotations in the following order:
i) rotation about the zaxis (),
ii) translation along the xaxis (a),
iii) translation along the zaxis (d), and,
iv) rotation about the xaxis ().
See my website for a DH matrix simulation using Java:
http://www.fcet.staffs.ac.uk/sow1/my_resources.htm
X
Y
a
a
i
is the distance from Z
i
to Z
i+1
along the X
i
axis.
i
is the angle between
Z
i
and Z
i+1
along the X
i
axis.
d
d
i
is the distance from X
i1
to X
i
along the Z
i
axis.
i
is the angle between X
i1
and
X
i
about the Z
i
axis.
i)
ii)
iii)
iv)
Z
The DH table for the four parameters for each link of a robot arm can be formed as
below:
α
a θ d
1
2
3
4
5
..
…
…
…
…
DH Example 1
Looking from above (along the negative zaxis) at a two link SCARA type robot arm,
both links rotate about the zaxis by
1
and
2
:
The links rotate about the zaxis, and the link lengths lie along the xaxis. The parameters
for link 1 are therefore: =0˚, a=125,d=0, =
1
. The parameters for link 2 are: =0˚,
a=60,d=0, =
1
.
The DH table would be:
α
a θ d
1
0
125
1
0
2
0
60
2
0
DH Example 2
Imagine a robot arm with two links that can rotate about the yaxis. Note that the yaxis is
going into the page. The links can rotate as in the dashed lines shown, these would be
rotating in the negative y direction.
Z
Y
X
Z
125 60
1
2
In DH matrices, there is no rotation about the yaxis, therefore we must rotate the
coordinate frame so that either the z or the x axes correspond to the rotation axis. I will
choose to rotate the xaxis by 90˚ so that the zaxis is going into the page (where the y
axis previously was) and the yaxis will be pointing downwards.
This link can also rotate about the zaxis by ‘
1
‘and positive rotation will make the link
go upwards, the parameters for link 1 would be: =90˚, a=0,d=0, =
1
.
Next, we have a link translation along the xaxis by 100, the end point of this link will
rotate by
2
about the zaxis, so the parameters for link 2 are: =0˚, a=100,d=0, =
2
.
The following link has a length of 50 along the xaxis, so the parameters are:
=0˚, a=50,d=0, =0.
The DH table would be:
α
a
θ d
1
90
0 0 0
2
0 100
1
0
3
0 50
2
0
100
X
Z
50
Y
X
Z (into page)
Y
(set the orientation of the axis to rotation about Y)
(translation along ‘a’ comes after the rotation)
What difference would it make to the
direction of the ‘’ angles if we initially
rotate about the xaxis by +90˚?
DH Example 3
DH Example 4
X
Y
Z
A revolute robot rotates about the Zaxis base by
1
Whilst
2
,
3
and
4
rotate about the Yaxis.
The DH matrix would be:
α
a θ d
1
90
0 θ
1
100
2
0 350
θ
2
0
3
0 200
θ
3
0
4
0 75 θ
4
0
1
3
4
2
100
350
200
75
(α rotation happens
after θ and
translation along ‘a’
in point 1)
θ
1
θ
2
200
150
20
100
A SCARA robot rotates abo
ut the z

axis
base by
1
(shoulder)
2
is the elbow rotation about the zaxis.
The DH matrix would be:
α
a θ d
1
0
0 θ
1
200
2
0
150
0 20
3
0
100
θ
2
0
Robotics Toolbox
To create the robot in example 2 using the ‘link’ command:
An extract from the Robotics Toolbox manual defines link:
DH and homogenous matrices
The DH matrix is an easy method of defining a homogenous matrix. The homogenous
matrix for the DH transform is:
1000
dccscss
dsscccs
a0sc
l1=link([pi/2 0 0 0])
l2=link([0 100 0 0])
l3=link([0 50 0 0])
L{1}=l1
L{2}=l2
L{3}=l3
R=robot(L)
drivebot(R)
L = link([alpha, a, theta, d])
Description The link function constructs a link object. The object contains kinematic and dynamic
parameters
as well as actuator and transmission parameters. The first form returns a default object, while the
second and third forms initialize the kinematic model based on Denavit and Hartenberg parameters.
By default the standard Denavit and Hartenberg conventions are assumed but a flag (mdh) can be set
if modified Denavit and Hartenberg conventions are required. The dynamic model can be initialized
using the fourth form of the constructor where dyn row is a 1 20 matrix which is one row of the
legacy dyn matrix.
The second last form given above is not a constructor but a link method that returns the link transformation
matrix for the given joint coordinate. The argument is given to the link object using parenthesis.
The single argument is taken as the link variable q and substituted for or D for a revolute or
prismatic link respectively.
The Denavit and Hartenberg parameters describe the spatial relationship between this link and the
previous one. The meaning of the fields for each model are summarized in the following table.
alpha i i 1 link twist angle
A Ai Ai 1 link length
theta i i link rotation angle
D Di Di link offset distance
sigma i i joint type; 0 for revolute, nonzero for prismatic
Note that the α angle is in radians, θ is set to zero (the
initial pose of the robot) but the command ‘drivebot’
allows the user to modify θ interactively using slider
controls. Also note that the slider for θ1 is redundant,
this alters the θ in the first column
Comments 0
Log in to post a comment