Adding ability to plan to multiple goals to cbirrt
Fixing error that prevented PlanToConfiguration from working. Now have helper method that both PlanToConfigurations and PlanToConfiguration use.
Merge pull request #11 from personalrobotics/feature/jeking_multi_goal_planning

Added PlanToConfigurations to CBiRRT