Fix motion-planning#9, Fix motion-planning#6

src/rrt/rrt_base.py

@@ -3,7 +3,7 @@
 import numpy as np
 
 from src.rrt.tree import Tree
-from src.utilities.geometry import dist_between_points
+from src.utilities.geometry import steer
 
 
 class RRTBase(object):
@@ -24,10 +24,8 @@ def __init__(self, X, Q, x_init, x_goal, max_samples, r, prc=0.01):
         self.Q = Q
         self.r = r
         self.prc = prc
-
         self.x_init = x_init
         self.x_goal = x_goal
-
         self.trees = []  # list of all trees
         self.add_tree()  # add initial tree
 
@@ -64,7 +62,7 @@ def nearby(self, tree, x, n):
         :param n: int, max number of neighbors to return
         :return: list of nearby vertices
         """
-        return list(self.trees[tree].V.nearest(x, num_results=n, objects="raw"))
+        return self.trees[tree].V.nearest(x, num_results=n, objects="raw")
 
     def get_nearest(self, tree, x):
         """
@@ -73,7 +71,7 @@ def get_nearest(self, tree, x):
         :param x: tuple, vertex around which searching
         :return: tuple, nearest vertex to x
         """
-        return self.nearby(tree, x, 1)[0]
+        return next(self.nearby(tree, x, 1))
 
     def new_and_near(self, tree, q):
         """
@@ -84,13 +82,11 @@ def new_and_near(self, tree, q):
         """
         x_rand = self.X.sample_free()
         x_nearest = self.get_nearest(tree, x_rand)
-        x_new = self.steer(x_nearest, x_rand, q[0])
+        x_new = self.bound_point(steer(x_nearest, x_rand, q[0]))
         # check if new point is in X_free and not already in V
         if not self.trees[0].V.count(x_new) == 0 or not self.X.obstacle_free(x_new):
             return None, None
-
         self.samples_taken += 1
-
         return x_new, x_nearest
 
     def connect_to_point(self, tree, x_a, x_b):
@@ -104,9 +100,7 @@ def connect_to_point(self, tree, x_a, x_b):
         if self.trees[tree].V.count(x_b) == 0 and self.X.collision_free(x_a, x_b, self.r):
             self.add_vertex(tree, x_b)
             self.add_edge(tree, x_b, x_a)
-
             return True
-
         return False
 
     def can_connect_to_goal(self, tree):
@@ -119,10 +113,8 @@ def can_connect_to_goal(self, tree):
         if self.x_goal in self.trees[tree].E and x_nearest in self.trees[tree].E[self.x_goal]:
             # tree is already connected to goal using nearest vertex
             return True
-
         if self.X.collision_free(x_nearest, self.x_goal, self.r):  # check if obstacle-free
             return True
-
         return False
 
     def get_path(self):
@@ -133,11 +125,8 @@ def get_path(self):
         if self.can_connect_to_goal(0):
             print("Can connect to goal")
             self.connect_to_goal(0)
-
             return self.reconstruct_path(0, self.x_init, self.x_goal)
-
         print("Could not connect to goal")
-
         return None
 
     def connect_to_goal(self, tree):
@@ -149,36 +138,6 @@ def connect_to_goal(self, tree):
         x_nearest = self.get_nearest(tree, self.x_goal)
         self.trees[tree].E[self.x_goal] = x_nearest
 
-    def steer(self, start, goal, distance):
-        """
-        Return a point in the direction of the goal, that is distance away from start
-        :param start: start location
-        :param goal: goal location
-        :param distance: distance away from start
-        :return: point in the direction of the goal, distance away from start
-        """
-        ab = np.empty(len(start), np.float)  # difference between start and goal
-        for i, (start_i, goal_i) in enumerate(zip(start, goal)):
-            ab[i] = goal_i - start_i
-
-        ab = tuple(ab)
-        zero_vector = tuple(np.zeros(len(ab)))
-
-        ba_length = dist_between_points(zero_vector, ab)  # get length of vector ab
-        unit_vector = np.fromiter((i / ba_length for i in ab), np.float, len(ab))
-        # scale vector to desired length
-        scaled_vector = np.fromiter((i * distance for i in unit_vector), np.float, len(unit_vector))
-        steered_point = np.add(start, scaled_vector)  # add scaled vector to starting location for final point
-
-        # if point is out-of-bounds, set to bound
-        for dim, dim_range in enumerate(self.X.dimension_lengths):
-            if steered_point[dim] < dim_range[0]:
-                steered_point[dim] = dim_range[0]
-            elif steered_point[dim] > dim_range[1]:
-                steered_point[dim] = dim_range[1]
-
-        return tuple(steered_point)
-
     def reconstruct_path(self, tree, x_init, x_goal):
         """
         Reconstruct path from start to goal
@@ -196,7 +155,6 @@ def reconstruct_path(self, tree, x_init, x_goal):
             current = self.trees[tree].E[current]
         path.append(x_init)
         path.reverse()
-
         return path
 
     def check_solution(self):
