% Inverse kinematics of 3R manipulator.

%

% USAGE:  [q, err] = invkin3r(X)

%

% where: X = position of tip [x, y, z]   COLUMN

%        q = joint angles (radians)      COLUMN

%      err = error parameter (0 = no error, 1 = illegal position)

%

% Note that there are no joint limits imposed or checked.

%

% DH parameters:

%

%    link    alpha     a     theta        d

%      1      90       0      var        0.5

%      2       0       1      var         0

%      3       0      0.8     var         0

%

% The world origin is taken at the base of the robot.



function [q, err] = invkin3r(X)



err = 0;

q = zeros(3,1);



d1 = 0.5;

a2 = 1.0;

a3 = 0.8;





px = X(1);

py = X(2);

pz = X(3);



% ===================   BODY JOINT  ============================



if py == 0 & px == 0          % point on the Z0 axis is a problem

  theta_1 = 0;

else

  theta_1 = atan2(py, px);    % use this as much as possible

end



c1 = cos(theta_1);

s1 = sin(theta_1);



% ===================   ELBOW joint 3 ==========================



% some dummy variables

t1 = c1*px + s1*py;

t2 = pz - d1;



c3 = (t1^2 + t2^2 - a3^2 - a2^2) / (2*a2*a3);



% check for error

if abs(c3) > 1

  err = 1;

  disp('Illegal position - too far away.')

  X

  return

end



s3 = -sqrt(1 - c3^2);

theta_3 = atan2(s3, c3);





% ==================  SHOULDER JOINT 2 =========================

num = t2*(a3*c3 + a2) - a3*s3*t1;

den = t1*(a3*c3 + a2) + a3*s3*t2;

theta_2 = atan2(num, den);





% assign the joint values to output vector.



q(1) = theta_1;

q(2) = theta_2;

q(3) = theta_3;



