Generalized Inversed Kinematics solver from the Humanoid Path Planner project