clear all
close all
clc

% Hoai Nam Huynh
% Compare torque measurements
% to fitted torques with the identified parameters

%%%%%%%%%%%%%%%%%%%%%%%%
% Loading measurements %
%%%%%%%%%%%%%%%%%%%%%%%%

% Acquisition parameters
Frequency = 250; % [Hz]
Traj_Duration = 15; % [s]

% At which instant in the trajectory the fitting starts
% then, fitting over "Traj_Duration" seconds
Start_Fitting = 35; % [%] at X [%] of the total duration of the trajectory
% if above 50 %, it will reduce at 50 %

% Load the identified parameters in 'param' variable
load('Identified_27_Parameters_Kuka.mat');

% Robot dimensions (Kuka UVic)
a1=0.35;
a2=1.35;
kr1=256.8732408;
kr2=267.4358921;
kr3=252.3303991;
g=-9.81;

% Just need one measurement for each trajectory
% to compare the fitted torque
% it is not the identification

% Measured trajectories (that need to be filtered)

% File 
FileName='Kuka_Measurement_11';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S11=data(:,1:3);
V11=data(:,4:6);
T11=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_21';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S21=data(:,1:3);
V21=data(:,4:6);
T21=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_31';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S31=data(:,1:3);
V31=data(:,4:6);
T31=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_41';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S41=data(:,1:3);
V41=data(:,4:6);
T41=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_41';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S41=data(:,1:3);
V41=data(:,4:6);
T41=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_51';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S51=data(:,1:3);
V51=data(:,4:6);
T51=data(:,7:9);
clear datacell

% File 
FileName='Kuka_Measurement_81';
% Skip Header of Data file
fmt = repmat('%f',1,9);
fid = fopen(strcat(FileName, '.txt'), 'rt');
datacell = textscan(fid, fmt, 'Delimiter', ',', 'HeaderLines', 1, 'CollectOutput', 1);
fclose(fid);
data = datacell{1};
S81=data(:,1:3);
V81=data(:,4:6);
T81=data(:,7:9);
clear datacell

% Time vector
Time_Vector = zeros(Frequency*Traj_Duration,1);
for index_t = 2 : size(Time_Vector,1)+1
    Time_Vector(index_t,1)=Time_Vector(index_t-1,1)+(1.0/Frequency);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filtering & Differentiation %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filtering of the torque
% and derivation of the acceleration from the velocity
% for all the trajetories

% Filter order parameter
f1=3;
f2=4;

