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