Fixed bug with goals/robots at obstacle.