%INVPUMA6DOF    Inverse Puma 6 Degrees of Freedom 
% Function for calculating the inverse kinematics of a full 6 DOF puma arm
%
% Syntax:  theta = invpuma6dof(T, param)
%
% Inputs:
%    T - The homogeneous transform that describes the end-effector frame.
%    param - [d1 l2 d4 d6] (suggested values [2 2 2 .2])
%            The non-zero lengths and offsets.
%
% Outputs:
%    theta - a 4x6 element array of joint angles. (up to four different
%            solutions, each consisting of six joint angles. Note: ignore
%            the extra 4 solutions obtained by adding pi to theta1)
%
% Other m-files required: trans.m, DHtrans.m, invht.m, inveuler.m
%
% See also: PUMA6DOF

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

function theta = invpuma6dof(T, param)

    % solve for the wrist centre
    T = T*trans(0, 0, -param(4));
    
    theta(1:4,1) = atan2(T(2,4), T(1,4));
    
    rSquared = T(1,4)^2 + T(2,4)^2;
    s        = T(3,4) - param(1);
    
    D = (rSquared + s^2 - param(2)^2 - param(3)^2)/(2*param(2)*param(3));
    theta(1:2,3) =  acos(D) - pi/2;
    theta(3:4,3) = -acos(D) - pi/2;
    
    theta(1:4,2) = atan2(s, sqrt(rSquared)) - asin(param(3)*cos(theta(1:4,3))/sqrt(rSquared + s^2));

    for i = 1:2:3 
        %      theta,      offset,  length,  twist
        DH = [ theta(i,1)  param(1) 0        pi/2  
               theta(i,2)  0        param(2) 0  
               theta(i,3)  0        0       -pi/2 ];
           
        t = DHtrans(DH(1, 1), DH(1, 2), DH(1, 3), DH(1, 4))*DHtrans(DH(2, 1), DH(2, 2), DH(2, 3), DH(2, 4))*DHtrans(DH(3, 1), DH(3, 2), DH(3, 3), DH(3, 4));
        T = invht(t)*T;
        
        % solve for inverse orientation
        [euler1, euler2] = inveuler(T);
        theta(i:i+1,4:6) = [euler1; euler2];               
    end