Add separate torque-based custom feedforward functions, update torque controller to use them

[JasonJShuler]

Description The Torque controller collects the custom feedforward functions, but it didn't use them. This was not a real problem as the feedforward functions calculated for use with a PIF tune is not compatible with the torque tune. (they use different units and require a different default function...)

In interfaces.py, we add the new delegate for torque-based feedforward functions, separate from the incompatible PIF-based functions.

latcontrol_torque.py is updated to capture the new functions within init, and update is updated to use the function.

This is @twilsonco 's hard work: I am just parting it out and submitting the PR

Justification It has been explained to me that GM's relationship between steering wheel torque and lateral acceleration is most problematic and is especially variable based on speed and in some cases even curvature direction. @qadmus and @twilsonco have developed a means of analyzing rlogs to create custom sigmoid feedforward functions that correct for this. @twilsonco updated the code to work with the torque controller.

Verification This code change has been present in @twilsonco 's fork, as well as a fork I have personally tested.

[hewers]

Rather delete code rather than add unused stubs. I don't see that a custom feedforward is required for torque control.

[twilsonco]

It's more of a sidenote that the torque controller was already fetching the other (traditional) custom FF functions. The point here is that this should be added to improve performance with the torque controller.

It makes a big difference in the torque controller for Volt and Acadia at least. We have torque FF fits generated also for Bolt, Bolt EUV, Suburban, Silverado/Sierra, and Escalade ESV. The results for the Volt, Acadia, Bolt EUV and Escalade ESV were very good, and all the other cars reported an improvement too.

Multiple users of my Fork (Volt, Escalade ESV, Acadia) and of the OPGM fork (Bolt EUV, Bolt, Silverado/Sierra) have been driving on this for some time.

[twilsonco]

~~Shall I add the Volt and Acadia torque FF functions to this PR with routes? The Acadia route will be recorded on a C2 with commatwo_master; the Volt will use master.~~

Here's the torque FF fit that will be used for Volt, vs a linear fit with kf = 0.33

~~And Acadia vs linear with kf = 0.4 (the one used will not have the horizontal offset shown here, which is only used in the fitting process to better capture the shape of the curve, despite persistent angle bias which varies between individual cars)~~ (edit: Acadia will be a followup PR)

~~In both cases,~~ the difference is at lower speeds where this completely resolves BANG BANG steering and gives smooth, accurate entry and exit of curves. At highway speeds the fits are very nearly linear, but still not quite.

[twilsonco]

Some information about the fit FF function:

Fit from approximately 55 hours of driving data with very good sampling of the lateral acceleration@speed space.

Substantial reduction in error vs linear feedforward, especially at neighborhood and city speeds where the steering performance is noticeably improved.

mean absolute error: Linear FF 0.1382, This FF 0.0543 STD: Linear FF 0.0974, This FF 0.044

4849 data points filtered down from over 12 million, selecting for points with:

• No driver applied steering torque
• Constant speed
• Constant steer angle (close to zero steer rate)
• Not saturated

Lateral acceleration includes roll compensation as computed in latcontrol_torque.py (by maintaining a VehicleModel using liveParams roll and steer ratio)

So the data indicate the amount of steer command necessary to hold a given angle (lateral acceleration) at a given speed.

Samples: 7489367

Regularizing...
Regularized samples: 4849

speed: DescribeResult(nobs=4849, minmax=(3.6483986912102533, 39.385506549864324), mean=21.251445849765716, variance=83.11116350256492, skewness=-0.0034927193667718967, kurtosis=-1.0781462617161734)
angle: DescribeResult(nobs=4849, minmax=(-2.9949600629168693, 3.5458351777642827), mean=0.016720055225671635, variance=1.3716756904161793, skewness=0.054041812835440906, kurtosis=-0.8102616828976719)
steer: DescribeResult(nobs=4849, minmax=(-1.0539705887729045, 1.082418296072218), mean=-0.017093641303920295, variance=0.2630886785304993, skewness=0.00578721456326116, kurtosis=-1.3149402845769018)

MAE old 0.1382, new 0.0543
STD old 0.0974, new 0.044

lat_accel 0.00-0.20:604, lat_accel 0.21-0.41:547, lat_accel 0.43-0.63:526, lat_accel 0.64-0.84:515, lat_accel 0.86-1.06:445
lat_accel 1.07-1.27:435, lat_accel 1.29-1.49:420, lat_accel 1.50-1.70:383, lat_accel 1.71-1.91:303, lat_accel 1.93-2.13:217
lat_accel 2.14-2.34:108, lat_accel 2.36-2.56:55, lat_accel 2.57-2.77:13, lat_accel 2.79-2.99:7, lat_accel 3.00-3.20:4

