% test the correctness of the floating-base forward and inverse dynamics % functions by checking that IDf is indeed the inverse of FDf. % 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 conditions. q = pi * (2*rand(NRJ,1) - 1); qd = 2*rand(NRJ,1) - 1; qdd = 2*rand(NRJ,1) - 1; X = Xrotz(2*pi*rand(1)) * Xroty(2*pi*rand(1)) * Xrotx(2*pi*rand(1)) * ... Xtrans(2*rand(3,1)-1); v = 2*rand(6,1) - 1; % step 3: use IDf to calculate the force required to produce qdd, and the % resulting floating-base acceleration. Then use FDf to calculate the % accelerations produced by this force. [a1,tau1] = IDf( tree, X, v, q, qd, qdd ); [a2,qdd2] = FDf( tree, X, v, q, qd, tau1 ); % step 4: compare results. In theory, we should have a1==a2 and % qdd==qdd2. However, rounding errors will make them slightly different. % Expect rounding errors in the vicinity of 1e-13 to 1e-14 on this test. (a1-a2)' (qdd-qdd2)'