fixed problems with kdl_codyco 0.0.2git