|
function T = myFk(theta1,theta2,theta3,theta4,theta5)
|
|
L1=0.151;L3=0.350; L4=0.139;
|
|
T01 = dh2Tmat(theta1,L1,0,pi/2);
|
|
T12 = dh2Tmat(theta2,0,0,pi/2);
|
|
T23 = dh2Tmat(theta3,L3,0,-pi/2);
|
|
T34 = dh2Tmat(theta4,0,0,pi/2);
|
|
T45 = dh2Tmat(theta5,L4,0,0);
|
|
|
|
T = T01*T12*T23*T34*T45;
|
|
end |