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

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