working SCP implementation for path planning around obstacles