Revert average pos and rot
Add rotation slerp and errorVec tweak