%STANFORD3DOF   Stanford three degrees of freedom robot
% takes the two joint angles and the prismatic extension of the last link,
% and calculates the forward kinematics of the Stanford Arm.
%
% plots a stick diagram of the arm along with the link coordinate frames. 
%
% Syntax:  stanford3dof(theta1, theta2, d3)
%
% Inputs:
%    theta1 - joint angle 1
%    theta2 - joint angle 2
%    d3 - prismatic extension of last link
%
% Other m-files required: plotframe.m, DHtrans.m, cosmetics.m, invht.m
%
% See also: PUMA3DOF

% Author: Travis Hydzik
% Last revision: 19 October 2004

function stanford3dof(theta1, theta2, d3)
    % DH parameters
    d1 = 2;
    % theta, offset, length, twist
    DH = [ theta1  d1  0   pi/2
           theta2  0   0  -pi/2  
           0       d3  0   0    ];
        
    hold on;
    
    % define and plotframe initial origin
    links(:,:,1) = trans(0, 0, 0);
    plotframe(links(:,:,1));
    
    % calculate each joint transform and store joint position
    for i = 1:size(DH, 1)
        % Denavit-Hartenberg transformation
        T = DHtrans(DH(i, 1), DH(i, 2), DH(i, 3), DH(i, 4));
        % store new link
        links(:,:,i+1) = links(:,:,i)*invht(links(:,:,1))*T*links(:,:,1);
        % draw links and frames
        line([links(1,4,i) links(1,4,i+1)], [links(2,4,i) links(2,4,i+1)], [links(3,4,i) links(3,4,i+1)]);
        plotframe(links(:,:,i+1)); 
    end
    
    hold off;
    
    cosmetics(3);