Change cost: tau^2 --> abs(tau)

Might work after all, with sufficient resolution. Also squaring the
torque is no the best idea. Compare 1 step at full torque has cost of
100, which is equivalent to 2500 steps with 0.02 torque (smallest
torque in 100 torque divisions).