Motion Planning in Medicine: Optimization and Simulation Alg