Right now there is the possibility to walk over the obstacle. We need to add a specific constraint for the foot stances to avoid that. Avoid overlapping of obstacles (we should have a function somewhere in utils.py)