Fixed. COM Jdot call for now. Should really be using a Jdot * v method, but this is good for now.
num_q fix. Fixes runEndPointJac test.