필터 지우기
필터 지우기

Husky Robot dissappears between two scenario - PRM algorithm - Robot opearing system tool

조회 수: 2 (최근 30일)
Özcan Furkan Çetin
Özcan Furkan Çetin 2023년 1월 16일
답변: Amey Waghmare 2023년 1월 19일
The robot arrives at the 1st target point from the starting point, but instead of continuing to go to the next target points, it disappears for 1-2 seconds and continues by coming again. How can I solve this problem?
scenario = robotScenario(UpdateRate=5);
floorColor = [0.9412 0.9412 0.9412];
addMesh(scenario,"Plane",Position=[4 5 0],Size=[8 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.2;
wallLengthx = 8;
wallLengthy = 10;
wallColor = [0.6510 0.6510 0.6510];
objectColor = [0.1490 0.1490 0.1490];
potColor = [0.1608 0.7608 0.2784];
pottedplantColor = [0.3176 0.6980 0.8588];
% Add outer walls
addMesh(scenario,"Box",Position=[wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx-wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallLengthy-wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
% Add objects
addMesh(scenario,"Box",Position=[1.4, 4.6, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.4, 1.9, 0.5],...
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.8, 3.8, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.9, 5, 0.5],...
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[3.7, 6.9, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
% Add pot
addMesh(scenario,"Box",Position=[4.8, 5.6, 0.5],...
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 8.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[7.2, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
% Add pottedplant
addMesh(scenario,"Box",Position=[3.7, 4, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.7, 7.8, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[1.4, 6.4, 0.5],...
Size=[0.4, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(90,90);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
MapSize=[15 15],...
MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [0.5 1];
goalPosition = [1.5 9];
goal2Position = [3.5 1];
goal3Position = [4.5 9];
goal4Position = [6 1]
finalPosition = [6 9.3];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
waypoints2 = findpath(planner,goalPosition,goal2Position);
waypoints3 = findpath(planner,goal2Position,goal3Position);
waypoints4 = findpath(planner,goal3Position,finalPosition);
% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
huskyRobot = loadrobot("clearpathHusky");
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
BaseTrajectory=traj);
[ax,~] = show3D(scenario);
lightangle(90,90)
view(60,50)
hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")
setup(scenario)
% Control simulation rate at 20 Hz.
r = rateControl(500);
% Status of robot in simulation.
robotStartMoving = false;
while advance(scenario)
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
currentPose = read(platform);
if ~any(isnan(currentPose))
% implies that robot is in the scene and performing simulation.
robotStartMoving = true;
end
if any(isnan(currentPose)) && robotStartMoving
% break, once robot reaches goal position.
break;
end
end
scenario = robotScenario(UpdateRate=5);
floorColor = [0.9412 0.9412 0.9412];
addMesh(scenario,"Plane",Position=[4 5 0],Size=[8 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.2;
wallLengthx = 8;
wallLengthy = 10;
wallColor = [0.6510 0.6510 0.6510];
objectColor = [0.1490 0.1490 0.1490];
potColor = [0.1608 0.7608 0.2784];
pottedplantColor = [0.3176 0.6980 0.8588];
% Add outer walls
addMesh(scenario,"Box",Position=[wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx-wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallLengthy-wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
% Add objects
addMesh(scenario,"Box",Position=[1.4, 4.6, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.4, 1.9, 0.5],...
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.8, 3.8, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.9, 5, 0.5],...
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[3.7, 6.9, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
% Add pot
addMesh(scenario,"Box",Position=[4.8, 5.6, 0.5],...
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 8.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[7.2, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
% Add pottedplant
addMesh(scenario,"Box",Position=[3.7, 4, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.7, 7.8, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[1.4, 6.4, 0.5],...
Size=[0.4, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(90,90);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
MapSize=[15 15],...
MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [1.5 9];
goalPosition = [3.5 1];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
huskyRobot = loadrobot("clearpathHusky");
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
BaseTrajectory=traj);
[ax,~] = show3D(scenario);
lightangle(90,90)
view(60,50)
hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")
setup(scenario)
% Control simulation rate at 20 Hz.
r = rateControl(500);
% Status of robot in simulation.
robotStartMoving = false;
while advance(scenario)
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
currentPose = read(platform);
if ~any(isnan(currentPose))
% implies that robot is in the scene and performing simulation.
robotStartMoving = true;
end
if any(isnan(currentPose)) && robotStartMoving
% break, once robot reaches goal position.
break;
end
end
restart(scenario)
scenario = robotScenario(UpdateRate=5);
floorColor = [0.9412 0.9412 0.9412];
addMesh(scenario,"Plane",Position=[4 5 0],Size=[8 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.2;
wallLengthx = 8;
wallLengthy = 10;
wallColor = [0.6510 0.6510 0.6510];
objectColor = [0.1490 0.1490 0.1490];
potColor = [0.1608 0.7608 0.2784];
pottedplantColor = [0.3176 0.6980 0.8588];
% Add outer walls
addMesh(scenario,"Box",Position=[wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx-wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallLengthy-wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
% Add objects
addMesh(scenario,"Box",Position=[1.4, 4.6, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.4, 1.9, 0.5],...
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.8, 3.8, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.9, 5, 0.5],...
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[3.7, 6.9, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
% Add pot
addMesh(scenario,"Box",Position=[4.8, 5.6, 0.5],...
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 8.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[7.2, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
% Add pottedplant
addMesh(scenario,"Box",Position=[3.7, 4, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.7, 7.8, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[1.4, 6.4, 0.5],...
Size=[0.4, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(90,90);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
MapSize=[15 15],...
MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [3.5 1];
goalPosition = [4.5 9];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
huskyRobot = loadrobot("clearpathHusky");
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
BaseTrajectory=traj);
[ax,~] = show3D(scenario);
lightangle(90,90)
view(60,50)
hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")
setup(scenario)
% Control simulation rate at 20 Hz.
r = rateControl(500);
% Status of robot in simulation.
robotStartMoving = false;
while advance(scenario)
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
currentPose = read(platform);
if ~any(isnan(currentPose))
% implies that robot is in the scene and performing simulation.
robotStartMoving = true;
end
if any(isnan(currentPose)) && robotStartMoving
% break, once robot reaches goal position.
break;
end
end
restart(scenario)
scenario = robotScenario(UpdateRate=5);
floorColor = [0.9412 0.9412 0.9412];
addMesh(scenario,"Plane",Position=[4 5 0],Size=[8 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.2;
wallLengthx = 8;
wallLengthy = 10;
wallColor = [0.6510 0.6510 0.6510];
objectColor = [0.1490 0.1490 0.1490];
potColor = [0.1608 0.7608 0.2784];
pottedplantColor = [0.3176 0.6980 0.8588];
% Add outer walls
addMesh(scenario,"Box",Position=[wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx-wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallLengthy-wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
% Add objects
addMesh(scenario,"Box",Position=[1.4, 4.6, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.4, 1.9, 0.5],...
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.8, 3.8, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.9, 5, 0.5],...
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[3.7, 6.9, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
% Add pot
addMesh(scenario,"Box",Position=[4.8, 5.6, 0.5],...
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 8.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[7.2, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
% Add pottedplant
addMesh(scenario,"Box",Position=[3.7, 4, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.7, 7.8, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[1.4, 6.4, 0.5],...
Size=[0.4, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(90,90);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
MapSize=[15 15],...
MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [4.5 9];
goalPosition = [6 1];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
huskyRobot = loadrobot("clearpathHusky");
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
BaseTrajectory=traj);
[ax,plotFrames] = show3D(scenario);
lightangle(90,90)
view(60,50)
hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")
setup(scenario)
% Control simulation rate at 20 Hz.
r = rateControl(500);
% Status of robot in simulation.
robotStartMoving = false;
while advance(scenario)
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
currentPose = read(platform);
if ~any(isnan(currentPose))
% implies that robot is in the scene and performing simulation.
robotStartMoving = true;
end
if any(isnan(currentPose)) && robotStartMoving
% break, once robot reaches goal position.
break;
end
end
restart(scenario)
scenario = robotScenario(UpdateRate=5);
floorColor = [0.9412 0.9412 0.9412];
addMesh(scenario,"Plane",Position=[4 5 0],Size=[8 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.2;
wallLengthx = 8;
wallLengthy = 10;
wallColor = [0.6510 0.6510 0.6510];
objectColor = [0.1490 0.1490 0.1490];
potColor = [0.1608 0.7608 0.2784];
pottedplantColor = [0.3176 0.6980 0.8588];
% Add outer walls
addMesh(scenario,"Box",Position=[wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx-wallWidth/2, wallLengthy/2, wallHeight/2],...
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallLengthy-wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLengthx/2, wallWidth/2, wallHeight/2],...
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
% Add objects
addMesh(scenario,"Box",Position=[1.4, 4.6, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.4, 1.9, 0.5],...
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[5.8, 3.8, 0.5],...
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.9, 5, 0.5],...
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[3.7, 6.9, 0.5],...
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
% Add pot
addMesh(scenario,"Box",Position=[4.8, 5.6, 0.5],...
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[2.7, 8.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[7.2, 2.5, 0.5],...
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
% Add pottedplant
addMesh(scenario,"Box",Position=[3.7, 4, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[6.7, 7.8, 0.5],...
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[1.4, 6.4, 0.5],...
Size=[0.4, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(90,90);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
MapSize=[15 15],...
MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [6 1];
goalPosition = [6 9.3];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
huskyRobot = loadrobot("clearpathHusky");
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
BaseTrajectory=traj);
[ax,plotFrames] = show3D(scenario);
lightangle(90,90)
view(60,50)
hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")
setup(scenario)
% Control simulation rate at 20 Hz.
r = rateControl(500);
% Status of robot in simulation.
robotStartMoving = false;
while advance(scenario)
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
currentPose = read(platform);
if ~any(isnan(currentPose))
% implies that robot is in the scene and performing simulation.
robotStartMoving = true;
end
if any(isnan(currentPose)) && robotStartMoving
% break, once robot reaches goal position.
break;
end
end
restart(scenario)

이 질문에 답변하려면 로그인하십시오.

답변 (1개)

Amey Waghmare
Amey Waghmare 2023년 1월 19일
Hi,
As per my understanding, the Husky Robot disappears when it reaches an intermediate goal point, reappears after few seconds and continues to go to next goal point. The disappearance of the robot is undesired and happens because creating the ‘robotScenario’ for each intermediate goal point trajectories.
In order to resolve the issue, consider creating a single ‘robotScenario’ and concatenating all the waypoints as follows,
startPosition = [0.5 1];
goalPosition = [1.5 9];
goal2Position = [3.5 1];
goal3Position = [4.5 9];
goal4Position = [6 1];
finalPosition = [6 9.3];
waypoints = findpath(planner,startPosition,goalPosition);
waypoints2 = findpath(planner,goalPosition,goal2Position);
waypoints3 = findpath(planner,goal2Position,goal3Position);
waypoints4 = findpath(planner,goal3Position,goal4Position);
waypoints5 = findpath(planner,goal4Position,finalPosition);
all_waypoints = [waypoints; waypoints2; waypoints3; waypoints4; waypoints5];
Now, to generate waypoint trajectory, pass ‘all_waypoints’ to the ‘waypointTrajectory’ function,
traj = waypointTrajectory(SampleRate=10,...
TimeOfArrival=firstInTime:lastInTime, ...
Waypoints=[all_waypoints, robotheight*ones(numWaypoints,1)], ...
ReferenceFrame="ENU");
To view all the waypoints during simulation, pass ‘all_waypoints’ to the plot function,
plot(ax,all_waypoints(:,1),all_waypoints(:,2),"-ms",...
LineWidth=2,...
MarkerSize=4,...
MarkerEdgeColor="b",...
MarkerFaceColor=[0.5 0.5 0.5]);
You can remove the remaining code where the new ‘robotScenario’ is being created for the intermediate waypoints (from line 172 to end). In this case, the robot will continue to go to next intermediate goal point without disappearing in between the simulation.
I hope this resolves the issue.

카테고리

Help Center 및 File Exchange에서 Motion Planning에 대해 자세히 알아보기

태그

Community Treasure Hunt

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

Start Hunting!

Translated by