Iterative path collision refinement

I am having a path that is resulted from RRT-connect, then it is pruned using Ramer–Douglas–Peucker. Once we get the path with reduced waypoints we applied cubic B-spline that is a built-in MATLAB function [pathSmooth, qd, qdd, pp] = bsplinepolytraj(cpts,tpts,tvec);

The smoothed path we check wherether it is a collisiono-free or not using the function [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);

Once collsision is detected, we need to to refine the collide path. The refinment function is [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);, and the process will continue while iwant is not empty. The program could not produced a path that is collision-free and goes for infinity.

The issue is: refinment cannot achieved due to some reasons which are not clear to me. Can anyone suggest something to tackl such issue

I have attached the following:

1. Initial path before prunning name as
2. Prunned path named as 'pathReduced.mat'
3. Smoothed path (that is colliding) named as 'smoothedPath.mat'
4. the map that we are working with name as 'environmentMatrix.mat'

The cod that we used asf follow:

%% Path reduction
% reducepoly function Reduce density of points in ROI using Ramer–Douglas–Peucker
% algorithm.
% P_reduced = reducepoly(P,tolerance) reduces the density of points in array
% P, where tolerance specifies how much a point can deviate from a straight
% line.
tolerance = 0.03;
pathReduced = reducepoly(path,tolerance);
% Check the number of row for pathReduced (must be at least 4 rows)
[row, column] = size(pathReduced);
if row < 4
    pathReduced = path;
end

%% Path smoothing sing B-spline smoothing function
% Inputs:
% cpts = pathReduced';
cpts = path';
% Calling Smoothing Funtion
[pathSmooth, pathLengthS, qd, qdd, pp] = BSpline(cpts);

%% Collision detection for the smoothed path and refinment
% Collision detection is performed by discrete samples
[cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
keyboard;
while ~isempty(iwant)
    [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);
    pathReduced = pathReducedRefined;
    cpts = pathReduced';
    % Calling Smoothing Funtion
    [pathSmooth, ComputationTime2, pathLengthS] = BSpline(cpts);
    [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
end

function [cc, iwant, Time, collideIndices] = collisionChecking(map, pathSmooth)
% % FUNCTION COLLISIONCHECKING test for collision occurence.
% %
% % Inputs:
% %   map         - logical map of size 500x500
% %   pathSmooth  - smoothed path in cartesian form produced using
% %                   [pathSmooth, pathLengthS] = BSpline(cpts).
% %
% % Outputs:
% %   cc          - flage of collision occurence: logic 1 represents
% %   collision, while logic 0 represetns collision-free condition
% %

tic;

v = pathSmooth;
env = map;
cc = true;
% Initialize arrays for storing intersections and their indices
iwant = zeros([],2) ;           % To store the coordinates of intersections
collideIndices = zeros([]);   % To store the indices in v that correspond to iwant
count = 0;
% Loop through the path v
for ii = 1:length(v)-1
    % Check if the point in v is in an obstacle-free area (intersection)
    if env(round(v(ii,1)), round(v(ii,2)))  % Use (y,x) indexing for the environment
        cc = false;
        %         disp('There is no intersection');
    else
        count = count + 1;
        
        iwant(count,:) = [round(v(ii,2)), round(v(ii,1))];  % Store the point in iwant
        collideIndices(count) = ii;                         % Store the index from v
        cc = true;
        %         disp('There is intersection');
    end
end

Time = toc;
end

[Walter Roberson]

Your for ii loop is setting cc according to the last item checked. That is probably wrong.

If you want cc to be false if any of the checks are false, then set cc = true before the loop and remove the cc = true line but keep the cc = false line -- that way cc will become false if even one of the checks fails.

If you want cc to be true if any of the checks are true, then set cc = false before the loop and remove the "cc = false| line, and leave the cc = true line -- that way cc will become true if even one of the checks works.