Separated out control algorithms
Integrated robot task space control into simulation