% Trajectory 1
[~,Qv1_1,Qv1_2,Qv1_3,~,~,~,~,~,~]=fftfilter_Hamed(S11',f1,f2);
[~,Qv1_1_d,Qv1_2_d,Qv1_3_d,Qv1_1_dd,Qv1_2_dd,Qv1_3_dd,~,~,~]=fftfilter_Hamed(V11',f1,f2);
[~,Tv1_1,Tv1_2,Tv1_3,~,~,~,~,~,~]=fftfilter_Hamed(T11',f1,f2);
close all % Figures

% Trajectory 2
[~,Qv2_1,Qv2_2,Qv2_3,~,~,~,~,~,~]=fftfilter_Hamed(S21',f1,f2);
[~,Qv2_1_d,Qv2_2_d,Qv2_3_d,Qv2_1_dd,Qv2_2_dd,Qv2_3_dd,~,~,~]=fftfilter_Hamed(V21',f1,f2);
[~,Tv2_1,Tv2_2,Tv2_3,~,~,~,~,~,~]=fftfilter_Hamed(T21',f1,f2);
close all % Figures

% Trajectory 3
[~,Qv3_1,Qv3_2,Qv3_3,~,~,~,~,~,~]=fftfilter_Hamed(S31',f1,f2);
[~,Qv3_1_d,Qv3_2_d,Qv3_3_d,Qv3_1_dd,Qv3_2_dd,Qv3_3_dd,~,~,~]=fftfilter_Hamed(V31',f1,f2);
[~,Tv3_1,Tv3_2,Tv3_3,~,~,~,~,~,~]=fftfilter_Hamed(T31',f1,f2);
close all % Figures

% Trajectory 4
[~,Qv4_1,Qv4_2,Qv4_3,~,~,~,~,~,~]=fftfilter_Hamed(S41',f1,f2);
[~,Qv4_1_d,Qv4_2_d,Qv4_3_d,Qv4_1_dd,Qv4_2_dd,Qv4_3_dd,~,~,~]=fftfilter_Hamed(V41',f1,f2);
[~,Tv4_1,Tv4_2,Tv4_3,~,~,~,~,~,~]=fftfilter_Hamed(T41',f1,f2);
close all % Figures

% Trajectory 5
[~,Qv5_1,Qv5_2,Qv5_3,~,~,~,~,~,~]=fftfilter_Hamed(S51',f1,f2);
[~,Qv5_1_d,Qv5_2_d,Qv5_3_d,Qv5_1_dd,Qv5_2_dd,Qv5_3_dd,~,~,~]=fftfilter_Hamed(V51',f1,f2);
[~,Tv5_1,Tv5_2,Tv5_3,~,~,~,~,~,~]=fftfilter_Hamed(T51',f1,f2);
close all % Figures

% Trajectory 8
[~,Qv8_1,Qv8_2,Qv8_3,~,~,~,~,~,~]=fftfilter_Hamed(S81',f1,f2);
[~,Qv8_1_d,Qv8_2_d,Qv8_3_d,Qv8_1_dd,Qv8_2_dd,Qv8_3_dd,~,~,~]=fftfilter_Hamed(V81',f1,f2);
[~,Tv8_1,Tv8_2,Tv8_3,~,~,~,~,~,~]=fftfilter_Hamed(T81',f1,f2);
close all % Figures

%%%%%%%%%%%%%%%%%%%%%%%%%
% Trimming trajectories %
%%%%%%%%%%%%%%%%%%%%%%%%%

% Trimming of the trajectories for 15 s of fitting
if (Start_Fitting > 50)
    Start_Fitting = 50;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generation of fitted torque %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ALL DATA in MAYEDA's JOINT FRAME (for the paper)

% Computing fitting torques for each trajectory
% Identification trajectories
Regressor_Traj1;  % Call of Matlab script
CurrentFittedTorque=A*Param;
Measured_Torque_Traj1 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj1 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

Regressor_Traj2;
CurrentFittedTorque=A*Param;
Measured_Torque_Traj2 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj2 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

Regressor_Traj3;
CurrentFittedTorque=A*Param;
Measured_Torque_Traj3 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj3 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

Regressor_Traj4;
CurrentFittedTorque=A*Param;
Measured_Torque_Traj4 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj4 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

% Validation trajectories
Regressor_Traj5;
CurrentFittedTorque=A*Param;
Measured_Torque_Traj5 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj5 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

Regressor_Traj8;
CurrentFittedTorque=A*Param;
Measured_Torque_Traj8 = [Time_Vector(:,1) -Tv1' -Tv2' -Tv3'];
Fitted_Torque_Traj8 = [Time_Vector(:,1) CurrentFittedTorque(1:3:3*N-2) CurrentFittedTorque(2:3:3*N-1) CurrentFittedTorque(3:3:3*N)];

%%%%%%%%%%%%%%%%%%%%%%%
% Plots for the paper %
%%%%%%%%%%%%%%%%%%%%%%%

% Plotting fitted trajectories used for the identification
h=figure(1);
set(h, 'Position', [100, 100, 800, 800]); 

% Trajectory 1
subplot(4,3,1)
plot(Time_Vector(:,1),Measured_Torque_Traj1(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj1(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,2)
plot(Time_Vector(:,1),Measured_Torque_Traj1(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj1(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,3)
plot(Time_Vector(:,1),Measured_Torque_Traj1(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj1(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

% Trajectory 2
subplot(4,3,4)
plot(Time_Vector(:,1),Measured_Torque_Traj2(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj2(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,5)
plot(Time_Vector(:,1),Measured_Torque_Traj2(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj2(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,6)
plot(Time_Vector(:,1),Measured_Torque_Traj2(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj2(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

% Trajectory 3
subplot(4,3,7)
plot(Time_Vector(:,1),Measured_Torque_Traj3(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj3(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,8)
plot(Time_Vector(:,1),Measured_Torque_Traj3(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj3(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,9)
plot(Time_Vector(:,1),Measured_Torque_Traj3(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj3(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

% Trajectory 4
subplot(4,3,10)
plot(Time_Vector(:,1),Measured_Torque_Traj4(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj4(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,11)
plot(Time_Vector(:,1),Measured_Torque_Traj4(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj4(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(4,3,12)
plot(Time_Vector(:,1),Measured_Torque_Traj4(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj4(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

% Plotting fitted trajectories used for the VALIDATION
h=figure(2);
set(h, 'Position', [100, 100, 800, 400]); 

% Trajectory 5
subplot(2,3,1)
plot(Time_Vector(:,1),Measured_Torque_Traj5(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj5(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(2,3,2)
plot(Time_Vector(:,1),Measured_Torque_Traj5(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj5(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(2,3,3)
plot(Time_Vector(:,1),Measured_Torque_Traj5(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj5(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

% Trajectory 5
subplot(2,3,4)
plot(Time_Vector(:,1),Measured_Torque_Traj8(:,2),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj8(:,2),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(2,3,5)
plot(Time_Vector(:,1),Measured_Torque_Traj8(:,3),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj8(:,3),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')

subplot(2,3,6)
plot(Time_Vector(:,1),Measured_Torque_Traj8(:,4),'k',...
     Time_Vector(:,1),Fitted_Torque_Traj8(:,4),'--k');
xlabel('Time [s]')
ylabel('Torque [Nm]')
