update README

.idea/preferred-vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="PreferredVcsStorage">
+    <preferredVcsName>ApexVCS</preferredVcsName>
+  </component>
+</project>

PathPlanning/StateLatticePlanner/state_lattice_planner.py

@@ -60,7 +60,6 @@ def generate_path(states, k0):
 
 def calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max):
     """
-
     calc uniform state
 
     :param np: number of position sampling
@@ -70,32 +69,90 @@ def calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max):
     :param a_max: position sampling max angle
     :param p_min: heading sampling min angle
     :param p_max: heading sampling max angle
-    :return:
+    :return: states list
     """
     states = []
 
     for i in range(np):
         a = a_min + (a_max - a_min) * i / (np - 1)
-        print(a)
         for j in range(nh):
             xf = d * math.cos(a)
             yf = d * math.sin(a)
-            yawf = p_min + (p_max - p_min) * j / (nh - 1) + a
+            if nh == 1:
+                yawf = (p_max - p_min) / 2 + a
+            else:
+                yawf = p_min + (p_max - p_min) * j / (nh - 1) + a
+
             states.append([xf, yf, yawf])
 
     return states
 
 
+def calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max):
+    """
+    calc biased state
+
+    :param goal_angle: goal orientation for biased sampling
+    :param ns: number of biased sampling
+    :param nxy: number of position sampling
+    :param nxy: number of position sampling
+    :param nh: number of heading sampleing
+    :param d: distance of terminal state
+    :param a_min: position sampling min angle
+    :param a_max: position sampling max angle
+    :param p_min: heading sampling min angle
+    :param p_max: heading sampling max angle
+    :return: states list
+    """
+
+    cnav = []
+    for i in range(ns - 1):
+        asi = a_min + (a_max - a_min) * i / (ns - 1)
+        cnavi = math.pi - abs(asi - goal_angle)
+        cnav.append(cnavi)
+
+    cnav_sum = sum(cnav)
+    cnav_max = max(cnav)
+
+    # normalize
+    for i in range(ns - 1):
+        cnav[i] = (cnav_max - cnav[i]) / (cnav_max * ns - cnav_sum)
+
+    csumnav = np.cumsum(cnav)
+    di = []
+    for i in range(nxy):
+        for ii in range(ns - 1):
+            if ii / ns >= i / (nxy - 1):
+                di.append(csumnav[ii])
+                break
+
+    states = []
+    for i in di:
+        a = a_min + (a_max - a_min) * i
+
+        for j in range(nh):
+            xf = d * math.cos(a)
+            yf = d * math.sin(a)
+            if nh == 1:
+                yawf = (p_max - p_min) / 2 + a
+            else:
+                yawf = p_min + (p_max - p_min) * j / (nh - 1) + a
+            states.append([xf, yf, yawf])
+
+    return states
+
+
+
 def uniform_terminal_state_sampling1():
     k0 = 0.0
-    np = 5
+    nxy = 5
     nh = 3
     d = 20
     a_min = - math.radians(45.0)
     a_max = math.radians(45.0)
     p_min = - math.radians(45.0)
     p_max = math.radians(45.0)
-    states = calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max)
+    states = calc_uniform_states(nxy, nh, d, a_min, a_max, p_min, p_max)
     result = generate_path(states, k0)
 
     for table in result:
@@ -112,14 +169,14 @@ def uniform_terminal_state_sampling1():
 
 def uniform_terminal_state_sampling2():
     k0 = 0.1
-    np = 6
+    nxy = 6
     nh = 3
     d = 20
     a_min = - math.radians(-10.0)
     a_max = math.radians(45.0)
     p_min = - math.radians(20.0)
     p_max = math.radians(20.0)
-    states = calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max)
+    states = calc_uniform_states(nxy, nh, d, a_min, a_max, p_min, p_max)
     result = generate_path(states, k0)
 
     for table in result:
@@ -134,10 +191,60 @@ def uniform_terminal_state_sampling2():
     print("Done")
 
 
+def biased_terminal_state_sampling1():
+    k0 = 0.0
+    nxy = 30
+    nh = 2
+    d = 20
+    a_min = math.radians(-45.0)
+    a_max = math.radians(45.0)
+    p_min = - math.radians(20.0)
+    p_max = math.radians(20.0)
+    ns = 100
+    goal_angle = math.radians(0.0)
+    states = calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max)
+    result = generate_path(states, k0)
+
+    for table in result:
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], table[4], table[5], k0)
+        plt.plot(xc, yc, "-r")
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+
+def biased_terminal_state_sampling2():
+    k0 = 0.0
+    nxy = 30
+    nh = 1
+    d = 20
+    a_min = math.radians(0.0)
+    a_max = math.radians(45.0)
+    p_min = - math.radians(20.0)
+    p_max = math.radians(20.0)
+    ns = 100
+    goal_angle = math.radians(30.0)
+    states = calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max)
+    result = generate_path(states, k0)
+
+    for table in result:
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], table[4], table[5], k0)
+        plt.plot(xc, yc, "-r")
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+
 
 def main():
     # uniform_terminal_state_sampling1()
-    uniform_terminal_state_sampling2()
+    # uniform_terminal_state_sampling2()
+    # biased_terminal_state_sampling1()
+    biased_terminal_state_sampling2()
 
 
 if __name__ == '__main__':

README.md

@@ -46,6 +46,12 @@ This code uses the model predictive trajectory generator to solve boundary probl
 
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_2.png)
 
+### Biased polar sampling results:
+
+![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_3.png)
+
+![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/StateLatticePlanner/Figure_4.png)
+
 
 
 ## RRT