def _detect_collisions( self ): colliders = self.world.colliders() solids = self.world.solids() for collider in colliders: polygon1 = collider.global_geometry # polygon1 for solid in solids: if solid is not collider: # don't test an object against itself polygon2 = solid.global_geometry # polygon2 if geometrics.check_nearness( polygon1, polygon2 ): # don't bother testing objects that are not near each other if geometrics.convex_polygon_intersect_test( polygon1, polygon2 ): raise CollisionException()
def __generate_rectangle_obstacles(self, world): """ Generate random rectangle obstacles :param world: The world for which they are generated :return: List of generated rectangle obstacles """ obs_min_dim = self.cfg["obstacle"]["rectangle"]["min_dim"] obs_max_dim = self.cfg["obstacle"]["rectangle"]["max_dim"] obs_max_combined_dim = self.cfg["obstacle"]["rectangle"][ "max_combined_dim"] obs_min_count = self.cfg["obstacle"]["rectangle"]["min_count"] obs_max_count = self.cfg["obstacle"]["rectangle"]["max_count"] obs_min_dist = self.cfg["obstacle"]["rectangle"]["min_distance"] obs_max_dist = self.cfg["obstacle"]["rectangle"]["max_distance"] # generate the obstacles obstacles = [] obs_dim_range = obs_max_dim - obs_min_dim obs_dist_range = obs_max_dist - obs_min_dist num_obstacles = randrange(obs_min_count, obs_max_count + 1) test_geometries = [r.global_geometry for r in world.robots] while len(obstacles) < num_obstacles: # generate dimensions width = obs_min_dim + (random() * obs_dim_range) height = obs_min_dim + (random() * obs_dim_range) while width + height > obs_max_combined_dim: height = obs_min_dim + (random() * obs_dim_range) # generate position dist = obs_min_dist + (random() * obs_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) # generate orientation theta = -pi + (random() * 2 * pi) # test if the obstacle overlaps the robots or the goal obstacle = RectangleObstacle(width, height, Pose(x, y, theta)) intersects = False for test_geometry in test_geometries: intersects |= geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry) if not intersects: obstacles.append(obstacle) return obstacles
def _detect_collisions(self): """ Checks the world for any collisions between colliding objects (robots) and solid objects (obstacles) Raises a CollisionException if a collision is detected """ colliders = self.world.colliders() solids = self.world.solids() for collider in colliders: polygon1 = collider.global_geometry # polygon1 for solid in solids: if solid is not collider: # don't test an object against itself polygon2 = solid.global_geometry # polygon2 if geometrics.check_nearness(polygon1, polygon2): # don't bother testing objects that are not near each other if geometrics.convex_polygon_intersect_test(polygon1, polygon2): raise CollisionException()
def __generate_random_features(self, world): """ Generate random features represented by octagon obstacles :param world: The world for which they are generated :return: List of generated features """ obs_radius = self.cfg["obstacle"]["feature"]["radius"] obs_min_count = self.cfg["obstacle"]["feature"]["min_count"] obs_max_count = self.cfg["obstacle"]["feature"]["max_count"] obs_min_dist = self.cfg["obstacle"]["feature"]["min_distance"] obs_max_dist = self.cfg["obstacle"]["feature"]["max_distance"] # generate the obstacles obstacles = [] obs_dist_range = obs_max_dist - obs_min_dist num_obstacles = randrange(obs_min_count, obs_max_count + 1) test_geometries = [r.global_geometry for r in world.robots] while len(obstacles) < num_obstacles: # generate position dist = obs_min_dist + (random() * obs_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) # generate orientation theta = -pi + (random() * 2 * pi) # test if the obstacle overlaps the robots or the goal obstacle = FeaturePoint(obs_radius, Pose(x, y, theta), 0) intersects = False for test_geometry in test_geometries: intersects |= geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry) if not intersects: obstacles.append(obstacle) for i, feature in enumerate(obstacles): feature.id = i return obstacles
def __check_obstacle_intersections(self, goal): """ Check for intersections between the goal and the obstacles :param goal: The goal position :return: Boolean value indicating if the goal is too close to an obstacle """ # generate a proximity test geometry for the goal min_clearance = self.cfg["goal"]["min_clearance"] n = 6 # goal is n sided polygon goal_test_geometry = [] for i in range(n): goal_test_geometry.append([ goal[0] + min_clearance * cos(i * 2 * pi / n), goal[1] + min_clearance * sin(i * 2 * pi / n) ]) goal_test_geometry = Polygon(goal_test_geometry) intersects = False for obstacle in self.current_obstacles: intersects |= geometrics.convex_polygon_intersect_test( goal_test_geometry, obstacle.global_geometry) return intersects
def random_map(self, world): # OBSTACLE PARAMS obs_min_dim = OBS_MIN_DIM obs_max_dim = OBS_MAX_DIM obs_max_combined_dim = OBS_MAX_COMBINED_DIM obs_min_count = OBS_MIN_COUNT obs_max_count = OBS_MAX_COUNT obs_min_dist = OBS_MIN_DIST obs_max_dist = OBS_MAX_DIST # GOAL PARAMS goal_min_dist = GOAL_MIN_DIST goal_max_dist = GOAL_MAX_DIST # BUILD RANDOM ELEMENTS # generate the goal goal_dist_range = goal_max_dist - goal_min_dist dist = goal_min_dist + (random() * goal_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) goal = [x, y] # generate a proximity test geometry for the goal r = MIN_GOAL_CLEARANCE n = 6 goal_test_geometry = [] for i in range(n): goal_test_geometry.append( [x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)]) goal_test_geometry = Polygon(goal_test_geometry) # generate the obstacles obstacles = [] obs_dim_range = obs_max_dim - obs_min_dim obs_dist_range = obs_max_dist - obs_min_dist num_obstacles = randrange(obs_min_count, obs_max_count + 1) test_geometries = [r.global_geometry for r in world.robots] + [goal_test_geometry] while len(obstacles) < num_obstacles: # generate dimensions width = obs_min_dim + (random() * obs_dim_range) height = obs_min_dim + (random() * obs_dim_range) while width + height > obs_max_combined_dim: height = obs_min_dim + (random() * obs_dim_range) # generate position dist = obs_min_dist + (random() * obs_dist_range) phi = -pi + (random() * 2 * pi) x = dist * sin(phi) y = dist * cos(phi) # generate orientation theta = -pi + (random() * 2 * pi) # test if the obstacle overlaps the robots or the goal obstacle = RectangleObstacle(width, height, Pose(x, y, theta)) intersects = False for test_geometry in test_geometries: intersects |= geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry) if not intersects: obstacles.append(obstacle) # update the current obstacles and goal self.current_obstacles = obstacles self.current_goal = goal # apply the new obstacles and goal to the world self.apply_to_world(world)
def random_map( self, world ): # OBSTACLE PARAMS obs_min_dim = OBS_MIN_DIM obs_max_dim = OBS_MAX_DIM obs_max_combined_dim = OBS_MAX_COMBINED_DIM obs_min_count = OBS_MIN_COUNT obs_max_count = OBS_MAX_COUNT obs_min_dist = OBS_MIN_DIST obs_max_dist = OBS_MAX_DIST # GOAL PARAMS goal_min_dist = GOAL_MIN_DIST goal_max_dist = GOAL_MAX_DIST # BUILD RANDOM ELEMENTS # generate the goal goal_dist_range = goal_max_dist - goal_min_dist dist = goal_min_dist + ( random() * goal_dist_range ) phi = -pi + ( random() * 2 * pi ) x = dist * sin( phi ) y = dist * cos( phi ) goal = [ x, y ] # generate a proximity test geometry for the goal r = MIN_GOAL_CLEARANCE n = 6 goal_test_geometry = [] for i in range( n ): goal_test_geometry.append( [ x + r*cos( i * 2*pi/n ), y + r*sin( i * 2*pi/n ) ] ) goal_test_geometry = Polygon( goal_test_geometry ) # generate the obstacles obstacles = [] obs_dim_range = obs_max_dim - obs_min_dim obs_dist_range = obs_max_dist - obs_min_dist num_obstacles = randrange( obs_min_count, obs_max_count+1 ) test_geometries = [ r.global_geometry for r in world.robots ] + [ goal_test_geometry ] while len( obstacles ) < num_obstacles: # generate dimensions width = obs_min_dim + ( random() * obs_dim_range ) height = obs_min_dim + ( random() * obs_dim_range ) while width + height > obs_max_combined_dim: height = obs_min_dim + ( random() * obs_dim_range ) # generate position dist = obs_min_dist + ( random() * obs_dist_range ) phi = -pi + ( random() * 2 * pi ) x = dist * sin( phi ) y = dist * cos( phi ) # generate orientation theta = -pi + ( random() * 2 * pi ) # test if the obstacle overlaps the robots or the goal obstacle = RectangleObstacle( width, height, Pose( x, y, theta ) ) intersects = False for test_geometry in test_geometries: intersects |= geometrics.convex_polygon_intersect_test( test_geometry, obstacle.global_geometry ) if intersects == False: obstacles.append( obstacle ) # update the current obstacles and goal self.current_obstacles = obstacles self.current_goal = goal # apply the new obstacles and goal to the world self.apply_to_world( world )