add a constraint on the position of the waypoint : must always be higher or equal to the init and goal position along the z axis.
TODO : replace the z axis with the contact normal, need to change the API to get this vector

Maybe need a specific case