Robot path plan enhancement