adjustments to jerk control when setting hyundai can acceleration

[pfeiferj]

Use the jerk that the plan is currently request on the acceleration as the lower limit value when sending accel commands to hyundai can vehicles.

small min/max values for jerk increases the resolution of the acceleration that the car provides. This allows the car to better fit the acceleration to the plan by making smaller adjustments to the acceleration to maintain/hit the target acceleration.

However, with too small of values for either the min or max jerk the car responds to changes in acceleration very sluggishly. Thus when we are requesting rapid changes to acceleration we need larger jerk values.

To achieve a good balance of resolution to responsiveness we can determine what the jerk in the plan acceleration was between the last acc command and the current command and use that as the basis of the values we request from the car. This allows a dynamic resolution that matches the current demands of the plan.

Data from runs: (left is with jerk changes, right is without)

As we can see in the data above, when we implement these changes it allows for much smaller jumps up and down while on a relatively flat section of road, but when we see large pitch increases and planned slow downs the true acceleration can still react quickly.

Subjectively, the smaller fluctuations in acceleration while on a flat(ish) section of road feel smoother and when speed changes are requested the car feels dynamic and accurate.

Note: I did also change tuning values for the longitudinal plan for my vehicle that I have not included in this merge request but are reflected in the data. The data above uses the values 0.9 for kpv and 0.1 for kiv. I suspect that the values I used will be reasonably close for larger vehicles but that the current 0.5 kpv and 0.0 kiv will be close for smaller vehicles. Once good values are confirmed I can update as needed.

[rchybicki]

I finally got around to testing this, and for Exp mode it takes out all of the "throttle hesitation" which was quite severe for me. I'm assuming because I'm on a hybrid, the instant torque and regen braking caused this, now it's all gone - great! The downside is, that stopping distance is a bit unpredictable now, it can sometimes stop very close to the lead car, depending on how the braking looked like. The difference in Exp mode comfort is significant, if we could improve the stopping distance safety without sacrificing that this would be a great improvement overall. Some cool stuff, Jacob! Any tips on how to tune this?

[pfeiferj]

The downside is, that stopping distance is a bit unpredictable now, it can sometimes stop very close to the lead car, depending on how the braking looked like.

I already had a fix for this, just forgot to push up to this branch. Just pushed it up. At this point I still have a little bit of undesired behavior when there's a long period of acceleration, in that situation it overshoots and continues accelerating after hitting the desired speed. So probably the kiv is too high and needs lowered, but i haven't gotten around to tuning that behavior out because it's fairly rare.

[rchybicki]

I had a strange situation today that I think might've been caused by the jerk changes. I had the lead car stop abruptly, but I still had quite a lot of distance to it, and there was a lot of hesitation and lag in the car's braking that I didn't see before. I took over to not risk hitting the guy, here's the graph from the braking:

My question is, when the accel bottoms out at -3.5, doesn't that make the jerk very small since the delta is now 0? Shouldn't this bit know that if we're requesting the min, make lower_jerk very high?

if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
  # calculate jerk from plan, give a small offset for the upper limit for the cars ecu
  lower_jerk = clip(abs(accel - self.accel_last) * 50, 0., 3.0)
  upper_jerk = lower_jerk + 0.5

[pfeiferj]

My question is, when the accel bottoms out at -3.5, doesn't that make the jerk very small since the delta is now 0?

The jerk would effectively go to zero in that situation. It actually might also explain why very occasionally i've seen overshoot after long periods of acceleration, the model may have wanted more acceleration than the limits and got clipped resulting in the same thing.

I just pushed up a change that should request more jerk as our acceleration deviates from the requested accel. Haven't tested it yet though but I would think it would fix this issue.

[pfeiferj]

I tried it out with the fix, didn't notice any bad behavior, but haven't driven with it much yet.

[rchybicki]

Ok, I'm also going to test the change, thanks. I'm thinking that this new min jerk calculation based on accel error might allow to simplify/remove this bit:
if CS.out.vEgoRaw < 4.:
if accel > 0:
lower_jerk = max(0.5, lower_jerk)
upper_jerk = lower_jerk + 0.5
else:
# When decelerating from very low speeds allow more jerk to prevent a slow stop
lower_jerk = max(0.2, lower_jerk)
upper_jerk = lower_jerk + 0.5

I'm guessing these edge cases might now be covered by the error correction

[pfeiferj]

Ok, I'm also going to test the change, thanks. I'm thinking that this new min jerk calculation based on accel error might allow to simplify/remove this bit:

    if CS.out.vEgoRaw < 4.:

      if accel > 0:

        lower_jerk = max(0.5, lower_jerk)

        upper_jerk = lower_jerk + 0.5

      else:

        # When decelerating from very low speeds allow more jerk to prevent a slow stop

        lower_jerk = max(0.2, lower_jerk)

        upper_jerk = lower_jerk + 0.5

I'm guessing these edge cases might now be covered by the error correction

