Graph-based Trajectory Planning through Programming by Demonstration