completing of state machines, better output from inv_kin_server
Successful run of Master2arm using trajopt