Using sub goal method of options for semi optimal path planning