Lateral PID: move steer feedforward to CarInterface

[hewers]

Description This is useful for cars with strange steer commands that aren't proportional to angle * speed ** 2.

I want to implement this in the best way, and I'm still figuring out Python's tricks (this is cargo cult coding), so suggest any improvements and we'll iterate.

On top of this change, I'll add GM steer feedforward and lateral tuning.

Verification Process replay is unchanged.

[ghost]

I've done some testing by making

steer_feedforward = (angle_steers_des_no_offset + params.averageOffsetDegrees) * CS.vEgo**2

and setting P and I to zero(so what the model was outputting was irrelevant). Then I tuned the kf to ensure that the wheel had no centering anymore(reducing kf if it was unstable and increasing if the wheel was centering).

I think this is a nice way to tune the kf term, but it also confirmed in my case that the values need to be quite a bit different depending on speed(which makes sense if we consider openpilot torque output as the simulatet driver torque, which the EPS is boosting by different amounts depending on the speed).

Haven't checked for nonlinearity in angle a lot, but if it's just the speed, wouldn't breakpoints like for P and I be a nicer solution?

[hewers]

@grekiki That's a smart idea to attempt tuning kf, very clean if we had the right function of (speed, angle) -> (steer) to begin with.

I also think that feedforward is best with (desired_curvature + params.angleOffsetDegrees - params.angleOffsetAverageDegrees), as the constant offset doesn't apply torque (angle sensor not zeroed) but the instant vehicle model error does. Working on proving that out.

Non-linearity: for Volt steer command it's not just speed but a sigmoid of angle, so speed breakpoints won't work.

  # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
  def get_steer_feedforward(self, desired_angle, v_ego):
      # Sigmoid maps [-inf,inf] to [-1,1].
      # This scalar gives sigmoid(34.4 deg) = sigmoid(1) = 0.5.
      # 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary scaling?
      desired_angle *= 0.02904609
      sigmoid = desired_angle / (1 + fabs(desired_angle))
      return sigmoid * (v_ego + 7 * CV.MPH_TO_MS) # minimum steer speed

Maybe this is "reverse engineering" the GM steering command.

[ghost]

@qadmus For the first formula, looking at this simulation for example (https://www.youtube.com/watch?v=cJyIYjwakKo) I'd say it's quite reasonable to assume that the front tyres are being "pushed" down by the weight of the engine thereby generating lateral loads to keep the car straight, which should generate centering force. (@pd0wm any opinion on that?) (thinking a bit more, I am not confident this is true. Slip seems complicated...)

For the sigmoid, seems interesting. From just physics, I'd say that the centering torque should be about proportional to the $sin(steering_angle)*v^2$ where steering_angle=steering_wheel_angle/steering_ratio(combine a=v^2/r with http://www.davdata.nl/math/turning_radius.html), though it doesn't seem like that would approximate your curve too well(with about 15 steering ratio you don't get to the "interesting part" of the sin quite yet).

Are there plots something you can make from any rlogs?

[hewers]

@grekiki the plots could work with any car's rlogs, but there are a few hardcoded values like GM's max steer command. See my branch tuning-scripts for work in progress. @sshane created all the principal work, I just riffed on it.

That sigmoid will hopefully come after this refactor, I tried fitting all the sigmoids with all the degrees of freedom for variables, arctan is close but 1/(1+|x|) is best.

[hewers]

Renamed LatControlPID.get_steer_feedforward()

@pd0wm , an alternative implementation is to do the best with the original LateralPID output, then try to re-map the steer command in CarController. But, that would interfere with clipping / saturation behavior.

I like how each car could get their own feedforward estimate here.

[pd0wm]

Yeah like that too!

If you're looking for another small project you could make all lateral control classes inherit from the same base class. After this change the PID controller has a different API for the constructor than the others, which could be confusing.

[ghost]

That sounds quite simmilar to #21967

[sshane]

I'll rebase it for this PR soon!