%% setup configuration

clear;
clc;

% robot parameters
D_BASE    = 0.035;
L_BASE    = 0.109;
L_ARM     = 0.146;
D_ELBOW   = 0.052;
L_FOREARM = 0.115;
L_WRIST   = 0.072;

% DH-table (original)
% L1 = Link([   0,    L_BASE,  D_BASE, -pi/2,     0]);
% L2 = Link([   0,         0,   L_ARM,     0, -pi/2]);
% L3 = Link([   0,   D_ELBOW,       0,  pi/2,  pi/2]);
% L4 = Link([   0, L_FOREARM,       0, -pi/2,     0]);
% L5 = Link([   0,         0,       0,  pi/2,     0]);
% L6 = Link([   0,   L_WRIST,       0,     0,     0]);


% DH-table (after modification)
L1 = Link('d',    L_BASE, 'a',  D_BASE, 'alpha', -pi/2, 'offset',     0);
L2 = Link('d',         0, 'a',   L_ARM, 'alpha',     0, 'offset',  -pi/2);
L3 = Link('d',         0, 'a', D_ELBOW, 'alpha', -pi/2, 'offset',     0);
L4 = Link('d', L_FOREARM, 'a',       0, 'alpha',  pi/2, 'offset',     0);
L5 = Link('d',         0, 'a',       0, 'alpha', -pi/2, 'offset',     0);
L6 = Link('d',   L_WRIST, 'a',       0, 'alpha',     0, 'offset',    pi);


%% simulate robot

robot = SerialLink([L1 L2 L3 L4 L5 L6], 'name', 'Robot Arm');
figure(1);
teach(robot);
% robot.plot([0,0,0,0,0,0]);

%% built-in MATALAB function
% theta = [0, 0, 0, 0, 0, 0];
% p=robot.fkine(theta)

initialPose = [ 0, 0, 1, 0.222;
                0, 1, 0,     0;
               -1, 0, 0, 0.307;
                0, 0, 0,     1;];

Pose = [ 0, 0, 1, 0.250;
         0, 1, 0,     0;
        -1, 0, 0, 0.350;
         0, 0, 0,     1;];
 
mask = [1 1 1 1 1 1];
q = ikine(robot, Pose,'mask',mask);

if isempty(q)
   disp('No solution');
   
else
   disp('Solution found');
   figure();
   robot.plot(q);
   robot.display();
end

 


%% self defined function

DH_table = [     0,    L_BASE, D_BASE, -pi/2;
             -pi/2,         0,  L_ARM,     0;
              pi/2,   D_ELBOW,      0,  pi/2;
                 0, L_FOREARM,      0, -pi/2;
                 0,         0,      0,  pi/2;
                 0,   L_WRIST,      0,     0;];

