function  [xdfb,tau] = IDfb( model, xfb, q, qd, qdd, f_ext )

% IDfb  Floating-Base Inverse Dynamics (=Hybrid Dynamics)
% [xdfb,tau]=IDfb(model,xfb,q,qd,qdd,f_ext) calculates the inverse dynamics
% of a floating-base kinematic tree via the algorithm in Table 9.6 of RBDA
% (which is really a special case of hybrid dynamics), using the same
% singularity-free representation of the motion of the floating base as
% used by FDfb.  xfb is a 13-element column vector containing: a unit
% quaternion specifying the orientation of the floating base (=body 6)'s
% coordinate frame relative to the fixed base; a 3D vector specifying the
% position of the origin of the floating base's coordinate frame in
% fixed-base coordinates; and a spatial vector giving the velocity of the
% floating base in fixed-base coordinates.  The return value xdfb is the
% time-derivative of xfb.  The arguments q, qd and qdd contain the
% position, velocity and acceleration variables for the real joints in the
% system (i.e., joints 7 onwards in the system model); so q(i), qd(i) and
% qdd(i) all apply to joint i+6.  The return value tau is the vector of
% force variables required to produce the given acceleration qdd.  f_ext is
% an optional argument specifying the external forces acting on the bodies.
% It can be omitted if there are no external forces.  If supplied, it must
% be a cell array of length model.NB, of which the first 5 elements are
% ignored, and f_ext{6} onward specify the forces acting on the floating
% base (body 6) onward.  The format of f_ext is explained in the source
% code of apply_external_forces.

a_grav = get_gravity(model);

qn = xfb(1:4);				% unit quaternion fixed-->f.b.
r = xfb(5:7);				% position of f.b. origin
Xup{6} = plux( rq(qn), r );		% xform fixed --> f.b. coords

vfb = xfb(8:end);
v{6} = Xup{6} * vfb;			% f.b. vel in f.b. coords

a{6} = zeros(6,1);

IC{6} = model.I{6};
pC{6} = model.I{6}*a{6} + crf(v{6})*model.I{6}*v{6};

for i = 7:model.NB
  [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i-6) );
  vJ = S{i}*qd(i-6);
  Xup{i} = XJ * model.Xtree{i};
  v{i} = Xup{i}*v{model.parent(i)} + vJ;
  a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd(i-6) + crm(v{i})*vJ;
  IC{i} = model.I{i};
  pC{i} = IC{i}*a{i} + crf(v{i})*IC{i}*v{i};
end

if nargin == 6 && length(f_ext) > 0
  prnt = model.parent(6:end) - 5;
  pC(6:end) = apply_external_forces(prnt, Xup(6:end), pC(6:end), f_ext(6:end));
end

for i = model.NB:-1:7
  IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i};
  pC{model.parent(i)} = pC{model.parent(i)} + Xup{i}'*pC{i};
end

a{6} = - IC{6} \ pC{6};			% floating-base acceleration
                                        % without gravity
for i = 7:model.NB
  a{i} = Xup{i} * a{model.parent(i)};
  tau(i-6,1) = S{i}'*(IC{i}*a{i} + pC{i});
end

qnd = rqd( vfb(1:3), qn );		% derivative of qn
rd = Vpt( vfb, r );			% lin vel of flt base origin
afb = Xup{6} \ a{6} + a_grav;		% f.b. accn in fixed-base coords

xdfb = [ qnd; rd; afb ];