This topic has been permanently closed and transferred to MATLAB Answers.

General
Follow


Kittiwith

Error using plannerBiRRT/plan

Kittiwith on 11 Dec 2023 (Edited on 12 Dec 2023)
Latest activity Reply by Walter Roberson on 11 Dec 2023

This is my code
%send food and medicine in flood situation
% 1. create map
clc
clear
tic
map3D = occupancyMap3D; % create space for
[xGround,yGround,zGround] = meshgrid(0:240,0:350, 0); %set floor x=500 y=500
xyzGround = [xGround(:) yGround(:) zGround(:)]; %use for mergegrid
occval = 0;
setOccupancy(map3D,xyzGround,occval)
% xspace = 20 y = 45 create
[xB1, yB1, zB1] = meshgrid(50:130, 60:110, 0:20); %unionmall
[xB2, yB2, zB2] = meshgrid(80:250, 160:240, 0:30); %centralladprao
[xB3, yB3, zB3] = meshgrid(220:250, 290:340, 0:60); %centarahotel
[xB4, yB4, zB4] = meshgrid(0:30, 10:40, 0:80); %condo
[xB5, yB5, zB5] = meshgrid(150:170, 80:100, 0:10); %house1
[xB6, yB6, zB6] = meshgrid(180:200, 80:100, 0:10); %house2
[xB7, yB7, zB7] = meshgrid(210:230, 80:100, 0:10); %house3
[xB11, yB11, zB11] = meshgrid(240:270, 80:100, 0:10); %house7
[xB8, yB8, zB8] = meshgrid(150:170, 50:70, 0:10); %house4
[xB9, yB9, zB9] = meshgrid(180:200, 50:70, 0:10); %house5
[xB10, yB10, zB10] = meshgrid(210:230, 50:70, 0:10); %house6
[xB12, yB12, zB12] = meshgrid(240:270, 50:70, 0:10); %house8
[xB13, yB13, zB13] = meshgrid(200:260, 0:30, 0:13); %villageoffice
xyzB = [xB1(:), yB1(:), zB1(:);
xB2(:), yB2(:), zB2(:);
xB3(:), yB3(:), zB3(:);
xB4(:), yB4(:), zB4(:);
xB5(:), yB5(:), zB5(:);
xB6(:), yB6(:), zB6(:);
xB7(:), yB7(:), zB7(:);
xB8(:), yB8(:), zB8(:);
xB9(:), yB9(:), zB9(:);
xB10(:), yB10(:), zB10(:);
xB11(:), yB11(:), zB11(:);
xB12(:), yB12(:), zB12(:);
xB13(:), yB13(:), zB13(:);];
obs = 0.65; % ;=not show in command window
my3Dmap = map3D;
updateOccupancy(my3Dmap,xyzB,obs);
%show(my3Dmap);
% 2. distination
%my3Dmap.FreeThreshold = my3Dmap.OccupiedThreshold;
startPose = [50 60 25 pi/2;
250 310 65 pi/2;
100 170 35 pi/2;
100 30 85 pi/2;
155 90 15 pi/2;
250 60 15 pi/2;
160 60 15 pi/2
220 15 20 pi/2;];
goalPose = startPose;
ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/3,...
"AirSpeed",5,...
"FlightPathAngleLimit",[-0.5 0.5],...
"Bounds",[-10 400;-10 4000; -10 100; -pi pi]);
%threshold = [(goalPose-0.5)' (goalPose+0.5)'; -pi pi];
%setWorkspaceGoalRegion(ss,goalPose,threshold)
sv = validatorOccupancyMap3D(ss,"Map",my3Dmap);
sv.ValidationDistance = 0.5;
planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 100;
%planner.GoalBias = 0.10;
%1,0.1,0.15,0.5,0.2,0.13,
planner.MaxIterations = 100000;
%planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3)) < 5);
planner.MaxNumTreeNodes = 10000;
numPose = length(startPose);
distances = zeros(numPose, numPose);
for i = 1:numPose
for j = 1:numPose
if i ~= j
rng(100, "twister");
start = [startPose(i,1), startPose(i,2), startPose(i,3), startPose(i,4)];
goal = [goalPose(j,1), goalPose(j,2), goalPose(j,3), goalPose(j,4)];
[pthObj, solnInfo] = plan(planner, start, goal);
smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss, sv, pthObj);
distances(i, j) = pathLength(smoothWaypointsObj);
end
end
end
toc
and the error below

1 Comment

Walter Roberson
Walter Roberson on 11 Dec 2023
The start location of [50 60 25] does not appear to be associated with any of the locations. For example it is not associated with unionmall because unionmall ends at 20.

See Also

Tags