MPPI Obstacle bypass critic

[SteveMacenski]

bypass_critic.mp4

This critic is an obstacle bypass critic to actively help the optimization problem bypass an obstacle blocking a path to handle dynamic obstacles more naturally in a variety of situations.

TODO

• Docs PR
• Clean up (remove TODOs, parameter changes)
• Dexory test / integration
• External validation testing by @adivardi / @CarlosHdezM

Related: #6091

[mergify]

This pull request is in conflict. Could you fix it @SteveMacenski?

[tonynajjar]

I merged main into your branch

[adivardi]

is that intended? or a test-only change?

[SteveMacenski]

test only!

[adivardi]

you could do it together with the previous for to save on 1 loop

[adivardi]

I have tried this today a bit in simulation, by adding a car perpendicular to the robot on the path (a car parallel to the path can already be overcome with my change).

With only a single car, the robot can overtake the car, so it is an improvement over the PathAlignCritic only 👍

---

I did find a case that break it, when adding a wall or another car on the side. Because only the front of the wall/car can be seen, it chooses the wrong side.
I think this case could be quite common. So what I want to try tomorrow is to also check the cost over the line connecting base_link and the target point in determineBestBypassSide.

Though I can see a case where this approach also fails - if the obstacle is spans a long part of the path. Since the target point is in the middle of the obstacle, it may happen that the line crosses the obstacle. Maybe you have other ideas?

You can see the video: The light blue point on the bottom right is (target_x, target_y)
https://github.com/user-attachments/assets/164bdef2-d88e-4f0e-b6ee-60498afa5af7

2. I also think it needs a bit more tuning. Especially, offset_from_furthest. Unlike PathAlign, I think this critic should trigger when the robot is really slow near obstacles (so when furthest_reached_path_point is smaller). I did one test with offset_from_furthest: 10 and it was slightly better (but more tests needed)

3. Right now, it is used for both the Don't apply when first getting bearing w.r.t. the path check and for choosing the reference point. Maybe it is worth having a difference param for these 2 uses, so they can be tuned independently?

4. Finally a question: Why do you set the final target point based on furthest_reached_path_point + offset_from_furthest_ and not on obstacle_idx (which is used for determineBestBypassSide) ?
   Nevermind, that makes no sense, because the target is then fixed in place and doesn't move to keep in front of the robot

[SteveMacenski]

I did find a case that break it, when adding a wall or another car on the side. Because only the front of the wall/car can be seen, it chooses the wrong side.

Oh, I see. That is an interesting situation.

So what I want to try tomorrow is to also check the cost over the line connecting base_link and the target point in determineBestBypassSide.

That does seems reasonable, how well did that work? Why not use blocked_idx rather than obstacle_idx for the check that way it uses the first path-point blocked rather than the middle point to get around that issue with a long-obstacle on the path for the line-of-sight check? That would seem to me to catch the case and we know its not in collision.

I also think it needs a bit more tuning. Especially, offset_from_furthest. Unlike PathAlign, I think this critic should trigger when the robot is really slow near obstacles (so when furthest_reached_path_point is smaller). I did one test with offset_from_furthest: 10 and it was slightly better (but more tests needed)

Do you think in general we should make it larger or smaller?

Right now, it is used for both the Don't apply when first getting bearing w.r.t. the path check and for choosing the reference point. Maybe it is worth having a difference param for these 2 uses, so they can be tuned independently?

I agree. Do you think we should just remove it or make it parameterized?

[adivardi]

I am working on a few improvement this week, hope to come back with some nice results soon.

Edit: So I tried a few variations for this issue. I will comment with more details tomorrow, but I think the best one is checking the line from the robot to a pose = (last free path pose + signed offset).

I still want to try out some tuning and some more ideas (like adding an hysteresis for the sign, as it tends to jump), but my main 2 issues right now are:

1. It tends to drive very close to obstacles corners, probably because the target pose tends to be somewhat behind them. Do you think I should just increase the offset? Or maybe the weight of the CostCritic or inflation?

2. When driving close to obstacle, the controller sometimes resets if it fails once. This causes the critic to disable because of the check Don't apply when first getting bearing w.r.t. the path check. I tried to remove this check, but then the critic remains enabled when the robot backups away from obstacles, which is not good (it leads to the robot not backing up enough, and getting stuck on the obstacle repeatedely).
   I think keeping the check but reducing the index from 20 to 10 is better.
   Another option would be to disable the critic when robot.speed < 0, but that won't break if the robot is tracking the path backwards.

