Able to read URDF -- now needs to parse