Error using plannerBiRRT/plan

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

Walter Roberson
Walter Roberson 2023년 12월 11일
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.

댓글을 달려면 로그인하십시오.

답변 (0개)

카테고리

도움말 센터File Exchange에서 Word games에 대해 자세히 알아보기

질문:

2023년 12월 11일

댓글:

2023년 12월 11일

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by