Skip to content

Commit d6fb8d6

Browse files
AtsushiSakaiAtsushiSakai
committed
remove error on codefactor
1 parent 0694eb2 commit d6fb8d6

File tree

7 files changed

+153
-432
lines changed

7 files changed

+153
-432
lines changed

‎ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py‎

Lines changed: 46 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,8 @@ def detect_collision(line_seg, circle):
6060
closest_point = a_vec + line_vec * proj / line_mag
6161
if np.linalg.norm(closest_point - c_vec) > radius:
6262
return False
63-
else:
64-
return True
63+
64+
return True
6565

6666

6767
def get_occupancy_grid(arm, obstacles):
@@ -74,7 +74,7 @@ def get_occupancy_grid(arm, obstacles):
7474
Args:
7575
arm: An instance of NLinkArm
7676
obstacles: A list of obstacles, with each obstacle defined as a list
77-
of xy coordinates and a radius.
77+
of xy coordinates and a radius.
7878
7979
Returns:
8080
Occupancy grid in joint space
@@ -120,16 +120,7 @@ def astar_torus(grid, start_node, goal_node):
120120

121121
parent_map = [[() for _ in range(M)] for _ in range(M)]
122122

123-
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
124-
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
125-
for i in range(heuristic_map.shape[0]):
126-
for j in range(heuristic_map.shape[1]):
127-
heuristic_map[i, j] = min(heuristic_map[i, j],
128-
i + 1 + heuristic_map[M - 1, j],
129-
M - i + heuristic_map[0, j],
130-
j + 1 + heuristic_map[i, M - 1],
131-
M - j + heuristic_map[i, 0]
132-
)
123+
heuristic_map = calc_heuristic_map(M, goal_node)
133124

134125
explored_heuristic_map = np.full((M, M), np.inf)
135126
distance_map = np.full((M, M), np.inf)
@@ -150,46 +141,21 @@ def astar_torus(grid, start_node, goal_node):
150141

151142
i, j = current_node[0], current_node[1]
152143

153-
neighbors = []
154-
if i - 1 >= 0:
155-
neighbors.append((i - 1, j))
156-
else:
157-
neighbors.append((M - 1, j))
158-
159-
if i + 1 < M:
160-
neighbors.append((i + 1, j))
161-
else:
162-
neighbors.append((0, j))
163-
164-
if j - 1 >= 0:
165-
neighbors.append((i, j - 1))
166-
else:
167-
neighbors.append((i, M - 1))
168-
169-
if j + 1 < M:
170-
neighbors.append((i, j + 1))
171-
else:
172-
neighbors.append((i, 0))
144+
neighbors = find_neighbors(i, j)
173145

174146
for neighbor in neighbors:
175147
if grid[neighbor] == 0 or grid[neighbor] == 5:
176148
distance_map[neighbor] = distance_map[current_node] + 1
177149
explored_heuristic_map[neighbor] = heuristic_map[neighbor]
178150
parent_map[neighbor[0]][neighbor[1]] = current_node
179151
grid[neighbor] = 3
180-
'''
181-
plt.cla()
182-
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
183-
plt.show()
184-
plt.pause(1e-5)
185-
'''
186152

187153
if np.isinf(explored_heuristic_map[goal_node]):
188154
route = []
189155
print("No route found.")
190156
else:
191157
route = [goal_node]
192-
while parent_map[route[0][0]][route[0][1]] is not ():
158+
while parent_map[route[0][0]][route[0][1]] != ():
193159
route.insert(0, parent_map[route[0][0]][route[0][1]])
194160

195161
print("The route found covers %d grid cells." % len(route))
@@ -203,6 +169,46 @@ def astar_torus(grid, start_node, goal_node):
203169
return route
204170

205171

