Fix the problem that the optimal path may be discarded.

[zarmars]

…mpared. Optimal path may be discarded.

Reference issue

What does this implement/fix?

1. Function 'mod' in numpy library behaves differently from 'fmod' in c++ and it should be treated specially in the implementation of the function 'mod2pi'.
2. In the implementation of function 'set_path', the basis of path length comparison should be the sum of the absolute value of its segment length, otherwise the optimal path may be lost.
   E.g. start point (0, 0, 0), end point defined as follows:
   t, u, w = -math.pi / 6., math.pi / 2., math.pi / 3.
   end_x = math.sin(w - u + t) + 2. * math.sin(u - t) + 2. * math.sin(t)
   end_y = -math.cos(w - u + t) + 2. * math.cos(u - t) - 2. * math.cos(t) + 1.
   end_yaw = w - u + t
   and the optimal path is 'L_t' + 'R_u' + 'L_w' which gives minimum path length math.pi(i.e. 3.14) while the original program presents a non-optimal path with path length 4.18.

Additional information

[zarmars]

F.Y.I
Reference of 'fmod' in c++
https://en.cppreference.com/w/cpp/numeric/math/fmod
Reference of numpy.mod
https://numpy.org/doc/stable/reference/generated/numpy.mod.html

[AtsushiSakai]

Indeed. Thank you!!