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

1 view (last 30 days)
Özcan Furkan Çetin
Özcan Furkan Çetin on 16 Jan 2023
Answered: Amey Waghmare on 19 Jan 2023
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)

Sign in to answer this question.

Answers (1)

Amey Waghmare
Amey Waghmare on 19 Jan 2023
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.

Categories

Find more on Motion Planning in Help Center and File Exchange

Tags

Community Treasure Hunt

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

Start Hunting!