Add torque mode

[IvLaptev]

For each command:
tau = T
dq = VelStopF
q = PosStopF
kp = 0
kd = 0