Fixed bug in new getJointKDLIndex function