172+
def find_neighbors(i, j):
173+
neighbors = []
174+
if i - 1 >= 0:
175+
neighbors.append((i - 1, j))
176+
else:
177+
neighbors.append((M - 1, j))
178+
179+
if i + 1 < M:
180+
neighbors.append((i + 1, j))
181+
else:
182+
neighbors.append((0, j))
183+
184+
if j - 1 >= 0:
185+
neighbors.append((i, j - 1))
186+
else:
187+
neighbors.append((i, M - 1))
188+
189+
if j + 1 < M:
190+
neighbors.append((i, j + 1))
191+
else:
192+
neighbors.append((i, 0))
193+
194+
return neighbors
195+
196+
197+
def calc_heuristic_map(M, goal_node):
198+
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
199+
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
200+
for i in range(heuristic_map.shape[0]):
201+
for j in range(heuristic_map.shape[1]):
202+
heuristic_map[i, j] = min(heuristic_map[i, j],
203+
i + 1 + heuristic_map[M - 1, j],
204+
M - i + heuristic_map[0, j],
205+
j + 1 + heuristic_map[i, M - 1],
206+
M - j + heuristic_map[i, 0]
207+
)
208+
209+
return heuristic_map
210+
211+
206212
class NLinkArm(object):
207213
"""
208214
Class for controlling and plotting a planar arm with an arbitrary number of links.

‎ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py‎

Lines changed: 55 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -92,8 +92,7 @@ def detect_collision(line_seg, circle):
9292
closest_point = a_vec + line_vec * proj / line_mag
9393
if np.linalg.norm(closest_point - c_vec) > radius:
9494
return False
95-
else:
96-
return True
95+
return True
9796

9897

9998
def get_occupancy_grid(arm, obstacles):
@@ -143,21 +142,16 @@ def astar_torus(grid, start_node, goal_node):
143142
Returns:
144143
Obstacle-free route in joint space from start_node to goal_node
145144
"""
145+
colors = ['white', 'black', 'red', 'pink', 'yellow', 'green', 'orange']
146+
levels = [0, 1, 2, 3, 4, 5, 6, 7]
147+
cmap, norm = from_levels_and_colors(levels, colors)
148+
146149
grid[start_node] = 4
147150
grid[goal_node] = 5
148151

149152
parent_map = [[() for _ in range(M)] for _ in range(M)]
150153

151-
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
152-
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
153-
for i in range(heuristic_map.shape[0]):
154-
for j in range(heuristic_map.shape[1]):
155-
heuristic_map[i, j] = min(heuristic_map[i, j],
156-
i + 1 + heuristic_map[M - 1, j],
157-
M - i + heuristic_map[0, j],
158-
j + 1 + heuristic_map[i, M - 1],
159-
M - j + heuristic_map[i, 0]
160-
)
154+
heuristic_map = calc_heuristic_map(M, goal_node)
161155

162156
explored_heuristic_map = np.full((M, M), np.inf)
163157
distance_map = np.full((M, M), np.inf)
@@ -178,52 +172,74 @@ def astar_torus(grid, start_node, goal_node):
178172

179173
i, j = current_node[0], current_node[1]
180174

181-
neighbors = []
182-
if i - 1 >= 0:
183-
neighbors.append((i - 1, j))
184-
else:
185-
neighbors.append((M - 1, j))
186-
187-
if i + 1 < M:
188-
neighbors.append((i + 1, j))
189-
else:
190-
neighbors.append((0, j))
191-
192-
if j - 1 >= 0:
193-
neighbors.append((i, j - 1))
194-
else:
195-
neighbors.append((i, M - 1))
196-
197-
if j + 1 < M:
198-
neighbors.append((i, j + 1))
199-
else:
200-
neighbors.append((i, 0))
175+
neighbors = find_neighbors(i, j)
201176

202177
for neighbor in neighbors:
203178
if grid[neighbor] == 0 or grid[neighbor] == 5:
204179
distance_map[neighbor] = distance_map[current_node] + 1
205180
explored_heuristic_map[neighbor] = heuristic_map[neighbor]
206181
parent_map[neighbor[0]][neighbor[1]] = current_node
207182
grid[neighbor] = 3
208-
'''
209-
plt.cla()
210-
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
211-
plt.show()
212-
plt.pause(1e-5)
213-
'''
214183

215184
if np.isinf(explored_heuristic_map[goal_node]):
216185
route = []
217186
print("No route found.")
218187
else:
219188
route = [goal_node]
220-
while parent_map[route[0][0]][route[0][1]] is not ():
189+
while parent_map[route[0][0]][route[0][1]] != ():
221190
route.insert(0, parent_map[route[0][0]][route[0][1]])
222191

223192
print("The route found covers %d grid cells." % len(route))
193+
for i in range(1, len(route)):
194+
grid[route[i]] = 6
195+
plt.cla()
196+
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
197+
plt.show()
198+
plt.pause(1e-2)
199+
224200
return route
225201

