% test the correctness of the floating-base forward and inverse dynamics
% functions by checking them against data prepared by FDab.
% step 1: create a floating-base kinematic tree, and adjust some of the
% pitches so that it contains revolute, prismatic and helical joints.
NRJ = 8; % number of real joints
tree = floatbase( autoTree( NRJ+1, 2, 1, 0.9 ) );
tree.pitch(7) = 0.1; % 1st real joint
tree.pitch(8) = inf; % 2nd real joint
% step 2: select random initial position, velocity and force conditions,
% and calculate the corresponding joint accelerations using FDab
q = pi * (2*rand(NRJ+6,1) - 1);
qd = 2*rand(NRJ+6,1) - 1;
tau = [0;0;0;0;0;0;2*rand(NRJ,1)-1];
qdd = FDab( tree, q, qd, tau );
% step 3: calculate the position, velocity and acceleration of the floating
% base using the kinematics function fbKin.
[X,v,a] = fbKin( q, qd, qdd );
% step 4: call IDf and FDf.
[a1,tau1] = IDf( tree, X, v, q(7:NRJ+6), qd(7:NRJ+6), qdd(7:NRJ+6) );
[a2,qdd2] = FDf( tree, X, v, q(7:NRJ+6), qd(7:NRJ+6), tau(7:NRJ+6) );
% step 5: compare results. In theory, we should have a1==a, a2==a,
% tau1==tau(7:NRJ+6) and qdd2==qdd(7:NRJ+6). However rounding errors will
% make them slightly different. Expect rounding errors in the vicinity of
% 1e-13 to 1e-14 on this test, and occasionally significantly larger
% errors.
[ a1-a, a2-a ]
[ tau1-tau(7:NRJ+6), qdd2-qdd(7:NRJ+6) ]