Question

Program a function to compute the forward kinematic matrix based on the robot’s DH parameters. You should have the following function:

A=FK(DH)

DH contains all the DH parameters

Answer 1

​The objective of forward kinematic analysis is to determine the cumulative
effect of the entire set of joint variables.

DH Parameters

Link lengths
a = [0, 650, 0, 0, 0, 0]

Link offsets
d = [0, 190, 0, 600, 0, 125]

Link twist angle
alpha = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]

So basically I finding the T transformation matrix for each link from the the base frame {B} to wrist frame {W}. This is my code;

Function to compute Forward Kinematics:

def forwardK(q):

#T06 is the location of Wrist frame, {W}, relative to Base frame, {B}
T01 = genT(q[0],0,d[0],0)
T12 = genT(q[1],a[0],d[1],alpha[0])
T23 = genT(q[2],a[1],d[2],alpha[1])
T34 = genT(q[3],a[2],d[3],alpha[2])
T45 = genT(q[4],a[3],d[4],alpha[3])
T56 = genT(q[5],a[4],d[5],alpha[4])

#Tool frame {T}
#T67 = genT(0,0,d[5],0)

T03 = matmul(T01,T12,T23)
T36 = matmul(T34,T45,T56)
T06 = matmul(T03,T36)
#T07 = matmul(T06,T67)

x = T[0][3]
y = T[1][3]
z = T[2][3]

print("X: ",x)
print("Y: ",y)
print("Z: ",z,"\n")

print("T: ",T,"\n")

return T06

The function to compute T Matrix

def genT(theta, a, d, alpha):
T = array([[cos(theta), (-sin(theta)), 0, a],
[sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (- d*sin(alpha))],
[sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
[0, 0, 0, 1]])

return T

from the T Matrix relating the {B} frame to the {W} frame position vector of the {w} [x y z] is extracted. R Matrix (orientation) of the {W} relative to the {B} is obtained by the following piece of code;

T = forwardK([30,-110,-30,0,0,0])
x = T[0][3]
y = T[1][3]
z = T[2][3]
R = T[0:3,0:3]

Where T is the transformation matrix relating {W} to {B}

​