
clear all
clc

lt = 48; lt_l= 194; lt_w = 49; lt_h = 107; % Trunk's dimensions
lr = 5; l0 = 43;                         % Cylinders & shoulder
l1= 126; l1_1 = 77; l1_2 = 49; l2= 108;   % Arm's link lenghts
L = [lt lt_l lt_w lt_h lr l0 l1 l1_1 l1_2 l2 ];% Vector L

% Initial joint angles ----------------------------------------------------
qi_l = [0; 0; 0; pi/2];                % Left arm initial configuration
qi_r = [0; 0; 0; -pi/2];               % Right arm initial configuration
% Joint angular&speed limits ----------------------------------------------
q_lim = [  -pi/4  0   -pi/2  0;...     % Joint physical limits
          3*pi/4  pi   pi/2  3*pi/4];  % Joint physical limits
dq_max = [2 2 2 2]';                   % Maximum joint angular velocities

warm = 1;       % braço esquerdo

%XYZ_f(1,:) = [0 lt_l/2+l0 lt+lt_h-lr-l1-l2];   % [0 0 0 0]
%XYZ_f(1,:) = [l2 lt_l/2+l0 lt+lt_h-lr-l1];     % [0 0 0 90]

disp('** RESULTS 1 [0 0 0 0]**  ---------------------------------------');
qi_l = [0; 0; 0; 0];     % colocar em graus
qi_l = qi_l*pi/180;
P_link_l = DirKin_4dof(qi_l,L,+1);
xyz_real= (P_link_l(:,3))'
XYZ_f(1,:) = P_link_l(:,3)';
[qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,warm);
qf = (qf_sol*180/pi);
if warm == 1
    qf(1) = qf(1)-45;
	qf(2) = qf(2)-85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)+85;
else
    qf(1) = qf(1)+45;
	qf(2) = qf(2)+85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)-90;
end
ang_real= qf'

disp('** RESULTS 2 [0 0 0 90]**  ---------------------------------------');
qi_l = [0; 0; 0; 90];     % colocar em graus
qi_l = qi_l*pi/180;
P_link_l = DirKin_4dof(qi_l,L,+1);
xyz_real= (P_link_l(:,3))'
XYZ_f(1,:) = P_link_l(:,3)';
[qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,warm);
qf = (qf_sol*180/pi);
if warm == 1
    qf(1) = qf(1)-45;
	qf(2) = qf(2)-85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)+85;
else
    qf(1) = qf(1)+45;
	qf(2) = qf(2)+85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)-90;
end
ang_real= qf'

disp('** RESULTS 3 [0 45 0 0]**  ---------------------------------------');
qi_l = [0; 45; 0; 0];     % colocar em graus
qi_l = qi_l*pi/180;
P_link_l = DirKin_4dof(qi_l,L,+1);
xyz_real= (P_link_l(:,3))'
XYZ_f(1,:) = P_link_l(:,3)';
[qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,warm);
qf = (qf_sol*180/pi);
if warm == 1
    qf(1) = qf(1)-45;
	qf(2) = qf(2)-85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)+85;
else
    qf(1) = qf(1)+45;
	qf(2) = qf(2)+85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)-90;
end
ang_real= qf'

disp('** RESULTS 4 [90 0 0 0]**  ---------------------------------------');
qi_l = [90; 0; 0; 0];     % colocar em graus
qi_l = qi_l*pi/180;
P_link_l = DirKin_4dof(qi_l,L,+1);
xyz_real= (P_link_l(:,3))'
XYZ_f(1,:) = P_link_l(:,3)';
[qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,warm);
qf = (qf_sol*180/pi);
if warm == 1
    qf(1) = qf(1)-45;
	qf(2) = qf(2)-85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)+85;
else
    qf(1) = qf(1)+45;
	qf(2) = qf(2)+85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)-90;
end
ang_real= qf'

disp('** RESULTS 5 [90 90 0 90]**  ---------------------------------------');
qi_l = [90; 90; 0; 90];     % colocar em graus
qi_l = qi_l*pi/180;
P_link_l = DirKin_4dof(qi_l,L,+1);
xyz_real= (P_link_l(:,3))'
XYZ_f(1,:) = P_link_l(:,3)';
[qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,warm);
qf = (qf_sol*180/pi);
if warm == 1
    qf(1) = qf(1)-45;
	qf(2) = qf(2)-85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)+85;
else
    qf(1) = qf(1)+45;
	qf(2) = qf(2)+85;
	qf(3) = qf(3)+0;
	qf(4) = -qf(4)-90;
end
ang_real= qf'