% function [theta] = IK(V);
% eg:       theta  = IK([0;0;-0.5])
%
% - performs the inverse kinemtics for a desired gripper position for the NUWAR robot
%
% where 
%       theta  = [theta1;theta2;theta3] - the actuated angles of each of the forearms
%       V      = the centre point of the gripper plate in base coordinates
%
% Copyright (c) 2001 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% pk at csse uwa edu au
% http://www.csse.uwa.edu.au/
function [theta] = IK_simp_2024(V);

if nargin == 0	%make sure that parameters have been supplied
   disp('Parameters are required:')
   help IK;
   return
end
alpha = -35.26;
beta  = 60;

Ra = 0.291;
La = 0.390;
Lb = 0.680;
Rb = 0.045;

if size(V) == [1 3]  % ensures V is a column vector
   V=V';
end

% convert the angles specified in degrees into radians
alpha  = alpha  * pi/180;
beta   = beta   * pi/180;

%set up motor axes - the gripper offset is accounted for here
motor1 = dhtrans(0, 0, Ra-Rb, -pi/2) * roty(beta) * rotx(alpha);
motor2 = rotz(2*pi/3) * motor1;
motor3 = rotz(4*pi/3) * motor1;

% work out the position of the end of the foreams in motor coordinates
V(4)=1;
V1 = invht(motor1) * V;
V2 = invht(motor2) * V;
V3 = invht(motor3) * V;

% then need to work out the motor angles - some simple trigonometry.
% 'gamma','phi' and 'psi' are checked to make sure that they contain no imaginary parts,
% since asin(x/y) returns complex values if x/y > 1. Similary with acos(x/y).
% x/y > 1 indicates that the desired point is outside the worspace of the robot.
% NOTE:	this checking could be combined at the end, but has been done here in three parts to
%			provide feedback regarding which arm is insoluble.

gamma1 = asin(V1(3) / Lb);
phi1 = atan(V1(1) / V1(2));
psi1 = acos((La^2 + (V1(1)/sin(phi1))^2 - (Lb * cos(gamma1))^2) / (2 * La * (V1(1)/sin(phi1))));
theta1 = pi/2 - (phi1 + psi1);
if ~isreal([gamma1 psi1]) 
   error('point out of workspace - 1st arm');
end

gamma2 = asin(V2(3) / Lb);
phi2 = atan(V2(1) / V2(2));
psi2 = acos((La^2 + (V2(1)/sin(phi2))^2 - (Lb * cos(gamma2))^2) / (2 * La * (V2(1)/sin(phi2))));
theta2 = pi/2 - (phi2 + psi2);
if ~isreal([gamma2 psi2]) 
   error('point out of workspace - 1st arm'); 
end

gamma3 = asin(V3(3) / Lb);
phi3 = atan(V3(1) / V3(2));
psi3 = acos((La^2 + (V3(1)/sin(phi3))^2 - (Lb * cos(gamma3))^2) / (2 * La * (V3(1)/sin(phi3))));
theta3 = pi/2 - (phi3 + psi3);
if ~isreal([gamma3 psi3]) 
   error('point out of workspace - 1st arm');
end

theta = [theta1;theta2;theta3]* 180/pi;  %returns values to degrees for output
end
% ROTX - Homogeneous transformation for a rotation about the x axis
%
% Usage: T = rotx(theta)
%
% Argument:  theta  - rotation about x axis
% Returns:    T     - 4x4 homogeneous transformation matrix
function T = rotx(theta)
T = [ 1     0           0        0
      0  cos(theta) -sin(theta)  0
      0  sin(theta)  cos(theta)  0
      0     0           0        1];
end

% ROTY - Homogeneous transformation for a rotation about the y axis
%
% Usage: T = roty(theta)
%
% Argument:  theta  - rotation about y axis
% Returns:    T     - 4x4 homogeneous transformation matrix
function T = roty(theta)
T = [ cos(theta)  0  sin(theta)  0
          0       1      0       0
     -sin(theta)  0  cos(theta)  0
          0       0      0       1];
end
% ROTZ - Homogeneous transformation for a rotation about the z axis
%
% Usage: T = rotz(theta)
%
% Argument:  theta  - rotation about z axis
% Returns:    T     - 4x4 homogeneous transformation matrix
function T = rotz(theta)
T = [ cos(theta) -sin(theta)  0   0
      sin(theta)  cos(theta)  0   0
          0           0       1   0
          0           0       0   1];
end
% INVHT - inverse of a homogeneous transformation matrix
%
% Usage:  Tinv = invht(T)
%
% Argument:   T    - 4x4 homogeneous transformation matrix
% Returns:    Tinv - inverse
function Tinv = invht(T)
  A = T(1:3,1:3);
  Tinv = [   A'   -A'*T(1:3,4)
           0 0 0       1      ];
end
% DHTRANS - computes Denavit Hartenberg matrix
%
% This function calculates the 4x4 homogeneous transformation matrix, representing
% the Denavit Hartenberg matrix, given link parameters of joint angle, length, joint
% offset and twist.
%
% Usage: T = DHtrans(theta, offset, length, twist)
% 
% Arguments:  theta - joint angle (rotation about local z)
%            offset - offset (translation along z)
%            length - translation along link x axis
%             twist - rotation about link x axis
%
% Returns:        T - 4x4 Homogeneous transformation matrix

function T = dhtrans(theta, offset, length, twist)
T = [ cos(theta) -sin(theta)*cos(twist)  sin(theta)*sin(twist) length*cos(theta)
      sin(theta)  cos(theta)*cos(twist) -cos(theta)*sin(twist) length*sin(theta)
          0             sin(twist)             cos(twist)         offset
          0                 0                      0                 1           ];
end