Fast numerical algorithms for optimal robot motion planning