%PUMA3DOF   Puma three degrees of freedom robot
% takes the three joint angles, calculates the forward kinematics of the
% Puma arm.
%
% plots a stick diagram of the arm along with the link coordinate frames. 
%
% Syntax:  puma3dof(theta1, theta2, theta3, drawmode)
%
% Inputs:
%    theta1 - joint angle 1
%    theta2 - joint angle 2
%    theta3 - joint angle 3
%    drawmode - A 2-vector controlling display of the arm
%       drawmode(1) controls type of robot representation that is displayed
%           0  - Nothing.
%           1  - Stick diagram of robot.
%           2  - Rendered `cylinder' representation of robot.
%       drawmode(2) controls whether coordinate frames are displayed
%           0  - No frames displayed.
%           1  - Coordinate frames are displayed
%
% Other m-files required: plotframe.m, DHtrans.m, cosmetics.m,
%    closedcylinder.m, invht.m, gripper.m
%
% See also: STANFORD3DOF

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

function puma3dof(theta1, theta2, theta3, drawmode)
    % DH parameters
    d1 = 3;
    l2 = 3;
    l3 = 3;
    % theta, offset, length, twist
    DH = [ theta1  d1  0   pi/2
           theta2  0   l2  0  
           theta3  0   l3  0    ];
    
    % define initial origin
    links(:,:,1) = trans(0, 0, 0);
    
    % allow for arbitrary DH parameters
    n = size(DH, 1);
    
    % calculate each joint transform and store joint position
    for i = 1:n
        T = DHtrans(DH(i, 1), DH(i, 2), DH(i, 3), DH(i, 4));
        links(:,:,i+1) = links(:,:,i)*invht(links(:,:,1))*T*links(:,:,1);
    end
    
    % radius of links
    r = 0.3;
    
    hold on;
    
    % drawmodes
    for i = 1:n+1    
        if drawmode(2)
            plotframe(links(:,:,i));
        end
        if drawmode(1) == 1 && i <= n
            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)]);
        elseif drawmode(1) == 2 && i <= n
            [X, Y, Z] = closedCylinder(links(:,4,i), links(:,4,i+1), r);
            surfl(X,Y,Z);
            if i < n
                [X, Y, Z] = closedCylinder(links(:,:,i+1)*[0 0 (r+r/10) 1]', links(:,:,i+1)*[0 0 -(r+r/10) 1]', r);
                surfl(X,Y,Z);
            end
        end
    end
    
    hold off;
     
    cosmetics(7);