Fixed sonar disabler
Created custom robot_state_publisher just for Baxter's end effector tfs since Baxter does not publish them himself
Switched to Rethink trajectory controller