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

fix: arm_obstacle_navigation calc_heuristic_map node through 4 corners error (#1034)

Browse Source 
* fix: arm_obstacle_navigation calc_heuristic_map node through 4 corners error

* style: remove trailing whitespace
This commit is contained in:
Weipu Shan
2024-12-20 20:32:57 +08:00
committed by GitHub
parent e5eea35ee4
commit 4bc7048044
2 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
View File

@@ -203,10 +203,10 @@ def calc_heuristic_map(M, goal_node):
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]
M - i - 1 + heuristic_map[M - 1, j],
i + heuristic_map[0, j],
M - j - 1 + heuristic_map[i, M - 1],
j + heuristic_map[i, 0]
)
return heuristic_map

10
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
View File

@@ -105,7 +105,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
@@ -234,10 +234,10 @@ def calc_heuristic_map(M, goal_node):
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]
M - i - 1 + heuristic_map[M - 1, j],
i + heuristic_map[0, j],
M - j - 1 + heuristic_map[i, M - 1],
j + heuristic_map[i, 0]
)
return heuristic_map