
clear all

% Link lengths ------------------------------------------------------------
 l1= 146; l1_1 = 81; l1_2 = 65; l2= 140; l3 = 47; % Link lenghts
 L = [l1 l1_1 l1_2 l2 l3];                                   % Vector L
% Global variables (only animation purposes!) -----------------------------
 global Hh P_trace
 Hh = (l1+l2+l3)*0.923; 
% Joint angular&speed limits ----------------------------------------------
 q_lim = [  -pi  -pi  -pi  -pi;...     % Joint physical limits
             pi   pi   pi  pi];        % Joint physical limits
 dq_max = [3 3 3 3 3 3]';              % Maximum joint angular velocities 

qi = [0 -10 0 80 -10 0];        % 0 -10 0 80 -10 0  <--->  0 0 -25.64
P = DirKin_6dof(qi);
P1 = P(:,4)';

XYZ_i = [0 0 -25.64];
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(1,:) = qf';

XYZ_i = [0 0 -10];          % inicial
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(3,:) = qf';
XYZ_i = [90 0 -10];         % final
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(4,:) = qf';

XYZ_i = [68 0 -16];         % inicial
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(6,:) = qf';
XYZ_i = [0 68 -16];
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(7,:) = qf';
XYZ_i = [-68 0 -16];
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(8,:) = qf';
XYZ_i = [0 -68 -16];        % final
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(9,:) = qf';

XYZ_i = [0 0 -25];          % inicial
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(11,:) = qf';
XYZ_i = [0 0 -10];         % final
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(12,:) = qf';

XYZ_i = [20 0 0];         % inicial
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(14,:) = qf';
XYZ_i = [20 0 -24];        % final
[qf,IsVal] = InvKin_6dof(XYZ_i);
q(15,:) = qf';

q