Preliminary Graphical Debugging for SeaLegs

Testing begins shortly, I just added some standard stuff so that I can
see how the robot is thinking.  I draw the wave models to see if they
are moving properly, then the test points to see if they are working
(always hard to fine tune scale), and the points that are being
generated to move to (easy to graph, but the code could do wack things
if I'm not careful)