function  qdd = FDcrbp( model, q, qd, tau, f_ext, grav_accn )

% FDcrbp  Forward Dynamics (planar) via Composite-Rigid-Body Algorithm.
% FDcrbp(model,q,qd,tau,f_ext,grav_accn) calculates the forward dynamics of
% a kinematic tree via the composite-rigid-body algorithm, evaluated using
% planar vectors.  q, qd and tau are vectors of joint position, velocity
% and force variables; and the return value is a vector of joint
% acceleration variables.  f_ext is a cell array specifying external forces
% acting on the bodies.  If f_ext == {} then there are no external forces;
% otherwise, f_ext{i} is a planar force vector giving the force acting on
% body i, expressed in body i coordinates.  Empty cells in f_ext are
% interpreted as zero forces.  grav_accn is a 2D vector expressing the
% linear acceleration due to gravity in the x-y plane.  The arguments f_ext
% and grav_accn are optional, and default to zero (i.e., {} and [0 0],
% respectively) if omitted.

if nargin == 4
  [H,C] = HandCp( model, q, qd );
elseif nargin == 5
  [H,C] = HandCp( model, q, qd, f_ext );
else
  [H,C] = HandCp( model, q, qd, f_ext, grav_accn );
end

qdd = H \ (tau - C);