%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ACTUAL VALUES
va1=1.0;
va2=1.0;
vd4=0.2; 
vA04 = subs(A04,{a1 a2 d4},{va1 va2 vd4})
vA04b = subs(A04b,{a1 a2 d4},{va1 va2 vd4})
vJ = subs(J,{a1 a2 d4},{va1 va2 vd4})
vJA = subs(JA,{a1 a2 d4},{va1 va2 vd4})

%The manipulator as a SerialLink
Lu(1) = Link('revolute', 'd', 0, 'a', va1, 'alpha', 0);
Lu(2) = Link('revolute', 'd', 0, 'a', va2, 'alpha', pi);
Lu(3) = Link('prismatic', 'theta', 0, 'a', 0, 'alpha', pi);
Lu(4) = Link('revolute', 'd', -vd4, 'a', 0, 'alpha', pi);
rob_a = SerialLink(Lu, 'name', 'scara')

Lu_b(1) = Link('revolute', 'd', 0, 'a', va1, 'alpha', 0);
Lu_b(2) = Link('revolute', 'd', 0, 'a', va2, 'alpha', 0);
Lu_b(3) = Link('prismatic', 'theta', 0, 'a', 0, 'alpha', 0);
Lu_b(4) = Link('revolute', 'd', -vd4, 'a', 0, 'alpha', pi);
rob_b = SerialLink(Lu_b, 'name', 'scara_b')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Direct kinematics verification
qa = [pi/4, pi/4,  0.1, pi/4]
qb = [pi/4, pi/4, -0.1, pi/4]
%qa = [0 0  0.1 0]
%qb = [0 0 -0.1 0]

% 
% vA01a = subs(A01,{a1 a2 d4},{va1 va2 vd4})
% vA01b = subs(A01b,{a1 a2 d4},{va1 va2 vd4})
% vT01a = subs(vA01a,{q1,q2,d3,q4},{qa})
% vT01b = subs(vA01b,{q1,q2,d3,q4},{qb})
% 
% vA02a = subs(A01*A12,{a1 a2 d4},{va1 va2 vd4})
% vA02b = subs(A01b*A12b,{a1 a2 d4},{va1 va2 vd4})
% vT02a = subs(vA02a,{q1,q2,d3,q4},{qa})
% vT02b = subs(vA02b,{q1,q2,d3,q4},{qb})
% 
% vA03a = subs(A01*A12*A23,{a1 a2 d4},{va1 va2 vd4})
% vA03b = subs(A01b*A12b*A23b,{a1 a2 d4},{va1 va2 vd4})
% vT03a = subs(vA03a,{q1,q2,d3,q4},{qa})
% vT03b = subs(vA03b,{q1,q2,d3,q4},{qb})
% 
% vA04a = subs(A01*A12*A23*A34,{a1 a2 d4},{va1 va2 vd4})
% vA04b = subs(A01b*A12b*A23b*A34b,{a1 a2 d4},{va1 va2 vd4})
% vT04a = subs(vA04a,{q1,q2,d3,q4},{qa})
% vT04b = subs(vA04b,{q1,q2,d3,q4},{qb})

Ta=rob_a.fkine(qa)
Tb=rob_b.fkine(qb)
format short
vT04a = subs(vA04,{q1,q2,d3,q4},{qa})
vT04b = subs(vA04b,{q1,q2,d3,q4},{qb})
verifyFK_a = Ta.inv*SE3(vT04a)
double(verifyFK_a.T)
verifyFK_b = Tb.inv*SE3(vT04b)
double(verifyFK_b.T)
verifyFK_ab = Tb.inv*SE3(Ta)
double(verifyFK_ab.T)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Jacobian verification
J1a = double(subs(vJ,{q1,q2,d3,q4},qa))
J1b = rob_a.jacob0(qa)
J1a_analytic = double(subs(vJA,{q1,q2,d3,q4},qa))
J1b_analytic = rob_a.jacob0(qa,'eul')
J1a_analytic_rpy = double(subs(vJArpy,{q1,q2,d3,q4},qa))
J1b_analytic_rpy = rob_a.jacob0(qa,'rpy')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Inverse kinematics verification