mph 08-13:152, mph 13-18:229, mph 18-23:289, mph 23-28:387, mph 28-33:358
mph 33-38:369, mph 38-43:320, mph 43-48:368, mph 48-53:330, mph 53-58:360
mph 58-63:395, mph 63-68:344, mph 68-73:337, mph 73-78:244, mph 78-83:314
mph 83-88:52, mph 88-93:1, ```

[twilsonco]

I also recorded a route with the stock torque controller with linear FF, having set the Volt's max lateral acceleration value in override.yaml such that the resulting kf would be 0.33 (the red line in the above plot). Here are some comparisons.

First of, bravo on the torque controller, @HaraldSchafer. It's great work!

On straights, and for many curves, the driver's experience is only subtly different between linear vs fit FF. However, at city speeds and especially neighborhood speeds, the difference is stark.

For all images, linear FF is on the left, and the fit FF is on the right.

Here's a 27mph tight curve:

Note that the custom FF tune in this PR has friction set to zero.

Here the fit FF (cyan line in bottom plot) is much smoother than that of the default torque controller. The jagged behavior of the feedforward in the default controller is the result of "friction compensation" as implemented in latcontrol_torque.py.

When testing the linear tune, I left friction at the value in params.yaml (~0.15). The FRICTION_THRESHOLD of up to ±0.2 is then interpolated and added to the feedforward for error values within ±0.15, cause a large feedforward increase for the first ±0.15m/s^2.

Combined with the low_speed_factor that effectively increases the magnitude of calculated error at low speed, friction compensation results in nearly instantaneous 50% jumps in the value of feedforward.

To the driver in the above image, (and as visible in the actual lateral acceleration; left side, second plot from top, red path) the steering wheel overshoots the correct angle, then overcorrects the other way, and this repeats through the curve. The lack of persistent steering torque results in the controller not being able to get back on the path, and you end up on the outside of the corner as in the left image. The controller is unstable in this state, and you can see the error (blue path, third plot from top) amplitude increasing throughout the curve. The error is overall negative (red path, third plot from top), hence losing the curve to the outside.

With the correct feedforward, and absent friction compensation, the steering response, though still less than perfectly smooth, is correct, so much less error accumulates though the curve. By the apex of the curve, the controller has regained smooth control (error becomes flat again instead of becoming unstable).

Note that the fit FF yields the same feedforward value (≈0.8) that is achieved by linear FF + friction compensation, making the FF boost that friction compensation provides necessary, but it does so earlier and more smoothly.

Here are two more images where the linear FF performed similarly to the fit FF. In the 45mph example the fit FF is still noticeably more precise to hit the desired angles from the drivers perspective, though it's much more subtle than at neighborhood speeds.

[twilsonco]

test route: c11fcb510a549332|2022-08-15--12-18-54

[haraschax]

This looks pretty good, and the results are pretty convincing!

I need to think about how this type of feedforward fits into the framework before we can merge. As it is it's not really designed to take feedforward functions that aren't linear. Since the PID gains are supposed to scale with lateral accel. In this case the PID gains would ideally be speed and torque dependent, will try to see if there is a clean way to do that that would work well with any custom feedforward needed for any car.

The PID should probably operate in the lateral accel domain, and then the output of that should be converted to a torque request value by a linear (or not) ff function.

[twilsonco]

Thanks! This was all possible thanks to @qadmus' initial work on custom FF fits.

I expect that the live torque will be able to adjust the kf for custom FF just as for linear FF. Reason being this function has guaranteed linear end behavior, so adjusting kf will just tilt the FF the same way live torque will affect linear FF.

I also think live torque will help this as much as it will help linear FF, since, as the data show, the fit still has error that could hypothetically be reduced to "zero" with the right kf value on each iteration.

[twilsonco]

Reimplemented following the changes introduced by live torque. @HaraldSchafer

After reviewing live-torque, it's conceptually incompatible with this approach, as it's doing an online linear fit.

[haraschax]

@twilsonco It should be possible to make it work. The linear fit should still be relevant as it models some real dynamics of the physical steering system. You just need to convert the inputs going into the linear fit from "torque request" to "real torque", which would be inverting some function that happens in your car's EPS.

And then you need to convert the output "real torque" back to "torque request" before you send the request. But hopefully you can make this conversion by doing a fit, and then live torque should work great