Calculate average desired curvature using planned distance instead of vego

[pfeiferj]

The problem:

The psis used when calculating the average desired curvature in get_lag_adjusted_curvature appear to be the psi based on the planned acceleration. In get_lag_adjusted_curvature we try to remove the distance from the psi to get the curvature. To determine the distance the function takes the current velocity times the lag. However, when the model is planning on having a positive or negative acceleration in the curve there will be a difference in distance the model plans on traveling vs the distance that would be traveled at the current velocity. This results in poor planner performance in the curve because the planner is using a falsely high or low curvature depending on negative or positive acceleration in the model.

Solution:

This change gets a simple average of the first planned velocity and the velocity interpolated at the lag time to then use in the distance calculation. This results in more accurate curve planning by the planner.

Graphs:

Without change:

Notice how the planner curvature (green second row) flattens out at the apex of the curve whenever we begin accelerating.

With change:

In this screenshot note how much smoother the planner curvature lowers and rises. Also note that this was even hitting the curve 10 mph faster than in the previous graph without the change, which traditionally would have caused poorer performance.

Personal Observation:

Control throughout curves feels noticeably smoother and more planted. Fewer odd jerks of the steering wheel. Seems not to cut the corner as bad either.

[pfeiferj]

Just made another observation from the graphs. Despite hitting the curve 10 mph faster there was still less roll than when I hit the curve without the change. This lines up with my impression of how it feels in the curves.

[rchybicki]

Not sure if this helps, but I tested the code out, and it feels better in many turns with speeds above 40kph, but there is one short, low speed turn on the way to my house that it's not making anymore when I apply these changes, like it didn't have enough torque - but there's also no torque limit warning.

Here's a clip and route before applying:
https://connect.comma.ai/36921882a3bf0f19/clips/f16a5ed8fadb456a8be06452f17471ff
36921882a3bf0f19|2023-05-08--15-20-48--10
https://connect.comma.ai/36921882a3bf0f19/1683552048292/1683552839093

and after applying these changes, with me taking over to not hit the curb:
https://connect.comma.ai/36921882a3bf0f19/clips/9db1a9a705684e1e943419b610f559be
36921882a3bf0f19|2023-05-08--08-33-44--9
https://connect.comma.ai/36921882a3bf0f19/1683527624477/1683528386889

I'm driving through that curve very often, I did multiple tests with and without the patch with the same results each time. I'm running a fork of the latest sunnypilot dev branch, with some changes on top but I think none that would affect lateral.

[pfeiferj]

Not sure if this helps, but I tested the code out, and it feels better in many turns with speeds above 40kph, but there is one short, low speed turn on the way to my house that it's not making anymore when I apply these changes, like it didn't have enough torque - but there's also no torque limit warning.

Here's a clip and route before applying: https://connect.comma.ai/36921882a3bf0f19/clips/f16a5ed8fadb456a8be06452f17471ff 36921882a3bf0f19|2023-05-08--15-20-48--10 https://connect.comma.ai/36921882a3bf0f19/1683552048292/1683552839093

and after applying these changes, with me taking over to not hit the curb: https://connect.comma.ai/36921882a3bf0f19/clips/9db1a9a705684e1e943419b610f559be 36921882a3bf0f19|2023-05-08--08-33-44--9 https://connect.comma.ai/36921882a3bf0f19/1683527624477/1683528386889

I'm driving through that curve very often, I did multiple tests with and without the patch with the same results each time. I'm running a fork of the latest sunnypilot dev branch, with some changes on top but I think none that would affect lateral.

Interesting, i'll take a look over the data in plotjuggler here in a bit. there's a chance that the model struggles with that curve and the way that get_lag_adjusted_curvature worked before caused it to request to turn more than the model actually desired, but i wouldn't think it would be that severe at those speed. I know there's still some amount of error in the calculations even with this change because it actually ends up requesting the curvature up to a point just before where we would expect to be distance wise. There's some things that could be done to deal with that last bit of error but my assumption was it was low enough of an error to not be worth the increase in complexity to fix it. I'll have to take a look at the data though to see if it seems to be caused by the error in the distance we're requesting.

[pfeiferj]

Not sure if this helps, but I tested the code out, and it feels better in many turns with speeds above 40kph, but there is one short, low speed turn on the way to my house that it's not making anymore when I apply these changes, like it didn't have enough torque - but there's also no torque limit warning.

Here's a clip and route before applying: https://connect.comma.ai/36921882a3bf0f19/clips/f16a5ed8fadb456a8be06452f17471ff 36921882a3bf0f19|2023-05-08--15-20-48--10 https://connect.comma.ai/36921882a3bf0f19/1683552048292/1683552839093

