Added draw_unit_cube
draw_box working
rotate robot body
added grid