1 #!/usr/bin/python
   2 #!encoding: utf-8
   3 #author: Estefanía Pereyra
   4 import random
   5 import math
   6 import pylab
   7 
   8 def main(qStart, cGoalX, cGoalY, cLimits, cObsX, cObsY, kLimit):
   9     RRTV = []
  10     RRTE = []
  11     RRTV.append(qStart)
  12     RRTE.append(qStart)
  13     k = 0
  14     qNew = qStart
  15     while (((qNew[0][0] < cGoalX[0] or qNew[1][0] < cGoalX[0] or qNew[2][0] < cGoalX[0] or qNew[3][0]< cGoalX[0])
  16          or (qNew[0][0] > cGoalX[1] or qNew[1][0] > cGoalX[1] or qNew[2][0] > cGoalX[1] or qNew[3][0] > cGoalX[1])
  17          or (qNew[0][1] < cGoalY[0] or qNew[1][1] < cGoalY[0] or qNew[2][1] < cGoalY[0] or qNew[3][1] < cGoalY[0])
  18          or (qNew[0][1] > cGoalY[1] or qNew[1][1] > cGoalY[1] or qNew[2][1] > cGoalY[1] or qNew[3][1]> cGoalY[1]))
  19          and k < kLimit):
  20         
  21         if((qNew[0][0] > cGoalX[0] and qNew[0][0] < cGoalX[1]) and (qNew[0][1]  > cGoalY[0] and qNew[0][1] < cGoalY[1])):
  22             qRand = qRandom([[qNew[0][0]-1, qNew[0][0]+1],[qNew[0][1]-1, qNew[0][1]+1]], cObsX, cObsY)
  23        
  24         elif((qNew[1][0] > cGoalX[0] and qNew[1][0] < cGoalX[1]) and (qNew[1][1] > cGoalY[0] and qNew[1][1] < cGoalY[1])):
  25             qRand = qRandom([[qNew[1][0]-1, qNew[1][0]+1],[qNew[1][1]-1, qNew[1][1]+1]], cObsX, cObsY)
  26        
  27         elif((qNew[2][0] > cGoalX[0] and qNew[2][0] < cGoalX[1]) and (qNew[2][1] > cGoalY[0] and qNew[2][1] < cGoalY[1])):
  28             qRand = qRandom([[qNew[2][0]-1, qNew[2][0]+1],[qNew[2][1]-1, qNew[2][1]+1]], cObsX, cObsY)
  29        
  30         elif((qNew[3][0] > cGoalX[0] and qNew[3][0] < cGoalX[1]) and (qNew[3][1] > cGoalY[0] and qNew[3][1] < cGoalY[1])):
  31             qRand = qRandom([[qNew[3][0]-1, qNew[3][0]+1],[qNew[3][1]-1, qNew[3][1]+1]], cObsX, cObsY)
  32         else:
  33             qRand = qRandom(cLimits, cObsX, cObsY)
  34         
  35         extendRRT(RRTV, RRTE, qRand, cLimits, cObsX, cObsY)
  36         qNew = RRTV[-1] 
  37         qPrev = RRTE[-1]
  38         k += 1
  39 
  40     if k < kLimit:
  41         path = findPath(RRTV,RRTE)
  42         print 'k:', k
  43         plot(path, cLimits, cObsX, cObsY, cGoalX, cGoalY)
  44         return path
  45 #        return True
  46     else:
  47         return False
  48 
  49 def plot(path, cLimits, cObsX, cObsY, cGoalX, cGoalY):
  50     
  51     pylab.xlim([cLimits[0][0], cLimits[0][1]])
  52     pylab.ylim([cLimits[1][0], cLimits[1][1]])
  53      
  54     xg = [[cGoalX[0], cGoalX[1]], [cGoalX[1], cGoalX[1]], 
  55           [cGoalX[1], cGoalX[0]], [cGoalX[0], cGoalX[0]]]
  56     yg = [[cGoalY[0], cGoalY[0]], [cGoalY[0], cGoalY[1]], 
  57           [cGoalY[1], cGoalY[1]], [cGoalY[1], cGoalY[0]]]
  58     pylab.plot(xg, yg,'g')
  59 
  60     xo1 = [[cObsX[0][0], cObsX[0][1]], [cObsX[0][1], cObsX[0][1]], 
  61            [cObsX[0][1], cObsX[0][0]], [cObsX[0][0], cObsX[0][0]]]
  62     yo1 = [[cObsY[0][0], cObsY[0][0]], [cObsY[0][0], cObsY[0][1]], 
  63            [cObsY[0][1], cObsY[0][1]], [cObsY[0][1], cObsY[0][0]]]
  64     xo2 = [[cObsX[1][0], cObsX[1][1]], [cObsX[1][1], cObsX[1][1]], 
  65            [cObsX[1][1], cObsX[1][0]], [cObsX[1][0], cObsX[1][0]]]
  66     yo2 = [[cObsY[1][0], cObsY[1][0]], [cObsY[1][0], cObsY[1][1]], 
  67            [cObsY[1][1], cObsY[1][1]], [cObsY[1][1], cObsY[1][0]]]
  68 
  69     pylab.plot(xo1, yo1,'r')
  70     pylab.plot(xo2, yo2,'r')
  71       
  72     for i in xrange(len(path)):
  73         x1 = [path[i][0][0]]
  74         x2 = [path[i][1][0]]
  75         x3 = [path[i][2][0]]
  76         x4 = [path[i][3][0]]
  77         y1 = [path[i][0][1]]
  78         y2 = [path[i][1][1]]
  79         y3 = [path[i][2][1]]
  80         y4 = [path[i][3][1]]
  81         pylab.plot(x1,y1, 'bo')
  82         pylab.plot(x2,y2, 'ro')
  83         pylab.plot(x3,y3, 'go')
  84         pylab.plot(x4,y4, 'yo')
  85 
  86         if i != 0:
  87             xp1 = [path[i-1][0][0], path[i][0][0]]
  88             xp2 = [path[i-1][1][0], path[i][1][0]]
  89             xp3 = [path[i-1][2][0], path[i][2][0]]
  90             xp4 = [path[i-1][3][0], path[i][3][0]]
  91             yp1 = [path[i-1][0][1], path[i][0][1]]
  92             yp2 = [path[i-1][1][1], path[i][1][1]]
  93             yp3 = [path[i-1][2][1], path[i][2][1]]
  94             yp4 = [path[i-1][3][1], path[i][3][1]]
  95             pylab.plot(xp1,yp1,'b')
  96             pylab.plot(xp2,yp2,'r')
  97             pylab.plot(xp3,yp3,'g')
  98             pylab.plot(xp4,yp4,'y')
  99 
 100     pylab.show()
 101     
 102 def findPath(RRTV, RRTE):
 103     path = []
 104     new = RRTV[-1]
 105     prev = RRTE[-1]
 106     path.append([[new[0][0],new[0][1],0.5],
 107                  [new[1][0],new[1][1],0.5],
 108                  [new[2][0],new[2][1],0.5],
 109                  [new[3][0],new[3][1],0.5]])
 110     
 111     while (new != RRTV[0]):
 112         for i in range(len(RRTV)):
 113             if(RRTV[i] == prev):
 114                 new = prev
 115                 prev = RRTE[i]
 116                 path.append([[new[0][0],new[0][1],0.5],
 117                              [new[1][0],new[1][1],0.5],
 118                              [new[2][0],new[2][1],0.5],
 119                              [new[3][0],new[3][1],0.5]])
 120     path.reverse()
 121     return path
 122 
 123             
 124 def qRandom (cLimits, cObsX, cObsY):
 125     qRand = [[random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
 126             [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
 127             [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
 128             [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])]]
 129     
 130     for i in range(len(qRand)):
 131         if (((qRand[i][0] >= cObsX[0][0] and qRand[i][0] <= cObsX[0][1]) 
 132          and (qRand[i][1] >= cObsY[0][0] and qRand[i][1] <= cObsY[0][1])) 
 133          or ((qRand[i][0] >= cObsX[1][0] and qRand[i][0] <= cObsX[1][1]) 
 134          and (qRand[i][1] >= cObsY[1][0] and qRand[i][1] <= cObsY[1][1]))):
 135              qRand = qRandom(cLimits, cObsX, cObsY)
 136             
 137     return qRand
 138 
 139 def extendRRT (RRTV, RRTE, qRand, cLimits, cObsX, cObsY):
 140     qNear, ind = nearestNeighbor(RRTV, qRand)
 141     qNew = newConfiguration(qNear, ind, qRand, RRTE)
 142     if (qNew !=0 and notCollisions(qNear, qNew, cLimits, cObsX, cObsY)):
 143         RRTV.append(qNew)
 144         RRTE.append(qNear)
 145     return 
 146 
 147 def nearestNeighbor(RRTV, qRand):
 148     for i in range(len(RRTV)):
 149         if i == 0:
 150             dist = math.sqrt((RRTV[i][0][1]-qRand[0][1])**2 + 
 151                              (RRTV[i][0][0]-qRand[0][0])**2 + 
 152                              (RRTV[i][1][1]-qRand[1][1])**2 + 
 153                              (RRTV[i][1][0]-qRand[1][0])**2 + 
 154                              (RRTV[i][2][1]-qRand[2][1])**2 + 
 155                              (RRTV[i][2][0]-qRand[2][0])**2 + 
 156                              (RRTV[i][3][1]-qRand[3][1])**2 + 
 157                              (RRTV[i][3][0]-qRand[3][0])**2)
 158 
 159             qNear = RRTV[i]
 160             ind = i
 161         else:
 162             distAux = math.sqrt((RRTV[i][0][1]-qRand[0][1])**2 + 
 163                                 (RRTV[i][0][0]-qRand[0][0])**2 + 
 164                                 (RRTV[i][1][1]-qRand[1][1])**2 + 
 165                                 (RRTV[i][1][0]-qRand[1][0])**2 + 
 166                                 (RRTV[i][2][1]-qRand[2][1])**2 + 
 167                                 (RRTV[i][2][0]-qRand[2][0])**2 + 
 168                                 (RRTV[i][3][1]-qRand[3][1])**2 + 
 169                                 (RRTV[i][3][0]-qRand[3][0])**2)
 170             
 171             if distAux < dist:
 172                 dist = distAux
 173                 qNear = RRTV[i]
 174                 ind = i
 175     return qNear, ind
 176 
 177 def newConfiguration(qNear, ind, qRand, RRTE):
 178     qNear1 = qNear
 179     qNear0 = RRTE[ind]
 180     theta1 = math.atan2(qNear1[0][1]-qNear0[0][1], qNear1[0][0]-qNear0[0][0])
 181     theta2 = math.atan2(qNear1[1][1]-qNear0[1][1], qNear1[1][0]-qNear0[1][0])
 182     theta3 = math.atan2(qNear1[2][1]-qNear0[2][1], qNear1[2][0]-qNear0[2][0])
 183     theta4 = math.atan2(qNear1[3][1]-qNear0[3][1], qNear1[3][0]-qNear0[3][0])
 184     step = 0.1
 185     qNewaux1 = []
 186     qNewaux2 = []
 187     qNewaux3 = []
 188     qNewaux4 = []
 189     delta = math.radians(-90)
 190     for i in range(3):
 191         qNewaux1.append([qNear[0][0] + step*math.cos(theta1+delta), qNear[0][1] + step*math.sin(theta1+delta)])
 192         qNewaux2.append([qNear[1][0] + step*math.cos(theta2+delta), qNear[1][1] + step*math.sin(theta2+delta)])
 193         qNewaux3.append([qNear[2][0] + step*math.cos(theta3+delta), qNear[2][1] + step*math.sin(theta3+delta)])
 194         qNewaux4.append([qNear[3][0] + step*math.cos(theta4+delta), qNear[3][1] + step*math.sin(theta4+delta)])
 195         delta += math.radians(90)
 196 
 197     nodos = []
 198     for i in range(len(qNewaux1)):
 199             for j in range(len(qNewaux2)):
 200                 for k in range(len(qNewaux3)):
 201                     for l in range(len(qNewaux4)):
 202                         nodos.append([qNewaux1[i],qNewaux2[j],qNewaux3[k],qNewaux4[l]])
 203     nodosUtiles = formacion(nodos)
 204     if nodosUtiles != 0:
 205         qNew, ind = nearestNeighbor(nodosUtiles, qRand)
 206         return qNew
 207     else:
 208         return 0
 209 
 210 def formacion(nodos):
 211     nodosvalidos = []
 212     n = 0
 213     for i in range(len(nodos)):
 214         norm = []
 215         norm.append(math.hypot(nodos[i][0][0] - nodos[i][1][0], nodos[i][0][1] - nodos[i][1][1]))
 216         norm.append(math.hypot(nodos[i][1][0] - nodos[i][2][0], nodos[i][1][1] - nodos[i][2][1]))
 217         norm.append(math.hypot(nodos[i][2][0] - nodos[i][3][0], nodos[i][2][1] - nodos[i][3][1]))
 218         norm.append(math.hypot(nodos[i][3][0] - nodos[i][0][0], nodos[i][3][1] - nodos[i][0][1]))
 219         norm.append(math.hypot(nodos[i][0][0] - nodos[i][2][0], nodos[i][0][1] - nodos[i][2][1]))
 220         norm.append(math.hypot(nodos[i][3][0] - nodos[i][1][0], nodos[i][3][1] - nodos[i][1][1]))
 221         if ((norm[0] < 1 and norm[0] > 0.49) and
 222             (norm[1] < 1 and norm[1] > 0.49) and
 223             (norm[2] < 1 and norm[2] > 0.49) and
 224             (norm[3] < 1 and norm[3] > 0.49) and
 225             (norm[4] < 1 and norm[4] > 0.49) and
 226             (norm[5] < 1 and norm[5] > 0.49)):
 227             nodosvalidos.append(nodos[i])
 228             n+=1
 229     if n == 0:
 230         return 0
 231     else:
 232         return nodosvalidos
 233 
 234 def notCollisions(qNear, qNew, cLimits, cObsX, cObsY):
 235     qAux = []
 236     dx = []
 237     dy = []
 238     k = 0.25
 239     for i in range(len(qNear)):
 240         dx.append((qNew[i][0]-qNear[i][0])/10)
 241         dy.append((qNew[i][1]-qNear[i][1])/10)
 242     for j in range(10):
 243         qAux.append([[qNear[0][0] + dx[0]*j, qNear[0][1] + dy[0]*j],
 244                      [qNear[1][0] + dx[1]*j, qNear[1][1] + dy[1]*j],
 245                      [qNear[2][0] + dx[2]*j, qNear[2][1] + dy[2]*j],
 246                      [qNear[3][0] + dx[3]*j, qNear[3][1] + dy[3]*j]])
 247     qAux.append(qNew)
 248     for i in range(len(qAux)):
 249         for j in range(4):
 250             if (((qAux[i][j][0] >= (cObsX[0][0]-k) and qAux[i][j][0] <= (cObsX[0][1]+k)) 
 251                 and (qAux[i][j][1] >= (cObsY[0][0]-k) and qAux[i][j][1] <= (cObsY[0][1]+k))) 
 252                 or ((qAux[i][j][0] >= (cObsX[1][0]-k) and qAux[i][j][0] <= (cObsX[1][1]+k)) 
 253                 and (qAux[i][j][1] >= (cObsY[1][0]-k) and qAux[i][j][1] <= (cObsY[1][1]+k)))):
 254                     return False 
 255     return True
 256 
 257 
 258 #--Inicialización--
 259 qStart = [[-2.25,-2.25],
 260           [-2.25,-1.75],
 261           [-1.75,-1.75],
 262           [-1.75,-2.25]]
 263 
 264 cLimits = [[-2.5,2.5],
 265            [-2.5,2.5]]
 266 
 267 
 268 cObsX = [[-2.5,-1.75],
 269         [0,2.5]]
 270 cObsY = [[-0.5,0.5],
 271         [-0.5,0.5]]
 272 
 273 cGoalX = [1.5,2.5]
 274 cGoalY = [1,2]
 275 
 276 klimit = 4000
 277 
 278 if(not main(qStart, cGoalX, cGoalY, cLimits, cObsX, cObsY, klimit)):
 279     print 'Path not found'