clc
clear all
% close all

% Refer to scara_3D_relative.png

% Define parameters
% =========================================================================
% Lenghts of links
l1 = .2;
l2 = .3;
l3 = .2;
l4 = .05; % Independent variable 3
% Absolute angles of joints
phi1 =  40 *pi/180; % Independent variable 1
phi2 = -50 *pi/180; % Independent variable 2

% Calculate the position of the joints
% =========================================================================
% Set P0 in the origin
P0 = [ 0 ; 0 ; 0];
% Links vectors
% Calculate the vector z1 which corresponds to the 1st link,
z1 = l1*[                0 ;                0 ;  1 ];
% ... and so on...
z2 = l2*[ cos(phi1       ) ; sin(phi1       ) ;  0 ];
z3 = l3*[ cos(phi1 + phi2) ; sin(phi1 + phi2) ;  0 ];
z4 = l4*[                0 ;                0 ; -1 ];
% Now calculate the position of nodes and joints
P1 = P0 + z1; % Shoulder
P2 = P1 + z2; % Elbow
P3 = P2 + z3; % Wrist
PE = P3 + z4; % End effector

% Display data
% =========================================================================
% Now assemble a matrix [2,4] with the positions in space of the joints
% P0-PE
jointsPositions = [P0 P1 P2 P3 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([0 0.3])