Added joint torques and fixed variable name.
Basic interpolation functions work for the gain scheduled controller.
Switched to varialbes for torques.
Controller class compiles.