Ejemplo n.º 1
0
    def _bounding_circle(self):
        # NOTE: this method is meant to give a quick bounding circle
        #   the circle calculated may not be the "minimum bounding circle"

        c = self._centroidish()
        r = 0.0
        for v in self.vertexes:
            d = linalg.distance(c, v)
            if d > r: r = d

        return c, r
Ejemplo n.º 2
0
  def _bounding_circle( self ):
    # NOTE: this method is meant to give a quick bounding circle
    #   the circle calculated may not be the "minimum bounding circle"

    c = self._centroidish()
    r = 0.0
    for v in self.vertexes:
      d = linalg.distance( c, v )
      if d > r: r = d

    return c, r
Ejemplo n.º 3
0
    def _draw_rich_traverse_path_to_frame(self):
        # when robot is moving fast, draw small, opaque dots
        # when robot is moving slow, draw large, transparent dots
        d_min, d_max = 0.0, 0.01574  # possible distances between dots
        r_min, r_max = 0.007, 0.02  # dot radius
        a_min, a_max = 0.3, 0.55  # dot alpha value
        m_r = (r_max - r_min) / (d_min - d_max)
        b_r = r_max - m_r * d_min
        m_a = (a_max - a_min) / (r_min - r_max)
        b_a = a_max - m_a * r_min

        prev_posn = self.traverse_path[0]
        frame = self.viewer.current_frame
        for posn in self.traverse_path[1::1]:
            d = linalg.distance(posn, prev_posn)
            r = (m_r * d) + b_r
            a = (m_a * r) + b_a
            frame.add_circle(pos=posn, radius=r, color="black", alpha=a)

            prev_posn = posn
Ejemplo n.º 4
0
  def _draw_rich_traverse_path_to_frame( self ):
    # when robot is moving fast, draw small, opaque dots
    # when robot is moving slow, draw large, transparent dots
    d_min,  d_max = 0.0, 0.01574  # possible distances between dots
    r_min,  r_max = 0.007, 0.02   # dot radius
    a_min,  a_max = 0.3, 0.55     # dot alpha value
    m_r = ( r_max - r_min ) / ( d_min - d_max )
    b_r = r_max - m_r*d_min
    m_a = ( a_max - a_min ) / ( r_min - r_max )
    b_a = a_max - m_a*r_min
    
    prev_posn = self.traverse_path[0]
    frame = self.viewer.current_frame
    for posn in self.traverse_path[1::1]:
      d = linalg.distance( posn, prev_posn )
      r = ( m_r*d ) + b_r
      a = ( m_a*r ) + b_a
      frame.add_circle( pos = posn,
                        radius = r,
                        color = "black",
                        alpha = a)

      prev_posn = posn
Ejemplo n.º 5
0
 def add_new_goal(self, world=None):
     """
     Adds a new goal
     """
     i = 3000
     max_dist = self.cfg["goal"]["max_distance"]
     while i > 0:
         i -= 1
         goal = self.__generate_new_goal()
         intersects = self.__check_obstacle_intersections(goal)
         if not intersects and world is None:
             self.current_goal = goal
             break
         elif not intersects and world is not None:  # add a new goal not far from the robot
             rob_x, rob_y = world.robots[
                 0].supervisor.estimated_pose.vposition()
             distance_to_robot = linalg.distance([rob_x, rob_y], goal)
             if distance_to_robot < 1 and distance_to_robot > 0.5:  # being able to a new goal not far from the robot
                 self.current_goal = goal
                 break
             if rob_x**2 + rob_y**2 > max_dist**2:  # not being able to a new goal near the robot
                 self.current_goal = goal
                 break
Ejemplo n.º 6
0
def check_nearness(geometry1, geometry2):
    c1, r1 = geometry1.bounding_circle
    c2, r2 = geometry2.bounding_circle
    return linalg.distance(c1, c2) <= r1 + r2
Ejemplo n.º 7
0
 def _distance_to_goal(self):
     return linalg.distance(self.supervisor.estimated_pose.vposition(),
                            self.supervisor.goal)
Ejemplo n.º 8
0
 def condition_at_goal(self):
     return linalg.distance(self.supervisor.estimated_pose.vposition(),
                            self.supervisor.goal) < D_STOP
 def condition_at_goal(self):
     return linalg.distance(self.supervisor.estimated_pose.vposition(), self.supervisor.goal) < self.cfg["goal_reached_distance"]
 def _distance_to_goal( self ):
   return linalg.distance( self.supervisor.estimated_pose.vposition(), self.supervisor.goal ) 
 def condition_at_goal( self ):
   return linalg.distance( self.supervisor.estimated_pose.vposition(), self.supervisor.goal ) < D_STOP
Ejemplo n.º 12
0
def check_nearness( geometry1, geometry2 ):
  c1, r1 = geometry1.bounding_circle
  c2, r2 = geometry2.bounding_circle
  return linalg.distance( c1, c2 ) <= r1 + r2