Planning graph optimization

Completed initial version of add/modify/remove and small clean-up within the graph construction and solving.