From fdc52f26e5971286b0b129e8e18a5b02fcaab654 Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Fri, 9 Feb 2024 14:09:41 +0000 Subject: [PATCH 01/12] Inefficent RRT* implemented. --- examples/python/rrt-star/README.md | 16 ++ examples/python/rrt-star/main.py | 226 ++++++++++++++++++++++ examples/python/rrt-star/requirements.txt | 2 + 3 files changed, 244 insertions(+) create mode 100644 examples/python/rrt-star/README.md create mode 100755 examples/python/rrt-star/main.py create mode 100644 examples/python/rrt-star/requirements.txt diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md new file mode 100644 index 000000000000..5aca0924b1c3 --- /dev/null +++ b/examples/python/rrt-star/README.md @@ -0,0 +1,16 @@ + + + + +This is an example template. It is not a real example. You can duplicate the directory and use it as a starting point for writing a real example. + +```bash +pip install -r examples/python/rrt-star/requirements.txt +python examples/python/rrt-star/main.py +``` diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py new file mode 100755 index 000000000000..eb55293959d4 --- /dev/null +++ b/examples/python/rrt-star/main.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +""" +Visualization of the pathfifnding algorithm RRT* in a simple enviroment. + +Run: +```bash +pip install -r examples/python/rrt-star/requirements.txt +python examples/python/rrt-star/main.py +``` +""" +from __future__ import annotations + +import argparse +import numpy as np +import numpy.typing as npt +import rerun as rr +from typing import Annotated, Literal, Generator +import math + +Point2D = Annotated[npt.NDArray[np.float64], Literal[2]] + +def segments_intersect(start0: Point2D, end0: Point2D, start1: Point2D, end1: Point2D) -> bool: + """ Checks if the segments (start0, end0) and (start1, end1) intersect. """ + dir0 = end0-start0 + dir1 = end1-start1 + mat = np.stack([dir0, dir1], axis=1) + if abs(np.linalg.det(mat)) <= 0.00001: # They are close to perpendicular + return False + s,t = np.linalg.solve(mat, start1-start0) + return (0 <= s <= 1) and (0 <= -t <= 1) + +def steer(start: Point2D, end: Point2D, max_step_size: float) -> Point2D: + dist = np.linalg.norm(start-end, 2) + if dist < max_step_size: + return end + else: + diff = end-start + direction = diff/np.linalg.norm(diff, 2) + return direction*max_step_size+start + +class Node: + parent: Node | None + pos: Point2D + cost: float + children: list[Node] + + def __init__(self, parent, position, cost): + self.parent = parent + self.pos = position + self.cost = cost + self.children = [] + +class ExplorationTree: + + root: Node + nb_nodes: int + + def __init__(self, root_pos): + self.root = Node(None, root_pos, 0) + self.nb_nodes = 1 + + def __iter__(self) -> Generator[Node, None, None]: + nxt = [self.root] + while len(nxt) >= 1: + cur = nxt.pop() + yield cur + for child in cur.children: + nxt.append(child) + + def segments(self) -> list[(Point2D, Point2D)]: + strips = [] + for node in self: + if node.parent is not None: + start = node.pos + end = node.parent.pos + strips.append((start, end)) + return strips + + def nearest(self, point: Point2D) -> Node: + min_dist = float('inf') + closest_node = None + for node in self: + dist = np.linalg.norm(point-node.pos, 2) + if dist < min_dist: + closest_node = node + min_dist = dist + + return closest_node + + def add_node(self, parent: Node, node: Node): + parent.children.append(node) + node.parent = parent + self.nb_nodes += 1 + + def in_neighbourhood(self, point: Point2D, radius: float) -> list[Node]: + return [ node for node in self if np.linalg.norm(node.pos-point, 2) < radius ] + +class Map: + def set_default_map(self): + segments = [ + ( (0, 0), (0, 1) ), + ( (0, 1), (1, 1) ), + ( (1, 1), (1, 0) ), + ( (1, 0), (0, 0) ), + ( (0.4, 0.0), (0.4, 0.5) ), + ( (0.75, 1.0), (0.75, 0.2) ), + ( (0.2, 0.2), (0.2, 0.8) ) + ] + for start, end in segments: + self.obstacles.append((np.array(start), np.array(end))) + + def log_obstacles(self, path: str): + rr.log(path, rr.LineStrips2D(self.obstacles)) + + def __init__(self): + self.obstacles = [] # List of lines as tuples of (start_point, end_point) + self.set_default_map() + + def intersects_obstacle(self, start: Point2D, end: Point2D) -> bool: + return not all( not segments_intersect(start, end, obs_start, obs_end) for (obs_start, obs_end) in self.obstacles ) + +def rrt(map, start: Point2D, end: Point2D, max_step_size: float, neighbourhood_size: float): + tree = ExplorationTree(start) + + step = 0 # How many iterations of the algorithm we have done. + nb_nodes = 1 + while True: + + random_point = np.random.rand(2) + + # Finds the closest node + closest_node = tree.nearest(random_point) + + new_point = steer(closest_node.pos, random_point, max_step_size) + + intersects_obs = map.intersects_obstacle(closest_node.pos, new_point) + + step += 1 + rr.set_time_sequence("step", step) + rr.log("map/tree/edges", rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128])) + rr.log( + "map/tree/vertices", + rr.Points2D([ node.pos for node in tree ]), + + # So that we can the cost at a node by hovering over it + rr.AnyValues(cost=[ float(node.cost) for node in tree]) + ) + rr.log("map/random_point", rr.Points2D([random_point], radii=0.008)) + rr.log("map/closest_node", rr.Points2D([closest_node.pos], radii=0.008)) + rr.log("map/new_point", rr.Points2D([new_point], radii=0.008)) + + color = np.array([0, 255, 0, 255]).astype(np.uint8) + if intersects_obs: + # The segment intersects a obstacle which means we will color it red. + color = np.array([255, 0, 0, 255]).astype(np.uint8) + rr.log("map/current_line", rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001)) + + if not intersects_obs: + + # Searches for the point in a neighbourhood that would result in the minimal cost (distance from start). + close_nodes = tree.in_neighbourhood(new_point, neighbourhood_size) + rr.log("map/close_nodes", rr.Points2D([node.pos for node in close_nodes])) + min_node = min( + filter(lambda node: not map.intersects_obstacle(node.pos, new_point), close_nodes+[closest_node]), + key=lambda node: node.cost + np.linalg.norm(node.pos - new_point, 2) + ) + + cost = np.linalg.norm(min_node.pos-new_point, 2) + added_node = Node(min_node, new_point, cost+min_node.cost) + tree.add_node(min_node, added_node) + + for node in close_nodes: + cost = added_node.cost + np.linalg.norm(added_node.pos-node.pos, 2) + if not map.intersects_obstacle(new_point, node.pos) and cost < node.cost: + + parent = node.parent + parent.children.remove(node) + + node.parent = added_node + added_node.children.append(node) + + if np.linalg.norm(new_point-end, 2) < max_step_size and not map.intersects_obstacle(new_point, end): + # `close_point` can be connected to `end` which means that we are done. + + end_node = Node(added_node, end, added_node.cost+np.linalg.norm(new_point-end, 2)) + tree.add_node(added_node, end_node) + + # Reconstruct shortest path in tree + path = [end] + cur_node = end_node + while cur_node.parent is not None: + cur_node = cur_node.parent + path.append(cur_node.pos) + + # rr.log("map/tree/lines", rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[[0, 0, 255, 128]])) + return path + +def main() -> None: + parser = argparse.ArgumentParser(description="Example of using the Rerun visualizer") + rr.script_add_args(parser) + parser.add_argument("--max-step-size", default=0.06) + args = parser.parse_args() + rr.script_setup(args, "rerun_example_my_example_name") + max_step_size = args.max_step_size + neighbourhood_size = max_step_size*2.0 + + start_point = np.array([0.1, 0.5]) + end_point = np.array([0.9, 0.2]) + + rr.set_time_sequence("step", 0) + rr.log("map/start_point", rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]])) + rr.log("map/end_point", rr.Points2D([end_point], radii=0.02, colors=[[255, 255, 0, 255]])) + + mp = Map() + mp.log_obstacles("map/obstacles") + path = rrt(mp, start_point, end_point, max_step_size, neighbourhood_size) + segments = [] + for i in range(len(path)-1): + segments.append((path[i], path[i+1])) + + rr.log("map/path", rr.LineStrips2D(segments, radii=0.002, colors=[0, 255, 255, 255])) + + rr.script_teardown(args) + +if __name__ == "__main__": + main() diff --git a/examples/python/rrt-star/requirements.txt b/examples/python/rrt-star/requirements.txt new file mode 100644 index 000000000000..fa4ff5da669a --- /dev/null +++ b/examples/python/rrt-star/requirements.txt @@ -0,0 +1,2 @@ +numpy +rerun-sdk From 643577041836fb820861cde0780b5dee2195e178 Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 11 Feb 2024 13:59:29 +0000 Subject: [PATCH 02/12] cleaner code and fixed bugs --- examples/python/rrt-star/README.md | 2 +- examples/python/rrt-star/main.py | 150 +++++++++++++++++------------ 2 files changed, 87 insertions(+), 65 deletions(-) diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index 5aca0924b1c3..f2591dbc323b 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -8,7 +8,7 @@ Place a screenshot in place of this comment Use `just upload --help` for instructions --> -This is an example template. It is not a real example. You can duplicate the directory and use it as a starting point for writing a real example. +This examples visualizes the pathfifnding algorithm RRT* in a simple enviroment. ```bash pip install -r examples/python/rrt-star/requirements.txt diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py index eb55293959d4..3e7ed36de013 100755 --- a/examples/python/rrt-star/main.py +++ b/examples/python/rrt-star/main.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ -Visualization of the pathfifnding algorithm RRT* in a simple enviroment. +This examples visualizes the pathfifnding algorithm RRT* in a simple enviroment. Run: ```bash @@ -8,16 +8,16 @@ python examples/python/rrt-star/main.py ``` """ + from __future__ import annotations import argparse +from typing import Annotated, Literal, Generator import numpy as np import numpy.typing as npt import rerun as rr -from typing import Annotated, Literal, Generator -import math -Point2D = Annotated[npt.NDArray[np.float64], Literal[2]] +Point2D = Annotated[npt.NDArray[np.float64], Literal[2]] def segments_intersect(start0: Point2D, end0: Point2D, start1: Point2D, end1: Point2D) -> bool: """ Checks if the segments (start0, end0) and (start1, end1) intersect. """ @@ -29,35 +29,40 @@ def segments_intersect(start0: Point2D, end0: Point2D, start1: Point2D, end1: Po s,t = np.linalg.solve(mat, start1-start0) return (0 <= s <= 1) and (0 <= -t <= 1) -def steer(start: Point2D, end: Point2D, max_step_size: float) -> Point2D: +def steer(start: Point2D, end: Point2D, radius: float) -> Point2D: + """ Finds the point in a disc around `start` that is closest to `end`. """ dist = np.linalg.norm(start-end, 2) - if dist < max_step_size: + if dist < radius: return end else: diff = end-start direction = diff/np.linalg.norm(diff, 2) - return direction*max_step_size+start + return direction*radius+start class Node: parent: Node | None pos: Point2D cost: float children: list[Node] - + def __init__(self, parent, position, cost): self.parent = parent self.pos = position self.cost = cost self.children = [] -class ExplorationTree: + def change_cost(self, delta_cost): + """ Modifies the cost of this node and all child nodes. """ + self.cost += delta_cost + for child_node in self.children: + child_node.change_cost(delta_cost) + +class RRTTree: root: Node - nb_nodes: int def __init__(self, root_pos): self.root = Node(None, root_pos, 0) - self.nb_nodes = 1 def __iter__(self) -> Generator[Node, None, None]: nxt = [self.root] @@ -68,6 +73,7 @@ def __iter__(self) -> Generator[Node, None, None]: nxt.append(child) def segments(self) -> list[(Point2D, Point2D)]: + """ Returns all the edges of the tree. """ strips = [] for node in self: if node.parent is not None: @@ -77,6 +83,7 @@ def segments(self) -> list[(Point2D, Point2D)]: return strips def nearest(self, point: Point2D) -> Node: + """ Finds the point in the tree that is closest to `point` """ min_dist = float('inf') closest_node = None for node in self: @@ -86,11 +93,10 @@ def nearest(self, point: Point2D) -> Node: min_dist = dist return closest_node - + def add_node(self, parent: Node, node: Node): parent.children.append(node) node.parent = parent - self.nb_nodes += 1 def in_neighbourhood(self, point: Point2D, radius: float) -> list[Node]: return [ node for node in self if np.linalg.norm(node.pos-point, 2) < radius ] @@ -99,19 +105,20 @@ class Map: def set_default_map(self): segments = [ ( (0, 0), (0, 1) ), - ( (0, 1), (1, 1) ), - ( (1, 1), (1, 0) ), - ( (1, 0), (0, 0) ), - ( (0.4, 0.0), (0.4, 0.5) ), - ( (0.75, 1.0), (0.75, 0.2) ), - ( (0.2, 0.2), (0.2, 0.8) ) + ( (0, 1), (2, 1) ), + ( (2, 1), (2, 0) ), + ( (2, 0), (0, 0) ), + ( (1.0, 0.0), (1.0, 0.65) ), + + ( (1.5, 1.0), (1.5, 0.2) ), + ( (0.4, 0.2), (0.4, 0.8) ), ] for start, end in segments: self.obstacles.append((np.array(start), np.array(end))) def log_obstacles(self, path: str): rr.log(path, rr.LineStrips2D(self.obstacles)) - + def __init__(self): self.obstacles = [] # List of lines as tuples of (start_point, end_point) self.set_default_map() @@ -119,31 +126,43 @@ def __init__(self): def intersects_obstacle(self, start: Point2D, end: Point2D) -> bool: return not all( not segments_intersect(start, end, obs_start, obs_end) for (obs_start, obs_end) in self.obstacles ) -def rrt(map, start: Point2D, end: Point2D, max_step_size: float, neighbourhood_size: float): - tree = ExplorationTree(start) +def path_to_root(node: Node) -> list[Point2D]: + path = [node.pos] + cur_node = node + while cur_node.parent is not None: + cur_node = cur_node.parent + path.append(cur_node.pos) + return path + +def rrt(mp: Map, start: Point2D, end: Point2D, max_step_size: float, neighbourhood_size: float, nb_iter: int | None): + tree = RRTTree(start) + + path = None step = 0 # How many iterations of the algorithm we have done. - nb_nodes = 1 - while True: + end_node = None + step_found = None - random_point = np.random.rand(2) - - # Finds the closest node - closest_node = tree.nearest(random_point) + while (nb_iter is not None and step < nb_iter) or (step_found is None or step < step_found*3): + random_point = np.multiply(np.random.rand(2), [2,1]) + closest_node = tree.nearest(random_point) new_point = steer(closest_node.pos, random_point, max_step_size) - - intersects_obs = map.intersects_obstacle(closest_node.pos, new_point) + intersects_obs = mp.intersects_obstacle(closest_node.pos, new_point) step += 1 rr.set_time_sequence("step", step) - rr.log("map/tree/edges", rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128])) + rr.log("map/close_nodes", rr.Clear(recursive=False)) + rr.log( + "map/tree/edges", + rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128]) + ) rr.log( "map/tree/vertices", - rr.Points2D([ node.pos for node in tree ]), + rr.Points2D([ node.pos for node in tree ], radii=0.002), - # So that we can the cost at a node by hovering over it - rr.AnyValues(cost=[ float(node.cost) for node in tree]) + # So that we can see the cost at a node by hovering over it. + rr.AnyValues(cost=[ float(node.cost) for node in tree]), ) rr.log("map/random_point", rr.Points2D([random_point], radii=0.008)) rr.log("map/closest_node", rr.Points2D([closest_node.pos], radii=0.008)) @@ -151,74 +170,77 @@ def rrt(map, start: Point2D, end: Point2D, max_step_size: float, neighbourhood_s color = np.array([0, 255, 0, 255]).astype(np.uint8) if intersects_obs: - # The segment intersects a obstacle which means we will color it red. color = np.array([255, 0, 0, 255]).astype(np.uint8) - rr.log("map/current_line", rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001)) + rr.log( + "map/new_edge", + rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001) + ) if not intersects_obs: - # Searches for the point in a neighbourhood that would result in the minimal cost (distance from start). + # Searches for the point in a neighbourhood that would result in the minimal cost (distance from start). close_nodes = tree.in_neighbourhood(new_point, neighbourhood_size) rr.log("map/close_nodes", rr.Points2D([node.pos for node in close_nodes])) + min_node = min( - filter(lambda node: not map.intersects_obstacle(node.pos, new_point), close_nodes+[closest_node]), + filter( + lambda node: not mp.intersects_obstacle(node.pos, new_point), + close_nodes+[closest_node] + ), key=lambda node: node.cost + np.linalg.norm(node.pos - new_point, 2) ) - + cost = np.linalg.norm(min_node.pos-new_point, 2) added_node = Node(min_node, new_point, cost+min_node.cost) tree.add_node(min_node, added_node) + # Modifies nearby nodes that would be reached faster by going through `added_node`. for node in close_nodes: cost = added_node.cost + np.linalg.norm(added_node.pos-node.pos, 2) - if not map.intersects_obstacle(new_point, node.pos) and cost < node.cost: + if not mp.intersects_obstacle(new_point, node.pos) and cost < node.cost: parent = node.parent parent.children.remove(node) - + node.parent = added_node + node.change_cost(cost-node.cost) added_node.children.append(node) - if np.linalg.norm(new_point-end, 2) < max_step_size and not map.intersects_obstacle(new_point, end): - # `close_point` can be connected to `end` which means that we are done. - + if np.linalg.norm(new_point-end, 2) < max_step_size and not mp.intersects_obstacle(new_point, end) and end_node is None: end_node = Node(added_node, end, added_node.cost+np.linalg.norm(new_point-end, 2)) tree.add_node(added_node, end_node) + step_found = step + if end_node: # Reconstruct shortest path in tree - path = [end] - cur_node = end_node - while cur_node.parent is not None: - cur_node = cur_node.parent - path.append(cur_node.pos) + path = path_to_root(end_node) + segments = [(path[i], path[i+1]) for i in range(len(path)-1)] + rr.log("map/path", rr.LineStrips2D(segments, radii=0.002, colors=[0, 255, 255, 255])) - # rr.log("map/tree/lines", rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[[0, 0, 255, 128]])) - return path + return path def main() -> None: parser = argparse.ArgumentParser(description="Example of using the Rerun visualizer") rr.script_add_args(parser) - parser.add_argument("--max-step-size", default=0.06) + parser.add_argument("--max-step-size", default=0.1) + parser.add_argument("--iterations", help="How many iterations it should do") args = parser.parse_args() - rr.script_setup(args, "rerun_example_my_example_name") + rr.script_setup(args, "") max_step_size = args.max_step_size - neighbourhood_size = max_step_size*2.0 + neighbourhood_size = max_step_size*1.5 - start_point = np.array([0.1, 0.5]) - end_point = np.array([0.9, 0.2]) + start_point = np.array([0.2, 0.5]) + end_point = np.array([1.8, 0.5]) + + # rr.log("map/points", rr.Points2D([[0.0,0.0], [2.0, 1.0]], colors=[255,255,255,255])) rr.set_time_sequence("step", 0) - rr.log("map/start_point", rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]])) - rr.log("map/end_point", rr.Points2D([end_point], radii=0.02, colors=[[255, 255, 0, 255]])) + rr.log("map/start", rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]])) + rr.log("map/destination", rr.Points2D([end_point], radii=0.02, colors=[[255, 255, 0, 255]])) mp = Map() mp.log_obstacles("map/obstacles") - path = rrt(mp, start_point, end_point, max_step_size, neighbourhood_size) - segments = [] - for i in range(len(path)-1): - segments.append((path[i], path[i+1])) - - rr.log("map/path", rr.LineStrips2D(segments, radii=0.002, colors=[0, 255, 255, 255])) + __path = rrt(mp, start_point, end_point, max_step_size, neighbourhood_size, args.iterations) rr.script_teardown(args) From 9b55bea842099b8e2e0b40c3ef74b42f5fd10334 Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Wed, 14 Feb 2024 11:54:15 +0100 Subject: [PATCH 03/12] fixed linting issuse --- examples/python/rrt-star/README.md | 4 +- examples/python/rrt-star/main.py | 192 +++++++++++++++++------------ 2 files changed, 114 insertions(+), 82 deletions(-) diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index f2591dbc323b..1bfed4a2918c 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -1,6 +1,6 @@ -This examples visualizes the pathfifnding algorithm RRT* in a simple enviroment. +This examples visualizes the pathfinding algorithm RRT* in a simple environment. ```bash pip install -r examples/python/rrt-star/requirements.txt diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py index 3e7ed36de013..7d49fd2794f9 100755 --- a/examples/python/rrt-star/main.py +++ b/examples/python/rrt-star/main.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ -This examples visualizes the pathfifnding algorithm RRT* in a simple enviroment. +Visualizes the pathfinding algorithm RRT* in a simple environment. Run: ```bash @@ -12,32 +12,40 @@ from __future__ import annotations import argparse -from typing import Annotated, Literal, Generator +from typing import Annotated, Generator, Literal + import numpy as np import numpy.typing as npt import rerun as rr Point2D = Annotated[npt.NDArray[np.float64], Literal[2]] + +def distance(point0: Point2D, point1: Point2D) -> float: + return float(np.linalg.norm(point0 - point1, 2)) + + def segments_intersect(start0: Point2D, end0: Point2D, start1: Point2D, end1: Point2D) -> bool: - """ Checks if the segments (start0, end0) and (start1, end1) intersect. """ - dir0 = end0-start0 - dir1 = end1-start1 + """Checks if the segments (start0, end0) and (start1, end1) intersect.""" + dir0 = end0 - start0 + dir1 = end1 - start1 mat = np.stack([dir0, dir1], axis=1) - if abs(np.linalg.det(mat)) <= 0.00001: # They are close to perpendicular + if abs(np.linalg.det(mat)) <= 0.00001: # They are close to perpendicular return False - s,t = np.linalg.solve(mat, start1-start0) - return (0 <= s <= 1) and (0 <= -t <= 1) + s, t = np.linalg.solve(mat, start1 - start0) + return (0 <= float(s) <= 1) and (0 <= -float(t) <= 1) + def steer(start: Point2D, end: Point2D, radius: float) -> Point2D: - """ Finds the point in a disc around `start` that is closest to `end`. """ - dist = np.linalg.norm(start-end, 2) + """Finds the point in a disc around `start` that is closest to `end`.""" + dist = distance(start, end) if dist < radius: return end else: - diff = end-start - direction = diff/np.linalg.norm(diff, 2) - return direction*radius+start + diff = end - start + direction = diff / np.linalg.norm(diff, 2) + return direction * radius + start + class Node: parent: Node | None @@ -45,23 +53,23 @@ class Node: cost: float children: list[Node] - def __init__(self, parent, position, cost): + def __init__(self, parent: Node | None, position: Point2D, cost: float) -> None: self.parent = parent self.pos = position self.cost = cost self.children = [] - def change_cost(self, delta_cost): - """ Modifies the cost of this node and all child nodes. """ + def change_cost(self, delta_cost: float) -> None: + """Modifies the cost of this node and all child nodes.""" self.cost += delta_cost for child_node in self.children: child_node.change_cost(delta_cost) -class RRTTree: +class RRTTree: root: Node - def __init__(self, root_pos): + def __init__(self, root_pos: Point2D) -> None: self.root = Node(None, root_pos, 0) def __iter__(self) -> Generator[Node, None, None]: @@ -72,8 +80,8 @@ def __iter__(self) -> Generator[Node, None, None]: for child in cur.children: nxt.append(child) - def segments(self) -> list[(Point2D, Point2D)]: - """ Returns all the edges of the tree. """ + def segments(self) -> list[tuple[Point2D, Point2D]]: + """Returns all the edges of the tree.""" strips = [] for node in self: if node.parent is not None: @@ -83,48 +91,53 @@ def segments(self) -> list[(Point2D, Point2D)]: return strips def nearest(self, point: Point2D) -> Node: - """ Finds the point in the tree that is closest to `point` """ - min_dist = float('inf') - closest_node = None + """Finds the point in the tree that is closest to `point`.""" + min_dist = distance(point, self.root.pos) + closest_node = self.root for node in self: - dist = np.linalg.norm(point-node.pos, 2) + dist = distance(point, node.pos) if dist < min_dist: closest_node = node min_dist = dist return closest_node - def add_node(self, parent: Node, node: Node): + def add_node(self, parent: Node, node: Node) -> None: parent.children.append(node) node.parent = parent - def in_neighbourhood(self, point: Point2D, radius: float) -> list[Node]: - return [ node for node in self if np.linalg.norm(node.pos-point, 2) < radius ] + def in_neighborhood(self, point: Point2D, radius: float) -> list[Node]: + return [node for node in self if distance(node.pos, point) < radius] + class Map: - def set_default_map(self): + obstacles: list[tuple[Point2D, Point2D]] + + def set_default_map(self) -> None: segments = [ - ( (0, 0), (0, 1) ), - ( (0, 1), (2, 1) ), - ( (2, 1), (2, 0) ), - ( (2, 0), (0, 0) ), - ( (1.0, 0.0), (1.0, 0.65) ), - - ( (1.5, 1.0), (1.5, 0.2) ), - ( (0.4, 0.2), (0.4, 0.8) ), + ((0, 0), (0, 1)), + ((0, 1), (2, 1)), + ((2, 1), (2, 0)), + ((2, 0), (0, 0)), + ((1.0, 0.0), (1.0, 0.65)), + ((1.5, 1.0), (1.5, 0.2)), + ((0.4, 0.2), (0.4, 0.8)), ] for start, end in segments: self.obstacles.append((np.array(start), np.array(end))) - def log_obstacles(self, path: str): + def log_obstacles(self, path: str) -> None: rr.log(path, rr.LineStrips2D(self.obstacles)) - def __init__(self): - self.obstacles = [] # List of lines as tuples of (start_point, end_point) + def __init__(self) -> None: + self.obstacles = [] # List of lines as tuples of (start, end) self.set_default_map() def intersects_obstacle(self, start: Point2D, end: Point2D) -> bool: - return not all( not segments_intersect(start, end, obs_start, obs_end) for (obs_start, obs_end) in self.obstacles ) + return not all( + not segments_intersect(start, end, obs_start, obs_end) for (obs_start, obs_end) in self.obstacles + ) + def path_to_root(node: Node) -> list[Point2D]: path = [node.pos] @@ -135,17 +148,23 @@ def path_to_root(node: Node) -> list[Point2D]: return path -def rrt(mp: Map, start: Point2D, end: Point2D, max_step_size: float, neighbourhood_size: float, nb_iter: int | None): +def rrt( + mp: Map, + start: Point2D, + end: Point2D, + max_step_size: float, + neighborhood_size: float, + num_iter: int | None, +) -> list[tuple[Point2D, Point2D]] | None: tree = RRTTree(start) path = None - step = 0 # How many iterations of the algorithm we have done. + step = 0 # How many iterations of the algorithm we have done. end_node = None step_found = None - while (nb_iter is not None and step < nb_iter) or (step_found is None or step < step_found*3): - - random_point = np.multiply(np.random.rand(2), [2,1]) + while (num_iter is not None and step < num_iter) or (step_found is None or step < step_found * 3): + random_point = np.multiply(np.random.rand(2), [2, 1]) closest_node = tree.nearest(random_point) new_point = steer(closest_node.pos, random_point, max_step_size) intersects_obs = mp.intersects_obstacle(closest_node.pos, new_point) @@ -154,15 +173,14 @@ def rrt(mp: Map, start: Point2D, end: Point2D, max_step_size: float, neighbourho rr.set_time_sequence("step", step) rr.log("map/close_nodes", rr.Clear(recursive=False)) rr.log( - "map/tree/edges", - rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128]) + "map/tree/edges", + rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128]), ) rr.log( - "map/tree/vertices", - rr.Points2D([ node.pos for node in tree ], radii=0.002), - + "map/tree/vertices", + rr.Points2D([node.pos for node in tree], radii=0.002), # So that we can see the cost at a node by hovering over it. - rr.AnyValues(cost=[ float(node.cost) for node in tree]), + rr.AnyValues(cost=[float(node.cost) for node in tree]), ) rr.log("map/random_point", rr.Points2D([random_point], radii=0.008)) rr.log("map/closest_node", rr.Points2D([closest_node.pos], radii=0.008)) @@ -172,77 +190,91 @@ def rrt(mp: Map, start: Point2D, end: Point2D, max_step_size: float, neighbourho if intersects_obs: color = np.array([255, 0, 0, 255]).astype(np.uint8) rr.log( - "map/new_edge", - rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001) + "map/new_edge", + rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001), ) if not intersects_obs: - - # Searches for the point in a neighbourhood that would result in the minimal cost (distance from start). - close_nodes = tree.in_neighbourhood(new_point, neighbourhood_size) + # Searches for the point in a neighborhood that would result in the minimal cost (distance from start). + close_nodes = tree.in_neighborhood(new_point, neighborhood_size) rr.log("map/close_nodes", rr.Points2D([node.pos for node in close_nodes])) min_node = min( filter( lambda node: not mp.intersects_obstacle(node.pos, new_point), - close_nodes+[closest_node] + close_nodes + [closest_node], ), - key=lambda node: node.cost + np.linalg.norm(node.pos - new_point, 2) + key=lambda node: node.cost + distance(node.pos, new_point), ) - cost = np.linalg.norm(min_node.pos-new_point, 2) - added_node = Node(min_node, new_point, cost+min_node.cost) + cost = distance(min_node.pos, new_point) + added_node = Node(min_node, new_point, cost + min_node.cost) tree.add_node(min_node, added_node) # Modifies nearby nodes that would be reached faster by going through `added_node`. for node in close_nodes: - cost = added_node.cost + np.linalg.norm(added_node.pos-node.pos, 2) + cost = added_node.cost + distance(added_node.pos, node.pos) if not mp.intersects_obstacle(new_point, node.pos) and cost < node.cost: - parent = node.parent - parent.children.remove(node) - - node.parent = added_node - node.change_cost(cost-node.cost) - added_node.children.append(node) - - if np.linalg.norm(new_point-end, 2) < max_step_size and not mp.intersects_obstacle(new_point, end) and end_node is None: - end_node = Node(added_node, end, added_node.cost+np.linalg.norm(new_point-end, 2)) + if parent is not None: + parent.children.remove(node) + + node.parent = added_node + node.change_cost(cost - node.cost) + added_node.children.append(node) + + if ( + distance(new_point, end) < max_step_size + and not mp.intersects_obstacle(new_point, end) + and end_node is None + ): + end_node = Node(added_node, end, added_node.cost + distance(new_point, end)) tree.add_node(added_node, end_node) step_found = step if end_node: # Reconstruct shortest path in tree path = path_to_root(end_node) - segments = [(path[i], path[i+1]) for i in range(len(path)-1)] - rr.log("map/path", rr.LineStrips2D(segments, radii=0.002, colors=[0, 255, 255, 255])) + segments = [(path[i], path[i + 1]) for i in range(len(path) - 1)] + rr.log( + "map/path", + rr.LineStrips2D(segments, radii=0.002, colors=[0, 255, 255, 255]), + ) return path + def main() -> None: parser = argparse.ArgumentParser(description="Example of using the Rerun visualizer") rr.script_add_args(parser) - parser.add_argument("--max-step-size", default=0.1) - parser.add_argument("--iterations", help="How many iterations it should do") + parser.add_argument("--max-step-size", type=float, default=0.1) + parser.add_argument("--iterations", type=int, help="How many iterations it should do") args = parser.parse_args() rr.script_setup(args, "") + max_step_size = args.max_step_size - neighbourhood_size = max_step_size*1.5 + neighborhood_size = max_step_size * 1.5 start_point = np.array([0.2, 0.5]) end_point = np.array([1.8, 0.5]) - # rr.log("map/points", rr.Points2D([[0.0,0.0], [2.0, 1.0]], colors=[255,255,255,255])) - rr.set_time_sequence("step", 0) - rr.log("map/start", rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]])) - rr.log("map/destination", rr.Points2D([end_point], radii=0.02, colors=[[255, 255, 0, 255]])) + rr.log( + "map/start", + rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]]), + ) + rr.log( + "map/destination", + rr.Points2D([end_point], radii=0.02, colors=[[255, 255, 0, 255]]), + ) mp = Map() mp.log_obstacles("map/obstacles") - __path = rrt(mp, start_point, end_point, max_step_size, neighbourhood_size, args.iterations) + + __path = rrt(mp, start_point, end_point, max_step_size, neighborhood_size, args.iterations) rr.script_teardown(args) + if __name__ == "__main__": main() From da97a393442ad88f788fe306d229d198f1305d4a Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Wed, 14 Feb 2024 12:49:21 +0100 Subject: [PATCH 04/12] changed path of data used for construction --- examples/python/rrt-star/main.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py index 7d49fd2794f9..201218308608 100755 --- a/examples/python/rrt-star/main.py +++ b/examples/python/rrt-star/main.py @@ -171,7 +171,7 @@ def rrt( step += 1 rr.set_time_sequence("step", step) - rr.log("map/close_nodes", rr.Clear(recursive=False)) + rr.log("map/new/close_nodes", rr.Clear(recursive=False)) rr.log( "map/tree/edges", rr.LineStrips2D(tree.segments(), radii=0.0005, colors=[0, 0, 255, 128]), @@ -182,22 +182,22 @@ def rrt( # So that we can see the cost at a node by hovering over it. rr.AnyValues(cost=[float(node.cost) for node in tree]), ) - rr.log("map/random_point", rr.Points2D([random_point], radii=0.008)) - rr.log("map/closest_node", rr.Points2D([closest_node.pos], radii=0.008)) - rr.log("map/new_point", rr.Points2D([new_point], radii=0.008)) + rr.log("map/new/random_point", rr.Points2D([random_point], radii=0.008)) + rr.log("map/new/closest_node", rr.Points2D([closest_node.pos], radii=0.008)) + rr.log("map/new/new_point", rr.Points2D([new_point], radii=0.008)) color = np.array([0, 255, 0, 255]).astype(np.uint8) if intersects_obs: color = np.array([255, 0, 0, 255]).astype(np.uint8) rr.log( - "map/new_edge", + "map/new/new_edge", rr.LineStrips2D([(closest_node.pos, new_point)], colors=[color], radii=0.001), ) if not intersects_obs: # Searches for the point in a neighborhood that would result in the minimal cost (distance from start). close_nodes = tree.in_neighborhood(new_point, neighborhood_size) - rr.log("map/close_nodes", rr.Points2D([node.pos for node in close_nodes])) + rr.log("map/new/close_nodes", rr.Points2D([node.pos for node in close_nodes])) min_node = min( filter( From 78f8a6d14d2e052465048494f193dcace5a8b3bb Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Fri, 16 Feb 2024 12:44:59 +0100 Subject: [PATCH 05/12] added screenshot and fixed typos in README --- examples/python/rrt-star/README.md | 11 +++++------ examples/python/rrt-star/main.py | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index 1bfed4a2918c..16431a9ca2c3 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -1,14 +1,13 @@ - + + + -This examples visualizes the pathfinding algorithm RRT* in a simple environment. +This example visualizes the path finding algorithm RRT\* in a simple environment. ```bash pip install -r examples/python/rrt-star/requirements.txt diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py index 201218308608..0508a8c7cf5d 100755 --- a/examples/python/rrt-star/main.py +++ b/examples/python/rrt-star/main.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ -Visualizes the pathfinding algorithm RRT* in a simple environment. +Visualizes the path finding algorithm RRT* in a simple environment. Run: ```bash From a4467ea343f3270a48ec2bc4f473b9c9b708e8ad Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 20:13:29 +0100 Subject: [PATCH 06/12] added brief description on how it works --- examples/python/rrt-star/main.py | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/examples/python/rrt-star/main.py b/examples/python/rrt-star/main.py index 0508a8c7cf5d..073a17d4c428 100755 --- a/examples/python/rrt-star/main.py +++ b/examples/python/rrt-star/main.py @@ -2,6 +2,15 @@ """ Visualizes the path finding algorithm RRT* in a simple environment. +The algorithm finds a path between two points by randomly expanding a tree from the start point. +After it has added a random edge to the tree it looks at nearby nodes to check if it's faster to +reach them through this new edge instead, and if so it changes the parent of these nodes. +This ensures that the algorithm will converge to the optimal path given enough time. + +A more detailed explanation can be found in the original paper +Karaman, S. Frazzoli, S. 2011. "Sampling-based algorithms for optimal motion planning". +or in the following medium article: https://theclassytim.medium.com/robotic-path-planning-rrt-and-rrt-212319121378 + Run: ```bash pip install -r examples/python/rrt-star/requirements.txt @@ -245,7 +254,7 @@ def rrt( def main() -> None: - parser = argparse.ArgumentParser(description="Example of using the Rerun visualizer") + parser = argparse.ArgumentParser(description="Visualization of the path finding algorithm RRT*.") rr.script_add_args(parser) parser.add_argument("--max-step-size", type=float, default=0.1) parser.add_argument("--iterations", type=int, help="How many iterations it should do") @@ -259,6 +268,23 @@ def main() -> None: end_point = np.array([1.8, 0.5]) rr.set_time_sequence("step", 0) + rr.log( + "description", + rr.TextDocument( + """ +Visualizes the path finding algorithm RRT* in a simple environment. + +The algorithm finds a [path](recording://map/path) between two points by randomly expanding a [tree](recording://map/tree/edges) from the [start point](recording://map/start). +After it has added a [random edge](recording://map/new/new_edge) to the tree it looks at [nearby nodes](recording://map/new/close_nodes) to check if it's faster to reach them through this [new edge](recording://map/new/new_edge) instead, and if so it changes the parent of these nodes. +This ensures that the algorithm will converge to the optimal path given enough time. + +A more detailed explanation can be found in the original paper +Karaman, S. Frazzoli, S. 2011. "Sampling-based algorithms for optimal motion planning". +or in [this medium article](https://theclassytim.medium.com/robotic-path-planning-rrt-and-rrt-212319121378) + """.strip(), + media_type=rr.MediaType.MARKDOWN, + ), + ) rr.log( "map/start", rr.Points2D([start_point], radii=0.02, colors=[[255, 255, 255, 255]]), From 5cd72b3970abcc572734a535ea723eb98854f1b8 Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 20:37:12 +0100 Subject: [PATCH 07/12] updated screenshot and added requirements to examples/python/requirements.txt --- examples/python/requirements.txt | 1 + examples/python/rrt-star/README.md | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/python/requirements.txt b/examples/python/requirements.txt index 405a8efb5a18..6d1495237921 100644 --- a/examples/python/requirements.txt +++ b/examples/python/requirements.txt @@ -33,3 +33,4 @@ -r stdio/requirements.txt -r structure_from_motion/requirements.txt -r template/requirements.txt +-r rrt-star/requirements.txt diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index 16431a9ca2c3..575a11134442 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -4,11 +4,15 @@ description = "Visualization of the path finding algorithm RRT* in a simple envi --> - + This example visualizes the path finding algorithm RRT\* in a simple environment. +A detailed explanation can be found in the original paper +Karaman, S. Frazzoli, S. 2011. "Sampling-based algorithms for optimal motion planning". +or in [this medium article](https://theclassytim.medium.com/robotic-path-planning-rrt-and-rrt-212319121378) + ```bash pip install -r examples/python/rrt-star/requirements.txt python examples/python/rrt-star/main.py From 9ee48714ebe6faf045c3958f753d24a671d7d14c Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 20:46:15 +0100 Subject: [PATCH 08/12] fix broken link and add words to spellchecker --- .vscode/settings.json | 2 ++ examples/python/rrt-star/README.md | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 90ef30e18ae1..6843f2149092 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -20,9 +20,11 @@ "emath", "flatbuffers", "framebuffer", + "Frazzoli", "hoverable", "ilog", "jumpflooding", + "Karaman", "Keypoint", "memoffset", "nyud", diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index 575a11134442..4ac2c66567ab 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -4,7 +4,7 @@ description = "Visualization of the path finding algorithm RRT* in a simple envi --> - + This example visualizes the path finding algorithm RRT\* in a simple environment. From df601f77f188aa785aa5d347d0112d3dc3b2f6df Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 20:54:20 +0100 Subject: [PATCH 09/12] added words too cspell.json --- docs/cspell.json | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/cspell.json b/docs/cspell.json index 321cfce2747a..cde2ac05e720 100644 --- a/docs/cspell.json +++ b/docs/cspell.json @@ -38,6 +38,7 @@ "Farooq", "Feichtenhofer", "Firman", + "Frazzoli", "GLES", "Georgios", "Girshick", @@ -51,6 +52,7 @@ "Huggingface", "Jitendra", "Joao", + "Karaman", "Kirillov", "LIMAP", "Landmarker", From 6bf5eb941f83a41f108b1fcdb506e83be2513b70 Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 21:34:36 +0100 Subject: [PATCH 10/12] fix ordering in requirements.txt --- examples/python/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python/requirements.txt b/examples/python/requirements.txt index 6d1495237921..77c69a8a21f2 100644 --- a/examples/python/requirements.txt +++ b/examples/python/requirements.txt @@ -27,10 +27,10 @@ -r raw_mesh/requirements.txt -r rgbd/requirements.txt -r ros_node/requirements.txt +-r rrt-star/requirements.txt -r segment_anything_model/requirements.txt -r shared_recording/requirements.txt -r signed_distance_fields/requirements.txt -r stdio/requirements.txt -r structure_from_motion/requirements.txt -r template/requirements.txt --r rrt-star/requirements.txt From 48476971401c69710e7c0ffd1a45b97206d2ef9b Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Sun, 18 Feb 2024 21:44:53 +0100 Subject: [PATCH 11/12] update link to screenshot again for some reason. --- examples/python/rrt-star/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index 4ac2c66567ab..a4425c390fb6 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -4,7 +4,7 @@ description = "Visualization of the path finding algorithm RRT* in a simple envi --> - + This example visualizes the path finding algorithm RRT\* in a simple environment. From d05576edebf1b39db5946d2764be77dad8b7e6be Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Wed, 21 Feb 2024 18:16:46 +0100 Subject: [PATCH 12/12] upload screenshot and change link in readme --- examples/python/rrt-star/README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/python/rrt-star/README.md b/examples/python/rrt-star/README.md index a4425c390fb6..23b1d9e274ff 100644 --- a/examples/python/rrt-star/README.md +++ b/examples/python/rrt-star/README.md @@ -1,10 +1,17 @@ - + RRT* example screenshot + + + + This example visualizes the path finding algorithm RRT\* in a simple environment.