DEBUG: working version of the iterative constraint update obstacle avoidance
working version of the iterative constraint update obstacle avoidance