%Plot.m

%% --------------------------- JOINT 1 ---------------------------------
%% JOINT 1: POSITION
figure()
plot(out.logsout.get('qSet_j1').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j1').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,1)*180/pi,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: MOTOR VELOCITY
% figure() 
% plot(out.logsout.get('qdmRef_j1').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j1').Values*60/2/pi,'LineWidth',1.5),grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Base','FontSize',24)

%% JOINT 1: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j1').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j1').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,1)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Base','FontSize',24')

%% JOINT 1: CURRENT
figure()
plot(out.logsout.get('iRef_j1').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iFB_j1').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,1),'LineWidth',1.5),grid minor
legend('iRef','iFB','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: ELECTROMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j1').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,1)*Motor.kc_1,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j1').Values,'LineWidth',1.5),grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j1').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Base','FontSize',24)

%% JOINT 1: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j1').Values,'LineWidth',1.5),grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% --------------------------- JOINT 2 ---------------------------------
%% JOINT 2: POSITION
figure() 
plot(out.logsout.get('qSet_j2').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j2').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,2)*180/pi+180,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: MOTOR VELOCITY
% figure()
% plot(out.logsout.get('qdmRef_j2').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j2').Values*60/2/pi,'LineWidth',1.5), grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Shoulder','FontSize',24)

%% JOINT 2: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j2').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j2').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,2)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Shoulder','FontSize',24')

%% JOINT 2: CURRENT [dq0]
figure()
plot(out.logsout.get('iRef_j2').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iq_j2').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('id_j2').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('i0_j2').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,2),'LineWidth',1.5),grid minor
legend('iRef','iq','id','i0','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: ELECTROMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j2').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,2)*Motor.kc_2,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j2').Values,'LineWidth',1.5), grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j2').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Shoulder','FontSize',24)

%% JOINT 2: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j2').Values,'LineWidth',1.5), grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------------- JOINT 3 ---------------------------------
%% JOINT 3: POSITION
figure()
plot(out.logsout.get('qSet_j3').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j3').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,3)*180/pi,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: MOTOR VELOCITY
% figure() 
% plot(out.logsout.get('qdmRef_j3').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j3').Values*60/2/pi,'LineWidth',1.5),grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Elbow','FontSize',24)

%% JOINT 3: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j3').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j3').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,3)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Elbow','FontSize',24')

%% JOINT 3: CURRENT
figure()
plot(out.logsout.get('iRef_j3').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iFB_j3').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,3),'LineWidth',1.5),grid minor
legend('iRef','iFB','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: ELECTROMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j3').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,3)*Motor.kc_3,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j3').Values,'LineWidth',1.5),grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j3').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Elbow','FontSize',24)

%% JOINT 3: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j3').Values,'LineWidth',1.5),grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% --------------------------- JOINT 4 ---------------------------------
%% JOINT 4: POSITION
figure()
plot(out.logsout.get('qSet_j4').Values*180/pi,'--r','LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j4').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,4)*180/pi+180,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: MOTOR VELOCITY
% figure() 
% plot(out.logsout.get('qdmRef_j4').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j4').Values*60/2/pi,'LineWidth',1.5),grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j4').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j4').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,4)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 1','FontSize',24')

%% JOINT 4: CURRENT
figure()
plot(out.logsout.get('iRef_j4').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iFB_j4').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,4),'LineWidth',1.5),grid minor
legend('iRef','iFB','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: ELECTROMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j4').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,4)*Motor.kc_4,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j4').Values,'LineWidth',1.5),grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j4').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j4').Values,'LineWidth',1.5),grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% --------------------------- JOINT 5 ---------------------------------
%% JOINT 5: POSITION
figure()
plot(out.logsout.get('qSet_j5').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j5').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,5)*180/pi,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: MOTOR VELOCITY
% figure() 
% plot(out.logsout.get('qdmRef_j5').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j5').Values*60/2/pi,'LineWidth',1.5),grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j5').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j5').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,5)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 2','FontSize',24')

%% JOINT 5: CURRENT
figure()
plot(out.logsout.get('iRef_j5').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iFB_j5').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,5),'LineWidth',1.5),grid minor
legend('iRef','iFB','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: ELECTROMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j5').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,5)*Motor.kc_5,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j5').Values,'LineWidth',1.5),grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j5').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j5').Values,'LineWidth',1.5),grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% --------------------------- JOINT 6 ---------------------------------
%% JOINT 6: POSITION
figure()
plot(out.logsout.get('qSet_j6').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('qFB_j6').Values*180/pi,'LineWidth',1.5)
hold on
plot(time(1:1250),q_actual(1:1250,6)*180/pi,'LineWidth',1.5),grid minor
legend('qSet','qFB','qActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: MOTOR VELOCITY
% figure() 
% plot(out.logsout.get('qdmRef_j6').Values*60/2/pi,'LineWidth',1.5)
% hold on
% plot(out.logsout.get('qdmFB_j6').Values*60/2/pi,'LineWidth',1.5),grid minor
% legend('qdmRef','qdmFB','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Motor velocity [RPM]','FontSize',20)
% title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: JOINT VELOCITY
figure() 
plot(out.logsout.get('qdmRef_j6').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(out.logsout.get('qdmFB_j6').Values*180/pi/101,'LineWidth',1.5)
hold on
plot(time(1:1250),qd_actual(1:1250,6)*180/pi,'LineWidth',1.5),grid minor
legend('qdRef','qdFB','qdActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 3','FontSize',24')

%% JOINT 6: CURRENT
figure()
plot(out.logsout.get('iRef_j6').Values,'LineWidth',1.5)
hold on
plot(out.logsout.get('iFB_j6').Values,'LineWidth',1.5)
hold on 
plot(time(1:1250),I_actual_f(1:1250,6),'LineWidth',1.5),grid minor
legend('iRef','iFB','iActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: ELECTOMAGNETIC TORQUE
figure() 
plot(out.logsout.get('Tem_j6').Values,'LineWidth',1.5)
hold on
plot(time(1:1250),I_actual_f(1:1250,6)*Motor.kc_6,'LineWidth',1.5),grid minor
legend('Tem','TemActual','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)
%% JOINT 6: BACKLASH TORQUE
figure() 
plot(out.logsout.get('C_j6').Values,'LineWidth',1.5),grid minor
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: Tem-C
% figure() 
% plot(out.logsout.get('Tem-C_j6').Values,'LineWidth',1.5),grid minor
% legend('Tem-C','FontSize',18)
% xlabel('Time [s]','FontSize',20)
% ylabel('Torque [Nm]','FontSize',20)
% title('Joint: Wrist 6','FontSize',24)

%% JOINT 6: UTIL TORQUE
figure() 
plot(out.logsout.get('Tu_j6').Values,'LineWidth',1.5),grid minor
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% TCP
figure()
plot(out.logsout.get('TCP_x').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('TCP_y').Values*180/pi,'LineWidth',1.5)
hold on
plot(out.logsout.get('TCP_z').Values*180/pi,'LineWidth',1.5),grid minor
legend('x','y','z','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('TCP coordinates [deg]','FontSize',20)
title('Tool Center Point','FontSize',24)

