Advertisement
Guest User

Untitled

a guest
Dec 7th, 2019
104
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.75 KB | None | 0 0
  1. def withinPoly(polygons, point):
  2. for poly in polygons:
  3. patch = createPolygonPatch(poly, 'grey')
  4. if(patch.contains_point(point)):
  5. return True
  6. return False
  7.  
  8. def RRT(robot, obstacles, startPoint, goalPoint):
  9.  
  10. points = dict()
  11. tree = dict()
  12. path = []
  13.  
  14. for i in range(0,50):
  15. x = random.uniform(0,10)
  16. y = random.uniform(0,10)
  17. if(withinPoly(obstacles,[x,y])):
  18. print("within polygon ",x, y)
  19. else:
  20. #points.append([x,y])
  21. points[i] = [x,y]
  22. print(len(points))
  23. points, adjListMap = growSimpleRRT(points)
  24. path = basicSearch(adjListMap, 1, 10)
  25. displayRRTandPath(points, adjListMap, path, startPoint, goalPoint, obstacles)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement