Clean Up.
Fixed Bug in "initializeGoalPosition"-forgot to initialize torque.

Other stuff are changes to try to implement interpolation. Values on
the right end of the grid dont get interpolated-oddly the left end
works.