[MPPI] obstacles critic alternative

[BriceRenaudeau]

As discussed in PR #3878 , here is an Issue to start a discussion about the obstacles critic.

Feature discussion

The ObstaclesCritic is a critic to increase trajectory score based on the distance to obstacles. With this critic, the MPPI will prefer trajectories that avoid obstacles. It works well but some points can be discussed.

• The obstacles_critic uses the cost from inflattion_layer to compute distances by using the invert function of the cost.

It makes inflation_layer cost_scaling_factor tunning useless, it's good because it reduces tunning and it may be dangerous as it has a quantization effect on the distance. It also increases the computation load.

• The score is computed differently when the robot is far from obstacles and when it is close.

      if (dist_to_obj < collision_margin_distance_) {
        traj_cost += (collision_margin_distance_ - dist_to_obj);
      }
      if (!near_goal) {
        repulsive_cost[i] += inflation_radius_ - dist_to_obj;
      }

The repulsive cost uses the inflation radius that is not related to safety distance. The traj_cost is computed but only used if trajectory_collide.

• If the footprint has some points in the inscribed area, the score is the same regardless of the distance to the obstacle. Because footprintCostAtPose returns the highest cost which will be "INSCRIBED" whatever the distance to the obstacle, distanceToObstacle(cost) will return the same distance so the score will be the same in a situation where it is important to evade obstacles.

Proposed alternative

The proposed InflationCostCrtic is a simplified version of the ObstaclesCritic;

• It copies all its features
• It doesn't compute distances
• It directly uses the costmap cost repulsive_cost[i] += pose_cost.cost
• It uses a fixed critical score if close to an obstacle

This critic benefits from the exponential factor of the inflation layer, it creates a behavior where the robot is forced in the middle of narrow corridors but can easily evade obstacles in wider spaces. It will be able to take shortcuts from the path without going too close to corners.

Here are the default parameters:

InflationCostCritic:
        enabled: true
        cost_power: 1
        repulsion_weight: 0.015
        critical_cost: 1000.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0

Implementation considerations

I will create a PR to share my code but it is not up to date this the latest commits.

[SteveMacenski]

Sorry, its taken me some time to get to this, Guillaume and I had alot of outcomes from our onsite week and finally ready to address this one!

Can you explain the dual checks on cost at point; those are expensive to do this many N times. I'm not sure why you didn't do the footprint cost checking in costAtPose and avoid the entire second point lookup? Ex there's one in inCollision and another in costAtPose translating the same data with worldToMap and pointCost. It looks like you refactored my functions, but I want to make sure I'm not missing something important here.

---

The main thing I'd like to discuss is the actual scoring part. I have a main concern:

      if (pose_cost.cost >= INSCRIBED_INFLATED_OBSTACLE) {
        repulsive_cost[i] += critical_cost_;

This bit means that the non-circular footprint's cost could be marked as critical_cost_. An edge of a robot can be within the radius of the obstacle, that's not actually a collision if the center point isn't in the radius - though this isn't the collision_cost_. The main concern I have is that it treats all poses where any single point is in the inflated space as the same cost (and very high). I suppose only semantically different than the existing critic (but now orientation also plays a part, so triggers more liberally here), but I thought this was trying to address that point. Would the robot simply reject navigating in space that is essentially fully covered by 253 costs except a narrow aisle in center? Have you tested narrow spaces relative to the robot size?

1000 * 0.015 = 15 which would correspond to a "virtual" penalty of 15m (since the other critics are in SI units). That feels excessive for non-circular robots and I'm shocked you can even get the robot to enter confined spaces at all given the big step function. I guess leading up to ~150-200 of pose_cost.cost to that number would make the 1000 less aggressive, but that's still a 5x+ increase.

How sensitively did you have to tune the gains to make it work reasonably well? If you made 0.015 into 0.03 or 0.06, does it explode and stop working or just degrade? Same with the 1000 setting. When I originally tried to make costs work, I found that the range of values that worked was so narrow, it would take a day or more of tuning a robot to get it to do anything. I'm curious if this has this problem or not, or the band of "reasonable" values are large.

It also treats unknown space as an obstacle without a call out to say its OK, but that's easy enough to fix. Note to self.

      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
        repulsive_cost[i] += pose_cost.cost;
      }

I'm curious, did you try normalizing this from 0-1.0 instead of using the raw costs? Did you run into any issues with that? That way the weights could be in a similar order as the others.

