def move_right(self, e) -> None: self.reset() if self._services.algorithm.map.size.n_dim == 3: p = Point(e.position.x + self.speed, e.position.y, e.position.z) else: p = Point(e.position.x + self.speed, e.position.y) self.move_valid_entity(e, p)
def cast_lines_on_screen(lines, width, height, distance_to_screen, clipping_distance): # clipping plane plane_point1 = Point(20, 2, clipping_distance) plane_point2 = Point(1, 1, clipping_distance) plane_point3 = Point(2, 30, clipping_distance) plane_points = [plane_point1, plane_point2, plane_point3] screen_lines = [] for line in lines: line_to_draw = line # skip lines that are behind the clipping plane if (line.start.z <= clipping_distance and line.end.z <= clipping_distance): continue # clip lines that go through the clipping plane if needed line_to_draw = clip_line(line, plane_points) # cast points = [] for point in line_to_draw.get_points(): s_point = cast_point_on_screen(point, width, height, distance_to_screen) points.append(s_point) screen_lines.append(Line(points[0], points[1], line.color)) return screen_lines
def init_direction_vectors(self): """ This function generates the neighbouring coordinates using the cartesian product. We must remove the first coordinate as this represents all zeros. Example Input: n dimensions = 2: Output: [(1, 0), (-1, 0), (1, 1), ...] """ self.ALL_POINTS_MOVE_VECTOR: List[Point] = \ list(map(lambda x: Point(*x), product(*[(0, -1, 1) for i in range(self.size.n_dim)]) ))[1:] """ This does the same as the above without generating diagonal coordinates. First we generate the positive direct neighbouring coordinates. We concatenate this to the negative coordinates and convert to a list of Points. """ POSITIVE_DIRECT_POINTS_DIMENSIONS: List[List[int]] = [[ 1 if i == pos else 0 for i in range(self.size.n_dim) ] for pos in range(self.size.n_dim)] ALL_DIRECT_POINTS_DIMENSIONS: List[List[int]] = POSITIVE_DIRECT_POINTS_DIMENSIONS + \ list(map(lambda x: [-i for i in x], POSITIVE_DIRECT_POINTS_DIMENSIONS)) self.DIRECT_POINTS_MOVE_VECTOR: List[Point] = \ list(map(lambda x: Point(*x), ALL_DIRECT_POINTS_DIMENSIONS))
def script_classic_trajectory(var=10): """ Script utilisant le filtre de Kalman avec des mesures simulées à partir d'une trajectoire inventée ! :return: """ print("script_classic_trajectory") l_points = [[-1000., 200.], [-1000., 800.], [-400., 1200.], [500., 500.], [1100., 180.]] dt = 0.025 real_path = generateur_chemin.generate_path(l_points=l_points, velocity_translation=25, velocity_rotation=0.7, dt=dt) measures_pos = np.array(real_path) real_path_point = [] for couple in real_path: x, y = couple pos = Point(x, y) real_path_point.append(pos) l_pos_filtre = [real_path_point[0]] l_pos_measured = [real_path_point[0]] vite = get_velocity(measures_pos, dt) measures = np.concatenate((measures_pos, vite), axis=1) measures = np.asmatrix(measures) filtering = FiltrageKalman(measures[0, :].T, dt=dt) for i in range(1, measures.shape[0]): x = measures[i, 0] y = measures[i, 1] # print "x et y", x, y, "i" ,i x_bruite, y_bruite = x + np.random.randn()*var, y + np.random.randn()*var filtering.update(x_bruite, y_bruite) pos = filtering.get_current_position() l_pos_filtre.append(pos) l_pos_measured.append(Point(x_bruite, y_bruite)) print(get_mer(real_path_point, l_pos_filtre)) print(get_mer(real_path_point, l_pos_measured)) return real_path_point, l_pos_filtre, l_pos_measured
def test_ne_instance(self) -> None: map1: SparseMap = SparseMap(Size(200, 200), Agent(Point(20, 20), 10), [Obstacle(Point(40, 40), 10), Obstacle(Point(100, 100), 40)], Goal(Point(180, 160), 10)) map2: int = 2 self.assertNotEqual(map1, map2)
def __get_rand_position(self, dimensions: Size, start: Optional[Point] = None) -> Point: if start is None: start = Point(*([0] * dimensions.n_dim)) return Point(*np.random.randint([*start], [*dimensions]))
def generateRandomLineSegments(n, min_x, max_x, min_y, max_y): segments = [] s = set() while n > 0: x1 = randint(min_x, max_x) x2 = randint(min_x, max_x) y1 = randint(min_y, max_y) y2 = randint(min_y, max_y) if x1 == x2: continue if x1 in s or x2 in s: continue if x1 > x2: segments.append(Segment(Point(x2, y2), Point(x1, y1))) else: segments.append(Segment(Point(x1, y1), Point(x2, y2))) s.add(x1) s.add(x2) n -= 1 return segments
def __subdivide(top_left_corner, dim): if dim.width < 2 * min_room_size.width - 1 and dim.height < 2 * min_room_size.height - 1: __place_room(grid, dim, top_left_corner) return elif dim.width < 2 * min_room_size.width - 1: is_vertical_split = False elif dim.height < 2 * min_room_size.height - 1: is_vertical_split = True else: is_vertical_split = int(torch.randint(0, 2, (1,)).item()) == 0 if dim.width <= max_room_size.width and dim.height <= max_room_size.height: if int(torch.randint(0, 2, (1,)).item()) == 0: __place_room(grid, dim, top_left_corner) return # split new_top_left_corner1 = top_left_corner if is_vertical_split: new_dim1 = Size(__get_subdivision_number(dim, True), dim.height) new_top_left_corner2 = Point(top_left_corner.x + new_dim1.width - 1, top_left_corner.y) new_dim2 = Size(dim.width - new_dim1.width + 1, dim.height) else: new_dim1 = Size(dim.width, __get_subdivision_number(dim, False)) new_top_left_corner2 = Point(top_left_corner.x, top_left_corner.y + new_dim1.height - 1) new_dim2 = Size(dim.width, dim.height - new_dim1.height + 1) __subdivide(new_top_left_corner1, new_dim1) __subdivide(new_top_left_corner2, new_dim2)
def _find_path_internal(self) -> None: """ Read super description The internal implementation of :ref:`find_path` """ #._get_grid() is in Algorithm class and gets the map grid: Map = self._get_grid() trace = main(grid) if trace == None or trace == 0: print('No path found') pass else: # for point in trace: # point.numpy() print("final path --------", trace) for point in trace[1:-1]: self.waypoint.add(Point(point[0] + 0.25, point[1] - 0.25)) self.waypoint.add(Point(point[0] + 0.25, point[1] + 0.25)) self.waypoint.add(Point(point[0] - 0.25, point[1] + 0.25)) self.waypoint.add(Point(point[0] - 0.25, point[1] - 0.25)) print('waypoint =====', self.waypoint) if type(trace) != bool: print( '______________________________________________________________' ) print('trace ======', trace) for point in trace: self.move_agent(Point(int(point[0]), int(point[1]))) self.key_frame(ignore_key_frame_skip=True)
def test_is_valid_position_invalid(self) -> None: map1: SparseMap = DenseMap([ [DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]).convert_to_sparse_map() self.assertFalse(map1.is_agent_valid_pos(Point(1, 0))) self.assertFalse(map1.is_agent_valid_pos(Point(-1, -1)))
def counterclockwise(self, p1, p2, p3): """ Check whether a turn from (p1, p2) to (p1, p3) is counter clockwise. If it is so, the return value is > 0, if the turn is clockwise, the return value is < 0. If vectors are collinear, the return value is 0. """ return self.cross(Point(p2.x - p1.x, p2.y - p1.y), Point(p3.x - p1.x, p3.y - p1.y))
def cast_point_on_screen(point, width, height, distance_to_screen): try: x = point.x * distance_to_screen / point.z + width / 2 y = point.y * distance_to_screen / point.z + height / 2 return Point(x, y, 1) except Exception as e: print(e, flush=True) return Point(width / 2, height / 2, 1)
def _get_new_vertex(self, q_near: Vertex, q_sample: Point, max_dist) -> Vertex: dir = q_sample.to_tensor() - q_near.position.to_tensor() if torch.norm(dir) <= max_dist: return Vertex(q_sample) dir_normalized = dir / torch.norm(dir) q_new = Point.from_tensor(q_near.position.to_tensor() + max_dist * dir_normalized) return Vertex(q_new)
def test_is_valid_position_normal(self) -> None: map1: DenseMap = DenseMap([ [DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]) self.assertTrue(map1.is_agent_valid_pos(Point(1, 1))) self.assertTrue(map1.is_agent_valid_pos(Point(0, 1))) self.assertTrue(map1.is_agent_valid_pos(Point(3, 2)))
def test_move_agent_out_of_bounds(self) -> None: map1: DenseMap = DenseMap([ [DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]) map1.move_agent(Point(-1, 0)) self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(0, 1))) self.assertEqual([Trace(Point(0, 1))], map1.trace)
def update(self, x, y): if self.acceleration_filtering(Point(x, y)): self.last_point = Point(x, y) self.kalman_filter.predict() self.kalman_filter.measure(np.array([x, y])[:, np.newaxis]) self.history.append(self.get_last_state()) else: self.last_point = None self.kalman_filter.predict()
def __init__(self, glyph): points = [Point(line.x, line.y) for line in glyph.lines] if glyph.orientation == HORIZONTAL: points.extend([Point(line.x + line.l - 1, line.y) for line in glyph.lines]) else: points.extend([Point(line.x, line.y + line.l - 1) for line in glyph.lines]) self.points = ConvexHull(points).points
def test_move_agent_out_of_bounds(self) -> None: map1: SparseMap = DenseMap([ [DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]).convert_to_sparse_map() map1.move_agent(Point(-1, 0)) self.assertEqual(Point(0, 1), map1.agent.position) self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_move_agent_no_trace(self) -> None: map1: SparseMap = DenseMap([ [DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]).convert_to_sparse_map() map1.move_agent(Point(1, 1), True) self.assertEqual(Point(1, 1), map1.agent.position) self.assertEqual([], map1.trace)
def test_convert_to_dense_map(self) -> None: map1: SparseMap = SparseMap( Size(4, 3), Agent(Point(0, 1)), [Obstacle(Point(0, 0)), Obstacle(Point(1, 0)), Obstacle(Point(2, 0)), Obstacle(Point(3, 0))], Goal(Point(3, 2)) ) map2: DenseMap = map1.convert_to_dense_map() self.assertEqual(map1, map2)
def test_move_agent_normal(self) -> None: map1: DenseMap = DenseMap( [[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]], [[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID]]]).convert_to_sparse_map() map1.move_agent(Point(0, 0, 0)) self.assertEqual(Point(0, 0, 0), map1.agent.position) self.assertTrue([Trace(Point(0, 0, 0))], map1.trace)
def test_is_valid_position_normal(self) -> None: map1: SparseMap = DenseMap([ [DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]).convert_to_sparse_map() self.assertTrue(map1.is_agent_valid_pos(Point(1, 1))) self.assertTrue(map1.is_agent_valid_pos(Point(0, 1))) self.assertTrue(map1.is_agent_valid_pos(Point(3, 2))) self.assertTrue(map1.is_agent_valid_pos(Point(0, 0)))
def test_move_agent_normal(self) -> None: map1: DenseMap = DenseMap([ [DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID], ]) map1.move_agent(Point(1, 1)) self.assertEqual(Point(1, 1), map1.agent.position) self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(1, 1))) self.assertTrue([Trace(Point(1, 1))], map1.trace)
def test_eq_sparse_map(self) -> None: map1: DenseMap = DenseMap( [[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.CLEAR_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]], [[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID], [DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]]) map2: SparseMap = SparseMap(Size(3, 2, 2), Agent(Point( 0, 1, 0)), [Obstacle(Point(0, 0, 0)), Obstacle(Point(1, 0, 0))], Goal(Point(0, 1, 1))) self.assertEqual(map1, map2)
def __render_arc(self, p1: Point, p2: Point, colour: Colour): center = p1 radius = 1 dir = p2.to_tensor() - p1.to_tensor() angle = torch.atan2(dir[1], dir[0]) angle %= 2 * np.pi self.get_renderer_view().draw_arc(center, angle, radius=radius, colour=colour)
def get_measures_from_state(self, x, y): """ (x,y) sont les coordonnées du robot """ point = Point(x, y) d1 = point.distance(self.beacon1) d2 = point.distance(self.beacon2) d3 = point.distance(self.beacon3) # d1 = sqrt((L/2.-x)**2+(l/2.-y)**2) # d2 = sqrt((-L/2.-x)**2+(l-y)**2) # d3 = sqrt((-x-L/2.)**2+y**2) return d1-d2, d1-d3, d2-d3
def test_move_agent_no_trace(self) -> None: map1: DenseMap = DenseMap([ [[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.EXTENDED_WALL_ID]], [[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]] ]) map1.move_agent(Point(0, 1, 1), True) self.assertEqual(Point(0, 1, 1), map1.agent.position) self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 1]) self.assertEqual([], map1.trace)
def get_measures_from_state(self, x, y): """ (x,y) sont les coordonnées du robot """ point = Point(x, y) d1 = point.distance(self.beacon1) d2 = point.distance(self.beacon2) d3 = point.distance(self.beacon3) # d1 = sqrt((L/2.-x)**2+(l/2.-y)**2) # d2 = sqrt((-L/2.-x)**2+(l-y)**2) # d3 = sqrt((-x-L/2.)**2+y**2) return d1 - d2, d1 - d3, d2 - d3
def test_move_agent_out_of_bounds(self) -> None: map1: DenseMap = DenseMap([ [[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.EXTENDED_WALL_ID]], [[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID], [DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]] ]) map1.move_agent(Point(-1, 0, 0)) self.assertEqual(Point(0, 1, 0), map1.agent.position) self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 0]) self.assertEqual([Trace(Point(0, 1, 0))], map1.trace)
def _setup_sim(self, config: Optional[Configuration] = None, goal: Optional[Point] = None) -> Simulator: """ Sets up the simulator (e.g. algorithm and map configuration). """ while self.grid is None or self.agent is None: rospy.loginfo("Waiting for grid and agent to initialise...") rospy.sleep(0.5) if config is None: config = Configuration() # general config.simulator_graphics = True config.simulator_key_frame_speed = 0.16 config.simulator_key_frame_skip = 20 config.get_agent_position = lambda: self._world_to_grid( Point(self.agent.pose.position.x, self.agent.pose.position.y)) config.visualiser_simulator_config = False # hide the simulator config window # algorithm if config.algorithm_name is None: config.algorithm_name = "WPN-view" config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[ config.algorithm_name] # map goal = Goal(Point(0, 0) if goal is None else goal) agent = Agent(self._world_to_grid( Point(self.agent.pose.position.x, self.agent.pose.position.y)), radius=self.INFLATE) mp = RosMap(agent, goal, lambda: self.grid, traversable_threshold=self.TRAVERSABLE_THRESHOLD, unmapped_value=-1, wp_publish=self._send_way_point, update_requested=self._map_update_requested, name="ROS Map") config.maps = {mp.name: mp} config.simulator_initial_map = list(config.maps.values())[0] config.map_name = list(config.maps.keys())[0] # create the simulator s = Services(config) s.algorithm.map.request_update() sim = Simulator(s) return sim
def __get_nearby_rooms_edges(room): room_top_left_point, room_size = room edges = [] full_edge = [] q = 0 for x in range(room_top_left_point.x, room_top_left_point.x + room_size.width): y = room_top_left_point.y # up if in_bounds(x, y): full_edge.append(Point(x, y)) n_x, n_y = x, y - 1 if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID: edges.append(q) q += 1 for y in range(room_top_left_point.y, room_top_left_point.y + room_size.height): x = room_top_left_point.x # right x = room_top_left_point.x + room_size.width - 1 if in_bounds(x, y): full_edge.append(Point(x, y)) n_x, n_y = x + 1, y if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID: edges.append(q) q += 1 for x in range(room_top_left_point.x, room_top_left_point.x + room_size.width): y = room_top_left_point.y # bottom y = room_top_left_point.y + room_size.height - 1 if in_bounds(x, y): full_edge.append(Point(x, y)) n_x, n_y = x, y + 1 if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID: edges.append(q) q += 1 for y in range(room_top_left_point.y, room_top_left_point.y + room_size.height): x = room_top_left_point.x # left if in_bounds(x, y): full_edge.append(Point(x, y)) n_x, n_y = x - 1, y if in_bounds(n_x, n_y) and grid[n_y][n_x] == DenseMap.WALL_ID: edges.append(q) q += 1 return edges, full_edge
""" Addition used to move move point in direction of the vector """ from structures import Vector, Point if __name__ == '__main__': point = Point(1,0) vector = Vector(2,3) point2 = point.add_vector(vector) print(point2.x, point2.y)
""" Substraction is used to figure out vector from one point to another V = P(destination) - P(current) """ from structures import Vector, Point if __name__ == '__main__': destination = Point(0, -1) current_position = Point(1, 1) v = destination.subtract(current_position) print(v.x, v.y)
""" Normalized vector is vector of length 1. Used where one direction is matter. E.g: direction of the view, direction of movement, etc. """ from structures import Vector, Point if __name__ == '__main__': p= Point(3,4) i = Point(1,2) pi = p.subtract(i) normalized_vector = pi.normalized() print("view vector: ", normalized_vector.x, normalized_vector.y) print("view vector length: ", normalized_vector.length())