In this work, we analyze and present an algorithm to find shortest-paths for generic rigid bodies. We derived the necessary conditions for optimality using Lagrange multipliers, and compared it to the conditions derived from Pontraygin’s Maximum Principle. We derived the equations of the necessary conditions using geometric Jacobian, drawing inspiration from the similarity between the rigid-body systems and the arm-like systems. In the previous work, the analysis focused on finding shortest-paths to reach goals in positions only. This work extends the analysis to find the shortest-path to reach a goal with complete configuration in 3D. We show that the algorithm is resolution complete even when the orientations are included. To overcome the complexity of 3D orientations, we describe the system using three points in the robot frame, and show that this parameter system is redundant but can derive the same necessary conditions as those derived using the minimum parameters (configuration). We used a 3D Dubins system to demonstrate the correctness of the analysis and the algorithm.
