Path Planning and Optimization for Cuspidal 6R Manipulators