mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
update
This commit is contained in:
zhm-real
+33
-25
@@ -19,7 +19,15 @@
|
|||||||
<select />
|
<select />
|
||||||
</component>
|
</component>
|
||||||
<component name="ChangeListManager">
|
<component name="ChangeListManager">
|
||||||
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
|
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
|
||||||
|
<change afterPath="$PROJECT_DIR$/Search_2D/best_first.py" afterDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py" beforeDir="false" />
|
||||||
|
<change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
|
||||||
|
</list>
|
||||||
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
|
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
|
||||||
<option name="SHOW_DIALOG" value="false" />
|
<option name="SHOW_DIALOG" value="false" />
|
||||||
<option name="HIGHLIGHT_CONFLICTS" value="true" />
|
<option name="HIGHLIGHT_CONFLICTS" value="true" />
|
||||||
@@ -70,7 +78,7 @@
|
|||||||
</list>
|
</list>
|
||||||
</option>
|
</option>
|
||||||
</component>
|
</component>
|
||||||
<component name="RunManager" selected="Python.Field_D_star">
|
<component name="RunManager" selected="Python.best_first">
|
||||||
<configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
<configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
||||||
<module name="Search-based Planning" />
|
<module name="Search-based Planning" />
|
||||||
<option name="INTERPRETER_OPTIONS" value="" />
|
<option name="INTERPRETER_OPTIONS" value="" />
|
||||||
@@ -92,27 +100,6 @@
|
|||||||
<option name="INPUT_FILE" value="" />
|
<option name="INPUT_FILE" value="" />
|
||||||
<method v="2" />
|
<method v="2" />
|
||||||
</configuration>
|
</configuration>
|
||||||
<configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
|
||||||
<module name="Search-based Planning" />
|
|
||||||
<option name="INTERPRETER_OPTIONS" value="" />
|
|
||||||
<option name="PARENT_ENVS" value="true" />
|
|
||||||
<envs>
|
|
||||||
<env name="PYTHONUNBUFFERED" value="1" />
|
|
||||||
</envs>
|
|
||||||
<option name="SDK_HOME" value="" />
|
|
||||||
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
|
|
||||||
<option name="IS_MODULE_SDK" value="true" />
|
|
||||||
<option name="ADD_CONTENT_ROOTS" value="true" />
|
|
||||||
<option name="ADD_SOURCE_ROOTS" value="true" />
|
|
||||||
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
|
|
||||||
<option name="PARAMETERS" value="" />
|
|
||||||
<option name="SHOW_COMMAND_LINE" value="false" />
|
|
||||||
<option name="EMULATE_TERMINAL" value="false" />
|
|
||||||
<option name="MODULE_MODE" value="false" />
|
|
||||||
<option name="REDIRECT_INPUT" value="false" />
|
|
||||||
<option name="INPUT_FILE" value="" />
|
|
||||||
<method v="2" />
|
|
||||||
</configuration>
|
|
||||||
<configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
<configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
||||||
<module name="Search-based Planning" />
|
<module name="Search-based Planning" />
|
||||||
<option name="INTERPRETER_OPTIONS" value="" />
|
<option name="INTERPRETER_OPTIONS" value="" />
|
||||||
@@ -176,6 +163,27 @@
|
|||||||
<option name="INPUT_FILE" value="" />
|
<option name="INPUT_FILE" value="" />
|
||||||
<method v="2" />
|
<method v="2" />
|
||||||
</configuration>
|
</configuration>
|
||||||
|
<configuration name="best_first" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
||||||
|
<module name="Search-based Planning" />
|
||||||
|
<option name="INTERPRETER_OPTIONS" value="" />
|
||||||
|
<option name="PARENT_ENVS" value="true" />
|
||||||
|
<envs>
|
||||||
|
<env name="PYTHONUNBUFFERED" value="1" />
|
||||||
|
</envs>
|
||||||
|
<option name="SDK_HOME" value="" />
|
||||||
|
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
|
||||||
|
<option name="IS_MODULE_SDK" value="true" />
|
||||||
|
<option name="ADD_CONTENT_ROOTS" value="true" />
|
||||||
|
<option name="ADD_SOURCE_ROOTS" value="true" />
|
||||||
|
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/best_first.py" />
|
||||||
|
<option name="PARAMETERS" value="" />
|
||||||
|
<option name="SHOW_COMMAND_LINE" value="false" />
|
||||||
|
<option name="EMULATE_TERMINAL" value="false" />
|
||||||
|
<option name="MODULE_MODE" value="false" />
|
||||||
|
<option name="REDIRECT_INPUT" value="false" />
|
||||||
|
<option name="INPUT_FILE" value="" />
|
||||||
|
<method v="2" />
|
||||||
|
</configuration>
|
||||||
<configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
|
<configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
|
||||||
<module name="Search-based Planning" />
|
<module name="Search-based Planning" />
|
||||||
<option name="INTERPRETER_OPTIONS" value="" />
|
<option name="INTERPRETER_OPTIONS" value="" />
|
||||||
@@ -199,19 +207,19 @@
|
|||||||
</configuration>
|
</configuration>
|
||||||
<list>
|
<list>
|
||||||
<item itemvalue="Python.dijkstra" />
|
<item itemvalue="Python.dijkstra" />
|
||||||
<item itemvalue="Python.Astar3D" />
|
|
||||||
<item itemvalue="Python.ARAstar" />
|
<item itemvalue="Python.ARAstar" />
|
||||||
<item itemvalue="Python.Potential_Field" />
|
<item itemvalue="Python.Potential_Field" />
|
||||||
<item itemvalue="Python.D_star_Lite" />
|
<item itemvalue="Python.D_star_Lite" />
|
||||||
<item itemvalue="Python.Field_D_star" />
|
<item itemvalue="Python.Field_D_star" />
|
||||||
|
<item itemvalue="Python.best_first" />
|
||||||
</list>
|
</list>
|
||||||
<recent_temporary>
|
<recent_temporary>
|
||||||
<list>
|
<list>
|
||||||
|
<item itemvalue="Python.best_first" />
|
||||||
<item itemvalue="Python.Field_D_star" />
|
<item itemvalue="Python.Field_D_star" />
|
||||||
<item itemvalue="Python.D_star_Lite" />
|
<item itemvalue="Python.D_star_Lite" />
|
||||||
<item itemvalue="Python.Potential_Field" />
|
<item itemvalue="Python.Potential_Field" />
|
||||||
<item itemvalue="Python.ARAstar" />
|
<item itemvalue="Python.ARAstar" />
|
||||||
<item itemvalue="Python.Astar3D" />
|
|
||||||
</list>
|
</list>
|
||||||
</recent_temporary>
|
</recent_temporary>
|
||||||
</component>
|
</component>
|
||||||
|
|||||||
@@ -1,445 +0,0 @@
|
|||||||
"""
|
|
||||||
This Reeds-Shepp Curves is from nathanlct's work
|
|
||||||
(https://github.com/nathanlct/reeds-shepp-curves)
|
|
||||||
"""
|
|
||||||
|
|
||||||
import os
|
|
||||||
import sys
|
|
||||||
from enum import Enum
|
|
||||||
|
|
||||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
||||||
"/../../../../Search-based Planning/")
|
|
||||||
|
|
||||||
from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
|
|
||||||
|
|
||||||
|
|
||||||
class Steering(Enum):
|
|
||||||
LEFT = 1
|
|
||||||
RIGHT = 2
|
|
||||||
STRAIGHT = 3
|
|
||||||
|
|
||||||
|
|
||||||
class Gear(Enum):
|
|
||||||
FORWARD = 1
|
|
||||||
BACKWARD = 2
|
|
||||||
|
|
||||||
|
|
||||||
class PathElement():
|
|
||||||
def __init__(self, param, steering, gear):
|
|
||||||
self.param = param
|
|
||||||
self.steering = steering
|
|
||||||
self.gear = gear
|
|
||||||
|
|
||||||
def __repr__(self):
|
|
||||||
if self.steering == Steering.LEFT:
|
|
||||||
steering_str = "left"
|
|
||||||
elif self.steering == Steering.RIGHT:
|
|
||||||
steering_str = "right"
|
|
||||||
else:
|
|
||||||
steering_str = "straight"
|
|
||||||
|
|
||||||
if self.gear == Gear.FORWARD:
|
|
||||||
gear_str = "forward"
|
|
||||||
else:
|
|
||||||
gear_str = "backward"
|
|
||||||
|
|
||||||
s = "{ Steering: " + steering_str + "\tGear: " + gear_str \
|
|
||||||
+ "\tdistance: " + str(round(self.param, 2)) + " }"
|
|
||||||
|
|
||||||
return s
|
|
||||||
|
|
||||||
def reverse_steering(self):
|
|
||||||
if self.steering == Steering.LEFT:
|
|
||||||
self.steering = Steering.RIGHT
|
|
||||||
elif self.steering == Steering.RIGHT:
|
|
||||||
self.steering = Steering.LEFT
|
|
||||||
|
|
||||||
def reverse_gear(self):
|
|
||||||
if self.gear == Gear.FORWARD:
|
|
||||||
self.gear = Gear.BACKWARD
|
|
||||||
else:
|
|
||||||
self.gear = Gear.FORWARD
|
|
||||||
|
|
||||||
|
|
||||||
def path_length(path):
|
|
||||||
"""
|
|
||||||
this one's obvious
|
|
||||||
"""
|
|
||||||
return sum([e.param for e in path])
|
|
||||||
|
|
||||||
|
|
||||||
def get_optimal_path(start, end):
|
|
||||||
"""
|
|
||||||
Return the shortest path from start to end among those that exist
|
|
||||||
"""
|
|
||||||
paths = get_all_paths(start, end)
|
|
||||||
i_min = 0
|
|
||||||
L_min = path_length(paths[0])
|
|
||||||
for i in range(1, len(paths) - 1):
|
|
||||||
L = path_length(paths[i])
|
|
||||||
if L <= L_min:
|
|
||||||
L_min, i_min = L, i
|
|
||||||
return paths[i_min]
|
|
||||||
|
|
||||||
|
|
||||||
def get_all_paths(start, end):
|
|
||||||
"""
|
|
||||||
Return a list of all the paths from start to end generated by the
|
|
||||||
12 functions and their variants
|
|
||||||
"""
|
|
||||||
path_fns = [path1, path2, path3, path4, path5, path6, path7, path8, path9, path10, path11, path12]
|
|
||||||
paths = []
|
|
||||||
|
|
||||||
# get coordinates of end in the set of axis where start is (0,0,0)
|
|
||||||
x, y, theta = change_of_basis(start, end)
|
|
||||||
|
|
||||||
for get_path in path_fns:
|
|
||||||
# get the four variants for each path type, cf article
|
|
||||||
paths.append(get_path(x, y, theta))
|
|
||||||
paths.append(timeflip(get_path(-x, y, -theta)))
|
|
||||||
paths.append(reflect(get_path(x, -y, -theta)))
|
|
||||||
paths.append(reflect(timeflip(get_path(-x, -y, theta))))
|
|
||||||
|
|
||||||
# remove path elements that have parameter 0
|
|
||||||
for i in range(len(paths)):
|
|
||||||
paths[i] = list(filter(lambda e: e.param != 0, paths[i]))
|
|
||||||
|
|
||||||
# remove empty paths
|
|
||||||
paths = list(filter(None, paths))
|
|
||||||
|
|
||||||
return paths
|
|
||||||
|
|
||||||
|
|
||||||
def timeflip(path):
|
|
||||||
"""
|
|
||||||
timeflip transform described around the end of the article
|
|
||||||
"""
|
|
||||||
new_path = path.copy()
|
|
||||||
for e in new_path:
|
|
||||||
e.reverse_gear()
|
|
||||||
return new_path
|
|
||||||
|
|
||||||
|
|
||||||
def reflect(path):
|
|
||||||
"""
|
|
||||||
reflect transform described around the end of the article
|
|
||||||
"""
|
|
||||||
new_path = path.copy()
|
|
||||||
for e in new_path:
|
|
||||||
e.reverse_steering()
|
|
||||||
return new_path
|
|
||||||
|
|
||||||
|
|
||||||
def path1(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.1: CSC (same turns)
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
u, t = R(x - math.sin(phi), y - 1 + math.cos(phi))
|
|
||||||
v = M(phi - t)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path2(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.2: CSC (opposite turns)
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = M(deg2rad(phi))
|
|
||||||
path = []
|
|
||||||
|
|
||||||
rho, t1 = R(x + math.sin(phi), y - 1 - math.cos(phi))
|
|
||||||
|
|
||||||
if rho * rho >= 4:
|
|
||||||
u = math.sqrt(rho * rho - 4)
|
|
||||||
t = M(t1 + math.atan2(2, u))
|
|
||||||
v = M(t - phi)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path3(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.3: C|C|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x - math.sin(phi)
|
|
||||||
eta = y - 1 + math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho <= 4:
|
|
||||||
A = math.acos(rho / 4)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
u = M(math.pi - 2 * A)
|
|
||||||
v = M(phi - t - u)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path4(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.4 (1): C|CC
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x - math.sin(phi)
|
|
||||||
eta = y - 1 + math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho <= 4:
|
|
||||||
A = math.acos(rho / 4)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
u = M(math.pi - 2 * A)
|
|
||||||
v = M(t + u - phi)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path5(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.4 (2): CC|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x - math.sin(phi)
|
|
||||||
eta = y - 1 + math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho <= 4:
|
|
||||||
u = math.acos(1 - rho * rho / 8)
|
|
||||||
A = math.asin(2 * math.sin(u) / rho)
|
|
||||||
t = M(theta + math.pi / 2 - A)
|
|
||||||
v = M(t - u - phi)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path6(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.7: CCu|CuC
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x + math.sin(phi)
|
|
||||||
eta = y - 1 - math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho <= 4:
|
|
||||||
if rho <= 2:
|
|
||||||
A = math.acos((rho + 2) / 4)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
u = M(A)
|
|
||||||
v = M(phi - t + 2 * u)
|
|
||||||
else:
|
|
||||||
A = math.acos((rho - 2) / 4)
|
|
||||||
t = M(theta + math.pi / 2 - A)
|
|
||||||
u = M(math.pi - A)
|
|
||||||
v = M(phi - t + 2 * u)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path7(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.8: C|CuCu|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x + math.sin(phi)
|
|
||||||
eta = y - 1 - math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
u1 = (20 - rho * rho) / 16
|
|
||||||
|
|
||||||
if rho <= 6 and 0 <= u1 <= 1:
|
|
||||||
u = math.acos(u1)
|
|
||||||
A = math.asin(2 * math.sin(u) / rho)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
v = M(t - phi)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path8(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.9 (1): C|C[pi/2]SC
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x - math.sin(phi)
|
|
||||||
eta = y - 1 + math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho >= 2:
|
|
||||||
u = math.sqrt(rho * rho - 4) - 2
|
|
||||||
A = math.atan2(2, u + 2)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
v = M(t - phi + math.pi / 2)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path9(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.9 (2): CSC[pi/2]|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x - math.sin(phi)
|
|
||||||
eta = y - 1 + math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho >= 2:
|
|
||||||
u = math.sqrt(rho * rho - 4) - 2
|
|
||||||
A = math.atan2(u + 2, 2)
|
|
||||||
t = M(theta + math.pi / 2 - A)
|
|
||||||
v = M(t - phi - math.pi / 2)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path10(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.10 (1): C|C[pi/2]SC
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x + math.sin(phi)
|
|
||||||
eta = y - 1 - math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho >= 2:
|
|
||||||
t = M(theta + math.pi / 2)
|
|
||||||
u = rho - 2
|
|
||||||
v = M(phi - t - math.pi / 2)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path11(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.10 (2): CSC[pi/2]|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x + math.sin(phi)
|
|
||||||
eta = y - 1 - math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho >= 2:
|
|
||||||
t = M(theta)
|
|
||||||
u = rho - 2
|
|
||||||
v = M(phi - t - math.pi / 2)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
|
|
||||||
|
|
||||||
def path12(x, y, phi):
|
|
||||||
"""
|
|
||||||
Formula 8.11: C|C[pi/2]SC[pi/2]|C
|
|
||||||
"""
|
|
||||||
r, theta = R(x, y)
|
|
||||||
phi = deg2rad(phi)
|
|
||||||
path = []
|
|
||||||
|
|
||||||
xi = x + math.sin(phi)
|
|
||||||
eta = y - 1 - math.cos(phi)
|
|
||||||
rho, theta = R(xi, eta)
|
|
||||||
|
|
||||||
if rho >= 4:
|
|
||||||
u = math.sqrt(rho * rho - 4) - 4
|
|
||||||
A = math.atan2(2, u + 4)
|
|
||||||
t = M(theta + math.pi / 2 + A)
|
|
||||||
v = M(t - phi)
|
|
||||||
|
|
||||||
if t >= 0 and u >= 0 and v >= 0:
|
|
||||||
path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.BACKWARD))
|
|
||||||
path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
|
|
||||||
|
|
||||||
return path
|
|
||||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -1,89 +0,0 @@
|
|||||||
import turtle
|
|
||||||
import random as rd
|
|
||||||
import os
|
|
||||||
import sys
|
|
||||||
|
|
||||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
||||||
"/../../../../Search-based Planning/")
|
|
||||||
|
|
||||||
from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
|
|
||||||
import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as rs
|
|
||||||
|
|
||||||
|
|
||||||
# drawing n units (eg turtle.forward(n)) will draw n * SCALE pixels
|
|
||||||
SCALE = 40
|
|
||||||
|
|
||||||
|
|
||||||
def scale(x):
|
|
||||||
"""
|
|
||||||
Scale the input coordinate(s).
|
|
||||||
"""
|
|
||||||
if type(x) is tuple or type(x) is list:
|
|
||||||
return [p * SCALE for p in x]
|
|
||||||
return x * SCALE
|
|
||||||
|
|
||||||
|
|
||||||
def unscale(x):
|
|
||||||
"""
|
|
||||||
Unscale the input coordinate(s).
|
|
||||||
"""
|
|
||||||
if type(x) is tuple or type(x) is list:
|
|
||||||
return [p / SCALE for p in x]
|
|
||||||
return x / SCALE
|
|
||||||
|
|
||||||
|
|
||||||
# note: bob is a turtle
|
|
||||||
|
|
||||||
def vec(bob):
|
|
||||||
"""
|
|
||||||
Draw an arrow.
|
|
||||||
"""
|
|
||||||
bob.down()
|
|
||||||
bob.pensize(3)
|
|
||||||
bob.forward(scale(1.2))
|
|
||||||
bob.right(25)
|
|
||||||
bob.backward(scale(.4))
|
|
||||||
bob.forward(scale(.4))
|
|
||||||
bob.left(50)
|
|
||||||
bob.backward(scale(.4))
|
|
||||||
bob.forward(scale(.4))
|
|
||||||
bob.right(25)
|
|
||||||
bob.pensize(1)
|
|
||||||
bob.up()
|
|
||||||
|
|
||||||
|
|
||||||
def goto(bob, pos, scale_pos=True):
|
|
||||||
"""
|
|
||||||
Go to a position without drawing.
|
|
||||||
"""
|
|
||||||
bob.up()
|
|
||||||
if scale_pos:
|
|
||||||
bob.setpos(scale(pos[:2]))
|
|
||||||
else:
|
|
||||||
bob.setpos(pos[:2])
|
|
||||||
bob.setheading(pos[2])
|
|
||||||
bob.down()
|
|
||||||
|
|
||||||
|
|
||||||
def draw_path(bob, path):
|
|
||||||
"""
|
|
||||||
Draw the path (list of RS.PathElements).
|
|
||||||
"""
|
|
||||||
for e in path:
|
|
||||||
gear = 1 if e.gear == rs.Gear.FORWARD else -1
|
|
||||||
if e.steering == rs.Steering.LEFT:
|
|
||||||
bob.circle(scale(1), gear * rad2deg(e.param))
|
|
||||||
elif e.steering == rs.Steering.RIGHT:
|
|
||||||
bob.circle(- scale(1), gear * rad2deg(e.param))
|
|
||||||
elif e.steering == rs.Steering.STRAIGHT:
|
|
||||||
bob.forward(gear * scale(e.param))
|
|
||||||
|
|
||||||
|
|
||||||
def set_random_pencolor(bob):
|
|
||||||
"""
|
|
||||||
Draws noodles.
|
|
||||||
"""
|
|
||||||
r, g, b = 1, 1, 1
|
|
||||||
while r + g + b > 2.5:
|
|
||||||
r, g, b = rd.uniform(0, 1), rd.uniform(0, 1), rd.uniform(0, 1)
|
|
||||||
bob.pencolor(r, g, b)
|
|
||||||
@@ -1,76 +0,0 @@
|
|||||||
import turtle
|
|
||||||
import random as rd
|
|
||||||
import math
|
|
||||||
|
|
||||||
import os
|
|
||||||
import sys
|
|
||||||
|
|
||||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
||||||
"/../../../../Search-based Planning/")
|
|
||||||
|
|
||||||
import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as RS
|
|
||||||
from Search_2D.Hybrid_Astar.ReedsSheppPlanner import draw
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
# points to be followed
|
|
||||||
# pts = [(-6, -7), (-6, 0), (-4, 6), (0, 5), (0, -2), (-2, -6), (3, -5), (3, 6), (6, 4)]
|
|
||||||
#
|
|
||||||
# # generate PATH so the vectors are pointing at each other
|
|
||||||
# PATH = []
|
|
||||||
# for i in range(len(pts) - 1):
|
|
||||||
# dx = pts[i + 1][0] - pts[i][0]
|
|
||||||
# dy = pts[i + 1][1] - pts[i][1]
|
|
||||||
# theta = math.atan2(dy, dx)
|
|
||||||
# PATH.append((pts[i][0], pts[i][1], utils.rad2deg(theta)))
|
|
||||||
# PATH.append((pts[-1][0], pts[-1][1], 0))
|
|
||||||
|
|
||||||
# or you can also manually set the angles:
|
|
||||||
PATH = [(-5, 5, 90), (-5, 5, -90), (1, 4, 180), (5, 4, 0), (6, -3, 90), (4, -4, -40), (-2, 0, 240),
|
|
||||||
(-6, -7, 160), (-7, -1, 80)]
|
|
||||||
|
|
||||||
# or just generate a random route:
|
|
||||||
# PATH = []
|
|
||||||
# for _ in range(10):
|
|
||||||
# PATH.append((rd.randint(-7, 7), rd.randint(-7, 7), rd.randint(0, 359)))
|
|
||||||
|
|
||||||
# init turtle
|
|
||||||
tesla = turtle.Turtle()
|
|
||||||
tesla.speed(0) # 0: fast; 1: slow, 8.4: cool
|
|
||||||
tesla.shape('arrow')
|
|
||||||
tesla.resizemode('user')
|
|
||||||
tesla.shapesize(1, 1)
|
|
||||||
|
|
||||||
# draw vectors representing points in PATH
|
|
||||||
for pt in PATH:
|
|
||||||
draw.goto(tesla, pt)
|
|
||||||
draw.vec(tesla)
|
|
||||||
|
|
||||||
# draw all routes found
|
|
||||||
tesla.speed(0)
|
|
||||||
for i in range(len(PATH) - 1):
|
|
||||||
paths = RS.get_all_paths(PATH[i], PATH[i + 1])
|
|
||||||
|
|
||||||
for path in paths:
|
|
||||||
draw.set_random_pencolor(tesla)
|
|
||||||
draw.goto(tesla, PATH[i])
|
|
||||||
draw.draw_path(tesla, path)
|
|
||||||
|
|
||||||
# draw shortest route
|
|
||||||
tesla.pencolor(1, 0, 0)
|
|
||||||
tesla.pensize(3)
|
|
||||||
tesla.speed(2)
|
|
||||||
draw.goto(tesla, PATH[0])
|
|
||||||
path_length = 0
|
|
||||||
for i in range(len(PATH) - 1):
|
|
||||||
path = RS.get_optimal_path(PATH[i], PATH[i + 1])
|
|
||||||
path_length += RS.path_length(path)
|
|
||||||
draw.draw_path(tesla, path)
|
|
||||||
|
|
||||||
print("Shortest path length: {} px.".format(int(draw.scale(path_length))))
|
|
||||||
|
|
||||||
turtle.done()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,50 +0,0 @@
|
|||||||
import math
|
|
||||||
|
|
||||||
|
|
||||||
def M(theta):
|
|
||||||
"""
|
|
||||||
Return the angle phi = theta mod (2 pi) such that -pi <= theta < pi.
|
|
||||||
"""
|
|
||||||
theta = theta % (2 * math.pi)
|
|
||||||
if theta < -math.pi:
|
|
||||||
return theta + 2 * math.pi
|
|
||||||
if theta >= math.pi:
|
|
||||||
return theta - 2 * math.pi
|
|
||||||
return theta
|
|
||||||
|
|
||||||
|
|
||||||
def R(x, y):
|
|
||||||
"""
|
|
||||||
Return the polar coordinates (r, theta) of the point (x, y).
|
|
||||||
"""
|
|
||||||
r = math.sqrt(x * x + y * y)
|
|
||||||
theta = math.atan2(y, x)
|
|
||||||
return r, theta
|
|
||||||
|
|
||||||
|
|
||||||
def change_of_basis(p1, p2):
|
|
||||||
"""
|
|
||||||
Given p1 = (x1, y1, theta1) and p2 = (x2, y2, theta2) represented in a
|
|
||||||
coordinate system with origin (0, 0) and rotation 0 (in degrees), return
|
|
||||||
the position and rotation of p2 in the coordinate system which origin
|
|
||||||
(x1, y1) and rotation theta1.
|
|
||||||
"""
|
|
||||||
theta1 = deg2rad(p1[2])
|
|
||||||
dx = p2[0] - p1[0]
|
|
||||||
dy = p2[1] - p1[1]
|
|
||||||
new_x = dx * math.cos(theta1) + dy * math.sin(theta1)
|
|
||||||
new_y = -dx * math.sin(theta1) + dy * math.cos(theta1)
|
|
||||||
new_theta = p2[2] - p1[2]
|
|
||||||
return new_x, new_y, new_theta
|
|
||||||
|
|
||||||
|
|
||||||
def rad2deg(rad):
|
|
||||||
return 180 * rad / math.pi
|
|
||||||
|
|
||||||
|
|
||||||
def deg2rad(deg):
|
|
||||||
return math.pi * deg / 180
|
|
||||||
|
|
||||||
|
|
||||||
def sign(x):
|
|
||||||
return 1 if x >= 0 else -1
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,127 @@
|
|||||||
|
"""
|
||||||
|
Best-First Searching
|
||||||
|
@author: huiming zhou
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import math
|
||||||
|
|
||||||
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||||
|
"/../../Search-based Planning/")
|
||||||
|
|
||||||
|
from Search_2D import queue
|
||||||
|
from Search_2D import plotting
|
||||||
|
from Search_2D import env
|
||||||
|
|
||||||
|
|
||||||
|
class BestFirst:
|
||||||
|
def __init__(self, s_start, s_goal):
|
||||||
|
self.s_start, self.s_goal = s_start, s_goal
|
||||||
|
|
||||||
|
self.Env = env.Env()
|
||||||
|
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
|
||||||
|
|
||||||
|
self.u_set = self.Env.motions # feasible input set
|
||||||
|
self.obs = self.Env.obs # position of obstacles
|
||||||
|
|
||||||
|
self.OPEN = queue.QueuePrior() # OPEN set
|
||||||
|
self.OPEN.put(self.s_start, self.Heuristic(self.s_start))
|
||||||
|
self.CLOSED = [] # CLOSED set / visited order
|
||||||
|
self.PARENT = {self.s_start: self.s_start}
|
||||||
|
|
||||||
|
def searching(self):
|
||||||
|
"""
|
||||||
|
Best-first Searching
|
||||||
|
:return: planning path, visited order
|
||||||
|
"""
|
||||||
|
|
||||||
|
while self.OPEN:
|
||||||
|
s = self.OPEN.get()
|
||||||
|
|
||||||
|
if s == self.s_goal:
|
||||||
|
break
|
||||||
|
self.CLOSED.append(s)
|
||||||
|
|
||||||
|
for s_n in self.get_neighbor(s):
|
||||||
|
if s_n not in self.PARENT: # node not explored
|
||||||
|
self.OPEN.put(s_n, self.Heuristic(s_n))
|
||||||
|
self.PARENT[s_n] = s
|
||||||
|
|
||||||
|
return self.extract_path(), self.CLOSED
|
||||||
|
|
||||||
|
def Heuristic(self, s):
|
||||||
|
"""
|
||||||
|
estimated distance between current state and goal state.
|
||||||
|
:param s: current state
|
||||||
|
:return: estimated distance
|
||||||
|
"""
|
||||||
|
|
||||||
|
h = math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1])
|
||||||
|
|
||||||
|
return h
|
||||||
|
|
||||||
|
def get_neighbor(self, s):
|
||||||
|
"""
|
||||||
|
find neighbors of state s that not in obstacles.
|
||||||
|
:param s: state
|
||||||
|
:return: neighbors
|
||||||
|
"""
|
||||||
|
|
||||||
|
s_list = []
|
||||||
|
|
||||||
|
for u in self.u_set:
|
||||||
|
s_next = tuple([s[i] + u[i] for i in range(2)])
|
||||||
|
if not self.is_collision(s, s_next):
|
||||||
|
s_list.append(s_next)
|
||||||
|
|
||||||
|
return s_list
|
||||||
|
|
||||||
|
def is_collision(self, s_start, s_end):
|
||||||
|
if s_start in self.obs or s_end in self.obs:
|
||||||
|
return True
|
||||||
|
|
||||||
|
if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
|
||||||
|
if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
|
||||||
|
s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
|
||||||
|
s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
|
||||||
|
else:
|
||||||
|
s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
|
||||||
|
s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
|
||||||
|
|
||||||
|
if s1 in self.obs or s2 in self.obs:
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def extract_path(self):
|
||||||
|
"""
|
||||||
|
Extract the path based on the relationship of nodes.
|
||||||
|
:return: The planning path
|
||||||
|
"""
|
||||||
|
|
||||||
|
path = [self.s_goal]
|
||||||
|
s = self.s_goal
|
||||||
|
|
||||||
|
while True:
|
||||||
|
s = self.PARENT[s]
|
||||||
|
path.append(s)
|
||||||
|
if s == self.s_start:
|
||||||
|
break
|
||||||
|
|
||||||
|
return list(path)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
s_start = (5, 5)
|
||||||
|
s_goal = (45, 25)
|
||||||
|
|
||||||
|
BF = BestFirst(s_start, s_goal)
|
||||||
|
plot = plotting.Plotting(s_start, s_goal)
|
||||||
|
|
||||||
|
path, visited = BF.searching()
|
||||||
|
plot.animation(path, visited, "Best-first Searching") # animation
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 191 KiB |
Reference in New Issue
Block a user