Improvement / workaround: MoveIt! collision checking in-between trajectory keyframes

[JenniferBuehler]

The MoveIt! motion planner only does collision checking at key frames (which is certainly intended due to performance reasons). If there is a collision in-between key frames, it is not detected. Therefore, in some instances, the arm may still collide with objects.

The probably easiest way to fix this is to check each trajectory by refining it to key-frames at very small intervals, then do the collision checking again, and only accept the trajectory if there are no collisions. If there are collisions, re-compute a new trajectory.

Until this fix is done, a way to detect such a "faulty" trajectory is to enable the display of the trajectory path in RViz and visually check for possible collisions. If one is found, then abort execution and try again.