226202

203+
def find_neighbors(i, j):
204+
neighbors = []
205+
if i - 1 >= 0:
206+
neighbors.append((i - 1, j))
207+
else:
208+
neighbors.append((M - 1, j))
209+
210+
if i + 1 < M:
211+
neighbors.append((i + 1, j))
212+
else:
213+
neighbors.append((0, j))
214+
215+
if j - 1 >= 0:
216+
neighbors.append((i, j - 1))
217+
else:
218+
neighbors.append((i, M - 1))
219+
220+
if j + 1 < M:
221+
neighbors.append((i, j + 1))
222+
else:
223+
neighbors.append((i, 0))
224+
225+
return neighbors
226+
227+
228+
def calc_heuristic_map(M, goal_node):
229+
X, Y = np.meshgrid([i for i in range(M)], [i for i in range(M)])
230+
heuristic_map = np.abs(X - goal_node[1]) + np.abs(Y - goal_node[0])
231+
for i in range(heuristic_map.shape[0]):
232+
for j in range(heuristic_map.shape[1]):
233+
heuristic_map[i, j] = min(heuristic_map[i, j],
234+
i + 1 + heuristic_map[M - 1, j],
235+
M - i + heuristic_map[0, j],
236+
j + 1 + heuristic_map[i, M - 1],
237+
M - j + heuristic_map[i, 0]
238+
)
239+
240+
return heuristic_map
241+
242+
227243
class NLinkArm(object):
228244
"""
229245
Class for controlling and plotting a planar arm with an arbitrary number of links.

‎PathPlanning/DubinsPath/__init__.py‎

Whitespace-only changes.

‎PathPlanning/Eta3SplinePath/eta3_spline_path.py‎

Lines changed: 32 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -36,16 +36,17 @@ def __init__(self, segments):
3636
for r, s in zip(segments[:-1], segments[1:]):
3737
assert(np.array_equal(r.end_pose, s.start_pose))
3838
self.segments = segments
39-
"""
40-
eta3_path::calc_path_point
41-
42-
input
43-
normalized interpolation point along path object, 0 <= u <= len(self.segments)
44-
returns
45-
2d (x,y) position vector
46-
"""
4739

4840
def calc_path_point(self, u):
41+
"""
42+
eta3_path::calc_path_point
43+
44+
input
45+
normalized interpolation point along path object, 0 <= u <= len(self.segments)
46+
returns
47+
2d (x,y) position vector
48+
"""
49+
4950
assert(u >= 0 and u <= len(self.segments))
5051
if np.isclose(u, len(self.segments)):
5152
segment_idx = len(self.segments) - 1
@@ -152,39 +153,41 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
152153
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
153154
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
154155
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
155-
156-
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)
156+
157+
self.s_dot = lambda u: max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array(
158+
[1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
157159
self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
158160
self.segment_length = self.f_length(1)[0]
159161

160-
"""
161-
eta3_path_segment::calc_point
162-
163-
input
164-
u - parametric representation of a point along the segment, 0 <= u <= 1
165-
returns
166-
(x,y) of point along the segment
167-
"""
168-
169162
def calc_point(self, u):
163+
"""
164+
eta3_path_segment::calc_point
165+
166+
input
167+
u - parametric representation of a point along the segment, 0 <= u <= 1
168+
returns
169+
(x,y) of point along the segment
170+
"""
170171
assert(u >= 0 and u <= 1)
171172
return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
172173

173-
"""
174-
eta3_path_segment::calc_deriv
175-
176-
input
177-
u - parametric representation of a point along the segment, 0 <= u <= 1
178-
returns
179-
(d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
180-
"""
181174
def calc_deriv(self, u, order=1):
175+
"""
176+
eta3_path_segment::calc_deriv
177+
178+
input
179+
u - parametric representation of a point along the segment, 0 <= u <= 1
180+
returns
181+
(d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
182+
"""
183+
182184
assert(u >= 0 and u <= 1)
183185
assert(order > 0 and order <= 2)
184186
if order == 1:
185-
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]))
187+
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]))
186188
else:
187-
return self.coeffs[:, 2:].dot(np.array([2, 6.*u, 12.*u**2, 20.*u**3, 30.*u**4, 42.*u**5]))
189+
return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
190+
188191

189192
def test1():
190193

0 commit comments

Comments
 (0)