update

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()

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)

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):

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)

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])

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :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
         """

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_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 s_goal
+        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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+        :return:  Cost for this motion
-        :note: cost function could be more complicate!
+        :note: Cost function could be more complicate!
         """
 
         if self.is_collision(s_start, s_goal):

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
+    #     # TODO scan graph for changed Cost, if Cost is changed update it
-    #     # make the graph cost via vectorization
+    #     # 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
+        returns, one can then follow a shortest path from x_start to
-        s_goal by always moving from the current vertex s, starting
+        x_goal by always moving from the current vertex s, starting
-        at s_start. , to any successor s' that minimizes c(s,s') + g(s') 
+        at x_start. , to any successor s' that minimizes c(s,s') + g(s')
-        until s_goal is reached (ties can be broken arbitrarily).'''
+        until x_goal is reached (ties can be broken arbitrarily).'''
         path = []
         s_goal = self.xt
         s = self.x0

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

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
+        returns, one can then follow a shortest path from x_start to
-        s_goal by always moving from the current vertex s, starting
+        x_goal by always moving from the current vertex s, starting
-        at s_start. , to any successor s' that minimizes c(s,s') + g(s') 
+        at x_start. , to any successor s' that minimizes c(s,s') + g(s')
-        until s_goal is reached (ties can be broken arbitrarily).'''
+        until x_goal is reached (ties can be broken arbitrarily).'''
         path = []
         s_goal = self.xt
         if not s_start:

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

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)