% This exercise refers to the robot described in file 3dof_serial_robot.png
% =========================================================================
clc
clear all
% close all
% Refer to RoboticsAndMechatronics_IndustrialRobotics_part3_2021 slide 10

% Define parameters
% =========================================================================
% Lenghts of links
l1 = 1;
l2 = 1.2;
l3 = 1.2;
% Absolute angles of joints
phi1 = + 0.5 * pi;
phi2 = + 0.1 * pi;
phi3 = + 3.5/2 * pi;

% Calculate the position of the joints
% =========================================================================
% Set P0 in the origin
P0 = [ 0 ; 0 ];
% First calculate the vector z1 which corresponds to the 1st link
z1 = l1 * [ cos(phi1) ; sin(phi1) ];
% Calculate point P1
P1 = P0 + z1;
% Calculate point P2 from point P0 by adding the 2nd link vector z2
z2 = l2 * [ cos(phi2) ; sin(phi2) ];
P2 = P1 + z2;
% Do the same for point P3 by using z3
z3 = l3 * [ cos(phi3) ; sin(phi3) ];
% P3 = P1 + z2 + z3;
% P3 = P2 + z3;
P3 = P0 + z1 + z2 + z3;

% Display data
% =========================================================================
% Now assemble a matrix [2,4] with the positions in space of the joints
% P0-P3
jointsPositions = [P0 P1 P2 P3];
% Plot the configuration of the robot
% Plot a polyline to represent the robot frame
plot(jointsPositions(1,:), jointsPositions(2,:),'r')
% Plot the end-effector point
hold on
plot(P3(1), P3(2), 'bo')
daspect([1 1 1])
axis equal