and after applying these changes, with me taking over to not hit the curb: https://connect.comma.ai/36921882a3bf0f19/clips/9db1a9a705684e1e943419b610f559be 36921882a3bf0f19|2023-05-08--08-33-44--9 https://connect.comma.ai/36921882a3bf0f19/1683527624477/1683528386889

I'm driving through that curve very often, I did multiple tests with and without the patch with the same results each time. I'm running a fork of the latest sunnypilot dev branch, with some changes on top but I think none that would affect lateral.

One other thought is that it could be related to the STEERING_RATE_COST in the lateral planner. It would stand to reason that the amount of extra curvature that get_lag_adjusted_curvature was requesting overcame the high steering rate cost that's set in the lateral planner currently, but that once it starts requesting a more accurate curvature the steering rate cost overcomes the curvature the model is requesting and causes that feeling of not having enough torque at low speeds

[rchybicki]

Not sure if this helps, but I tested the code out, and it feels better in many turns with speeds above 40kph, but there is one short, low speed turn on the way to my house that it's not making anymore when I apply these changes, like it didn't have enough torque - but there's also no torque limit warning.
Here's a clip and route before applying: https://connect.comma.ai/36921882a3bf0f19/clips/f16a5ed8fadb456a8be06452f17471ff 36921882a3bf0f19|2023-05-08--15-20-48--10 https://connect.comma.ai/36921882a3bf0f19/1683552048292/1683552839093
and after applying these changes, with me taking over to not hit the curb: https://connect.comma.ai/36921882a3bf0f19/clips/9db1a9a705684e1e943419b610f559be 36921882a3bf0f19|2023-05-08--08-33-44--9 https://connect.comma.ai/36921882a3bf0f19/1683527624477/1683528386889
I'm driving through that curve very often, I did multiple tests with and without the patch with the same results each time. I'm running a fork of the latest sunnypilot dev branch, with some changes on top but I think none that would affect lateral.

One other thought is that it could be related to the STEERING_RATE_COST in the lateral planner. It would stand to reason that the amount of extra curvature that get_lag_adjusted_curvature was requesting overcame the high steering rate cost that's set in the lateral planner currently, but that once it starts requesting a more accurate curvature the steering rate cost overcomes the curvature the model is requesting and causes that feeling of not having enough torque at low speeds

Ok, let me know if I can help in any way or if you could use more routes.

[YassineYousfi]

why is the (v + velocities[0]) / 2) needed?

[pfeiferj]

why is the (v + velocities[0]) / 2) needed?

it's a rough approximation of the average velocity between now and the point we're calculating the curvature at. average velocity * delay would be the distance the model thinks you're going to be at for the psi value. If we use only the velocity at the delay then we would have a similar issue as using v_ego, the final velocity * delay will either overshoot or undershoot based off of if the starting velocity was higher or lower than the final velocity.

[pfeiferj]

another thought i had since i made this PR was potentially exposing the x solution from the longitudinal mpc in the longitudinalPlan. Grabbing the x solution position at the delay would likely be more accurate than this rough average.

[YassineYousfi]

That makes more sense. Or you can integrate the speeds up to t=delay using np.cumsum if you want to quickly experiment without changing the messages.

[pfeiferj]

Switched over to mpc distance. Initial impression is good, but i'll be getting in more driving today.

[pfeiferj]

Pretty happy with how it's performing. I've made a route public to use as an example: https://connect.comma.ai/6cc716ffa2eb1f32/1684276696320/1684277240226

[pfeiferj]

@YassineYousfi any thoughts on this in its current state? Or anything you'd like me to do for validation?

[james5294]

This single-handedly removes crossing the yellow line on every other corner for me. Without this modification op is almost unusable on the winding side roads where I live. I have tried it on master. I have tried it on other forks, no matter where I place this commit the effects are immediate.

22 sonata limited

[rchybicki]

My issue is also resolved, it was model based - no longer a problem on Nicky, and after driving with this change for a longer while I can attest that it does mostly solve corner cutting on tighter turns.

[james5294]

without this I still experienced cutting corner, with both new models, including the newest, hot coffee.

[pfeiferj]

I still experienced cutting corner, with both new models, including the newest, hot coffee.

Do you mean with this change? Rchybicki is saying that with this change most of his cutting is gone

[rchybicki]

I still experienced cutting corner, with both new models, including the newest, hot coffee.

On stock Nicki me too, with this PR and Nicki nearly no more corner cutting.

My issue, the one that was resolved, was about not having enough torque on one specific turn - that one is resolved and was only model related.

[YassineYousfi]

We are looking into this. Some turn cutting comes from higher in the planner stack which we are also looking into.

[pfeiferj]

hmm, not sure how but i managed to have my cereal and panda out of sync with master. updated to pull in latest master