From c1ba9bab0a48100aa3f81286a2015c5112bb1f9f Mon Sep 17 00:00:00 2001 From: Francisco Moretti Date: Thu, 17 Oct 2019 09:09:54 -0300 Subject: [PATCH] fix check segment intersection with obstacle The previous method only worked because the step that checked collision from a point of the line to obstacles was the same as the minimum obstacle radius. If the obstacle radius is very small a great amount of steps would be required. Thus It's better to check the distance from the segment to the obstacles directly and compare with obstacle radius Now there is no need to have two check functions. --- .../InformedRRTStar/informed_rrt_star.py | 52 ++++++++++--------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 4a32801d..b9769bea 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -74,10 +74,9 @@ class InformedRRTStar: newNode = self.get_new_node(theta, nind, nearestNode) d = self.line_cost(nearestNode, newNode) - isCollision = self.collision_check(newNode, self.obstacle_list) - isCollisionEx = self.check_collision_extend(nearestNode, theta, d) + isCollision = self.check_collision(nearestNode, theta, d) - if isCollision and isCollisionEx: + if isCollision: nearInds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearInds) @@ -110,7 +109,7 @@ class InformedRRTStar: dy = newNode.y - self.node_list[i].y d = math.sqrt(dx ** 2 + dy ** 2) theta = math.atan2(dy, dx) - if self.check_collision_extend(self.node_list[i], theta, d): + if self.check_collision(self.node_list[i], theta, d): dList.append(self.node_list[i].cost + d) else: dList.append(float('inf')) @@ -194,17 +193,6 @@ class InformedRRTStar: minIndex = dList.index(min(dList)) return minIndex - @staticmethod - def collision_check(newNode, obstacleList): - for (ox, oy, size) in obstacleList: - dx = ox - newNode.x - dy = oy - newNode.y - d = dx * dx + dy * dy - if d <= 1.1 * size ** 2: - return False # collision - - return True # safe - def get_new_node(self, theta, nind, nearestNode): newNode = copy.deepcopy(nearestNode) @@ -234,19 +222,35 @@ class InformedRRTStar: if nearNode.cost > scost: theta = math.atan2(newNode.y - nearNode.y, newNode.x - nearNode.x) - if self.check_collision_extend(nearNode, theta, d): + if self.check_collision(nearNode, theta, d): nearNode.parent = n_node - 1 nearNode.cost = scost + + @staticmethod + def distance_squared_point_to_segment(v, w, p): + # Return minimum distance between line segment vw and point p + if (np.array_equal(v, w)): + return (p-v).dot(p-v) # v == w case + l2 = (w-v).dot(w-v) # i.e. |w-v|^2 - avoid a sqrt + # Consider the line extending the segment, parameterized as v + t (w - v). + # We find projection of point p onto the line. + # It falls where t = [(p-v) . (w-v)] / |w-v|^2 + # We clamp t from [0,1] to handle points outside the segment vw. + t = max(0, min(1, (p - v).dot(w - v) / l2)) + projection = v + t * (w - v) # Projection falls on the segment + return (p-projection).dot(p-projection) - def check_collision_extend(self, nearNode, theta, d): + def check_collision(self, nearNode, theta, d): tmpNode = copy.deepcopy(nearNode) - - for i in range(int(d / self.expand_dis)): - tmpNode.x += self.expand_dis * math.cos(theta) - tmpNode.y += self.expand_dis * math.sin(theta) - if not self.collision_check(tmpNode, self.obstacle_list): - return False - + endx = tmpNode.x + math.cos(theta)*d + endy = tmpNode.y + math.sin(theta)*d + for (ox, oy, size) in self.obstacle_list: + dd = self.distance_squared_point_to_segment( + np.array([tmpNode.x, tmpNode.y]), + np.array([endx, endy]), + np.array([ox, oy])) + if dd <= 1.1 * size**2: + return False # collision return True def get_final_course(self, lastIndex):