Matlab Source Code for Articulated-Body Forward Dynamics
(c) 2004 Roy Featherstone
Cut and paste the source code below into a file called abadyn.m

------------------------------------------------------------------------------

function qdd = abadyn( robot, q, qd, tau, grav_accn )
% ABADYN  Articulated Body Dynamics.
% abadyn(robot,q,qd,tau) calculates the forward dynamics of a robot using
% the articulated-body algorithm, evaluated in link coordinates.
% Gravity is simulated by a fictitious base acceleration of [0,0,9.81] m/s^2
% in base coordinates.  This can be overridden by supplying a 3D vector as
% an optional fifth argument.

if nargin == 4
  grav_accn = [0;0;0;0;0;9.81];		% default gravity
else
  grav_accn = [0;0;0;grav_accn(1);grav_accn(2);grav_accn(3)];
end

for i = 1:robot.NB
  if robot.pitch(i) == 0		% revolute joint
    Xj = Xrotz(q(i));
    S{i} = [0;0;1;0;0;0];
  elseif robot.pitch(i) == inf		% prismatic joint
    Xj = Xtrans([0 0 q(i)]);
    S{i} = [0;0;0;0;0;1];
  else					% helical joint
    Xj = Xrotz(q(i)) * Xtrans([0 0 q(i)*robot.pitch(i)]);
    S{i} = [0;0;1;0;0;robot.pitch(i)];
  end
  Xup{i} = Xj * robot.Xlink{i};
end

for i = 1:robot.NB
  if robot.parent(i) == 0
    v{i} = S{i} * qd(i);
    c{i} = zeros(6,1);
  else
    v{i} = Xup{i}*v{robot.parent(i)} + S{i}*qd(i);
    c{i} = crossM(v{i}) * S{i} * qd(i);
  end
  IA{i} = robot.Ilink{i};
  p{i} = crossF(v{i}) * robot.Ilink{i} * v{i};
end

for i = robot.NB:-1:1
  h{i} = IA{i} * S{i};
  d{i} = S{i}' * h{i};
  u{i} = tau(i) - h{i}'*c{i} - S{i}'*p{i};
  par = robot.parent(i);
  if par ~= 0
    IA{par} = IA{par} + Xup{i}' * (IA{i} - h{i}*h{i}'/d{i}) * Xup{i};
    p{par} = p{par} + Xup{i}' * (p{i} + IA{i}*c{i} + h{i} * u{i}/d{i});
  end
end

for i = 1:robot.NB
  if robot.parent(i) == 0
    a{i} = Xup{i} * grav_accn;
  else
    a{i} = Xup{i} * a{robot.parent(i)};
  end
  qdd(i,1) = (u{i} - h{i}'*a{i})/d{i};
  a{i} = a{i} + c{i} + S{i}*qdd(i);
end