Include and link to catkinized version of sbpl_geometry_utils
Added joint limit check for torso