Zaslat SMS: Whole-body motion planning in dynamic environments