Finding the Time-Optimal Parameterization of a Path (TOPP) subject to second-order constraints (e.g. acceleration, torque, contact stability, etc.) is an important and well-studied problem in robotics. In comparison, TOPP subject to third-order constraints (e.g. jerk, torque rate, etc.) has received far less attention and remains largely open. In this paper, we investigate the structure of the TOPP problem with third-order constraints. In particular, we identify two major difficulties: (i) how to smoothly connect optimal profiles, and (ii) how to address singularities, which stop profile integration prematurely. We propose a new algorithm, TOPP3, which addresses these two difficulties and thereby constitutes an important milestone towards an efficient computational solution to TOPP with third-order constraints.
Finding the time-optimal parameterization of a given path subject to kinodynamic constraints is an essential component in many robotic theories and applications. The objective of this paper is to provide a general, fast, and robust implementation of this component. For this, we give a complete solution to the issue of dynamic singularities, which are the main cause of failure in existing implementations. We then present an open-source implementation of the algorithm in C++ Python and demonstrate its robustness and speed in various robotics settings.