diff --git a/.idea/preferred-vcs.xml b/.idea/preferred-vcs.xml new file mode 100644 index 00000000..848cfc44 --- /dev/null +++ b/.idea/preferred-vcs.xml @@ -0,0 +1,6 @@ + + + + ApexVCS + + \ No newline at end of file diff --git a/PathPlanning/StateLatticePlanner/Figure_3.png b/PathPlanning/StateLatticePlanner/Figure_3.png new file mode 100644 index 00000000..09a4909d Binary files /dev/null and b/PathPlanning/StateLatticePlanner/Figure_3.png differ diff --git a/PathPlanning/StateLatticePlanner/Figure_4.png b/PathPlanning/StateLatticePlanner/Figure_4.png new file mode 100644 index 00000000..3c468efd Binary files /dev/null and b/PathPlanning/StateLatticePlanner/Figure_4.png differ diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index fc334c37..d8293557 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/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__': diff --git a/README.md b/README.md index 874431ce..290788f7 100644 --- a/README.md +++ b/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