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

bug fix. issue was kinematic constraints were poorly handled. Need some more testing. Also cleaning. Normalization now only made on matrix H

all cases tested and pass for 9 step capturability. Missing python bindings and comparison with python implementation in pin inv dyn