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

nSteps = 100;

% Fixed parameters
% =========================================================================
% Here we define the main parameters for the simulation.
% The lenght of the links:
l1 = .2;
l2 = .1;
l3 = .4;
l4 = .5;
l5 = .2;

% Joint parameters
% =========================================================================
% The angular joint positions we can get from the inverse kinematics
% analysis for the start and end poses. See the script
% "inverse_3Darm_workspaceTrajectory.m" for a reference on how to do it for
% this manipulator.
% Angles phi1,...,phi4 at start pose:
phi1_start =   0.00 *pi/180;
phi2_start =  70.65 *pi/180;
phi3_start = -91.43 *pi/180;
phi4_start = 110.78 *pi/180;
% Angles phi1,...,phi4 at end pose:
phi1_end =    90.00 *pi/180;
phi2_end =   102.47 *pi/180;
phi3_end =  -141.87 *pi/180;
phi4_end =   -20.60 *pi/180;

% Trajectory
% =========================================================================
% Here we assume a linear trajectory in joint-space between the start
% position angles and the end position angles:
phi1_array = linspace(phi1_start, phi1_end, nSteps);
phi2_array = linspace(phi2_start, phi2_end, nSteps);
phi3_array = linspace(phi3_start, phi3_end, nSteps);
phi4_array = linspace(phi4_start, phi4_end, nSteps);

% Base point
P0 = [0;0;0;1];

% Calculate links
% =========================================================================
% Here we define the links in their local frame of reference. Generally, we
% consider the link to extend along the x axis, while we consider joints to
% be aligned with the z axis. The location of the y axis generally follows.
r1 = [ 0 0 l1]';
r2 = [l2 0  0]';
r3 = [l3 0  0]';
r4 = [l4 0  0]';
r5 = [l5 0  0]';

P1_trajectory = zeros(3,nSteps);
P2_trajectory = zeros(3,nSteps);
P3_trajectory = zeros(3,nSteps);
P4_trajectory = zeros(3,nSteps);
PE_trajectory = zeros(3,nSteps);
for iStep = 1:nSteps
    % Calculate matrices
    % =====================================================================
    % Here we can apply the relevant rotations. In particular, for this
    % robot there is a simple rotation around the z axis between the frame
    % and the 1st link; a fixed 90 degree rotation around the x-axis
    % between the 1st and 2nd link, and then 3 rotations around the z-axes
    % of the next joints.
    A1 = [         Rz(phi1_array(iStep)) r1 ; 0 0 0 1];
    A2 = [Rx(pi/2)*Rz(phi2_array(iStep)) r2 ; 0 0 0 1];
    A3 = [         Rz(phi3_array(iStep)) r3 ; 0 0 0 1];
    A4 = [         Rz(phi4_array(iStep)) r4 ; 0 0 0 1];
    A5 = [         Rz(        0        ) r5 ; 0 0 0 1];

    % Calculate coordinates of points
    % =====================================================================
    % Apply the homogeneous transformation matrices
    % Note that the vectors [0 0 0 1]' are relative to the origin in each
    % local frame of reference. This way we can calculate the chain points
    % P1, P2,..., PE easily.
    % Also note the composition of homogeneous matrices.
    P1 = A1*[0 0 0 1]';
    P2 = A1*A2*[0 0 0 1]';
    P3 = A1*A2*A3*[0 0 0 1]';
    P4 = A1*A2*A3*A4*[0 0 0 1]';
    PE = A1*A2*A3*A4*A5*[0 0 0 1]';
    
    % Save in arrays
    P1_trajectory(:,iStep) = P1(1:3);
    P2_trajectory(:,iStep) = P2(1:3);
    P3_trajectory(:,iStep) = P3(1:3);
    P4_trajectory(:,iStep) = P4(1:3);
    PE_trajectory(:,iStep) = PE(1:3);

    % Concatenate points for display purposes
    points = [P0 P1 P2 P3 P4 PE];
    % Plot the configuration of the robot
    hold off
    plot3(points(1,:),points(2,:),points(3,:),'-o')
    hold on;
    plot3(P1_trajectory(1,1:iStep),P1_trajectory(2,1:iStep),P1_trajectory(3,1:iStep))
    plot3(P2_trajectory(1,1:iStep),P2_trajectory(2,1:iStep),P2_trajectory(3,1:iStep))
    plot3(P3_trajectory(1,1:iStep),P3_trajectory(2,1:iStep),P3_trajectory(3,1:iStep))
    plot3(P4_trajectory(1,1:iStep),P4_trajectory(2,1:iStep),P4_trajectory(3,1:iStep))
    plot3(PE_trajectory(1,1:iStep),PE_trajectory(2,1:iStep),PE_trajectory(3,1:iStep))
    %hold on
    daspect([1 1 1]);
    grid on
    xlim([-.1 1.2])
    ylim([-.1 1.2])
    zlim([0 1])
    view([140 30])
    xlabel('X axis')
    ylabel('Y axis')
    pause(.02)
end

% Plot joint variables
figure
plot(phi1_array)
hold on
plot(phi2_array)
plot(phi3_array)
plot(phi4_array)
xlabel('Steps')
ylabel('Joint angles [rad]')
legend('phi1','phi2','phi3','phi4')






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];