Possibly, but for the accelerating jerk I'm skeptical just because of actuator lag

[rchybicki]

I drove for 2-3 hours yesterday and haven't had the insufficient braking problem, so that is potentially solved, but the downside is that it's now more jerky, it feels closer to stock exp mode now. I'll try making the
min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 50)
a smaller fraction of the delta and see how that works.

[pfeiferj]

I drove for 2-3 hours yesterday and haven't had the insufficient braking problem, so that is potentially solved, but the downside is that it's now more jerky, it feels closer to stock exp mode now. I'll try making the

min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 50)

a smaller fraction of the delta and see how that works.

Originally (before this PR) I had used the difference in actual accel and target accel as the jerk and had noticed it wasn't much of an improvement over stock behavior. So it does make sense that you're seeing the same when using it as a min.

Using a smaller fraction as you suggested would likely help. currently it's dividing by 50 because this code gets hit at 50 hz. But with this being a minimum, dividing by more than 50 shouldnt be a problem and likely would still help relieve the lag in the capped acceleration situation.

Another potential solution would be to only use the min value if it's greater than say 0.25, or possibly to do both that and the suggestion you made. I think I'll start by going with your suggestion and trying dividing by 100 for the min value. If it doesn't help I'll try adding some logic for when to use the min value

[rchybicki]

I've been driving with:
min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 25)
and
min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 15)
and some time with stock jerk after that. Stock jerk feels super jerky compared to both in exp mode, definitely uncomfortable. With 15 it's still more jerky than without min_required_jerk, so there might still be room for improvement, and I haven't seen any issues with hard braking yet. In general, the difference between stock and this is significant, definitely a keeper for me and possibly for upstream when it's tuned.
For testing I removed the low-speed positive accel increase and haven't felt any difference yet, I think the accel error might be taking care of it but I need to use it some more to be sure.

[pfeiferj]

I've been driving with:

min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 25)

and

min_required_jerk = min(2.5, abs(accel - CS.out.aEgo) * 15)

Lol I flipped the direction in my head in my last comment, my divide by 100 would have actually been multiply by 25. So I'll just update this to be multiply by 15 since you've already tried that and not gotten any bad breaking behavior.

I did have a PR for this upstream at one point and it got closed recently, which I never really proved it didn't cause any problems so makes sense (especially makes sense after you pointed out the issues you saw lol). they also said they don't have a whole lot of time to validate themselves, so aren't really accepting tuning for individual car types like this. So I probably won't open upstream in the near future

[rchybicki]

Been tuning the approach for the last two weeks, here's what I'm using:

# long_plan.accels can be empty, use current accel as a fallback
req_accel = self.sm['longitudinalPlan'].accels[0] if len(self.sm['longitudinalPlan'].accels) else accel
min_required_jerk = min(2.5, abs(req_accel - CS.out.aEgo) * 20)
max_required_jerk = 3.0
      
lower_jerk = clip(abs(accel - self.accel_last) * 20, min_required_jerk, max_required_jerk)

#allow highest jerk instantly for emergency braking
if accel < -3.:
lower_jerk = 3.

upper_jerk = lower_jerk + 0.5
stopping = stopping and CS.out.vEgoRaw < 0.01
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, upper_jerk, lower_jerk, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled, CS, escc, self.CP.carFingerprint))
self.accel_last = accel

I calculate the error as the plan - aEgo instead of MPC accel vs aEgo. It's smoother now but still works for strong braking. I'm not sure if the accel < -3 override is needed, but I'm assuming it will shave off a couple hundred milliseconds potentially on a strong brake.

[pfeiferj]

I calculate the error as the plan - aEgo instead of MPC accel vs aEgo. It's smoother now but still works for strong braking.

Nice! that makes a lot of sense, prevents the mpc corrections from influencing things.

not sure if the accel < -3 override is needed,

Maybe not, but honestly I like erring on the side of stopping speed here and keeping it.

[pfeiferj]

@rchybicki what are you doing to pass the submaster into the car controller? I'm not seeing a great way to do it cleanly and was curious how you went about it

[rchybicki]

Initially my clunky approach was to pass the long_plan to the update fn, but that meant changing all the interface files for all the cars - not great. Recently Sunny added sm to the Hyundai carcontroller and now I'm using that:
https://github.com/sunnyhaibin/sunnypilot/blob/dev-c3/selfdrive/car/hyundai/carcontroller.py
in init
self.sm = messaging.SubMaster(['longitudinalPlan'])

[pfeiferj]

https://github.com/sunnyhaibin/sunnypilot/blob/dev-c3/selfdrive/car/hyundai/carcontroller.py
in init
self.sm = messaging.SubMaster(['longitudinalPlan'])

interesting, I thought that you could only have one sub master per process, but I guess that must only be the case for pub master

[rchybicki]

