Update to be compatible with kdl_codyco rovereto branch