def __init__(self, init_config, step_dist, part_num):
self.init = init_config
self.tree = Graph(init_config)
self.epsilon = step_dist
self.section = part_num
self.world = Obstacles(part_num)
self.path = None
self.cntrl = [np.zeros(2)]
class RRT(object):
def __init__(self, init_config, step_dist, part_num):
self.init = init_config
self.tree = Graph(init_config)
self.epsilon = step_dist
self.section = part_num
self.world = Obstacles(part_num)
self.path = None
self.cntrl = [np.zeros(2)]
def _distance(self, c1, c2):
if self.section == 1:
return np.linalg.norm(c1-c2)
else: # ARM configuration space
c1 = np.asarray(c1)
c2 = np.asarray(c2)
dist3 = np.minimum(np.absolute(c1-c2),2*np.pi-np.absolute(c1-c2))
w = np.array([3,2,1])
return np.dot(w,dist3)
def _generate_random_node(self):
if self.section == 1:
return np.random.rand(2)*self.world.room_dimensions + self.world.room_offset
else:
jointRangeDeg = np.array([155,297,205])
jointRangeOffsetDeg = np.array([12.5,-2.5,0.0])-jointRangeDeg/2
return np.random.rand(3)*(jointRangeDeg*(np.pi/180))-(jointRangeOffsetDeg*(np.pi/180))
def _nearest_neighbor(self, rand):
nn = self.tree.nodes[0]
nn_arg = 0
for i,n in enumerate(self.tree.nodes):
if self._distance(n, rand) < self._distance(nn, rand):
nn = n
nn_arg = i
return nn, nn_arg
def _step_from_to(self, c1, c2):
if self.section == 1:
if self._distance(c1, c2) < self.epsilon:
return c2, (c2-c1)/np.linalg.norm(c2-c1)
else:
return c1 + self.epsilon*(c2-c1)/np.linalg.norm(c2-c1), self.epsilon*(c2-c1)/np.linalg.norm(c2-c1)
else: # ^^^ Check c2-c1
if self._distance(c1,c2) < self.epsilon:
return c2, torusVect(c1,c2)/distance(c1,c2)
else:
return c1 + self.epsilon*torusVect(c1,c2)/distance(c1,c2), self.epsilon*torusVect(c1,c2)/distance(c1,c2)
def torusVect(c1,c2):
c1 = np.array(c1)
c2 = np.array(c2)
return np.minimum(np.absolute(c1-c2),2*np.pi-np.absolute(c1-c2))
def _find_shortest_path(self):
k = len(self.tree.adjacency)-1
short_path = [k]
print "k: ", short_path
while short_path[-1] is not -1:
short_path.append(self.tree.adjacency[k])
k = self.tree.adjacency[k]
short_path.reverse()
short_path.pop(0)
nodes_array = np.asarray(self.tree.nodes)
self.path = nodes_array[short_path]
cntrl_array = np.asarray(self.cntrl)
self.cntrl = cntrl_array[short_path]
def generate_tree(self, goal, clearance):
#constants
Xi = 0.0
Yi = 0.0
Xg = 5.0
Yg = 5.0
XDIM = 8.0
YDIM = 8.0
RESOLUTION = 0.01
XDIMPIX = int (XDIM / RESOLUTION)
YDIMPIX = int (YDIM / RESOLUTION)
WINSIZE = [XDIMPIX, YDIMPIX]
EPSILON = 0.1 / RESOLUTION
pygame.init()
screen = pygame.display.set_mode(WINSIZE)
pygame.display.set_caption('RRT Jones/Lakshmanan ECE550 F16')
white = 255, 240, 200
black = 20, 20, 40
green = 10, 250, 10
red = 250, 10, 10
blue = 10, 10, 250
screen.fill(black)
new_node = self.init
while self._distance(new_node, goal) > clearance:
rand = self._generate_random_node()
if not self.world.is_colliding([rand[0], rand[1], 0]):
nn, nn_arg = self._nearest_neighbor(rand)
new_node, u = self._step_from_to(nn, rand)
pygame.draw.line(screen,white,list((nn-self.world.room_offset)*100.0),list((new_node-self.world.room_offset)*100.0))
pygame.draw.circle(screen, green, map(int,list((np.array([0,0])-self.world.room_offset)*100)),5, 5)
pygame.draw.circle(screen, red, map(int,list((np.array([5,5])-self.world.room_offset)*100)),5, 5)
pygame.draw.line(screen,blue,list((rand-self.world.room_offset)*100.0),list((rand-self.world.room_offset)*100.0))
pygame.display.update()
if not self.world.is_colliding([new_node[0], new_node[1], 0]):
self.tree.add_node(new_node, nn_arg)
self.cntrl.append(u)
self._find_shortest_path()
self.path = np.append(self.path, [goal], axis = 0)
