Fix draw-footprints
Add A to B footstep planning
Add more samples for opengl drawing