Visualize obstacles in 3d. Fixed the problem with foot transforms..