robo-dani

I think the issue may be that the p2'sindex `i + 1` is out of bounds. ```C++ for (size_t i = 0; i < poses.size(); i++) { auto p1 =...