URDF for cube robot for doing BFS 3d path