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