Improve path alignment / follow critics so they don't try to shortcut turns (and/or obstacle critic to better handle it) about mppic HOT 7 CLOSED

increase prune dist to help with path alignment?

from mppic.

I found the issue, the last critic to still use the path ratio metric is the PathFollow critic making it only score intermittently. Since we prune the path based on costmap size, that metric is bad measure of the distance to the actual goal pose which isn't going to be on the path at all for most of the navigation request. I'm going to update it with the measure I replaced all the other critics with and then it should score more regularly to be retuned in proportion to the Goal critic to balance driving to the goal at the end of the pruned path with the mid-point from the PathFollow offset. Maybe even remove the Goal Critic from being used outside of approach to goal.

EZ-PZ. Retuning will be difficult(ish) but should have a solution to this in #99 shortly in a couple of days. Easy code fix, just need to make sure it doesn't introduce other issues and retune for reasonable non-short-cutting behavior.

from mppic.

Mhm, well fixing the PathFollow helps, but I now see that the path align critic has some very strange features. With it removed, the robot moves much slower and actually tracks the path more closely. I need to dig into why the PathAlign critic has any forward-driving characteristics at all (and if there are some issues here why its not aligning as well with the path).

Edit: it seems like the if(segment_short(s)) line is called for nearly all things, which is very strange and means that the distance metrics from the path are probably not right if we're not actually computing u, so its not surprising to me then that the path align critic isn't fully working. Something's not quite right here.

const auto && P2_P1_norm_sq = xt::eval(P2_P1_dx * P2_P1_dx + P2_P1_dy * P2_P1_dy);
const auto segment_short = P2_P1_norm_sq < 1e-3f;

These seem pretty straight forwardly correct and path points should be ~5cm apart, not less than 1mm.

Edit2: sqrt(1e-3) = 0.032 which is suspiciously close to what the actual path resolution might be. I lowered this to 1e-6 so that its 1mm of non-square distance apart. This may have been an oversight I made at some point when optimizing for reducing sqrts. That seems to make us actually now evaluate u but still has a forward driving force I don't understand and isn't well aligning the path even with a high weight

Edit3: the forward-driving characteristics are because the PathAlign population of furthest_reached_path_point is higher than that of the setPathFurthestPointIfNotSet for use in the other critics. So when that's not used, then the value is different impacting those critics functions. That's something to look into later. Still trying to figure out why it is the path align critic seems to do such a poor job of path-alignment. Whether I set the weight to 0 or 5000, I see little to no difference.

from mppic.

Found it! Obstacle critic was set to cost = <cost> instead of += so everything prior to it was being zeroed out and not counted (e.g. Path Align). With that patch, we get its results considered now. Will now retune tomorrow and find the bug in setPathFurthestPointIfNotSet / furthest_reached_path_point

from mppic.

@artofnothingness I found that in the PathAlign critic, the condition of u < 0 or u >= 1 represents >99% of the samples distances taken for the shortest distance to the path. That means all the interpolation stuff we're doing isn't even being functionally used. I think that means we can simply this critic to be simply path_points - trajectory_points and find the minimum for the axes without the need for any of what this critic is doing. And potentially removing the 3x nested for loop which should speed things up here.

Even without tensorifying it, just removing all the U stuff makes it run at 30hz with 2000 samples with every single path and trajectory point with no appreciable change in path tracking quality 😄 Tensorified can be simplified to

  const auto T_x = xt::view(data.trajectories.x, xt::all(), xt::all(), xt::newaxis());
  const auto T_y = xt::view(data.trajectories.y, xt::all(), xt::all(), xt::newaxis());

  const auto P_x = xt::view(data.path.x, xt::range(_, -1));  // path points
  const auto P_y = xt::view(data.path.y, xt::range(_, -1));  // path points

  const size_t time_steps = T_x.shape(1);

  auto diffs_x = T_x - P_x;
  auto diffs_y = T_y - P_y;
  // size should = (batches, timesteps, subtraction values)
  auto && dists_sq = xt::eval(diffs_x * diffs_x + diffs_y * diffs_y);

  auto mins = xt::amin(dists_sq, {1}); // size (batches, timesteps)
  auto mean_dists = xt::sum(xt::sqrt(mins), {1}, immediate) / time_steps; // size (batches)
  data.costs += xt::pow(mean_dists * weight_, power_);

Which also considers all points without needing steps. I'm ultimately going to go with the non-tensor version though - the amin takes a ton of time and I'm not sure why. It takes a solid 12 ms by itself putting it back in the range of the old performance and a bit worse. Non-tensor version runs in << 1ms and puts the typical steady-state run rate at ~70hz with 2000 samples. Maybe there's something I'm doing wrong here, but ultimately its probably not worth thrashing over for days on end if I have another solution that works well.

from mppic.

The shortcutting also appears to be from the Path Angle critic - since we look very far ahead for the path angle to apply the critic, it can create shortcutting. Its needed to balance a local minima condition, but I've shown by removing it (without triggering that condition) path tracking increases significantly. Some reworking here will likely be all we need in conjunction with the changes above to call this a wrap.

from mppic.

#106 to resolve

from mppic.