% 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