Note to self: I decoupled this into a separate if statement (not else if) so we can apply both mechanisms. That resulted in a smoother behavior and could probably be adapted here. No action for you - that's for me to test.

---

At the end of the day, if it works; it works and happy to have another person take a look and succeed where I did not! I am concerned about working with confined spaces and how sensitive the parameter space is, so your experiences creating and tuning this would be valuable. I'll plan to download and play with this after I get your notes and see what I think too

[SteveMacenski]

After some testing: while I don't believe its the end-all-be-all solution for eveything, I suppose it doesn't hurt to provide options. I found that it struggled

• To go from zero-cost space to costed space -- causing a stutter in behavior at the transition. Depending on how you tune this, will make a big difference if you have a "good" or "bad" experience. Though, just a different trade off from the current critic's annoying behavior.
• Seems to get closer to obstacles in some situations, but those are more edge-case-y situations where you're asking it to flip around and go another way in a tight space. Perhaps more forgivable of a getting-close-to-things situation than the usual navigating around corners situation

I'm going to do a little code cleanup and open a new PR for inclusion with documentation. I'll link back here for you to review

[SteveMacenski]

#4090 Please review! Pausing for lunch, but if you're reading this, I probably already added the docs and unit tests, which are my next steps. Once I'm done with those quality steps and I have your testing 👍 I can merge and announce the update!

[BriceRenaudeau]

Hi Steve,

Can you explain the dual checks on cost at point

The costAtPose here is not expensive, it only gets the cost at the trajectory point not the entire footprint.
The function is not well named in this case.
Like this, even if the footprint is in an obstacle radius, it still gets a more detailed cost.

However, the collision is computed using the footprint.
That's why I have dual checks.

I guess leading up to ~150-200 of pose_cost.cost to that number would make the 1000 less aggressive, but that's still a 5x+ increase.

The costmap scores are in the range of 0-252 where the robot can "fit". I chose a 4x increase making 253(INSCRIBED_INFLATED_OBSTACLE) into a 1000 score. Most of the time we don't want the the center of the robot to be in an inscribed area, it means an obstacle is IN the robot. We all add some footprint_padding to create tolerance or safety margins around the footprint. In our case, the footprint_padding can be changed in specific areas to to small or bigger.
This behavior allows the trajectory to go into an inscribed area slightly.

How sensitively did you have to tune the gains to make it work reasonably well?

We may have a non-conventional tuning. We wanted a TEB-like behavior where the robot can largely evade the path to avoid obstacles without the need to recompute the full path.
To do so, the GoalCritic has a wide threshold_to_consider and a high cost_weight and a small PathFollowCritic. This naturally leads to the robot cutting corner trajectories and going close to the corner. We needed a critics that kept the robot far enough from the corner to avoid slowing down the robot but not linearly with the distance.
The repulsion_weight of 0.015 makes inscribed areas a big 15 and the near inscribed point a 3.7. The score decrease is tuned with the inflation layer cost_scaling_factor.
It is as sensitive as the other critics, making any cost twice bigger makes big changes.

I am concerned about working with confined spaces

The robot can go through doors with less than 20cm on each side.

The orange and blu polygons are collision_monitor slow_down and limit polygons.

Struggle to go from zero-cost space to costed space

I saw it too, that's why our inflation_radius is equal to the local costmap size. I think I remember it happening also on the obstacle_critic.

---

I will look at the Cleaned MR.

[SteveMacenski]

The costAtPose here is not expensive, it only gets the cost at the trajectory point not the entire footprint.

Its still heavy to check even just the point. I've done benchmarking on this, even with just checking a single point cost each of the 2000*56 times is the single slowest operation in the controller, actually bringing down the performance quite significantly. I wouldn't have expected you to analyze it yet to that degree, but really drilling into optimizing those checks is some future work for me since it is so nasty and is the main contributor for why you can't run this at 50,60,80 hz on older CPUs.

In my refactor, I restored it to only a single point check.

into a 1000 score. Most of the time we don't want the the center of the robot to be in an inscribed area, it means an obstacle is IN the robot

Ah, that's the subtlty here, you're checking collisions based on the SE2 footprint, but scoring based on center costs. That is subtle and worth you testing to make sure I didn't break anything with that refactor. I'll also make a note of that in the code since I missed that and I read / refactored it.

[BriceRenaudeau]

I will check again.