Removed checking for duplicate state while adding states to graph as it rarely happens and this checking causes issues with
policy execution. Cheaper to always add when deviated as it shows higher success rate.
Merge branch 'master' of github.com:sauravag/FIRM-OMPL