Write proper tests for the commander classes

[toliver]

At least test all the methods.

Preferably test all methods with a representative set of parameters. Some configuration changes that break things only show when using certain joint groups, or other combinations of parameters and circumstances.

We could try to maintain a reasonable set of tests, but it would also be useful to be able to run the examples in sr_examples as tests (not sure how to do that though).