%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ACTUAL VALUES
va2=1.0;
vd4=1.0;
vd6=0.0; 
vA06 = subs(A06,{a2 d4 d6},{va2 vd4 vd6})
vJ = subs(J,{a2 d4 d6},{va2 vd4 vd6})

%The manipulator as a SerialLink
Lu(1) = Link([0   0   0  pi/2]); %[revolute d a alpha]
Lu(2) = Link([0   0 va2     0]);
Lu(3) = Link([0   0   0  pi/2]);
Lu(4) = Link([0 vd4   0 -pi/2]);
Lu(5) = Link([0   0   0  pi/2]);
Lu(6) = Link([0 vd6   0     0]);
rob = SerialLink(Lu, 'name', 'sixmanip')
rob.links(3).offset = pi/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Direct kinematics verification
%qa = [0,pi/4,pi/4,0,pi/4,0]
qa = [pi/4,pi/3,pi/2,0,pi/4,0]
T3=rob.fkine(qa)
format short
vT06 = subs(vA06,{q1,q2,q3,q4,q5,q6},{qa})
verifyFK = T3.inv*SE3(vT06)
double(verifyFK.T)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Jacobian verification
J1a = double(subs(vJ,{q1,q2,q3,q4,q5,q6},qa));
J1b = rob.jacob0(qa);
inv(J1a)*J1b

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Inverse kinematics verification
vpe = T3.t
vRe = T3.R
%a3 is substituted by d4 (see page77)
pos(1,:) = double(subs(solspos(1,:),{pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

pos(2,:) = double(subs(solspos(2,:),{pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

pos(3,:) = double(subs(solspos(3,:),{pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

pos(4,:) = double(subs(solspos(4,:),{pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(1,:) = double(subs(solsori(1,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(1,1) pos(1,2) pos(1,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(2,:) = double(subs(solsori(1,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(2,1) pos(2,2) pos(2,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(3,:) = double(subs(solsori(1,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(3,1) pos(3,2) pos(3,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(4,:) = double(subs(solsori(1,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(4,1) pos(4,2) pos(4,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(5,:) = double(subs(solsori(2,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(1,1) pos(1,2) pos(1,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(6,:) = double(subs(solsori(2,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(2,1) pos(2,2) pos(2,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(7,:) = double(subs(solsori(2,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(3,1) pos(3,2) pos(3,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

ori(8,:) = double(subs(solsori(2,:),{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(4,1) pos(4,2) pos(4,3) vpe(1) vpe(2) vpe(3) ...
    vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))


sols(1,:) = [pos(1,:) ori(1,:)]
sols(2,:) = [pos(2,:) ori(2,:)]
sols(3,:) = [pos(3,:) ori(3,:)]
sols(4,:) = [pos(4,:) ori(4,:)]
sols(5,:) = [pos(1,:) ori(5,:)]
sols(6,:) = [pos(2,:) ori(6,:)]
sols(7,:) = [pos(3,:) ori(7,:)]
sols(8,:) = [pos(4,:) ori(8,:)]

rob.fkine(rob.ikine(T3))
rob.fkine(sols(1,:))
rob.fkine(sols(2,:))
rob.fkine(sols(3,:))
rob.fkine(sols(4,:))
rob.fkine(sols(5,:))
rob.fkine(sols(6,:))
rob.fkine(sols(7,:))
rob.fkine(sols(8,:))


% double(subs(R03,{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(1,1) pos(1,2) pos(1,3) vpe(1) vpe(2) vpe(3) ...
%     vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))
% double(subs(R03,{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(2,1) pos(2,2) pos(2,3) vpe(1) vpe(2) vpe(3) ...
%     vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))
% double(subs(R03,{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(3,1) pos(3,2) pos(3,3) vpe(1) vpe(2) vpe(3) ...
%     vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))
% double(subs(R03,{q1 q2 q3 pex pey pez nx ny nz sx sy sz ax ay az a2 a3 d6},{pos(4,1) pos(4,2) pos(4,3) vpe(1) vpe(2) vpe(3) ...
%     vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3) va2 vd4 vd6}))

rob.plot(sols(1,:))
figure
rob2 = SerialLink(rob, 'name', 'rob2');
rob2.plot(sols(2,:))
figure
rob3 = SerialLink(rob, 'name', 'rob3');
rob3.plot(sols(3,:))
figure
rob4 = SerialLink(rob, 'name', 'rob4');
rob4.plot(sols(4,:))
figure
rob5 = SerialLink(rob, 'name', 'rob5');
rob5.plot(sols(5,:))
figure
rob6 = SerialLink(rob, 'name', 'rob6');
rob6.plot(sols(6,:))
figure
rob7 = SerialLink(rob, 'name', 'rob7');
rob7.plot(sols(7,:))
figure
rob8 = SerialLink(rob, 'name', 'rob8');
rob8.plot(sols(8,:))
