MPPI: Logic issue in findPathFurthestReachedPoint for certain paths

[tonynajjar]

Observation

In the following example:

The pose visualized at the end of the path is the pose returned by findPathFurthestReachedPoint. However this is wrong as it is clear that none of the trajectories' end pose is closest to that end point (which is what findPathFurthestReachedPoint looks for)

Bug Explanation

The function 'findPathFurthestReachedPoint' contains a flawed optimization that causes many trajectories to incorrectly match the points further along the path, even when they are actually closest to earlier path points.

The inner loop uses a constrained search starting from 'max_id_by_trajectories'

This creates a cascading constraint where each trajectory can only search path points from where the previous best trajectory reached forward. This works well when all trajectories progress monotonically forward along the path, however breaks down in a scenario, like the one illustrated above, where the candidate trajectories approach the path "from another direction".

Because the search length becomes limited, some trajectories, in this case the ones facing forward, are falsely considered to be closer to points further along the path when in reality, when searching from the beginning, they are the closest to poses from the beginning of the path.

It's not the easiest thing to visualize, I hope it's clear?

Solution Proposal

1 - Remove the optimization and search from the beginning of the path

2- I'm not sure that's valid but can't we instead use the last optimal path to get the furthest reached point?

P.S: we can add the optimization to stop the search if max_id_by_trajectories is already at the end of the path

[SteveMacenski]

1 - Remove the optimization and search from the beginning of the path

What are the runtime impacts of that? I'm sure we didn't do that for no reason 🙃

2- I'm not sure that's valid but can't we instead use the last optimal path to get the furthest reached point?

That wouldn't be valid, if we accelerate then each iteration's optimal trajectory will be longer than the last.

---

Claude Code gives me something like:

inline size_t findPathFurthestReachedPoint(const CriticData & data)
{
  int traj_cols = data.trajectories.x.cols();
  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
  const auto dists = dx * dx + dy * dy;
  
  int max_id = 0;
  size_t n_rows = dists.rows();
  size_t n_cols = dists.cols();
  
  for (size_t i = 0; i < n_rows; i++) {
    // Early exit if already at path end
    if (max_id == static_cast<int>(n_cols - 1)) {
      break;
    }
    
    Eigen::Index min_id;
    dists.row(i).minCoeff(&min_id);
    max_id = std::max(max_id, static_cast<int>(min_id));
  }
  
  return max_id;
}

What do you think about this instead? That uses vectorization for more of this function which can help with the performance question.

[tonynajjar]

I did some quick tests that hinted that the use of eigen did not make things faster but that's surprising and also not conclusive. In the meantime I made this draft PR (in a rush) that fixes the issue by removing the max_id_by_trajectories optimization and adds another optimization of breaking early if we reached the end of the path.

What remains is profiling but my vacation is imminent + busy for a while afterwards, so if someone takes this over I won't be sad :)

[tonynajjar]

P.S: more food for thought about this issue, should in-collision candidate trajectories be taken into account? It takes only one to ruin the result. In the attached picture, imagine if the path was actually symmetrical with respect to a horizontal line, then the in-collision candidate trajectory in the pic below, would have skewed the result

I touched on this here

One solution I thought about that could solve this particular case without affecting any other behaviors is to only consider trajectories that are not in-collision for the calculation of furthest_reached_path_point. This does however add the requirement for the CostCritic to come before other critics using furthest_reached_path_point.

[SteveMacenski]

I think I responded to some of the potential downsides of that in the same thread 😉 #5661 (comment)