Removed commented code
Changed in kdlkinematics