Add deadband option for tracking goals

[AllisonJohn]

Force the solver to track more closely by increasing the error when it is outside a certain range from the reference.

Using exampleTracking.py with the effort goal at 0.01 weight, the best results I got were with a multiplicative deadband error that is a constant when the error is outside the range.

Function:
double computeDeadband(double y, double s, double, r, double error) {
double tanh_curve = (std::tanh(s * (-r - error)) + 1) + (std::tanh(s * (error - r)) + 1);
return 1 + tanh_curve * y;
}

The computed value is added here:
integrand += m_state_weights[iref] * (error * error * deadband_error); in calcIntegrandImpl

For the example, the best deadband was with y=15, s=22, and r=0.1 (the labels have f instead of y)

For the example with one of the joints switched to a quadratic function, the best deadband was y=10, s=15, and r=0.07, similar to the linear one.

For both of these if I tried to decrease r or increase s, the objective would become unstable and it wouldn't solve. So these are the closest I can get it to the reference.

Some other things I tried were adding the deadband: integrand += m_state_weights[iref] * (error * error + deadband_error); and various functions for y such as exponential, quadratic, and absolute value. Some of these worked partially but not as well as the constant multiplier.

Including the deadband usually causes the solve to take much longe, for example 49 seconds instead of 2 seconds for the best result in the second graph. But with the constant multiplied deadband it could be low at 9 seconds for a low y of 5.

[AllisonJohn]

This is what the total multiplied-by-deadband error with y=10, s=15, r=0.5 (purple) looks like compared to the normal error^2 (green)

[AllisonJohn]

An error that I run into sometimes in this: Cannot call restoration phase at point that is almost feasible (violation 3.763656e-14).
Happened with (y=1, s=20, r=0.1)