Solution:

We can calculate all the angle deviations from current joint positions and then choose one
set that minimize the overall joint rotation needed.

Solution:
Factors might affect repeatability:
1. Sensor resolution and noise
2. Actuator control accuracy
3. Looseness of joints
4. Flexibility of links

Factors might affect accuracy:

1. All of the above
2. D.H. parameter calibration
3. Sensor calibration
Matlab code:
syms L1 L2 L3 theta1 theta2 theta3;
L1 = 4;
L2 = 3;
L3 = 2;

a = L3;
alpha = 0;
d = 0;
theta = 0;
T3fH = HomoTrans(a,alpha,d,theta);

fprintf('## Input case:\n')

% i) =========================
T0fH = [1 0 0 9
0 1 0 0
0 0 1 0
0 0 0 1]

% ii) ========================
% T0fH = [ 0.5 -0.866 0 7.5373
% 0.866 0.6 0 3.9266
% 0 0 1 0
% 0 0 0 1]

% iii) =======================
% T0fH = [ 0 1 0 -3
% -1 0 0 2
% 0 0 1 0
% 0 0 0 1]

% iv) ======================
% T0fH = [0.866 0.5 0 -3.1245
% -0.5 0.866 0 9.1674
% 0 0 1 0
% 0 0 0 1]

U1 = T0fH * inv(T3fH); %T0f3

Px = U1(1,4);
Py = U1(2,4);
Pz = U1(3,4);
nx = U1(1,1);
ny = U1(2,1);
nz = U1(3,1);

% Solve for 2 sets of joint angles

for i = 1:2
if i == 1
sign_t1 = 1;
elseif i == 2
sign_t1 = -1;
end
k = (Px^2 + Py^2 + L1^2 - L2^2)/2/L1;
t1 = -atan2(Px,Py) + atan2(real(k),sign_t1*sqrt(Px^2+Py^2-real(k)^2));
c1 = cos(t1);
s1 = sin(t1);
c2 = (c1*Px+s1*Py-L1)/L2;
s2 = (c1*Py-s1*Px)/L2;
t2 = atan2(s2,c2);
c12 = cos(t1+t2);
s12 = sin(t1+t2);
c3 = nx*c12 + ny*s12;
s3 = ny*c12 - nx*s12;
t3 = atan2(s3,c3);

% Save result to matrix form and convert rad to deg

t1_a(i) = t1*180/pi;
t2_a(i) = t2*180/pi;
t3_a(i) = t3*180/pi;
end

fprintf('## Inverse Kinematics:\n')

VarNames = {'Theta1', 'Theta2','Theta3'};
Angles=table(t1_a', t2_a', t3_a', 'VariableNames',VarNames)

% Forward Kinematics re-check

for i = 1:2
theta1 = t1_a(i) *pi/180;
theta2 = t2_a(i) *pi/180;
theta3 = t3_a(i) *pi/180;

%Frame 0 to 1
a = 0;
alpha = 0;
d = 0;
theta = theta1;
T0f1 = HomoTrans(a,alpha,d,theta);

%Frame 1 to 2
 a = L1;
alpha = 0;
d = 0;
theta = theta2;
T1f2 = HomoTrans(a,alpha,d,theta);

%Frame 2 to 3
a = L2;
alpha = 0;
d = 0;
theta = theta3;
T2f3 = HomoTrans(a,alpha,d,theta);

%Frame 3 to H
a = L3;
alpha = 0;
d = 0;
theta = 0;
T3fH = HomoTrans(a,alpha,d,theta);

fprintf('## Forward Kinematics Re-check for Solution Set %d:\n',i)

T0fH = T0f1 * T1f2 * T2f3 *T3fH
end

% This function computes the homogeneous transform matrix from MDH params
function T = HomoTrans(a,alpha,d,theta)
T = [ cos(theta) -sin(theta) 0 a
sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d
sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) cos(alpha)*d
0 0 0 1 ];
end

Test Case (i):

Execution result:
## Input case:
T0fH =
1 0 0 9
0 1 0 0
0 0 1 0
0 0 0 1

## Inverse Kinematics:
Angles =
Theta1 Theta2 Theta3
______ ______ ______
0 0 0
0 0 0

## Forward Kinematics Re‐check for Solution Set 1:

T0fH =
1 0 0 9
0 1 0 0
0 0 1 0
0 0 0 1

## Forward Kinematics Re‐check for Solution Set 2:

T0fH =
1 0 0 9
0 1 0 0
0 0 1 0
0 0 0 1

Test Case (ii):

Execution result:
## Input case:
T0fH =
0.5000 ‐0.8660 0 7.5373
0.8660 0.6000 0 3.9266
0 0 1.0000 0
0 0 0 1.0000

## Inverse Kinematics:
Angles =
Theta1 Theta2 Theta3
______ ______ ______
9.9999 20 29.999
27.114 ‐20 52.885

## Forward Kinematics Re‐check for Solution Set 1:

T0fH =
0.5000 ‐0.8660 0 7.5373
0.8660 0.5000 0 3.9266
0 0 1.0000 0
0 0 0 1.0000

## Forward Kinematics Re‐check for Solution Set 2:

T0fH =
0.5000 ‐0.8660 0 7.5373
0.8660 0.5000 0 3.9266
0 0 1.0000 0
0 0 0 1.0000

It is found in this case that the element (2,2) should be 0.5. It is wrong because the norm of
every column and every row in a rotation matrix should equal to 1.

Test Case (iii):

Execution result:
## Input case:
T0fH =
0 1 0 ‐3
‐1 0 0 2
0 0 1 0
0 0 0 1

## Inverse Kinematics:
Angles =
Theta1 Theta2 Theta3
______ ______ ______
90 90 90
163.74 ‐90 ‐163.74

## Forward Kinematics Re‐check for Solution Set 1:

T0fH =
0.0000 1.0000 0 ‐3.0000
‐1.0000 0.0000 0 2.0000
0 0 1.0000 0
0 0 0 1.0000

## Forward Kinematics Re‐check for Solution Set 2:

T0fH =
‐0.0000 1.0000 0 ‐3.0000
‐1.0000 ‐0.0000 0 2.0000
0 0 1.0000 0
0 0 0 1.0000

Test Case (iv):

Execution result:
## Input case:
T0fH =
0.8660 0.5000 0 ‐3.1245
‐0.5000 0.8660 0 9.1674
0 0 1.0000 0
0 0 0 1.0000

Error using atan2

Inputs must be real.

Error in Matlab_EX_4 (line 57)

t1 = ‐atan2(Px,Py) + atan2(real(k),sign_t1*sqrt(Px^2+Py^2‐real(k)^2));

In this case, the program fail because the value of sqrt(Px^2+Py^2‐real(k)^2) is not a real
number. This means that the position (Px, Py) is out of reachability. By inspection we can
easily find that Py = 9.1674 > (L1+L2+L3) which is not reasonable.