Fixed a bug in Heuristic RRT when Goal not found.
GetBasePoseForObjectGrasp now returns discrete base config