fork download
copy 
  1. scenario = robotScenario(UpdateRate=5);
  2. floorColor = [0.5882 0.2941 0];
  3. addMesh(scenario,"Plane",Position=[5 5 0],Size=[10 10],Color=floorColor);
  4. wallHeight = 1;
  5. wallWidth = 0.25;
  6. wallLength = 10;
  7. wallColor = [1 1 0.8157];
  8. % Add outer walls.
  9. addMesh(scenario,"Box",Position=[wallWidth/2, wallLength/2, wallHeight/2], ...
  10.  Size=[wallWidth, wallLength, wallHeight], Color=wallColor, 
  11. IsBinaryOccupied=true);
  12. addMesh(scenario,"Box",Position=[wallLength-wallWidth/2, wallLength/2, 
  13. wallHeight/2], ...
  14.  Size=[wallWidth, wallLength, wallHeight], Color=wallColor, 
  15. IsBinaryOccupied=true);
  16. addMesh(scenario,"Box",Position=[wallLength/2, wallLength-wallWidth/2, 
  17. wallHeight/2], ...
  18.  Size=[wallLength, wallWidth, wallHeight], Color=wallColor, 
  19. IsBinaryOccupied=true);
  20. addMesh(scenario,"Box",Position=[wallLength/2, wallWidth/2, wallHeight/2], ...
  21.  Size=[wallLength, wallWidth, wallHeight], Color=wallColor, 
  22. IsBinaryOccupied=true);
  23. % Add inner walls.
  24. addMesh(scenario,"Box",Position=[wallLength/8, wallLength/3, wallHeight/2],...
  25.  Size=[wallLength/4, wallWidth, 
  26. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  27. addMesh(scenario,"Box",Position=[wallLength/4, wallLength/3, wallHeight/2],...
  28.  Size=[wallWidth, wallLength/6, 
  29. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  30. addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
  31. wallHeight/2],...
  32.  Size=[wallLength/2, wallWidth, 
  33. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  34. addMesh(scenario,"Box",Position=[wallLength/2, wallLength/2, wallHeight/2],...
  35.  Size=[wallWidth, wallLength/3, 
  36. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  37. addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2.5, 
  38. wallHeight/2],...
  39.  Size=[wallLength/3, wallWidth, 
  40. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  41. addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
  42. wallHeight/2],...
  43.  Size=[wallLength/3, wallWidth, 
  44. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  45. addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
  46. wallHeight/3.5],...
  47.  Size=[wallLength/2.5, wallWidth, 
  48. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  49. addMesh(scenario,"Box",Position=[wallLength/6, wallLength/3.5, wallHeight/4],...
  50.  Size=[wallWidth, wallLength/9, 
  51. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  52. addMesh(scenario,"Box",Position=[wallLength/7, wallLength/3, wallHeight/4],...
  53.  Size=[wallWidth, wallLength/9, 
  54. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  55. addMesh(scenario,"Box",Position=[wallLength/4, wallLength/5, wallHeight/7],...
  56.  Size=[wallWidth, wallLength/11, 
  57. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  58. addMesh(scenario,"Box",Position=[wallLength/3, wallLength/2, wallHeight/4],...
  59.  Size=[wallWidth, wallLength/3, 
  60. wallHeight],Color=wallColor,IsBinaryOccupied=true);
  61. show3D(scenario);
  62. lightangle(-45,30);
  63. view(60,50);
  64. map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
  65.  MapSize=[15 15],...
  66.  MapHeightLimits=[0 3]);
  67. inflate(map,0.3);
  68. show(map)
  69. startPosition = [1 1];
  70. goalPosition = [8 8];
  71. rng(100)
  72. numnodes = 2000;
  73. planner = mobileRobotPRM(map,numnodes);
  74. planner.ConnectionDistance = 1;
  75. waypoints = findpath(planner,startPosition,goalPosition);
  76. %obtain binary grid
  77. % Robot height from base.
  78. robotheight = 0.12;
  79. % Number of waypoints.
  80. numWaypoints = size(waypoints,1);
  81. % Robot arrival time at first waypoint.
  82. firstInTime = 0;
  83. % Robot arrival time at last waypoint.
  84. lastInTime = firstInTime + (numWaypoints-1);
  85. % Generate waypoint trajectory with waypoints from planned path.
  86. traj = waypointTrajectory(SampleRate=10,...
  87.  TimeOfArrival=firstInTime:lastInTime, ...
  88.  Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], 
  89. ...
  90.  ReferenceFrame="ENU");
  91. huskyRobot = loadrobot("clearpathHusky");
  92. platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
  93.  BaseTrajectory=traj);
  94. [ax,plotFrames] = show3D(scenario);
  95. lightangle(-45,30)
  96. view(60,50)
  97. hold(ax,"on")
  98. % pp3
  99. plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
  100.  LineWidth=2,...
  101.  MarkerSize=4,...
  102.  MarkerEdgeColor="b",...
  103.  MarkerFaceColor=[0.5 0.5 0.5]);
  104. hold(ax,"off")
  105. setup(scenario)
  106. % Control simulation rate at 20 Hz.
  107. r = rateControl(20);
  108. % Status of robot in simulation.
  109. robotStartMoving = false;
  110. while advance(scenario)
  111.  show3D(scenario,Parent=ax,FastUpdate=true);
  112.  waitfor(r);
  113.  currentPose = read(platform);
  114.  if ~any(isnan(currentPose))
  115.  % implies that robot is in the scene and performing simulation.
  116.  robotStartMoving = true;
  117.  end
  118.  if any(isnan(currentPose)) && robotStartMoving
  119.  % break, once robot reaches goal position.
  120.  break;
  121.  end
  122. en
Success #stdin #stdout 0.03s 25560KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
scenario = robotScenario(UpdateRate=5);
floorColor = [0.5882 0.2941 0];
addMesh(scenario,"Plane",Position=[5 5 0],Size=[10 10],Color=floorColor);
wallHeight = 1;
wallWidth = 0.25;
wallLength = 10;
wallColor = [1 1 0.8157];
% Add outer walls.
addMesh(scenario,"Box",Position=[wallWidth/2, wallLength/2, wallHeight/2], ...
 Size=[wallWidth, wallLength, wallHeight], Color=wallColor, 
IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength-wallWidth/2, wallLength/2, 
wallHeight/2], ...
 Size=[wallWidth, wallLength, wallHeight], Color=wallColor, 
IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength-wallWidth/2, 
wallHeight/2], ...
 Size=[wallLength, wallWidth, wallHeight], Color=wallColor, 
IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallWidth/2, wallHeight/2], ...
 Size=[wallLength, wallWidth, wallHeight], Color=wallColor, 
IsBinaryOccupied=true);
% Add inner walls.
addMesh(scenario,"Box",Position=[wallLength/8, wallLength/3, wallHeight/2],...
 Size=[wallLength/4, wallWidth, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/4, wallLength/3, wallHeight/2],...
 Size=[wallWidth, wallLength/6, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
wallHeight/2],...
 Size=[wallLength/2, wallWidth, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength/2, wallHeight/2],...
 Size=[wallWidth, wallLength/3, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2.5, 
wallHeight/2],...
 Size=[wallLength/3, wallWidth, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
wallHeight/2],...
 Size=[wallLength/3, wallWidth, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, 
wallHeight/3.5],...
 Size=[wallLength/2.5, wallWidth, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/6, wallLength/3.5, wallHeight/4],...
 Size=[wallWidth, wallLength/9, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/7, wallLength/3, wallHeight/4],...
 Size=[wallWidth, wallLength/9, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/4, wallLength/5, wallHeight/7],...
 Size=[wallWidth, wallLength/11, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/3, wallLength/2, wallHeight/4],...
 Size=[wallWidth, wallLength/3, 
wallHeight],Color=wallColor,IsBinaryOccupied=true);
show3D(scenario);
lightangle(-45,30);
view(60,50);
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
 MapSize=[15 15],...
 MapHeightLimits=[0 3]);
inflate(map,0.3);
show(map)
startPosition = [1 1];
goalPosition = [8 8];
rng(100)
numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;
waypoints = findpath(planner,startPosition,goalPosition);
%obtain binary grid
% 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(-45,30)
view(60,50)
hold(ax,"on")
% pp3
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(20);
% 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
en
https://ideone.com/r8Mnbc
language:
PHP (php 7.3.5)
created:
12 days ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!