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


% Fixed parameters
% =========================================================================
l2 = 0.40;
phi2 = -60 *pi/180;
l3 = 0.20;

% 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, phi3 and l1 at start pose:
phi1_start =  75.00 *pi/180;
l1_start   =   0.30;
phi3_start = -90.00 *pi/180;
% Angles phi1, phi4 and l1 at end pose:
phi1_end =   110.00 *pi/180;
l1_end   =     0.10;
phi3_end =   -20.00 *pi/180;

% Trajectory
% =========================================================================
% Here we assume a linear trajectory in joint-space between the start
% position angles and the end position angles:
nSteps = 100;
% Define arrays
phi1_array = linspace(phi1_start, phi1_end, nSteps);
l1_array = linspace(l1_start, l1_end, nSteps);
phi3_array = linspace(phi3_start, phi3_end, nSteps);

% Base point
P0 = [0 ; 0];

% Initialize arrays for speed of execution
P1_trajectory = zeros(2,nSteps);
P2_trajectory = zeros(2,nSteps);
P3_trajectory = zeros(2,nSteps);
for iStep = 1:nSteps
    phi1 = phi1_array(iStep);
    l1 = l1_array(iStep);
    phi3 = phi3_array(iStep);
    % Calculate coordinates of points
    P1 = l1 * [cos(phi1) ; sin(phi1)];
    P2 = P1 + l2*[cos(phi1 + phi2) ; sin(phi1 + phi2)];
    P3 = P2 + l3*[cos(phi1 + phi2 + phi3) ; sin(phi1 + phi2 + phi3)];
    
    % Save in arrays
    P1_trajectory(:,iStep) = P1;
    P2_trajectory(:,iStep) = P2;
    P3_trajectory(:,iStep) = P3;
    phi1_array(iStep) = phi1;
    l1_array(iStep) = l1;
    phi3_array(iStep) = phi3;

    % Concatenate points for display purposes
    points = [P0 P1 P2 P3];
    % Plot the configuration of the robot
    hold off
    plot(points(1,:),points(2,:),'-o')
    hold on;
    plot(P1_trajectory(1,1:iStep),P1_trajectory(2,1:iStep))
    plot(P2_trajectory(1,1:iStep),P2_trajectory(2,1:iStep))
    plot(P3_trajectory(1,1:iStep),P3_trajectory(2,1:iStep))
    %hold on
    daspect([1 1 1]);
    grid on
    xlim([-.3 1.2])
    ylim([-.3 1.2])
    xlabel('X axis')
    ylabel('Y axis')
    drawnow
end

% Plot joint variables
figure
plot(l1_array)
hold on
plot(phi1_array)
plot(phi3_array)
xlabel('Steps')
ylabel('Parameter values')
legend('l1','\phi_1','\phi_3')