Added subsystem testing for trajectory server. Server currently works for reachable poses; infinite loop when unreachable (unreachable flag currently not very enlightening, as all points are pretty much unreachable...)