Plan fails when there are impossible contours
Sometimes a contour is impossible due to corresponding ik solutions not existing at every waypoint.
Options:
-
ignore contour and keep going until can't go anymore (current behavior)
-
exit plan so far
-
take deeper dive into ik because the ik solutions should exist - ikfast only giving 4 solutions instead of 8
-
Check ikfast is returning sufficient solutions -
fix ikfast if necessary -
Check if the error is caused by a 360 degree wrap around leading to large angle differences -
fix if necessary -
Pre-compute contour trajectories by both arms for contours and output how many contours are impossible -
if ignore=True, mark these as complete and keep printing, else stop immidiately if there are any impossible contours