% test the correctness of the planar-vector dynamics routines FDabp, FDcrbp
% and IDp by checking that FDabp and FDcrbp are inverses of IDp, and by
% checking that HandCp gives the same result as HandC.

% step 1: create planar-vector and spatial-vector models of a complicated
% kinematic tree.

tree_p = autoTreep( 12, 1.5, 0.95 );	% planar-vector version
tree_s = autoTree( 12, 1.5, 0, 0.95 );	% spatial-vector version

% step 2: choose random initial conditions

q   = pi * (2*rand(12,1) - 1);
qd  = 2*rand(12,1) - 1;
qdd = 2*rand(12,1) - 1;

% step 3: use IDp to calculate the force required to produce qdd; then use
% both FDabp and FDcrbp to calculate the acceleration that this force
% produces.

tau = IDp( tree_p, q, qd, qdd );
qdd_ab = FDabp( tree_p, q, qd, tau );
qdd_crb = FDcrbp( tree_p, q, qd, tau );

% step 4: compare results.  In theory, we should have qdd_ab==qdd and
% qdd_crb==qdd.  However, rounding errors will make them slightly
% different.  Expect rounding errors in the vicinity of 1e-14 on this
% test.

(qdd_ab-qdd)'
(qdd_crb-qdd)'

% step 5: calculate H and C via both HandC and HandCp.  To make the test a
% little stronger, we shall use a custom value for gravity.

[Hs,Cs] = HandC( tree_s, q, qd, {}, [1;2;0] );
[Hp,Cp] = HandCp( tree_p, q, qd, {}, [1;2] );

% step 7: compare results.  Most rounding errors will be exactly zero in
% this test.  Nonzero errors will range from approximately 1e-17 to 1e-15.

(Cs-Cp)'
Hs-Hp