added rrt_connect

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
 ----------------------------------
 

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

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

gennav/planners/rrt/rrt_connect.py

@@ -0,0 +1,228 @@
+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
+        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)
+            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):
+        """
+        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):
+            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)

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_connect_test  # noqa: F401
 from tests.test_planners import rrt_test  # noqa: F401

tests/test_planners/rrt_connect_test.py

@@ -0,0 +1,46 @@
+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
+
+# import timeit
+# import time
+
+
+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)
+
+        # start_time = timeit.default_timer()
+
+        path, _ = my_tree.plan(start, goal, poly)
+
+        # 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()