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

% Define parameters
% =========================================================================
% Lenghts of links
l1 = .1;
l2 = .1;
l3 = .4;
l4 = .3;
l5 = .2;
% Absolute angles of joints
phi1 = 30 *pi/180;
phi2 = 30 *pi/180;
phi3 = 110 *pi/180;
phi4 = 30 *pi/180;

% Calculate the position of the joints
% =========================================================================
% Set P0 in the origin
P0 = [ 0 ; 0 ; 0 ];
% Calculate the vector zi which corresponds to the i-th link
z1 = [0 ; 0 ; l1];
z2 = Rz(phi1) * [0 ; 0 ; l2];
z3 = Rz(phi1)*Rx(phi2) * [0 ; 0 ; l3];
z4 = Rz(phi1)*Rx(phi2)*Rx(phi3) * [0 ; 0 ; l4];
z5 = Rz(phi1)*Rx(phi2)*Rx(phi3)*Rx(phi4) * [0 ; 0 ; l5];
% Use the vectors z1,...,z5 to calculate the joints positions
P1 = P0 + z1;
P2 = P1 + z2;
P3 = P2 + z3;
P4 = P3 + z4;
PE = P4 + z5;

% Display data
% =========================================================================
% Assemble a matrix [2,4] with the positions in space of the joints
% P0-P3
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([-72 14])



% Rotation functions
function M = Rx(phi)
% Rotation around axis x
M = [1 0         0 ; ...
     0 cos(phi) -sin(phi) ; ...
     0 sin(phi)  cos(phi)];

function M = Ry(phi)
% Rotation around axis y
M = [cos(phi) 0 sin(phi) ; ...
     0        1 0 ; ...
     sin(phi) 0 cos(phi)];

function M = Rz(phi)
% Rotation around axis z
M = [cos(phi) -sin(phi) 0 ; ...
     sin(phi)  cos(phi) 0 ; ...
     0          0       1];