Unpredictable trajectory between two known points

[FrancoisLasson]

Currently, a Bezier trajectory is equivalent to a line of robot poses without any constraints between known poses. Hence, we can't check positions and orientations of the tool between two poses. This could causes several problems of collision (video).

I think the best way to fix this problem is to use Descartes.

[shaun-edwards]

It looks like you are moving through a singularity. Descartes will try to avoid singularities if it can. However, even Descartes relies on a discretized path, so a very sparse path is not guaranteed to not have these problems.

Have you tried re-sampling your path and apply your current planning method?