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

1 回表示 (過去 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 で Robotics についてさらに検索

タグ

Community Treasure Hunt

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

Start Hunting!

Translated by