From c1ba9bab0a48100aa3f81286a2015c5112bb1f9f Mon Sep 17 00:00:00 2001 From: Francisco Moretti Date: Thu, 17 Oct 2019 09:09:54 -0300 Subject: [PATCH 1/4] 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): From 3f14311cc54cb071b530a03b7a0245a61987c300 Mon Sep 17 00:00:00 2001 From: Francisco Moretti Date: Sun, 3 Nov 2019 12:07:56 -0300 Subject: [PATCH 2/4] correct variable name --- PathPlanning/InformedRRTStar/informed_rrt_star.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index b9769bea..98a239ab 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -74,9 +74,9 @@ class InformedRRTStar: newNode = self.get_new_node(theta, nind, nearestNode) d = self.line_cost(nearestNode, newNode) - isCollision = self.check_collision(nearestNode, theta, d) + noCollision = self.check_collision(nearestNode, theta, d) - if isCollision: + if noCollision: nearInds = self.find_near_nodes(newNode) newNode = self.choose_parent(newNode, nearInds) From 178dca3e343749b9e3ec3d1fda7a89f464532cda Mon Sep 17 00:00:00 2001 From: Francisco Moretti Date: Sun, 3 Nov 2019 12:12:53 -0300 Subject: [PATCH 3/4] add collision check to node near to goal --- .../InformedRRTStar/informed_rrt_star.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 98a239ab..a8352bc0 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -84,6 +84,7 @@ class InformedRRTStar: self.rewire(newNode, nearInds) if self.is_near_goal(newNode): + if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y): solutionSet.add(newNode) lastIndex = len(self.node_list) - 1 tempPath = self.get_final_course(lastIndex) @@ -240,18 +241,22 @@ class InformedRRTStar: projection = v + t * (w - v) # Projection falls on the segment return (p-projection).dot(p-projection) + def check_segment_collision(self, x1, y1, x2, y2): + for (ox, oy, size) in self.obstacle_list: + dd = self.distance_squared_point_to_segment( + np.array([x1, y1]), + np.array([x2, y2]), + np.array([ox, oy])) + if dd <= size**2: + return False # collision + return True + + def check_collision(self, nearNode, theta, d): tmpNode = copy.deepcopy(nearNode) 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 + return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy) def get_final_course(self, lastIndex): path = [[self.goal.x, self.goal.y]] From 3e5cad819247edd892ae7f8f0192170e8a4c31bd Mon Sep 17 00:00:00 2001 From: Francisco Moretti Date: Sun, 3 Nov 2019 12:23:48 -0300 Subject: [PATCH 4/4] fix indentation --- PathPlanning/InformedRRTStar/informed_rrt_star.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index a8352bc0..62a94064 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -85,13 +85,13 @@ class InformedRRTStar: if self.is_near_goal(newNode): if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y): - solutionSet.add(newNode) - lastIndex = len(self.node_list) - 1 - tempPath = self.get_final_course(lastIndex) - tempPathLen = self.get_path_len(tempPath) - if tempPathLen < pathLen: - path = tempPath - cBest = tempPathLen + solutionSet.add(newNode) + lastIndex = len(self.node_list) - 1 + tempPath = self.get_final_course(lastIndex) + tempPathLen = self.get_path_len(tempPath) + if tempPathLen < pathLen: + path = tempPath + cBest = tempPathLen if animation: self.draw_graph(xCenter=xCenter,