Add urdf support to wholeBodyStatesLocal