Allow setting N=0 to select gridpoints automatically

[hungpham2511]

Changes in this PRs:

• Implement an algorithm to choose gridpoint automatically
• Integrate to TOPPRA with N=0

Checklists:

• [ ] Update HISTORY.md with a single line describing this PR

[hashb]

I ported this snippet from drake library (link). It uses linked list instead of vector. It significantly reduces time complexity of function since we don't have to call sort. Would be great to have it integrated into toppra library.

toppra::Vector calculateGridPoints(const toppra::GeometricPathPtr path, const double max_err_threshold,
                                   const int max_iteration, const double max_seg_length, const int min_nb_points) {
  auto path_interval = path->pathInterval();
  std::forward_list<double> gridpts{path_interval(0), path_interval(1)};
  int num_grid_points = 2;
  for (int iter = 0; iter < max_iteration; iter++) {
    bool add_points{false};
    for (auto point = gridpts.begin(); std::next(point, 1) != gridpts.end(); point++) {
      const double mid_pt = 0.5 * (*point + *(std::next(point, 1)));
      const double dist = *(std::next(point, 1)) - *point;
      if (dist > max_seg_length) {
        add_points = true;
        gridpts.emplace_after(point, mid_pt);
        num_grid_points++;
        point++;
        continue;
      }

      const double error = 0.5 * std::pow(dist, 2) * path->eval_single(mid_pt, 2).cwiseAbs().maxCoeff();
      if (error > max_err_threshold) {
        add_points = true;
        gridpts.emplace_after(point, mid_pt);
        num_grid_points++;
        point++;
        continue;
      }
    }
    if (!add_points) {
      break;
    }
  }
  while (num_grid_points < min_nb_points) {
    for (auto point = gridpts.begin(); std::next(point, 1) != gridpts.end(); std::advance(point, 2)) {
      const double mid_pt = 0.5 * (*point + *(std::next(point, 1)));
      gridpts.emplace_after(point, mid_pt);
      num_grid_points++;
    }
  }
  std::vector<double> result(gridpts.begin(), gridpts.end());
  return Eigen::Map<toppra::Vector, Eigen::Unaligned>(result.data(), result.size());
}

[hungpham2511]

Nice work! Thanks.

[hungpham2511]

@jmirabel let me know if you would like to have a look. Otherwise I will merge this in a few days.

[hungpham2511]

Do you expect some kind of regularity in the spacing between points ? I don't completely grasp the logic.

It's a port of the propose gridpoint procedure in the Python code. I will copy the docstring over. That should explain what is happening.