Fixed position interpolation

* Fixed position interpolation
* Started work on rotation interpolation