changed MH subalgorithm for kinematics chains
Merge branch 'master' of github.com:nassimeblinlaas/step_generator