Added URDF and minor clean up in IK