toppra icon indicating copy to clipboard operation
toppra copied to clipboard

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

Open TungYiLai opened this issue 1 year ago • 0 comments

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

TungYiLai avatar Apr 10 '23 08:04 TungYiLai