hand_of_god: Improving servoing, gazebo math api is wonky