Fix bug where goal handle was not being reset.

- When an action goal failed due to path constraint violations, the currently
  active goal was not being reset.
Trivial log message fix.
Restore use of INVALID_JOINTS error code.

- When rejecting a goal for invalid joints reasons, use INVALID_JOINT, and not
  INVALID_GOAL.
Allow to tune robot hardware simulator fidelity.

- Add a smoothing topic, that allows to set an exponential smoothing factor,
  where 0 means perfect control and 1 means no control at all (don't move).
Complete test suite for controller.