Pause and play, compliance mode

Added:

1. Pause and play feature
2. Compliance mode trajectory follow