function KE = ComputeKE(p)
KE = zeros(length(p.xdot),1);
for j = 1:p.dof
    KE = KE + .5*(p.mass)*(p.xdot(:,j).^2);
end
