Forward Kinematics for Links 4, 1, 3
© Dr. Greg R. Luecke
Our home-made robot uses Links 4, 1, and 3, as shown in Figure 1
Z3
Z1
Z0
d3
{3}
X3
LE
L3
XE
{E}
{2}
{0},{1}
L1
X2
X0 X1
t
ZE
Z2
Figure 1
4-1-3 robot configuration
i
ai-1
i-1
1
0
0
2
L1
+90
3
0
-90
E
LE
+90
Table of D-H parameters
di
0
-t
L3+d3*
0
i
1*
2*
0
0
For this robot, we need to touch the point (x, y, z). Orientation cannot be independently fixed while also
touching this point.
Note that symbolocally, this is:
x

 0R
y 
0
E d

T

E d

z
0 0 0 1 
Forward Kinematics for Links 4, 1, 3
©Dr. Greg R. Luecke
page 2 of 4
We'll use MATLAB to do the forward kinematics computations:
%Link_4_1_3
%incorporates forward kinematics
clear all
close all
%define the symbolic D-H parameters:
syms aim1 alfim1 di thi th1 th2 L1 L2 L3 LE t d3
%Define the four D-H operations:
Tx=[1 0 0 0;0 cos(alfim1) -sin(alfim1) 0;0 sin(alfim1) cos(alfim1) 0;0 0
0 1];
Dx=[1 0 0 aim1;0 1 0 0;0 0 1 0;0 0 0 1];
Tz=[cos(thi) -sin(thi) 0 0;sin(thi) cos(thi) 0 0;0 0 1 0 ;0 0 0 1];
Dz=[1 0 0 0;0 1 0 0;0 0 1 di;0 0 0 1];
%Concatenate in one homogeneous transform
AtB=Tx*Dx*Tz*Dz;
%Joint 1: set the values of the for D-H variables
%th1=0;
alfim1=0;
thi=th1;
aim1=0;
di=0;
%Find the value of the transform "0 to 1"
t01=subs(AtB)
%Joint 2: set the values of the for D-H variables
%th2=0;
alfim1=90*pi/180;
thi=th2;
aim1=L1;
di=-t;
%Find the value of the transform "1 to 2"
t12=subs(AtB)
%Joint 3: set the values of the for D-H variables
%th3=0;
alfim1=-90*pi/180;
thi=0;
aim1=0;
di=L3+d3;
%Find the value of the transform "2 to 3"
t23=subs(AtB)
%Joint E: set the values of the for D-H variables
%th4=0;
alfim1=90*pi/180;
thi=0;
aim1=LE;
di=0;
%Find the value of the transform "3 to E"
t3E=subs(AtB)
t0E = t01*t12*t23*t3E
Forward Kinematics for Links 4, 1, 3
©Dr. Greg R. Luecke
page 3 of 4
t01 =
[ cos(th1), -sin(th1), 0, 0]
[ sin(th1), cos(th1), 0, 0]
[
0,
0, 1, 0]
[
0,
0, 0, 1]
t12 =
[ cos(th2), -sin(th2), 0, L1]
[
0,
0, -1, t]
[ sin(th2), cos(th2), 0, 0]
[
0,
0, 0, 1]
t23 =
[ 1, 0, 0,
0]
[ 0, 0, 1, L3 + d3]
[ 0, -1, 0,
0]
[ 0, 0, 0,
1]
t3E =
[ 1, 0, 0, LE]
[ 0, 0, -1, 0]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]
t0E =
[ cos(th1)*cos(th2), -cos(th1)*sin(th2), sin(th1), L1*cos(th1) t*sin(th1) - cos(th1)*sin(th2)*(L3 + d3) + LE*cos(th1)*cos(th2)]
[ cos(th2)*sin(th1), -sin(th1)*sin(th2), -cos(th1), L1*sin(th1) +
t*cos(th1) - sin(th1)*sin(th2)*(L3 + d3) + LE*cos(th2)*sin(th1)]
[
sin(th2),
cos(th2),
0,
cos(th2)*(L3 + d3) + LE*sin(th2)]
[
0,
0,
0,
1]
Following the procedure by Craig in section 4.6:
0
ETd

 E0T 01T 21T 23T E3T
note: fixed and known
c1c2  c1s2 s1
s c  s s  c
0
0 1 2 3
1 2
1
1 2
ETd  1T 2T 3T ET 
 s2
c2
0

0 0 0

 0R
T 01T 21T 23T E3T   E
0 0 0
0
E d
0
PEorig 

1 
L1c1  ts1  c1s2 L3  d 3   LE c1c2 
L1c1  ts1  c1s2 L3  d 3   LE c1c2 

c2 L3  d 3 LE s2

1

Forward Kinematics for Links 4, 1, 3
©Dr. Greg R. Luecke
page 4 of 4