Starting Position added twice

[basil-huber]

In case of obstacleOverlapPolicy==ELEVATE_ROBOT, the starting position is added twice to the robot's position. Once before checking the intersection with obstacles (here) and once afterwards (here).
This leads not only to a wrong starting position, but can also lead to a collision at the robots final starting position.