% test the correctness of FDab, FDcrb and ID by checking that FDab and
% FDcrb are inverses of ID.
% step 1: create a complicated kinematic tree, and adjust some of the
% pitches so that it contains helical and prismatic as well as revolute
% joints.
tree = autoTree( 12, 1.5, 1, 0.95 );
tree.pitch(3) = 0.1;
tree.pitch(5) = inf;
tree.pitch(7) = -0.1;
tree.pitch(9) = inf;
% 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 ID to calculate the force required to produce qdd; then use
% both FDab and FDcrb to calculate the acceleration that this force
% produces.
tau = ID( tree, q, qd, qdd );
qdd_ab = FDab( tree, q, qd, tau );
qdd_crb = FDcrb( tree, 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)'