Fixed resetting joint states when performing mode switches

[Joschi3]

Changed Goal Resetting and mode switching logic slightly

Changes

• if no controller is active, actuators change to position mode and hold their current position
• At startup, make sure all actuators are in position control mode
• when switching control mode, made sure resetting joint states is always safe, even if the control mode switch fails
  • Note: for some actuators, e.g., the velocity goal value must be set to maximum speed in position control since it is used as a limit
• Bugfix: Zero Torque value in position control mode, preventing any motions
• Bugfix: Disabled e-stop and torque service when the hardware interface is not active