Refactored baxter_controllers and added velocity controller