fixed bugs for single iteration
code runs for subgoals. subgoalSeqs still has bugs
fixed bugs. runs with obstacles and subgoalSeqs
code all working