Command velocity conversion added, may be unstable. Subject to test!