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

4 views (last 30 days)
Ö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];
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];
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
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");
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;
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
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];
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];
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
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");
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;
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
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];
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];
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
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");
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;
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
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];
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];
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
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");
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;
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
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];
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];
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallWidth, wallLengthy, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[wallLengthx, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=objectColor,IsBinaryOccupied=true);
Size=[0.4, 0.4, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.6, 1],Color=potColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
Size=[0.6, 0.4, 1],Color=pottedplantColor,IsBinaryOccupied=true);
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");
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;
show3D(scenario,Parent=ax,FastUpdate=true);
waitfor(r);
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)

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.