Robot path plan with Objects