First version of URDF -> DH converted working, still some occasional fail but should work well in real word examples