Fixing two arm test, non-FT urdf wrist