toppra
toppra copied to clipboard
`ConstAccel::evalParams()` computes wrong `k_grid` when `ts[i]` is an element in `m_ts`
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.