Fix/update scene before planning

Fabian Reisterrequested to merge
fix/update-scene-before-planning into master

To avoid collisions when static objects have been moved.

Merge request reports