%PUMA6DOF Puma Six Degree of Freedom
%  Function for calculating the forward kinematics of a full 6 DOF puma arm
%
% Syntax:  [T, J] = puma6dof(theta, param, drawmode)
%
% Inputs:
%    theta - a 6 element array of joint angles
%    param - [d1 l2 d4 d6] (The non-zero lengths and offsets.) suggested
%            values [2 2 2 .2]
%    drawmode - a 3 element vector controlling what is displayed
%       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
%       drawmode(3) controls whether or not display frames are cleared between
%       successive drawings of the robot.
%           0  - Clear screen between successive drawings of the robot.
%           1  - Don't clear screen
%
% Outputs:
%    T - the homogeneous transform that describes the end-effector frame
%    J - the 6x6 Jacobian matrix corresponding to the current arm configuration.
%
%      - If drawmode(1) ~= 0 the function will draw a 3D diagram of the arm
%        on the screen
%
% Other m-files required: trans.m, DHtrans.m, invht.m, closedCylinder.m,
%                         gripper.m, cosmetics.m
%
% See also: PUMAMRCNTRL

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

function [T, J] = puma6dof(theta, param, drawmode)

    %      theta    offset   length   twist
    DH = [ theta' [ param(1) 0        pi/2  
                    0        param(2) 0  
                    0        0       -pi/2  
                    param(3) 0       -pi/2  
                    0        0        pi/2  
                    param(4) 0        0    ] ];
            
    % define initial origin
    links(:,:,1) = trans(0, 0, 0);
    
    % calculate each joint transform and store joint position
    for i = 1:6
        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
    T = links(:,:,7);
        
    % calculate Jacobian [ cross(z, p - o); z ]
    for i = 1:6
        J(:,i) = [cross(links(1:3,3,i), links(1:3,4,7) - links(1:3,4,i)); links(1:3,3,i)];
    end
    
    % radius of links
    r = 0.15;

    % drawmodes
    if drawmode(1) ~= 0
        if ~drawmode(3)
            clf
        else
            hold on;
        end
        hold on;
        for i = 1:7
            if drawmode(2)
                plotframe(links(:,:,i));
            end
            if drawmode(1) == 1 && i < 7
                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
                if i < 6
                    [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
                if i < 7
                    [X, Y, Z] = closedCylinder(links(:,4,i), links(:,4,i+1), r);
                    surfl(X,Y,Z);
                end
                    gripper(T, pi/6);
            end
        end
    hold off;
    cosmetics(4);
    pause(0.05);
end