mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
remove error on codefactor
This commit is contained in:
Atsushi Sakai
@@ -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.
|
||||
|
||||
@@ -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.
|
||||
|
||||
0
PathPlanning/DubinsPath/__init__.py
Normal file
0
PathPlanning/DubinsPath/__init__.py
Normal 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]))
|
||||
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[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
|
||||
|
||||
|
||||
def test1():
|
||||
|
||||
|
||||
@@ -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()
|
||||
@@ -4,13 +4,18 @@ Path Planning Sample Code with RRT for car like robot.
|
||||
author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import random
|
||||
import math
|
||||
import copy
|
||||
import numpy as np
|
||||
import dubins_path_planning
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import copy
|
||||
import math
|
||||
import random
|
||||
import sys
|
||||
sys.path.append("../DubinsPath/")
|
||||
try:
|
||||
import dubins_path_planning
|
||||
except:
|
||||
raise
|
||||
|
||||
|
||||
show_animation = True
|
||||
|
||||
@@ -183,9 +188,9 @@ class RRT():
|
||||
plt.pause(0.01)
|
||||
|
||||
def GetNearestListIndex(self, nodeList, rnd):
|
||||
dlist = [(node.x - rnd[0]) ** 2 +
|
||||
(node.y - rnd[1]) ** 2 +
|
||||
(node.yaw - rnd[2] ** 2) for node in nodeList]
|
||||
dlist = [(node.x - rnd[0]) ** 2
|
||||
+ (node.y - rnd[1]) ** 2
|
||||
+ (node.yaw - rnd[2] ** 2) for node in nodeList]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
return minind
|
||||
|
||||
@@ -1,3 +1,7 @@
|
||||
import subprocess
|
||||
import os.path
|
||||
import os
|
||||
import glob
|
||||
"""
|
||||
|
||||
Jupyter notebook converter to rst file
|
||||
@@ -8,11 +12,6 @@ author: Atsushi Sakai
|
||||
|
||||
NOTEBOOK_DIR = "../"
|
||||
|
||||
import glob
|
||||
import os
|
||||
import os.path
|
||||
import subprocess
|
||||
|
||||
|
||||
def get_notebook_path_list(ndir):
|
||||
path = glob.glob(ndir + "**/*.ipynb", recursive=True)
|
||||
@@ -29,8 +28,8 @@ def convert_rst(rstpath):
|
||||
after = ".. code-block:: ipython3"
|
||||
filedata = filedata.replace(before, after)
|
||||
|
||||
with open(rstpath, "w") as file:
|
||||
file.write(filedata)
|
||||
with open(rstpath, "w") as ffile:
|
||||
ffile.write(filedata)
|
||||
|
||||
|
||||
def generate_rst(npath):
|
||||
|
||||
Reference in New Issue
Block a user