@@ -206,9 +164,13 @@ def check_solution(self):
             path = self.get_path()
             if path is not None:
                 return True, path
-
         # check if can connect to goal after generating max_samples
         if self.samples_taken >= self.max_samples:
             return True, self.get_path()
-
         return False, None
+
+    def bound_point(self, point):
+        # if point is out-of-bounds, set to bound
+        point = np.maximum(point, self.X.dimension_lengths[:, 0])
+        point = np.minimum(point, self.X.dimension_lengths[:, 1])
+        return tuple(point)

src/rrt/rrt_connect.py

@@ -3,6 +3,7 @@
 import numpy as np
 
 from src.rrt.rrt_base import RRTBase
+from src.utilities.geometry import steer
 
 
 class Status(enum.Enum):
@@ -44,7 +45,7 @@ def unswap(self):
 
     def extend(self, tree, x_rand):
         x_nearest = self.get_nearest(tree, x_rand)
-        x_new = self.steer(x_nearest, x_rand, self.Q[0])
+        x_new = steer(self.X, x_nearest, x_rand, self.Q[0])
         if self.connect_to_point(tree, x_nearest, x_new):
             if np.abs(np.sum(np.array(x_new) - np.array(x_rand))) < 1e-2:
                 return x_new, Status.REACHED
@@ -78,6 +79,5 @@ def rrt_connect(self):
                     second_part = self.reconstruct_path(1, self.x_goal, self.get_nearest(1, x_new))
                     second_part.reverse()
                     return first_part + second_part
-
             self.swap_trees()
             self.samples_taken += 1

src/search_space/search_space.py

@@ -1,12 +1,10 @@
 # This file is subject to the terms and conditions defined in
 # file 'LICENSE', which is part of this source code package.
-import math
-import random
 
 import numpy as np
 from rtree import index
 
-from src.utilities.geometry import dist_between_points
+from src.utilities.geometry import es_points_along_line
 from src.utilities.obstacle_generation import obstacle_generator
 
 
@@ -66,21 +64,14 @@ def collision_free(self, start, end, r):
         :param r: resolution of points to sample along edge when checking for collisions
         :return: True if line segment does not intersect an obstacle, False otherwise
         """
-        dist = dist_between_points(start, end)
-        # divide line between points into equidistant points at given resolution
-        dim_linspaces = [np.linspace(s_i, e_i, int(math.ceil(dist / r))) for s_i, e_i in zip(start, end)]
-
-        coll_free = all(map(self.obstacle_free, zip(*dim_linspaces)))
-
+        points = es_points_along_line(start, end, r)
+        coll_free = all(map(self.obstacle_free, points))
         return coll_free
 
     def sample(self):
         """
         Return a random location within X
         :return: random location within X (not necessarily X_free)
         """
-        x = np.empty(len(self.dimension_lengths), np.float)
-        for dimension in range(len(self.dimension_lengths)):
-            x[dimension] = random.uniform(self.dimension_lengths[dimension][0], self.dimension_lengths[dimension][1])
-
+        x = np.random.uniform(self.dimension_lengths[:, 0], self.dimension_lengths[:, 1])
         return tuple(x)

src/utilities/geometry.py

@@ -1,9 +1,10 @@
 # This file is subject to the terms and conditions defined in
 # file 'LICENSE', which is part of this source code package.
 
-import math
 from itertools import tee
 
+import numpy as np
+
 
 def dist_between_points(a, b):
     """
@@ -12,14 +13,44 @@ def dist_between_points(a, b):
     :param b: second point
     :return: Euclidean distance between a and b
     """
-    distance = sum(map(lambda a_b: (a_b[0] - a_b[1]) ** 2, zip(a, b)))
-
-    return math.sqrt(distance)
+    distance = np.linalg.norm(np.array(b) - np.array(a))
+    return distance
 
 
 def pairwise(iterable):
     "s -> (s0,s1), (s1,s2), (s2, s3), ..."
     a, b = tee(iterable)
     next(b, None)
-
     return zip(a, b)
+
+
+def es_points_along_line(start, end, r):
+    """
+    Equally-spaced points along a line defined by start, end, with resolution r
+    :param start: starting point
+    :param end: ending point
+    :param r: maximum distance between points
+    :return: yields points along line from start to end, separated by distance r
+    """
+    d = dist_between_points(start, end)
+    n_points = int(np.ceil(d / r))
+    if n_points > 1:
+        step = d / (n_points - 1)
+        for i in range(n_points):
+            next_point = steer(start, end, i * step)
+            yield next_point
+
+
+def steer(start, goal, d):
+    """
+    Return a point in the direction of the goal, that is distance away from start
+    :param start: start location
+    :param goal: goal location
+    :param d: distance away from start
+    :return: point in the direction of the goal, distance away from start
+    """
+    start, end = np.array(start), np.array(goal)
+    v = end - start
+    u = v / (np.sqrt(np.sum(v ** 2)))
+    steered_point = start + u * d
+    return tuple(steered_point)