Matlab Source Code for Composite Rigid Body Forward Dynamics
(c) 2004 Roy Featherstone
Cut and paste the source code below into files as indicated

----------- fwddyn.m ---------------------------------------------------------

function qdd = fwddyn( robot, q, qd, tau, grav_accn )
% FWDDYN  Calculate robot forward dynamics.
% fwddyn(robot,q,qd,tau) calculates the forward dynamics of a robot using
% the composite-rigid-body method, 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
  [H,C] = dynamics( robot, q, qd );
else
  [H,C] = dynamics( robot, q, qd, grav_accn );
end

qdd = H \ (tau - C);

----------- dynamics.m -------------------------------------------------------

function [H,C,diagnostic] = dynamics( robot, q, qd, grav_accn )
% DYNAMICS  Calculate robot equations of motion.
% dynamics(robot,q,qd) calculates the coefficients of the joint-space equation
% of motion, tau = H(q) qdd + C(d,qd), where q, qd and qdd are the joint
% position, velocity and acceleration vectors, H is the joint-space inertia
% matrix, C is the vector of gravity and velocity-product terms, and tau is
% the joint force vector.  Algorithm: modified Newton-Euler for C,
% Composite-Rigid-Body for H, both 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 fourth
% argument.  The return value is [H,C,diagnostic] where diagnostic is an
% optional structure containing most of the partial results of the
% calculation.

if nargin == 3
  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{i} = Xrotz(q(i));
    S{i} = [0;0;1;0;0;0];
  elseif robot.pitch(i) == inf		% prismatic joint
    Xj{i} = Xtrans([0 0 q(i)]);
    S{i} = [0;0;0;0;0;1];
  else					% helical joint
    Xj{i} = 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{i} * robot.Xlink{i};
end

for i = 1:robot.NB
  if robot.parent(i) == 0
    v{i} = S{i} * qd(i);
    avp{i} = Xup{i} * grav_accn;
  else
    v{i} = Xup{i}*v{robot.parent(i)} + S{i}*qd(i);
    avp{i} = Xup{i}*avp{robot.parent(i)} + crossM(v{i})*S{i}*qd(i);
  end
  fvp{i} = robot.Ilink{i}*avp{i} + crossF(v{i})*robot.Ilink{i}*v{i};
end

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

IC = robot.Ilink;			% composite inertia calculation

for i = robot.NB:-1:1
  if robot.parent(i) ~= 0
    IC{robot.parent(i)} = IC{robot.parent(i)} + Xup{i}'*IC{i}*Xup{i};
  end
end

H = zeros(robot.NB);

for i = 1:robot.NB
  fh = IC{i} * S{i};
  H(i,i) = S{i}' * fh;
  j = i;
  while robot.parent(j) > 0
    fh = Xup{j}' * fh;
    j = robot.parent(j);
    H(i,j) = S{j}' * fh;
    H(j,i) = H(i,j);
  end
end

if nargout == 3
  diagnostic.Xj = Xj;
  diagnostic.Xup = Xup;
  diagnostic.S = S;
  diagnostic.v = v;
  diagnostic.avp = avp;
  diagnostic.fvp = fvp;
  diagnostic.IC = IC;
end