diff --git a/Sampling_based_Planning/rrt_3D/BIT_star3D.py b/Sampling_based_Planning/rrt_3D/BIT_star3D.py index 229c65f..343ed01 100644 --- a/Sampling_based_Planning/rrt_3D/BIT_star3D.py +++ b/Sampling_based_Planning/rrt_3D/BIT_star3D.py @@ -33,107 +33,167 @@ class BIT_star: def __init__(self): self.env = env() self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) - self.maxiter = 1000 + self.maxiter = 1000 # used for determining how many batches needed # radius calc self.eta = 1 # bigger or equal to 1 self.n = 1000 - self.Xf_hat = 1 # TODO self.nn = 1 # TODO + + self.edgeCost = {} # corresponding to c + self.heuristic_edgeCost = {} # correspoinding to c_hat def run(self): - V = {self.xstart} - E = set() - T = (V, E) # tree - Xsamples = {self.xgoal} - QE = set() - QV = set() - r = np.inf + self.V = {self.xstart} + self.E = set() + self.Parent = {} + self.T = (self.V, self.E) # tree + self.Xsamples = {self.xgoal} + self.QE = set() + self.QV = set() + self.r = np.inf ind = 0 while True: - if len(QE) == 0 and len(QV) == 0: - Xsamples, V, E = self.Prune(self.g_T(self.xgoal), Xsamples, V, E) - Vold = copy.deepcopy(V) - QV = copy.deepcopy(V) - r = self.radius(len(V) + len(Xsamples)) - while self.BestQueueValue(QV) <= self.BestQueueValue(QE): - QV, QE = self.ExpandVertex(self.BestInQueue(QV), QV, QE, Xsamples, Vold, E, V, r) - (vm, xm) = self.BestInQueue(QE) - QE.difference_update({(vm, xm)}) + # for the first round + if len(self.QE) == 0 and len(self.QV) == 0: + self.Prune(self.g_T(self.xgoal)) + self.Xsamples = self.Sample(m, self.g_T(self.xgoal)) # sample function + self.Vold = copy.deepcopy(self.V) + self.QV = copy.deepcopy(self.V) + self.r = self.radius(len(self.V) + len(self.Xsamples)) + while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'): + self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV')) + (vm, xm) = self.BestInQueue(self.QE, mode = 'QE') + self.QE.difference_update({(vm, xm)}) if self.g_T(vm) + self.c_hat(vm, xm) + self.h_hat(xm) < self.g_T(self.xgoal): if self.g_hat(vm) + self.c(vm, xm) + self.h_hat(xm) < self.g_T(self.xgoal): if self.g_T(vm) + self.c(vm, xm) < self.g_T(xm): - if xm in V: - E.difference_update({(v, x) for (v, x) in E if x == xm}) + if xm in self.V: + self.E.difference_update({(v, x) for (v, x) in self.E if x == xm}) else: - Xsamples.difference_update({xm}) - V.add(xm) - QV.add(xm) - E.add((vm, xm)) - QE.difference_update({(v, x) for (v, x) in QE if x == xm and self.g_T(v) + self.c_hat(v, x) >= self.g_T(x)}) + self.Xsamples.difference_update({xm}) + self.V.add(xm) + self.QV.add(xm) + self.E.add((vm, xm)) + self.Parent[vm] = xm # add parent or update parent + self.QE.difference_update({(v, x) for (v, x) in self.QE if x == xm and self.g_T(v) + self.c_hat(v, x) >= self.g_T(x)}) else: - QE = set() - QV = set() + self.QE = set() + self.QV = set() ind += 1 if ind > self.maxiter: break - return T + return self.T - def ExpandVertex(self, v , QV, QE, Xsamples, Vold, E, V, r): - QV.difference_update({v}) - Xnear = {x for x in Xsamples if getDist(x, v) <= r} - QE = {(v, x) for v in V for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)} - if v not in Vold: - Vnear = {w for w in V if getDist(w, v) <= r} - QE.update({(v,w) for v in V for w in Vnear if \ - ((v,w) not in E) and \ + def Sample(self, m, cost): + # TODO need the informed rrt + pass + + def ExpandVertex(self, v): + self.QV.difference_update({v}) + Xnear = {x for x in self.Xsamples if getDist(x, v) <= self.r} + self.QE.update({(v, x) for v in self.V for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)}) + if v not in self.Vold: + Vnear = {w for w in self.V if getDist(w, v) <= self.r} + self.QE.update({(v,w) for v in self.V for w in Vnear if \ + ((v,w) not in self.E) and \ (self.g_hat(v) + self.c_hat(v, w) + self.h_hat(w) < self.g_T(self.xgoal)) and \ (self.g_T(v) + self.c_hat(v, w) < self.g_T(w))}) - return QV, QE - def Prune(self, c, Xsamples, V, E): - Xsamples = {x for x in Xsamples if self.f_hat(x) >= c} - V.difference_update({v for v in V if self.f_hat(v) >=c}) - E.difference_update({(v, w) for (v, w) in E if (self.f_hat(v) > c) or (self.f_hat(w) > c)}) - Xsamples.update({v for v in V if self.g_T(v) == np.inf}) - V.difference_update({v for v in V if self.g_T(v) == np.inf}) - return Xsamples, V, E + def Prune(self, c): + self.Xsamples = {x for x in self.Xsamples if self.f_hat(x) >= c} + self.V.difference_update({v for v in self.V if self.f_hat(v) >= c}) + self.E.difference_update({(v, w) for (v, w) in self.E if (self.f_hat(v) > c) or (self.f_hat(w) > c)}) + self.Xsamples.update({v for v in self.V if self.g_T(v) == np.inf}) + self.V.difference_update({v for v in self.V if self.g_T(v) == np.inf}) def radius(self, q): return 2 * self.eta * (1 + 1/self.n) ** (1/self.n) * \ - (self.Lambda(self.Xf_hat) / self.Zeta ) ** (1/self.n) * \ + (self.Lambda(self.Xf_hat(self.V)) / self.Zeta ) ** (1/self.n) * \ (np.log(q) / q) ** (1/self.n) def Lambda(self, inputset): # lebesgue measure of a set, defined as # mu: L(Rn) --> [0, inf], e.g. volume - pass + return len(inputset) + + def Xf_hat(self, X): + # the X is a set, defined as {x in X | fhat(x) <= cbest} + # where cbest is current best cost. + cbest = self.g_T(self.xgoal) + return {x for x in X if self.f_hat(x) <= cbest} def Zeta(self): - # unit ball - pass + # Lebesgue measure of a n dimensional unit ball + # since it's the 3D, use volume + return 4/3 * np.pi - def BestInQueue(self, inputset): - pass + def BestInQueue(self, inputset, mode): + # returns the best vertex in the vertex queue given this ordering + # mode = 'QE' or 'QV' + _, best_state = self.find_best(inputset, mode) + return best_state - def BestQueueValue(self, inputset): - pass + def BestQueueValue(self, inputset, mode): + # returns the best value in the vertex queue given this ordering + # mode = 'QE' or 'QV' + best_val, _ = self.find_best(inputset, mode) + return best_val + def find_best(self, inputset, mode): + min_val, min_state = np.inf, None + for state in inputset: + if mode == 'QE': + curr_val = self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) + elif mode == 'QV': + curr_val = self.g_T(state) + self.h_hat(state) + if curr_val < min_val: + min_val, min_state = curr_val, state + return min_val, min_state + def g_hat(self, v): - pass - - def c(self, v, w): - pass - - def c_hat(self, v, w): - pass - - def f_hat(self, v): - pass + return getDist(self.xstart, v) def h_hat(self, v): - pass + return getDist(self.xgoal, v) + + def f_hat(self, v): + # f = g + h: estimate cost + return self.g_hat(v) + self.h_hat(v) + + def c(self, v, w): + # admissible estimate of the cost of an edge between state v, w + if (v,w) in self.edgeCost: + pass + else: + collide, dist = isCollide(self, v, w) + if collide: + self.edgeCost[(v,w)] = np.inf + else: + self.edgeCost[(v,w)] = dist + return self.edgeCost[(v,w)] + + def c_hat(self, v, w): + # c_hat < c < np.inf + # heuristic estimate of the edge cost, since c is expensive + if (v,w) in self.heuristic_edgeCost: + pass + else: + self.heuristic_edgeCost[(v,w)] = getDist(v, w) + return self.heuristic_edgeCost[(v,w)] def g_T(self, v): - pass + # represent cost-to-come from the start in the tree, + # if the state is not in tree, or unreachable, return inf + if v in self.Parent: + cost_to_come = 0 + while v != self.xstart: + cost_to_come += self.c(v, self.Parent[v]) + v = self.Parent[v] + return cost_to_come + elif v == self.xstart: + return 0 + else: + return np.inf + \ No newline at end of file diff --git a/Sampling_based_Planning/rrt_3D/queue.py b/Sampling_based_Planning/rrt_3D/queue.py index 1bd43fb..63c74ad 100644 --- a/Sampling_based_Planning/rrt_3D/queue.py +++ b/Sampling_based_Planning/rrt_3D/queue.py @@ -26,6 +26,11 @@ class MinheapPQ: heapq.heappush(self.pq, entry) self.nodes.add(item) + def put_set(self, dictin): + '''add a new dict into the priority queue''' + for item, priority in enumerate(dictin): + self.put(item, priority) + def check_remove(self, item): if item not in self.entry_finder: return @@ -33,6 +38,34 @@ class MinheapPQ: entry[-1] = self.REMOVED self.nodes.remove(item) + def check_remove_set(self, set_input): + if len(set_input) == 0: + return + for item in set_input: + if item not in self.entry_finder: + continue + entry = self.entry_finder.pop(item) + entry[-1] = self.REMOVED + self.nodes.remove(item) + + def priority_filtering(self, threshold, mode): + # mode: bigger: check and remove those key vals bigger than threshold + if mode == 'lowpass': + for entry in self.enumerate(): + item = entry[2] + if entry[0] >= threshold: # priority + _ = self.entry_finder.pop(item) + entry[-1] = self.REMOVED + self.nodes.remove(item) + # mode: smaller: check and remove those key vals smaller than threshold + elif mode == 'highpass': + for entry in self.enumerate(): + item = entry[2] + if entry[0] <= threshold: # priority + _ = self.entry_finder.pop(item) + entry[-1] = self.REMOVED + self.nodes.remove(item) + def get(self): """Remove and return the lowest priority task. Raise KeyError if empty.""" while self.pq: