Move slightly up and down between contours
Current motion planning goes directly to start of contour. For safety, before the start of each contour, plan to slightly above starting point and go down. After the end of each contour, go slightly up.
These additional movements should be added to between contour trajectories.
- if current movement is going to a contour (and not home or staying put), plan to slightly above the end pose and then move down linearly.
- if current motion is being planned from a contour, move slightly up first, then plan to end point