[transition test] add method to compute stability constraints from w, and factorize stability method in functions

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