Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def withinPoly(polygons, point):
- for poly in polygons:
- patch = createPolygonPatch(poly, 'grey')
- if(patch.contains_point(point)):
- return True
- return False
- def RRT(robot, obstacles, startPoint, goalPoint):
- points = dict()
- tree = dict()
- path = []
- for i in range(0,50):
- x = random.uniform(0,10)
- y = random.uniform(0,10)
- if(withinPoly(obstacles,[x,y])):
- print("within polygon ",x, y)
- else:
- #points.append([x,y])
- points[i] = [x,y]
- print(len(points))
- points, adjListMap = growSimpleRRT(points)
- path = basicSearch(adjListMap, 1, 10)
- displayRRTandPath(points, adjListMap, path, startPoint, goalPoint, obstacles)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement