Reverting kdl cmake code