Use body velocity to control instead of the PD coupling

When Using CartesianPDCoupling, there is
a reference frame problem
Add gravity comp controler in test