Optimized-motion planning : theory and implementation /