Planning With Attitude

Planning trajectories for floating-base robotic systems that experience large attitude changes is challenging due to the nontrivial group structure of 3D rotations. This paper introduces a powerful and accessible approach for optimization-based planning on the space of rotations using only standard linear algebra and vector calculus. We demonstrate the effectiveness of the approach by adapting Newton's method to solve the canonical Wahba's problem, and modify the trajectory optimization solver ALTRO to plan directly on the space of unit quaternions, achieving superior convergence on problems involving significant changes in attitude.