Joint mode interface

Merging to use it with the compliant arm.
Ask @hilario for details