 %UR5_mainPP.m
%% Data loading
clear
clc
% close all
addpath('functions')
addpath('geom')
addpath('PP_traj_data')
addpath('files')

traj = 5;               %Number of trajectory
backlash = 0*101*pi/180;    %[rad] joint 2

trajectory = ['PP',num2str(traj),'.mat'];
load(trajectory,'q_target','qd_target','qdd_target','I_actual')
Time = length(q_target)/125; %Being the sample frequency equal to 125Hz

[q0,qset] = testlaw_PP(q_target,qd_target,qdd_target,Time);
run parameters.m

%% Simulation
model = 'UR5_PHM_PP_newHD_tutti_giunti';
load_system(model)
tic 
out = sim(model); 
toc 

%% Saving
out_c = compress_data(out,1);  
temp = Sim.temp;  
mass = payload.m;
save(['dataCompressed/outC_t',num2str(traj),'_nomcond_HD_tutti'],'out_c','temp','-v7.3')
