% Problem 3.6

% Notation: {k} means coordinate system k. 
%           The Original/start coordinate system is therefore denoted {0}.

% First we must assign the DH parameters.
% In {0}, we can choose the x axis to be any direction
% orthogonal/perpendicular to the z axis.
%
% In this case, the x axis in {0} is pointing to the right,
% and the z axis in {0} is pointing upwards.
% This gives us the following DH parameter table:
%
%   Link    a_i alfa_i  d_i theta_i
%      1      0  -pi/2   L1  theta1
%      2     L2      0    0  theta2
%      3     L3      0    0  theta3

% Link 1, 2 and 3 gives the matrices A1, A2 and A3 respectively.
% In fact A1 is the homogeneous transformation from {1} to {0}.
% We can give A1 a more describing variable name: H_1to0 := A1.
% A2 is from {2} to {1}, so H_2to1 := A2.
% A3 is from {3} to {2}, so H_3to2 := A3.

% The transformation from the end effector to the origin ({3} to {0} is:
% H_3to0 = H_1to0*H_2to1*H3to2 = A1*A2*A3

% Calculating H_3to0 by hand is tedious, and prone to calculation errors.
% To calculate H_3to0, we must learn how to use the symbolic toolbox.
% The reason for this is that L1, L2, L3, theta1, theta2 and theta3
% are not unknown values.

% First we define the symbols we need uing syms.
% r1, r2 and r3 are theta1, theta2, theta3 (only more compact).
syms L1 L2 L3 r1 r2 r3

% In some cases you might encounter problems as the symbols are not
% assumed to be real. You would get conj() functions.
% To avoid this, you can declare each symbol in this alternative way:
% L1 = sym('L1', 'real')

% We will use the function DHmatrix to calculate A1, A2 and A3.
% Ai = DHmatrix(a_i, alfa_i, d_i, theta_i)
A1 = DHmatrix(0, sym(-pi/2), L1, r1);
A2 = DHmatrix(L2, 0, 0, r2);
A3 = DHmatrix(L3, 0, 0, r3);

% Note that I use sym(-pi/2). This is to ensure that -pi/2 is interpreted
% as the expression -pi/2, and not it's rounded off numeric value.

H_3to0 = A1*A2*A3

% H_3to0 can be simplified. The function simple, simplifies symbolic
% expressions, and the result is very good.
H_3to0_simple = simple(H_3to0)

% simplify(H_3to0) will also simplify the expressions.


% Say we want to calculate H_3to0 with 
%   theta1 = theta2 = theta3 = 0, and
%   L1 = L2 = L3 = 1
% To do this, we assign these values to the symbolic variables:
L1 = 1;
L2 = 1;
L3 = 3;
r1 = 0;
r2 = 0;
r3 = 0;

% L1, L2, L3, r1, r2 and r3 are now numeric values, and not symbols.
% To get the value of H_3to0 with these values we write:
H_3to0_evalueted = eval(H_3to0_simple)
