function y = gcFD( model, FDfun, u ) % gcFD Simulink wrapper for forward dynamics functions % y=gcFD(model,FDfun,u) is a Simulink wrapper for the forward-dynamics % functions FDab, FDcrb, FDfb and FDgq. It serves two purposes: (1) to % accept ground contact forces in the format produced by the spatial_v2 % Simulink ground model and convert them into the format required by the % dynamics functions, and (2) to concatenate into a single vector the two % return values of FDfb. The first argument is the model data structure; % the second is the function handle of the dynamics function to be used; % and the third is the concatenation of the vectors q, qd, tau and % (optionally) the ground contact force data (f_gc) from which f_ext can % be calculated. (For FDfb, u is the concatenation of x, q, qd, tau and % optional f_gc.) The ground contact force data, if supplied, is simply % the concatenation of the spatial (or planar) forces arising from each % point defined in model.gc.point, all expressed in base coordinates. N = model.NB; if isequal( FDfun, @FDfb ) x = u(1:13); q = u(14:N+7); qd = u(N+8:2*N+1); tau = u(2*N+2:3*N-5); f_gc = u(3*N-4:end); else q = u(1:N); qd = u(N+1:2*N); tau = u(2*N+1:3*N); f_gc = u(3*N+1:end); end if length(f_gc) > 0 vecsize = length(model.Xtree{1}); % 3 for planar, 6 for spatial b = model.gc.body; % b(i) == body number of point i f_gc = reshape( f_gc, vecsize, length(b) ); f_ext = cell(1,N); % for each body i that is mentioned at least once, calculate the sum of % all forces acting on that body for i = unique(b) f_ext{i} = sum( f_gc(:,b==i), 2 ); end else f_ext = {}; end if isequal( FDfun, @FDfb ) [xd, qdd] = FDfb( model, x, q, qd, tau, f_ext ); y = [xd; qdd]; else y = FDfun( model, q, qd, tau, f_ext ); end