def __init__(self, x, y, robot, obstacles, start, goal, cspace, display_tree=False): '''Defines a motion planning problem. Args: x: float. the width of the map's area y: float. the height of the map's area robot: a robot.Robot instance obstacles: list of geometry.Objects self.robot can't move through start: tuple of floats: starting configuration of self.robot goal: tuple of floats: goal configuration of self.robot cspace: robot.Configuration space of self.robot display_tree: bool. if True, draw the generated plan trees ''' self.x = x self.y = y self.robot = robot self.obstacles = obstacles self.start = start self.goal = goal self.region = AABB(Point(0, 0), Point(x, y)) self.cspace = cspace self.display_tree = display_tree assert self.valid_configuration(self.start) assert self.valid_configuration(self.goal)
def __init__(self, size, x=0, y=0, vx=0, vy=0, ax=0, ay=0): """Build the component.""" AABB.__init__(self, x, y, size) self.velocity = Vector(vx, vy) self.acceleration = Vector(ax, ay) self.acceleration += _world.gravity self.contact = False
class AABBTestCase(unittest.TestCase): def setUp(self): self.box = AABB((0,0), 3, 4) def test_contain(self): self.assertNotIn((-1,2), self.box) self.assertNotIn((5,2), self.box) self.assertNotIn((2,-1), self.box) self.assertNotIn((2,5), self.box) self.assertIn((2,2), self.box) self.assertIn((0,0), self.box) self.assertIn((2,3), self.box) self.assertNotIn((3,4), self.box) def test_aire(self): self.assertEqual(self.box.aire(), 12)
class Problem: # defines a motion-planning problem def __init__(self, x, y, robot, obstacles, start, goal, cspace, display_tree=False): '''Defines a motion planning problem. Args: x: float. the width of the map's area y: float. the height of the map's area robot: a robot.Robot instance obstacles: list of geometry.Objects self.robot can't move through start: tuple of floats: starting configuration of self.robot goal: tuple of floats: goal configuration of self.robot cspace: robot.Configuration space of self.robot display_tree: bool. if True, draw the generated plan trees ''' self.x = x self.y = y self.robot = robot self.obstacles = obstacles self.start = start self.goal = goal self.region = AABB(Point(0, 0), Point(x, y)) self.cspace = cspace self.display_tree = display_tree assert self.valid_configuration(self.start) assert self.valid_configuration(self.goal) # Generate a regular polygon that does not collide with other # obstacles or the robot at start and goal. def generate_random_regular_poly(self, num_verts, radius, angle=uniform(-pi, pi)): '''Generates a regular polygon that does not collide with other obstacles or the robot at start and goal. This polygon is added to self.obstacles. To make it random, keep the default angle argument. Args: num_verts: int. the number of vertices of the polygon >= 3 radius: float. the distance from the center of the polygon to any vertex > 0 angle: float. the angle in radians between the origin and the first vertex. the default is a random value between -pi and pi. ''' (min_verts, max_verts) = num_verts (min_radius, max_radius) = radius assert not (min_verts < 3 or min_verts > max_verts or \ min_radius <= 0 or min_radius > max_radius) reference = Point(random() * self.x, random() * self.y) distance = uniform(min_radius, max_radius) sides = randint(min_verts, max_verts) obj = Object(reference, [Polygon([Point(distance*cos(angle + 2*n*pi/sides), distance*sin(angle + 2*n*pi/sides)) \ for n in range(sides)])]) if any([obj.collides(current) for current in self.obstacles]) or \ obj.collides(self.robot.configuration(self.start)) or \ obj.collides(self.robot.configuration(self.goal)) or \ not self.region.contains(obj): self.generate_random_regular_poly(num_verts, radius, angle=angle) else: self.obstacles.append(obj) # Generate a polygon that does not collide with other # obstacles or the robot at start and goal. def generate_random_poly(self, num_verts, radius): '''Generates a random polygon that does not collide with other obstacles or the robot at start and goal. This polygon is added to self.obstacles. Args: num_verts: int. the number of vertices of the polygon >= 3 radius: float. a reference distance between the origin and some vertex of the polygon > 0 ''' (min_verts, max_verts) = num_verts (min_radius, max_radius) = radius assert not (min_verts < 3 or min_verts > max_verts or \ min_radius <= 0 or min_radius > max_radius) reference = Point(random() * self.x, random() * self.y) verts = randint(min_verts, max_verts) points = [Point(0, 0)] for i in range(verts): angle = 2 * pi * random() points.append(((max_radius - min_radius) * random() + min_radius) * Point(cos(angle), sin(angle))) obj = Object(reference, [Polygon(convex_hull(points))]) if any([obj.collides(current) for current in self.obstacles]) or \ obj.collides(self.robot.configuration(self.start)) or \ obj.collides(self.robot.configuration(self.goal)) or \ not self.region.contains(obj): self.generate_random_poly(num_verts, radius) else: self.obstacles.append(obj) def valid_configuration(self, configuration): '''Checks if the given configuration is valid in this Problem's configuration space and doesn't collide with any obstacles. Args: configuration: tuple of floats - tuple describing a robot configuration Returns: bool. True if the given configuration is valid, False otherwise ''' return self.cspace.valid_configuration(configuration) \ and not self.collide(configuration) def collide(self, configuration): # Check collision for configuration '''Checks if the given configuration collides with any of this Problem's obstacles. Args: configuration: tuple of floats - tuple describing a robot configuration Returns: bool. True if the given configuration is in collision, False otherwise ''' config_robot = self.robot.configuration(configuration) return any([config_robot.collides(obstacle) \ for obstacle in self.obstacles]) or \ not self.region.contains(config_robot) def test_path(self, start, end): # check path for collisions '''Checks if the path from start to end collides with any obstacles. Args: start: tuple of floats - tuple describing robot's start configuration end: tuple of floats - tuple describing robot's end configuration Returns: bool. False if the path collides with any obstacles, True otherwise ''' path = self.cspace.path(start, end) for configuration in path: if self.collide(configuration): return False return True def safe_path(self, start, end): # returns subset of path that is safe '''Checks if the path from start to end collides with any obstacles. Args: start: tuple of floats - tuple describing robot's start configuration end: tuple of floats - tuple describing robot's end configuration Returns: list of tuples along the path that are not in collision. ''' path = self.cspace.path(start, end) safe_path = [] for configuration in path: if self.collide(configuration): return safe_path safe_path.append(configuration) return safe_path def path_distance(self, path): # add up distances along path '''Adds up the distance along the given path. Args: path: list of tuples describing configurations of the robot Returns: float. the total distance along the path ''' distance = 0 for i in range(len(path) - 1): distance += self.cspace.distance(path[i], path[i + 1]) return distance # Given a path (list of configurations) return a (shorter) path def smooth_path(self, path, attempts=100): # random short-cutting '''Given a path (list of configurations), return a possibly shorter path. Args: path: list of tuples describing configurations of the robot attemps: int. the number of times to try to smooth the path Returns: list of tuples describing configurations of the robot that is possibly shorter than the given path ''' ######################### # Your code here for _ in range(attempts): ind = choice(range(len(path) - 1)) conf = path[ind] target_ind = choice(range(ind + 1, len(path))) target_conf = path[target_ind] if self.test_path(conf, target_conf): path = path[:ind + 1] + path[target_ind:] return path ######################### # Uni-directional RRT. # Returns a collision free path from self.start to self.goal # Sample the goal with goal_sample probability def rrt_planning(self, max_iterations=1000, goal_sample=.05): '''Uni-directional RRT. Tries to find a collision free path from start to goal. Args: max_iterations: int. the maximum number of configurations to sample goal_sample: float. the probability of sampling the goal point Returns: a list of tuples representing a path of collision-free configurations of the robot ''' rrt = RRT(TreeNode(self.start), self.cspace) self.rrts = [rrt] ######################### # Your code here def sample_config(): prob = random() if prob < goal_sample: return self.goal else: return self.cspace.sample() for i in range(max_iterations): sample = sample_config() neighbor = rrt.nearest(sample) path = self.safe_path(neighbor.value, sample) if len(path) > 1: child_node = rrt.add_configuration(neighbor, path[0]) for p in path[1:]: child_node = rrt.add_configuration(child_node, p) # rrt.add_configuration(neighbor, path[-1]) if path[-1] == self.goal: return self.bfs(rrt, self.goal) return None ######################### @staticmethod def construct_path(node): path = [] while node is not None: path.append(node.value) node = node.parent return path[::-1] def bfs(self, rrt, target): queue = [rrt.root] while len(queue) > 0: node = queue.pop(0) if node.value == target: return self.construct_path(node) for child in node.children: queue.append(child) raise Exception("Did not find a path when it is expected") # Bi-directional RRT # Returns a collision free path from self.start to self.goal def bidirectional_rrt_planning(self, max_iterations=1000): '''Bidirectional RRT. Tries to find a collision free path from start to goal. Args: max_iterations: int. the maximum number of configurations to sample Returns: a list of tuples representing a path of collision-free configurations of the robot ''' rrt_start = RRT(TreeNode(self.start), self.cspace) rrt_goal = RRT(TreeNode(self.goal), self.cspace) self.rrts = [rrt_start, rrt_goal] ######################### # Your code here def explore(rrt): sample = self.cspace.sample() neighbor = rrt.nearest(sample) path = self.safe_path(neighbor.value, sample) for conf in path: rrt.add_configuration(neighbor, conf) def connect(rrt, rrt_target): nodes = [] queue = [rrt_target.root] while len(queue) > 0: node = queue.pop(0) nodes.append(node) for child in node.children: queue.append(child) target = choice(nodes).value neighbor = rrt.nearest(target) path = self.safe_path(neighbor.value, target) if len(path) > 1: rrt.add_configuration(neighbor, path[-1]) if path[-1] == target: return self.bfs(rrt_start, target) + self.bfs( rrt_goal, target)[-2::-1] return None for _ in range(max_iterations): if randint(0, 1) == 0: explore(rrt_start) else: explore(rrt_goal) if randint(0, 1) == 0: result = connect(rrt_start, rrt_goal) else: result = connect(rrt_goal, rrt_start) if result is not None: return result return None ######################### def draw_robot_path(self, path, color=None): '''Draws the robot in each configuration along a path in a Tkinter window. Args: path: list of tuples describing configurations of the robot color: string. Tkinter color of the robot ''' for i in range(1, len(path) - 1): # don't draw start and end self.robot.configuration(path[i]).draw(self.window, color) self.window.update() raw_input('Next?') def run_and_display(self, method, display=True): # runs the problem and displays result '''Runs the Problem with the given methods and draws the path in a Tkinter window. Args: method: list of strings of the methods to try, must be "rrt" or "birrt" display: bool. if True, draw the Problem in a Tkinter window Returns: bool. True if a collision free path is found, False otherwise ''' def draw_problem(): for obs in self.obstacles: obs.draw(self.window, 'red') self.robot.configuration(self.start).draw(self.window, 'orange') self.robot.configuration(self.goal).draw(self.window, 'green') if display: self.window = DrawingWindow(600, 600, 0, self.x, 0, self.y, 'RRT Planning') draw_problem() t1 = time() if method == 'rrt': path = self.rrt_planning() elif method == 'birrt': path = self.bidirectional_rrt_planning() else: raise NotImplemented print 'RRT took', time() - t1, 'seconds' if display and self.display_tree: for rrt in self.rrts: rrt.draw(self.window) if path == None: print 'No path found' return False else: print 'Path found with ' + str(len(path)-1) + \ ' movements of distance ', self.path_distance(path) smooth_path = self.smooth_path(path) print 'Smoothed path found with ' + str(len(smooth_path)-1) + \ ' movements of distance ', self.path_distance(smooth_path) # print("start:") # print(self.start) # print("goal:") # print(self.goal) # print("path:") # print(path) # interpolated smooth path spath = [] for i in range(1, len(smooth_path)): spath.extend( self.cspace.path(smooth_path[i - 1], smooth_path[i])) # make sure path is collision free if any([self.collide(c) for c in spath]): print 'Collision in smoothed path' return False if display: print(path) self.draw_robot_path(path, color='yellow') self.window.clear() draw_problem() self.draw_robot_path(spath, color='gold') while raw_input('End? (y or n)') != 'y': pass return True
def __init__(self): """Set config values for world AABB.""" AABB.__init__(self, config['offset'][0], config['offset'][1], config['size']) self.gravity = Vector(*config['gravity'])
def setUp(self): self.box = AABB((0,0), 3, 4)