Kent Libby forward kinematic script

fwkin.m

%% For this function, z is defined to be the axis coming out of the plane the base is on (z is up)
    %This uses degrees
function p = fwkin(t1, t2, t3)
L1=135;
L2=175;
L3=169.28;
TX12=[cosd(t1) -sind(t1) 0 0;
    sind(t1) cosd(t1) 0 0;
    0 0 1 L1;
    0 0 0 1];
TX23=[cosd(t2) 0 sind(t2) sind(t2)*L2;
      0 1 0 0;
      -sind(t2) 0 cosd(t2) cosd(t2)*L2;
      0 0 0 1];
TX34= [cosd(t3) 0 sind(t3) sind(t3)*L3;
      0 1 0 0;
      -sind(t3) 0 cosd(t3) cosd(t3)*L3;
      0 0 0 1];
  %x-> y-> z
  initp=TX12*TX23*TX34;
  p=initp(1:3,4);
end