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
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
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
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
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
def check_nearness(geometry1, geometry2): c1, r1 = geometry1.bounding_circle c2, r2 = geometry2.bounding_circle return linalg.distance(c1, c2) <= r1 + r2
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
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
def check_nearness( geometry1, geometry2 ): c1, r1 = geometry1.bounding_circle c2, r2 = geometry2.bounding_circle return linalg.distance( c1, c2 ) <= r1 + r2