diff --git a/Sampling-based Planning/gif/RRT_STAR_2D.gif b/Sampling-based Planning/gif/RRT_STAR_2D.gif new file mode 100644 index 0000000..c4ddfcb Binary files /dev/null and b/Sampling-based Planning/gif/RRT_STAR_2D.gif differ diff --git a/Sampling-based Planning/rrt_2D/informed_rrt_star.py b/Sampling-based Planning/rrt_2D/informed_rrt_star.py new file mode 100644 index 0000000..891d9fc --- /dev/null +++ b/Sampling-based Planning/rrt_2D/informed_rrt_star.py @@ -0,0 +1,211 @@ +""" +INFORMED_RRT_STAR 2D +@author: huiming zhou +""" + +import os +import sys +import math +import random +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling-based Planning/") + +from rrt_2D import env +from rrt_2D import plotting +from rrt_2D import utils +from rrt_2D import queue + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.parent = None + + +class IRrtStar: + def __init__(self, x_start, x_goal, step_len, + goal_sample_rate, search_radius, iter_max): + self.x_start = Node(x_start) + self.x_goal = Node(x_goal) + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.search_radius = search_radius + self.iter_max = iter_max + + self.env = env.Env() + self.plotting = plotting.Plotting(x_start, x_goal) + self.utils = utils.Utils() + + # self.fig, self.ax = plt.subplots() + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.V = [self.x_start] + self.X_soln = set() + self.path = None + + def planning(self): + c_best = np.inf + c_min = self.Line(self.x_start, self.x_goal) + x_center = np.array([[(self.x_start.x + self.x_goal.x) / 2.0], + [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]]) + a1 = np.array([[(self.x_goal.x - self.x_start.x) / c_min], + [(self.x_goal.y - self.x_start.y) / c_min], [0.0]]) + e_theta = math.atan2(a1[1], a1[0]) + id1_t = np.array([[1.0, 0.0, 0.0]]) + M = a1 @ id1_t + U, S, Vh = np.linalg.svd(M, True, True) + C = np.dot(np.dot(U, np.diag( + [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + + for k in range(self.iter_max): + if k % 500 == 0: + print(k) + + if self.X_soln: + c_best = min([self.Cost(x) for x in self.X_soln]) + + x_rand = self.Sample(self.x_start, self.x_goal, c_best, x_center, C) + x_nearest = self.Nearest(self.V, x_rand) + x_new = self.Steer(x_nearest, x_rand) + + if x_new and not self.utils.is_collision(x_nearest, x_new): + self.V.append(x_new) + X_near = self.Near(self.V, x_new) + x_min = x_nearest + c_min = self.Cost(x_min) + self.Line(x_nearest, x_new) + + for x_near in X_near: + c_new = self.Cost(x_near) + self.Line(x_near, x_new) + if c_new < c_min: + x_new.parent = x_near + c_min = c_new + + for x_near in X_near: + c_near = self.Cost(x_near) + c_new = self.Cost(x_new) + self.Line(x_new, x_near) + if c_new < c_near: + x_near.parent = x_new + + if self.InGoalRegion(x_new): + self.X_soln.add(x_new) + + path = self.ExtractPath(self.V[-1]) + self.plotting.animation(self.V, path, "Informed rrt*") + + def ExtractPath(self, node): + path = [[self.x_goal.x, self.x_goal.y]] + + while node.parent: + path.append([node.x, node.y]) + node = node.parent + + path.append([self.x_start.x, self.x_start.y]) + + return path + + def InGoalRegion(self, node): + if self.Line(node, self.x_goal) < self.step_len: + return True + + return False + + def Steer(self, x_start, x_goal): + dist, theta = self.get_distance_and_angle(x_start, x_goal) + dist = min(self.step_len, dist) + node_new = Node((x_start.x + dist * math.cos(theta), + x_start.y + dist * math.sin(theta))) + node_new.parent = x_start + + return node_new + + def Near(self, nodelist, node): + n = len(nodelist) + 1 + r = 2 * self.search_radius * math.sqrt((math.log(n) / n)) + + dist_table = [math.hypot(nd.x - node.x, nd.y - node.y) for nd in nodelist] + X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r and + not self.utils.is_collision(node, nodelist[ind])] + + return X_near + + def Sample(self, x_start, x_goal, c_max, x_center, C): + if c_max < np.inf: + c_min = self.Line(x_start, x_goal) + r = [c_max / 2.0, + math.sqrt(c_max ** 2 - c_min ** 2) / 2.0, + math.sqrt(c_max ** 2 - c_min ** 2) / 2.0] + L = np.diag(r) + x_ball = self.SampleUnitNBall() + x_rand = np.dot(np.dot(C, L), x_ball) + x_center + else: + x_rand = self.SampleFreeSpace() + + return x_rand + + def SampleFreeSpace(self): + delta = self.utils.delta + + if np.random.random() > self.goal_sample_rate: + return [np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)] + + return [self.x_goal.x, self.x_goal.y] + + @staticmethod + def SampleUnitNBall(): + x, y = random.random(), random.random() + + if y < x: + x, y = y, x + + sample = np.array([[y * math.cos(2 * math.pi * x / y)], + [y * math.sin(2 * math.pi * x / y)], [0.0]]) + + return sample + + @staticmethod + def Nearest(nodelist, n): + return nodelist[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) + for nd in nodelist]))] + + @staticmethod + def Line(x_start, x_goal): + return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) + + @staticmethod + def Cost(node_p): + node = node_p + cost = 0.0 + + while node.parent: + cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) + node = node.parent + + return cost + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + +def main(): + x_start = (2, 2) # Starting node + x_goal = (49, 24) # Goal node + + rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 4000) + rrt_star.planning() + + +if __name__ == '__main__': + main() diff --git a/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py b/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py index e69de29..3aa2b41 100644 --- a/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py +++ b/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py @@ -0,0 +1,134 @@ +""" +Optimal_Bidirectional_RRT 2D +@author: huiming zhou +""" + +import os +import sys +import math +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling-based Planning/") + +from rrt_2D import env +from rrt_2D import plotting +from rrt_2D import utils +from rrt_2D import queue + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.parent = None + + +class OBiRrt: + def __init__(self, x_start, x_goal, step_len, + goal_sample_rate, search_radius, iter_max): + self.x_init = Node(x_start) + self.x_goal = Node(x_goal) + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.search_radius = search_radius + self.iter_max = iter_max + + self.Ta = [self.x_init] + self.Tb = [self.x_goal] + self.c_best = np.inf + self.sigma_best = {} + self.path = [] + self.visited = [] + + self.env = env.Env() + self.plotting = plotting.Plotting(x_start, x_goal) + self.utils = utils.Utils() + # self.fig, self.ax = plt.subplots() + + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + def planning(self): + for k in range(self.iter_max): + x_rand = self.generate_random_node(self.x_goal, self.goal_sample_rate) + x_nearest = self.nearest_neighbor(self.Ta, x_rand) + x_new = self.steer(x_nearest, x_rand) + X_near_ind = self.nearest_neighbor(self.Ta, x_new) + L_near = [] + + for x_near_ind in X_near_ind: + x_near = self.Ta[x_near_ind] + sigma_near = self.steer(x_near, x_new) + c_near = self.cost(x_near) + self.cost(sigma_near) + L_near.append((c_near, x_near, sigma_near)) + + L_near.sort() + + for c_near, x_near, sigma_near in L_near: + if c_near + self.cost_to_go() + + + def cost_to_go(self, x_start, x_goal): + return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) + + def generate_random_node(self, goal_point, goal_sample_rate): + delta = self.utils.delta + + if np.random.random() > goal_sample_rate: + return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) + + return goal_point + + @staticmethod + def nearest_neighbor(V, node): + return V[int(np.argmin([math.hypot(nd.x - node.x, nd.y - node.y) for nd in V]))] + + def steer(self, node_start, node_goal): + dist, theta = self.get_distance_and_angle(node_start, node_goal) + + dist = min(self.step_len, dist) + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) + + node_new.parent = node_start + + return node_new + + def find_near_neighbor(self, V, node): + n = len(V) + 1 + r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) + + dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in V] + dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and + not self.utils.is_collision(node, V[ind])] + + return dist_table_index + + def get_new_cost(self, node_start, node_end): + dist, _ = self.get_distance_and_angle(node_start, node_end) + + return self.cost(node_start) + dist + + @staticmethod + def cost(node_p): + node = node_p + cost = 0.0 + + while node.parent: + cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) + node = node.parent + + return cost + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py index 90735f4..1a9db94 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -218,7 +218,7 @@ class RrtStar: continue for node_c in node.child: - node_c.cost = self.get_new_cost(node, node_c) + node_c.Cost = self.get_new_cost(node, node_c) OPEN.put(node_c) def extract_path(self, node_end): diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index e22e781..61b457c 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -66,9 +66,9 @@ class rrtstar(): Xnear = near(self,xnew) self.V.append(xnew) # add point # visualization(self) - # minimal path and minimal cost + # minimal path and minimal Cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) - # connecting along minimal cost path + # connecting along minimal Cost path for xnear in Xnear: xnear = tuple(xnear) c1 = cost(self, xnear) + getDist(xnew, xnear) diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 107e16a..0adab8f 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -199,7 +199,7 @@ def steer(initparams, x, y): return xnew def cost(initparams, x): - '''here use the additive recursive cost function''' + '''here use the additive recursive Cost function''' if x == initparams.x0: return 0 return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x]) diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py index 58e3c12..0867f89 100644 --- a/Search-based Planning/Search_2D/ARAstar.py +++ b/Search-based Planning/Search_2D/ARAstar.py @@ -24,7 +24,7 @@ class AraStar: self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles self.e = e # initial weight - self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come self.OPEN = {self.s_start: self.fvalue(self.s_start)} # priority queue / OPEN set self.CLOSED = set() # CLOSED set @@ -155,11 +155,11 @@ class AraStar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/Anytime_D_star.py b/Search-based Planning/Search_2D/Anytime_D_star.py index c82b588..4cebb48 100644 --- a/Search-based Planning/Search_2D/Anytime_D_star.py +++ b/Search-based Planning/Search_2D/Anytime_D_star.py @@ -217,11 +217,11 @@ class ADStar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/Astar.py b/Search-based Planning/Search_2D/Astar.py index 8d2361c..444a4d4 100644 --- a/Search-based Planning/Search_2D/Astar.py +++ b/Search-based Planning/Search_2D/Astar.py @@ -25,7 +25,7 @@ class Astar: self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come self.OPEN = queue.QueuePrior() # priority queue / OPEN set self.OPEN.put(self.s_start, self.fvalue(self.s_start)) self.CLOSED = [] # CLOSED set / VISITED order @@ -48,7 +48,7 @@ class Astar: new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = float("inf") - if new_cost < self.g[s_n]: # conditions for updating cost + if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s self.OPEN.put(s_n, self.fvalue(s_n)) @@ -99,7 +99,7 @@ class Astar: new_cost = g[s] + self.cost(s, s_n) if s_n not in g: g[s_n] = float("inf") - if new_cost < g[s_n]: # conditions for updating cost + if new_cost < g[s_n]: # conditions for updating Cost g[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n)) @@ -122,11 +122,11 @@ class Astar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): @@ -153,7 +153,7 @@ class Astar: def fvalue(self, x): """ - f = g + h. (g: cost to come, h: heuristic function) + f = g + h. (g: Cost to come, h: heuristic function) :param x: current state :return: f """ diff --git a/Search-based Planning/Search_2D/Bidirectional_a_star.py b/Search-based Planning/Search_2D/Bidirectional_a_star.py index 4f01dfd..8ffafc1 100644 --- a/Search-based Planning/Search_2D/Bidirectional_a_star.py +++ b/Search-based Planning/Search_2D/Bidirectional_a_star.py @@ -25,8 +25,8 @@ class BidirectionalAstar: self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # cost to come: from s_start - self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # cost to come: form s_goal + self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come: from x_start + self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # Cost to come: form x_goal self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching self.OPEN_fore.put(self.s_start, @@ -142,11 +142,11 @@ class BidirectionalAstar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/D_star.py b/Search-based Planning/Search_2D/D_star.py index 792620a..23522e4 100644 --- a/Search-based Planning/Search_2D/D_star.py +++ b/Search-based Planning/Search_2D/D_star.py @@ -177,11 +177,11 @@ class Dstar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py index ea10baf..6277b68 100644 --- a/Search-based Planning/Search_2D/D_star_Lite.py +++ b/Search-based Planning/Search_2D/D_star_Lite.py @@ -150,11 +150,11 @@ class DStar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/Dijkstra.py b/Search-based Planning/Search_2D/Dijkstra.py index 1d98611..9b4d4fb 100644 --- a/Search-based Planning/Search_2D/Dijkstra.py +++ b/Search-based Planning/Search_2D/Dijkstra.py @@ -25,7 +25,7 @@ class Dijkstra: self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles - self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come + self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come self.OPEN = queue.QueuePrior() # priority queue / OPEN set self.OPEN.put(self.s_start, 0) self.CLOSED = [] # closed set & visited @@ -89,11 +89,11 @@ class Dijkstra: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py index 70e816a..92b2656 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search-based Planning/Search_2D/LPAstar.py @@ -149,11 +149,11 @@ class LpaStar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py index 7363f0e..f3594cb 100644 --- a/Search-based Planning/Search_2D/LRTAstar.py +++ b/Search-based Planning/Search_2D/LRTAstar.py @@ -50,7 +50,7 @@ class LrtAstarN: for x in h_value: self.h_table[x] = h_value[x] - s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in OPEN set + s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_start -> expected node in OPEN set self.path.append(path_k) def extract_path_in_CLOSE(self, s_start, h_value): @@ -95,7 +95,7 @@ class LrtAstarN: OPEN = queue.QueuePrior() # OPEN set OPEN.put(x_start, self.h(x_start)) CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come + g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come PARENT = {x_start: x_start} # relations count = 0 # counter @@ -113,7 +113,7 @@ class LrtAstarN: new_cost = g_table[s] + self.cost(s, s_n) if s_n not in g_table: g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating cost + if new_cost < g_table[s_n]: # conditions for updating Cost g_table[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) @@ -177,11 +177,11 @@ class LrtAstarN: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py index 3575eac..b73be99 100644 --- a/Search-based Planning/Search_2D/RTAAstar.py +++ b/Search-based Planning/Search_2D/RTAAstar.py @@ -90,7 +90,7 @@ class RtaAstar: OPEN = queue.QueuePrior() # OPEN set OPEN.put(x_start, self.h_table[x_start]) CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come + g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come PARENT = {x_start: x_start} # relations count = 0 # counter @@ -108,7 +108,7 @@ class RtaAstar: new_cost = g_table[s] + self.cost(s, s_n) if s_n not in g_table: g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating cost + if new_cost < g_table[s_n]: # conditions for updating Cost g_table[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) @@ -186,11 +186,11 @@ class RtaAstar: def cost(self, s_start, s_goal): """ - Calculate cost for this motion + Calculate Cost for this motion :param s_start: starting node :param s_goal: end node - :return: cost for this motion - :note: cost function could be more complicate! + :return: Cost for this motion + :note: Cost function could be more complicate! """ if self.is_collision(s_start, s_goal): diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 43c0307..42075c6 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -41,7 +41,7 @@ class Anytime_Dstar(object): # init children set: self.CHILDREN = {} - # init cost set + # init Cost set self.COST = defaultdict(lambda: defaultdict(dict)) # for visualization @@ -88,7 +88,7 @@ class Anytime_Dstar(object): return self.rhs[xi] def updatecost(self, range_changed=None, new=None, old=None, mode=False): - # scan graph for changed cost, if cost is changed update it + # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if isinbound(old, xi, mode) or isinbound(new, xi, mode): @@ -100,8 +100,8 @@ class Anytime_Dstar(object): return CHANGED # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False): - # # TODO scan graph for changed cost, if cost is changed update it - # # make the graph cost via vectorization + # # TODO scan graph for changed Cost, if Cost is changed update it + # # make the graph Cost via vectorization # CHANGED = set() # Allnodes = np.array(list(self.CLOSED)) # isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \ @@ -112,7 +112,7 @@ class Anytime_Dstar(object): # CHANGED.add(xi) # self.CHILDREN[xi] = set(children(self, xi)) # for xj in self.CHILDREN: - # self.COST[xi][xj] = cost(self, xi, xj) + # self.COST[xi][xj] = Cost(self, xi, xj) # --------------main functions for Anytime D star @@ -177,7 +177,7 @@ class Anytime_Dstar(object): # islargelychanged = True self.Path = [] - # update cost with changed environment + # update Cost with changed environment if ischanged: # CHANGED = self.updatecost(True, new2, old2, mode='obb') CHANGED = self.updatecost(True, new2, old2) @@ -207,10 +207,10 @@ class Anytime_Dstar(object): def path(self, s_start=None): '''After ComputeShortestPath() - returns, one can then follow a shortest path from s_start to - s_goal by always moving from the current vertex s, starting - at s_start. , to any successor s' that minimizes c(s,s') + g(s') - until s_goal is reached (ties can be broken arbitrarily).''' + returns, one can then follow a shortest path from x_start to + x_goal by always moving from the current vertex s, starting + at x_start. , to any successor s' that minimizes c(s,s') + g(s') + until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt s = self.x0 diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 09d4cea..2953e16 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -175,7 +175,7 @@ class D_star(object): sparent = self.b[self.x0] else: sparent = self.b[s] - # if there is a change of cost, or a collision. + # if there is a change of Cost, or a collision. if cost(self, s, sparent) == np.inf: self.modify(s) continue diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index d328f64..278fb60 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -41,7 +41,7 @@ class D_star_Lite(object): # init children set: self.CHILDREN = {} - # init cost set + # init Cost set self.COST = defaultdict(lambda: defaultdict(dict)) # for visualization @@ -51,7 +51,7 @@ class D_star_Lite(object): self.done = False def updatecost(self, range_changed=None, new=None, old=None, mode=False): - # scan graph for changed cost, if cost is changed update it + # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if isinbound(old, xi, mode) or isinbound(new, xi, mode): @@ -100,7 +100,7 @@ class D_star_Lite(object): def UpdateVertex(self, u): # if still in the hunt - if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal + if not getDist(self.xt, u) <= self.env.resolution: # originally: u != x_goal if u in self.CHILDREN and len(self.CHILDREN[u]) == 0: self.rhs[u] = np.inf else: @@ -147,7 +147,7 @@ class D_star_Lite(object): ischanged = False self.V = set() while getDist(self.x0, self.xt) > 2*self.env.resolution: - #---------------------------------- at specific times, the environment is changed and cost is updated + #---------------------------------- at specific times, the environment is changed and Cost is updated if t % 2 == 0: new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation') new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation') @@ -163,9 +163,9 @@ class D_star_Lite(object): self.x0 = children_new[np.argmin([self.getcost(self.x0,s_p) + self.getg(s_p) for s_p in children_new])] # TODO add the moving robot position codes self.env.start = self.x0 - # ---------------------------------- if any cost changed, update km, reset slast, + # ---------------------------------- if any Cost changed, update km, reset slast, # for all directed edgees (u,v) with chaged edge costs, - # update the edge cost c(u,v) and update vertex u. then replan + # update the edge Cost c(u,v) and update vertex u. then replan if ischanged: self.km += heuristic_fun(self, self.x0, s_last) s_last = self.x0 @@ -186,10 +186,10 @@ class D_star_Lite(object): def path(self, s_start=None): '''After ComputeShortestPath() - returns, one can then follow a shortest path from s_start to - s_goal by always moving from the current vertex s, starting - at s_start. , to any successor s' that minimizes c(s,s') + g(s') - until s_goal is reached (ties can be broken arbitrarily).''' + returns, one can then follow a shortest path from x_start to + x_goal by always moving from the current vertex s, starting + at x_start. , to any successor s' that minimizes c(s,s') + g(s') + until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt if not s_start: diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index a4df422..497fbb1 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/Search-based Planning/Search_3D/LP_Astar3D.py @@ -48,7 +48,7 @@ class Lifelong_Astar(object): self.CHILDREN = {} self.getCHILDRENset() - # initialize cost list + # initialize Cost list self.COST = {} _ = self.costset() @@ -58,7 +58,7 @@ class Lifelong_Astar(object): children = self.CHILDREN[xi] toUpdate = [self.cost(xj,xi) for xj in children] if xi in self.COST: - # if the old cost not equal to new cost + # if the old Cost not equal to new Cost diff = np.not_equal(self.COST[xi],toUpdate) cd = np.array(children)[diff] for i in cd: @@ -126,7 +126,7 @@ class Lifelong_Astar(object): j = x nei = self.CHILDREN[x] gset = [self.g[xi] for xi in nei] - # collision check and make g cost inf + # collision check and make g Cost inf for i in range(len(nei)): if self.isCollide(nei[i],j)[0]: gset[i] = np.inf diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index b421f35..116282f 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -336,7 +336,7 @@ def cost(initparams, i, j, dist=None, settings='Euclidean'): def initcost(initparams): - # initialize cost dictionary, could be modifed lateron + # initialize Cost dictionary, could be modifed lateron c = defaultdict(lambda: defaultdict(dict)) # two key dicionary for xi in initparams.X: cdren = children(initparams, xi)