Initial commit of robot visualization
Robot visualization almost ready