% test the correctness of the hybrid dynamics function, HD, by comparing it
% with data prepared by 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, and compute corresponding force
% vector.
q = pi * (2*rand(12,1) - 1);
qd = 2*rand(12,1) - 1;
qdd = 2*rand(12,1) - 1;
tau = ID( tree, q, qd, qdd );
% step 3: choose a random forward-dynamics joint set, and call HD.
fd = rand(12,1) > 0.5;
[qdd_out,tau_out] = HD( tree, fd, q, qd, qdd, tau );
% step 4: compare results. In theory, we should have qdd_out==qdd and
% tau_out==tau. However, rounding errors will make the calculated values
% (but not the copied values) slightly different. Thus, for each joint i
% such that fd(i)==1, we would expect a rounding error in qdd_out(i); and
% for each joint i such that fd(i)==0, we would expect a rounding error in
% tau_out(i). Expect rounding errors in the vicinity of 1e-14 on this test.
[ qdd_out-qdd, tau_out-tau ]