function [R F] = RHS(p,bodies,n)

if(get(p,'Fixed') > 0)
  R = [];
else

  M = MassMatrix(p);
  A = eye(12);
  MM = zeros(6);
  for i = 1:6
      for j = 1:6
         MM(i,j) = p.mu_inf(i,j);
      end
  end

  
  
  
F_Sum = zeros(1,6);
fcn = get(p,'ForceFcns');
F = cell(p.n_Forces,1);
for nn = 1:p.n_Forces
  F{nn} = fcn{nn}(p,bodies,n);
  F_Sum = F_Sum + F{nn};    
end  
  

  A(7:12,7:12) = inv(M+MM);
  R = A*[p.sv(7:12); F_Sum'];
end
  