hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
603 stars 167 forks source link

`ConstAccel::evalParams()` computes wrong `k_grid` when `ts[i]` is an element in `m_ts` #230

Open TungYiLai opened 1 year ago

TungYiLai commented 1 year ago

Describe the bug Based on the description, the function should find k_grid s.t m_ts[k_grid] <= ts[i] < m_ts[k_grid + 1] but it doesn't.

To Reproduce Try this code snippet

constexpr std::array<double, 7> m_ts = {0, 0.1, 0.2, 0.4, 0.8, 1.6, 3.2};
constexpr double ts = m_ts[2];
const auto ptr = std::lower_bound(m_ts.data() + 1, m_ts.data() + m_ts.size() - 1, ts);
std::cout << "ts: " << ts << ", k_grid: " << (ptr - m_ts.data() - 1) << std::endl;

The last line outputs ts: 0.2, k_grid: 1

Expected behavior The correct k_grid should be 3 instead of 2.

Version 35f8c7d5e245517fb4e0031a3c890934792fdfc1