refactored footstep trajectory in own class --> better readability of motionplanner
new architecture stable on non-replanning tasks
cleaned up, fixed replanning, fixed wrong starting position