Happy to hear suggestions on these two points

[SteveMacenski]

I'm back from vacation now - did you do that testing and any other results / details (I think you mentioned later posting more details?)? I wanted to get that info before responding to your remarks and picking this back up again.

[adivardi]

So I tested quite a lot and tried multiple variations for different issues with it. (all in simulation as our robot is not available until next week).

things I added;

• check the line connecting base_link with point last_free + signed_offset -> solves the issue with the wall next to the obstacle
• Add hysteresis for the side - prefer the previous side found unless it is not valid anymore -> also reduces a lot of side switching due to inaccurate perception
• split offset_from_furthest to 2 params
• Tested a few idea to make it resume path tracking earlier before the obstacle is fully passed, so we can go back to the global path earlier -> nothing really good

You can see my current implementation here (WIP AI code warning 😅 )

---

The main issues I still have are what I wrote last time (repeat from above + extra comments):

1. It tends to drive very close to obstacles corners, probably because the target pose tends to be somewhat behind them.

   • It did get better with the side hysteresis, but still sometimes an issue
   • I tried tuning the costmap inflation and increasing the CostCritic weight but it didn't really help

2. When driving close to obstacle, the controller sometimes resets if it fails once. This causes the critic to disable because of the check Don't apply when first getting bearing w.r.t. the path check. I tried to remove this check, but then the critic remains enabled when the robot backups away from obstacles, which is not good (it leads to the robot not backing up enough, and getting stuck on the obstacle repeatedly).

   • I tried removing the check and disabling the critic when robot.speed < 0 and it seems to improve the obstacle handling (but only 5 repetitions in sim). I want to try this a bit more tomorrow
   • I think that would not work when tracking the global path backwards, so I was thinking to instead disable the critic when robot.state is pointing to the opposite direction from the target

Happy for suggestions!

[adivardi]

Update:
I implemented this disable the critic when robot.state is pointing to the opposite direction from the target and removed the Don't apply when first getting bearing w.r.t. the path check
=> works much better! it gets stuck a lot less on obstacles' corners while not degrading the reverse behavior. The distance to obstacles is not really increased (still passes just outside the inscribed radius cost) but it gets stuck only rarely

[SteveMacenski]

So open questions I'm aware of to compress the discussion into one place for continuing work this week:

1. Side-wall / large obstacle next to obstacle

check the line connecting base_link with point last_free + signed_offset -> solves the issue with the wall next to the obstacle

Cool! Do you have a patch I can test / integrate? Your branch is a bit ... AI generated and has some other items in it that make it hard for me to pull out just one thing at a time.

2. Parameterize or remove 'Don't apply when first getting bearing w.r.t. the path check' check

Is there any compelling reason to keep this on or should we just simply remove the condition?

3. It tends to drive very close to obstacles corners

I implemented this disable the critic when robot.state is pointing to the opposite direction from the target and

Sounds like you make have resolved that with. Similar: can I get a patch? I also need to consider bidirectional cases to make sure this works well symmetrically, but I can do that and have Tony verify.

---

Add hysteresis for the side - prefer the previous side found unless it is not valid anymore -> also reduces a lot of side switching due to inaccurate perception

How often did you run into that? I tried that but found that it sometimes made commitment to bad sides due to inaccurate perception and found this to be worse off.

split offset_from_furthest to 2 params

I think this needs some explanation :-)

^^ how many of the items above were 'testing' features vs items you think I should integrate into the final solution? Any others?

[adivardi]

Yes, I couldn't test on the robot yet, so I didn't clean up the code yet, it's a bit AI-style...

The list 2 comments above are things that work well, with the following update:

• split offset_from_furthest to 2 params -> becomes: disable the critic when robot.state is pointing to the opposite direction from the target and removed the Don't apply when first getting bearing w.r.t. the path check (which was 1 of the 2 params)

-> this works quite well. It still drives close to corners, but the bypass critic remains active and it reduces the times it gets stuck by a lot.

---

Side hysteresis:
I had issues with the side jumping whenever I have an obstacle which shows up "hollow" in the map (as perception only sees the convex outline). It usually happened while driving next to the obstacle, so the effect was not fully detrimental. I didn't see bad side effects to adding this, so I kept it. Could be that it doesn't work well on certain scenarios I don't have in my tests.

---

I hope to finally test it next week on the robot, then I can clean up a bit and send some patches/