Needed correct quat for first point as well. Maybe it's time to refactor make_minimum_twist function a bit..