github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 01:28:23 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into arm-manipulation

Browse Source
This commit is contained in:
Takayuki Murooka
2019-08-18 21:08:11 +09:00
parent 47b497019a b762fd036e
commit f2268f3536
200 changed files with 8797 additions and 4068 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.github/FUNDING.yml vendored Normal file
View File

@@ -0,0 +1,3 @@
# These are supported funding model platforms
patreon: myenigma
custom: https://www.paypal.me/myenigmapay/

4
.gitignore vendored
View File

@@ -1,4 +1,6 @@
*.csv
*.gif
*.g2o
# Byte-compiled / optimized / DLL files
__pycache__/
@@ -66,3 +68,5 @@ target/
#Ipython Notebook
.ipynb_checkpoints
matplotrecorder/*

4
.gitmodules vendored
View File

@@ -1,4 +0,0 @@
[submodule "doc/PythonRoboticsPaper"]
path = doc/PythonRoboticsPaper
url = https://github.com/AtsushiSakai/PythonRoboticsPaper

4
.lgtm.yml Normal file
View File

@@ -0,0 +1,4 @@
extraction:
python:
python_setup:
version: 3

8
.travis.yml
View File

@@ -3,8 +3,6 @@ language: python
matrix:
include:
- os: linux
#- os: osx
#python: "3.6-dev"
python:
- 3.6
@@ -20,14 +18,14 @@ before_install:
- conda update -q conda
# Useful for debugging any issues with conda
- conda info -a
- conda install python==3.6.8
install:
- conda install numpy
- conda install numpy==1.15
- conda install scipy
- conda install matplotlib
- conda install pandas
- conda install -c conda-forge lapack
- conda install -c cvxgrp cvxpy
- pip install cvxpy
- conda install coveralls
script:

6
AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
View File

@@ -51,12 +51,12 @@ class Quadrotor():
yaw = self.yaw
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch)
* sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
])
def plot(self):
def plot(self): # pragma: no cover
T = self.transformation_matrix()
p1_t = np.matmul(T, self.p1)

BIN
AerialNavigation/drone_3d_trajectory_following/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 12 MiB

4
AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py
View File

@@ -73,8 +73,8 @@ def quad_sim(x_c, y_c, z_c):
# des_x_pos = calculate_position(x_c[i], t)
# des_y_pos = calculate_position(y_c[i], t)
des_z_pos = calculate_position(z_c[i], t)
des_x_vel = calculate_velocity(x_c[i], t)
des_y_vel = calculate_velocity(y_c[i], t)
# des_x_vel = calculate_velocity(x_c[i], t)
# des_y_vel = calculate_velocity(y_c[i], t)
des_z_vel = calculate_velocity(z_c[i], t)
des_x_acc = calculate_acceleration(x_c[i], t)
des_y_acc = calculate_acceleration(y_c[i], t)

BIN
AerialNavigation/rocket_powered_landing/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 929 KiB

56
AerialNavigation/rocket_powered_landing/rocket_powered_landing.ipynb
View File

File diff suppressed because one or more lines are too long

118
AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
View File

@@ -7,7 +7,7 @@ author: Sven Niederberger
Ref:
- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper
by Michael Szmuk and Behçet Açıkmeşe.
by Michael Szmuk and Behcet Acıkmese.
- EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of "Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime
@@ -129,12 +129,12 @@ class Rocket_Model_6DoF:
[vx],
[vy],
[vz],
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy *
(q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 +
2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy *
(q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy
* (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2
+ 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy
* (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
[-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz],
[0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy],
[0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx],
@@ -154,20 +154,20 @@ class Rocket_Model_6DoF:
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz -
q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz +
q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy -
q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy, -
0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz, -
0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz
- q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz
+ q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy
- q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy,
- 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz,
- 0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wy, -0.5 * wz, 0,
0.5 * wx, 0.5 * q3, 0.5 * q0, -0.5 * q1],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy, -
0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy,
- 0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
@@ -184,12 +184,12 @@ class Rocket_Model_6DoF:
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 *
(-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 *
q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) /
m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2
* (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2
* q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3)
/ m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
@@ -227,12 +227,12 @@ class Rocket_Model_6DoF:
def dir_cosine(self, q):
return np.matrix([
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +
q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *
(q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -
q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
+ q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
* (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3]
- q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
])
def omega(self, w):
@@ -460,15 +460,15 @@ class SCProblem:
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
constraints += [
self.var['X'][:, k + 1] ==
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x))
* self.var['X'][:, k]
+ cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u))
* self.var['U'][:, k]
+ cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u))
* self.var['U'][:, k + 1]
+ self.par['S_bar'][:, k] * self.var['sigma']
+ self.par['z_bar'][:, k]
+ self.var['nu'][:, k]
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
self.var['X'][:, k] +
cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
self.var['U'][:, k] +
cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
self.var['U'][:, k + 1] +
self.par['S_bar'][:, k] * self.var['sigma'] +
self.par['z_bar'][:, k] +
self.var['nu'][:, k]
for k in range(K - 1)
]
@@ -486,10 +486,10 @@ class SCProblem:
# Objective:
sc_objective = cvxpy.Minimize(
self.par['weight_sigma'] * self.var['sigma']
+ self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf')
+ self.par['weight_delta'] * self.var['delta_norm']
+ self.par['weight_delta_sigma'] * self.var['sigma_norm']
self.par['weight_sigma'] * self.var['sigma'] +
self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') +
self.par['weight_delta'] * self.var['delta_norm'] +
self.par['weight_delta_sigma'] * self.var['sigma_norm']
)
objective = sc_objective
@@ -550,20 +550,20 @@ class SCProblem:
def axis3d_equal(X, Y, Z, ax):
max_range = np.array([X.max() - X.min(), Y.max() -
Y.min(), Z.max() - Z.min()]).max()
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
max_range = np.array([X.max() - X.min(), Y.max()
- Y.min(), Z.max() - Z.min()]).max()
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
- 1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
- 1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
- 1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
# Comment or uncomment following both lines to test the fake bounding box:
for xb, yb, zb in zip(Xb, Yb, Zb):
ax.plot([xb], [yb], [zb], 'w')
def plot_animation(X, U):
def plot_animation(X, U): # pragma: no cover
fig = plt.figure()
ax = fig.gca(projection='3d')
@@ -576,14 +576,14 @@ def plot_animation(X, U):
axis3d_equal(X[2, :], X[3, :], X[1, :], ax)
rx, ry, rz = X[1:4, k]
vx, vy, vz = X[4:7, k]
# vx, vy, vz = X[4:7, k]
qw, qx, qy, qz = X[7:11, k]
CBI = np.array([
[1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
2 * (qx * qz - qw * qy)],
[2 * (qx * qy - qw * qz), 1 - 2 *
(qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
[2 * (qx * qy - qw * qz), 1 - 2
* (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
[2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
1 - 2 * (qx ** 2 + qy ** 2)]
])
@@ -618,8 +618,6 @@ def main():
integrator = Integrator(m, K)
problem = SCProblem(m, K)
last_linear_cost = None
converged = False
w_delta = W_DELTA
for it in range(iterations):
@@ -633,7 +631,7 @@ def main():
X_last=X, U_last=U, sigma_last=sigma,
weight_sigma=W_SIGMA, weight_nu=W_NU,
weight_delta=w_delta, weight_delta_sigma=W_DELTA_SIGMA)
info = problem.solve()
problem.solve()
X = problem.get_variable('X')
U = problem.get_variable('U')
@@ -658,7 +656,7 @@ def main():
print(f'Converged after {it + 1} iterations.')
break
if show_animation:
if show_animation: # pragma: no cover
plot_animation(X, U)
print("done!!")

BIN
ArmNavigation/arm_obstacle_navigation/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.9 MiB

88
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
View File

@@ -60,8 +60,8 @@ def detect_collision(line_seg, circle):
closest_point = a_vec + line_vec * proj / line_mag
if np.linalg.norm(closest_point - c_vec) > radius:
return False
else:
return True
return True
def get_occupancy_grid(arm, obstacles):
@@ -74,7 +74,7 @@ def get_occupancy_grid(arm, obstacles):
Args:
arm: An instance of NLinkArm
obstacles: A list of obstacles, with each obstacle defined as a list
of xy coordinates and a radius.
of xy coordinates and a radius.
Returns:
Occupancy grid in joint space
@@ -120,16 +120,7 @@ def astar_torus(grid, start_node, goal_node):
parent_map = [[() for _ in range(M)] for _ in range(M)]
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
i + 1 + heuristic_map[M - 1, j],
M - i + heuristic_map[0, j],
j + 1 + heuristic_map[i, M - 1],
M - j + heuristic_map[i, 0]
)
heuristic_map = calc_heuristic_map(M, goal_node)
explored_heuristic_map = np.full((M, M), np.inf)
distance_map = np.full((M, M), np.inf)
@@ -150,26 +141,7 @@ def astar_torus(grid, start_node, goal_node):
i, j = current_node[0], current_node[1]
neighbors = []
if i - 1 >= 0:
neighbors.append((i - 1, j))
else:
neighbors.append((M - 1, j))
if i + 1 < M:
neighbors.append((i + 1, j))
else:
neighbors.append((0, j))
if j - 1 >= 0:
neighbors.append((i, j - 1))
else:
neighbors.append((i, M - 1))
if j + 1 < M:
neighbors.append((i, j + 1))
else:
neighbors.append((i, 0))
neighbors = find_neighbors(i, j)
for neighbor in neighbors:
if grid[neighbor] == 0 or grid[neighbor] == 5:
@@ -177,19 +149,13 @@ def astar_torus(grid, start_node, goal_node):
explored_heuristic_map[neighbor] = heuristic_map[neighbor]
parent_map[neighbor[0]][neighbor[1]] = current_node
grid[neighbor] = 3
'''
plt.cla()
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
plt.show()
plt.pause(1e-5)
'''
if np.isinf(explored_heuristic_map[goal_node]):
route = []
print("No route found.")
else:
route = [goal_node]
while parent_map[route[0][0]][route[0][1]] is not ():
while parent_map[route[0][0]][route[0][1]] != ():
route.insert(0, parent_map[route[0][0]][route[0][1]])
print("The route found covers %d grid cells." % len(route))
@@ -203,6 +169,46 @@ def astar_torus(grid, start_node, goal_node):
return route
def find_neighbors(i, j):
neighbors = []
if i - 1 >= 0:
neighbors.append((i - 1, j))
else:
neighbors.append((M - 1, j))
if i + 1 < M:
neighbors.append((i + 1, j))
else:
neighbors.append((0, j))
if j - 1 >= 0:
neighbors.append((i, j - 1))
else:
neighbors.append((i, M - 1))
if j + 1 < M:
neighbors.append((i, j + 1))
else:
neighbors.append((i, 0))
return neighbors
def calc_heuristic_map(M, goal_node):
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
i + 1 + heuristic_map[M - 1, j],
M - i + heuristic_map[0, j],
j + 1 + heuristic_map[i, M - 1],
M - j + heuristic_map[i, 0]
)
return heuristic_map
class NLinkArm(object):
"""
Class for controlling and plotting a planar arm with an arbitrary number of links.
@@ -235,7 +241,7 @@ class NLinkArm(object):
self.end_effector = np.array(self.points[self.n_links]).T
def plot(self, obstacles=[]):
def plot(self, obstacles=[]): # pragma: no cover
plt.cla()
for obstacle in obstacles:

98
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
View File

@@ -53,7 +53,7 @@ def animate(grid, arm, route):
theta2 = 2 * pi * node[1] / M - pi
arm.update_joints([theta1, theta2])
plt.subplot(1, 2, 2)
arm.plot(plt, obstacles=obstacles)
arm.plot_arm(plt, obstacles=obstacles)
plt.xlim(-2.0, 2.0)
plt.ylim(-3.0, 3.0)
plt.show()
@@ -92,8 +92,7 @@ def detect_collision(line_seg, circle):
closest_point = a_vec + line_vec * proj / line_mag
if np.linalg.norm(closest_point - c_vec) > radius:
return False
else:
return True
return True
def get_occupancy_grid(arm, obstacles):
@@ -143,21 +142,16 @@ def astar_torus(grid, start_node, goal_node):
Returns:
Obstacle-free route in joint space from start_node to goal_node
"""
colors = ['white', 'black', 'red', 'pink', 'yellow', 'green', 'orange']
levels = [0, 1, 2, 3, 4, 5, 6, 7]
cmap, norm = from_levels_and_colors(levels, colors)
grid[start_node] = 4
grid[goal_node] = 5
parent_map = [[() for _ in range(M)] for _ in range(M)]
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
i + 1 + heuristic_map[M - 1, j],
M - i + heuristic_map[0, j],
j + 1 + heuristic_map[i, M - 1],
M - j + heuristic_map[i, 0]
)
heuristic_map = calc_heuristic_map(M, goal_node)
explored_heuristic_map = np.full((M, M), np.inf)
distance_map = np.full((M, M), np.inf)
@@ -178,26 +172,7 @@ def astar_torus(grid, start_node, goal_node):
i, j = current_node[0], current_node[1]
neighbors = []
if i - 1 >= 0:
neighbors.append((i - 1, j))
else:
neighbors.append((M - 1, j))
if i + 1 < M:
neighbors.append((i + 1, j))
else:
neighbors.append((0, j))
if j - 1 >= 0:
neighbors.append((i, j - 1))
else:
neighbors.append((i, M - 1))
if j + 1 < M:
neighbors.append((i, j + 1))
else:
neighbors.append((i, 0))
neighbors = find_neighbors(i, j)
for neighbor in neighbors:
if grid[neighbor] == 0 or grid[neighbor] == 5:
@@ -205,25 +180,66 @@ def astar_torus(grid, start_node, goal_node):
explored_heuristic_map[neighbor] = heuristic_map[neighbor]
parent_map[neighbor[0]][neighbor[1]] = current_node
grid[neighbor] = 3
'''
plt.cla()
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
plt.show()
plt.pause(1e-5)
'''
if np.isinf(explored_heuristic_map[goal_node]):
route = []
print("No route found.")
else:
route = [goal_node]
while parent_map[route[0][0]][route[0][1]] is not ():
while parent_map[route[0][0]][route[0][1]] != ():
route.insert(0, parent_map[route[0][0]][route[0][1]])
print("The route found covers %d grid cells." % len(route))
for i in range(1, len(route)):
grid[route[i]] = 6
plt.cla()
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
plt.show()
plt.pause(1e-2)
return route
def find_neighbors(i, j):
neighbors = []
if i - 1 >= 0:
neighbors.append((i - 1, j))
else:
neighbors.append((M - 1, j))
if i + 1 < M:
neighbors.append((i + 1, j))
else:
neighbors.append((0, j))
if j - 1 >= 0:
neighbors.append((i, j - 1))
else:
neighbors.append((i, M - 1))
if j + 1 < M:
neighbors.append((i, j + 1))
else:
neighbors.append((i, 0))
return neighbors
def calc_heuristic_map(M, goal_node):
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
for i in range(heuristic_map.shape[0]):
for j in range(heuristic_map.shape[1]):
heuristic_map[i, j] = min(heuristic_map[i, j],
i + 1 + heuristic_map[M - 1, j],
M - i + heuristic_map[0, j],
j + 1 + heuristic_map[i, M - 1],
M - j + heuristic_map[i, 0]
)
return heuristic_map
class NLinkArm(object):
"""
Class for controlling and plotting a planar arm with an arbitrary number of links.
@@ -256,7 +272,7 @@ class NLinkArm(object):
self.end_effector = np.array(self.points[self.n_links]).T
def plot(self, myplt, obstacles=[]):
def plot_arm(self, myplt, obstacles=[]): # pragma: no cover
myplt.cla()
for obstacle in obstacles:

6
ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py
View File

@@ -22,7 +22,7 @@ class NLinkArm(object):
self.lim = sum(link_lengths)
self.goal = np.array(goal).T
if show_animation:
if show_animation: # pragma: no cover
self.fig = plt.figure()
self.fig.canvas.mpl_connect('button_press_event', self.click)
@@ -46,10 +46,10 @@ class NLinkArm(object):
np.sin(np.sum(self.joint_angles[:i]))
self.end_effector = np.array(self.points[self.n_links]).T
if self.show_animation:
if self.show_animation: # pragma: no cover
self.plot()
def plot(self):
def plot(self): # pragma: no cover
plt.cla()
for i in range(self.n_links + 1):

BIN
ArmNavigation/n_joint_arm_to_point_control/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.5 MiB

6
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -21,7 +21,7 @@ MOVING_TO_GOAL = 2
show_animation = True
def main():
def main(): # pragma: no cover
"""
Creates an arm using the NLinkArm class and uses its inverse kinematics
to move it to the desired position.
@@ -159,5 +159,5 @@ def ang_diff(theta1, theta2):
if __name__ == '__main__':
main()
# animation()
# main()
animation()

7
ArmNavigation/two_joint_arm_to_point_control/Planar_Two_Link_IK.ipynb
View File

@@ -6,10 +6,7 @@
"source": [
"## Two joint arm to point control\n",
"\n",
"![TwoJointArmToPointControl](https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif)\n",
"\n",
"\n",
"\n",
"![TwoJointArmToPointControl](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif)\n",
"\n",
"This is two joint arm to a point control simulation.\n",
"\n",
@@ -418,7 +415,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.7"
"version": "3.6.8"
}
},
"nbformat": 4,

BIN
ArmNavigation/two_joint_arm_to_point_control/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.1 MiB

6
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -61,7 +61,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
return theta1, theta2
def plot_arm(theta1, theta2, x, y):
def plot_arm(theta1, theta2, x, y): # pragma: no cover
shoulder = np.array([0, 0])
elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
wrist = elbow + \
@@ -94,7 +94,7 @@ def ang_diff(theta1, theta2):
return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
def click(event):
def click(event): # pragma: no cover
global x, y
x = event.xdata
y = event.ydata
@@ -111,7 +111,7 @@ def animation():
GOAL_TH=0.01, theta1=theta1, theta2=theta2)
def main():
def main(): # pragma: no cover
fig = plt.figure()
fig.canvas.mpl_connect("button_press_event", click)
two_joint_arm()

0
PathTracking/.gitignore → Bipedal/__init__.py
View File

171
Bipedal/bipedal_planner/bipedal_planner.py Normal file
View File

@@ -0,0 +1,171 @@
"""
Bipedal Walking with modifying designated footsteps
author: Takayuki Murooka (takayuki5168)
"""
import numpy as np
import math
from matplotlib import pyplot as plt
import matplotlib.patches as pat
from mpl_toolkits.mplot3d import Axes3D
import mpl_toolkits.mplot3d.art3d as art3d
class BipedalPlanner(object):
def __init__(self):
self.ref_footsteps = None
self.g = 9.8
def set_ref_footsteps(self, ref_footsteps):
self.ref_footsteps = ref_footsteps
def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, time_width):
time_split = 100
for i in range(time_split):
delta_time = time_width / time_split
x_dot2 = self.g / z_c * (x - px_star)
x += x_dot * delta_time
x_dot += x_dot2 * delta_time
y_dot2 = self.g / z_c * (y - py_star)
y += y_dot * delta_time
y_dot += y_dot2 * delta_time
if i % 10 == 0:
self.com_trajectory.append([x, y])
return x, x_dot, y, y_dot
def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
if self.ref_footsteps is None:
print("No footsteps")
return
# set up plotter
if plot:
fig = plt.figure()
ax = Axes3D(fig)
com_trajectory_for_plot = []
self.com_trajectory = []
self.ref_p = [] # reference footstep positions
self.act_p = [] # actual footstep positions
px, py = 0.0, 0.0 # reference footstep position
px_star, py_star = px, py # modified footstep position
xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0
time = 0.0
n = 0
self.ref_p.append([px, py, 0])
self.act_p.append([px, py, 0])
for i in range(len(self.ref_footsteps)):
# simulate x, y and those of dot of inverted pendulum
xi, xi_dot, yi, yi_dot = self.inverted_pendulum(
xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, T_sup)
# update time
time += T_sup
n += 1
# calculate px, py, x_, y_, vx_, vy_
f_x, f_y, f_theta = self.ref_footsteps[n - 1]
rotate_mat = np.array([[math.cos(f_theta), -math.sin(f_theta)],
[math.sin(f_theta), math.cos(f_theta)]])
if n == len(self.ref_footsteps):
f_x_next, f_y_next, f_theta_next = 0., 0., 0.
else:
f_x_next, f_y_next, f_theta_next = self.ref_footsteps[n]
rotate_mat_next = np.array([[math.cos(f_theta_next), -math.sin(f_theta_next)],
[math.sin(f_theta_next), math.cos(f_theta_next)]])
T_c = math.sqrt(z_c / self.g)
C = math.cosh(T_sup / T_c)
S = math.sinh(T_sup / T_c)
px, py = list(np.array(
[px, py]) + np.dot(rotate_mat, np.array([f_x, -1 * math.pow(-1, n) * f_y])))
x_, y_ = list(np.dot(rotate_mat_next, np.array(
[f_x_next / 2., math.pow(-1, n) * f_y_next / 2.])))
vx_, vy_ = list(np.dot(rotate_mat_next, np.array(
[(1 + C) / (T_c * S) * x_, (C - 1) / (T_c * S) * y_])))
self.ref_p.append([px, py, f_theta])
# calculate reference COM
xd, xd_dot = px + x_, vx_
yd, yd_dot = py + y_, vy_
# calculate modified footsteps
D = a * math.pow(C - 1, 2) + b * math.pow(S / T_c, 2)
px_star = -a * (C - 1) / D * (xd - C * xi - T_c * S * xi_dot) - \
b * S / (T_c * D) * (xd_dot - S / T_c * xi - C * xi_dot)
py_star = -a * (C - 1) / D * (yd - C * yi - T_c * S * yi_dot) - \
b * S / (T_c * D) * (yd_dot - S / T_c * yi - C * yi_dot)
self.act_p.append([px_star, py_star, f_theta])
# plot
if plot:
# for plot trajectory, plot in for loop
for c in range(len(self.com_trajectory)):
if c > len(com_trajectory_for_plot):
# set up plotter
plt.cla()
ax.set_zlim(0, z_c * 2)
ax.set_aspect('equal', 'datalim')
# update com_trajectory_for_plot
com_trajectory_for_plot.append(self.com_trajectory[c])
# plot com
ax.plot([p[0] for p in com_trajectory_for_plot], [p[1] for p in com_trajectory_for_plot], [
0 for p in com_trajectory_for_plot], color="red")
# plot inverted pendulum
ax.plot([px_star, com_trajectory_for_plot[-1][0]],
[py_star, com_trajectory_for_plot[-1][1]],
[0, z_c], color="green", linewidth=3)
ax.scatter([com_trajectory_for_plot[-1][0]],
[com_trajectory_for_plot[-1][1]],
[z_c], color="green", s=300)
# foot rectangle for self.ref_p
foot_width = 0.06
foot_height = 0.04
for j in range(len(self.ref_p)):
angle = self.ref_p[j][2] + \
math.atan2(foot_height, foot_width) - math.pi
r = math.sqrt(
math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2))
rec = pat.Rectangle(xy=(self.ref_p[j][0] + r * math.cos(angle), self.ref_p[j][1] + r * math.sin(angle)),
width=foot_width, height=foot_height, angle=self.ref_p[j][2] * 180 / math.pi, color="blue", fill=False, ls=":")
ax.add_patch(rec)
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")
# foot rectangle for self.act_p
for j in range(len(self.act_p)):
angle = self.act_p[j][2] + \
math.atan2(foot_height, foot_width) - math.pi
r = math.sqrt(
math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2))
rec = pat.Rectangle(xy=(self.act_p[j][0] + r * math.cos(angle), self.act_p[j][1] + r * math.sin(angle)),
width=foot_width, height=foot_height, angle=self.act_p[j][2] * 180 / math.pi, color="blue", fill=False)
ax.add_patch(rec)
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")
plt.draw()
plt.pause(0.001)
if plot:
plt.show()
if __name__ == "__main__":
bipedal_planner = BipedalPlanner()
footsteps = [[0.0, 0.2, 0.0],
[0.3, 0.2, 0.0],
[0.3, 0.2, 0.2],
[0.3, 0.2, 0.2],
[0.0, 0.2, 0.2]]
bipedal_planner.set_ref_footsteps(footsteps)
bipedal_planner.walk(plot=True)

239
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py Normal file
View File

@@ -0,0 +1,239 @@
"""
Ensemble Kalman Filter(EnKF) localization sample
author: Ryohei Sasaki(rsasaki0109)
Ref:
- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
"""
import numpy as np
import math
import matplotlib.pyplot as plt
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
# Ensemble Kalman filter parameter
NP = 20 # Number of Particle
show_animation = True
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
return u
def observation(xTrue, xd, u, RFID):
xTrue = motion_model(xTrue, u)
z = np.zeros((0, 4))
for i in range(len(RFID[:, 0])):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, anglen, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to input
ud = np.array([[
u[0, 0] + np.random.randn() * Rsim[0, 0],
u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
def motion_model(x, u):
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F.dot(x) + B.dot(u)
return x
def calc_LM_Pos(x, landmarks):
landmarks_pos = np.zeros((2*landmarks.shape[0], 1))
for (i, lm) in enumerate(landmarks):
landmarks_pos[2*i] = x[0, 0] + lm[0] * \
math.cos(x[2, 0] + lm[1]) + np.random.randn() * \
Qsim[0, 0]/np.sqrt(2)
landmarks_pos[2*i+1] = x[1, 0] + lm[0] * \
math.sin(x[2, 0] + lm[1]) + np.random.randn() * \
Qsim[0, 0]/np.sqrt(2)
return landmarks_pos
def calc_covariance(xEst, px):
cov = np.zeros((3, 3))
for i in range(px.shape[1]):
dx = (px[:, i] - xEst)[0:3]
cov += dx.dot(dx.T)
return cov
def enkf_localization(px, xEst, PEst, z, u):
"""
Localization with Ensemble Kalman filter
"""
pz = np.zeros((z.shape[0]*2, NP)) # Particle store of z
for ip in range(NP):
x = np.array([px[:, ip]]).T
# Predict with random input sampling
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud = np.array([[ud1, ud2]]).T
x = motion_model(x, ud)
px[:, ip] = x[:, 0]
z_pos = calc_LM_Pos(x, z)
pz[:, ip] = z_pos[:, 0]
x_ave = np.mean(px, axis=1)
x_dif = px - np.tile(x_ave, (NP, 1)).T
z_ave = np.mean(pz, axis=1)
z_dif = pz - np.tile(z_ave, (NP, 1)).T
U = 1/(NP-1) * x_dif @ z_dif.T
V = 1/(NP-1) * z_dif @ z_dif.T
K = U @ np.linalg.inv(V) # Kalman Gain
z_lm_pos = z[:, [2, 3]].reshape(-1,)
px_hat = px + K @ (np.tile(z_lm_pos, (NP, 1)).T - pz)
xEst = np.average(px_hat, axis=1).reshape(4, 1)
PEst = calc_covariance(xEst, px_hat)
return xEst, PEst, px_hat
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
else:
bigind = 1
smallind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
# close to 0 (~10^-20), catch these cases and set the respective variable to 0
try:
a = math.sqrt(eigval[bigind])
except ValueError:
a = 0
try:
b = math.sqrt(eigval[smallind])
except ValueError:
b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def main():
print(__file__ + " start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.zeros((4, 1))
xTrue = np.zeros((4, 1))
PEst = np.eye(4)
px = np.zeros((4, NP)) # Particle store of x
xDR = np.zeros((4, 1)) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst, px = enkf_localization(px, xEst, PEst, z, ud)
# store data history
hxEst = np.hstack((hxEst, xEst))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation:
plt.cla()
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(px[0, :], px[1, :], ".r")
plt.plot(np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")
plt.plot(np.array(hxDR[0, :]).flatten(),
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
#plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
if __name__ == '__main__':
main()

BIN
Localization/extended_kalman_filter/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 19 MiB

34
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -6,17 +6,22 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import numpy as np
import math
import numpy as np
import matplotlib.pyplot as plt
# Estimation parameter of EKF
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
# Covariance for EKF simulation
Q = np.diag([
0.1, # variance of location on x-axis
0.1, # variance of location on y-axis
np.deg2rad(1.0), # variance of yaw angle
1.0 # variance of velocity
])**2 # predict state covariance
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
# Simulation parameter
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
Rsim = np.diag([0.5, 0.5])**2
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
GPS_NOISE = np.diag([0.5, 0.5])**2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
@@ -27,7 +32,7 @@ show_animation = True
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
u = np.array([[v], [yawrate]])
return u
@@ -36,14 +41,10 @@ def observation(xTrue, xd, u):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
z = np.array([[zx, zy]]).T
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
# add noise to input
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
ud = np.array([[ud1, ud2]]).T
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
xd = motion_model(xd, ud)
@@ -62,19 +63,18 @@ def motion_model(x, u):
[0.0, DT],
[1.0, 0.0]])
x = F@x + B@u
x = F @ x + B @ u
return x
def observation_model(x):
# Observation Model
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
z = H@x
z = H @ x
return z
@@ -134,7 +134,7 @@ def ekf_estimation(xEst, PEst, z, u):
return xEst, PEst
def plot_covariance_ellipse(xEst, PEst):
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
@@ -193,7 +193,7 @@ def main():
if show_animation:
plt.cla()
plt.plot(hz[:, 0], hz[:, 1], ".g")
plt.plot(hz[0, :], hz[1, :], ".g")
plt.plot(hxTrue[0, :].flatten(),
hxTrue[1, :].flatten(), "-b")
plt.plot(hxDR[0, :].flatten(),

4
Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb
View File

@@ -37,7 +37,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"![EKF](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/extended_kalman_filter/animation.gif)\n",
"![EKF](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif)\n",
"\n",
"This is a sensor fusion localization with Extended Kalman Filter(EKF).\n",
"\n",
@@ -256,7 +256,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.7"
"version": "3.6.8"
}
},
"nbformat": 4,

BIN
Localization/histogram_filter/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 7.0 MiB

117
Localization/histogram_filter/histogram_filter.py
View File

@@ -11,15 +11,16 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import copy
from scipy.stats import norm
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.ndimage import gaussian_filter
from scipy.stats import norm
# Parameters
EXTEND_AREA = 10.0 # [m] grid map extention length
EXTEND_AREA = 10.0 # [m] grid map extended length
SIM_TIME = 50.0 # simulation time [s]
DT = 0.1 # time tick [s]
MAX_RANGE = 10.0 # maximum observation range
@@ -33,7 +34,7 @@ MINY = -5.0
MAXX = 15.0
MAXY = 25.0
# simulation paramters
# simulation parameters
NOISE_RANGE = 2.0 # [m] 1σ range noise parameter
NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
@@ -41,11 +42,11 @@ NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
show_animation = True
class grid_map():
class GridMap():
def __init__(self):
self.data = None
self.xyreso = None
self.xy_reso = None
self.minx = None
self.miny = None
self.maxx = None
@@ -56,20 +57,19 @@ class grid_map():
self.dy = 0.0 # movement distance
def histogram_filter_localization(gmap, u, z, yaw):
def histogram_filter_localization(grid_map, u, z, yaw):
grid_map = motion_update(grid_map, u, yaw)
gmap = motion_update(gmap, u, yaw)
grid_map = observation_update(grid_map, z, RANGE_STD)
gmap = observation_update(gmap, z, RANGE_STD)
return gmap
return grid_map
def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
# predicted range
x = ix * gmap.xyreso + gmap.minx
y = iy * gmap.xyreso + gmap.miny
x = ix * gmap.xy_reso + gmap.minx
y = iy * gmap.xy_reso + gmap.miny
d = math.sqrt((x - z[iz, 1])**2 + (y - z[iz, 2])**2)
# likelihood
@@ -93,8 +93,8 @@ def observation_update(gmap, z, std):
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([v, yawrate]).reshape(2, 1)
yaw_rate = 0.1 # [rad/s]
u = np.array([v, yaw_rate]).reshape(2, 1)
return u
@@ -115,9 +115,9 @@ def motion_model(x, u):
return x
def draw_heatmap(data, mx, my):
def draw_heat_map(data, mx, my):
maxp = max([max(igmap) for igmap in data])
plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.Blues)
plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues"))
plt.axis("equal")
@@ -156,60 +156,57 @@ def normalize_probability(gmap):
return gmap
def init_gmap(xyreso, minx, miny, maxx, maxy):
def init_gmap(xy_reso, minx, miny, maxx, maxy):
grid_map = GridMap()
gmap = grid_map()
grid_map.xy_reso = xy_reso
grid_map.minx = minx
grid_map.miny = miny
grid_map.maxx = maxx
grid_map.maxy = maxy
grid_map.xw = int(round((grid_map.maxx - grid_map.minx) / grid_map.xy_reso))
grid_map.yw = int(round((grid_map.maxy - grid_map.miny) / grid_map.xy_reso))
gmap.xyreso = xyreso
gmap.minx = minx
gmap.miny = miny
gmap.maxx = maxx
gmap.maxy = maxy
gmap.xw = int(round((gmap.maxx - gmap.minx) / gmap.xyreso))
gmap.yw = int(round((gmap.maxy - gmap.miny) / gmap.xyreso))
grid_map.data = [[1.0 for _ in range(grid_map.yw)] for _ in range(grid_map.xw)]
grid_map = normalize_probability(grid_map)
gmap.data = [[1.0 for i in range(gmap.yw)] for i in range(gmap.xw)]
gmap = normalize_probability(gmap)
return gmap
return grid_map
def map_shift(gmap, xshift, yshift):
def map_shift(grid_map, x_shift, y_shift):
tgmap = copy.deepcopy(grid_map.data)
tgmap = copy.deepcopy(gmap.data)
for ix in range(grid_map.xw):
for iy in range(grid_map.yw):
nix = ix + x_shift
niy = iy + y_shift
for ix in range(gmap.xw):
for iy in range(gmap.yw):
nix = ix + xshift
niy = iy + yshift
if 0 <= nix < grid_map.xw and 0 <= niy < grid_map.yw:
grid_map.data[ix + x_shift][iy + y_shift] = tgmap[ix][iy]
if nix >= 0 and nix < gmap.xw and niy >= 0 and niy < gmap.yw:
gmap.data[ix + xshift][iy + yshift] = tgmap[ix][iy]
return gmap
return grid_map
def motion_update(gmap, u, yaw):
def motion_update(grid_map, u, yaw):
grid_map.dx += DT * math.cos(yaw) * u[0]
grid_map.dy += DT * math.sin(yaw) * u[0]
gmap.dx += DT * math.cos(yaw) * u[0]
gmap.dy += DT * math.sin(yaw) * u[0]
x_shift = grid_map.dx // grid_map.xy_reso
y_shift = grid_map.dy // grid_map.xy_reso
xshift = gmap.dx // gmap.xyreso
yshift = gmap.dy // gmap.xyreso
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
grid_map.dx -= x_shift * grid_map.xy_reso
grid_map.dy -= y_shift * grid_map.xy_reso
if abs(xshift) >= 1.0 or abs(yshift) >= 1.0: # map should be shifted
gmap = map_shift(gmap, int(xshift), int(yshift))
gmap.dx -= xshift * gmap.xyreso
gmap.dy -= yshift * gmap.xyreso
grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
gmap.data = gaussian_filter(gmap.data, sigma=MOTION_STD)
return gmap
return grid_map
def calc_grid_index(gmap):
mx, my = np.mgrid[slice(gmap.minx - gmap.xyreso / 2.0, gmap.maxx + gmap.xyreso / 2.0, gmap.xyreso),
slice(gmap.miny - gmap.xyreso / 2.0, gmap.maxy + gmap.xyreso / 2.0, gmap.xyreso)]
mx, my = np.mgrid[slice(gmap.minx - gmap.xy_reso / 2.0, gmap.maxx + gmap.xy_reso / 2.0, gmap.xy_reso),
slice(gmap.miny - gmap.xy_reso / 2.0, gmap.maxy + gmap.xy_reso / 2.0, gmap.xy_reso)]
return mx, my
@@ -226,8 +223,8 @@ def main():
time = 0.0
xTrue = np.zeros((4, 1))
gmap = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
mx, my = calc_grid_index(gmap) # for grid map visualization
grid_map = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
mx, my = calc_grid_index(grid_map) # for grid map visualization
while SIM_TIME >= time:
time += DT
@@ -238,11 +235,11 @@ def main():
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = observation(xTrue, u, RFID)
gmap = histogram_filter_localization(gmap, u, z, yaw)
grid_map = histogram_filter_localization(grid_map, u, z, yaw)
if show_animation:
plt.cla()
draw_heatmap(gmap.data, mx, my)
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RFID[:, 0], RFID[:, 1], ".k")
for i in range(z.shape[0]):

BIN
Localization/particle_filter/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 7.7 MiB

2
Localization/particle_filter/particle_filter.py
View File

@@ -156,7 +156,7 @@ def resampling(px, pw):
return px, pw
def plot_covariance_ellipse(xEst, PEst):
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)

BIN
Localization/unscented_kalman_filter/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 10 MiB

33
Localization/unscented_kalman_filter/unscented_kalman_filter.py
View File

@@ -11,13 +11,18 @@ import scipy.linalg
import math
import matplotlib.pyplot as plt
# Estimation parameter of UKF
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
R = np.diag([1.0, np.deg2rad(40.0)])**2
# Covariance for UKF simulation
Q = np.diag([
0.1, # variance of location on x-axis
0.1, # variance of location on y-axis
np.deg2rad(1.0), # variance of yaw angle
1.0 # variance of velocity
])**2 # predict state covariance
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
# Simulation parameter
Qsim = np.diag([0.5, 0.5])**2
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
GPS_NOISE = np.diag([0.5, 0.5])**2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
@@ -42,14 +47,10 @@ def observation(xTrue, xd, u):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
z = np.array([[zx, zy]]).T
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
# add noise to input
ud1 = u[0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1] + np.random.randn() * Rsim[1, 1]
ud = np.array([ud1, ud2])
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
xd = motion_model(xd, ud)
@@ -100,7 +101,9 @@ def generate_sigma_points(xEst, PEst, gamma):
def predict_sigma_motion(sigma, u):
# Sigma Points prediction with motion model
"""
Sigma Points prediction with motion model
"""
for i in range(sigma.shape[1]):
sigma[:, i:i + 1] = motion_model(sigma[:, i:i + 1], u)
@@ -108,7 +111,9 @@ def predict_sigma_motion(sigma, u):
def predict_sigma_observation(sigma):
# Sigma Points prediction with observation model
"""
Sigma Points prediction with observation model
"""
for i in range(sigma.shape[1]):
sigma[0:2, i] = observation_model(sigma[:, i])
@@ -161,7 +166,7 @@ def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
return xEst, PEst
def plot_covariance_ellipse(xEst, PEst):
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)

BIN
Mapping/circle_fitting/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 216 KiB

8
Mapping/circle_fitting/circle_fitting.py
View File

@@ -75,13 +75,13 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
rangedb = [float("inf") for _ in range(
int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
for i in range(len(thetal)):
for i, _ in enumerate(thetal):
angleid = math.floor(thetal[i] / angle_reso)
if rangedb[angleid] > rangel[i]:
rangedb[angleid] = rangel[i]
for i in range(len(rangedb)):
for i, _ in enumerate(rangedb):
t = i * angle_reso
if rangedb[i] != float("inf"):
rx.append(rangedb[i] * math.cos(t))
@@ -90,7 +90,7 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
return rx, ry
def plot_circle(x, y, size, color="-b"):
def plot_circle(x, y, size, color="-b"): # pragma: no cover
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
@@ -122,7 +122,7 @@ def main():
ex, ey, er, error = circle_fitting(x, y)
print("Error:", error)
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.axis("equal")
plt.plot(0.0, 0.0, "*r")

BIN
Mapping/gaussian_grid_map/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 238 KiB

2
Mapping/gaussian_grid_map/gaussian_grid_map.py
View File

@@ -71,7 +71,7 @@ def main():
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD)
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")

260
Mapping/grid_map_lib/grid_map_lib.py Normal file
View File

@@ -0,0 +1,260 @@
"""
Grid map library in python
author: Atsushi Sakai
"""
import matplotlib.pyplot as plt
import numpy as np
class GridMap:
"""
GridMap class
"""
def __init__(self, width, height, resolution,
center_x, center_y, init_val=0.0):
"""__init__
:param width: number of grid for width
:param height: number of grid for heigt
:param resolution: grid resolution [m]
:param center_x: center x position [m]
:param center_y: center y position [m]
:param init_val: initial value for all grid
"""
self.width = width
self.height = height
self.resolution = resolution
self.center_x = center_x
self.center_y = center_y
self.left_lower_x = self.center_x - \
(self.width / 2.0) * self.resolution
self.left_lower_y = self.center_y - \
(self.height / 2.0) * self.resolution
self.ndata = self.width * self.height
self.data = [init_val] * self.ndata
def get_value_from_xy_index(self, x_ind, y_ind):
"""get_value_from_xy_index
when the index is out of grid map area, return None
:param x_ind: x index
:param y_ind: y index
"""
grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
if 0 <= grid_ind <= self.ndata:
return self.data[grid_ind]
else:
return None
def get_xy_index_from_xy_pos(self, x_pos, y_pos):
"""get_xy_index_from_xy_pos
:param x_pos: x position [m]
:param y_pos: y position [m]
"""
x_ind = self.calc_xy_index_from_position(
x_pos, self.left_lower_x, self.width)
y_ind = self.calc_xy_index_from_position(
y_pos, self.left_lower_y, self.height)
return x_ind, y_ind
def set_value_from_xy_pos(self, x_pos, y_pos, val):
"""set_value_from_xy_pos
return bool flag, which means setting value is succeeded or not
:param x_pos: x position [m]
:param y_pos: y position [m]
:param val: grid value
"""
x_ind, y_ind = self.get_xy_index_from_xy_pos(x_pos, y_pos)
if (not x_ind) or (not y_ind):
return False # NG
flag = self.set_value_from_xy_index(x_ind, y_ind, val)
return flag
def set_value_from_xy_index(self, x_ind, y_ind, val):
"""set_value_from_xy_index
return bool flag, which means setting value is succeeded or not
:param x_ind: x index
:param y_ind: y index
:param val: grid value
"""
if (x_ind is None) or (y_ind is None):
print(x_ind, y_ind)
return False, False
grid_ind = int(y_ind * self.width + x_ind)
if 0 <= grid_ind < self.ndata:
self.data[grid_ind] = val
return True # OK
else:
return False # NG
def set_value_from_polygon(self, pol_x, pol_y, val, inside=True):
"""set_value_from_polygon
Setting value inside or outside polygon
:param pol_x: x position list for a polygon
:param pol_y: y position list for a polygon
:param val: grid value
:param inside: setting data inside or outside
"""
# making ring polygon
if (pol_x[0] != pol_x[-1]) or (pol_y[0] != pol_y[-1]):
pol_x.append(pol_x[0])
pol_y.append(pol_y[0])
# setting value for all grid
for x_ind in range(self.width):
for y_ind in range(self.height):
x_pos, y_pos = self.calc_grid_central_xy_position_from_xy_index(
x_ind, y_ind)
flag = self.check_inside_polygon(x_pos, y_pos, pol_x, pol_y)
if flag is inside:
self.set_value_from_xy_index(x_ind, y_ind, val)
def calc_grid_index_from_xy_index(self, x_ind, y_ind):
grid_ind = int(y_ind * self.width + x_ind)
return grid_ind
def calc_grid_central_xy_position_from_xy_index(self, x_ind, y_ind):
x_pos = self.calc_grid_central_xy_position_from_index(
x_ind, self.left_lower_x)
y_pos = self.calc_grid_central_xy_position_from_index(
y_ind, self.left_lower_y)
return x_pos, y_pos
def calc_grid_central_xy_position_from_index(self, index, lower_pos):
return lower_pos + index * self.resolution + self.resolution / 2.0
def calc_xy_index_from_position(self, pos, lower_pos, max_index):
ind = int(np.floor((pos - lower_pos) / self.resolution))
if 0 <= ind <= max_index:
return ind
else:
return None
def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
val = self.get_value_from_xy_index(xind, yind)
if val >= occupied_val:
return True
else:
return False
def expand_grid(self):
xinds, yinds = [], []
for ix in range(self.width):
for iy in range(self.height):
if self.check_occupied_from_xy_index(ix, iy):
xinds.append(ix)
yinds.append(iy)
for (ix, iy) in zip(xinds, yinds):
self.set_value_from_xy_index(ix + 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy + 1, val=1.0)
self.set_value_from_xy_index(ix + 1, iy + 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy - 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy - 1, val=1.0)
@staticmethod
def check_inside_polygon(iox, ioy, x, y):
npoint = len(x) - 1
inside = False
for i1 in range(npoint):
i2 = (i1 + 1) % (npoint + 1)
if x[i1] >= x[i2]:
min_x, max_x = x[i2], x[i1]
else:
min_x, max_x = x[i1], x[i2]
if not min_x < iox < max_x:
continue
if (y[i1] + (y[i2] - y[i1]) / (x[i2] - x[i1])
* (iox - x[i1]) - ioy) > 0.0:
inside = not inside
return inside
def plot_grid_map(self, ax=None):
grid_data = np.reshape(np.array(self.data), (self.height, self.width))
if not ax:
fig, ax = plt.subplots()
heat_map = ax.pcolor(grid_data, cmap="Blues", vmin=0.0, vmax=1.0)
plt.axis("equal")
# plt.show()
return heat_map
def test_polygon_set():
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.plot_grid_map()
plt.axis("equal")
plt.grid(True)
def test_position_set():
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
grid_map.plot_grid_map()
def main():
print("start!!")
test_position_set()
test_polygon_set()
plt.show()
print("done!!")
if __name__ == '__main__':
main()

BIN
Mapping/kmeans_clustering/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 144 KiB

6
Mapping/kmeans_clustering/kmeans_clustering.py
View File

@@ -73,7 +73,7 @@ def update_clusters(clusters):
mind = min(dlist)
min_id = dlist.index(mind)
clusters.labels[ip] = min_id
cost += min_id
cost += mind
return clusters, cost
@@ -124,7 +124,7 @@ def calc_association(cx, cy, clusters):
inds = []
for ic in range(len(cx)):
for ic, _ in enumerate(cx):
tcx = cx[ic]
tcy = cy[ic]
@@ -161,7 +161,7 @@ def main():
clusters = kmeans_clustering(rx, ry, ncluster)
# for animation
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
inds = calc_association(cx, cy, clusters)
for ic in inds:

BIN
Mapping/lidar_to_grid_map/animation.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 279 KiB

BIN
Mapping/lidar_to_grid_map/grid_map_example.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 372 KiB

154
Mapping/lidar_to_grid_map/lidar01.csv Normal file
View File

@@ -0,0 +1,154 @@
0.008450416037156572,0.5335
0.046902201120156306,0.5345
0.08508127850753233,0.537
0.1979822644959155,0.2605
0.21189035697274505,0.2625
0.2587960806200922,0.26475
0.3043382657893199,0.2675
0.34660795861105775,0.27075
0.43632879047139106,0.59
0.4739624524675188,0.60025
0.5137777760286397,0.611
0.5492297764597742,0.6265
0.5895905154121426,0.64
0.6253152235389017,0.6565
0.6645851317087743,0.676
0.6997644244442851,0.6975
0.7785769484796541,0.3345
0.7772134100015329,0.74575
0.8652979956881222,0.3315
0.8996591653367609,0.31775
0.9397471965935056,0.31275
0.9847439663714841,0.31125
1.0283771976713423,0.31325
1.0641019057981014,0.31975
1.1009174447073562,0.3335
1.2012738766970301,0.92275
1.2397256617800307,0.95325
1.2779047391674068,0.9865
1.316629231946031,1.01775
1.3561718478115274,1.011
1.3948963405901518,1.0055
1.4330754179775278,1.00225
1.4731634492342724,0.99975
1.5113425266216485,0.9975
1.5517032655740168,1.001
1.5896096352657691,1.00275
1.6288795434356418,1.008
1.6684221593011381,1.0135
1.7066012366885142,1.022
1.7453257294671385,1.02875
1.7862318838107551,0.9935
1.8257744996762515,1.0025
1.8661352386286207,0.96075
1.9045870237116205,0.92125
1.9465840088377355,0.8855
1.986944747790103,0.85725
2.025669240568728,0.832
2.065757271825472,0.80675
2.1066634261690886,0.78875
2.1464787497302105,0.7705
2.1865667809869542,0.75625
2.2261093968524506,0.74475
2.2683790896741876,0.68275
2.3090125363221823,0.6375
2.3510095214482956,0.59925
2.3916429680962885,0.5665
2.4330945378311526,0.538
2.4783640153047557,0.50825
2.5203610004308707,0.4875
2.562903400948233,0.46825
2.599173524466238,0.45
2.642806755766097,0.4355
2.685076448587836,0.42275
2.722437402888339,0.4125
2.766888757275069,0.40125
2.8007045115324587,0.39525
2.841883373571701,0.385
2.8819714048284446,0.3805
2.922332143780814,0.38575
2.9637837135156797,0.38425
3.0005992524249336,0.36575
3.0401418682904318,0.3765
3.079957191851552,0.3915
3.115409192282687,0.408
3.154679100452558,0.4265
3.184949654666836,0.447
3.2242195628367085,0.4715
3.2574899017028507,0.49875
3.2959416867858504,0.52875
3.3292120256519926,0.564
3.3665729799524957,0.6055
3.4031158111661277,0.6515
3.438022396206014,0.70675
3.4756560582021407,0.771
3.513562427893893,0.77075
3.5522869206725183,0.7785
3.621827383056667,0.79575
3.65918833735717,0.8045
3.697367414744546,0.81725
3.7377281536969154,0.83325
3.775634523388667,0.8415
3.8135408930804187,0.85575
3.8522653858590425,0.87325
3.8898990478551703,0.88725
3.9299870791119154,0.906
3.9665299103255465,0.9265
4.006072526191043,0.94575
4.043978895882795,0.97175
4.081885265574547,1.02075
4.1206097583531704,1.046
4.1587888357405465,1.07025
4.196422497736674,1.097
4.234874282819675,1.12575
4.286688744988257,0.73475
4.324322406984384,0.72225
4.364410438241129,0.731
4.405862007975994,0.7405
4.44267754688525,0.749
4.484129116620115,0.76025
4.522853609398739,0.76825
4.560759979090491,0.77125
4.5989390564778665,0.77725
4.640117918517108,0.782
4.679115118991357,0.78425
4.717294196378733,0.789
4.757109519939853,0.78825
4.796652135805349,0.7855
4.8337403824102285,0.786
4.871646752101981,0.78275
4.9109166602718535,0.7785
4.950186568441726,0.7635
4.990274599698471,0.74725
5.028180969390222,0.737
5.0677235852557185,0.72575
5.109720570381833,0.71525
5.149263186247329,0.70625
5.1863514328522085,0.69975
5.230530079543315,0.693
5.269799987713188,0.68925
5.307979065100563,0.68425
5.347248973270435,0.68275
5.386518881440308,0.68075
5.426606912697053,0.68175
5.465604113171301,0.67825
5.502419652080556,0.6835
5.543871221815422,0.6885
5.580959468420302,0.67925
5.624319992024535,0.6555
5.660044700151294,0.639
5.700950854494911,0.623
5.740220762664784,0.6075
5.783581286269018,0.59475
5.820124117482649,0.58475
5.861848394913139,0.57325
5.899209349213642,0.565
5.938751965079138,0.55525
5.9782945809446355,0.55175
6.017564489114507,0.546
6.059288766544997,0.5405
6.097467843932373,0.53825
6.139464829058487,0.534
6.176825783358991,0.5325
6.215822983833238,0.53125
6.252911230438118,0.53075
1 0.008450416037156572 0.5335
2 0.046902201120156306 0.5345
3 0.08508127850753233 0.537
4 0.1979822644959155 0.2605
5 0.21189035697274505 0.2625
6 0.2587960806200922 0.26475
7 0.3043382657893199 0.2675
8 0.34660795861105775 0.27075
9 0.43632879047139106 0.59
10 0.4739624524675188 0.60025
11 0.5137777760286397 0.611
12 0.5492297764597742 0.6265
13 0.5895905154121426 0.64
14 0.6253152235389017 0.6565
15 0.6645851317087743 0.676
16 0.6997644244442851 0.6975
17 0.7785769484796541 0.3345
18 0.7772134100015329 0.74575
19 0.8652979956881222 0.3315
20 0.8996591653367609 0.31775
21 0.9397471965935056 0.31275
22 0.9847439663714841 0.31125
23 1.0283771976713423 0.31325
24 1.0641019057981014 0.31975
25 1.1009174447073562 0.3335
26 1.2012738766970301 0.92275
27 1.2397256617800307 0.95325
28 1.2779047391674068 0.9865
29 1.316629231946031 1.01775
30 1.3561718478115274 1.011
31 1.3948963405901518 1.0055
32 1.4330754179775278 1.00225
33 1.4731634492342724 0.99975
34 1.5113425266216485 0.9975
35 1.5517032655740168 1.001
36 1.5896096352657691 1.00275
37 1.6288795434356418 1.008
38 1.6684221593011381 1.0135
39 1.7066012366885142 1.022
40 1.7453257294671385 1.02875
41 1.7862318838107551 0.9935
42 1.8257744996762515 1.0025
43 1.8661352386286207 0.96075
44 1.9045870237116205 0.92125
45 1.9465840088377355 0.8855
46 1.986944747790103 0.85725
47 2.025669240568728 0.832
48 2.065757271825472 0.80675
49 2.1066634261690886 0.78875
50 2.1464787497302105 0.7705
51 2.1865667809869542 0.75625
52 2.2261093968524506 0.74475
53 2.2683790896741876 0.68275
54 2.3090125363221823 0.6375
55 2.3510095214482956 0.59925
56 2.3916429680962885 0.5665
57 2.4330945378311526 0.538
58 2.4783640153047557 0.50825
59 2.5203610004308707 0.4875
60 2.562903400948233 0.46825
61 2.599173524466238 0.45
62 2.642806755766097 0.4355
63 2.685076448587836 0.42275
64 2.722437402888339 0.4125
65 2.766888757275069 0.40125
66 2.8007045115324587 0.39525
67 2.841883373571701 0.385
68 2.8819714048284446 0.3805
69 2.922332143780814 0.38575
70 2.9637837135156797 0.38425
71 3.0005992524249336 0.36575
72 3.0401418682904318 0.3765
73 3.079957191851552 0.3915
74 3.115409192282687 0.408
75 3.154679100452558 0.4265
76 3.184949654666836 0.447
77 3.2242195628367085 0.4715
78 3.2574899017028507 0.49875
79 3.2959416867858504 0.52875
80 3.3292120256519926 0.564
81 3.3665729799524957 0.6055
82 3.4031158111661277 0.6515
83 3.438022396206014 0.70675
84 3.4756560582021407 0.771
85 3.513562427893893 0.77075
86 3.5522869206725183 0.7785
87 3.621827383056667 0.79575
88 3.65918833735717 0.8045
89 3.697367414744546 0.81725
90 3.7377281536969154 0.83325
91 3.775634523388667 0.8415
92 3.8135408930804187 0.85575
93 3.8522653858590425 0.87325
94 3.8898990478551703 0.88725
95 3.9299870791119154 0.906
96 3.9665299103255465 0.9265
97 4.006072526191043 0.94575
98 4.043978895882795 0.97175
99 4.081885265574547 1.02075
100 4.1206097583531704 1.046
101 4.1587888357405465 1.07025
102 4.196422497736674 1.097
103 4.234874282819675 1.12575
104 4.286688744988257 0.73475
105 4.324322406984384 0.72225
106 4.364410438241129 0.731
107 4.405862007975994 0.7405
108 4.44267754688525 0.749
109 4.484129116620115 0.76025
110 4.522853609398739 0.76825
111 4.560759979090491 0.77125
112 4.5989390564778665 0.77725
113 4.640117918517108 0.782
114 4.679115118991357 0.78425
115 4.717294196378733 0.789
116 4.757109519939853 0.78825
117 4.796652135805349 0.7855
118 4.8337403824102285 0.786
119 4.871646752101981 0.78275
120 4.9109166602718535 0.7785
121 4.950186568441726 0.7635
122 4.990274599698471 0.74725
123 5.028180969390222 0.737
124 5.0677235852557185 0.72575
125 5.109720570381833 0.71525
126 5.149263186247329 0.70625
127 5.1863514328522085 0.69975
128 5.230530079543315 0.693
129 5.269799987713188 0.68925
130 5.307979065100563 0.68425
131 5.347248973270435 0.68275
132 5.386518881440308 0.68075
133 5.426606912697053 0.68175
134 5.465604113171301 0.67825
135 5.502419652080556 0.6835
136 5.543871221815422 0.6885
137 5.580959468420302 0.67925
138 5.624319992024535 0.6555
139 5.660044700151294 0.639
140 5.700950854494911 0.623
141 5.740220762664784 0.6075
142 5.783581286269018 0.59475
143 5.820124117482649 0.58475
144 5.861848394913139 0.57325
145 5.899209349213642 0.565
146 5.938751965079138 0.55525
147 5.9782945809446355 0.55175
148 6.017564489114507 0.546
149 6.059288766544997 0.5405
150 6.097467843932373 0.53825
151 6.139464829058487 0.534
152 6.176825783358991 0.5325
153 6.215822983833238 0.53125
154 6.252911230438118 0.53075

220
Mapping/lidar_to_grid_map/lidar_to_grid_map.py Normal file
View File

@@ -0,0 +1,220 @@
"""
LIDAR to 2D grid map example
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
from collections import deque
EXTEND_AREA = 1.0
def file_read(f):
"""
Reading LIDAR laser beams (angles and corresponding distance data)
"""
measures = [line.split(",") for line in open(f)]
angles = []
distances = []
for measure in measures:
angles.append(float(measure[0]))
distances.append(float(measure[1]))
angles = np.array(angles)
distances = np.array(distances)
return angles, distances
def bresenham(start, end):
"""
Implementation of Bresenham's line drawing algorithm
See en.wikipedia.org/wiki/Bresenham's_line_algorithm
Bresenham's Line Algorithm
Produces a np.array from start and end (original from roguebasin.com)
>>> points1 = bresenham((4, 4), (6, 10))
>>> print(points1)
np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
"""
# setup initial conditions
x1, y1 = start
x2, y2 = end
dx = x2 - x1
dy = y2 - y1
is_steep = abs(dy) > abs(dx) # determine how steep the line is
if is_steep: # rotate line
x1, y1 = y1, x1
x2, y2 = y2, x2
swapped = False # swap start and end points if necessary and store swap state
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
dx = x2 - x1 # recalculate differentials
dy = y2 - y1 # recalculate differentials
error = int(dx / 2.0) # calculate error
ystep = 1 if y1 < y2 else -1
# iterate over bounding box generating points between start and end
y = y1
points = []
for x in range(x1, x2 + 1):
coord = [y, x] if is_steep else (x, y)
points.append(coord)
error -= abs(dy)
if error < 0:
y += ystep
error += dx
if swapped: # reverse the list if the coordinates were swapped
points.reverse()
points = np.array(points)
return points
def calc_grid_map_config(ox, oy, xyreso):
"""
Calculates the size, and the maximum distances according to the the measurement center
"""
minx = round(min(ox) - EXTEND_AREA / 2.0)
miny = round(min(oy) - EXTEND_AREA / 2.0)
maxx = round(max(ox) + EXTEND_AREA / 2.0)
maxy = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((maxx - minx) / xyreso))
yw = int(round((maxy - miny) / xyreso))
print("The grid map is ", xw, "x", yw, ".")
return minx, miny, maxx, maxy, xw, yw
def atan_zero_to_twopi(y, x):
angle = math.atan2(y, x)
if angle < 0.0:
angle += math.pi * 2.0
return angle
def init_floodfill(cpoint, opoints, xypoints, mincoord, xyreso):
"""
cpoint: center point
opoints: detected obstacles points (x,y)
xypoints: (x,y) point pairs
"""
centix, centiy = cpoint
prev_ix, prev_iy = centix - 1, centiy
ox, oy = opoints
xw, yw = xypoints
minx, miny = mincoord
pmap = (np.ones((xw, yw))) * 0.5
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
free_area = bresenham((prev_ix, prev_iy), (ix, iy))
for fa in free_area:
pmap[fa[0]][fa[1]] = 0 # free area 0.0
prev_ix = ix
prev_iy = iy
return pmap
def flood_fill(cpoint, pmap):
"""
cpoint: starting point (x,y) of fill
pmap: occupancy map generated from Bresenham ray-tracing
"""
# Fill empty areas with queue method
sx, sy = pmap.shape
fringe = deque()
fringe.appendleft(cpoint)
while fringe:
n = fringe.pop()
nx, ny = n
# West
if nx > 0:
if pmap[nx - 1, ny] == 0.5:
pmap[nx - 1, ny] = 0.0
fringe.appendleft((nx - 1, ny))
# East
if nx < sx - 1:
if pmap[nx + 1, ny] == 0.5:
pmap[nx + 1, ny] = 0.0
fringe.appendleft((nx + 1, ny))
# North
if ny > 0:
if pmap[nx, ny - 1] == 0.5:
pmap[nx, ny - 1] = 0.0
fringe.appendleft((nx, ny - 1))
# South
if ny < sy - 1:
if pmap[nx, ny + 1] == 0.5:
pmap[nx, ny + 1] = 0.0
fringe.appendleft((nx, ny + 1))
def generate_ray_casting_grid_map(ox, oy, xyreso, breshen=True):
"""
The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False)
"""
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)]
centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
# occupancy grid computed with bresenham ray casting
if breshen:
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
for laser_beam in laser_beams:
pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
# occupancy grid computed with with flood fill
else:
pmap = init_floodfill((centix, centiy), (ox, oy), (xw, yw), (minx, miny), xyreso)
flood_fill((centix, centiy), pmap)
pmap = np.array(pmap, dtype=np.float)
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso))
iy = int(round((y - miny) / xyreso))
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
return pmap, minx, maxx, miny, maxy, xyreso
def main():
"""
Example usage
"""
print(__file__, "start")
xyreso = 0.02 # x-y grid resolution
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, True)
xyres = np.array(pmap).shape
plt.figure(1, figsize=(10,4))
plt.subplot(122)
plt.imshow(pmap, cmap="PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
plt.clim(-0.4, 1.4)
plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor=True)
plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5)
plt.colorbar()
plt.subplot(121)
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
plt.axis("equal")
plt.plot(0.0, 0.0, "ob")
plt.gca().set_aspect("equal", "box")
bottom, top = plt.ylim() # return the current ylim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
plt.grid(True)
plt.show()
if __name__ == '__main__':
main()

318
Mapping/lidar_to_grid_map/lidar_to_grid_map_tutorial.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Mapping/raycasting_grid_map/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 37 KiB

3
Mapping/raycasting_grid_map/raycasting_grid_map.py
View File

@@ -121,7 +121,8 @@ def main():
oy = (np.random.rand(4) - 0.5) * 10.0
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
ox, oy, xyreso, yawreso)
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")

304
Mapping/rectangle_fitting/rectangle_fitting.py
View File

@@ -1,135 +1,267 @@
"""
Object shape recognition with rectangle fitting
Object shape recognition with L-shape fitting
author: Atsushi Sakai (@Atsushi_twi)
Ref:
- [Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University](https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/)
"""
import matplotlib.pyplot as plt
import math
import random
import numpy as np
import itertools
from enum import Enum
from simulator import VehicleSimulator, LidarSimulator
show_animation = True
def circle_fitting(x, y):
"""
Circle Fitting with least squared
input: point x-y positions
output cxe x center position
cye y center position
re radius of circle
error: prediction error
"""
class LShapeFitting():
sumx = sum(x)
sumy = sum(y)
sumx2 = sum([ix ** 2 for ix in x])
sumy2 = sum([iy ** 2 for iy in y])
sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])
class Criteria(Enum):
AREA = 1
CLOSENESS = 2
VARIANCE = 3
F = np.array([[sumx2, sumxy, sumx],
[sumxy, sumy2, sumy],
[sumx, sumy, len(x)]])
def __init__(self):
# Parameters
self.criteria = self.Criteria.VARIANCE
self.min_dist_of_closeness_crit = 0.01 # [m]
self.dtheta_deg_for_serarch = 1.0 # [deg]
self.R0 = 3.0 # [m] range segmentation param
self.Rd = 0.001 # [m] range segmentation param
G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
[-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
[-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])
def fitting(self, ox, oy):
T = np.linalg.inv(F).dot(G)
# step1: Adaptive Range Segmentation
idsets = self._adoptive_range_segmentation(ox, oy)
cxe = float(T[0] / -2)
cye = float(T[1] / -2)
re = math.sqrt(cxe**2 + cye**2 - T[2])
# step2 Rectangle search
rects = []
for ids in idsets: # for each cluster
cx = [ox[i] for i in range(len(ox)) if i in ids]
cy = [oy[i] for i in range(len(oy)) if i in ids]
rects.append(self._rectangle_search(cx, cy))
error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
return rects, idsets
return (cxe, cye, re, error)
def _calc_area_criterion(self, c1, c2):
c1_max = max(c1)
c2_max = max(c2)
c1_min = min(c1)
c2_min = min(c2)
alpha = -(c1_max - c1_min) * (c2_max - c2_min)
return alpha
def _calc_closeness_criterion(self, c1, c2):
c1_max = max(c1)
c2_max = max(c2)
c1_min = min(c1)
c2_min = min(c2)
D1 = [min([np.linalg.norm(c1_max - ic1),
np.linalg.norm(ic1 - c1_min)]) for ic1 in c1]
D2 = [min([np.linalg.norm(c2_max - ic2),
np.linalg.norm(ic2 - c2_min)]) for ic2 in c2]
beta = 0
for i, _ in enumerate(D1):
d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_crit)
beta += (1.0 / d)
return beta
def _calc_variance_criterion(self, c1, c2):
c1_max = max(c1)
c2_max = max(c2)
c1_min = min(c1)
c2_min = min(c2)
D1 = [min([np.linalg.norm(c1_max - ic1),
np.linalg.norm(ic1 - c1_min)]) for ic1 in c1]
D2 = [min([np.linalg.norm(c2_max - ic2),
np.linalg.norm(ic2 - c2_min)]) for ic2 in c2]
E1, E2 = [], []
for (d1, d2) in zip(D1, D2):
if d1 < d2:
E1.append(d1)
else:
E2.append(d2)
V1 = 0.0
if E1:
V1 = - np.var(E1)
V2 = 0.0
if E2:
V2 = - np.var(E2)
gamma = V1 + V2
return gamma
def _rectangle_search(self, x, y):
X = np.array([x, y]).T
dtheta = np.deg2rad(self.dtheta_deg_for_serarch)
minp = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - dtheta, dtheta):
e1 = np.array([np.cos(theta), np.sin(theta)])
e2 = np.array([-np.sin(theta), np.cos(theta)])
c1 = X @ e1.T
c2 = X @ e2.T
# Select criteria
if self.criteria == self.Criteria.AREA:
cost = self._calc_area_criterion(c1, c2)
elif self.criteria == self.Criteria.CLOSENESS:
cost = self._calc_closeness_criterion(c1, c2)
elif self.criteria == self.Criteria.VARIANCE:
cost = self._calc_variance_criterion(c1, c2)
if minp[0] < cost:
minp = (cost, theta)
# calc best rectangle
sin_s = np.sin(minp[1])
cos_s = np.cos(minp[1])
c1_s = X @ np.array([cos_s, sin_s]).T
c2_s = X @ np.array([-sin_s, cos_s]).T
rect = RectangleData()
rect.a[0] = cos_s
rect.b[0] = sin_s
rect.c[0] = min(c1_s)
rect.a[1] = -sin_s
rect.b[1] = cos_s
rect.c[1] = min(c2_s)
rect.a[2] = cos_s
rect.b[2] = sin_s
rect.c[2] = max(c1_s)
rect.a[3] = -sin_s
rect.b[3] = cos_s
rect.c[3] = max(c2_s)
return rect
def _adoptive_range_segmentation(self, ox, oy):
# Setup initial cluster
S = []
for i, _ in enumerate(ox):
C = set()
R = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]])
for j, _ in enumerate(ox):
d = np.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2)
if d <= R:
C.add(j)
S.append(C)
# Merge cluster
while 1:
no_change = True
for (c1, c2) in list(itertools.permutations(range(len(S)), 2)):
if S[c1] & S[c2]:
S[c1] = (S[c1] | S.pop(c2))
no_change = False
break
if no_change:
break
return S
def get_sample_points(cx, cy, cr, angle_reso):
x, y, angle, r = [], [], [], []
class RectangleData():
# points sampling
for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
nx = cx + cr * math.cos(theta)
ny = cy + cr * math.sin(theta)
nangle = math.atan2(ny, nx)
nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)
def __init__(self):
self.a = [None] * 4
self.b = [None] * 4
self.c = [None] * 4
x.append(nx)
y.append(ny)
angle.append(nangle)
r.append(nr)
self.rect_c_x = [None] * 5
self.rect_c_y = [None] * 5
# ray casting filter
rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
def plot(self):
self.calc_rect_contour()
plt.plot(self.rect_c_x, self.rect_c_y, "-r")
return rx, ry
def calc_rect_contour(self):
self.rect_c_x[0], self.rect_c_y[0] = self.calc_cross_point(
self.a[0:2], self.b[0:2], self.c[0:2])
self.rect_c_x[1], self.rect_c_y[1] = self.calc_cross_point(
self.a[1:3], self.b[1:3], self.c[1:3])
self.rect_c_x[2], self.rect_c_y[2] = self.calc_cross_point(
self.a[2:4], self.b[2:4], self.c[2:4])
self.rect_c_x[3], self.rect_c_y[3] = self.calc_cross_point(
[self.a[3], self.a[0]], [self.b[3], self.b[0]], [self.c[3], self.c[0]])
self.rect_c_x[4], self.rect_c_y[4] = self.rect_c_x[0], self.rect_c_y[0]
def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
rx, ry = [], []
rangedb = [float("inf") for _ in range(
int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
for i in range(len(thetal)):
angleid = math.floor(thetal[i] / angle_reso)
if rangedb[angleid] > rangel[i]:
rangedb[angleid] = rangel[i]
for i in range(len(rangedb)):
t = i * angle_reso
if rangedb[i] != float("inf"):
rx.append(rangedb[i] * math.cos(t))
ry.append(rangedb[i] * math.sin(t))
return rx, ry
def plot_circle(x, y, size, color="-b"):
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
plt.plot(xl, yl, color)
def calc_cross_point(self, a, b, c):
x = (b[0] * -c[1] - b[1] * -c[0]) / (a[0] * b[1] - a[1] * b[0])
y = (a[1] * -c[0] - a[0] * -c[1]) / (a[0] * b[1] - a[1] * b[0])
return x, y
def main():
# simulation parameters
simtime = 15.0 # simulation time
dt = 1.0 # time tick
simtime = 30.0 # simulation time
dt = 0.2 # time tick
cx = -2.0 # initial x position of obstacle
cy = -8.0 # initial y position of obstacle
cr = 1.0 # obstacle radious
theta = np.deg2rad(30.0) # obstacle moving direction
angle_reso = np.deg2rad(3.0) # sensor angle resolution
v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0),
0.0, 50.0 / 3.6, 3.0, 5.0)
v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0),
0.0, 50.0 / 3.6, 4.0, 10.0)
lshapefitting = LShapeFitting()
lidar_sim = LidarSimulator()
time = 0.0
while time <= simtime:
time += dt
cx += math.cos(theta)
cy += math.cos(theta)
v1.update(dt, 0.1, 0.0)
v2.update(dt, 0.1, -0.05)
x, y = get_sample_points(cx, cy, cr, angle_reso)
ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso)
ex, ey, er, error = circle_fitting(x, y)
print("Error:", error)
rects, idsets = lshapefitting.fitting(ox, oy)
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.axis("equal")
plt.plot(0.0, 0.0, "*r")
plot_circle(cx, cy, cr)
plt.plot(x, y, "xr")
plot_circle(ex, ey, er, "-r")
plt.pause(dt)
v1.plot()
v2.plot()
# Plot range observation
for ids in idsets:
x = [ox[i] for i in range(len(ox)) if i in ids]
y = [oy[i] for i in range(len(ox)) if i in ids]
for (ix, iy) in zip(x, y):
plt.plot([0.0, ix], [0.0, iy], "-og")
plt.plot([ox[i] for i in range(len(ox)) if i in ids],
[oy[i] for i in range(len(ox)) if i in ids],
"o")
for rect in rects:
rect.plot()
plt.pause(0.1)
print("Done")

144
Mapping/rectangle_fitting/simulator.py Normal file
View File

@@ -0,0 +1,144 @@
"""
Simulator
author: Atsushi Sakai
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import random
class VehicleSimulator():
def __init__(self, ix, iy, iyaw, iv, max_v, w, L):
self.x = ix
self.y = iy
self.yaw = iyaw
self.v = iv
self.max_v = max_v
self.W = w
self.L = L
self._calc_vehicle_contour()
def update(self, dt, a, omega):
self.x += self.v * np.cos(self.yaw) * dt
self.y += self.v * np.sin(self.yaw) * dt
self.yaw += omega * dt
self.v += a * dt
if self.v >= self.max_v:
self.v = self.max_v
def plot(self):
plt.plot(self.x, self.y, ".b")
# convert global coordinate
gx, gy = self.calc_global_contour()
plt.plot(gx, gy, "--b")
def calc_global_contour(self):
gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) +
self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) +
self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
return gx, gy
def _calc_vehicle_contour(self):
self.vc_x = []
self.vc_y = []
self.vc_x.append(self.L / 2.0)
self.vc_y.append(self.W / 2.0)
self.vc_x.append(self.L / 2.0)
self.vc_y.append(-self.W / 2.0)
self.vc_x.append(-self.L / 2.0)
self.vc_y.append(-self.W / 2.0)
self.vc_x.append(-self.L / 2.0)
self.vc_y.append(self.W / 2.0)
self.vc_x.append(self.L / 2.0)
self.vc_y.append(self.W / 2.0)
self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y)
def _interporate(self, x, y):
rx, ry = [], []
dtheta = 0.05
for i in range(len(x) - 1):
rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)])
ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)])
rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1]
for θ in np.arange(0.0, 1.0, dtheta)])
ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1]
for θ in np.arange(0.0, 1.0, dtheta)])
return rx, ry
class LidarSimulator():
def __init__(self):
self.range_noise = 0.01
def get_observation_points(self, vlist, angle_reso):
x, y, angle, r = [], [], [], []
# store all points
for v in vlist:
gx, gy = v.calc_global_contour()
for vx, vy in zip(gx, gy):
vangle = math.atan2(vy, vx)
vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise,
1.0 + self.range_noise)
x.append(vx)
y.append(vy)
angle.append(vangle)
r.append(vr)
# ray casting filter
rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso)
return rx, ry
def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso):
rx, ry = [], []
rangedb = [float("inf") for _ in range(
int(np.floor((np.pi * 2.0) / angle_reso)) + 1)]
for i in range(len(thetal)):
angleid = int(round(thetal[i] / angle_reso))
if rangedb[angleid] > rangel[i]:
rangedb[angleid] = rangel[i]
for i in range(len(rangedb)):
t = i * angle_reso
if rangedb[i] != float("inf"):
rx.append(rangedb[i] * np.cos(t))
ry.append(rangedb[i] * np.sin(t))
return rx, ry
def main():
print("start!!")
print("done!!")
if __name__ == '__main__':
main()

352
PathPlanning/AStar/a_star.py
View File

@@ -1,6 +1,6 @@
"""
A* grid based planning
A* grid planning
author: Atsushi Sakai(@Atsushi_twi)
Nikos Kanargias (nkana@tee.gr)
@@ -9,177 +9,214 @@ See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
show_animation = True
class Node:
class AStarPlanner:
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
def __init__(self, ox, oy, reso, rr):
"""
Initialize grid map for a star planning
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
self.reso = reso
self.rr = rr
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, pind):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
def planning(self, sx, sy, gx, gy):
"""
A star path search
input:
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
output:
rx: x position list of the final path
ry: y position list of the final path
"""
nstart = self.Node(self.calc_xyindex(sx, self.minx),
self.calc_xyindex(sy, self.miny), 0.0, -1)
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
self.calc_xyindex(gy, self.miny), 0.0, -1)
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
while 1:
if len(open_set) == 0:
print("Open set is empty..")
break
c_id = min(
open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
current = open_set[c_id]
# show graph
if show_animation: # pragma: no cover
plt.plot(self.calc_grid_position(current.x, self.minx),
self.calc_grid_position(current.y, self.miny), "xc")
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# Remove the item from the open set
del open_set[c_id]
# Add it to the closed set
closed_set[c_id] = current
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node)
def calc_fianl_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
return rx, ry
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node # discovered a new node
else:
if open_set[n_id].cost > node.cost:
# This path is the best until now. record it
open_set[n_id] = node
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
rx, ry = self.calc_final_path(ngoal, closed_set)
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
return rx, ry
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
def calc_final_path(self, ngoal, closedset):
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
pind = n.pind
motion = get_motion_model()
return rx, ry
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
@staticmethod
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)
return d
while 1:
c_id = min(
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
def calc_grid_position(self, index, minp):
"""
calc grid position
# show graph
if show_animation:
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
:param index:
:param minp:
:return:
"""
pos = index * self.reso + minp
return pos
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
def calc_xyindex(self, position, min_pos):
return round((position - min_pos) / self.reso)
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
def calc_grid_index(self, node):
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
def verify_node(self, node):
px = self.calc_grid_position(node.x, self.minx)
py = self.calc_grid_position(node.y, self.miny)
if n_id in closedset:
continue
if px < self.minx:
return False
elif py < self.miny:
return False
elif px >= self.maxx:
return False
elif py >= self.maxy:
return False
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
# collision check
if self.obmap[node.x][node.y]:
return False
if n_id not in openset:
openset[n_id] = node # Discover a new node
else:
if openset[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
return True
rx, ry = calc_fianl_path(ngoal, closedset, reso)
def calc_obstacle_map(self, ox, oy):
return rx, ry
self.minx = round(min(ox))
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
print("minx:", self.minx)
print("miny:", self.miny)
print("maxx:", self.maxx)
print("maxy:", self.maxy)
self.xwidth = round((self.maxx - self.minx) / self.reso)
self.ywidth = round((self.maxy - self.miny) / self.reso)
print("xwidth:", self.xwidth)
print("ywidth:", self.ywidth)
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
# obstacle map generation
self.obmap = [[False for i in range(self.ywidth)]
for i in range(self.xwidth)]
for ix in range(self.xwidth):
x = self.calc_grid_position(ix, self.minx)
for iy in range(self.ywidth):
y = self.calc_grid_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
if d <= self.rr:
self.obmap[ix][iy] = True
break
@staticmethod
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x >= maxx:
return False
elif node.y >= maxy:
return False
if obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(ox, oy, reso, vr):
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
# print("minx:", minx)
# print("miny:", miny)
# print("maxx:", maxx)
# print("maxy:", maxy)
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# print("xwidth:", xwidth)
# print("ywidth:", ywidth)
# obstacle map generation
obmap = [[False for i in range(xwidth)] for i in range(ywidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
return motion
def main():
@@ -190,40 +227,41 @@ def main():
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# set obstable positions
ox, oy = [], []
for i in range(60):
for i in range(-10, 60):
ox.append(i)
oy.append(0.0)
for i in range(60):
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
oy.append(i)
for i in range(61):
for i in range(-10, 61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
for i in range(-10, 61):
ox.append(-10.0)
oy.append(i)
for i in range(40):
for i in range(-10, 40):
ox.append(20.0)
oy.append(i)
for i in range(40):
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation:
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
if show_animation:
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()

BIN
PathPlanning/AStar/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.9 MiB

BIN
PathPlanning/BatchInformedRRTStar/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.6 MiB

264
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -14,10 +14,11 @@ author: Karan Chawla(@karanchawla)
Reference: https://arxiv.org/abs/1405.5848
"""
import random
import numpy as np
import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -26,8 +27,14 @@ class RTree(object):
# Class to represent the explicit tree created
# while sampling through the state space
def __init__(self, start=[0, 0], lowerLimit=[0, 0], upperLimit=[10, 10], resolution=1):
def __init__(self, start=None, lowerLimit=None, upperLimit=None, resolution=1.0):
if upperLimit is None:
upperLimit = [10, 10]
if lowerLimit is None:
lowerLimit = [0, 0]
if start is None:
start = [0, 0]
self.vertices = dict()
self.edges = []
self.start = start
@@ -42,20 +49,21 @@ class RTree(object):
self.num_cells[idx] = np.ceil(
(upperLimit[idx] - lowerLimit[idx]) / resolution)
vertex_id = self.realWorldToNodeId(start)
vertex_id = self.real_world_to_node_id(start)
self.vertices[vertex_id] = []
def getRootId(self):
@staticmethod
def get_root_id():
# return the id of the root of the tree
return 0
def addVertex(self, vertex):
def add_vertex(self, vertex):
# add a vertex to the tree
vertex_id = self.realWorldToNodeId(vertex)
vertex_id = self.real_world_to_node_id(vertex)
self.vertices[vertex_id] = []
return vertex_id
def addEdge(self, v, x):
def add_edge(self, v, x):
# create an edge between v and x vertices
if (v, x) not in self.edges:
self.edges.append((v, x))
@@ -63,7 +71,7 @@ class RTree(object):
self.vertices[v].append(x)
self.vertices[x].append(v)
def realCoordsToGridCoord(self, real_coord):
def real_coords_to_grid_coord(self, real_coord):
# convert real world coordinates to grid space
# depends on the resolution of the grid
# the output is the same as real world coords if the resolution
@@ -71,10 +79,10 @@ class RTree(object):
coord = [0] * self.dimension
for i in range(len(coord)):
start = self.lowerLimit[i] # start of the grid space
coord[i] = np.around((real_coord[i] - start) / self.resolution)
coord[i] = int(np.around((real_coord[i] - start) / self.resolution))
return coord
def gridCoordinateToNodeId(self, coord):
def grid_coordinate_to_node_id(self, coord):
# This function maps a grid coordinate to a unique
# node id
nodeId = 0
@@ -85,12 +93,12 @@ class RTree(object):
nodeId = nodeId + coord[i] * product
return nodeId
def realWorldToNodeId(self, real_coord):
def real_world_to_node_id(self, real_coord):
# first convert the given coordinates to grid space and then
# convert the grid space coordinates to a unique node id
return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord))
return self.grid_coordinate_to_node_id(self.real_coords_to_grid_coord(real_coord))
def gridCoordToRealWorldCoord(self, coord):
def grid_coord_to_real_world_coord(self, coord):
# This function smaps a grid coordinate in discrete space
# to a configuration in the full configuration space
config = [0] * self.dimension
@@ -102,7 +110,7 @@ class RTree(object):
config[i] = start + grid_step
return config
def nodeIdToGridCoord(self, node_id):
def node_id_to_grid_coord(self, node_id):
# This function maps a node id to the associated
# grid coordinate
coord = [0] * len(self.lowerLimit)
@@ -115,12 +123,10 @@ class RTree(object):
node_id = node_id - (coord[i] * prod)
return coord
def nodeIdToRealWorldCoord(self, nid):
# This function maps a node in discrete space to a configuraiton
def node_id_to_real_world_coord(self, nid):
# This function maps a node in discrete space to a configuration
# in the full configuration space
return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid))
# Uses Batch Informed Trees to find a path from start to goal
return self.grid_coord_to_real_world_coord(self.node_id_to_grid_coord(nid))
class BITStar(object):
@@ -135,6 +141,8 @@ class BITStar(object):
self.maxrand = randArea[1]
self.maxIter = maxIter
self.obstacleList = obstacleList
self.startId = None
self.goalId = None
self.vertex_queue = []
self.edge_queue = []
@@ -154,8 +162,8 @@ class BITStar(object):
upperLimit=upperLimit, resolution=0.01)
def setup_planning(self):
self.startId = self.tree.realWorldToNodeId(self.start)
self.goalId = self.tree.realWorldToNodeId(self.goal)
self.startId = self.tree.real_world_to_node_id(self.start)
self.goalId = self.tree.real_world_to_node_id(self.goal)
# add goal to the samples
self.samples[self.goalId] = self.goal
@@ -163,30 +171,30 @@ class BITStar(object):
self.f_scores[self.goalId] = 0
# add the start id to the tree
self.tree.addVertex(self.start)
self.tree.add_vertex(self.start)
self.g_scores[self.startId] = 0
self.f_scores[self.startId] = self.computeHeuristicCost(
self.f_scores[self.startId] = self.compute_heuristic_cost(
self.startId, self.goalId)
# max length we expect to find in our 'informed' sample space, starts as infinite
cBest = self.g_scores[self.goalId]
# Computing the sampling space
cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) +
pow(self.start[0] - self.goal[1], 2)) / 1.5
cMin = math.sqrt(pow(self.start[0] - self.goal[0], 2)
+ pow(self.start[1] - self.goal[1], 2)) / 1.5
xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0],
[(self.goal[1] - self.start[1]) / 2.0], [0]])
[(self.start[1] + self.goal[1]) / 2.0], [0]])
a1 = np.array([[(self.goal[0] - self.start[0]) / cMin],
[(self.goal[1] - self.start[1]) / cMin], [0]])
etheta = math.atan2(a1[1], a1[0])
# first column of idenity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = np.dot(a1, id1_t)
U, S, Vh = np.linalg.svd(M, 1, 1)
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)
self.samples.update(self.informedSample(
self.samples.update(self.informed_sample(
200, cBest, cMin, xCenter, C))
return etheta, cMin, xCenter, C, cBest
@@ -207,7 +215,7 @@ class BITStar(object):
else:
m = 100
cBest = self.g_scores[self.goalId]
self.samples.update(self.informedSample(
self.samples.update(self.informed_sample(
m, cBest, cMin, xCenter, C))
# make the old vertices the new vertices
@@ -225,26 +233,25 @@ class BITStar(object):
foundGoal = False
# run until done
while (iterations < self.maxIter):
while iterations < self.maxIter:
cBest = self.setup_sample(iterations,
foundGoal, cMin, xCenter, C, cBest)
# expand the best vertices until an edge is better than the vertex
# this is done because the vertex cost represents the lower bound
# on the edge cost
while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()):
self.expandVertex(self.bestInVertexQueue())
while self.best_vertex_queue_value() <= self.best_edge_queue_value():
self.expand_vertex(self.best_in_vertex_queue())
# add the best edge to the tree
bestEdge = self.bestInEdgeQueue()
bestEdge = self.best_in_edge_queue()
self.edge_queue.remove(bestEdge)
# Check if this can improve the current solution
estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.computeDistanceCost(
bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + self.computeHeuristicCost(
bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
actualCostOfEdge = self.g_scores[bestEdge[0]] + \
self.computeDistanceCost(bestEdge[0], bestEdge[1])
estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.compute_distance_cost(
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
estimatedCostOfEdge = self.compute_distance_cost(self.startId, bestEdge[0]) + self.compute_heuristic_cost(
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
actualCostOfEdge = self.g_scores[bestEdge[0]] + self.compute_distance_cost(bestEdge[0], bestEdge[1])
f1 = estimatedCostOfVertex < self.g_scores[self.goalId]
f2 = estimatedCostOfEdge < self.g_scores[self.goalId]
@@ -252,46 +259,44 @@ class BITStar(object):
if f1 and f2 and f3:
# connect this edge
firstCoord = self.tree.nodeIdToRealWorldCoord(
firstCoord = self.tree.node_id_to_real_world_coord(
bestEdge[0])
secondCoord = self.tree.nodeIdToRealWorldCoord(
secondCoord = self.tree.node_id_to_real_world_coord(
bestEdge[1])
path = self.connect(firstCoord, secondCoord)
lastEdge = self.tree.realWorldToNodeId(secondCoord)
lastEdge = self.tree.real_world_to_node_id(secondCoord)
if path is None or len(path) == 0:
continue
nextCoord = path[len(path) - 1, :]
nextCoordPathId = self.tree.realWorldToNodeId(
nextCoordPathId = self.tree.real_world_to_node_id(
nextCoord)
bestEdge = (bestEdge[0], nextCoordPathId)
if(bestEdge[1] in self.tree.vertices.keys()):
if bestEdge[1] in self.tree.vertices.keys():
continue
else:
try:
del self.samples[bestEdge[1]]
except(KeyError):
except KeyError:
pass
eid = self.tree.addVertex(nextCoord)
eid = self.tree.add_vertex(nextCoord)
self.vertex_queue.append(eid)
if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId:
print("Goal found")
foundGoal = True
self.tree.addEdge(bestEdge[0], bestEdge[1])
self.tree.add_edge(bestEdge[0], bestEdge[1])
g_score = self.computeDistanceCost(
g_score = self.compute_distance_cost(
bestEdge[0], bestEdge[1])
self.g_scores[bestEdge[1]] = g_score + \
self.g_scores[bestEdge[0]]
self.f_scores[bestEdge[1]] = g_score + \
self.computeHeuristicCost(bestEdge[1], self.goalId)
self.updateGraph()
self.g_scores[bestEdge[1]] = g_score + self.g_scores[bestEdge[0]]
self.f_scores[bestEdge[1]] = g_score + self.compute_heuristic_cost(bestEdge[1], self.goalId)
self.update_graph()
# visualize new edge
if animation:
self.drawGraph(xCenter=xCenter, cBest=cBest,
cMin=cMin, etheta=etheta, samples=self.samples.values(),
start=firstCoord, end=secondCoord, tree=self.tree.edges)
self.draw_graph(xCenter=xCenter, cBest=cBest,
cMin=cMin, etheta=etheta, samples=self.samples.values(),
start=firstCoord, end=secondCoord)
self.remove_queue(lastEdge, bestEdge)
@@ -306,15 +311,14 @@ class BITStar(object):
return self.find_final_path()
def find_final_path(self):
plan = []
plan.append(self.goal)
plan = [self.goal]
currId = self.goalId
while (currId != self.startId):
plan.append(self.tree.nodeIdToRealWorldCoord(currId))
while currId != self.startId:
plan.append(self.tree.node_id_to_real_world_coord(currId))
try:
currId = self.nodes[currId]
except(KeyError):
print("Path key error")
except KeyError:
print("cannot find Path")
return []
plan.append(self.start)
@@ -324,29 +328,30 @@ class BITStar(object):
def remove_queue(self, lastEdge, bestEdge):
for edge in self.edge_queue:
if(edge[1] == bestEdge[1]):
if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]:
if(lastEdge, bestEdge[1]) in self.edge_queue:
if edge[1] == bestEdge[1]:
dist_cost = self.compute_distance_cost(edge[1], bestEdge[1])
if self.g_scores[edge[1]] + dist_cost >= self.g_scores[self.goalId]:
if (lastEdge, bestEdge[1]) in self.edge_queue:
self.edge_queue.remove(
(lastEdge, bestEdge[1]))
def connect(self, start, end):
# A function which attempts to extend from a start coordinates
# to goal coordinates
steps = int(self.computeDistanceCost(self.tree.realWorldToNodeId(
start), self.tree.realWorldToNodeId(end)) * 10)
steps = int(self.compute_distance_cost(self.tree.real_world_to_node_id(
start), self.tree.real_world_to_node_id(end)) * 10)
x = np.linspace(start[0], end[0], num=steps)
y = np.linspace(start[1], end[1], num=steps)
for i in range(len(x)):
if(self._collisionCheck(x[i], y[i])):
if(i == 0):
if self._collision_check(x[i], y[i]):
if i == 0:
return None
# if collision, send path until collision
return np.vstack((x[0:i], y[0:i])).transpose()
return np.vstack((x, y)).transpose()
def _collisionCheck(self, x, y):
def _collision_check(self, x, y):
for (ox, oy, size) in self.obstacleList:
dx = ox - x
dy = oy - y
@@ -355,45 +360,44 @@ class BITStar(object):
return True # collision
return False
# def prune(self, c):
def computeHeuristicCost(self, start_id, goal_id):
def compute_heuristic_cost(self, start_id, goal_id):
# Using Manhattan distance as heuristic
start = np.array(self.tree.nodeIdToRealWorldCoord(start_id))
goal = np.array(self.tree.nodeIdToRealWorldCoord(goal_id))
start = np.array(self.tree.node_id_to_real_world_coord(start_id))
goal = np.array(self.tree.node_id_to_real_world_coord(goal_id))
return np.linalg.norm(start - goal, 2)
def computeDistanceCost(self, vid, xid):
def compute_distance_cost(self, vid, xid):
# L2 norm distance
start = np.array(self.tree.nodeIdToRealWorldCoord(vid))
stop = np.array(self.tree.nodeIdToRealWorldCoord(xid))
start = np.array(self.tree.node_id_to_real_world_coord(vid))
stop = np.array(self.tree.node_id_to_real_world_coord(xid))
return np.linalg.norm(stop - start, 2)
# Sample free space confined in the radius of ball R
def informedSample(self, m, cMax, cMin, xCenter, C):
def informed_sample(self, m, cMax, cMin, xCenter, C):
samples = dict()
print("g_Score goal id: ", self.g_scores[self.goalId])
for i in range(m + 1):
if cMax < float('inf'):
r = [cMax / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0]
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
L = np.diag(r)
xBall = self.sampleUnitBall()
xBall = self.sample_unit_ball()
rnd = np.dot(np.dot(C, L), xBall) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
random_id = self.tree.realWorldToNodeId(rnd)
random_id = self.tree.real_world_to_node_id(rnd)
samples[random_id] = rnd
else:
rnd = self.sampleFreeSpace()
random_id = self.tree.realWorldToNodeId(rnd)
rnd = self.sample_free_space()
random_id = self.tree.real_world_to_node_id(rnd)
samples[random_id] = rnd
return samples
# Sample point in a unit ball
def sampleUnitBall(self):
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
@@ -404,63 +408,64 @@ class BITStar(object):
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
def sampleFreeSpace(self):
def sample_free_space(self):
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand)]
return rnd
def bestVertexQueueValue(self):
if(len(self.vertex_queue) == 0):
def best_vertex_queue_value(self):
if len(self.vertex_queue) == 0:
return float('inf')
values = [self.g_scores[v] +
self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue]
values = [self.g_scores[v]
+ self.compute_heuristic_cost(v, self.goalId) for v in self.vertex_queue]
values.sort()
return values[0]
def bestEdgeQueueValue(self):
if(len(self.edge_queue) == 0):
def best_edge_queue_value(self):
if len(self.edge_queue) == 0:
return float('inf')
# return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) +
self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue]
values = [self.g_scores[e[0]] + self.compute_distance_cost(e[0], e[1])
+ self.compute_heuristic_cost(e[1], self.goalId) for e in self.edge_queue]
values.sort(reverse=True)
return values[0]
def bestInVertexQueue(self):
def best_in_vertex_queue(self):
# return the best value in the vertex queue
v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId))
v_plus_vals = [(v, self.g_scores[v] + self.compute_heuristic_cost(v, self.goalId))
for v in self.vertex_queue]
v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1])
# print(v_plus_vals)
return v_plus_vals[0][0]
def bestInEdgeQueue(self):
e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost(
e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue]
def best_in_edge_queue(self):
e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost(
e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId)) for e in self.edge_queue]
e_and_values = sorted(e_and_values, key=lambda x: x[2])
return (e_and_values[0][0], e_and_values[0][1])
return e_and_values[0][0], e_and_values[0][1]
def expandVertex(self, vid):
def expand_vertex(self, vid):
self.vertex_queue.remove(vid)
# get the coordinates for given vid
currCoord = np.array(self.tree.nodeIdToRealWorldCoord(vid))
currCoord = np.array(self.tree.node_id_to_real_world_coord(vid))
# get the nearest value in vertex for every one in samples where difference is
# less than the radius
neigbors = []
for sid, scoord in self.samples.items():
scoord = np.array(scoord)
if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid):
if np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid:
neigbors.append((sid, scoord))
# add an edge to the edge queue is the path might improve the solution
for neighbor in neigbors:
sid = neighbor[0]
estimated_f_score = self.computeDistanceCost(
self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
h_cost = self.compute_heuristic_cost(sid, self.goalId)
estimated_f_score = self.compute_distance_cost(
self.startId, vid) + h_cost + self.compute_distance_cost(vid, sid)
if estimated_f_score < self.g_scores[self.goalId]:
self.edge_queue.append((vid, sid))
@@ -469,22 +474,22 @@ class BITStar(object):
def add_vertex_to_edge_queue(self, vid, currCoord):
if vid not in self.old_vertices:
neigbors = []
neighbors = []
for v, edges in self.tree.vertices.items():
if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue:
vcoord = self.tree.nodeIdToRealWorldCoord(v)
if(np.linalg.norm(vcoord - currCoord, 2) <= self.r):
neigbors.append((vid, vcoord))
v_coord = self.tree.node_id_to_real_world_coord(v)
if np.linalg.norm(currCoord - v_coord, 2) <= self.r:
neighbors.append((vid, v_coord))
for neighbor in neigbors:
for neighbor in neighbors:
sid = neighbor[0]
estimated_f_score = self.computeDistanceCost(self.startId, vid) + \
self.computeDistanceCost(
vid, sid) + self.computeHeuristicCost(sid, self.goalId)
if estimated_f_score < self.g_scores[self.goalId] and (self.g_scores[vid] + self.computeDistanceCost(vid, sid)) < self.g_scores[sid]:
estimated_f_score = self.compute_distance_cost(self.startId, vid) + self.compute_distance_cost(
vid, sid) + self.compute_heuristic_cost(sid, self.goalId)
if estimated_f_score < self.g_scores[self.goalId] and (
self.g_scores[vid] + self.compute_distance_cost(vid, sid)) < self.g_scores[sid]:
self.edge_queue.append((vid, sid))
def updateGraph(self):
def update_graph(self):
closedSet = []
openSet = []
currId = self.startId
@@ -498,22 +503,21 @@ class BITStar(object):
openSet.remove(currId)
# Check if we're at the goal
if(currId == self.goalId):
self.nodes[self.goalId]
if currId == self.goalId:
break
if(currId not in closedSet):
if currId not in closedSet:
closedSet.append(currId)
# find a non visited successor to the current node
successors = self.tree.vertices[currId]
for succesor in successors:
if(succesor in closedSet):
if succesor in closedSet:
continue
else:
# claculate tentative g score
g_score = self.g_scores[currId] + \
self.computeDistanceCost(currId, succesor)
self.compute_distance_cost(currId, succesor)
if succesor not in openSet:
# add the successor to open set
openSet.append(succesor)
@@ -522,14 +526,13 @@ class BITStar(object):
# update g and f scores
self.g_scores[succesor] = g_score
self.f_scores[succesor] = g_score + \
self.computeHeuristicCost(succesor, self.goalId)
self.f_scores[succesor] = g_score + self.compute_heuristic_cost(succesor, self.goalId)
# store the parent and child
self.nodes[succesor] = currId
def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
samples=None, start=None, end=None, tree=None):
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
samples=None, start=None, end=None):
plt.clf()
for rnd in samples:
if rnd is not None:
@@ -549,9 +552,10 @@ class BITStar(object):
plt.grid(True)
plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta):
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - etheta
cx = xCenter[0]
@@ -569,7 +573,7 @@ class BITStar(object):
plt.plot(px, py, "--c")
def main():
def main(maxIter=80):
print("Starting Batch Informed Trees Star planning")
obstacleList = [
(5, 5, 0.5),
@@ -581,7 +585,7 @@ def main():
]
bitStar = BITStar(start=[-1, 0], goal=[3, 8], obstacleList=obstacleList,
randArea=[-2, 15])
randArea=[-2, 15], maxIter=maxIter)
path = bitStar.plan(animation=show_animation)
print("Done")

24
PathPlanning/BezierPath/bezier_path.py
View File

@@ -1,15 +1,14 @@
"""
Path Planning with Bezier curve.
Path planning with Bezier curve.
author: Atsushi Sakai(@Atsushi_twi)
"""
from __future__ import division, print_function
import scipy.special
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
import scipy.special
show_animation = True
@@ -94,7 +93,8 @@ def bezier_derivatives_control_points(control_points, n_derivatives):
w = {0: control_points}
for i in range(n_derivatives):
n = len(w[i])
w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)])
w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])
for j in range(n - 1)])
return w
@@ -111,7 +111,7 @@ def curvature(dx, dy, ddx, ddy):
return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""Plot arrow."""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
@@ -155,17 +155,19 @@ def main():
tangent = np.array([point, point + dt])
normal = np.array([point, point + [- dt[1], dt[0]]])
curvature_center = point + np.array([- dt[1], dt[0]]) * radius
circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1)
circle = plt.Circle(tuple(curvature_center), radius,
color=(0, 0.8, 0.8), fill=False, linewidth=1)
assert path.T[0][0] == start_x, "path is invalid"
assert path.T[1][0] == start_y, "path is invalid"
assert path.T[0][-1] == end_x, "path is invalid"
assert path.T[1][-1] == end_y, "path is invalid"
if show_animation:
if show_animation: # pragma: no cover
fig, ax = plt.subplots()
ax.plot(path.T[0], path.T[1], label="Bezier Path")
ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points")
ax.plot(control_points.T[0], control_points.T[1],
'--o', label="Control Points")
ax.plot(x_target, y_target)
ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
ax.plot(normal[:, 0], normal[:, 1], label="Normal")
@@ -196,10 +198,10 @@ def main2():
assert path.T[0][-1] == end_x, "path is invalid"
assert path.T[1][-1] == end_y, "path is invalid"
if show_animation:
if show_animation: # pragma: no cover
plt.plot(path.T[0], path.T[1], label="Offset=" + str(offset))
if show_animation:
if show_animation: # pragma: no cover
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
plt.legend()

0
paper/.gitkeep → PathPlanning/ClosedLoopRRTStar/__init__.py
View File

BIN
PathPlanning/ClosedLoopRRTStar/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.1 MiB

374
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -1,101 +1,65 @@
"""
Path Planning Sample Code with Closed loop RRT for car like robot.
Path planning Sample Code with Closed loop RRT for car like robot.
author: AtsushiSakai(@Atsushi_twi)
"""
import sys
sys.path.append("../ReedsSheppPath/")
import random
import math
import copy
import numpy as np
import pure_pursuit
import matplotlib.pyplot as plt
import os
import sys
import reeds_shepp_path_planning
import unicycle_model
import matplotlib.pyplot as plt
import numpy as np
import pure_pursuit
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")
try:
import reeds_shepp_path_planning
import unicycle_model
from rrt_star_reeds_shepp import RRTStarReedsShepp
except ImportError:
raise
show_animation = True
target_speed = 10.0 / 3.6
STEP_SIZE = 0.1
class RRT():
class ClosedLoopRRTStar(RRTStarReedsShepp):
"""
Class for RRT Planning
Class for Closed loop RRT star planning
"""
def __init__(self, start, goal, obstacleList, randArea,
maxIter=200):
def __init__(self, start, goal, obstacle_list, rand_area,
max_iter=200,
connect_circle_dist=50.0
):
super().__init__(start, goal, obstacle_list, rand_area,
max_iter=max_iter,
connect_circle_dist=connect_circle_dist,
)
self.target_speed = 10.0 / 3.6
self.yaw_th = np.deg2rad(3.0)
self.xy_th = 0.5
self.invalid_travel_ratio = 5.0
def planning(self, animation=True):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
"""
self.start = Node(start[0], start[1], start[2])
self.end = Node(goal[0], goal[1], goal[2])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.obstacleList = obstacleList
self.maxIter = maxIter
def try_goal_path(self):
goal = Node(self.end.x, self.end.y, self.end.yaw)
newNode = self.steer(goal, len(self.nodeList) - 1)
if newNode is None:
return
if self.CollisionCheck(newNode, self.obstacleList):
# print("goal path is OK")
self.nodeList.append(newNode)
def Planning(self, animation=True):
"""
Pathplanning
do planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
self.try_goal_path()
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if newNode is None:
continue
if self.CollisionCheck(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
if newNode is None:
continue
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
self.try_goal_path()
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
# planning with RRTStarReedsShepp
super().planning(animation=animation)
# generate coruse
path_indexs = self.get_best_last_indexs()
path_indexs = self.get_goal_indexes()
flag, x, y, yaw, v, t, a, d = self.search_best_feasible_path(
path_indexs)
@@ -108,14 +72,13 @@ class RRT():
best_time = float("inf")
fx = None
fx, fy, fyaw, fv, ft, fa, fd = None, None, None, None, None, None, None
# pure pursuit tracking
for ind in path_indexs:
path = self.gen_final_course(ind)
path = self.generate_final_course(ind)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(
path)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(path)
if flag and best_time >= t[-1]:
print("feasible path is found")
@@ -130,145 +93,50 @@ class RRT():
fy.append(self.end.y)
fyaw.append(self.end.yaw)
return True, fx, fy, fyaw, fv, ft, fa, fd
else:
return False, None, None, None, None, None, None, None
def calc_tracking_path(self, path):
path = np.array(path[::-1])
ds = 0.2
for i in range(10):
lx = path[-1, 0]
ly = path[-1, 1]
lyaw = path[-1, 2]
move_yaw = math.atan2(path[-2, 1] - ly, path[-2, 0] - lx)
if abs(lyaw - move_yaw) >= math.pi / 2.0:
print("back")
ds *= -1
lstate = np.array(
[lx + ds * math.cos(lyaw), ly + ds * math.sin(lyaw), lyaw])
# print(lstate)
path = np.vstack((path, lstate))
return path
return False, None, None, None, None, None, None, None
def check_tracking_path_is_feasible(self, path):
# print("check_tracking_path_is_feasible")
cx = np.array(path[:, 0])
cy = np.array(path[:, 1])
cyaw = np.array(path[:, 2])
cx = np.array([state[0] for state in path])[::-1]
cy = np.array([state[1] for state in path])[::-1]
cyaw = np.array([state[2] for state in path])[::-1]
goal = [cx[-1], cy[-1], cyaw[-1]]
cx, cy, cyaw = pure_pursuit.extend_path(cx, cy, cyaw)
speed_profile = pure_pursuit.calc_speed_profile(
cx, cy, cyaw, target_speed)
cx, cy, cyaw, self.target_speed)
t, x, y, yaw, v, a, d, find_goal = pure_pursuit.closed_loop_prediction(
cx, cy, cyaw, speed_profile, goal)
yaw = [self.pi_2_pi(iyaw) for iyaw in yaw]
yaw = [reeds_shepp_path_planning.pi_2_pi(iyaw) for iyaw in yaw]
if not find_goal:
print("cannot reach goal")
if abs(yaw[-1] - goal[2]) >= math.pi / 4.0:
if abs(yaw[-1] - goal[2]) >= self.yaw_th * 10.0:
print("final angle is bad")
find_goal = False
travel = sum([abs(iv) * unicycle_model.dt for iv in v])
# print(travel)
origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2)
for (dx, dy) in zip(np.diff(cx), np.diff(cy))])
# print(origin_travel)
if (travel / origin_travel) >= 5.0:
if (travel / origin_travel) >= self.invalid_travel_ratio:
print("path is too long")
find_goal = False
if not self.CollisionCheckWithXY(x, y, self.obstacleList):
if not self.collision_check_with_xy(x, y, self.obstacle_list):
print("This path is collision")
find_goal = False
return find_goal, x, y, yaw, v, t, a, d
def choose_parent(self, newNode, nearinds):
if len(nearinds) == 0:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if tNode is None:
continue
if self.CollisionCheck(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
if newNode is None:
return None
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def steer(self, rnd, nind):
# print(rnd)
nearestNode = self.nodeList[nind]
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw,
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
newNode.x = px[-1]
newNode.y = py[-1]
newNode.yaw = pyaw[-1]
newNode.path_x = px
newNode.path_y = py
newNode.path_yaw = pyaw
newNode.cost += sum([abs(c) for c in clen])
newNode.parent = nind
return newNode
def get_random_point(self):
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
node = Node(rnd[0], rnd[1], rnd[2])
return node
def get_best_last_indexs(self):
# print("get_best_last_index")
YAWTH = np.deg2rad(1.0)
XYTH = 0.5
def get_goal_indexes(self):
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
for (i, node) in enumerate(self.node_list):
if self.calc_dist_to_goal(node.x, node.y) <= self.xy_th:
goalinds.append(i)
print("OK XY TH num is")
print(len(goalinds))
@@ -276,106 +144,17 @@ class RRT():
# angle check
fgoalinds = []
for i in goalinds:
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
if abs(self.node_list[i].yaw - self.end.yaw) <= self.yaw_th:
fgoalinds.append(i)
print("OK YAW TH num is")
print(len(fgoalinds))
return fgoalinds
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y, self.end.yaw]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
path_x = reversed(node.path_x)
path_y = reversed(node.path_y)
path_yaw = reversed(node.path_yaw)
for (ix, iy, iyaw) in zip(path_x, path_y, path_yaw):
path.append([ix, iy, iyaw])
# path.append([node.x, node.y])
goalind = node.parent
path.append([self.start.x, self.start.y, self.start.yaw])
@staticmethod
def collision_check_with_xy(x, y, obstacle_list):
path = np.array(path[::-1])
return path
def calc_dist_to_goal(self, x, y):
return np.linalg.norm([x - self.end.x, y - self.end.y])
def find_near_nodes(self, newNode):
nnode = len(self.nodeList)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
# r = self.expandDis * 5.0
dlist = [(node.x - newNode.x) ** 2 +
(node.y - newNode.y) ** 2 +
(node.yaw - newNode.yaw) ** 2
for node in self.nodeList]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
def rewire(self, newNode, nearinds):
nnode = len(self.nodeList)
for i in nearinds:
nearNode = self.nodeList[i]
tNode = self.steer(nearNode, nnode - 1)
if tNode is None:
continue
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def DrawGraph(self, rnd=None):
"""
Draw Graph
"""
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(
self.start.x, self.start.y, self.start.yaw)
reeds_shepp_path_planning.plot_arrow(
self.end.x, self.end.y, self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2 +
(node.y - rnd.y) ** 2 +
(node.yaw - rnd.yaw) ** 2 for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def CollisionCheck(self, node, obstacleList):
for (ox, oy, size) in obstacleList:
for (ix, iy) in zip(node.path_x, node.path_y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
return True # safe
def CollisionCheckWithXY(self, x, y, obstacleList):
for (ox, oy, size) in obstacleList:
for (ox, oy, size) in obstacle_list:
for (ix, iy) in zip(x, y):
dx = ox - ix
dy = oy - iy
@@ -386,26 +165,10 @@ class RRT():
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
self.path_x = []
self.path_y = []
self.path_yaw = []
self.cost = 0.0
self.parent = None
def main():
print("Start rrt start planning")
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
print("Start" + __file__)
# ====Search Path with RRT====
obstacleList = [
obstacle_list = [
(5, 5, 1),
(4, 6, 1),
(4, 8, 1),
@@ -419,17 +182,20 @@ def main():
# Set Initial parameters
start = [0.0, 0.0, np.deg2rad(0.0)]
goal = [6.0, 7.0, np.deg2rad(90.0)]
goal = [gx, gy, gyaw]
rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList)
flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)
closed_loop_rrt_star = ClosedLoopRRTStar(start, goal,
obstacle_list,
[-2.0, 20.0],
max_iter=max_iter)
flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(animation=show_animation)
if not flag:
print("cannot find feasible path")
# Draw final path
if show_animation:
rrt.DrawGraph()
closed_loop_rrt_star.draw_graph()
plt.plot(x, y, '-r')
plt.grid(True)
plt.pause(0.001)

24
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -5,9 +5,11 @@ Path tracking simulation with pure pursuit steering control and PID speed contro
author: Atsushi Sakai
"""
import numpy as np
import math
import matplotlib.pyplot as plt
import numpy as np
import unicycle_model
Kp = 2.0 # speed propotional gain
@@ -75,7 +77,7 @@ def calc_target_index(state, cx, cy):
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
dy = cy[ind + 1] - cy[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
@@ -131,15 +133,15 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
a.append(ai)
d.append(di)
if target_ind % 1 == 0 and animation:
if target_ind % 1 == 0 and animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("speed:" + str(round(state.v, 2)) +
"tind:" + str(target_ind))
plt.title("speed:" + str(round(state.v, 2))
+ "tind:" + str(target_ind))
plt.pause(0.0001)
else:
@@ -153,6 +155,7 @@ def set_stop_point(target_speed, cx, cy, cyaw):
forward = True
d = []
is_back = False
# Set stop point
for i in range(len(cx) - 1):
@@ -196,7 +199,7 @@ def calc_speed_profile(cx, cy, cyaw, target_speed):
speed_profile, d = set_stop_point(target_speed, cx, cy, cyaw)
if animation:
if animation: # pragma: no cover
plt.plot(speed_profile, "xb")
return speed_profile
@@ -220,9 +223,8 @@ def extend_path(cx, cy, cyaw):
return cx, cy, cyaw
def main():
def main(): # pragma: no cover
# target course
import numpy as np
cx = np.arange(0, 50, 0.1)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
@@ -278,7 +280,7 @@ def main():
plt.axis("equal")
plt.grid(True)
subplots(1)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
@@ -286,7 +288,7 @@ def main():
plt.show()
def main2():
def main2(): # pragma: no cover
import pandas as pd
data = pd.read_csv("rrt_course.csv")
cx = np.array(data["x"])
@@ -322,7 +324,7 @@ def main2():
plt.show()
if __name__ == '__main__':
if __name__ == '__main__': # pragma: no cover
print("Pure pursuit path tracking simulation start")
# main()
main2()

2
PathPlanning/ClosedLoopRRTStar/unicycle_model.py
View File

@@ -42,7 +42,7 @@ def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
if __name__ == '__main__':
if __name__ == '__main__': # pragma: no cover
print("start unicycle simulation")
import matplotlib.pyplot as plt

BIN
PathPlanning/Dijkstra/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 6.5 MiB

311
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -1,7 +1,9 @@
"""
Dijkstra grid based planning
Grid based Dijkstra planning
author: Atsushi Sakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
@@ -9,210 +11,235 @@ import math
show_animation = True
class Dijkstra:
class Node:
def __init__(self, ox, oy, reso, rr):
"""
Initialize map for a star planning
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
self.reso = reso
self.rr = rr
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, pind):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
def planning(self, sx, sy, gx, gy):
"""
dijkstra path search
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
input:
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
motion = get_motion_model()
output:
rx: x position list of the final path
ry: y position list of the final path
"""
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
nstart = self.Node(self.calc_xyindex(sx, self.minx),
self.calc_xyindex(sy, self.miny), 0.0, -1)
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
self.calc_xyindex(gy, self.miny), 0.0, -1)
while 1:
c_id = min(openset, key=lambda o: openset[o].cost)
current = openset[c_id]
# print("current", current)
openset, closedset = dict(), dict()
openset[self.calc_index(nstart)] = nstart
# show graph
if show_animation:
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
while 1:
c_id = min(openset, key=lambda o: openset[o].cost)
current = openset[c_id]
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# show graph
if show_animation: # pragma: no cover
plt.plot(self.calc_position(current.x, self.minx),
self.calc_position(current.y, self.miny), "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0], current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
# Remove the item from the open set
del openset[c_id]
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
# Add it to the closed set
closedset[c_id] = current
if n_id in closedset:
continue
# Otherwise if it is already in the open set
if n_id in openset:
if openset[n_id].cost > node.cost:
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
else:
openset[n_id] = node
# expand search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
n_id = self.calc_index(node)
rx, ry = calc_final_path(ngoal, closedset, reso)
if n_id in closedset:
continue
return rx, ry
if not self.verify_node(node):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
else:
if openset[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
def calc_final_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
rx, ry = self.calc_final_path(ngoal, closedset)
return rx, ry
return rx, ry
def calc_final_path(self, ngoal, closedset):
# generate final course
rx, ry = [self.calc_position(ngoal.x, self.minx)], [
self.calc_position(ngoal.y, self.miny)]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(self.calc_position(n.x, self.minx))
ry.append(self.calc_position(n.y, self.miny))
pind = n.pind
def verify_node(node, obmap, minx, miny, maxx, maxy):
return rx, ry
if obmap[node.x][node.y]:
return False
def calc_heuristic(self, n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x > maxx:
return False
elif node.y > maxy:
return False
def calc_position(self, index, minp):
pos = index*self.reso+minp
return pos
return True
def calc_xyindex(self, position, minp):
return round((position - minp)/self.reso)
def calc_index(self, node):
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
def calc_obstacle_map(ox, oy, reso, vr):
def verify_node(self, node):
px = self.calc_position(node.x, self.minx)
py = self.calc_position(node.y, self.miny)
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
# print("minx:", minx)
# print("miny:", miny)
# print("maxx:", maxx)
# print("maxy:", maxy)
if px < self.minx:
return False
elif py < self.miny:
return False
elif px >= self.maxx:
return False
elif py >= self.maxy:
return False
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# print("xwidth:", xwidth)
# print("ywidth:", ywidth)
if self.obmap[node.x][node.y]:
return False
# obstacle map generation
obmap = [[False for i in range(xwidth)] for i in range(ywidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
return True
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_obstacle_map(self, ox, oy):
self.minx = round(min(ox))
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
print("minx:", self.minx)
print("miny:", self.miny)
print("maxx:", self.maxx)
print("maxy:", self.maxy)
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
self.xwidth = round((self.maxx - self.minx)/self.reso)
self.ywidth = round((self.maxy - self.miny)/self.reso)
print("xwidth:", self.xwidth)
print("ywidth:", self.ywidth)
# obstacle map generation
self.obmap = [[False for i in range(self.ywidth)]
for i in range(self.xwidth)]
for ix in range(self.xwidth):
x = self.calc_position(ix, self.minx)
for iy in range(self.ywidth):
y = self.calc_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= self.rr:
self.obmap[ix][iy] = True
break
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
def get_motion_model(self):
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
return motion
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
sx = -5.0 # [m]
sy = -5.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
ox = []
oy = []
for i in range(60):
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
ox.append(i)
oy.append(0.0)
for i in range(60):
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
oy.append(i)
for i in range(61):
for i in range(-10, 61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
for i in range(-10, 61):
ox.append(-10.0)
oy.append(i)
for i in range(40):
for i in range(-10, 40):
ox.append(20.0)
oy.append(i)
for i in range(40):
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation:
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
rx, ry = dijkstra.planning(sx, sy, gx, gy)
if show_animation:
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()

0
PathPlanning/DubinsPath/__init__.py Normal file
View File

BIN
PathPlanning/DubinsPath/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 75 KiB

41
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -6,8 +6,9 @@ author Atsushi Sakai(@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -136,8 +137,8 @@ def LRL(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# nomalize
def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
# normalize
dx = ex
dy = ey
D = math.sqrt(dx ** 2.0 + dy ** 2.0)
@@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
bcost = cost
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
"""
Dubins path plannner
@@ -199,18 +200,18 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c)
lex, ley, leyaw, c, D_ANGLE)
px = [math.cos(-syaw) * x + math.sin(-syaw) *
y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) *
y + sy for x, y in zip(lpx, lpy)]
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw)
* y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
return px, py, pyaw, mode, clen
def generate_course(length, mode, c):
def generate_course(length, mode, c, D_ANGLE):
px = [0.0]
py = [0.0]
@@ -218,21 +219,21 @@ def generate_course(length, mode, c):
for m, l in zip(mode, length):
pd = 0.0
if m is "S":
if m == "S":
d = 1.0 * c
else: # turning couse
d = np.deg2rad(3.0)
d = D_ANGLE
while pd < abs(l - d):
# print(pd, l)
px.append(px[-1] + d / c * math.cos(pyaw[-1]))
py.append(py[-1] + d / c * math.sin(pyaw[-1]))
if m is "L": # left turn
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
@@ -240,18 +241,18 @@ def generate_course(length, mode, c):
px.append(px[-1] + d / c * math.cos(pyaw[-1]))
py.append(py[-1] + d / c * math.sin(pyaw[-1]))
if m is "L": # left turn
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""
Plot arrow
"""

BIN
PathPlanning/DynamicWindowApproach/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.3 MiB

117
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -10,15 +10,26 @@ import math
import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.append("../../")
show_animation = True
def dwa_control(x, u, config, goal, ob):
"""
Dynamic Window Approach control
"""
dw = calc_dynamic_window(x, config)
u, traj = calc_final_input(x, u, dw, config, goal, ob)
return u, traj
class Config():
# simulation parameters
"""
simulation parameter class
"""
def __init__(self):
# robot parameter
@@ -29,15 +40,19 @@ class Config():
self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss]
self.v_reso = 0.01 # [m/s]
self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s]
self.dt = 0.1 # [s]
self.dt = 0.1 # [s] Time tick for motion prediction
self.predict_time = 3.0 # [s]
self.to_goal_cost_gain = 1.0
self.to_goal_cost_gain = 0.15
self.speed_cost_gain = 1.0
self.robot_radius = 1.0 # [m]
self.obstacle_cost_gain = 1.0
self.robot_radius = 1.0 # [m] for collision check
def motion(x, u, dt):
# motion model
"""
motion model
"""
x[2] += u[1] * dt
x[0] += u[0] * math.cos(x[2]) * dt
@@ -49,6 +64,9 @@ def motion(x, u, dt):
def calc_dynamic_window(x, config):
"""
calculation dynamic window based on current state x
"""
# Dynamic window from robot specification
Vs = [config.min_speed, config.max_speed,
@@ -67,9 +85,12 @@ def calc_dynamic_window(x, config):
return dw
def calc_trajectory(xinit, v, y, config):
def predict_trajectory(x_init, v, y, config):
"""
predict trajectory with an input
"""
x = np.array(xinit)
x = np.array(x_init)
traj = np.array(x)
time = 0
while time <= config.predict_time:
@@ -81,42 +102,44 @@ def calc_trajectory(xinit, v, y, config):
def calc_final_input(x, u, dw, config, goal, ob):
"""
calculation final input with dinamic window
"""
xinit = x[:]
min_cost = 10000.0
min_u = u
min_u[0] = 0.0
x_init = x[:]
min_cost = float("inf")
best_u = [0.0, 0.0]
best_traj = np.array([x])
# evalucate all trajectory with sampled input in dynamic window
for v in np.arange(dw[0], dw[1], config.v_reso):
for y in np.arange(dw[2], dw[3], config.yawrate_reso):
traj = calc_trajectory(xinit, v, y, config)
traj = predict_trajectory(x_init, v, y, config)
# calc cost
to_goal_cost = calc_to_goal_cost(traj, goal, config)
to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(traj, goal, config)
speed_cost = config.speed_cost_gain * \
(config.max_speed - traj[-1, 3])
ob_cost = calc_obstacle_cost(traj, ob, config)
# print(ob_cost)
ob_cost = config.obstacle_cost_gain*calc_obstacle_cost(traj, ob, config)
final_cost = to_goal_cost + speed_cost + ob_cost
#print (final_cost)
# search minimum trajectory
if min_cost >= final_cost:
min_cost = final_cost
min_u = [v, y]
best_u = [v, y]
best_traj = traj
return min_u, best_traj
return best_u, best_traj
def calc_obstacle_cost(traj, ob, config):
# calc obstacle cost inf: collistion, 0:free
"""
calc obstacle cost inf: collision
"""
skip_n = 2
skip_n = 2 # for speed up
minr = float("inf")
for ii in range(0, len(traj[:, 1]), skip_n):
@@ -137,40 +160,30 @@ def calc_obstacle_cost(traj, ob, config):
def calc_to_goal_cost(traj, goal, config):
# calc to goal cost. It is 2D norm.
"""
calc to goal cost with angle difference
"""
goal_magnitude = math.sqrt(goal[0]**2 + goal[1]**2)
traj_magnitude = math.sqrt(traj[-1, 0]**2 + traj[-1, 1]**2)
dot_product = (goal[0] * traj[-1, 0]) + (goal[1] * traj[-1, 1])
error = dot_product / (goal_magnitude * traj_magnitude)
error_angle = math.acos(error)
cost = config.to_goal_cost_gain * error_angle
dx = goal[0] - traj[-1, 0]
dy = goal[1] - traj[-1, 1]
error_angle = math.atan2(dy, dx)
cost = abs(error_angle - traj[-1, 2])
return cost
def dwa_control(x, u, config, goal, ob):
# Dynamic Window control
dw = calc_dynamic_window(x, config)
u, traj = calc_final_input(x, u, dw, config, goal, ob)
return u, traj
def plot_arrow(x, y, yaw, length=0.5, width=0.1):
def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
head_length=width, head_width=width)
plt.plot(x, y)
def main():
def main(gx=10, gy=10):
print(__file__ + " start!!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# goal position [x(m), y(m)]
goal = np.array([10, 10])
goal = np.array([gx, gy])
# obstacles [x(m) y(m), ....]
ob = np.array([[-1, -1],
[0, 2],
@@ -184,21 +197,20 @@ def main():
[12.0, 12.0]
])
# input [forward speed, yawrate]
u = np.array([0.0, 0.0])
config = Config()
traj = np.array(x)
for i in range(1000):
u, ltraj = dwa_control(x, u, config, goal, ob)
while True:
u, ptraj = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
x = motion(x, u, config.dt) # simulate robot
traj = np.vstack((traj, x)) # store state history
# print(traj)
if show_animation:
plt.cla()
plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
plt.plot(ptraj[:, 0], ptraj[:, 1], "-g")
plt.plot(x[0], x[1], "xr")
plt.plot(goal[0], goal[1], "xb")
plt.plot(ob[:, 0], ob[:, 1], "ok")
@@ -207,8 +219,9 @@ def main():
plt.grid(True)
plt.pause(0.0001)
# check goal
if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
# check reaching goal
dist_to_goal = math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2)
if dist_to_goal <= config.robot_radius:
print("Goal!!")
break

BIN
PathPlanning/Eta3SplinePath/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 560 KiB

63
PathPlanning/Eta3SplinePath/eta3_spline_path.py
View File

@@ -36,16 +36,17 @@ class eta3_path(object):
for r, s in zip(segments[:-1], segments[1:]):
assert(np.array_equal(r.end_pose, s.start_pose))
self.segments = segments
"""
eta3_path::calc_path_point
input
normalized interpolation point along path object, 0 <= u <= len(self.segments)
returns
2d (x,y) position vector
"""
def calc_path_point(self, u):
"""
eta3_path::calc_path_point
input
normalized interpolation point along path object, 0 <= u <= len(self.segments)
returns
2d (x,y) position vector
"""
assert(u >= 0 and u <= len(self.segments))
if np.isclose(u, len(self.segments)):
segment_idx = len(self.segments) - 1
@@ -152,39 +153,41 @@ class eta3_path_segment(object):
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
self.s_dot = lambda u : max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))), 1e-6)
self.s_dot = lambda u: max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array(
[1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
self.segment_length = self.f_length(1)[0]
"""
eta3_path_segment::calc_point
input
u - parametric representation of a point along the segment, 0 <= u <= 1
returns
(x,y) of point along the segment
"""
def calc_point(self, u):
"""
eta3_path_segment::calc_point
input
u - parametric representation of a point along the segment, 0 <= u <= 1
returns
(x,y) of point along the segment
"""
assert(u >= 0 and u <= 1)
return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
"""
eta3_path_segment::calc_deriv
input
u - parametric representation of a point along the segment, 0 <= u <= 1
returns
(d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
"""
def calc_deriv(self, u, order=1):
"""
eta3_path_segment::calc_deriv
input
u - parametric representation of a point along the segment, 0 <= u <= 1
returns
(d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
"""
assert(u >= 0 and u <= 1)
assert(order > 0 and order <= 2)
if order == 1:
return self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))
else:
return self.coeffs[:, 2:].dot(np.array([2, 6.*u, 12.*u**2, 20.*u**3, 30.*u**4, 42.*u**5]))
return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))
return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
def test1():

127
PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
View File

@@ -17,14 +17,19 @@ from matplotlib.collections import LineCollection
import sys
import os
sys.path.append(os.path.relpath("../Eta3SplinePath"))
from eta3_spline_path import eta3_path, eta3_path_segment
try:
from eta3_spline_path import eta3_path, eta3_path_segment
except:
raise
show_animation = True
class MaxVelocityNotReached(Exception):
def __init__(self, actual_vel, max_vel):
self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(actual_vel, max_vel)
self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(
actual_vel, max_vel)
class eta3_trajectory(eta3_path):
@@ -34,6 +39,7 @@ class eta3_trajectory(eta3_path):
input
segments: list of `eta3_trajectory_segment` instances defining a continuous trajectory
"""
def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0):
# ensure that all inputs obey the assumptions of the model
assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
@@ -47,8 +53,9 @@ class eta3_trajectory(eta3_path):
self.max_jerk = float(max_jerk)
length_array = np.array([s.segment_length for s in self.segments])
# add a zero to the beginning for finding the correct segment_id
self.cum_lengths = np.concatenate((np.array([0]), np.cumsum(length_array)))
## compute velocity profile on top of the path
self.cum_lengths = np.concatenate(
(np.array([0]), np.cumsum(length_array)))
# compute velocity profile on top of the path
self.velocity_profile()
self.ui_prev = 0
self.prev_seg_id = 0
@@ -84,16 +91,17 @@ class eta3_trajectory(eta3_path):
# solve for the maximum achievable velocity based on the kinematic limits imposed by max_accel and max_jerk
# this leads to a quadratic equation in v_max: a*v_max**2 + b*v_max + c = 0
a = 1 / self.max_accel
b = 3. * self.max_accel / (2. * self.max_jerk) + v_s1 / self.max_accel - (self.max_accel**2 / self.max_jerk + v_s1) / self.max_accel
b = 3. * self.max_accel / (2. * self.max_jerk) + v_s1 / self.max_accel - (
self.max_accel**2 / self.max_jerk + v_s1) / self.max_accel
c = s_s1 + s_sf - self.total_length - 7. * self.max_accel**3 / (3. * self.max_jerk**2) \
- v_s1 * (self.max_accel / self.max_jerk + v_s1 / self.max_accel) \
+ (self.max_accel**2 / self.max_jerk + v_s1 / self.max_accel)**2 / (2. * self.max_accel)
v_max = ( -b + np.sqrt(b**2 - 4. * a * c) ) / (2. * a)
# v_max represents the maximum velocity that could be attained if there was no cruise period
+ (self.max_accel**2 / self.max_jerk + v_s1 /
self.max_accel)**2 / (2. * self.max_accel)
v_max = (-b + np.sqrt(b**2 - 4. * a * c)) / (2. * a)
# v_max represents the maximum velocity that could be attained if there was no cruise period
# (i.e. driving at constant speed without accelerating or jerking)
# if this velocity is less than our desired max velocity, the max velocity needs to be updated
# XXX the way to handle this `if` condition needs to be more thoroughly worked through
if self.max_vel > v_max:
# when this condition is tripped, there will be no cruise period (s_cruise=0)
self.max_vel = v_max
@@ -112,22 +120,25 @@ class eta3_trajectory(eta3_path):
# Section 1: accelerate at max_accel
index = 1
# compute change in velocity over the section
delta_v = (self.max_vel - self.max_jerk * (self.max_accel / self.max_jerk)**2 / 2.) - self.vels[index-1]
delta_v = (self.max_vel - self.max_jerk * (self.max_accel /
self.max_jerk)**2 / 2.) - self.vels[index - 1]
self.times[index] = delta_v / self.max_accel
self.vels[index] = self.vels[index-1] + self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index-1] * self.times[index] + self.max_accel * self.times[index]**2 / 2.
self.vels[index] = self.vels[index - 1] + \
self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index - 1] * \
self.times[index] + self.max_accel * self.times[index]**2 / 2.
# Section 2: decrease acceleration (down to 0) until max speed is hit
index = 2
self.times[index] = self.max_accel / self.max_jerk
self.vels[index] = self.vels[index-1] + self.max_accel * self.times[index] \
self.vels[index] = self.vels[index - 1] + self.max_accel * self.times[index] \
- self.max_jerk * self.times[index]**2 / 2.
# as a check, the velocity at the end of the section should be self.max_vel
if not np.isclose(self.vels[index], self.max_vel):
raise MaxVelocityNotReached(self.vels[index], self.max_vel)
self.seg_lengths[index] = self.vels[index-1] * self.times[index] + self.max_accel * self.times[index]**2 / 2. \
self.seg_lengths[index] = self.vels[index - 1] * self.times[index] + self.max_accel * self.times[index]**2 / 2. \
- self.max_jerk * self.times[index]**3 / 6.
# Section 3: will be done last
@@ -135,21 +146,25 @@ class eta3_trajectory(eta3_path):
# Section 4: apply min jerk until min acceleration is hit
index = 4
self.times[index] = self.max_accel / self.max_jerk
self.vels[index] = self.max_vel - self.max_jerk * self.times[index]**2 / 2.
self.seg_lengths[index] = self.max_vel * self.times[index] - self.max_jerk * self.times[index]**3 / 6.
self.vels[index] = self.max_vel - \
self.max_jerk * self.times[index]**2 / 2.
self.seg_lengths[index] = self.max_vel * self.times[index] - \
self.max_jerk * self.times[index]**3 / 6.
# Section 5: continue deceleration at max rate
index = 5
# compute velocity change over sections
delta_v = self.vels[index-1] - v_sf
delta_v = self.vels[index - 1] - v_sf
self.times[index] = delta_v / self.max_accel
self.vels[index] = self.vels[index-1] - self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index-1] * self.times[index] - self.max_accel * self.times[index]**2 / 2.
self.vels[index] = self.vels[index - 1] - \
self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index - 1] * \
self.times[index] - self.max_accel * self.times[index]**2 / 2.
# Section 6(final): max jerk to get to zero velocity and zero acceleration simultaneously
index = 6
self.times[index] = t_sf
self.vels[index] = self.vels[index-1] - self.max_jerk * t_sf**2 / 2.
self.vels[index] = self.vels[index - 1] - self.max_jerk * t_sf**2 / 2.
try:
assert np.isclose(self.vels[index], 0)
@@ -164,7 +179,8 @@ class eta3_trajectory(eta3_path):
# the length of the cruise section is whatever length hasn't already been accounted for
# NOTE: the total array self.seg_lengths is summed because the entry for the cruise segment is
# initialized to 0!
self.seg_lengths[index] = self.total_length - self.seg_lengths.sum()
self.seg_lengths[index] = self.total_length - \
self.seg_lengths.sum()
self.vels[index] = self.max_vel
self.times[index] = self.seg_lengths[index] / self.max_vel
@@ -174,8 +190,11 @@ class eta3_trajectory(eta3_path):
self.total_time = self.times.sum()
def get_interp_param(self, seg_id, s, ui, tol=0.001):
f = lambda u: self.segments[seg_id].f_length(u)[0] - s
fprime = lambda u: self.segments[seg_id].s_dot(u)
def f(u):
return self.segments[seg_id].f_length(u)[0] - s
def fprime(u):
return self.segments[seg_id].s_dot(u)
while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol:
ui -= f(ui) / fprime(ui)
ui = max(0, min(ui, 1))
@@ -190,12 +209,14 @@ class eta3_trajectory(eta3_path):
elif time <= self.times[:2].sum():
delta_t = time - self.times[0]
linear_velocity = self.vels[0] + self.max_accel * delta_t
s = self.seg_lengths[0] + self.vels[0] * delta_t + self.max_accel * delta_t**2 / 2.
s = self.seg_lengths[0] + self.vels[0] * \
delta_t + self.max_accel * delta_t**2 / 2.
linear_accel = self.max_accel
elif time <= self.times[:3].sum():
delta_t = time - self.times[:2].sum()
linear_velocity = self.vels[1] + self.max_accel * delta_t - self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:2].sum() + self.vels[1] * delta_t + self.max_accel * delta_t**2 /2. \
linear_velocity = self.vels[1] + self.max_accel * \
delta_t - self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:2].sum() + self.vels[1] * delta_t + self.max_accel * delta_t**2 / 2. \
- self.max_jerk * delta_t**3 / 6.
linear_accel = self.max_accel - self.max_jerk * delta_t
elif time <= self.times[:4].sum():
@@ -206,19 +227,22 @@ class eta3_trajectory(eta3_path):
elif time <= self.times[:5].sum():
delta_t = time - self.times[:4].sum()
linear_velocity = self.vels[3] - self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:4].sum() + self.vels[3] * delta_t - self.max_jerk * delta_t**3 / 6.
s = self.seg_lengths[:4].sum() + self.vels[3] * \
delta_t - self.max_jerk * delta_t**3 / 6.
linear_accel = -self.max_jerk * delta_t
elif time <= self.times[:-1].sum():
delta_t = time - self.times[:5].sum()
linear_velocity = self.vels[4] - self.max_accel * delta_t
s = self.seg_lengths[:5].sum() + self.vels[4] * delta_t - self.max_accel * delta_t**2 / 2.
s = self.seg_lengths[:5].sum() + self.vels[4] * \
delta_t - self.max_accel * delta_t**2 / 2.
linear_accel = -self.max_accel
elif time < self.times.sum():
delta_t = time - self.times[:-1].sum()
linear_velocity = self.vels[5] - self.max_accel * delta_t + self.max_jerk * delta_t**2 / 2.
linear_velocity = self.vels[5] - self.max_accel * \
delta_t + self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:-1].sum() + self.vels[5] * delta_t - self.max_accel * delta_t**2 / 2. \
+ self.max_jerk * delta_t**3 / 6.
linear_accel = -self.max_accel + self.max_jerk*delta_t
linear_accel = -self.max_accel + self.max_jerk * delta_t
else:
linear_velocity = 0.
s = self.total_length
@@ -232,15 +256,15 @@ class eta3_trajectory(eta3_path):
else:
# compute interpolation parameter using length from current segment's starting point
curr_segment_length = s - self.cum_lengths[seg_id]
ui = self.get_interp_param(seg_id=seg_id, s=curr_segment_length, ui=self.ui_prev)
ui = self.get_interp_param(
seg_id=seg_id, s=curr_segment_length, ui=self.ui_prev)
if not seg_id == self.prev_seg_id:
self.ui_prev = 0
else:
self.ui_prev = ui
self.prev_seg_id = seg_id
# TODO(jwd): normalize!
# compute angular velocity of current point= (ydd*xd - xdd*yd) / (xd**2 + yd**2)
d = self.segments[seg_id].calc_deriv(ui, order=1)
dd = self.segments[seg_id].calc_deriv(ui, order=2)
@@ -250,7 +274,8 @@ class eta3_trajectory(eta3_path):
# ut - time-derivative of interpolation parameter u
ut = linear_velocity / su
# utt - time-derivative of ut
utt = linear_accel / su - (d[0] * dd[0] + d[1] * dd[1]) / su**2 * ut
utt = linear_accel / su - \
(d[0] * dd[0] + d[1] * dd[1]) / su**2 * ut
xt = d[0] * ut
yt = d[1] * ut
xtt = dd[0] * ut**2 + d[0] * utt
@@ -261,7 +286,8 @@ class eta3_trajectory(eta3_path):
# combine path point with orientation and velocities
pos = self.segments[seg_id].calc_point(ui)
state = np.array([pos[0], pos[1], np.arctan2(d[1], d[0]), linear_velocity, angular_velocity])
state = np.array([pos[0], pos[1], np.arctan2(
d[1], d[0]), linear_velocity, angular_velocity])
return state
@@ -278,15 +304,16 @@ def test1(max_vel=0.5):
trajectory_segments.append(eta3_path_segment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5)
traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101)
state = np.empty((5, times.size))
for j, t in enumerate(times):
state[:, j] = traj.calc_traj_point(t)
if show_animation:
if show_animation: # pragma: no cover
# plot the path
plt.plot(state[0, :], state[1, :])
plt.pause(1.0)
@@ -311,8 +338,9 @@ def test2(max_vel=0.5):
trajectory_segments.append(eta3_path_segment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5)
traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101)
state = np.empty((5, times.size))
@@ -346,7 +374,7 @@ def test3(max_vel=2.0):
end_pose = [5.5, 1.5, 0]
kappa = [0, 0, 0, 0]
# NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
#was: eta = [0, 0, 0, 0, 0, 0], now is:
# was: eta = [0, 0, 0, 0, 0, 0], now is:
eta = [0.5, 0.5, 0, 0, 0, 0]
trajectory_segments.append(eta3_path_segment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
@@ -376,7 +404,8 @@ def test3(max_vel=2.0):
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5, max_jerk=1)
traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5, max_jerk=1)
# interpolate at several points along the path
times = np.linspace(0, traj.total_time, 1001)
@@ -392,8 +421,8 @@ def test3(max_vel=2.0):
points = np.array([x, y]).T.reshape(-1, 1, 2)
segs = np.concatenate([points[:-1], points[1:]], axis=1)
lc = LineCollection(segs, cmap=plt.get_cmap('inferno'))
ax.set_xlim(np.min(x)-1, np.max(x)+1)
ax.set_ylim(np.min(y)-1, np.max(y)+1)
ax.set_xlim(np.min(x) - 1, np.max(x) + 1)
ax.set_ylim(np.min(y) - 1, np.max(y) + 1)
lc.set_array(state[3, :])
lc.set_linewidth(3)
ax.add_collection(lc)
@@ -422,8 +451,8 @@ def main():
"""
recreate path from reference (see Table 1)
"""
#test1()
#test2()
# test1()
# test2()
test3()

BIN
PathPlanning/FrenetOptimalTrajectory/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.4 MiB

24
PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py
View File

@@ -40,7 +40,7 @@ class Spline:
self.b.append(tb)
def calc(self, t):
u"""
"""
Calc position
if t is outside of the input x, return None
@@ -60,7 +60,7 @@ class Spline:
return result
def calcd(self, t):
u"""
"""
Calc first derivative
if t is outside of the input x, return None
@@ -77,7 +77,7 @@ class Spline:
return result
def calcdd(self, t):
u"""
"""
Calc second derivative
"""
@@ -92,13 +92,13 @@ class Spline:
return result
def __search_index(self, x):
u"""
"""
search data segment index
"""
return bisect.bisect(self.x, x) - 1
def __calc_A(self, h):
u"""
"""
calc matrix A for spline coefficient c
"""
A = np.zeros((self.nx, self.nx))
@@ -116,7 +116,7 @@ class Spline:
return A
def __calc_B(self, h):
u"""
"""
calc matrix B for spline coefficient c
"""
B = np.zeros(self.nx)
@@ -128,7 +128,7 @@ class Spline:
class Spline2D:
u"""
"""
2D Cubic Spline class
"""
@@ -148,7 +148,7 @@ class Spline2D:
return s
def calc_position(self, s):
u"""
"""
calc position
"""
x = self.sx.calc(s)
@@ -157,7 +157,7 @@ class Spline2D:
return x, y
def calc_curvature(self, s):
u"""
"""
calc curvature
"""
dx = self.sx.calcd(s)
@@ -168,7 +168,7 @@ class Spline2D:
return k
def calc_yaw(self, s):
u"""
"""
calc yaw
"""
dx = self.sx.calcd(s)
@@ -192,7 +192,7 @@ def calc_spline_course(x, y, ds=0.1):
return rx, ry, ryaw, rk, s
def main():
def main(): # pragma: no cover
print("Spline 2D test")
import matplotlib.pyplot as plt
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
@@ -235,5 +235,5 @@ def main():
plt.show()
if __name__ == '__main__':
if __name__ == '__main__': # pragma: no cover
main()

10
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -18,6 +18,8 @@ import copy
import math
import cubic_spline_planner
SIM_LOOP = 500
# Parameter
MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s]
MAX_ACCEL = 2.0 # maximum acceleration [m/ss]
@@ -257,7 +259,7 @@ def check_collision(fp, ob):
def check_paths(fplist, ob):
okind = []
for i in range(len(fplist)):
for i, _ in enumerate(fplist):
if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
continue
elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check
@@ -329,7 +331,7 @@ def main():
area = 20.0 # animation area length [m]
for i in range(500):
for i in range(SIM_LOOP):
path = frenet_optimal_planning(
csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)
@@ -343,7 +345,7 @@ def main():
print("Goal")
break
if show_animation:
if show_animation: # pragma: no cover
plt.cla()
plt.plot(tx, ty)
plt.plot(ob[:, 0], ob[:, 1], "xk")
@@ -356,7 +358,7 @@ def main():
plt.pause(0.0001)
print("Finish")
if show_animation:
if show_animation: # pragma: no cover
plt.grid(True)
plt.pause(0.0001)
plt.show()

308
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py Normal file
View File

@@ -0,0 +1,308 @@
"""
Grid based sweep planner
author: Atsushi Sakai
"""
import math
import os
import sys
from enum import IntEnum
import matplotlib.pyplot as plt
import numpy as np
sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/"))
try:
from grid_map_lib import GridMap
except ImportError:
raise
do_animation = True
class SweepSearcher:
class SweepDirection(IntEnum):
UP = 1
DOWN = -1
class MovingDirection(IntEnum):
RIGHT = 1
LEFT = -1
def __init__(self, mdirection, sdirection, xinds_goaly, goaly):
self.moving_direction = mdirection
self.sweep_direction = sdirection
self.turing_window = []
self.update_turning_window()
self.xinds_goaly = xinds_goaly
self.goaly = goaly
def move_target_grid(self, cxind, cyind, gmap):
nxind = self.moving_direction + cxind
nyind = cyind
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
return nxind, nyind
else: # occupided
ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
if (ncxind is None) and (ncyind is None):
# moving backward
ncxind = -self.moving_direction + cxind
ncyind = cyind
if gmap.check_occupied_from_xy_index(ncxind, ncyind):
# moved backward, but the grid is occupied by obstacle
return None, None
else:
# keep moving until end
while not gmap.check_occupied_from_xy_index(ncxind + self.moving_direction, ncyind, occupied_val=0.5):
ncxind += self.moving_direction
self.swap_moving_direction()
return ncxind, ncyind
def find_safe_turning_grid(self, cxind, cyind, gmap):
for (dxind, dyind) in self.turing_window:
nxind = dxind + cxind
nyind = dyind + cyind
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
return nxind, nyind
return None, None
def is_search_done(self, gmap):
for ix in self.xinds_goaly:
if not gmap.check_occupied_from_xy_index(ix, self.goaly, occupied_val=0.5):
return False
# all lower grid is occupied
return True
def update_turning_window(self):
self.turing_window = [
(self.moving_direction, 0.0),
(self.moving_direction, self.sweep_direction),
(0, self.sweep_direction),
(-self.moving_direction, self.sweep_direction),
]
def swap_moving_direction(self):
self.moving_direction *= -1
self.update_turning_window()
def search_start_grid(self, grid_map):
xinds = []
y_ind = 0
if self.sweep_direction == self.SweepDirection.DOWN:
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
elif self.sweep_direction == self.SweepDirection.UP:
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
if self.moving_direction == self.MovingDirection.RIGHT:
return min(xinds), y_ind
elif self.moving_direction == self.MovingDirection.LEFT:
return max(xinds), y_ind
raise ValueError("self.moving direction is invalid ")
def find_sweep_direction_and_start_posi(ox, oy):
# find sweep_direction
max_dist = 0.0
vec = [0.0, 0.0]
sweep_start_pos = [0.0, 0.0]
for i in range(len(ox) - 1):
dx = ox[i + 1] - ox[i]
dy = oy[i + 1] - oy[i]
d = np.sqrt(dx ** 2 + dy ** 2)
if d > max_dist:
max_dist = d
vec = [dx, dy]
sweep_start_pos = [ox[i], oy[i]]
return vec, sweep_start_pos
def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi):
tx = [ix - sweep_start_posi[0] for ix in ox]
ty = [iy - sweep_start_posi[1] for iy in oy]
th = math.atan2(sweep_vec[1], sweep_vec[0])
c = np.cos(-th)
s = np.sin(-th)
rx = [ix * c - iy * s for (ix, iy) in zip(tx, ty)]
ry = [ix * s + iy * c for (ix, iy) in zip(tx, ty)]
return rx, ry
def convert_global_coordinate(x, y, sweep_vec, sweep_start_posi):
th = math.atan2(sweep_vec[1], sweep_vec[0])
c = np.cos(th)
s = np.sin(th)
tx = [ix * c - iy * s for (ix, iy) in zip(x, y)]
ty = [ix * s + iy * c for (ix, iy) in zip(x, y)]
rx = [ix + sweep_start_posi[0] for ix in tx]
ry = [iy + sweep_start_posi[1] for iy in ty]
return rx, ry
def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
yind = None
xinds = []
if from_upper:
xrange = range(grid_map.height)[::-1]
yrange = range(grid_map.width)[::-1]
else:
xrange = range(grid_map.height)
yrange = range(grid_map.width)
for iy in xrange:
for ix in yrange:
if not grid_map.check_occupied_from_xy_index(ix, iy):
yind = iy
xinds.append(ix)
if yind:
break
return xinds, yind
def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10):
width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid
height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid
center_x = np.mean(ox)
center_y = np.mean(oy)
grid_map = GridMap(width, height, reso, center_x, center_y)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.expand_grid()
xinds_goaly = []
goaly = 0
if sweep_direction == SweepSearcher.SweepDirection.UP:
xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
return grid_map, xinds_goaly, goaly
def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
# search start grid
cxind, cyind = sweep_searcher.search_start_grid(gmap)
if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
print("Cannot find start grid")
return [], []
x, y = gmap.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
px, py = [x], [y]
if grid_search_animation:
fig, ax = plt.subplots()
while True:
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None):
print("Done")
break
x, y = gmap.calc_grid_central_xy_position_from_xy_index(
cxind, cyind)
px.append(x)
py.append(y)
gmap.set_value_from_xy_index(cxind, cyind, 0.5)
if grid_search_animation:
gmap.plot_grid_map(ax=ax)
plt.pause(1.0)
gmap.plot_grid_map()
return px, py
def planning(ox, oy, reso,
moving_direction=SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=SweepSearcher.SweepDirection.UP,
):
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(ox, oy)
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, sweeping_direction)
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_goaly, goaly)
px, py = sweep_path_search(sweep_searcher, gmap)
rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
print("Path length:", len(rx))
return rx, ry
def planning_animation(ox, oy, reso): # pragma: no cover
px, py = planning(ox, oy, reso)
# animation
if do_animation:
for ipx, ipy in zip(px, py):
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.plot(ipx, ipy, "or")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
def main(): # pragma: no cover
print("start!!")
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
planning_animation(ox, oy, reso)
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
reso = 1.3
planning_animation(ox, oy, reso)
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
planning_animation(ox, oy, reso)
plt.show()
print("done!!")
if __name__ == '__main__':
main()

231
PathPlanning/HybridAStar/a_star.py Normal file
View File

@@ -0,0 +1,231 @@
"""
A* grid based planning
author: Nikos Kanargias (nkana@tee.gr)
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import math
import heapq
import matplotlib.pyplot as plt
show_animation = False
class Node:
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
def calc_final_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
return rx, ry
def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
motion = get_motion_model()
openset, closedset = dict(), dict()
openset[calc_index(ngoal, xw, minx, miny)] = ngoal
pq = []
pq.append((0, calc_index(ngoal, xw, minx, miny)))
while 1:
if not pq:
break
cost, c_id = heapq.heappop(pq)
if c_id in openset:
current = openset[c_id]
closedset[c_id] = current
openset.pop(c_id)
else:
continue
# show graph
if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
# Remove the item from the open set
# expand search grid based on motion model
for i, _ in enumerate(motion):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if n_id in closedset:
continue
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
heapq.heappush(
pq, (node.cost, calc_index(node, xw, minx, miny)))
else:
if openset[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
heapq.heappush(
pq, (node.cost, calc_index(node, xw, minx, miny)))
rx, ry = calc_final_path(closedset[calc_index(
nstart, xw, minx, miny)], closedset, reso)
return rx, ry, closedset
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x >= maxx:
return False
elif node.y >= maxy:
return False
if obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(ox, oy, reso, vr):
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# obstacle map generation
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 2.0 # [m]
robot_size = 1.0 # [m]
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry, _ = dp_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':
show_animation = True
main()

103
PathPlanning/HybridAStar/car.py Normal file
View File

@@ -0,0 +1,103 @@
"""
Car model for Hybrid A* path planning
author: Zheng Zh (@Zhengzh)
"""
import matplotlib.pyplot as plt
from math import sqrt, cos, sin, tan, pi
WB = 3. # rear to front wheel
W = 2. # width of car
LF = 3.3 # distance from rear to vehicle front end
LB = 1.0 # distance from rear to vehicle back end
MAX_STEER = 0.6 # [rad] maximum steering angle
WBUBBLE_DIST = (LF - LB) / 2.0
WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1)
# vehicle rectangle verticles
VRX = [LF, LF, -LB, -LB, LF]
VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
for x, y, yaw in zip(xlist, ylist, yawlist):
cx = x + WBUBBLE_DIST * cos(yaw)
cy = y + WBUBBLE_DIST * sin(yaw)
ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
if not ids:
continue
if not rectangle_check(x, y, yaw,
[ox[i] for i in ids], [oy[i] for i in ids]):
return False # collision
return True # no collision
def rectangle_check(x, y, yaw, ox, oy):
# transform obstacles to base link frame
c, s = cos(-yaw), sin(-yaw)
for iox, ioy in zip(ox, oy):
tx = iox - x
ty = ioy - y
rx = c * tx - s * ty
ry = s * tx + c * ty
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
return False # no collision
return True # collision
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
"""Plot arrow."""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
else:
plt.arrow(x, y, length * cos(yaw), length * sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
# plt.plot(x, y)
def plot_car(x, y, yaw):
car_color = '-k'
c, s = cos(yaw), sin(yaw)
car_outline_x, car_outline_y = [], []
for rx, ry in zip(VRX, VRY):
tx = c * rx - s * ry + x
ty = s * rx + c * ry + y
car_outline_x.append(tx)
car_outline_y.append(ty)
arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
plot_arrow(arrow_x, arrow_y, arrow_yaw)
plt.plot(car_outline_x, car_outline_y, car_color)
def pi_2_pi(angle):
return (angle + pi) % (2 * pi) - pi
def move(x, y, yaw, distance, steer, L=WB):
x += distance * cos(yaw)
y += distance * sin(yaw)
yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
return x, y, yaw
if __name__ == '__main__':
x, y, yaw = 0., 0., 1.
plt.axis('equal')
plot_car(x, y, yaw)
plt.show()

386
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -2,41 +2,67 @@
Hybrid A* path planning
author: Atsushi Sakai (@Atsushi_twi)
author: Zheng Zh (@Zhengzh)
"""
import heapq
import scipy.spatial
import numpy as np
import math
import matplotlib.pyplot as plt
import sys
sys.path.append("../ReedsSheppPath/")
try:
from a_star import dp_planning # , calc_obstacle_map
import reeds_shepp_path_planning as rs
from car import move, check_car_collision, MAX_STEER, WB, plot_car
except:
raise
import math
import numpy as np
import scipy.spatial
import matplotlib.pyplot as plt
# import reeds_shepp_path_planning as rs
# import heapq
EXTEND_AREA = 5.0 # [m]
XY_GRID_RESOLUTION = 2.0 # [m]
YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad]
MOTION_RESOLUTION = 0.1 # [m] path interporate resolution
N_STEER = 20.0 # number of steer command
H_COST = 1.0
VR = 1.0 # robot radius
SB_COST = 100.0 # switch back penalty cost
BACK_COST = 5.0 # backward penalty cost
STEER_CHANGE_COST = 5.0 # steer angle change penalty cost
STEER_COST = 1.0 # steer angle change penalty cost
H_COST = 5.0 # Heuristic cost
show_animation = True
class Node:
def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind):
# store kd-tree
def __init__(self, xind, yind, yawind, direction,
xlist, ylist, yawlist, directions,
steer=0.0, pind=None, cost=None):
self.xind = xind
self.yind = yind
self.yawind = yawind
self.direction = direction
self.xlist = x
self.ylist = y
self.yawlist = yaw
self.directionlist = directions
self.xlist = xlist
self.ylist = ylist
self.yawlist = yawlist
self.directions = directions
self.steer = steer
self.cost = cost
self.pind = pind
self.cost = cost
class Path:
def __init__(self, xlist, ylist, yawlist, directionlist, cost):
self.xlist = xlist
self.ylist = ylist
self.yawlist = yawlist
self.directionlist = directionlist
self.cost = cost
class KDTree:
@@ -64,9 +90,9 @@ class KDTree:
dist.append(idist)
return index, dist
else:
dist, index = self.tree.query(inp, k=k)
return index, dist
dist, index = self.tree.query(inp, k=k)
return index, dist
def search_in_distance(self, inp, r):
"""
@@ -80,32 +106,179 @@ class KDTree:
class Config:
def __init__(self, ox, oy, xyreso, yawreso):
min_x_m = min(ox) - EXTEND_AREA
min_y_m = min(oy) - EXTEND_AREA
max_x_m = max(ox) + EXTEND_AREA
max_y_m = max(oy) + EXTEND_AREA
min_x_m = min(ox)
min_y_m = min(oy)
max_x_m = max(ox)
max_y_m = max(oy)
ox.append(min_x_m)
oy.append(min_y_m)
ox.append(max_x_m)
oy.append(max_y_m)
self.minx = int(min_x_m / xyreso)
self.miny = int(min_y_m / xyreso)
self.maxx = int(max_x_m / xyreso)
self.maxy = int(max_y_m / xyreso)
self.minx = round(min_x_m / xyreso)
self.miny = round(min_y_m / xyreso)
self.maxx = round(max_x_m / xyreso)
self.maxy = round(max_y_m / xyreso)
self.xw = int(self.maxx - self.minx)
self.yw = int(self.maxy - self.miny)
self.xw = round(self.maxx - self.minx)
self.yw = round(self.maxy - self.miny)
self.minyaw = int(- math.pi / yawreso) - 1
self.maxyaw = int(math.pi / yawreso)
self.yaww = int(self.maxyaw - self.minyaw)
self.minyaw = round(- math.pi / yawreso) - 1
self.maxyaw = round(math.pi / yawreso)
self.yaww = round(self.maxyaw - self.minyaw)
def analytic_expantion(current, ngoal, c, ox, oy, kdtree):
def calc_motion_inputs():
return False, None # no update
for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, N_STEER),[0.0])):
for d in [1, -1]:
yield [steer, d]
def get_neighbors(current, config, ox, oy, kdtree):
for steer, d in calc_motion_inputs():
node = calc_next_node(current, steer, d, config, ox, oy, kdtree)
if node and verify_index(node, config):
yield node
def calc_next_node(current, steer, direction, config, ox, oy, kdtree):
x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]
arc_l = XY_GRID_RESOLUTION * 1.5
xlist, ylist, yawlist = [], [], []
for dist in np.arange(0, arc_l, MOTION_RESOLUTION):
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
xlist.append(x)
ylist.append(y)
yawlist.append(yaw)
if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
return None
d = direction == 1
xind = round(x / XY_GRID_RESOLUTION)
yind = round(y / XY_GRID_RESOLUTION)
yawind = round(yaw / YAW_GRID_RESOLUTION)
addedcost = 0.0
if d != current.direction:
addedcost += SB_COST
# steer penalty
addedcost += STEER_COST * abs(steer)
# steer change penalty
addedcost += STEER_CHANGE_COST * abs(current.steer - steer)
cost = current.cost + addedcost + arc_l
node = Node(xind, yind, yawind, d, xlist,
ylist, yawlist, [d],
pind=calc_index(current, config),
cost=cost, steer=steer)
return node
def is_same_grid(n1, n2):
if n1.xind == n2.xind and n1.yind == n2.yind and n1.yawind == n2.yawind:
return True
return False
def analytic_expantion(current, goal, c, ox, oy, kdtree):
sx = current.xlist[-1]
sy = current.ylist[-1]
syaw = current.yawlist[-1]
gx = goal.xlist[-1]
gy = goal.ylist[-1]
gyaw = goal.yawlist[-1]
max_curvature = math.tan(MAX_STEER) / WB
paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw,
max_curvature, step_size=MOTION_RESOLUTION)
if not paths:
return None
best_path, best = None, None
for path in paths:
if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree):
cost = calc_rs_path_cost(path)
if not best or best > cost:
best = cost
best_path = path
return best_path
def update_node_with_analystic_expantion(current, goal,
c, ox, oy, kdtree):
apath = analytic_expantion(current, goal, c, ox, oy, kdtree)
if apath:
plt.plot(apath.x, apath.y)
fx = apath.x[1:]
fy = apath.y[1:]
fyaw = apath.yaw[1:]
fcost = current.cost + calc_rs_path_cost(apath)
fpind = calc_index(current, c)
fd = []
for d in apath.directions[1:]:
fd.append(d >= 0)
fsteer = 0.0
fpath = Node(current.xind, current.yind, current.yawind,
current.direction, fx, fy, fyaw, fd,
cost=fcost, pind=fpind, steer=fsteer)
return True, fpath
return False, None
def calc_rs_path_cost(rspath):
cost = 0.0
for l in rspath.lengths:
if l >= 0: # forward
cost += l
else: # back
cost += abs(l) * BACK_COST
# swich back penalty
for i in range(len(rspath.lengths) - 1):
if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: # switch back
cost += SB_COST
# steer penalyty
for ctype in rspath.ctypes:
if ctype != "S": # curve
cost += STEER_COST * abs(MAX_STEER)
# ==steer change penalty
# calc steer profile
nctypes = len(rspath.ctypes)
ulist = [0.0] * nctypes
for i in range(nctypes):
if rspath.ctypes[i] == "R":
ulist[i] = - MAX_STEER
elif rspath.ctypes[i] == "L":
ulist[i] = MAX_STEER
for i in range(len(rspath.ctypes) - 1):
cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
return cost
def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
@@ -118,49 +291,109 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
yawreso: yaw angle resolution [rad]
"""
# start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
# tox, toy = ox[:], oy[:]
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:]
# obkdtree = KDTree(np.vstack((tox, toy)).T)
obkdtree = KDTree(np.vstack((tox, toy)).T)
# c = Config(tox, toy, xyreso, yawreso)
config = Config(tox, toy, xyreso, yawreso)
# nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
# True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
# ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
# True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
nstart = Node(round(start[0] / xyreso), round(start[1] / xyreso), round(start[2] / yawreso),
True, [start[0]], [start[1]], [start[2]], [True], cost=0)
ngoal = Node(round(goal[0] / xyreso), round(goal[1] / xyreso), round(goal[2] / yawreso),
True, [goal[0]], [goal[1]], [goal[2]], [True])
# openList, closedList = {}, {}
# h = []
# # goalqueue = queue.PriorityQueue()
# pq = []
# openList[calc_index(nstart, c)] = nstart
# heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
openList, closedList = {}, {}
# while True:
# if not openList:
# print("Error: Cannot find path, No open set")
# return [], [], []
_, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1],
ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR)
# c_id, cost = heapq.heappop(pq)
# current = openList.pop(c_id)
# closedList[c_id] = current
pq = []
openList[calc_index(nstart, config)] = nstart
heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config),
calc_index(nstart, config)))
# isupdated, fpath = analytic_expantion(
# current, ngoal, c, ox, oy, obkdtree)
while True:
if not openList:
print("Error: Cannot find path, No open set")
return [], [], []
# # print(current)
cost, c_id = heapq.heappop(pq)
if c_id in openList:
current = openList.pop(c_id)
closedList[c_id] = current
else:
continue
rx, ry, ryaw = [], [], []
if show_animation: # pragma: no cover
plt.plot(current.xlist[-1], current.ylist[-1], "xc")
if len(closedList.keys()) % 10 == 0:
plt.pause(0.001)
return rx, ry, ryaw
isupdated, fpath = update_node_with_analystic_expantion(
current, ngoal, config, ox, oy, obkdtree)
if isupdated:
break
for neighbor in get_neighbors(current, config, ox, oy, obkdtree):
neighbor_index = calc_index(neighbor, config)
if neighbor_index in closedList:
continue
if neighbor not in openList \
or openList[neighbor_index].cost > neighbor.cost:
heapq.heappush(
pq, (calc_cost(neighbor, h_dp, ngoal, config),
neighbor_index))
openList[neighbor_index] = neighbor
path = get_final_path(closedList, fpath, nstart, config)
return path
def calc_cost(n, h, ngoal, c):
def calc_cost(n, h_dp, goal, c):
ind = (n.yind - c.miny) * c.xw + (n.xind - c.minx)
if ind not in h_dp:
return n.cost + 999999999 # collision cost
return n.cost + H_COST * h_dp[ind].cost
hcost = 1.0
return (n.cost + H_COST * hcost)
def get_final_path(closed, ngoal, nstart, config):
rx, ry, ryaw = list(reversed(ngoal.xlist)), list(
reversed(ngoal.ylist)), list(reversed(ngoal.yawlist))
direction = list(reversed(ngoal.directions))
nid = ngoal.pind
finalcost = ngoal.cost
while nid:
n = closed[nid]
rx.extend(list(reversed(n.xlist)))
ry.extend(list(reversed(n.ylist)))
ryaw.extend(list(reversed(n.yawlist)))
direction.extend(list(reversed(n.directions)))
nid = n.pind
rx = list(reversed(rx))
ry = list(reversed(ry))
ryaw = list(reversed(ryaw))
direction = list(reversed(direction))
# adjust first direction
direction[0] = direction[1]
path = Path(rx, ry, ryaw, direction, finalcost)
return path
def verify_index(node, c):
xind, yind = node.xind, node.yind
if xind >= c.minx and xind <= c.maxx and yind >= c.miny \
and yind <= c.maxy:
return True
return False
def calc_index(node, c):
@@ -201,21 +434,30 @@ def main():
start = [10.0, 10.0, np.deg2rad(90.0)]
goal = [50.0, 50.0, np.deg2rad(-90.0)]
xyreso = 2.0
yawreso = np.deg2rad(15.0)
rx, ry, ryaw = hybrid_a_star_planning(
start, goal, ox, oy, xyreso, yawreso)
plt.plot(ox, oy, ".k")
# rs.plot_arrow(start[0], start[1], start[2])
# rs.plot_arrow(goal[0], goal[1], goal[2])
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
plt.show()
print(__file__ + " start!!")
path = hybrid_a_star_planning(
start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
x = path.xlist
y = path.ylist
yaw = path.yawlist
for ix, iy, iyaw in zip(x, y, yaw):
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plot_car(ix, iy, iyaw)
plt.pause(0.0001)
print(__file__ + " done!!")
if __name__ == '__main__':

BIN
PathPlanning/InformedRRTStar/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 4.8 MiB

189
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

@@ -4,22 +4,22 @@ Informed RRT* path planning
author: Karan Chawla
Atsushi Sakai(@Atsushi_twi)
Reference: Informed RRT*: Optimal Sampling-based Path Planning Focused via
Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via
Direct Sampling of an Admissible Ellipsoidal Heuristichttps://arxiv.org/pdf/1404.2334.pdf
"""
import random
import numpy as np
import math
import copy
import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
class InformedRRTStar():
class InformedRRTStar:
def __init__(self, start, goal,
obstacleList, randArea,
@@ -27,16 +27,17 @@ class InformedRRTStar():
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.expandDis = expandDis
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.min_rand = randArea[0]
self.max_rand = randArea[1]
self.expand_dis = expandDis
self.goal_sample_rate = goalSampleRate
self.max_iter = maxIter
self.obstacle_list = obstacleList
self.node_list = None
def InformedRRTStarSearch(self, animation=True):
def informed_rrt_star_search(self, animation=True):
self.nodeList = [self.start]
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space, starts as infinite
cBest = float('inf')
pathLen = float('inf')
@@ -44,8 +45,8 @@ class InformedRRTStar():
path = None
# Computing the sampling space
cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) +
pow(self.start.y - self.goal.y, 2))
cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
+ pow(self.start.y - self.goal.y, 2))
xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
[(self.start.y + self.goal.y) / 2.0], [0]])
a1 = np.array([[(self.goal.x - self.start.x) / cMin],
@@ -55,62 +56,62 @@ class InformedRRTStar():
# first column of idenity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = a1 @ id1_t
U, S, Vh = np.linalg.svd(M, 1, 1)
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 i in range(self.maxIter):
for i in range(self.max_iter):
# Sample space is defined by cBest
# cMin is the minimum distance between the start point and the goal
# xCenter is the midpoint between the start and the goal
# cBest changes when a new path is found
rnd = self.informed_sample(cBest, cMin, xCenter, C)
nind = self.getNearestListIndex(self.nodeList, rnd)
nearestNode = self.nodeList[nind]
nind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[nind]
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = self.getNewNode(theta, nind, nearestNode)
d = self.lineCost(nearestNode, newNode)
newNode = self.get_new_node(theta, nind, nearestNode)
d = self.line_cost(nearestNode, newNode)
isCollision = self.__CollisionCheck(newNode, self.obstacleList)
isCollision = self.collision_check(newNode, self.obstacle_list)
isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
if isCollision and isCollisionEx:
nearInds = self.findNearNodes(newNode)
newNode = self.chooseParent(newNode, nearInds)
nearInds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearInds)
self.nodeList.append(newNode)
self.node_list.append(newNode)
self.rewire(newNode, nearInds)
if self.isNearGoal(newNode):
if self.is_near_goal(newNode):
solutionSet.add(newNode)
lastIndex = len(self.nodeList) - 1
tempPath = self.getFinalCourse(lastIndex)
tempPathLen = self.getPathLen(tempPath)
lastIndex = len(self.node_list) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if tempPathLen < pathLen:
path = tempPath
cBest = tempPathLen
if animation:
self.drawGraph(xCenter=xCenter,
cBest=cBest, cMin=cMin,
etheta=etheta, rnd=rnd)
self.draw_graph(xCenter=xCenter,
cBest=cBest, cMin=cMin,
etheta=etheta, rnd=rnd)
return path
def chooseParent(self, newNode, nearInds):
def choose_parent(self, newNode, nearInds):
if len(nearInds) == 0:
return newNode
dList = []
for i in nearInds:
dx = newNode.x - self.nodeList[i].x
dy = newNode.y - self.nodeList[i].y
dx = newNode.x - self.node_list[i].x
dy = newNode.y - self.node_list[i].y
d = math.sqrt(dx ** 2 + dy ** 2)
theta = math.atan2(dy, dx)
if self.check_collision_extend(self.nodeList[i], theta, d):
dList.append(self.nodeList[i].cost + d)
if self.check_collision_extend(self.node_list[i], theta, d):
dList.append(self.node_list[i].cost + d)
else:
dList.append(float('inf'))
@@ -126,29 +127,30 @@ class InformedRRTStar():
return newNode
def findNearNodes(self, newNode):
nnode = len(self.nodeList)
def find_near_nodes(self, newNode):
nnode = len(self.node_list)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
dlist = [(node.x - newNode.x) ** 2 +
(node.y - newNode.y) ** 2 for node in self.nodeList]
dlist = [(node.x - newNode.x) ** 2
+ (node.y - newNode.y) ** 2 for node in self.node_list]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
def informed_sample(self, cMax, cMin, xCenter, C):
if cMax < float('inf'):
r = [cMax / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0]
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
L = np.diag(r)
xBall = self.sampleUnitBall()
xBall = self.sample_unit_ball()
rnd = np.dot(np.dot(C, L), xBall) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sampleFreeSpace()
rnd = self.sample_free_space()
return rnd
def sampleUnitBall(self):
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
@@ -159,69 +161,73 @@ class InformedRRTStar():
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
def sampleFreeSpace(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand)]
def sample_free_space(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = [random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)]
else:
rnd = [self.goal.x, self.goal.y]
return rnd
def getPathLen(self, path):
@staticmethod
def get_path_len(path):
pathLen = 0
for i in range(1, len(path)):
node1_x = path[i][0]
node1_y = path[i][1]
node2_x = path[i - 1][0]
node2_y = path[i - 1][1]
pathLen += math.sqrt((node1_x - node2_x) **
2 + (node1_y - node2_y)**2)
pathLen += math.sqrt((node1_x - node2_x)
** 2 + (node1_y - node2_y) ** 2)
return pathLen
def lineCost(self, node1, node2):
return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
@staticmethod
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
def getNearestListIndex(self, nodes, rnd):
dList = [(node.x - rnd[0])**2 +
(node.y - rnd[1])**2 for node in nodes]
@staticmethod
def get_nearest_list_index(nodes, rnd):
dList = [(node.x - rnd[0]) ** 2
+ (node.y - rnd[1]) ** 2 for node in nodes]
minIndex = dList.index(min(dList))
return minIndex
def __CollisionCheck(self, newNode, obstacleList):
@staticmethod
def collision_check(newNode, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - newNode.x
dy = oy - newNode.y
d = dx * dx + dy * dy
if d <= 1.1 * size**2:
if d <= 1.1 * size ** 2:
return False # collision
return True # safe
def getNewNode(self, theta, nind, nearestNode):
def get_new_node(self, theta, nind, nearestNode):
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.x += self.expand_dis * math.cos(theta)
newNode.y += self.expand_dis * math.sin(theta)
newNode.cost += self.expandDis
newNode.cost += self.expand_dis
newNode.parent = nind
return newNode
def isNearGoal(self, node):
d = self.lineCost(node, self.goal)
if d < self.expandDis:
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
if d < self.expand_dis:
return True
return False
def rewire(self, newNode, nearInds):
nnode = len(self.nodeList)
n_node = len(self.node_list)
for i in nearInds:
nearNode = self.nodeList[i]
nearNode = self.node_list[i]
d = math.sqrt((nearNode.x - newNode.x)**2 +
(nearNode.y - newNode.y)**2)
d = math.sqrt((nearNode.x - newNode.x) ** 2
+ (nearNode.y - newNode.y) ** 2)
scost = newNode.cost + d
@@ -229,30 +235,30 @@ class InformedRRTStar():
theta = math.atan2(newNode.y - nearNode.y,
newNode.x - nearNode.x)
if self.check_collision_extend(nearNode, theta, d):
nearNode.parent = nnode - 1
nearNode.parent = n_node - 1
nearNode.cost = scost
def check_collision_extend(self, nearNode, theta, d):
tmpNode = copy.deepcopy(nearNode)
for i in range(int(d / self.expandDis)):
tmpNode.x += self.expandDis * math.cos(theta)
tmpNode.y += self.expandDis * math.sin(theta)
if not self.__CollisionCheck(tmpNode, self.obstacleList):
for i in range(int(d / self.expand_dis)):
tmpNode.x += self.expand_dis * math.cos(theta)
tmpNode.y += self.expand_dis * math.sin(theta)
if not self.collision_check(tmpNode, self.obstacle_list):
return False
return True
def getFinalCourse(self, lastIndex):
def get_final_course(self, lastIndex):
path = [[self.goal.x, self.goal.y]]
while self.nodeList[lastIndex].parent is not None:
node = self.nodeList[lastIndex]
while self.node_list[lastIndex].parent is not None:
node = self.node_list[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
plt.clf()
if rnd is not None:
@@ -260,13 +266,13 @@ class InformedRRTStar():
if cBest != float('inf'):
self.plot_ellipse(xCenter, cBest, cMin, etheta)
for node in self.nodeList:
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
plt.plot([node.x, self.node_list[node.parent].x], [
node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
@@ -275,9 +281,10 @@ class InformedRRTStar():
plt.grid(True)
plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta):
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - etheta
cx = xCenter[0]
@@ -295,7 +302,7 @@ class InformedRRTStar():
plt.plot(px, py, "--c")
class Node():
class Node:
def __init__(self, x, y):
self.x = x
@@ -320,12 +327,12 @@ def main():
# Set params
rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
randArea=[-2, 15], obstacleList=obstacleList)
path = rrt.InformedRRTStarSearch(animation=show_animation)
path = rrt.informed_rrt_star_search(animation=show_animation)
print("Done!!")
# Plot path
if show_animation:
rrt.drawGraph()
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01)
@@ -333,4 +340,4 @@ def main():
if __name__ == '__main__':
main()
main()

169
PathPlanning/LQRPlanner/LQRplanner.py
View File

@@ -6,114 +6,113 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la
import math
import random
show_animation = True
import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la
MAX_TIME = 100.0 # Maximum simulation time
DT = 0.1 # Time tick
SHOW_ANIMATION = True
def LQRplanning(sx, sy, gx, gy):
class LQRPlanner:
rx, ry = [sx], [sy]
def __init__(self):
self.MAX_TIME = 100.0 # Maximum simulation time
self.DT = 0.1 # Time tick
self.GOAL_DIST = 0.1
self.MAX_ITER = 150
self.EPS = 0.01
x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector
def lqr_planning(self, sx, sy, gx, gy, show_animation=SHOW_ANIMATION):
# Linear system model
A, B = get_system_model()
rx, ry = [sx], [sy]
found_path = False
x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector
time = 0.0
while time <= MAX_TIME:
time += DT
# Linear system model
A, B = self.get_system_model()
u = LQR_control(A, B, x)
found_path = False
x = A @ x + B @ u
time = 0.0
while time <= self.MAX_TIME:
time += self.DT
rx.append(x[0, 0] + gx)
ry.append(x[1, 0] + gy)
u = self.lqr_control(A, B, x)
d = math.sqrt((gx - rx[-1])**2 + (gy - ry[-1])**2)
if d <= 0.1:
# print("Goal!!")
found_path = True
break
x = A @ x + B @ u
# animation
if show_animation:
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.pause(1.0)
rx.append(x[0, 0] + gx)
ry.append(x[1, 0] + gy)
if not found_path:
print("Cannot found path")
return [], []
d = math.sqrt((gx - rx[-1]) ** 2 + (gy - ry[-1]) ** 2)
if d <= self.GOAL_DIST:
found_path = True
break
return rx, ry
# animation
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.pause(1.0)
if not found_path:
print("Cannot found path")
return [], []
def solve_DARE(A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
maxiter = 150
eps = 0.01
return rx, ry
for i in range(maxiter):
Xn = A.T * X * A - A.T * X * B * \
la.inv(R + B.T * X * B) * B.T * X * A + Q
if (abs(Xn - X)).max() < eps:
def solve_dare(self, A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X, Xn = Q, Q
for i in range(self.MAX_ITER):
Xn = A.T * X * A - A.T * X * B * \
la.inv(R + B.T * X * B) * B.T * X * A + Q
if (abs(Xn - X)).max() < self.EPS:
break
X = Xn
break
X = Xn
return Xn
return Xn
def dlqr(self, A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
# ref Bertsekas, p.151
"""
def dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
# ref Bertsekas, p.151
"""
# first, try to solve the ricatti equation
X = self.solve_dare(A, B, Q, R)
# first, try to solve the ricatti equation
X = solve_DARE(A, B, Q, R)
# compute the LQR gain
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
# compute the LQR gain
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
eigValues = la.eigvals(A - B @ K)
eigVals, eigVecs = la.eig(A - B @ K)
return K, X, eigValues
return K, X, eigVals
def get_system_model(self):
A = np.array([[self.DT, 1.0],
[0.0, self.DT]])
B = np.array([0.0, 1.0]).reshape(2, 1)
def get_system_model():
return A, B
A = np.array([[DT, 1.0],
[0.0, DT]])
B = np.array([0.0, 1.0]).reshape(2, 1)
def lqr_control(self, A, B, x):
return A, B
Kopt, X, ev = self.dlqr(A, B, np.eye(2), np.eye(1))
u = -Kopt @ x
def LQR_control(A, B, x):
Kopt, X, ev = dlqr(A, B, np.eye(2), np.eye(1))
u = -Kopt @ x
return u
return u
def main():
@@ -122,15 +121,17 @@ def main():
ntest = 10 # number of goal
area = 100.0 # sampling area
lqr_planner = LQRPlanner()
for i in range(ntest):
sx = 6.0
sy = 6.0
gx = random.uniform(-area, area)
gy = random.uniform(-area, area)
rx, ry = LQRplanning(sx, sy, gx, gy)
rx, ry = lqr_planner.lqr_planning(sx, sy, gx, gy)
if show_animation:
if SHOW_ANIMATION: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
@@ -138,23 +139,5 @@ def main():
plt.pause(1.0)
def main1():
print(__file__ + " start!!")
sx = 6.0
sy = 6.0
gx = 10.0
gy = 10.0
rx, ry = LQRplanning(sx, sy, gx, gy)
if show_animation:
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()

BIN
PathPlanning/LQRPlanner/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 615 KiB

BIN
PathPlanning/LQRRRTStar/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.5 MiB

366
PathPlanning/LQRRRTStar/lqr_rrt_star.py
View File

@@ -5,112 +5,166 @@ Path planning code with LQR RRT*
author: AtsushiSakai(@Atsushi_twi)
"""
import sys
sys.path.append("../LQRPlanner/")
import random
import math
import copy
import numpy as np
import math
import os
import random
import sys
import matplotlib.pyplot as plt
import LQRplanner
import numpy as np
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRTStar/")
try:
from LQRplanner import LQRPlanner
from rrt_star import RRTStar
except ImportError:
raise
show_animation = True
LQRplanner.show_animation = False
STEP_SIZE = 0.05 # step size of local path
XYTH = 0.5 # [m] acceptance xy distance in final paths
class RRT():
class LQRRRTStar(RRTStar):
"""
Class for RRT Planning
Class for RRT star planning with LQR planning
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=200):
def __init__(self, start, goal, obstacle_list, rand_area,
goal_sample_rate=10,
max_iter=200,
connect_circle_dist=50.0,
step_size=0.2
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
def planning(self, animation=True):
self.curvature = 1.0
self.goal_xy_th = 0.5
self.step_size = step_size
self.lqr_planner = LQRPlanner()
def planning(self, animation=True, search_until_max_iter=True):
"""
Pathplanning
RRT Star planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.get_nearest_index(self.nodeList, rnd)
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
newNode = self.steer(rnd, nind)
if newNode is None:
continue
if self.check_collision(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
if newNode is None:
continue
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
if self.check_collision(new_node, self.obstacle_list):
near_indexes = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_indexes)
if new_node:
self.node_list.append(new_node)
self.rewire(new_node, near_indexes)
if animation and i % 5 == 0:
self.draw_graph(rnd=rnd)
self.draw_graph(rnd)
# generate coruse
lastIndex = self.get_best_last_index()
if lastIndex is None:
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
print("reached max iteration")
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
else:
print("Cannot find path")
return None
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def search_best_goal_node(self):
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.goal_xy_th]
if not goal_inds:
return None
path = self.gen_final_course(lastIndex)
min_cost = min([self.node_list[i].cost for i in goal_inds])
for i in goal_inds:
if self.node_list[i].cost == min_cost:
return i
return None
def calc_new_cost(self, from_node, to_node):
wx, wy = self.lqr_planner.lqr_planning(
from_node.x, from_node.y, to_node.x, to_node.y, show_animation=False)
px, py, course_lengths = self.sample_path(wx, wy, self.step_size)
if not course_lengths:
return float("inf")
return from_node.cost + sum(course_lengths)
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)
)
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
def generate_final_course(self, goal_index):
print("final")
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_index]
while node.parent:
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
node = node.parent
path.append([self.start.x, self.start.y])
return path
def choose_parent(self, newNode, nearinds):
if len(nearinds) == 0:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if tNode is None:
continue
if self.check_collision(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2*math.pi) - math.pi
def sample_path(self, wx, wy, step):
px, py, clen = [], [], []
@@ -124,163 +178,33 @@ class RRT():
dx = np.diff(px)
dy = np.diff(py)
clen = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
clen = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
return px, py, clen
def steer(self, rnd, nind):
def steer(self, from_node, to_node):
nearestNode = self.nodeList[nind]
wx, wy = self.lqr_planner.lqr_planning(
from_node.x, from_node.y, to_node.x, to_node.y, show_animation=False)
wx, wy = LQRplanner.LQRplanning(
nearestNode.x, nearestNode.y, rnd.x, rnd.y)
px, py, clen = self.sample_path(wx, wy, STEP_SIZE)
px, py, course_lens = self.sample_path(wx, wy, self.step_size)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
newNode = copy.deepcopy(from_node)
newNode.x = px[-1]
newNode.y = py[-1]
newNode.path_x = px
newNode.path_y = py
newNode.cost += sum([abs(c) for c in clen])
newNode.parent = nind
newNode.cost += sum([abs(c) for c in course_lens])
newNode.parent = from_node
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y]
node = Node(rnd[0], rnd[1])
return node
def get_best_last_index(self):
# print("get_best_last_index")
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
goalinds.append(i)
if len(goalinds) == 0:
return None
mincost = min([self.nodeList[i].cost for i in goalinds])
for i in goalinds:
if self.nodeList[i].cost == mincost:
return i
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
goalind = node.parent
path.append([self.start.x, self.start.y])
return path
def calc_dist_to_goal(self, x, y):
return np.linalg.norm([x - self.end.x, y - self.end.y])
def find_near_nodes(self, newNode):
nnode = len(self.nodeList)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
dlist = [(node.x - newNode.x) ** 2 +
(node.y - newNode.y) ** 2
for node in self.nodeList]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
def rewire(self, newNode, nearinds):
nnode = len(self.nodeList)
for i in nearinds:
nearNode = self.nodeList[i]
tNode = self.steer(nearNode, nnode - 1)
if tNode is None:
continue
obstacleOK = self.check_collision(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "or")
plt.plot(self.end.x, self.end.y, "or")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def get_nearest_index(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2 +
(node.y - rnd.y) ** 2
for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def check_collision(self, node, obstacleList):
px = np.array(node.path_x)
py = np.array(node.path_y)
for (ox, oy, size) in obstacleList:
dx = ox - px
dy = oy - py
d = dx ** 2 + dy ** 2
dmin = min(d)
if dmin <= size ** 2:
return False # collision
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.cost = 0.0
self.parent = None
def main():
print("Start rrt start planning")
def main(maxIter=200):
print("Start " + __file__)
# ====Search Path with RRT====
obstacleList = [
@@ -296,12 +220,14 @@ def main():
start = [0.0, 0.0]
goal = [6.0, 7.0]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.planning(animation=show_animation)
lqr_rrt_star = LQRRRTStar(start, goal,
obstacleList,
[-2.0, 15.0])
path = lqr_rrt_star.planning(animation=show_animation)
# Draw final path
if show_animation:
rrt.draw_graph()
if show_animation: # pragma: no cover
lqr_rrt_star.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)

BIN
PathPlanning/ModelPredictiveTrajectoryGenerator/animation2.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 527 KiB

BIN
PathPlanning/ModelPredictiveTrajectoryGenerator/k0animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 348 KiB

BIN
PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 400 KiB

57
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -6,9 +6,11 @@ author: Atsushi Sakai(@Atsushi_twi)
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
import numpy as np
import motion_model
# optimization parameter
@@ -16,11 +18,11 @@ max_iter = 100
h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance
cost_th = 0.1
show_animation = False
show_animation = True
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""
Plot arrow
"""
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
@@ -37,7 +39,7 @@ def calc_diff(target, x, y, yaw):
return d
def calc_J(target, p, h, k0):
def calc_j(target, p, h, k0):
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0] + h[0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
@@ -68,7 +70,6 @@ def calc_J(target, p, h, k0):
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 1.0
maxa = 2.0
@@ -91,8 +92,7 @@ def selection_learning_param(dp, p, k0, target):
return mina
def show_trajectory(target, xc, yc):
def show_trajectory(target, xc, yc): # pragma: no cover
plt.clf()
plot_arrow(target.x, target.y, target.yaw)
plt.plot(xc, yc, "-r")
@@ -111,7 +111,7 @@ def optimize_trajectory(target, k0, p):
print("path is ok cost is:" + str(cost))
break
J = calc_J(target, p, h, k0)
J = calc_j(target, p, h, k0)
try:
dp = - np.linalg.inv(J) @ dc
except np.linalg.linalg.LinAlgError:
@@ -123,7 +123,7 @@ def optimize_trajectory(target, k0, p):
p += alpha * np.array(dp)
# print(p.T)
if show_animation:
if show_animation: # pragma: no cover
show_trajectory(target, xc, yc)
else:
xc, yc, yawc, p = None, None, None, None
@@ -132,7 +132,7 @@ def optimize_trajectory(target, k0, p):
return xc, yc, yawc, p
def test_optimize_trajectory():
def test_optimize_trajectory(): # pragma: no cover
# target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0))
@@ -142,35 +142,16 @@ def test_optimize_trajectory():
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
show_trajectory(target, x, y)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
if show_animation:
show_trajectory(target, x, y)
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
def test_trajectory_generate():
s = 5.0 # [m]
k0 = 0.0
km = np.deg2rad(30.0)
kf = np.deg2rad(-30.0)
# plt.plot(xk, yk, "xr")
# plt.plot(t, kp)
# plt.show()
x, y = motion_model.generate_trajectory(s, km, kf, k0)
plt.plot(x, y, "-r")
plt.axis("equal")
plt.grid(True)
plt.show()
def main():
def main(): # pragma: no cover
print(__file__ + " start!!")
# test_trajectory_generate()
test_optimize_trajectory()

5
PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
View File

@@ -18,7 +18,7 @@ class State:
def pi_2_pi(angle):
return (angle + math.pi) % (2*math.pi) - math.pi
return (angle + math.pi) % (2 * math.pi) - math.pi
def update(state, v, delta, dt, L):
@@ -74,5 +74,6 @@ def generate_last_state(s, km, kf, k0):
state = State()
[update(state, v, ikp, dt, L) for ikp in kp]
_ = [update(state, v, ikp, dt, L) for ikp in kp]
return state.x, state.y, state.yaw

BIN
PathPlanning/PotentialFieldPlanning/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 4.9 MiB

9
PathPlanning/PotentialFieldPlanning/potential_field_planning.py
View File

@@ -52,7 +52,7 @@ def calc_repulsive_potential(x, y, ox, oy, rr):
# search nearest obstacle
minid = -1
dmin = float("inf")
for i in range(len(ox)):
for i, _ in enumerate(ox):
d = np.hypot(x - ox[i], y - oy[i])
if dmin >= d:
dmin = d
@@ -106,7 +106,7 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
while d >= reso:
minp = float("inf")
minix, miniy = -1, -1
for i in range(len(motion)):
for i, _ in enumerate(motion):
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
if inx >= len(pmap) or iny >= len(pmap[0]):
@@ -157,12 +157,9 @@ def main():
plt.axis("equal")
# path generation
rx, ry = potential_field_planning(
_, _ = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
print(rx)
print(ry)
if show_animation:
plt.show()

BIN
PathPlanning/ProbabilisticRoadMap/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 6.5 MiB

40
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -45,7 +45,7 @@ class KDTree:
self.tree = scipy.spatial.cKDTree(data)
def search(self, inp, k=1):
u"""
"""
Search NN
inp: input data, single frame or multi frame
@@ -62,12 +62,12 @@ class KDTree:
dist.append(idist)
return index, dist
else:
dist, index = self.tree.query(inp, k=k)
return index, dist
dist, index = self.tree.query(inp, k=k)
return index, dist
def search_in_distance(self, inp, r):
u"""
"""
find points with in a distance r
"""
@@ -161,12 +161,18 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
"""
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
gy: goal y position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
rr: robot radius [m]
road_map: ??? [m]
sample_x: ??? [m]
sample_y: ??? [m]
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
"""
nstart = Node(sx, sy, 0.0, -1)
@@ -175,9 +181,12 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset, closedset = dict(), dict()
openset[len(road_map) - 2] = nstart
path_found = True
while True:
if len(openset) == 0:
if not openset:
print("Cannot find path")
path_found = False
break
c_id = min(openset, key=lambda o: openset[o].cost)
@@ -217,6 +226,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[n_id].pind = c_id
else:
openset[n_id] = node
if path_found is False:
return [], []
# generate final course
rx, ry = [ngoal.x], [ngoal.y]
@@ -230,9 +242,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
return rx, ry
def plot_road_map(road_map, sample_x, sample_y):
def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
for i in range(len(road_map)):
for i, _ in enumerate(road_map):
for ii in range(len(road_map[i])):
ind = road_map[i][ii]
@@ -249,8 +261,8 @@ def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree):
sample_x, sample_y = [], []
while len(sample_x) <= N_SAMPLE:
tx = (random.random() - minx) * (maxx - minx)
ty = (random.random() - miny) * (maxy - miny)
tx = (random.random() * (maxx - minx)) + minx
ty = (random.random() * (maxy - miny)) + miny
index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1))
@@ -307,7 +319,7 @@ def main():
rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
assert len(rx) != 0, 'Cannot found path'
assert rx, 'Cannot found path'
if show_animation:
plt.plot(rx, ry, "-r")

BIN
PathPlanning/QuinticPolynomialsPlanner/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.1 MiB

31
PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
View File

@@ -1,19 +1,20 @@
"""
Quinitc Polynomials Planner
Quintic Polynomials Planner
author: Atsushi Sakai (@Atsushi_twi)
Ref:
- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
import numpy as np
# parameter
MAX_T = 100.0 # maximum time to the goal [s]
MIN_T = 5.0 # minimum time to the goal[s]
@@ -21,7 +22,7 @@ MIN_T = 5.0 # minimum time to the goal[s]
show_animation = True
class quinic_polynomial:
class QuinticPolynomial:
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
@@ -72,9 +73,9 @@ class quinic_polynomial:
return xt
def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
"""
quinic polynomial planner
quintic polynomial planner
input
sx: start x position [m]
@@ -109,9 +110,11 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
axg = ga * math.cos(gyaw)
ayg = ga * math.sin(gyaw)
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
for T in np.arange(MIN_T, MAX_T, MIN_T):
xqp = quinic_polynomial(sx, vxs, axs, gx, vxg, axg, T)
yqp = quinic_polynomial(sy, vys, ays, gy, vyg, ayg, T)
xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
@@ -145,8 +148,8 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
print("find path!!")
break
if show_animation:
for i in range(len(rx)):
if show_animation: # pragma: no cover
for i, _ in enumerate(time):
plt.cla()
plt.grid(True)
plt.axis("equal")
@@ -163,7 +166,7 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
return time, rx, ry, ryaw, rv, ra, rj
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""
Plot arrow
"""
@@ -194,10 +197,10 @@ def main():
max_jerk = 0.5 # max jerk [m/sss]
dt = 0.1 # time tick [s]
time, x, y, yaw, v, a, j = quinic_polynomials_planner(
time, x, y, yaw, v, a, j = quintic_polynomials_planner(
sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)
if show_animation:
if show_animation: # pragma: no cover
plt.plot(x, y, "-r")
plt.subplots()

0
PathPlanning/RRT/__init__.py Normal file
View File

BIN
PathPlanning/RRT/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.2 MiB

219
PathPlanning/RRT/rrt.py Normal file
View File

@@ -0,0 +1,219 @@
"""
Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRT)
author: AtsushiSakai(@Atsushi_twi)
"""
import math
import random
import matplotlib.pyplot as plt
show_animation = True
class RRT:
"""
Class for RRT planning
"""
class Node:
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.parent = None
def __init__(self, start, goal, obstacle_list, rand_area,
expand_dis=3.0, path_resolution=1.0, goal_sample_rate=5, max_iter=500):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
"""
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = []
def planning(self, animation=True):
"""
rrt path planning
animation: flag for animation on or off
"""
self.node_list = [self.start]
for i in range(self.max_iter):
rnd_node = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
nearest_node = self.node_list[nearest_ind]
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
if animation and i % 5 == 0:
self.draw_graph(rnd_node)
if self.calc_dist_to_goal(new_node.x, new_node.y) <= self.expand_dis:
print("Goal!!")
return self.generate_final_course(len(self.node_list) - 1)
if animation and i % 5:
self.draw_graph(rnd_node)
return None # cannot find path
def steer(self, from_node, to_node, extend_length=float("inf")):
new_node = self.Node(from_node.x, from_node.y)
d, theta = self.calc_distance_and_angle(new_node, to_node)
new_node.path_x = [new_node.x]
new_node.path_y = [new_node.y]
if extend_length > d:
extend_length = d
n_expand = math.floor(extend_length / self.path_resolution)
for _ in range(n_expand):
new_node.x += self.path_resolution * math.cos(theta)
new_node.y += self.path_resolution * math.sin(theta)
new_node.path_x.append(new_node.x)
new_node.path_y.append(new_node.y)
d, _ = self.calc_distance_and_angle(new_node, to_node)
if d <= self.path_resolution:
new_node.x = to_node.x
new_node.y = to_node.y
new_node.path_x[-1] = to_node.x
new_node.path_y[-1] = to_node.y
new_node.parent = from_node
return new_node
def generate_final_course(self, goal_ind):
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_ind]
while node.parent is not None:
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
def calc_dist_to_goal(self, x, y):
dx = x - self.end.x
dy = y - self.end.y
return math.sqrt(dx ** 2 + dy ** 2)
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
@staticmethod
def get_nearest_node_index(node_list, rnd_node):
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
** 2 for node in node_list]
minind = dlist.index(min(dlist))
return minind
@staticmethod
def check_collision(node, obstacleList):
for (ox, oy, size) in obstacleList:
dx_list = [ox - x for x in node.path_x]
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= size ** 2:
return False # collision
return True # safe
@staticmethod
def calc_distance_and_angle(from_node, to_node):
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.sqrt(dx ** 2 + dy ** 2)
theta = math.atan2(dy, dx)
return d, theta
def main(gx=5.0, gy=10.0):
print("start " + __file__)
# ====Search Path with RRT====
obstacleList = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
] # [x,y,size]
# Set Initial parameters
rrt = RRT(start=[0, 0],
goal=[gx, gy],
rand_area=[-2, 15],
obstacle_list=obstacleList)
path = rrt.planning(animation=show_animation)
if path is None:
print("Cannot find path")
else:
print("found path!!")
# Draw final path
if show_animation:
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
if __name__ == '__main__':
main()

188
PathPlanning/RRT/rrt_with_pathsmoothing.py
View File

@@ -1,151 +1,29 @@
"""
Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
Path planning Sample Code with RRT with path smoothing
@author: AtsushiSakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import random
import math
import copy
import os
import random
import sys
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
try:
from rrt import RRT
except ImportError:
raise
show_animation = True
class RRT():
"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacleList, randArea, expandDis=1.0, goalSampleRate=5, maxIter=500):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.expandDis = expandDis
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
def Planning(self, animation=True):
"""
Pathplanning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
while True:
# Random Sampling
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
self.minrand, self.maxrand)]
else:
rnd = [self.end.x, self.end.y]
# Find nearest node
nind = self.GetNearestListIndex(self.nodeList, rnd)
# print(nind)
# expand tree
nearestNode = self.nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.parent = nind
if not self.__CollisionCheck(newNode, self.obstacleList):
continue
self.nodeList.append(newNode)
# check goal
dx = newNode.x - self.end.x
dy = newNode.y - self.end.y
d = math.sqrt(dx * dx + dy * dy)
if d <= self.expandDis:
print("Goal!!")
break
if animation:
self.DrawGraph(rnd)
path = [[self.end.x, self.end.y]]
lastIndex = len(self.nodeList) - 1
while self.nodeList[lastIndex].parent is not None:
node = self.nodeList[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def DrawGraph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
for (x, y, size) in self.obstacleList:
self.PlotCircle(x, y, size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def PlotCircle(self, x, y, size):
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
plt.plot(xl, yl, "-k")
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
** 2 for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def __CollisionCheck(self, node, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - node.x
dy = oy - node.y
d = math.sqrt(dx * dx + dy * dy)
if d <= size:
return False # collision
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
def GetPathLength(path):
def get_path_length(path):
le = 0
for i in range(len(path) - 1):
dx = path[i + 1][0] - path[i][0]
@@ -156,7 +34,7 @@ def GetPathLength(path):
return le
def GetTargetPoint(path, targetL):
def get_target_point(path, targetL):
le = 0
ti = 0
lastPairLen = 0
@@ -171,17 +49,14 @@ def GetTargetPoint(path, targetL):
break
partRatio = (le - targetL) / lastPairLen
# print(partRatio)
# print((ti,len(path),path[ti],path[ti+1]))
x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
# print((x,y))
return [x, y, ti]
def LineCollisionCheck(first, second, obstacleList):
def line_collision_check(first, second, obstacleList):
# Line Equation
x1 = first[0]
@@ -198,7 +73,7 @@ def LineCollisionCheck(first, second, obstacleList):
for (ox, oy, size) in obstacleList:
d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b))
if d <= (size):
if d <= size:
return False
# print("OK")
@@ -206,20 +81,15 @@ def LineCollisionCheck(first, second, obstacleList):
return True # OK
def PathSmoothing(path, maxIter, obstacleList):
# print("PathSmoothing")
def path_smoothing(path, max_iter, obstacle_list):
le = get_path_length(path)
le = GetPathLength(path)
for i in range(maxIter):
for i in range(max_iter):
# Sample two points
pickPoints = [random.uniform(0, le), random.uniform(0, le)]
pickPoints.sort()
# print(pickPoints)
first = GetTargetPoint(path, pickPoints[0])
# print(first)
second = GetTargetPoint(path, pickPoints[1])
# print(second)
first = get_target_point(path, pickPoints[0])
second = get_target_point(path, pickPoints[1])
if first[2] <= 0 or second[2] <= 0:
continue
@@ -231,7 +101,7 @@ def PathSmoothing(path, maxIter, obstacleList):
continue
# collision check
if not LineCollisionCheck(first, second, obstacleList):
if not line_collision_check(first, second, obstacle_list):
continue
# Create New path
@@ -241,7 +111,7 @@ def PathSmoothing(path, maxIter, obstacleList):
newPath.append([second[0], second[1]])
newPath.extend(path[second[2] + 1:])
path = newPath
le = GetPathLength(path)
le = get_path_length(path)
return path
@@ -258,16 +128,16 @@ def main():
(9, 5, 2)
] # [x,y,size]
rrt = RRT(start=[0, 0], goal=[5, 10],
randArea=[-2, 15], obstacleList=obstacleList)
path = rrt.Planning(animation=show_animation)
rand_area=[-2, 15], obstacle_list=obstacleList)
path = rrt.planning(animation=show_animation)
# Path smoothing
maxIter = 1000
smoothedPath = PathSmoothing(path, maxIter, obstacleList)
smoothedPath = path_smoothing(path, maxIter, obstacleList)
# Draw final path
if show_animation:
rrt.DrawGraph()
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.plot([x for (x, y) in smoothedPath], [

173
PathPlanning/RRT/simple_rrt.py
View File

@@ -1,173 +0,0 @@
"""
Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
author: AtsushiSakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import random
import math
import copy
show_animation = True
class RRT():
"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacleList,
randArea, expandDis=1.0, goalSampleRate=5, maxIter=500):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.expandDis = expandDis
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
def Planning(self, animation=True):
"""
Pathplanning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
while True:
# Random Sampling
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
self.minrand, self.maxrand)]
else:
rnd = [self.end.x, self.end.y]
# Find nearest node
nind = self.GetNearestListIndex(self.nodeList, rnd)
# print(nind)
# expand tree
nearestNode = self.nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.parent = nind
if not self.__CollisionCheck(newNode, self.obstacleList):
continue
self.nodeList.append(newNode)
print("nNodelist:", len(self.nodeList))
# check goal
dx = newNode.x - self.end.x
dy = newNode.y - self.end.y
d = math.sqrt(dx * dx + dy * dy)
if d <= self.expandDis:
print("Goal!!")
break
if animation:
self.DrawGraph(rnd)
path = [[self.end.x, self.end.y]]
lastIndex = len(self.nodeList) - 1
while self.nodeList[lastIndex].parent is not None:
node = self.nodeList[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def DrawGraph(self, rnd=None):
"""
Draw Graph
"""
plt.clf()
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
** 2 for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def __CollisionCheck(self, node, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - node.x
dy = oy - node.y
d = math.sqrt(dx * dx + dy * dy)
if d <= size:
return False # collision
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
def main():
print("start simple RRT path planning")
# ====Search Path with RRT====
obstacleList = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
] # [x,y,size]
# Set Initial parameters
rrt = RRT(start=[0, 0], goal=[5, 10],
randArea=[-2, 15], obstacleList=obstacleList)
path = rrt.Planning(animation=show_animation)
# Draw final path
if show_animation:
rrt.DrawGraph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.show()
if __name__ == '__main__':
main()

0
PathPlanning/RRTDubins/__init__.py Normal file
View File

BIN
PathPlanning/RRTDubins/animation.gif
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 13 MiB

308
PathPlanning/RRTDubins/dubins_path_planning.py
View File

@@ -1,308 +0,0 @@
#! /usr/bin/python
# -*- coding: utf-8 -*-
"""
Dubins path planner sample code
author Atsushi Sakai(@Atsushi_twi)
License MIT
"""
import math
import numpy as np
def mod2pi(theta):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def LSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d + sa - sb
mode = ["L", "S", "L"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((cb - ca), tmp0)
t = mod2pi(-alpha + tmp1)
p = math.sqrt(p_squared)
q = mod2pi(beta - tmp1)
# print(np.rad2deg(t), p, np.rad2deg(q))
return t, p, q, mode
def RSR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d - sa + sb
mode = ["R", "S", "R"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((ca - cb), tmp0)
t = mod2pi(alpha - tmp1)
p = math.sqrt(p_squared)
q = mod2pi(-beta + tmp1)
return t, p, q, mode
def LSR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
mode = ["L", "S", "R"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
t = mod2pi(-alpha + tmp2)
q = mod2pi(-mod2pi(beta) + tmp2)
return t, p, q, mode
def RSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
mode = ["R", "S", "L"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
t = mod2pi(alpha - tmp2)
q = mod2pi(beta - tmp2)
return t, p, q, mode
def RLR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["R", "L", "R"]
tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
if abs(tmp_rlr) > 1.0:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
q = mod2pi(alpha - beta - t + mod2pi(p))
return t, p, q, mode
def LRL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["L", "R", "L"]
tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
if abs(tmp_lrl) > 1:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.)
q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# nomalize
dx = ex
dy = ey
D = math.sqrt(dx ** 2.0 + dy ** 2.0)
d = D / c
# print(dx, dy, D, d)
theta = mod2pi(math.atan2(dy, dx))
alpha = mod2pi(- theta)
beta = mod2pi(eyaw - theta)
# print(theta, alpha, beta, d)
planners = [LSL, RSR, LSR, RSL, RLR, LRL]
bcost = float("inf")
bt, bp, bq, bmode = None, None, None, None
for planner in planners:
t, p, q, mode = planner(alpha, beta, d)
if t is None:
# print("".join(mode) + " cannot generate path")
continue
cost = (abs(t) + abs(p) + abs(q))
if bcost > cost:
bt, bp, bq, bmode = t, p, q, mode
bcost = cost
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
"""
Dubins path plannner
input:
sx x position of start point [m]
sy y position of start point [m]
syaw yaw angle of start point [rad]
ex x position of end point [m]
ey y position of end point [m]
eyaw yaw angle of end point [rad]
c curvature [1/m]
output:
px
py
pyaw
mode
"""
ex = ex - sx
ey = ey - sy
lex = math.cos(syaw) * ex + math.sin(syaw) * ey
ley = - math.sin(syaw) * ex + math.cos(syaw) * ey
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c)
px = [math.cos(-syaw) * x + math.sin(-syaw) *
y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) *
y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
# print(syaw)
# pyaw = lpyaw
# plt.plot(pyaw, "-r")
# plt.plot(lpyaw, "-b")
# plt.plot(eyaw, "*r")
# plt.plot(syaw, "*b")
# plt.show()
return px, py, pyaw, mode, clen
def generate_course(length, mode, c):
px = [0.0]
py = [0.0]
pyaw = [0.0]
for m, l in zip(mode, length):
pd = 0.0
if m is "S":
d = 1.0 / c
else: # turning couse
d = np.deg2rad(3.0)
while pd < abs(l - d):
# print(pd, l)
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
if m is "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
if m is "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
Plot arrow
"""
import matplotlib.pyplot as plt
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
if __name__ == '__main__':
print("Dubins path planner sample start!!")
import matplotlib.pyplot as plt
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
plt.plot(px, py, label="final course " + "".join(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
# for (ix, iy, iyaw) in zip(px, py, pyaw):
# plot_arrow(ix, iy, iyaw, fc="b")
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()

Some files were not shown because too many files have changed in this diff Show More