Removed kdl dependency