Minor changes. Added a plan method for with rrtstar to the test code.