Some new findings on the jerk front, I still thought low speed SNG on exp mode was too jerky, and I was suspecting
upper_jerk = lower_jerk + 0.5
to be the cause. I thought I'd try lower_jerk at 0 and upper_jerk where lower_jerk was or a bit higher. Turns out lower jerk only applies to deceleration, so my car wouldn't slow down ever :-)
That explains why comma had lower_jerk set higher than upper_jerk, they wanted lower_jerk high for safety and upper_jerk lower for comfort. I'm testing out lower_jerk = upper_jerk, and I can already feel it's smoother - that's because upper_jerk doesn't add the 0.5 at all speeds I believe.
The base jerk is now *100, since we have to compensate for the lack of +0.5, and min_required_jerk is *30 and *15 for breaking and accel - seems to be working fine under hard braking.

   braking = accel < 0 and accel - self.accel_last <= 0
    # long_plan.accels can be empty, use current accel as a fallback
    req_accel = self.sm['longitudinalPlan'].accels[0] if len(self.sm['longitudinalPlan'].accels) else accel
    min_required_jerk = min(2.5, abs(req_accel - CS.out.aEgo) * (30 if braking else 15))
    max_required_jerk = 3.0
    
    jerk = clip(abs(accel - self.accel_last) * 50 * 2, min_required_jerk, max_required_jerk)

    #allow highest jerk instantly for emergency braking
    if accel < -3.:
      jerk = 5.

    upper_jerk = jerk
    lower_jerk = jerk


"JerkUpperLimit": clip(upper_jerk, 0.1, 5.0), # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": clip(lower_jerk, 0.1, 5.0),  # stock usually is 0.5 but sometimes uses higher values

[pfeiferj]

Turns out lower jerk only applies to deceleration

Very cool! Can't believe I didn't notice that before. So basically that suggests each jerk is the max jerk the car will apply either up or down for their respective values.

[pfeiferj]

That leaves me with another idea, when accelerating we probably want the lower jerk to be small, and when decelerating we probably want the upper jerk to be small. That way if the ecu over shoots before our next control cycle it wont try to over compensate the other way

[pfeiferj]

@rchybicki I was just thinking about some past behavior i've seen and realized the lower jerk probably isn't just deceleration and is instead just the amount of jerk down from the current acceleration. So If you're going 3m/s and in one second you wanted to be going 2m/s you would want 1m/s of lower_jerk

[rchybicki]

That leaves me with another idea, when accelerating we probably want the lower jerk to be small, and when decelerating we probably want the upper jerk to be small. That way if the ecu over shoots before our next control cycle it wont try to over compensate the other way

good idea, haven't thought about that, I'll try that too. We can compare the the current accel to the previous one, and make the opposite jerk 0. I'll check how that works.

[rchybicki]

@rchybicki I was just thinking about some past behavior i've seen and realized the lower jerk probably isn't just deceleration and is instead just the amount of jerk down from the current acceleration. So If you're going 3m/s and in one second you wanted to be going 2m/s you would want 1m/s of lower_jerk

Hmm I'm not sure what it really is limiting. I thought that would be the direction of the change like you describe, but when I set lower_jerk to 0 and tried to drive, it would overshoot the set speed significantly but it would stop accelerating at some point. I think I tested 40kph set went to 60kph. If lower_jerk would apply to all acceleration reduction it should never stop accelerating I think. Or maybe it applies to all reduction/increase, but 0 isn't really 0 but nearly 0 and that's why it overshot 40 to 60kph. We need to test more :-)

[pfeiferj]

Hmm I'm not sure what it really is limiting. I thought that would be the direction of the change like you describe, but when I set lower_jerk to 0 and tried to drive, it would overshoot the set speed significantly but it would stop accelerating at some point. I think I tested 40kph set went to 60kph. If lower_jerk would apply to all acceleration reduction it should never stop accelerating I think. Or maybe it applies to all reduction/increase, but 0 isn't really 0 but nearly 0 and that's why it overshot 40 to 60kph. We need to test more :-)

I tried just setting upper/lower jerk to 1/2 the opposite depending on if we were increasing or decreasing acceleration and it didn't seem to cause any problems, im going to try something a bit more aggressive than 1/2 today and see if it smooths things out

[pfeiferj]

@rchybicki haven't driven on it much, but just a simple check of

if accel > aEgo:
  lower_jerk = 0
elif accel <= aEgo:
  upper_jerk = 0

doesn't seem to cause any problems and my initial impression is that it does make things smoother

[rchybicki]

Same experience here, I'm using:

upper_jerk = jerk if accel >= self.accel_last else 0
lower_jerk = jerk if accel <= self.accel_last else 0

[PugTechnology]

Random comment incoming!

Is there any progress on this idea? I feel that my vehicle would benefit from this change

[pfeiferj]

Random comment incoming!

Is there any progress on this idea? I feel that my vehicle would benefit from this change

I've been running the following for a while, never got around to updating this draft pr branch:
commaai/openpilot@master...pfeiferj:openpilot:pfeifer-hkg-long-control-tune

[PugTechnology]

Awesome! I'll test it out on mine and see how it flows