Control root link rather than CoM (temporarely)

- Since there's a problem with the CoM Jacobian, replace CoM control
with root link control, which works fine.
- Fix bugs