Пример #1
0
  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()
Пример #2
0
    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
Пример #3
0
    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()
Пример #4
0
    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
Пример #5
0
 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
Пример #6
0
    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)
Пример #7
0
  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 )