%% Exercise 1

P = [ 7 3 1 1]'

%

theta = 90

% rotation about z-axis with a theta angle 
R1= [cosd(theta) -sind(theta) 0 0
     sind(theta) cosd(theta) 0 0
     0             0          1 0
     0             0          0 1]

%

% rotation about y-axis
R2 = [cosd(theta)   0   sind(theta)   0
       0            1      0          0
       -sind(theta)  0   cosd(theta)   0
       0            0      0          1]

%

 % translation by [4 -3 7]
 T = [1 0 0  4
      0 1 0  -3
      0 0 1   7
      0 0 0   1]

%

% Now make the calculation
T*R2*R1*P

%
%% Exercise 2
%

% = Rot(z,θi)*Trans(z,di)* Rot(x,αi)* Trans(x,ri)
syms theta r d alpha

% Rot(z,θi) - rotation about z-axis with theta angle
R1 = [cos(theta) -sin(theta)   0 0
        sin(theta) cos(theta)  0 0
        0            0         1 0
        0            0         0 1]

% Trans(z,di)
T1 = [1 0 0 0
      0 1 0 0
      0 0 1 d
      0 0 0 1]

% Rot(x,αi) - rotation about x-axis with alpha angle
R2 = [1           0             0         0
      0     cos(alpha)    -sin(alpha)     0
      0     sin(alpha)    cos(alpha)      0 
      0           0           0           1]

% Trans(x,ri)
T2 = [1 0 0 r
      0 1 0 0
      0 0 1 0
      0 0 0 1]

DH = R1*T1*R2*T2

%
%% Exercise 3
%

DH
syms alpha1 alpha2 r1 r2 theta1 theta2 d1 d2
% DH matrix  for joint 1 
DH1 = [cos(theta1) -cos(alpha1)*sin(theta1) sin(alpha1)*sin(theta1) r1*cos(theta1)
       sin(theta1) cos(alpha1)*cos(theta1)  -sin(alpha1)*cos(theta1) r1*sin(theta1)
       0            sin(alpha1)                 cos(alpha1)           d1
       0              0                            0                  1]

% DH matrix for joint2
DH2 = [cos(theta2) -cos(alpha2)*sin(theta2) sin(alpha2)*sin(theta2) r2*cos(theta2)
       sin(theta2) cos(alpha2)*cos(theta2)  -sin(alpha2)*cos(theta2) r2*sin(theta2)
       0            sin(alpha2)                 cos(alpha2)           d2
       0              0                            0                  1]

% position of hand (i.e. end-effector or tool)
Position_of_end_effector  = DH1*DH2

%
%% Exercise 4
%
% Similar approach with Exercise 3. Now we have 3 joints therefore 3 DH matrices which need to be multiplied together to find the position of the end-effector of the 3-joints robot (3R).
%
%
%% Exercise 5
%
% Create your function in  a separate file. This is a Matlab requirement. The filename should be the same with the function name with the extension ".m" (without the quotes). E.g. function myfunction should be defined in  a file myfunction.m.
%
% E.g. the following Matlab code should be defined in a separate file dh.m:
% function res = dh(d, r, alpha, theta)
%    res  = [cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta)  r*cos(theta)
%            sin(theta) cos(alpha)*cos(theta)  -sin(alpha)*cos(theta) r*sin(theta)
%               0            sin(alpha)                 cos(alpha)           d
%               0              0                            0                1]
% end
%
%
%% Exercise 6
%
% Choose your own (some random) values for all the symbolic variables theta1, theta2, theta3, etc.
% and call your dh function created in Exercise 5, six times to calculate the DH matrices for the 6 joints:
%

theta1 = pi/2; theta2=0; theta3=pi/4; theta4=0, theta5=2*pi/3, theta6=0;
r2 = 5; r3 = 1
d3 = 2; d4 = 9
% DH matrix for joint 1
dh1 = dh(0, 0, 0, theta1)

% DH matrix for joint 2
%  convert -90 degrees to radians
dh2 = dh(0, 0, -pi/4, theta2)

% DH matrix for joint 3
dh3 = dh(d3, r2, 0, theta3)


% DH matrix for joint 4
dh4 = dh(d4, r3, -pi/4, theta4)


% DH matrix for joint 5
dh5 = dh(0, 0, pi/4, theta5)

% DH matrix for joint 6
dh6 = dh(0, 0, -pi/4, theta6)

position_of_end_effector_PUMA560 = dh1*dh2*dh3*dh4*dh5*dh6

%

models
mdl_puma560
p560.teach

%