From 18ddd47889efdca8e654c77f3479e9c3df6e5cb3 Mon Sep 17 00:00:00 2001 From: Harshal Deshpande Date: Sat, 24 Oct 2020 17:26:07 +0530 Subject: [PATCH 1/2] added rrt_connect --- gennav/planners/__init__.py | 2 +- gennav/planners/rrt/__init__.py | 1 + gennav/planners/rrt/rrt_connect.py | 218 ++++++++++++++++++++++++ tests/__init__.py | 1 + tests/test_planners/rrt_connect_test.py | 40 +++++ 5 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 gennav/planners/rrt/rrt_connect.py create mode 100644 tests/test_planners/rrt_connect_test.py diff --git a/gennav/planners/__init__.py b/gennav/planners/__init__.py index b01441c..7f2e163 100644 --- a/gennav/planners/__init__.py +++ b/gennav/planners/__init__.py @@ -1,4 +1,4 @@ from gennav.planners.base import Planner # noqa: F401 from gennav.planners.potential_field import PotentialField # noqa: F401 from gennav.planners.prm import PRM, PRMStar # noqa: F401 -from gennav.planners.rrt import RRG, RRT, InformedRRTstar # noqa: F401 +from gennav.planners.rrt import RRG, RRT, InformedRRTstar, RRTConnect # noqa: F401 diff --git a/gennav/planners/rrt/__init__.py b/gennav/planners/rrt/__init__.py index 3e104df..edb8aea 100644 --- a/gennav/planners/rrt/__init__.py +++ b/gennav/planners/rrt/__init__.py @@ -1,3 +1,4 @@ from gennav.planners.rrt.informed_rrtstar import InformedRRTstar # noqa: F401 from gennav.planners.rrt.rrg import RRG # noqa: F401 from gennav.planners.rrt.rrt import RRT # noqa: F401 +from gennav.planners.rrt.rrt_connect import RRTConnect # noqa: F401 diff --git a/gennav/planners/rrt/rrt_connect.py b/gennav/planners/rrt/rrt_connect.py new file mode 100644 index 0000000..b25c674 --- /dev/null +++ b/gennav/planners/rrt/rrt_connect.py @@ -0,0 +1,218 @@ +import numpy as np + +from gennav.planners import Planner +from gennav.utils import RobotState, Trajectory +from gennav.utils.common import Node +from gennav.utils.custom_exceptions import ( + InvalidGoalState, + InvalidStartState, + PathNotFound, +) +from gennav.utils.geometry import Point, compute_distance + + +class RRTConnect(Planner): + """RRTConnect Planner Class. + + Drawback: A lot of nearest neighbour searches are performed. + Should use kd-trees to optimize this + + Args: + sampler (gennav.utils.sampler.Sampler): sampler to get random states + expand_dis (float): distance to expand tree by at each step + """ + + def __init__(self, sampler, expand_dis=0.1, *args, **kwargs): + super(RRTConnect, self).__init__() + self.sampler = sampler + self.expand_dis = expand_dis + + def expand_tree(self, node_list, target_state, env): + """ + A function to expand the tree in the direction of target node + """ + distance_list = [ + compute_distance(node.state.position, target_state.position) + for node in node_list + ] + + try: + # for python2 + nearest_node_index = min( + xrange(len(distance_list)), key=distance_list.__getitem__ + ) + except NameError: + # for python 3 + nearest_node_index = distance_list.index(min(distance_list)) + + nearest_node = node_list[nearest_node_index] + + # Create new point in the direction of sampled point + theta = np.arctan2( + target_state.position.y - nearest_node.state.position.y, + target_state.position.x - nearest_node.state.position.x, + ) + new_point = Point() + new_point.x = nearest_node.state.position.x + self.expand_dis * np.cos(theta) + new_point.y = nearest_node.state.position.y + self.expand_dis * np.sin(theta) + + # Check whether new point is inside an obstacles + traj = Trajectory(path=[nearest_node.state, RobotState(position=new_point)]) + + new_node = Node.from_coordinates(new_point) + new_node.parent = nearest_node + + if not env.get_traj_status(traj): + valid = False + else: + valid = True + + return new_node, valid + + def plan(self, start, goal, env): + """ + 1. expand 1 tree. + 2. make the new vertex target for the other tree. expand the + other tree until met an obstacle + + """ + # Check if start and goal states are obstacle free + if not env.get_status(start): + raise InvalidStartState(start, message="Start state is in obstacle.") + + if not env.get_status(goal): + raise InvalidGoalState(goal, message="Goal state is in obstacle.") + + # Initialize start and goal nodes + start_node = Node(state=start) + goal_node = Node(state=goal) + + got_path = False + + # Initialize trajectory + traj = Trajectory(path=[start, goal]) + + # If the line between start and goal is obstacle free then connect directly + # otherwise use rrt_connect + if not env.get_traj_status(traj): + node_list_from_start = [start_node] + node_list_from_goal = [goal_node] + tree_flag = True # this flag is used to switch between the trees + # if true expand from start node tree + + while True: + rnd_state = self.sampler() + # Randomly expand either of the trees + if tree_flag: + # print("expanding from start") + new_node, valid = self.expand_tree( + node_list_from_start, rnd_state, env + ) + if valid: + node_list_from_start.append(new_node) + else: + continue + else: + # print("expanding from goal") + new_node, valid = self.expand_tree( + node_list_from_goal, rnd_state, env + ) + if valid: + node_list_from_goal.append(new_node) + else: + continue + + # Use the new_node as the target for the other tree to expand + if tree_flag: + # print("expanding towards goal") + while True: + new_node_target, valid = self.expand_tree( + node_list_from_goal, new_node.state, env + ) + if not valid: + break + else: + node_list_from_goal.append(new_node_target) + # Check if target has been reached or if there is direct connection to target + distance_to_target = compute_distance( + new_node_target.state.position, new_node.state.position + ) + traj = Trajectory( + path=[new_node_target.state, new_node.state] + ) + if ( + distance_to_target < self.expand_dis + or env.get_traj_status(traj) + ): + got_path = True + break + else: + # print("expanding towards start") + while True: + new_node_target, valid = self.expand_tree( + node_list_from_start, new_node.state, env + ) + if not valid: + break + else: + node_list_from_start.append(new_node_target) + # Check if target has been reached or if there is direct connection to target + distance_to_target = compute_distance( + new_node_target.state.position, new_node.state.position + ) + traj = Trajectory( + path=[new_node_target.state, new_node.state] + ) + if ( + distance_to_target < self.expand_dis + or env.get_traj_status(traj) + ): + got_path = True + break + + if got_path: + break + + tree_flag = not tree_flag + + # Get the actual path from the + path = [] + last_node = node_list_from_goal[-1] + while last_node.parent is not None: + path.append(last_node.state) + last_node = last_node.parent + path.append(goal) + + path = path[::-1] # goal becomes the first element + + last_node = node_list_from_start[-1] + while last_node.parent is not None: + path.append(last_node.state) + last_node = last_node.parent + path.append(start) + + info_dict = {} + info_dict["node_list"] = node_list_from_start + node_list_from_goal + path = Trajectory(path[::-1]) + + if len(path.path) == 1: + raise PathNotFound(path, message="Path contains only one state") + + return (path, info_dict) + + else: + goal_node.parent = start_node + node_list = [start_node, goal_node] + + path = [] + last_node = node_list[-1] + while last_node.parent is not None: + path.append(last_node.state) + last_node = last_node.parent + path.append(start) + + info_dict = {} + info_dict["node_list"] = node_list + path = Trajectory(path[::-1]) + + return (path, info_dict) diff --git a/tests/__init__.py b/tests/__init__.py index b595b10..cb381c4 100644 --- a/tests/__init__.py +++ b/tests/__init__.py @@ -1,3 +1,4 @@ from tests.test_graph_search import astar_test # noqa: F401 from tests.test_planners import prm_test # noqa: F401 from tests.test_planners import rrt_test # noqa: F401 +from tests.test_planners import rrt_connect_test # noqa: F401 diff --git a/tests/test_planners/rrt_connect_test.py b/tests/test_planners/rrt_connect_test.py new file mode 100644 index 0000000..8f52a86 --- /dev/null +++ b/tests/test_planners/rrt_connect_test.py @@ -0,0 +1,40 @@ +from gennav.envs import PolygonEnv +from gennav.planners.rrt.rrt_connect import RRTConnect +from gennav.utils import RobotState +from gennav.utils.geometry import Point +from gennav.utils.samplers import UniformRectSampler + + +def test_rrt_connect(): + general_obstacles_list = [ + [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], + [ + [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], + [(4, 0), (4, 5), (5, 5), (5, 0)], + [(8, 2), (8, 7), (10, 7), (10, 2)], + ], + ] + sampler = UniformRectSampler(-5, 15, -5, 15) + poly = PolygonEnv(buffer_dist=0.5) + my_tree = RRTConnect(sampler=sampler, expand_dis=0.1) + start = RobotState(position=Point(1, 1)) + goal = RobotState(position=Point(10, 10)) + + for obstacles in general_obstacles_list: + poly.update(obstacles) + path, _ = my_tree.plan(start, goal, poly) + + # from gennav.utils.visualisation import visualize_node + + # node_list = _["node_list"] + # visualize_node(node_list, poly) + + # from gennav.envs.common import visualize_path + + # visualize_path(path, poly) + + assert poly.get_traj_status(path) is True + + +# if __name__ == "__main__": +# test_rrt_connect() \ No newline at end of file From 24cbc29773be94462211bae22fe67486a73e075d Mon Sep 17 00:00:00 2001 From: Harshal Deshpande Date: Sat, 24 Oct 2020 18:11:38 +0530 Subject: [PATCH 2/2] added docs --- docs/source/gennav.planners.rrt.rst | 8 ++++++++ gennav/planners/rrt/rrt_connect.py | 24 +++++++++++++++++------- tests/__init__.py | 2 +- tests/test_planners/rrt_connect_test.py | 14 ++++++++++---- 4 files changed, 36 insertions(+), 12 deletions(-) diff --git a/docs/source/gennav.planners.rrt.rst b/docs/source/gennav.planners.rrt.rst index 514ed08..0361583 100644 --- a/docs/source/gennav.planners.rrt.rst +++ b/docs/source/gennav.planners.rrt.rst @@ -28,6 +28,14 @@ gennav.planners.rrt.rrt module :undoc-members: :show-inheritance: +gennav.planners.rrt.rrt\_connect module +--------------------------------------- + +.. automodule:: gennav.planners.rrt.rrt_connect + :members: + :undoc-members: + :show-inheritance: + gennav.planners.rrt.rrtstar module ---------------------------------- diff --git a/gennav/planners/rrt/rrt_connect.py b/gennav/planners/rrt/rrt_connect.py index b25c674..9ce32c6 100644 --- a/gennav/planners/rrt/rrt_connect.py +++ b/gennav/planners/rrt/rrt_connect.py @@ -1,5 +1,4 @@ import numpy as np - from gennav.planners import Planner from gennav.utils import RobotState, Trajectory from gennav.utils.common import Node @@ -14,8 +13,8 @@ class RRTConnect(Planner): """RRTConnect Planner Class. - Drawback: A lot of nearest neighbour searches are performed. - Should use kd-trees to optimize this + Drawback: A lot of nearest neighbour searches are performed. + Should use kd-trees to optimize this. Args: sampler (gennav.utils.sampler.Sampler): sampler to get random states @@ -30,6 +29,13 @@ def __init__(self, sampler, expand_dis=0.1, *args, **kwargs): def expand_tree(self, node_list, target_state, env): """ A function to expand the tree in the direction of target node + Args: + node_list (list): Starting state + target_state (gennav.utils.RobotState): Target state + env (gennav.envs.Environment): Base class for an envrionment. + Returns: + gennav.utils.common.Node: new_node towards the target node + bool: Boolean to determine if the new_state is valid or not """ distance_list = [ compute_distance(node.state.position, target_state.position) @@ -71,10 +77,14 @@ def expand_tree(self, node_list, target_state, env): def plan(self, start, goal, env): """ - 1. expand 1 tree. - 2. make the new vertex target for the other tree. expand the - other tree until met an obstacle - + Plans path from start to goal avoiding obstacles. + Args: + start (gennav.utils.RobotState): Starting state + end (gennav.utils.RobotState): Goal state + env (gennav.envs.Environment): Base class for an envrionment. + Returns: + gennav.utils.Trajectory: The planned path as trajectory + dict: Dictionary containing additional information """ # Check if start and goal states are obstacle free if not env.get_status(start): diff --git a/tests/__init__.py b/tests/__init__.py index cb381c4..366ee51 100644 --- a/tests/__init__.py +++ b/tests/__init__.py @@ -1,4 +1,4 @@ from tests.test_graph_search import astar_test # noqa: F401 from tests.test_planners import prm_test # noqa: F401 -from tests.test_planners import rrt_test # noqa: F401 from tests.test_planners import rrt_connect_test # noqa: F401 +from tests.test_planners import rrt_test # noqa: F401 diff --git a/tests/test_planners/rrt_connect_test.py b/tests/test_planners/rrt_connect_test.py index 8f52a86..c08ba10 100644 --- a/tests/test_planners/rrt_connect_test.py +++ b/tests/test_planners/rrt_connect_test.py @@ -4,6 +4,9 @@ from gennav.utils.geometry import Point from gennav.utils.samplers import UniformRectSampler +# import timeit +# import time + def test_rrt_connect(): general_obstacles_list = [ @@ -22,19 +25,22 @@ def test_rrt_connect(): for obstacles in general_obstacles_list: poly.update(obstacles) + + # start_time = timeit.default_timer() + path, _ = my_tree.plan(start, goal, poly) - # from gennav.utils.visualisation import visualize_node + # total_time = timeit.default_timer() - start_time + # print("RRTConnect took {} s to build the tree".format(total_time)) + # from gennav.utils.visualisation import visualize_node # node_list = _["node_list"] # visualize_node(node_list, poly) - # from gennav.envs.common import visualize_path - # visualize_path(path, poly) assert poly.get_traj_status(path) is True # if __name__ == "__main__": -# test_rrt_connect() \ No newline at end of file +# test_rrt_connect()