shortenpath is erroring out since the input path that is fed to shortenpath is not collision free when check with stateValidator. The following check can be added to exampleHelperGenerateMazeDataset to shorten only the paths that are valid.
% pathObj path to be shortened
% stateValidator validator used to check whether path is valid or not
isPathValid = true;
for i=1:pathObj.NumStates-1
isvalid = stateValidator.isMotionValid(pathObj.States(i,:), pathObj.States(i+1,:));
if ~isvalid
isPathValid = false;
break;
end
end
if isPathValid
shortenedPath = shortenpath(pathObj, stateValidator);
end
