function direct_3Darm_homogeneous()
% This exercise is relative to the figure in file
% 4dof_robot_relative_v2.png
clc
% close all

% Define parameters
% =========================================================================
% Lenghts of links
l1 = .1;
l2 = .1;
l3 = .4;
l4 = .3;
l5 = .2;
phi1 = 0 *pi/180;
phi2 = 60 *pi/180;
phi3 = 110 *pi/180;
phi4 = 50 *pi/180;


% Calculate the position of the joints
% =========================================================================
% Set P0 in the origin in homogeneous vector form
P0 = [ 0 ; 0 ; 0 ; 1];

% Calculate links vectors r1,...,r5 in the local frame
r1 = [0 0 l1]';
r2 = [l2 0 0]';
r3 = [l3 0 0]';
r4 = [l4 0 0]';
r5 = [l5 0 0]';

% Calculate homogeneous transformation matrices (HTM) from
% frame i to frame i+1:
% Frame 0 to frame 1
A1 = [Rz(phi1)          r1 ; 0 0 0 1];
% Frame 1 to frame 2
A2 = [Rx(pi/2)*Rz(phi2) r2 ; 0 0 0 1];
% Frame 2 to frame 3
A3 = [Rz(phi3)          r3 ; 0 0 0 1];
% Frame 3 to frame 4
A4 = [Rz(phi4)          r4 ; 0 0 0 1];
% Frame 4 to frame 5
A5 = [Rz(  0 )          r5 ; 0 0 0 1];

% Now calculate the HTM from frame 0 to frame i
A01 = A1;
A02 = A1*A2;
A03 = A1*A2*A3;
A04 = A1*A2*A3*A4;
A05 = A1*A2*A3*A4*A5;

% Calculate coordinates of points by multiplying the HTM by a null vector
% in homogeneous form ([0;0;0;1])
P1 = A01*[0 0 0 1]';
P2 = A02*[0 0 0 1]';
P3 = A03*[0 0 0 1]';
P4 = A04*[0 0 0 1]';
PE = A05*[0 0 0 1]';


% Display data
% =========================================================================
% Now assemble a matrix [2,4] with the positions in space of the joints
% P0-PE
jointsPositions = [P0 P1 P2 P3 P4 PE];
% Plot the configuration of the robot
% Plot a polyline to represent the robot frame
plot3(jointsPositions(1,:),jointsPositions(2,:),jointsPositions(3,:),'-o')
% Plot the end-effector point
hold on
plot3(PE(1),PE(2),PE(3),'ro')
daspect([1 1 1]);
grid on
xlim([-1 1])
ylim([-1 1])
zlim([-1 1])
view([-26 18])




function M = Rx(phi)
M = [1 0         0 ; ...
     0 cos(phi) -sin(phi) ; ...
     0 sin(phi)  cos(phi)];

function M = Ry(phi)
M = [cos(phi) 0 sin(phi) ; ...
     0        1 0 ; ...
     sin(phi) 0 cos(phi)];

function M = Rz(phi)
M = [cos(phi) -sin(phi) 0 ; ...
     sin(phi)  cos(phi) 0 ; ...
     0          0       1];