Add torque limits

We have 2 ways to deal with torque limit violations:
- Compute an addition to tau (the feedforward term) that brings  the total force into an acceptable range
- Trigger emergency and go to damping