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

% IDp  Inverse Dynamics (planar) via Recursive Newton-Euler Algorithm.
% IDp(model,q,qd,qdd,f_ext,grav_accn) calculates the inverse dynamics of a
% kinematic tree via the recursive Newton-Euler algorithm, evaluated using
% planar vectors.  q, qd and qdd are vectors of joint position, velocity
% and acceleration variables; and the return value is a vector of joint
% force 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 < 6
  a_grav = [0;0;0];
else
  a_grav = [0;grav_accn(1);grav_accn(2)];
end

external_force = ( nargin > 4 && length(f_ext) > 0 );

for i = 1:model.NB
  [ XJ, S{i} ] = jcalcp( model.jcode(i), q(i) );
  vJ = S{i}*qd(i);
  Xup{i} = XJ * model.Xtree{i};
  if model.parent(i) == 0
    v{i} = vJ;
    a{i} = -Xup{i}*a_grav + S{i}*qdd(i);
  else
    v{i} = Xup{i}*v{model.parent(i)} + vJ;
    a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd(i) + crmp(v{i})*vJ;
  end
  f{i} = model.I{i}*a{i} + crfp(v{i})*model.I{i}*v{i};
  if external_force && length(f_ext{i}) > 0
    f{i} = f{i} - f_ext{i};
  end
end

for i = model.NB:-1:1
  tau(i,1) = S{i}' * f{i};
  if model.parent(i) ~= 0
    f{model.parent(i)} = f{model.parent(i)} + Xup{i}'*f{i};
  end
end