Seol mar théacs é seo: Simultaneous scene reconstruction and whole-body motion planning for safe operation in dynamic environments