Removed dependency on kdl