%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% verification using the Robotics toolbox

%get the gravity used
vg = twolink.gravity(2)
%set the COM
vl1 = vsa1/2
vl2 = vsa2/2
%define two test configurations
qa = [pi/3 pi/3]
qb = [pi/4 pi/4]
q_dot = [0.1 0.1]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test the inertia matrix
twolink.inertia(qa)
twolink.inertia(qb)
vB = subs(B,{m1 Izz1 sa1 l1 m2 Izz2 sa2 l2}, {vm1 vIzz1 vsa1 vl1 vm2 vIzz2 vsa2 vl2})
vBqa = double(subs(vB,q2, qa(2)))
vBqb = double(subs(vB,q2, qb(2)))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test the Coriolis and centrifugal terms
twolink.coriolis(qa,q_dot)
twolink.coriolis(qb,q_dot)
vC = subs(C, {m1 Izz1 sa1 l1 m2 Izz2 sa2 l2}, {vm1 vIzz1 vsa1 vl1 vm2 vIzz2 vsa2 vl2})
vCqa = double(subs(vC,{q2, qdot1, qdot2}, {qa(2) q_dot(1) q_dot(2)}))
vCqb = double(subs(vC,{q2, qdot1, qdot2}, {qb(2) q_dot(1) q_dot(2)}))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test the gravity term
twolink.gravload(qa)
twolink.gravload(qb)
vG = subs(G, {m1 Izz1 sa1 l1 m2 Izz2 sa2 l2 g}, {vm1 vIzz1 vsa1 vl1 vm2 vIzz2 vsa2 vl2 vg})
vGqa = double(subs(vG, {q1 q2},qa))
vGqb = double(subs(vG, {q1 q2},qb))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Test the friction terms
twolink.friction(q_dot)
vF = double(-subs(F,{Bm1, Bm2, Gear1, Gear2},{vBm1, vBm2, vGear1, vGear2}).*q_dot)