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

Added modified version of the arm obstacle avoidance simulation.

Browse Source 
Most of the code comes from the great original simulation.
In this version, both the work space and joint space visualization are
done in the same figure, which provides a better understanding of the
simulation.

Obstacles have been made more tricky in the joint space.

Added facility to stop the simulatino by pressing 'q'.

Added some comments.
This commit is contained in:
Tullio Facchinetti
2019-01-07 14:48:08 +01:00
parent 4af9c8e9a7
commit a558096a83
1 changed files with 277 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

277
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py Normal file
View File

@@ -0,0 +1,277 @@
"""
Obstacle navigation using A* on a toroidal grid
Author: Daniel Ingram (daniel-s-ingram)
Tullio Facchinetti (tullio.facchinetti@unipv.it)
"""
from math import pi
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import from_levels_and_colors
import sys
plt.ion()
# Simulation parameters
M = 100
obstacles = [[1.75, 0.75, 0.6], [0.55, 1.5, 0.5], [0, -1, 0.7]]
def press(event):
"""Exit from the simulation."""
if event.key == 'q' or event.key == 'Q':
print('Quitting upon request.')
sys.exit(0)
def main():
# Arm geometry in the working space
link_length = [0.5, 1.5]
initial_link_angle = [0, 0]
arm = NLinkArm(link_length, initial_link_angle)
# (x, y) co-ordinates in the joint space [cell]
start = (10, 50)
goal = (58, 56)
grid = get_occupancy_grid(arm, obstacles)
route = astar_torus(grid, start, goal)
if len(route) >= 0:
animate(grid, arm, route)
def animate(grid, arm, route):
fig, axs = plt.subplots(1, 2)
fig.canvas.mpl_connect('key_press_event', press)
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)
for i, node in enumerate(route):
plt.subplot(1, 2, 1)
grid[node] = 6
plt.cla()
plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None)
theta1 = 2 * pi * node[0] / M - pi
theta2 = 2 * pi * node[1] / M - pi
arm.update_joints([theta1, theta2])
plt.subplot(1, 2, 2)
arm.plot(plt, obstacles=obstacles)
plt.show()
# Uncomment here to save the sequence of frames
#plt.savefig('frame{:04d}.png'.format(i))
plt.pause(0.1)
def detect_collision(line_seg, circle):
"""
Determines whether a line segment (arm link) is in contact
with a circle (obstacle).
Credit to: http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
Args:
line_seg: List of coordinates of line segment endpoints e.g. [[1, 1], [2, 2]]
circle: List of circle coordinates and radius e.g. [0, 0, 0.5] is a circle centered
at the origin with radius 0.5
Returns:
True if the line segment is in contact with the circle
False otherwise
"""
a_vec = np.array([line_seg[0][0], line_seg[0][1]])
b_vec = np.array([line_seg[1][0], line_seg[1][1]])
c_vec = np.array([circle[0], circle[1]])
radius = circle[2]
line_vec = b_vec - a_vec
line_mag = np.linalg.norm(line_vec)
circle_vec = c_vec - a_vec
proj = circle_vec.dot(line_vec / line_mag)
if proj <= 0:
closest_point = a_vec
elif proj >= line_mag:
closest_point = b_vec
else:
closest_point = a_vec + line_vec * proj / line_mag
if np.linalg.norm(closest_point - c_vec) > radius:
return False
else:
return True
def get_occupancy_grid(arm, obstacles):
"""
Discretizes joint space into M values from -pi to +pi
and determines whether a given coordinate in joint space
would result in a collision between a robot arm and obstacles
in its environment.
Args:
arm: An instance of NLinkArm
obstacles: A list of obstacles, with each obstacle defined as a list
of xy coordinates and a radius.
Returns:
Occupancy grid in joint space
"""
grid = [[0 for _ in range(M)] for _ in range(M)]
theta_list = [2 * i * pi / M for i in range(-M // 2, M // 2 + 1)]
for i in range(M):
for j in range(M):
arm.update_joints([theta_list[i], theta_list[j]])
points = arm.points
collision_detected = False
for k in range(len(points) - 1):
for obstacle in obstacles:
line_seg = [points[k], points[k + 1]]
collision_detected = detect_collision(line_seg, obstacle)
if collision_detected:
break
if collision_detected:
break
grid[i][j] = int(collision_detected)
return np.array(grid)
def astar_torus(grid, start_node, goal_node):
"""
Finds a path between an initial and goal joint configuration using
the A* Algorithm on a tororiadal grid.
Args:
grid: An occupancy grid (ndarray)
start_node: Initial joint configuation (tuple)
goal_node: Goal joint configuration (tuple)
Returns:
Obstacle-free route in joint space from start_node to goal_node
"""
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]
)
explored_heuristic_map = np.full((M, M), np.inf)
distance_map = np.full((M, M), np.inf)
explored_heuristic_map[start_node] = heuristic_map[start_node]
distance_map[start_node] = 0
while True:
grid[start_node] = 4
grid[goal_node] = 5
current_node = np.unravel_index(
np.argmin(explored_heuristic_map, axis=None), explored_heuristic_map.shape)
min_distance = np.min(explored_heuristic_map)
if (current_node == goal_node) or np.isinf(min_distance):
break
grid[current_node] = 2
explored_heuristic_map[current_node] = np.inf
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))
for neighbor in neighbors:
if grid[neighbor] == 0 or grid[neighbor] == 5:
distance_map[neighbor] = distance_map[current_node] + 1
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 ():
route.insert(0, parent_map[route[0][0]][route[0][1]])
print("The route found covers %d grid cells." % len(route))
return route
class NLinkArm(object):
"""
Class for controlling and plotting a planar arm with an arbitrary number of links.
"""
def __init__(self, link_lengths, joint_angles):
self.n_links = len(link_lengths)
if self.n_links != len(joint_angles):
raise ValueError()
self.link_lengths = np.array(link_lengths)
self.joint_angles = np.array(joint_angles)
self.points = [[0, 0] for _ in range(self.n_links + 1)]
self.lim = sum(link_lengths)
self.update_points()
def update_joints(self, joint_angles):
self.joint_angles = joint_angles
self.update_points()
def update_points(self):
for i in range(1, self.n_links + 1):
self.points[i][0] = self.points[i - 1][0] + \
self.link_lengths[i - 1] * \
np.cos(np.sum(self.joint_angles[:i]))
self.points[i][1] = self.points[i - 1][1] + \
self.link_lengths[i - 1] * \
np.sin(np.sum(self.joint_angles[:i]))
self.end_effector = np.array(self.points[self.n_links]).T
def plot(self, myplt, obstacles=[]):
myplt.cla()
for obstacle in obstacles:
circle = myplt.Circle(
(obstacle[0], obstacle[1]), radius=0.5 * obstacle[2], fc='k')
myplt.gca().add_patch(circle)
for i in range(self.n_links + 1):
if i is not self.n_links:
myplt.plot([self.points[i][0], self.points[i + 1][0]],
[self.points[i][1], self.points[i + 1][1]], 'r-')
myplt.plot(self.points[i][0], self.points[i][1], 'k.')
myplt.xlim([-self.lim, self.lim])
myplt.ylim([-self.lim, self.lim])
myplt.draw()
#myplt.pause(1e-5)
if __name__ == '__main__':
main()