├── .idea ├── Artificial-Potential-Field.iml ├── encodings.xml ├── misc.xml ├── modules.xml └── vcs.xml ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg ├── Aritifcial-potential-2.py ├── Artificial-Potential-1.py ├── Artificial-Potential-final.py ├── Artificial-potential-controller-2.py ├── Artificial-potential-controller.py ├── Artificial-potential-final-2.py ├── Artificial-potential-without-controller.py ├── Connection.py ├── README.md ├── count.bmp ├── count1.bmp ├── first.py ├── goal.jpg ├── images ├── WIN_20151203_18_22_30_Pro.jpg ├── WIN_20151203_18_22_59_Pro.jpg ├── WIN_20151203_18_23_11_Pro.jpg ├── WIN_20151203_18_23_30_Pro.jpg ├── WIN_20151203_18_23_37_Pro.jpg ├── WIN_20151203_18_23_46_Pro.jpg ├── WIN_20151203_18_24_13_Pro.jpg ├── WIN_20151203_18_24_59_Pro.jpg ├── WIN_20151203_18_25_06_Pro.jpg ├── WIN_20151203_18_25_10_Pro.jpg ├── WIN_20151203_18_25_14_Pro.jpg ├── WIN_20151203_18_25_20_Pro.jpg ├── WIN_20151203_18_25_36_Pro.jpg ├── WIN_20151203_18_26_48_Pro.jpg ├── WIN_20151203_18_26_52_Pro.jpg ├── WIN_20151203_18_26_55_Pro.jpg ├── WIN_20151203_18_26_58_Pro.jpg ├── WIN_20151203_18_27_02_Pro.jpg ├── WIN_20151203_18_27_10_Pro.jpg ├── WIN_20151203_18_27_19_Pro.jpg ├── WIN_20151203_18_28_49_Pro.jpg ├── WIN_20151203_18_28_52_Pro.jpg ├── WIN_20151203_18_28_55_Pro.jpg ├── WIN_20151203_18_28_57_Pro.jpg ├── WIN_20151203_18_29_19_Pro.jpg ├── WIN_20151203_18_29_29_Pro.jpg ├── WIN_20151203_18_29_43_Pro.jpg └── WIN_20151203_18_30_01_Pro.jpg ├── output ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg └── 6.jpg ├── output3.jpg ├── output31.jpg ├── output33.jpg ├── robot.jpg ├── sample.jpg ├── sample1.jpg ├── save1.bmp ├── save1.jpg ├── save_sample_image.py └── test.py /.idea/Artificial-Potential-Field.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | -------------------------------------------------------------------------------- /.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/1.jpg -------------------------------------------------------------------------------- /2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/2.jpg -------------------------------------------------------------------------------- /3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/3.jpg -------------------------------------------------------------------------------- /4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/4.jpg -------------------------------------------------------------------------------- /5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/5.jpg -------------------------------------------------------------------------------- /Aritifcial-potential-2.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | 9 | images = glob.glob('*.jpg') 10 | 11 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 12 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 13 | return True 14 | else: 15 | return False 16 | 17 | def printx(x): 18 | #print x 19 | pass 20 | 21 | def check_obstacles(arr, ansx, ansy): 22 | if arr[ansx][ansy][0] == 255: 23 | return True 24 | else: 25 | return False 26 | 27 | def feasible(arr, x, y): 28 | ex, ey, ez = arr.shape 29 | x = int(x) 30 | y = int(y) 31 | 32 | if check_boundaries(ex, ey, x, y): 33 | return not check_obstacles(arr, x, y) 34 | else: 35 | return False 36 | 37 | def obstacle(j, angle, cx, cy, arr): 38 | i = j+1 39 | while True: 40 | x = int(cx + i*math.sin(angle)) 41 | y = int(cy + i*math.cos(angle)) 42 | if not feasible(arr, x, y): 43 | break 44 | i += 1 45 | return i-j 46 | 47 | def path(arr, sx, sy, dx, dy): 48 | sol = [] 49 | rd = math.pi/8 #robot direction 50 | #robot size 51 | rx = 10 52 | ry = 10 53 | rspeed = 10 #robot speed 54 | mrspeed = 10 #max robot speed 55 | #s = 10 #safety distance 56 | macc = 10 #max acceleration 57 | maxt = 10*math.pi/180 #maximum turn 58 | dt = 30 #distance threshold 59 | k = 3 #degree of calculating potentials 60 | apscaling = 300000 #scaling factor for attractive potential 61 | rpscaling = 300000 #scaling factor for repulsive potential 62 | minApotential = 0.5 #minimum attractive potential 63 | rdiagonal = math.sqrt((rx/2)**2 + (ry/2)**2) 64 | 65 | ################################## 66 | ################################## 67 | 68 | ex, ey, ez = arr.shape #boundary values 69 | 70 | #current position 71 | cx = sx 72 | cy = sy 73 | ctheta = rd #current direction 74 | pathfound = False 75 | 76 | if sx <= -1 or sy <= -1 or sx >= ex or sy >= ey: 77 | print ('No solution exist as source is either on obstacle or outside boundary') 78 | return sol 79 | 80 | if dx <= -1 or dy <= -1 or dx >= ex or dy >= ey: 81 | print('No solution exist as goal is either on obstacle or outside boundary') 82 | return sol 83 | 84 | pi = math.pi 85 | while not pathfound: 86 | #calculate distance from obstacle at front 87 | df = obstacle(rx/2, ctheta, cx,cy, arr) 88 | 89 | #calculate distance from obstacle at left 90 | dl = obstacle(ry/2, ctheta-pi/2, cx,cy, arr) 91 | 92 | #calculate distance from obstacle at right 93 | dr = obstacle(ry/2, ctheta+pi/2, cx,cy, arr) 94 | 95 | #calculate distance from front-right-diagonal 96 | dfrd = obstacle(rdiagonal, ctheta+pi/4, cx,cy, arr) 97 | 98 | #calculate distance from front-left-diagonal 99 | dfld = obstacle(rdiagonal, ctheta-pi/4, cx,cy, arr) 100 | 101 | #calculate angle and distance from goal 102 | 103 | gtheta = math.atan2(dx-cx, dy-cy) 104 | 105 | dg = math.sqrt((cx-dx)*(cx-dx)+ (cy-dy)*(cy-dy)) #distance from goal 106 | 107 | if dg < dt: 108 | pathfound = True 109 | 110 | #calculate Potetials : 111 | 112 | rPx = ((1.0/df)**k)*math.sin(ctheta) + ((1.0/dl)**k)*math.sin(ctheta-pi/2) + ((1.0/dr)**k)*math.sin(ctheta+pi/2) + ((1.0/dfld)**k)*math.sin(ctheta-pi/4) + ((1.0/dfrd)**k)*math.sin(ctheta+pi/4) 113 | rPy = ((1.0/df)**k)*math.cos(ctheta) + ((1.0/dl)**k)*math.cos(ctheta-pi/2) + ((1.0/dr)**k)*math.cos(ctheta+pi/2) + ((1.0/dfld)**k)*math.cos(ctheta-pi/4) + ((1.0/dfrd)**k)*math.cos(ctheta+pi/4) 114 | 115 | aPx = max(((1.0/dg)**k)*apscaling, minApotential)* math.sin(gtheta) 116 | aPy = max(((1.0/dg)**k)*apscaling, minApotential)* math.cos(gtheta) 117 | 118 | tPx = aPx-rPx*rpscaling 119 | tPy = aPy-rPy*rpscaling 120 | 121 | #calculate new direction 122 | ntheta = math.atan2(rspeed*math.sin(ctheta) + tPx, rspeed*math.cos(ctheta)+ tPy)-ctheta 123 | 124 | while ntheta > math.pi: 125 | ntheta -= 2*math.pi 126 | while ntheta < -math.pi: 127 | ntheta += 2*math.pi 128 | ntheta = min(maxt, ntheta) 129 | ntheta = max(-maxt, ntheta) 130 | 131 | ctheta += ntheta 132 | #print ctheta*180/pi 133 | 134 | #setting speed based on robot acceleration and speed 135 | speed = math.sqrt((rspeed*math.sin(ctheta)+tPx)**2+ (rspeed*math.cos(ctheta)+tPy)**2) 136 | speed = min(rspeed+macc, speed) 137 | rspeed = max(rspeed-macc, speed) 138 | rspeed = min(rspeed, mrspeed) 139 | rspeed = max(rspeed, 0) 140 | 141 | if rspeed == 0: 142 | print 'Robot can\'t move' 143 | return sol 144 | 145 | #calculatig new positions 146 | cx = cx + (rspeed*math.sin(ctheta)) 147 | cy = cy + (rspeed*math.cos(ctheta)) 148 | 149 | if not feasible(arr, cx, cy): 150 | sol.append((int(cx), int(cy))) 151 | print 'robot collides' 152 | return sol 153 | 154 | sol.append((int(cx), int(cy))) 155 | 156 | return sol 157 | 158 | count = 0 159 | def main(): 160 | for im in images: 161 | 162 | img = cv2.imread(im) 163 | 164 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 165 | img2 = cv2.medianBlur(cimg,13) 166 | 167 | ret,thresh1 = cv2.threshold(cimg,40,255,cv2.THRESH_BINARY) 168 | t2 = copy.copy(thresh1) 169 | 170 | x, y = thresh1.shape 171 | print x, y 172 | arr = np.zeros((x, y, 3), np.uint8) 173 | final_contours= [] 174 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 175 | for i in range(len(contours)): 176 | cnt = contours[i] 177 | if cv2.contourArea(cnt) > 300 and cv2.contourArea(cnt) < 5000 : 178 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 179 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 180 | final_contours.append(cnt) 181 | print '1' 182 | arr1 = np.zeros((x, y, 3), np.uint8) 183 | for i in range(x): 184 | for j in range(y): 185 | if arr[i][j][0] ==255: 186 | arr1[i][j] = [0, 0, 0] 187 | else: 188 | arr1[i][j] = [255, 255, 255] 189 | 190 | cv2.imwrite('count.bmp', arr1) 191 | 192 | sx = 30 193 | sy = 50 194 | dx = 100 195 | dy = 200 196 | 197 | cmax = 50 198 | start = time.clock() 199 | #sol = path_planning(arr, sx, sy, dx, dy) 200 | sol = path(arr, sx, sy, dx, dy) 201 | #sol = [(100, 100),(100, 103), (100, 106), (100, 109), (100, 112), (100, 115)] 202 | if len(sol) == 0: 203 | print 'No solution exist ' 204 | continue 205 | print '2' 206 | for i in sol: 207 | # print arr[i[0], i[1]] 208 | # print i[0], i[1] 209 | arr[i[0], i[1]] = (255, 255, 0) 210 | img[i[0], i[1]] = (255, 0, 255) 211 | 212 | print 'time: ', time.clock()-start 213 | arr[sx][sy] = (0, 255, 255) 214 | arr[dx][dy] = (0, 255, 255) 215 | # cv2.circle(arr, (sol[len(sol)-1][1], sol[len(sol)-1][0]), 5, (0, 0, 255)) 216 | # cv2.circle(arr, (sx, sy), 6, (0, 255, 255)) 217 | # cv2.circle(arr, (dx, dy), 6, (0, 255, 255)) 218 | 219 | #draw(math.pi/4, arr) 220 | cv2.imshow('image', img) 221 | cv2.imshow('arr', arr) 222 | cv2.waitKey(0) 223 | cv2.destroyAllWindows() 224 | main() -------------------------------------------------------------------------------- /Artificial-Potential-1.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Queue as Q 9 | 10 | class pixel(object): 11 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 12 | self.penalty = penalty 13 | self.pointx = int(pointx) 14 | self.pointy = int(pointy) 15 | 16 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 26 | return True 27 | else: 28 | return False 29 | 30 | def printx(x): 31 | #print x 32 | pass 33 | 34 | def check_obstacles(arr, ansx, ansy): 35 | if arr[ansx][ansy][0] == 255: 36 | return True 37 | else: 38 | return False 39 | 40 | def feasible(arr, x, y): 41 | ex, ey, ez = arr.shape 42 | x = int(x) 43 | y = int(y) 44 | 45 | if check_boundaries(ex, ey, x, y): 46 | return not check_obstacles(arr, x, y) 47 | else: 48 | return False 49 | 50 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 51 | ansx = sx 52 | ansy = sy 53 | flag = True 54 | printx('6') 55 | count = 1 56 | while True: 57 | if count > q_star: 58 | return (-1, -1) 59 | ansx = sx + count*math.sin(theta) 60 | ansy = sy + count*math.cos(theta) 61 | 62 | if check_boundaries(x, y, ansx, ansy) == False: 63 | break 64 | else: 65 | if check_obstacles(arr, ansx, ansy) == True: 66 | break 67 | count += 5 68 | 69 | 70 | 71 | printx('7') 72 | return (ansx-sx,ansy- sy) 73 | 74 | flx = 10000 75 | fly = 10000 76 | 77 | 78 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 79 | forcex = 0 80 | forcey = 0 81 | neta = 30000 82 | x, y , z= arr.shape 83 | printx('8') 84 | #arr1 = np.zeros((x, y, 3), np.uint8) 85 | for i in range(8): 86 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 87 | # if ox == -1: 88 | # cv2.line(arr, (sy, sx), (int(sy+q_star*math.sqrt(2)), int(sx+q_star*math.sqrt(2))), (255, 255, 255), 1, cv2.LINE_AA) 89 | # else: 90 | # cv2.line(arr, (sy, sx), (int(sy+oy), int(sx+ox)), (255, 255, 255), 1, cv2.LINE_AA) 91 | 92 | #print 'ox ', ox, 'oy ', oy, i*45 93 | theta = i*math.pi/4 94 | ox = math.fabs(ox) 95 | oy = math.fabs(oy) 96 | 97 | d = math.hypot(ox,oy) 98 | #print d, i*45 99 | #sleep(2) 100 | fx = 0 101 | fy = 0 102 | if ox == -1 or oy == -1: 103 | fx = 0 104 | fy = 0 105 | else: 106 | if d == 0: 107 | d = 1 108 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 109 | fx = f*math.sin(theta) 110 | fy = f*math.cos(theta) 111 | 112 | # print 'fx ', fx, 'fy ', fy, theta, f 113 | forcex += fx 114 | forcey += fy 115 | # sleep(1) 116 | 117 | 118 | 119 | printx('9') 120 | 121 | #cv2.imshow('img1', arr) 122 | #cv2.waitKey(0) 123 | #sleep(20) 124 | 125 | return (forcex, forcey) 126 | 127 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 128 | forcex = 0 129 | forcey = 0 130 | tau = 1 #constant 131 | printx('10') 132 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 133 | if d > d_star: 134 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 135 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 136 | 137 | else: 138 | forcex += ((dx-sx)*tau) 139 | forcey += ((dy-sy)*tau) 140 | 141 | printx('11') 142 | return (forcex, forcey) 143 | 144 | def path_planning(arr, sx1, sy1, dx, dy): 145 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 146 | return [] 147 | #print '3' 148 | v = 4 #velocity magnitude 149 | t = 1 #time lapse 150 | theta = 0 #initial angle 151 | x,y,z = arr.shape 152 | theta_const = math.pi*30/180 153 | 154 | sx = sx1 155 | sy = sy1 156 | 157 | sol = [] 158 | sol.append((sx, sy)) 159 | q_star = 50000 160 | d_star = 20000 161 | 162 | sx += int(v*math.sin(theta)) 163 | sy += int(v*math.cos(theta)) 164 | sol.append((sx, sy)) 165 | 166 | ''' 167 | if Q and P are two vectors and @ is angle between them 168 | 169 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 170 | 171 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 172 | ''' 173 | 174 | printx ('4') 175 | count = 0 176 | while count < 1000: 177 | count += 1 178 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 179 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 180 | 181 | tx = gx+fx 182 | ty = gy+fy 183 | if(tx < 0): 184 | tx = max(tx, -flx) 185 | else: 186 | tx = min(tx, flx) 187 | if(ty < 0): 188 | ty = max(ty, -fly) 189 | else: 190 | ty = min(ty, fly) 191 | theta1 = math.atan2(tx, ty) 192 | #tx *= -1 193 | #ty *= -1 194 | print sx, sy, gx, gy, fx, fy, tx, ty 195 | 196 | if arr[sx][sy][0] == 255: 197 | print gx, gy, fx, fy 198 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 199 | print theta1*180/math.pi, theta*180/math.pi 200 | sleep(10) 201 | 202 | #theta1 = math.atan2(tx, ty) 203 | P = v 204 | angle = theta1-theta 205 | 206 | Q = math.sqrt(tx*tx + ty*ty) 207 | 208 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 209 | 210 | if theta2 < 0: 211 | theta2 = max(theta2, -theta_const) 212 | else: 213 | theta2 = min(theta2, theta_const) 214 | 215 | theta += theta2 216 | 217 | theta = (theta + 2*math.pi)%(2*math.pi) 218 | 219 | #print theta, theta2 220 | t1 = 0.0 221 | t2 = 0.0 222 | sx = sx + v*math.sin(theta) 223 | sy = sy + v*math.cos(theta) 224 | sx = int(sx) 225 | sy = int(sy) 226 | ''' 227 | if theta < math.pi and theta >= 0: 228 | sx += int(math.ceil(math.sin(theta))) 229 | else: 230 | sx += int(math.floor(math.sin(theta))) 231 | 232 | if theta <= math.pi/2 and theta > -1*math.pi/2: 233 | sy += int(math.ceil(math.cos(theta))) 234 | else: 235 | sy += int(math.floor(math.cos(theta))) 236 | ''' 237 | # print theta 238 | if not check_boundaries(x, y, sx, sy): 239 | print 'out of boundaries' , sx, sy 240 | return sol 241 | 242 | if sx < dx+ 10 and sx > dx - 10 and sy < dy+10 and sy > dy-10: 243 | break 244 | 245 | sol.append((sx, sy)) 246 | 247 | printx('5') 248 | return sol 249 | 250 | def draw(theta, arr): 251 | x = 4 252 | y = 20 253 | ex, ey, ez = arr.shape 254 | while check_boundaries(ex, ey, x, y): 255 | arr[x][y] = (255, 255, 0) 256 | x += int(2*math.sin(theta)) 257 | y += int(2*math.cos(theta)) 258 | 259 | 260 | 261 | def main(): 262 | count = 0 263 | for im in images: 264 | 265 | img = cv2.imread(im) 266 | 267 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 268 | img2 = cv2.medianBlur(cimg,13) 269 | 270 | ret,thresh1 = cv2.threshold(cimg,40,255,cv2.THRESH_BINARY) 271 | t2 = copy.copy(thresh1) 272 | 273 | x, y = thresh1.shape 274 | print x, y 275 | arr = np.zeros((x, y, 3), np.uint8) 276 | final_contours= [] 277 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 278 | for i in range(len(contours)): 279 | cnt = contours[i] 280 | if cv2.contourArea(cnt) > 300 and cv2.contourArea(cnt) < 5000 : 281 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 282 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 283 | final_contours.append(cnt) 284 | print '1' 285 | arr1 = np.zeros((x, y, 3), np.uint8) 286 | for i in range(x): 287 | for j in range(y): 288 | if arr[i][j][0] ==255: 289 | arr1[i][j] = [0, 0, 0] 290 | else: 291 | arr1[i][j] = [255, 255, 255] 292 | 293 | cv2.imwrite('count.bmp', arr1) 294 | 295 | sx = 30 296 | sy = 50 297 | dx = 30 298 | dy = 200 299 | 300 | cmax = 50 301 | start = time.clock() 302 | sol = path_planning(arr, sx, sy, dx, dy) 303 | #sol = path(arr, sx, sy, dx, dy) 304 | #sol = [(100, 100),(100, 103), (100, 106), (100, 109), (100, 112), (100, 115)] 305 | if len(sol) == 0: 306 | print 'No solution exist ' 307 | continue 308 | print '2' 309 | for i in sol: 310 | # print arr[i[0], i[1]] 311 | # 312 | 313 | 314 | # print i[0], i[1] 315 | arr[i[0], i[1]] = (255, 255, 0) 316 | img[i[0], i[1]] = (255, 0, 255) 317 | 318 | print 'time: ', time.clock()-start 319 | arr[sx][sy] = (0, 255, 255) 320 | arr[dx][dy] = (0, 255, 255) 321 | # cv2.circle(arr, (sol[len(sol)-1][1], sol[len(sol)-1][0]), 5, (0, 0, 255)) 322 | # cv2.circle(arr, (sx, sy), 6, (0, 255, 255)) 323 | # cv2.circle(arr, (dx, dy), 6, (0, 255, 255)) 324 | 325 | #draw(math.pi/4, arr) 326 | cv2.imshow('image', img) 327 | cv2.imshow('arr', arr) 328 | cv2.waitKey(0) 329 | cv2.destroyAllWindows() 330 | main() -------------------------------------------------------------------------------- /Artificial-Potential-final.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Queue as Q 9 | 10 | images = glob.glob('*.jpg') 11 | 12 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 13 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 14 | return True 15 | else: 16 | return False 17 | 18 | def printx(x): 19 | #print x 20 | pass 21 | 22 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 23 | if arr[ansx][ansy][0] == 255: 24 | return True 25 | else: 26 | return False 27 | 28 | def feasible(arr, x, y): #function to check if a point is feasible or not 29 | ex, ey, ez = arr.shape 30 | x = int(x) 31 | y = int(y) 32 | 33 | if check_boundaries(ex, ey, x, y): 34 | return not check_obstacles(arr, x, y) 35 | else: 36 | return False 37 | 38 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 39 | ansx = sx 40 | ansy = sy 41 | flag = True 42 | count = 1 43 | while True: 44 | if count > q_star: 45 | return (-1, -1) 46 | ansx = sx + count*math.sin(theta) 47 | ansy = sy + count*math.cos(theta) 48 | 49 | if check_boundaries(x, y, ansx, ansy) == False: 50 | break 51 | else: 52 | if check_obstacles(arr, ansx, ansy) == True: 53 | break 54 | count += 1 55 | 56 | return (ansx-sx,ansy- sy) 57 | 58 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 59 | forcex = 0 60 | forcey = 0 61 | neta = 300000000 62 | x, y , z= arr.shape 63 | for i in range(8): 64 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 65 | theta = i*math.pi/4 66 | ox = math.fabs(ox) 67 | oy = math.fabs(oy) 68 | 69 | d = math.hypot(ox,oy) 70 | fx = 0 71 | fy = 0 72 | if ox == -1 or oy == -1: 73 | fx = 0 74 | fy = 0 75 | else: 76 | if d == 0: 77 | d = 1 78 | 79 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 80 | fx = f*math.sin(theta) 81 | fy = f*math.cos(theta) 82 | 83 | forcex += fx 84 | forcey += fy 85 | 86 | return (forcex, forcey) 87 | 88 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 89 | forcex = 0 90 | forcey = 0 91 | tau = 20 #constant 92 | printx('10') 93 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 94 | if d > d_star: 95 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 96 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 97 | 98 | else: 99 | forcex += ((dx-sx)*tau) 100 | forcey += ((dy-sy)*tau) 101 | 102 | printx('11') 103 | return (forcex, forcey) 104 | 105 | def path_planning(arr, sx1, sy1, dx, dy): 106 | ''' 107 | 108 | :param arr: input map 109 | :param sx1: source x 110 | :param sy1: source y 111 | :param dx: destination x 112 | :param dy: destination y 113 | :return: path 114 | ''' 115 | 116 | #Parameters Declaration 117 | 118 | flx = 100000 #maximum total force in x 119 | fly = 100000 #maximum total force in y 120 | v = 10 #velocity magnitude 121 | t = 1 #time lapse 122 | theta = 0 #initial angle 123 | x,y,z = arr.shape 124 | theta_const = math.pi*30/180 #maximum allowed turn angle 125 | q_star = 1000 126 | d_star = 8000 127 | 128 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 129 | return [] 130 | sx = sx1 131 | sy = sy1 132 | 133 | sol = [] 134 | sol.append((sx, sy)) 135 | 136 | 137 | sx += int(v*math.sin(theta)) 138 | sy += int(v*math.cos(theta)) 139 | sol.append((sx, sy)) 140 | 141 | ''' 142 | if Q and P are two vectors and @ is angle between them 143 | 144 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 145 | 146 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 147 | ''' 148 | 149 | #count = 0 150 | while True: 151 | #count += 1 152 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 153 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 154 | print 'fx ', fx, ' fy ', fy, ' gx ', gx, ' gy ', gy 155 | 156 | tx = gx+fx 157 | ty = gy+fy 158 | if(tx < 0): 159 | tx = max(tx, -flx) 160 | else: 161 | tx = min(tx, flx) 162 | if(ty < 0): 163 | ty = max(ty, -fly) 164 | else: 165 | ty = min(ty, fly) 166 | 167 | print 'tx ', tx, ' ty ', ty 168 | theta1 = math.atan2(tx, ty) 169 | cv2.circle(arr, (sy, sx), 3, (255, 0, 0), 3) 170 | if arr[sx][sy][0] == 255: 171 | print gx, gy, fx, fy 172 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 173 | print theta1*180/math.pi, theta*180/math.pi 174 | cv2.circle(arr, (sy, sx ), 3, (255, 0, 0), 3) 175 | cv2.imshow('arr', arr) 176 | cv2.waitKey(0) 177 | 178 | P = v 179 | angle = theta1-theta #angle between velocity and force vector 180 | 181 | Q = math.sqrt(tx*tx + ty*ty) 182 | 183 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 184 | 185 | if theta2 < 0: 186 | theta2 = max(theta2, -theta_const) 187 | else: 188 | theta2 = min(theta2, theta_const) 189 | 190 | theta += theta2 191 | 192 | theta = (theta + 2*math.pi)%(2*math.pi) 193 | 194 | sx = sx + v*math.sin(theta) 195 | sy = sy + v*math.cos(theta) 196 | sx = int(sx) 197 | sy = int(sy) 198 | 199 | if not check_boundaries(x, y, sx, sy): 200 | print 'out of boundaries' , sx, sy 201 | return sol 202 | 203 | if sx < dx+ 5 and sx > dx - 5 and sy < dy+5 and sy > dy-5: 204 | break 205 | 206 | sol.append((sx, sy)) 207 | 208 | return sol 209 | 210 | count = 0 211 | def main(): 212 | counter = 1 213 | #for im in images: 214 | 215 | img = cv2.imread('sample.jpg') 216 | 217 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 218 | img2 = cv2.medianBlur(cimg,13) 219 | 220 | ret,thresh1 = cv2.threshold(cimg,100,120,cv2.THRESH_BINARY) 221 | t2 = copy.copy(thresh1) 222 | 223 | x, y = thresh1.shape 224 | print 'x ', x , ' y ', y 225 | arr = np.zeros((x, y, 3), np.uint8) 226 | final_contours= [] 227 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 228 | for i in range(len(contours)): 229 | cnt = contours[i] 230 | if cv2.contourArea(cnt) > 4000 and cv2.contourArea(cnt) < 15000 : 231 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 232 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 233 | final_contours.append(cnt) 234 | arr1 = np.zeros((x, y, 3), np.uint8) 235 | for i in range(x): 236 | for j in range(y): 237 | if arr[i][j][0] ==255: 238 | arr1[i][j] = [0, 0, 0] 239 | else: 240 | arr1[i][j] = [255, 255, 255] 241 | #cv2.imshow('arr1', arr1) 242 | 243 | cv2.imwrite('count.bmp', arr1) 244 | 245 | sx = 400 246 | sy = 300 247 | dx = 60 248 | dy = 300 249 | start = time.clock() 250 | cv2.circle(arr1, (sy, sx), 2, (255, 0, 0)) 251 | cv2.circle(arr1, (dy, dx), 2, (255, 0, 0)) 252 | cv2.imshow('arr', arr1) 253 | k = cv2.waitKey(0) 254 | cv2.destroyWindow('arr') 255 | sol = path_planning(arr, sx, sy, dx, dy) 256 | if len(sol) == 0: 257 | print 'No solution exist ' 258 | #continue 259 | for i in range(len(sol)): 260 | start = (sol[i][1], sol[i][0]) 261 | cv2.circle(arr,start, 1, [255, 255, 255]) 262 | cv2.circle(img, start, 1, [255, 255, 255]) 263 | 264 | #print 'time: ', time.clock()-start 265 | 266 | arr[sx][sy] = (0, 255, 255) 267 | arr[dx][dy] = (0, 255, 255) 268 | 269 | output = "output/"+`counter` 270 | output += ".jpg" 271 | cv2.imwrite(output, img) 272 | counter += 1 273 | 274 | 275 | cv2.imshow('image', img) 276 | cv2.imshow('arr', arr) 277 | cv2.waitKey(0) 278 | cv2.destroyAllWindows() 279 | main() -------------------------------------------------------------------------------- /Artificial-potential-controller-2.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Connection as conn 9 | 10 | cap = cv2.VideoCapture(1) 11 | 12 | import Queue as Q 13 | 14 | images = glob.glob('*.jpg') 15 | 16 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 17 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 18 | return True 19 | else: 20 | return False 21 | 22 | def printx(x): 23 | #print x 24 | pass 25 | 26 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 27 | if arr[ansx][ansy][0] == 255: 28 | return True 29 | else: 30 | return False 31 | 32 | def feasible(arr, x, y): #function to check if a point is feasible or not 33 | ex, ey, ez = arr.shape 34 | x = int(x) 35 | y = int(y) 36 | 37 | if check_boundaries(ex, ey, x, y): 38 | return not check_obstacles(arr, x, y) 39 | else: 40 | return False 41 | 42 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 43 | ansx = sx 44 | ansy = sy 45 | flag = True 46 | count = 1 47 | while True: 48 | if count > q_star: 49 | return (-1, -1) 50 | ansx = sx + count*math.sin(theta) 51 | ansy = sy + count*math.cos(theta) 52 | 53 | if check_boundaries(x, y, ansx, ansy) == False: 54 | break 55 | else: 56 | if check_obstacles(arr, ansx, ansy) == True: 57 | break 58 | count += 1 59 | 60 | 61 | return (ansx-sx,ansy- sy) 62 | 63 | def obstacle_force(arr, sx, sy, q_star, theta1): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 64 | forcex = 0 65 | forcey = 0 66 | neta = 30000000000000 67 | x, y , z= arr.shape 68 | for i in range(-8, 9): 69 | (ox,oy) = dist(sx, sy, x, y, (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi), arr, q_star) 70 | theta = (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi) 71 | fx = 0 72 | fy = 0 73 | #print 'ox ', ox, 'oy ', oy 74 | if ox == -1 or oy == -1: 75 | fx = 0 76 | fy = 0 77 | else: 78 | ox = math.fabs(ox) 79 | oy = math.fabs(oy) 80 | d = math.hypot(ox, oy) 81 | if d == 0: 82 | d = 1 83 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 84 | fx = f*math.sin(theta) 85 | fy = f*math.cos(theta) 86 | 87 | forcex += fx 88 | forcey += fy 89 | thet = math.atan2(forcex, forcey) 90 | arr1 = arr 91 | #cv2.line(arr1, (sy, sx), (int(sy + 10*math.cos(thet)), int(sx + math.sin(thet))), (0, 255, 255), 1) 92 | #cv2.imshow('arr', arr1) 93 | #k = cv2.waitKey(0) 94 | return (forcex, forcey) 95 | 96 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 97 | forcex = 0 98 | forcey = 0 99 | tau = 10000000 #constant 100 | printx('10') 101 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 102 | if d > d_star: 103 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 104 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 105 | 106 | else: 107 | forcex += ((dx-sx)*tau) 108 | forcey += ((dy-sy)*tau) 109 | 110 | printx('11') 111 | return (forcex, forcey) 112 | 113 | def path_planning(arr, sx1, sy1, dx, dy, theta): 114 | 115 | theta12= theta 116 | 117 | ''' 118 | :param arr: input map 119 | :param sx1: source x 120 | :param sy1: source y 121 | :param dx: destination x 122 | :param dy: destination y 123 | :return: path 124 | ''' 125 | 126 | #Parameters Declaration 127 | 128 | flx = 10000 #maximum total force in x 129 | fly = 10000 #maximum total force in y 130 | v = 25 #velocity magnitude 131 | t = 1 #time lapse 132 | x,y,z = arr.shape 133 | theta_const = math.pi*45/180 #maximum allowed turn angle 134 | q_star = 150 135 | d_star = 2 136 | 137 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 138 | return [] 139 | sx = sx1 140 | sy = sy1 141 | ''' 142 | if Q and P are two vectors and @ is angle between them 143 | 144 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 145 | 146 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 147 | ''' 148 | 149 | (fx, fy) = obstacle_force(arr, sx, sy, q_star, theta) 150 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 151 | 152 | tx = gx+fx 153 | ty = gy+fy 154 | if(tx < 0): 155 | tx = max(tx, -flx) 156 | else: 157 | tx = min(tx, flx) 158 | if(ty < 0): 159 | ty = max(ty, -fly) 160 | else: 161 | ty = min(ty, fly) 162 | theta1 = math.atan2(tx, ty) 163 | 164 | if arr[sx][sy][0] == 255: 165 | print gx, gy, fx, fy 166 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 167 | print theta1*180/math.pi, theta*180/math.pi 168 | 169 | P = v 170 | angle = theta1-theta #angle between velocity and force vector 171 | 172 | Q = math.sqrt(tx*tx + ty*ty) 173 | 174 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 175 | 176 | if theta2 < 0: 177 | theta2 = max(theta2, -theta_const) 178 | else: 179 | theta2 = min(theta2, theta_const) 180 | 181 | theta += theta2 182 | 183 | theta = (theta + 2*math.pi)%(2*math.pi) 184 | 185 | sx = sx + v*math.sin(theta) 186 | sy = sy + v*math.cos(theta) 187 | sx = int(sx) 188 | sy = int(sy) 189 | 190 | if not check_boundaries(x, y, sx, sy): 191 | print 'out of boundaries' , sx, sy 192 | print 'sx ', sx, ' sy' 193 | return (sx, sy, theta2) 194 | 195 | def show_image(im): 196 | cv2.imshow('image', im) 197 | k = cv2.waitKey(0) 198 | 199 | def find_goal(frame): 200 | # converting to HSV 201 | 202 | hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 203 | #show_image(hsv) 204 | 205 | lower_blue = np.array([113, 40, 29]) 206 | upper_blue = np.array([123, 180, 255]) 207 | 208 | mask = cv2.inRange(hsv, lower_blue, upper_blue) 209 | #show_image(mask) 210 | result = cv2.bitwise_and(frame, frame, mask=mask) 211 | #show_image(result) 212 | blur = cv2.blur(result, (5, 5)) 213 | 214 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 215 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 216 | 217 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 218 | # th3 = cv2.adaptiveThreshold(bw2,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ 219 | # cv2.THRESH_BINARY,11,2) 220 | edges = cv2.Canny(th3, 100, 200) 221 | th4 = copy.copy(th3) 222 | 223 | perimeter = 0 224 | j = 0 225 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 226 | # print len(contours) 227 | # if(len(contours) > 5): 228 | # continue 229 | cnt = np.array([]) 230 | for i in range(len(contours)): 231 | if (perimeter < cv2.contourArea(contours[i])): 232 | perimeter = cv2.contourArea(contours[i]) 233 | j = i; 234 | cnt = contours[j] 235 | if (len(cnt) == 0): 236 | return (-1, -1) 237 | cv2.drawContours(frame, cnt, -1, (0, 255, 0), 3) 238 | x = 0 239 | y = 0 240 | #print 'find goal' 241 | #print len(cnt), j 242 | #print cnt 243 | for i in range(len(cnt)): 244 | x = x + cnt[i][0][0] 245 | y = y + cnt[i][0][1] 246 | x = x/len(cnt) 247 | y = y/len(cnt) 248 | #print x, y 249 | x = int(x) 250 | y = int(y) 251 | cv2.circle(frame, (x, y), 5, (255, 0, 255), -1) 252 | 253 | #cv2.imshow('image', frame) 254 | #k = cv2.waitKey(0) 255 | 256 | return (int(x), int(y)) 257 | 258 | def find_robot(im): 259 | hsv = cv2.cvtColor(im, cv2.COLOR_BGR2HSV) 260 | lower = np.array([50, 28, 0]) 261 | upper = np.array([60, 168, 255]) 262 | mask = cv2.inRange(hsv, lower, upper) 263 | result = cv2.bitwise_and(im, im, mask=mask) 264 | blur = cv2.blur(result, (5, 5)) 265 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 266 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 267 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 268 | edges = cv2.Canny(th3, 100, 200) 269 | th4 = copy.copy(th3) 270 | perimeter = 0 271 | j = 0 272 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 273 | cnt = np.array([]) 274 | for i in range(len(contours)): 275 | if (perimeter < cv2.contourArea(contours[i])): 276 | perimeter = cv2.contourArea(contours[i]) 277 | j = i; 278 | cnt = contours[j] 279 | 280 | x = 0 281 | y = 0 282 | for i in range(len(cnt)): 283 | x = x + cnt[i][0][0] 284 | y = y + cnt[i][0][1] 285 | x = x / len(cnt) 286 | y = y / len(cnt) 287 | #print x, y 288 | x = int(x) 289 | y = int(y) 290 | cv2.circle(im, (x, y), 5, (255, 0, 255), 2) 291 | #show_image(im) 292 | return (int(x), int(y)) 293 | 294 | def get_direction(): 295 | direction = 0 296 | return direction 297 | 298 | def classify(img): 299 | cimg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 300 | img2 = cv2.medianBlur(cimg, 13) 301 | 302 | ret, thresh1 = cv2.threshold(cimg, 100, 120, cv2.THRESH_BINARY) 303 | t2 = copy.copy(thresh1) 304 | 305 | x, y = thresh1.shape 306 | arr = np.zeros((x, y, 3), np.uint8) 307 | final_contours = [] 308 | image, contours, hierarchy = cv2.findContours(t2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 309 | #cv2.imshow('image', image) 310 | #k = cv2.waitKey(0) 311 | for i in range(len(contours)): 312 | cnt = contours[i] 313 | if cv2.contourArea(cnt) > 3000 and cv2.contourArea(cnt) < 25000: 314 | cv2.drawContours(img, [cnt], -1, [0, 255, 255]) 315 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 316 | final_contours.append(cnt) 317 | #cv2.imshow('arr', arr) 318 | #k = cv2.waitKey(0) 319 | return arr 320 | 321 | def negate(arr): 322 | (x, y, z) = arr.shape 323 | arr1 = np.zeros((x, y, 3), np.uint8) 324 | for i in range(x): 325 | for j in range(y): 326 | if arr[i][j][0] == 255: 327 | arr1[i][j] = [0, 0, 0] 328 | else: 329 | arr1[i][j] = [255, 255, 255] 330 | return arr1 331 | 332 | def getAngle(po, im): 333 | angle = math.atan2(float(po[1][0]-po[0][0]),float(po[1][1]-po[0][1])) 334 | #angle = (angle + 2*math.pi)% (2*math.pi) 335 | return angle 336 | 337 | count = 0 338 | 339 | def main(): 340 | counter = 1 341 | 342 | _, im = cap.read() 343 | (x, y, z) = im.shape 344 | print x, y, z 345 | img1 = im 346 | arr = classify(img1) 347 | arr1 = negate(arr) 348 | #cv2.imshow('classify1', arr) 349 | #cv2.imshow('classify', arr1) 350 | #k = cv2.waitKey(0) 351 | 352 | (dy, dx) = find_goal(im) 353 | print 'dx ', dx, 'dy ', dy 354 | cv2.circle(im, (dy,dx), 4, (255, 255, 0), 3) 355 | print 'goal' 356 | print dy, dx 357 | cv2.imshow('arr', arr1) 358 | 359 | k = cv2.waitKey(0) 360 | 361 | _, im = cap.read() 362 | cv2.imshow('img', im) 363 | k = cv2.waitKey(0) 364 | 365 | (sy1, sx1) = find_robot(im) 366 | cv2.circle(im, (sy1, sx1), 4, (255, 255, 0), 3) 367 | print 'sx1 ' + `sx1` 368 | print 's y1 ' + `sy1` 369 | sx2 = 0 370 | sy2 = 0 371 | #cv2.imshow('img', im) 372 | #k = cv2.waitKey(0) 373 | 374 | 375 | connection_bot = conn.socket_connection() 376 | connection_bot_var = connection_bot.connect_to_server('192.168.43.173', 12345) 377 | 378 | 379 | while True: 380 | _, im = cap.read() 381 | (sy2, sx2) = find_robot(im) 382 | print 'sx2 ' + `sx2` 383 | print 'sy2 ' + `sy2` 384 | if sx1 > sx2 + 5 or sx1 < sx2-5 or sy1 > sy2 + 5 or sy1 < sy2 -5: 385 | break 386 | po = [] 387 | po.append([sx1, sy1]) 388 | po.append([sx2, sy2]) 389 | direction = getAngle(po, im) 390 | 391 | cv2.line(im, (sy1, sx1), (int(sy1 + 40*math.cos(direction)), int(sx1 + 40*math.sin(direction))), (255, 0, 0), 3) 392 | #cv2.imshow('im', im) 393 | #k = cv2.waitKey(0) 394 | (sx1, sy1) = (sx2, sy2) 395 | 396 | while True: 397 | time.sleep(0.7) 398 | _, im = cap.read() 399 | 400 | img = im 401 | (sy2, sx2) = find_robot(im) 402 | print sx2, sy2 403 | 404 | if sx2 + 20 > dx and sx2-20 < dx and sy2 + 20 > dy and sy2-20 < dy: 405 | print 'reached goal' 406 | break 407 | 408 | po = [] 409 | po.append([sx1, sy1]) 410 | po.append([sx2, sy2]) 411 | direction = getAngle(po, im) 412 | cv2.circle(img, (sy1, sx1), 2, (255, 255, 255), 1) 413 | cv2.circle(img, (sy2, sx2), 2, (255, 255, 255), 1) 414 | cv2.line(img, (sy1, sx1), (int(sy1 + 40 * math.cos(direction)), int(sx1 + 40 * math.sin(direction))),(255, 0, 0), 5) 415 | ang = direction 416 | d1 = direction 417 | (x, y, theta2) = path_planning(arr, sx2, sy2, dx, dy, direction) 418 | if x == -1 and y == -1: 419 | break 420 | print 'theta ', theta2*180/math.pi 421 | ''' 422 | po = [] 423 | po.append([sx2, sy2]) 424 | po.append([x, y]) 425 | theta = getAngle(po, im) 426 | direction = d1-theta 427 | #direction = (direction + 2*math.pi) %(2*math.pi) 428 | #print 'direction ' +`direction` 429 | #cv2.line(img, (sy2, sx2), ()) 430 | ''' 431 | send_result = "{:20}".format(theta2) 432 | 433 | arr[sx2][sy2] = (0, 255, 255) 434 | arr[dx][dy] = (0, 255, 255) 435 | 436 | print sx2, sy2, x, y, dx, dy 437 | (sx1, sy1) = (sx2, sy2) 438 | connection_bot_var.send(send_result) 439 | 440 | cv2.imshow('image', img) 441 | cv2.imshow('arr', arr) 442 | cv2.waitKey(0) 443 | cv2.destroyAllWindows() 444 | main() -------------------------------------------------------------------------------- /Artificial-potential-controller.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Connection as conn 9 | 10 | cap = cv2.VideoCapture(1) 11 | 12 | import Queue as Q 13 | 14 | images = glob.glob('*.jpg') 15 | 16 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 17 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 18 | return True 19 | else: 20 | return False 21 | 22 | def printx(x): 23 | #print x 24 | pass 25 | 26 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 27 | if arr[ansx][ansy][0] == 255: 28 | return True 29 | else: 30 | return False 31 | 32 | def feasible(arr, x, y): #function to check if a point is feasible or not 33 | ex, ey, ez = arr.shape 34 | x = int(x) 35 | y = int(y) 36 | 37 | if check_boundaries(ex, ey, x, y): 38 | return not check_obstacles(arr, x, y) 39 | else: 40 | return False 41 | 42 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 43 | ansx = sx 44 | ansy = sy 45 | flag = True 46 | count = 1 47 | while True: 48 | if count > q_star: 49 | return (-1, -1) 50 | ansx = sx + count*math.sin(theta) 51 | ansy = sy + count*math.cos(theta) 52 | 53 | if check_boundaries(x, y, ansx, ansy) == False: 54 | break 55 | else: 56 | if check_obstacles(arr, ansx, ansy) == True: 57 | break 58 | count += 1 59 | 60 | 61 | return (ansx-sx,ansy- sy) 62 | 63 | def obstacle_force(arr, sx, sy, q_star, theta1): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 64 | forcex = 0 65 | forcey = 0 66 | neta = 300000000000 67 | x, y , z= arr.shape 68 | for i in range(-8, 9): 69 | (ox,oy) = dist(sx, sy, x, y, (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi), arr, q_star) 70 | theta = (theta1 + i*math.pi/16 + 2*math.pi)%(2*math.pi) 71 | fx = 0 72 | fy = 0 73 | #print 'ox ', ox, 'oy ', oy 74 | if ox == -1 or oy == -1: 75 | fx = 0 76 | fy = 0 77 | else: 78 | ox = math.fabs(ox) 79 | oy = math.fabs(oy) 80 | d = math.hypot(ox, oy) 81 | if d == 0: 82 | d = 1 83 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 84 | fx = f*math.sin(theta) 85 | fy = f*math.cos(theta) 86 | 87 | forcex += fx 88 | forcey += fy 89 | thet = math.atan2(forcex, forcey) 90 | arr1 = arr 91 | cv2.line(arr1, (sy, sx), (int(sy + 10*math.cos(thet)), int(sx + math.sin(thet))), (0, 255, 255), 1) 92 | cv2.imshow('arr', arr1) 93 | k = cv2.waitKey(20) 94 | return (forcex, forcey) 95 | 96 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 97 | forcex = 0 98 | forcey = 0 99 | tau = 1000000 #constant 100 | printx('10') 101 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 102 | if d > d_star: 103 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 104 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 105 | 106 | else: 107 | forcex += ((dx-sx)*tau) 108 | forcey += ((dy-sy)*tau) 109 | 110 | printx('11') 111 | return (forcex, forcey) 112 | 113 | def path_planning(arr, sx1, sy1, dx, dy, theta): 114 | 115 | theta12= theta 116 | 117 | ''' 118 | :param arr: input map 119 | :param sx1: source x 120 | :param sy1: source y 121 | :param dx: destination x 122 | :param dy: destination y 123 | :return: path 124 | ''' 125 | 126 | #Parameters Declaration 127 | 128 | flx = 10000 #maximum total force in x 129 | fly = 10000 #maximum total force in y 130 | v = 5 #velocity magnitude 131 | t = 1 #time lapse 132 | x,y,z = arr.shape 133 | theta_const = math.pi*45/180 #maximum allowed turn angle 134 | q_star = 350 135 | d_star = 2 136 | 137 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 138 | return [] 139 | sx = sx1 140 | sy = sy1 141 | ''' 142 | if Q and P are two vectors and @ is angle between them 143 | 144 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 145 | 146 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 147 | ''' 148 | 149 | (fx, fy) = obstacle_force(arr, sx, sy, q_star, theta) 150 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 151 | 152 | tx = gx+fx 153 | ty = gy+fy 154 | if(tx < 0): 155 | tx = max(tx, -flx) 156 | else: 157 | tx = min(tx, flx) 158 | if(ty < 0): 159 | ty = max(ty, -fly) 160 | else: 161 | ty = min(ty, fly) 162 | theta1 = math.atan2(tx, ty) 163 | 164 | if arr[sx][sy][0] == 255: 165 | print gx, gy, fx, fy 166 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 167 | print theta1*180/math.pi, theta*180/math.pi 168 | 169 | P = v 170 | angle = theta1-theta #angle between velocity and force vector 171 | 172 | Q = math.sqrt(tx*tx + ty*ty) 173 | 174 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 175 | 176 | if theta2 < 0: 177 | theta2 = max(theta2, -theta_const) 178 | else: 179 | theta2 = min(theta2, theta_const) 180 | 181 | theta += theta2 182 | 183 | theta = (theta + 2*math.pi)%(2*math.pi) 184 | 185 | sx = sx + v*math.sin(theta) 186 | sy = sy + v*math.cos(theta) 187 | sx = int(sx) 188 | sy = int(sy) 189 | 190 | if not check_boundaries(x, y, sx, sy): 191 | print 'out of boundaries' , sx, sy 192 | print 'sx ', sx, ' sy' 193 | return (sx, sy, theta2) 194 | 195 | def show_image(im): 196 | cv2.imshow('image', im) 197 | k = cv2.waitKey(0) 198 | 199 | def find_goal(img): 200 | # converting to HSV 201 | frame = copy.copy(img) 202 | hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 203 | #show_image(hsv) 204 | 205 | lower_blue = np.array([113, 40, 29]) 206 | upper_blue = np.array([123, 180, 255]) 207 | 208 | mask = cv2.inRange(hsv, lower_blue, upper_blue) 209 | #show_image(mask) 210 | result = cv2.bitwise_and(frame, frame, mask=mask) 211 | #show_image(result) 212 | blur = cv2.blur(result, (5, 5)) 213 | 214 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 215 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 216 | 217 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 218 | # th3 = cv2.adaptiveThreshold(bw2,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ 219 | # cv2.THRESH_BINARY,11,2) 220 | edges = cv2.Canny(th3, 100, 200) 221 | th4 = copy.copy(th3) 222 | 223 | perimeter = 0 224 | j = 0 225 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 226 | # print len(contours) 227 | # if(len(contours) > 5): 228 | # continue 229 | cnt = np.array([]) 230 | for i in range(len(contours)): 231 | if (perimeter < cv2.contourArea(contours[i])): 232 | perimeter = cv2.contourArea(contours[i]) 233 | j = i; 234 | cnt = contours[j] 235 | if (len(cnt) == 0): 236 | return (-1, -1) 237 | cv2.drawContours(frame, cnt, -1, (0, 255, 0), 3) 238 | x = 0 239 | y = 0 240 | #print 'find goal' 241 | #print len(cnt), j 242 | #print cnt 243 | for i in range(len(cnt)): 244 | x = x + cnt[i][0][0] 245 | y = y + cnt[i][0][1] 246 | x = x/len(cnt) 247 | y = y/len(cnt) 248 | #print x, y 249 | x = int(x) 250 | y = int(y) 251 | cv2.circle(frame, (x, y), 5, (255, 0, 255), -1) 252 | 253 | cv2.imshow('image', frame) 254 | cv2.imwrite('goal.jpg', frame) 255 | k = cv2.waitKey(0) 256 | 257 | return (int(x), int(y)) 258 | 259 | def find_robot(frame): 260 | im = copy.copy(frame) 261 | hsv = cv2.cvtColor(im, cv2.COLOR_BGR2HSV) 262 | lower = np.array([50, 28, 0]) 263 | upper = np.array([60, 168, 255]) 264 | mask = cv2.inRange(hsv, lower, upper) 265 | result = cv2.bitwise_and(im, im, mask=mask) 266 | blur = cv2.blur(result, (5, 5)) 267 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 268 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 269 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 270 | edges = cv2.Canny(th3, 100, 200) 271 | th4 = copy.copy(th3) 272 | perimeter = 0 273 | j = 0 274 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 275 | cnt = np.array([]) 276 | for i in range(len(contours)): 277 | if (perimeter < cv2.contourArea(contours[i])): 278 | perimeter = cv2.contourArea(contours[i]) 279 | j = i; 280 | cnt = contours[j] 281 | 282 | x = 0 283 | y = 0 284 | for i in range(len(cnt)): 285 | x = x + cnt[i][0][0] 286 | y = y + cnt[i][0][1] 287 | x = x / len(cnt) 288 | y = y / len(cnt) 289 | #print x, y 290 | x = int(x) 291 | y = int(y) 292 | cv2.circle(im, (x, y), 5, (255, 0, 255), 2) 293 | cv2.imshow('img', im) 294 | k = cv2.waitKey(0) 295 | cv2.imwrite('robot.jpg', im) 296 | #show_image(im) 297 | return (int(x), int(y)) 298 | 299 | def get_direction(): 300 | direction = 0 301 | return direction 302 | 303 | def classify(img): 304 | cimg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 305 | img2 = cv2.medianBlur(cimg, 13) 306 | 307 | ret, thresh1 = cv2.threshold(cimg, 100, 120, cv2.THRESH_BINARY) 308 | t2 = copy.copy(thresh1) 309 | 310 | x, y = thresh1.shape 311 | arr = np.zeros((x, y, 3), np.uint8) 312 | final_contours = [] 313 | image, contours, hierarchy = cv2.findContours(t2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 314 | #cv2.imshow('image', image) 315 | #k = cv2.waitKey(0) 316 | for i in range(len(contours)): 317 | cnt = contours[i] 318 | if cv2.contourArea(cnt) > 3600 and cv2.contourArea(cnt) < 25000: 319 | cv2.drawContours(img, [cnt], -1, [0, 255, 255]) 320 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 321 | final_contours.append(cnt) 322 | cv2.imshow('arr', arr) 323 | k = cv2.waitKey(0) 324 | return arr 325 | 326 | def negate(arr): 327 | (x, y, z) = arr.shape 328 | arr1 = np.zeros((x, y, 3), np.uint8) 329 | for i in range(x): 330 | for j in range(y): 331 | if arr[i][j][0] == 255: 332 | arr1[i][j] = [0, 0, 0] 333 | else: 334 | arr1[i][j] = [255, 255, 255] 335 | return arr1 336 | 337 | def getAngle(po, im): 338 | angle = math.atan2(float(po[1][0]-po[0][0]),float(po[1][1]-po[0][1])) 339 | #angle = (angle + 2*math.pi)% (2*math.pi) 340 | return angle 341 | 342 | count = 0 343 | 344 | def main(): 345 | counter = 1 346 | 347 | im = cv2.imread('sample1.jpg') 348 | input_image = copy.copy(im) 349 | (x, y, z) = im.shape 350 | print x, y, z 351 | img1 = copy.copy(im) 352 | arr = classify(img1) 353 | arr1 = negate(arr) 354 | cv2.imshow('arr', arr) 355 | k = cv2.waitKey(0) 356 | dx, dy = 50, 50 357 | sx1, sy1 = 500, 1000 358 | (dy, dx) = find_goal(im) 359 | (sy1, sx1) = find_robot(im) 360 | (sx2, sy2) = (sx1, sy1) 361 | print 'sx1, sy1 : ', sx1, sy1 362 | cv2.circle(img1, (sy1, sx1), 1, (255, 0 , 0)) 363 | cv2.circle(img1, (dy, dx), 1, (255, 0, 0)) 364 | cv2.imshow('arr', img1) 365 | k = cv2.waitKey(20) 366 | count = 0 367 | while True: 368 | 369 | if sx2 + 20 > dx and sx2-20 < dx and sy2 + 20 > dy and sy2-20 < dy: 370 | print 'reached goal' 371 | break 372 | 373 | po = [] 374 | po.append([sx1, sy1]) 375 | po.append([sx2, sy2]) 376 | direction = getAngle(po, im) 377 | if count == 0: 378 | direction = 0 379 | count = count + 1 380 | d1 = direction 381 | print sx2, sy2, direction, 'attr' 382 | (x, y, theta2) = path_planning(arr, sx2, sy2, dx, dy, direction) 383 | print 'theta2', theta2*180/math.pi 384 | po = [] 385 | po.append([sx2, sy2]) 386 | po.append([x, y]) 387 | theta = getAngle(po, im) 388 | direction = theta-d1 389 | cv2.line(im, (sy2, sx2), (int(sy2 + 20*math.cos(theta2)), int(sx2 + 20*math.sin(theta2))), (255, 255, 0), 2) 390 | cv2.line(im, (sy2, sx2), (y, x), (0, 0, 255), 2) 391 | cv2.line(input_image, (sy2, sx2), (y, x), (255, 255, 255), 1) 392 | cv2.imshow('im', im) 393 | cv2.imshow('input', input_image) 394 | k = cv2.waitKey(20) 395 | (sx1, sy1) = (sx2, sy2) 396 | (sx2, sy2) = (x, y) 397 | 398 | cv2.imwrite('output/2.jpg', input_image) 399 | cv2.imshow('arr', arr) 400 | cv2.waitKey(0) 401 | cv2.destroyAllWindows() 402 | 403 | main() -------------------------------------------------------------------------------- /Artificial-potential-final-2.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Queue as Q 9 | 10 | images = glob.glob('*.jpg') 11 | 12 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 13 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 14 | return True 15 | else: 16 | return False 17 | 18 | def printx(x): 19 | #print x 20 | pass 21 | 22 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 23 | if arr[ansx][ansy][0] == 255: 24 | return True 25 | else: 26 | return False 27 | 28 | def feasible(arr, x, y): #function to check if a point is feasible or not 29 | ex, ey, ez = arr.shape 30 | x = int(x) 31 | y = int(y) 32 | 33 | if check_boundaries(ex, ey, x, y): 34 | return not check_obstacles(arr, x, y) 35 | else: 36 | return False 37 | 38 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 39 | ansx = sx 40 | ansy = sy 41 | flag = True 42 | count = 1 43 | while True: 44 | if count > q_star: 45 | return (-1, -1) 46 | ansx = sx + count*math.sin(theta) 47 | ansy = sy + count*math.cos(theta) 48 | 49 | if check_boundaries(x, y, ansx, ansy) == False: 50 | break 51 | else: 52 | if check_obstacles(arr, ansx, ansy) == True: 53 | break 54 | count += 1 55 | 56 | return (ansx-sx,ansy- sy) 57 | 58 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 59 | forcex = 0 60 | forcey = 0 61 | neta = 30000000 62 | x, y , z= arr.shape 63 | for i in range(8): 64 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 65 | theta = i*math.pi/4 66 | ox = math.fabs(ox) 67 | oy = math.fabs(oy) 68 | 69 | d = math.hypot(ox,oy) 70 | fx = 0 71 | fy = 0 72 | if ox == -1 or oy == -1: 73 | fx = 0 74 | fy = 0 75 | else: 76 | if d == 0: 77 | d = 1 78 | if d > q_star: 79 | fx = 0 80 | fy = 0 81 | else: 82 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 83 | fx = f*math.sin(theta) 84 | fy = f*math.cos(theta) 85 | 86 | forcex += fx 87 | forcey += fy 88 | 89 | return (forcex, forcey) 90 | 91 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 92 | forcex = 0 93 | forcey = 0 94 | tau = 100000000 #constant 95 | printx('10') 96 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 97 | if d > d_star: 98 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 99 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 100 | 101 | else: 102 | forcex += ((dx-sx)*tau) 103 | forcey += ((dy-sy)*tau) 104 | 105 | printx('11') 106 | return (forcex, forcey) 107 | 108 | def path_planning(arr, sx1, sy1, dx, dy): 109 | ''' 110 | 111 | :param arr: input map 112 | :param sx1: source x 113 | :param sy1: source y 114 | :param dx: destination x 115 | :param dy: destination y 116 | :return: path 117 | ''' 118 | 119 | #Parameters Declaration 120 | 121 | flx = 100000 #maximum total force in x 122 | fly = 100000 #maximum total force in y 123 | v = 3 #velocity magnitude 124 | t = 1 #time lapse 125 | theta = 0 #initial angle 126 | x,y,z = arr.shape 127 | theta_const = math.pi*80/180 #maximum allowed turn angle 128 | q_star = 100 129 | d_star = 10 130 | 131 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 132 | return [] 133 | sx = sx1 134 | sy = sy1 135 | 136 | sol = [] 137 | sol.append((sx, sy)) 138 | 139 | 140 | sx += int(v*math.sin(theta)) 141 | sy += int(v*math.cos(theta)) 142 | sol.append((sx, sy)) 143 | 144 | ''' 145 | if Q and P are two vectors and @ is angle between them 146 | 147 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 148 | 149 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 150 | ''' 151 | 152 | #count = 0 153 | while True: 154 | #count += 1 155 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 156 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 157 | 158 | tx = gx+fx 159 | ty = gy+fy 160 | if(tx < 0): 161 | tx = max(tx, -flx) 162 | else: 163 | tx = min(tx, flx) 164 | if(ty < 0): 165 | ty = max(ty, -fly) 166 | else: 167 | ty = min(ty, fly) 168 | theta1 = math.atan2(tx, ty) 169 | arr1 = arr 170 | if arr[sx][sy][0] == 255: 171 | print gx, gy, fx, fy 172 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 173 | print theta1*180/math.pi, theta*180/math.pi 174 | cv2.circle(arr1, (sy, sx), 3, (255, 0, 0), 4) 175 | cv2.imshow('arr12', arr1) 176 | k = cv2.waitKey(0) 177 | 178 | P = v 179 | angle = theta1-theta #angle between velocity and force vector 180 | 181 | Q = math.sqrt(tx*tx + ty*ty) 182 | 183 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 184 | 185 | if theta2 < 0: 186 | theta2 = max(theta2, -theta_const) 187 | else: 188 | theta2 = min(theta2, theta_const) 189 | 190 | theta += theta2 191 | 192 | theta = (theta + 2*math.pi)%(2*math.pi) 193 | 194 | sx = sx + v*math.sin(theta) 195 | sy = sy + v*math.cos(theta) 196 | sx = int(sx) 197 | sy = int(sy) 198 | 199 | if not check_boundaries(x, y, sx, sy): 200 | print 'out of boundaries' , sx, sy 201 | return sol 202 | 203 | if sx < dx+ 5 and sx > dx - 5 and sy < dy+5 and sy > dy-5: 204 | break 205 | 206 | sol.append((sx, sy)) 207 | 208 | return sol 209 | 210 | count = 0 211 | def main(): 212 | counter = 1 213 | #for im in images: 214 | 215 | img = cv2.imread('sample.jpg') 216 | 217 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 218 | img2 = cv2.medianBlur(cimg,13) 219 | 220 | ret,thresh1 = cv2.threshold(cimg,100,120,cv2.THRESH_BINARY) 221 | t2 = copy.copy(thresh1) 222 | 223 | x, y = thresh1.shape 224 | arr = np.zeros((x, y, 3), np.uint8) 225 | final_contours= [] 226 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 227 | for i in range(len(contours)): 228 | cnt = contours[i] 229 | if cv2.contourArea(cnt) > 5000 and cv2.contourArea(cnt) < 15000 : 230 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 231 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 232 | final_contours.append(cnt) 233 | arr1 = np.zeros((x, y, 3), np.uint8) 234 | for i in range(x): 235 | for j in range(y): 236 | if arr[i][j][0] ==255: 237 | arr1[i][j] = [0, 0, 0] 238 | else: 239 | arr1[i][j] = [255, 255, 255] 240 | cv2.imshow('arr12', arr1) 241 | k = cv2.waitKey(0) 242 | cv2.destroyWindow('arr1') 243 | 244 | cv2.imwrite('count.bmp', arr1) 245 | 246 | sx = 400 247 | sy = 300 248 | dx = 60 249 | dy = 500 250 | start = time.clock() 251 | sol = path_planning(arr, sx, sy, dx, dy) 252 | if len(sol) == 0: 253 | print 'No solution exist ' 254 | #continue 255 | for i in range(len(sol)): 256 | start = (sol[i][1], sol[i][0]) 257 | cv2.circle(arr,start, 1, [255, 255, 255]) 258 | cv2.circle(img, start, 1, [255, 255, 255]) 259 | 260 | #print 'time: ', time.clock()-start 261 | 262 | arr[sx][sy] = (0, 255, 255) 263 | arr[dx][dy] = (0, 255, 255) 264 | 265 | output = "output/"+`counter` 266 | output += ".jpg" 267 | cv2.imwrite(output, img) 268 | counter += 1 269 | 270 | 271 | cv2.imshow('image', img) 272 | cv2.imshow('arr', arr) 273 | cv2.waitKey(0) 274 | cv2.destroyAllWindows() 275 | main() -------------------------------------------------------------------------------- /Artificial-potential-without-controller.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Connection as conn 9 | 10 | cap = cv2.VideoCapture(1) 11 | 12 | import Queue as Q 13 | 14 | images = glob.glob('*.jpg') 15 | 16 | def check_boundaries(ex, ey, nx, ny): #ex, ey :- end points of frame 17 | if nx > -1 and ny > -1 and nx < ex and ny < ey: 18 | return True 19 | else: 20 | return False 21 | 22 | def printx(x): 23 | #print x 24 | pass 25 | 26 | def check_obstacles(arr, ansx, ansy): #function to check whether a given point is on obstacle or not 27 | if arr[ansx][ansy][0] == 255: 28 | return True 29 | else: 30 | return False 31 | 32 | def feasible(arr, x, y): #function to check if a point is feasible or not 33 | ex, ey, ez = arr.shape 34 | x = int(x) 35 | y = int(y) 36 | 37 | if check_boundaries(ex, ey, x, y): 38 | return not check_obstacles(arr, x, y) 39 | else: 40 | return False 41 | 42 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 43 | ansx = sx 44 | ansy = sy 45 | flag = True 46 | count = 1 47 | while True: 48 | if count > q_star: 49 | return (-1, -1) 50 | ansx = sx + count*math.sin(theta) 51 | ansy = sy + count*math.cos(theta) 52 | 53 | if check_boundaries(x, y, ansx, ansy) == False: 54 | break 55 | else: 56 | if check_obstacles(arr, ansx, ansy) == True: 57 | break 58 | count += 1 59 | 60 | return (ansx-sx,ansy- sy) 61 | 62 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 63 | forcex = 0 64 | forcey = 0 65 | neta = 30000 66 | x, y , z= arr.shape 67 | for i in range(8): 68 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 69 | theta = i*math.pi/4 70 | ox = math.fabs(ox) 71 | oy = math.fabs(oy) 72 | 73 | d = math.hypot(ox,oy) 74 | fx = 0 75 | fy = 0 76 | if ox == -1 or oy == -1: 77 | fx = 0 78 | fy = 0 79 | else: 80 | if d == 0: 81 | d = 1 82 | f = (neta*(1.0/q_star- 1.0/d))/(d*d) 83 | fx = f*math.sin(theta) 84 | fy = f*math.cos(theta) 85 | 86 | forcex += fx 87 | forcey += fy 88 | 89 | return (forcex, forcey) 90 | 91 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 92 | forcex = 0 93 | forcey = 0 94 | tau = 1 #constant 95 | printx('10') 96 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 97 | if d > d_star: 98 | forcex += ((d_star*tau*math.sin(math.atan2(dx-sx, dy-sy)))) 99 | forcey += ((d_star*tau*math.cos(math.atan2(dx-sx, dy-sy)))) 100 | 101 | else: 102 | forcex += ((dx-sx)*tau) 103 | forcey += ((dy-sy)*tau) 104 | 105 | printx('11') 106 | return (forcex, forcey) 107 | 108 | def path_planning(arr, sx1, sy1, dx, dy, theta): 109 | 110 | theta12= theta 111 | 112 | ''' 113 | :param arr: input map 114 | :param sx1: source x 115 | :param sy1: source y 116 | :param dx: destination x 117 | :param dy: destination y 118 | :return: path 119 | ''' 120 | 121 | #Parameters Declaration 122 | 123 | flx = 10000 #maximum total force in x 124 | fly = 10000 #maximum total force in y 125 | v = 40 #velocity magnitude 126 | t = 1 #time lapse 127 | x,y,z = arr.shape 128 | theta_const = math.pi*90/180 #maximum allowed turn angle 129 | q_star = 50000 130 | d_star = 20000 131 | 132 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 133 | return [] 134 | sx = sx1 135 | sy = sy1 136 | ''' 137 | if Q and P are two vectors and @ is angle between them 138 | 139 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 140 | 141 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 142 | ''' 143 | 144 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 145 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 146 | 147 | tx = gx+fx 148 | ty = gy+fy 149 | if(tx < 0): 150 | tx = max(tx, -flx) 151 | else: 152 | tx = min(tx, flx) 153 | if(ty < 0): 154 | ty = max(ty, -fly) 155 | else: 156 | ty = min(ty, fly) 157 | theta1 = math.atan2(tx, ty) 158 | 159 | if arr[sx][sy][0] == 255: 160 | print gx, gy, fx, fy 161 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 162 | print theta1*180/math.pi, theta*180/math.pi 163 | sleep(10) 164 | 165 | P = v 166 | angle = theta1-theta #angle between velocity and force vector 167 | 168 | Q = math.sqrt(tx*tx + ty*ty) 169 | 170 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 171 | 172 | if theta2 < 0: 173 | theta2 = max(theta2, -theta_const) 174 | else: 175 | theta2 = min(theta2, theta_const) 176 | 177 | theta += theta2 178 | 179 | theta = (theta + 2*math.pi)%(2*math.pi) 180 | 181 | sx = sx + v*math.sin(theta) 182 | sy = sy + v*math.cos(theta) 183 | sx = int(sx) 184 | sy = int(sy) 185 | 186 | if not check_boundaries(x, y, sx, sy): 187 | print 'out of boundaries' , sx, sy 188 | 189 | return (sx, sy, theta2) 190 | 191 | def show_image(im): 192 | cv2.imshow('image', im) 193 | k = cv2.waitKey(0) 194 | 195 | def find_goal(frame): 196 | # converting to HSV 197 | 198 | hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) 199 | #show_image(hsv) 200 | 201 | lower_blue = np.array([113, 40, 29]) 202 | upper_blue = np.array([123, 180, 255]) 203 | 204 | mask = cv2.inRange(hsv, lower_blue, upper_blue) 205 | #show_image(mask) 206 | result = cv2.bitwise_and(frame, frame, mask=mask) 207 | #show_image(result) 208 | blur = cv2.blur(result, (5, 5)) 209 | 210 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 211 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 212 | 213 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 214 | # th3 = cv2.adaptiveThreshold(bw2,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ 215 | # cv2.THRESH_BINARY,11,2) 216 | edges = cv2.Canny(th3, 100, 200) 217 | th4 = copy.copy(th3) 218 | 219 | perimeter = 0 220 | j = 0 221 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 222 | # print len(contours) 223 | # if(len(contours) > 5): 224 | # continue 225 | cnt = np.array([]) 226 | for i in range(len(contours)): 227 | if (perimeter < cv2.contourArea(contours[i])): 228 | perimeter = cv2.contourArea(contours[i]) 229 | j = i; 230 | cnt = contours[j] 231 | if (len(cnt) == 0): 232 | return (-1, -1) 233 | cv2.drawContours(frame, cnt, -1, (0, 255, 0), 3) 234 | x = 0 235 | y = 0 236 | #print 'find goal' 237 | #print len(cnt), j 238 | #print cnt 239 | for i in range(len(cnt)): 240 | x = x + cnt[i][0][0] 241 | y = y + cnt[i][0][1] 242 | x = x/len(cnt) 243 | y = y/len(cnt) 244 | #print x, y 245 | x = int(x) 246 | y = int(y) 247 | cv2.circle(frame, (x, y), 5, (255, 0, 255), -1) 248 | 249 | #cv2.imshow('image', frame) 250 | #k = cv2.waitKey(0) 251 | 252 | return (int(x), int(y)) 253 | 254 | def find_robot(im): 255 | hsv = cv2.cvtColor(im, cv2.COLOR_BGR2HSV) 256 | lower = np.array([50, 28, 0]) 257 | upper = np.array([60, 168, 255]) 258 | mask = cv2.inRange(hsv, lower, upper) 259 | result = cv2.bitwise_and(im, im, mask=mask) 260 | blur = cv2.blur(result, (5, 5)) 261 | bw = cv2.cvtColor(blur, cv2.COLOR_HSV2BGR) 262 | bw2 = cv2.cvtColor(bw, cv2.COLOR_BGR2GRAY) 263 | ret, th3 = cv2.threshold(bw2, 30, 255, cv2.THRESH_BINARY) 264 | edges = cv2.Canny(th3, 100, 200) 265 | th4 = copy.copy(th3) 266 | perimeter = 0 267 | j = 0 268 | image, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 269 | cnt = np.array([]) 270 | for i in range(len(contours)): 271 | if (perimeter < cv2.contourArea(contours[i])): 272 | perimeter = cv2.contourArea(contours[i]) 273 | j = i; 274 | cnt = contours[j] 275 | 276 | x = 0 277 | y = 0 278 | for i in range(len(cnt)): 279 | x = x + cnt[i][0][0] 280 | y = y + cnt[i][0][1] 281 | x = x / len(cnt) 282 | y = y / len(cnt) 283 | #print x, y 284 | x = int(x) 285 | y = int(y) 286 | cv2.circle(im, (x, y), 5, (255, 0, 255), 2) 287 | #show_image(im) 288 | return (int(x), int(y)) 289 | 290 | def get_direction(): 291 | direction = 0 292 | return direction 293 | 294 | def classify(img): 295 | cimg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 296 | img2 = cv2.medianBlur(cimg, 13) 297 | 298 | ret, thresh1 = cv2.threshold(cimg, 100, 120, cv2.THRESH_BINARY) 299 | t2 = copy.copy(thresh1) 300 | 301 | x, y = thresh1.shape 302 | arr = np.zeros((x, y, 3), np.uint8) 303 | final_contours = [] 304 | image, contours, hierarchy = cv2.findContours(t2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 305 | #cv2.imshow('image', image) 306 | #k = cv2.waitKey(0) 307 | for i in range(len(contours)): 308 | cnt = contours[i] 309 | if cv2.contourArea(cnt) > 35000 and cv2.contourArea(cnt) < 15000: 310 | cv2.drawContours(img, [cnt], -1, [0, 255, 255]) 311 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 312 | final_contours.append(cnt) 313 | cv2.imshow('arr', arr) 314 | k = cv2.waitKey(0) 315 | return arr 316 | 317 | def negate(arr): 318 | (x, y, z) = arr.shape 319 | arr1 = np.zeros((x, y, 3), np.uint8) 320 | for i in range(x): 321 | for j in range(y): 322 | if arr[i][j][0] == 255: 323 | arr1[i][j] = [0, 0, 0] 324 | else: 325 | arr1[i][j] = [255, 255, 255] 326 | return arr1 327 | 328 | def getAngle(po, im): 329 | angle = math.atan2(float(po[1][0]-po[0][0]),float(po[1][1]-po[0][1])) 330 | angle = (angle + 2*math.pi)% (2*math.pi) 331 | return angle 332 | 333 | count = 0 334 | 335 | def main(): 336 | counter = 1 337 | im = cv2.imread('sample.jpg') 338 | #_, im = cap.read() 339 | (x, y, z) = im.shape 340 | print x, y, z 341 | img1 = im 342 | arr = classify(img1) 343 | arr1 = negate(arr) 344 | cv2.imshow('classify1', arr) 345 | cv2.imshow('classify', arr1) 346 | k = cv2.waitKey(0) 347 | 348 | (dy, dx) = find_goal(im) 349 | cv2.circle(im, (dy,dx), 4, (255, 255, 255), 3) 350 | print 'goal' 351 | print dy, dx 352 | direction = -math.pi/4 353 | #_, im = cap.read() 354 | 355 | img = im 356 | (sy2, sx2) = find_robot(im) 357 | print sx2, sy2 358 | 359 | if sx2 + 10 > dx and sx2-10 < dx and sy2 + 10 > dy and sy2-10 < dy: 360 | print 'reached goal' 361 | 362 | cv2.imwrite('count.bmp', arr1) 363 | 364 | cv2.line(im, (sy2, sx2), (int(sy2 + 40 * math.cos(direction)), int(sx2 + 40 * math.sin(direction))), (255, 255, 255), 3) 365 | 366 | (x, y, direction) = path_planning(arr, sx2, sy2, dx, dy, direction) 367 | #print 'direction ' +`direction` 368 | #direction = (direction + 2*math.pi) %(2*math.pi) 369 | po = [] 370 | po.append([sx2, sy2]) 371 | po.append([x, y]) 372 | direction = getAngle(po, im) 373 | print 'direction', direction 374 | direction = 0 375 | 376 | cv2.line(im, (sy2, sx2), (int(sy2 + 40*math.cos(direction)), int(sx2 + 40*math.sin(direction))), (255, 0 , 0), 3) 377 | #cv2.imshow('image', im) 378 | #k = cv2.waitKey(0) 379 | 380 | #cv2.line(im, (sy2, sx2), (y, x), (0, 255, 255), 3) 381 | 382 | #cv2.line(img, (sy2, sx2), ()) 383 | 384 | send_result = "{:20}".format(direction) 385 | #print 'send result ' + `send_result` 386 | #connection_bot_var.send(send_result) 387 | 388 | arr[sx2][sy2] = (0, 255, 255) 389 | arr[dx][dy] = (0, 255, 255) 390 | ''' 391 | cv2.line(img, (sy2, sx2), (y, x), (255, 0, 0), 5) 392 | cv2.circle(img, (sy2, sx2), 2, (255, 255, 255), 1) 393 | cv2.circle(img, (y, x), 2, (255, 255, 255), 1) 394 | 395 | cv2.line(img, (sy2, sx2), (dy, dx), (255, 0, 0), 5) 396 | cv2.circle(img, (sy2, sx2), 2, (255, 255, 255), 1) 397 | cv2.circle(img, (dy, dx), 2, (255, 255, 255), 1) 398 | ''' 399 | cv2.imshow('img', img) 400 | cv2.waitKey(0) 401 | print sx2, sy2, x, y, dx, dy, direction*180/math.pi 402 | (sx1, sy1) = (sx2, sy2) 403 | 404 | cv2.imshow('image', img) 405 | cv2.imshow('arr', arr) 406 | cv2.waitKey(0) 407 | cv2.destroyAllWindows() 408 | main() -------------------------------------------------------------------------------- /Connection.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import socket 3 | 4 | class socket_connection: 5 | 6 | def connect_to_server(self,host,port): 7 | try: 8 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 9 | sock.connect((host,port)) 10 | return sock 11 | except: 12 | print "Unexpected error:", sys.exc_info()[0] 13 | print "Error in Connecting" 14 | 15 | def start_server(self,host,port): 16 | server_sock = socket.socket() # Create a socket object 17 | server_sock.bind((host, port)) # use host if u know about the IP address or manually give the IP address 18 | 19 | print 'Server Started' 20 | server_sock.listen(5) # Waiting for client 21 | socket_connection.c, addr = server_sock.accept() # Establish connection with client. 22 | print 'Got connection from', addr 23 | return socket_connection.c 24 | 25 | def read_data(self,sock): 26 | size = 2 27 | try: 28 | data = sock.recv(size) 29 | #print data 30 | return data 31 | except : 32 | print "Error in Recieving" 33 | 34 | def send_data(self,sock): 35 | # one way 36 | dat = raw_input('Enter Data : ') 37 | try: 38 | sock.send(dat) 39 | print dat 40 | except : 41 | print "Error in Sending" 42 | 43 | 44 | def __init__(self): 45 | print "connect_to_server(host,port)" 46 | print "start_server(host,port)" 47 | ''' 48 | try: 49 | s = socket_connection() 50 | soc = s.start_server('192.168.43.96',12346) 51 | 52 | while True: 53 | re = s.read_data(soc) 54 | print re 55 | if re == '0_0': 56 | break 57 | except KeyboardInterrupt: 58 | print 'broke' 59 | soc.close 60 | ''' 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Artificial-Potential-Field 2 | Implementation of Artificial Potential Field (Reactive Method of Motion Planing) 3 | 4 | For basics and working of Potential Field Motion Planning one can refer to http://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf 5 | 6 | This is basic implementation of potential field motion planning. Here we condsider our bot as positively charged body and goal as a negatively charged body and all obstacles as positively charge bodies. 7 | This way goal will attracts bot but obstacles will repel it from itself. Hence bot will reach to goal avoiding obstacles in these different potential fields. 8 | 9 | ``` 10 | Attractive force = 11 | - tau(q(current) -q(goal)), if d(q(current), d(q(goal), q(current)) <= d* 12 | - d*(tau*(q(current)-q(goal)))/d(q(current), q(goal)), if d(q(goal), q(current)) > d* 13 | 14 | Repulsive Force = 15 | - n(1/Q* - 1/D(q))*(1/D(q))^2* d'(q), if D(q) < Q* 16 | - 0, if D(q) >= Q* 17 | ``` 18 | 19 | Prerequisites 20 | - Python 21 | - OpenCV 22 | - Numpy 23 | - Matplotlib 24 | 25 | Input Images 26 | It will take all images in root folder as input images. 27 | 28 | Sample Input Images: 29 | 30 | ![Alt text](1.jpg?raw=true "Sample Image") 31 | 32 | ![Alt text](2.jpg?raw=true "Sample Image") 33 | 34 | Output Images: 35 | 36 | ![Alt text](output/1.jpg?raw=true "Sample Image") 37 | 38 | ![Alt text](output/2.jpg?raw=true "Sample Image") 39 | 40 | -------------------------------------------------------------------------------- /count.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/count.bmp -------------------------------------------------------------------------------- /count1.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/count1.bmp -------------------------------------------------------------------------------- /first.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import copy 4 | import glob 5 | import math 6 | import time 7 | from time import sleep 8 | import Queue as Q 9 | 10 | class pixel(object): 11 | def __init__(self, penalty, pointx, pointy): # parent is that pixel from which this current pixel is generated 12 | self.penalty = penalty 13 | self.pointx = int(pointx) 14 | self.pointy = int(pointy) 15 | 16 | def __cmp__(self, other): # comparable which will return self.penalty -1 and ny > -1 and nx < ex and ny < ey: 26 | return True 27 | else: 28 | return False 29 | 30 | def printx(x): 31 | #print x 32 | pass 33 | 34 | def check_obstacles(arr, ansx, ansy): 35 | if arr[ansx][ansy][0] == 255: 36 | return True 37 | else: 38 | return False 39 | 40 | def feasible(arr, x, y): 41 | ex, ey, ez = arr.shape 42 | x = int(x) 43 | y = int(y) 44 | 45 | if check_boundaries(ex, ey, x, y): 46 | return not check_obstacles(arr, x, y) 47 | else: 48 | return False 49 | 50 | def dist(sx, sy, x, y, theta, arr, q_star): #distance of obstacle in direction theta in radians 51 | ansx = sx 52 | ansy = sy 53 | flag = True 54 | printx('6') 55 | count = 0 56 | while True: 57 | if count > q_star: 58 | return (-1, -1) 59 | # print ansx, ansy, x, y, 180*theta/math.pi 60 | if theta <= math.pi: 61 | tx = int(math.ceil(math.sin(theta))) 62 | else: 63 | tx = int(math.floor(math.sin(theta))) 64 | 65 | count += 1 66 | if theta <= math.pi/2 and theta >= 3*math.pi/2: 67 | ty = int(math.ceil(math.cos(theta))) 68 | else: 69 | ty = int(math.floor(math.cos(theta))) 70 | ansx += tx 71 | ansy += ty 72 | #print tx, ty 73 | 74 | one = check_boundaries(x, y, ansx, ansy) 75 | if one == False: 76 | break 77 | else: 78 | two = check_obstacles(arr, ansx, ansy) 79 | if two == True: 80 | break 81 | 82 | 83 | 84 | printx('7') 85 | return (math.fabs(ansx-sx), math.fabs(ansy- sy)) 86 | 87 | 88 | 89 | def obstacle_force(arr, sx, sy, q_star): #sx,sy :- source dx, dy:- destination q-star:- threshold distance of obstacles 90 | forcex = 0 91 | forcey = 0 92 | neta = 30 93 | x, y , z= arr.shape 94 | printx('8') 95 | for i in range(8): 96 | (ox,oy) = dist(sx, sy, x, y, i*math.pi/4, arr, q_star) 97 | 98 | # print 'ox ', ox, 'oy ', oy 99 | fx = 0 100 | fy = 0 101 | if ox == -1 or oy == -1: 102 | fx = 0 103 | fy = 0 104 | else: 105 | if ox == 0: 106 | ox = 1 107 | if oy == 0: 108 | oy = 1 109 | fx = (neta*(1.0/q_star - 1.0/ox))/(ox*ox) 110 | fy = (neta*(1.0/q_star - 1.0/oy))/(oy*oy) 111 | forcex += fx 112 | forcey += fy 113 | printx('9') 114 | return (forcex, forcey) 115 | 116 | def goal_force(arr, sx, sy, dx, dy, d_star): # sx, sy :- source dx, dy:- destination d_star:- threshold distance from goal 117 | forcex = 0 118 | forcey = 0 119 | tau = 1 #constant 120 | printx('10') 121 | d = math.sqrt((dx-sx)*(dx-sx) + (dy-sy)*(dy-sy)) 122 | if d > d_star: 123 | forcex += ((d_star*tau)/math.sqrt(2)) 124 | forcey += ((d_star*tau)/math.sqrt(2)) 125 | else: 126 | forcex += (math.fabs(dx-sx)*tau) 127 | forcey += (math.fabs(dy-sy)*tau) 128 | 129 | printx('11') 130 | return (forcex, forcey) 131 | 132 | def path_planning(arr, sx1, sy1, dx, dy): 133 | if arr[sx1][sy1][0] == 255 or arr[dx][dy][0] == 255: 134 | return [] 135 | #print '3' 136 | v = 1 #velocity magnitude 137 | t = 1 #time lapse 138 | theta = 0 #initial angle 139 | x,y,z = arr.shape 140 | 141 | sx = sx1 142 | sy = sy1 143 | 144 | sol = [] 145 | sol.append((sx, sy)) 146 | q_star = 50 147 | d_star = 20 148 | 149 | sx += int(v*math.sin(theta)) 150 | sy += int(v*math.cos(theta)) 151 | sol.append((sx, sy)) 152 | 153 | ''' 154 | if Q and P are two vectors and @ is angle between them 155 | 156 | resultant ,R = (P^2 + R^2 + 2*P*Q cos @)^(1/2) 157 | 158 | resultant, theta = atan((Q*sin @)/(P+Q*cos @)) 159 | ''' 160 | 161 | printx ('4') 162 | while True: 163 | (fx, fy) = obstacle_force(arr, sx, sy, q_star) 164 | (gx, gy) = goal_force(arr, sx, sy, dx, dy, d_star) 165 | 166 | tx = gx+fx 167 | ty = gy+fy 168 | theta1 = math.atan2(tx, ty) 169 | #tx *= -1 170 | #ty *= -1 171 | print sx, sy, gx, gy, fx, fy 172 | 173 | if arr[sx][sy][0] == 255: 174 | print gx, gy, fx, fy 175 | print 'tx ', tx, ' ty ', ty, 'sx ', sx, ' sy ', sy 176 | print theta1*180/math.pi, theta*180/math.pi 177 | sleep(1) 178 | 179 | #theta1 = math.atan2(tx, ty) 180 | P = v 181 | angle = theta1-theta 182 | 183 | Q = math.sqrt(tx*tx + ty*ty) 184 | 185 | theta2 = math.atan2((Q*math.sin(angle)),((P + Q*math.cos(angle)))) #resultant angle with velocity 186 | 187 | theta += theta2 188 | 189 | theta = (theta + 2*math.pi)%(2*math.pi) 190 | 191 | #print theta, theta2 192 | t1 = 0.0 193 | t2 = 0.0 194 | if theta < math.pi and theta >= 0: 195 | sx += int(math.ceil(math.sin(theta))) 196 | else: 197 | sx += int(math.floor(math.sin(theta))) 198 | 199 | if theta <= math.pi/2 and theta > -1*math.pi/2: 200 | sy += int(math.ceil(math.cos(theta))) 201 | else: 202 | sy += int(math.floor(math.cos(theta))) 203 | 204 | # print theta 205 | 206 | 207 | if not check_boundaries(x, y, sx, sy): 208 | print 'out of boundaries' , sx, sy 209 | return sol 210 | 211 | if sx < dx+ 4 and sx > dx - 4 and sy < dy+4 and sy > dy-4: 212 | break 213 | 214 | sol.append((sx, sy)) 215 | 216 | printx('5') 217 | return sol 218 | 219 | def draw(theta, arr): 220 | x = 4 221 | y = 20 222 | ex, ey, ez = arr.shape 223 | while check_boundaries(ex, ey, x, y): 224 | arr[x][y] = (255, 255, 0) 225 | x += int(2*math.sin(theta)) 226 | y += int(2*math.cos(theta)) 227 | 228 | 229 | def obstacle(j, angle, cx, cy, arr): 230 | i = j+1 231 | while True: 232 | x = int(cx + i*math.sin(angle)) 233 | y = int(cy + i*math.cos(angle)) 234 | if not feasible(arr, x, y): 235 | break 236 | i += 1 237 | return i-j 238 | 239 | def path(arr, sx, sy, dx, dy): 240 | sol = [] 241 | rd = math.pi/8 #robot direction 242 | #robot size 243 | rx = 10 244 | ry = 10 245 | rspeed = 10 #robot speed 246 | mrspeed = 10 #max robot speed 247 | #s = 10 #safety distance 248 | macc = 10 #max acceleration 249 | maxt = 10*math.pi/180 #maximum turn 250 | dt = 30 #distance threshold 251 | k = 3 #degree of calculating potentials 252 | apscaling = 300000 #scaling factor for attractive potential 253 | rpscaling = 300000 #scaling factor for repulsive potential 254 | minApotential = 0.5 #minimum attractive potential 255 | rdiagonal = math.sqrt((rx/2)**2 + (ry/2)**2) 256 | 257 | ################################## 258 | ################################## 259 | 260 | ex, ey, ez = arr.shape #boundary values 261 | 262 | #current position 263 | cx = sx 264 | cy = sy 265 | ctheta = rd #current direction 266 | pathfound = False 267 | 268 | if sx <= -1 or sy <= -1 or sx >= ex or sy >= ey: 269 | print ('No solution exist as source is either on obstacle or outside boundary') 270 | return sol 271 | 272 | if dx <= -1 or dy <= -1 or dx >= ex or dy >= ey: 273 | print('No solution exist as goal is either on obstacle or outside boundary') 274 | return sol 275 | 276 | pi = math.pi 277 | while not pathfound: 278 | #calculate distance from obstacle at front 279 | i = rx/2+1 280 | while True: 281 | x = int(cx + i*math.sin(ctheta)) 282 | y = int(cy + i*math.cos(ctheta)) 283 | if not feasible(arr, x, y): 284 | break 285 | i += 1 286 | #df = i-rx/2 287 | df = obstacle(rx/2, ctheta, cx,cy, arr) 288 | 289 | #calculate distance from obstacle at left 290 | i = ry/2+1 291 | while True: 292 | x = int(cx + i*math.sin(ctheta-pi/2)) 293 | y = int(cy + i*math.cos(ctheta-pi/2)) 294 | if not feasible(arr, x, y): 295 | break 296 | i += 1 297 | #dl = i-ry/2 298 | dl = obstacle(ry/2, ctheta-pi/2, cx,cy, arr) 299 | 300 | #calculate distance from obstacle at right 301 | i = ry/2+1 302 | while True: 303 | x = int(cx + i*math.sin(ctheta+pi/2)) 304 | y = int(cy + i*math.cos(ctheta+pi/2)) 305 | if not feasible(arr, x, y): 306 | break 307 | i += 1 308 | #dr = i-ry/2 309 | dr = obstacle(ry/2, ctheta+pi/2, cx,cy, arr) 310 | #calculate distance from front-right-diagonal 311 | i = rdiagonal+1 312 | while True: 313 | x = int(cx + i*math.sin(ctheta+pi/4)) 314 | y = int(cy + i*math.cos(ctheta+pi/4)) 315 | if not feasible(arr, x, y): 316 | break 317 | i += 1 318 | #dfrd = i- rdiagonal 319 | dfrd = obstacle(rdiagonal, ctheta+pi/4, cx,cy, arr) 320 | #calculate distance from front-left-diagonal 321 | i = rdiagonal+1 322 | while True: 323 | x = int(cx + i*math.sin(ctheta-pi/4)) 324 | y = int(cy + i*math.cos(ctheta-pi/4)) 325 | if not feasible(arr, x, y): 326 | break 327 | i += 1 328 | #dfld = i- rdiagonal 329 | dfld = obstacle(rdiagonal, ctheta-pi/4, cx,cy, arr) 330 | #calculate angle and distance from goal 331 | gtheta = math.atan2(dx-cx, dy-cy) 332 | dg = math.sqrt((cx-dx)*(cx-dx)+ (cy-dy)*(cy-dy)) #distance from goal 333 | 334 | if dg < dt: 335 | pathfound = True 336 | 337 | #calculate Potetials : 338 | 339 | rPx = ((1.0/df)**k)*math.sin(ctheta) + ((1.0/dl)**k)*math.sin(ctheta-pi/2) + ((1.0/dr)**k)*math.sin(ctheta+pi/2) + ((1.0/dfld)**k)*math.sin(ctheta-pi/4) + ((1.0/dfrd)**k)*math.sin(ctheta+pi/4) 340 | rPy = ((1.0/df)**k)*math.cos(ctheta) + ((1.0/dl)**k)*math.cos(ctheta-pi/2) + ((1.0/dr)**k)*math.cos(ctheta+pi/2) + ((1.0/dfld)**k)*math.cos(ctheta-pi/4) + ((1.0/dfrd)**k)*math.cos(ctheta+pi/4) 341 | 342 | aPx = max(((1.0/dg)**k)*apscaling, minApotential)* math.sin(gtheta) 343 | aPy = max(((1.0/dg)**k)*apscaling, minApotential)* math.cos(gtheta) 344 | 345 | tPx = aPx-rPx*rpscaling 346 | tPy = aPy-rPy*rpscaling 347 | 348 | #calculate new direction 349 | ntheta = math.atan2(rspeed*math.sin(ctheta) + tPx, rspeed*math.cos(ctheta)+ tPy)-ctheta 350 | 351 | while ntheta > math.pi: 352 | ntheta -= 2*math.pi 353 | while ntheta < -math.pi: 354 | ntheta += 2*math.pi 355 | ntheta = min(maxt, ntheta) 356 | ntheta = max(-maxt, ntheta) 357 | 358 | ctheta += ntheta 359 | #print ctheta*180/pi 360 | 361 | #setting speed based on robot acceleration and speed 362 | speed = math.sqrt((rspeed*math.sin(ctheta)+tPx)**2+ (rspeed*math.cos(ctheta)+tPy)**2) 363 | speed = min(rspeed+macc, speed) 364 | rspeed = max(rspeed-macc, speed) 365 | rspeed = min(rspeed, mrspeed) 366 | rspeed = max(rspeed, 0) 367 | 368 | if rspeed == 0: 369 | print 'Robot can\'t move' 370 | return sol 371 | 372 | #calculatig new positions 373 | cx = cx + (rspeed*math.sin(ctheta)) 374 | cy = cy + (rspeed*math.cos(ctheta)) 375 | 376 | if not feasible(arr, cx, cy): 377 | sol.append((int(cx), int(cy))) 378 | print 'robot collides' 379 | return sol 380 | 381 | sol.append((int(cx), int(cy))) 382 | 383 | return sol 384 | 385 | count = 0 386 | def main(): 387 | for im in images: 388 | 389 | img = cv2.imread(im) 390 | 391 | cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 392 | img2 = cv2.medianBlur(cimg,13) 393 | 394 | ret,thresh1 = cv2.threshold(cimg,40,255,cv2.THRESH_BINARY) 395 | t2 = copy.copy(thresh1) 396 | 397 | x, y = thresh1.shape 398 | print x, y 399 | arr = np.zeros((x, y, 3), np.uint8) 400 | final_contours= [] 401 | image, contours, hierarchy = cv2.findContours(t2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 402 | for i in range(len(contours)): 403 | cnt = contours[i] 404 | if cv2.contourArea(cnt) > 300 and cv2.contourArea(cnt) < 5000 : 405 | cv2.drawContours(img, [cnt],-1, [0, 255, 255]) 406 | cv2.fillConvexPoly(arr, cnt, [255, 255, 255]) 407 | final_contours.append(cnt) 408 | print '1' 409 | arr1 = np.zeros((x, y, 3), np.uint8) 410 | for i in range(x): 411 | for j in range(y): 412 | if arr[i][j][0] ==255: 413 | arr1[i][j] = [0, 0, 0] 414 | else: 415 | arr1[i][j] = [255, 255, 255] 416 | 417 | cv2.imwrite('count.bmp', arr1) 418 | 419 | sx = 30 420 | sy = 50 421 | dx = 100 422 | dy = 200 423 | 424 | cmax = 50 425 | start = time.clock() 426 | #sol = path_planning(arr, sx, sy, dx, dy) 427 | sol = path(arr, sx, sy, dx, dy) 428 | #sol = [(100, 100),(100, 103), (100, 106), (100, 109), (100, 112), (100, 115)] 429 | if len(sol) == 0: 430 | print 'No solution exist ' 431 | continue 432 | print '2' 433 | for i in sol: 434 | # print arr[i[0], i[1]] 435 | print i[0], i[1] 436 | arr[i[0], i[1]] = (255, 255, 0) 437 | img[i[0], i[1]] = (255, 0, 255) 438 | 439 | print 'time: ', time.clock()-start 440 | arr[sx][sy] = (0, 255, 255) 441 | arr[dx][dy] = (0, 255, 255) 442 | # cv2.circle(arr, (sol[len(sol)-1][1], sol[len(sol)-1][0]), 5, (0, 0, 255)) 443 | # cv2.circle(arr, (sx, sy), 6, (0, 255, 255)) 444 | # cv2.circle(arr, (dx, dy), 6, (0, 255, 255)) 445 | 446 | #draw(math.pi/4, arr) 447 | cv2.imshow('image', img) 448 | cv2.imshow('arr', arr) 449 | cv2.waitKey(0) 450 | cv2.destroyAllWindows() 451 | main() -------------------------------------------------------------------------------- /goal.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/goal.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_22_30_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_22_30_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_22_59_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_22_59_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_23_11_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_23_11_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_23_30_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_23_30_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_23_37_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_23_37_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_23_46_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_23_46_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_24_13_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_24_13_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_24_59_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_24_59_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_25_06_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_25_06_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_25_10_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_25_10_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_25_14_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_25_14_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_25_20_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_25_20_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_25_36_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_25_36_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_26_48_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_26_48_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_26_52_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_26_52_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_26_55_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_26_55_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_26_58_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_26_58_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_27_02_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_27_02_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_27_10_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_27_10_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_27_19_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_27_19_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_28_49_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_28_49_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_28_52_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_28_52_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_28_55_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_28_55_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_28_57_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_28_57_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_29_19_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_29_19_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_29_29_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_29_29_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_29_43_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_29_43_Pro.jpg -------------------------------------------------------------------------------- /images/WIN_20151203_18_30_01_Pro.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/images/WIN_20151203_18_30_01_Pro.jpg -------------------------------------------------------------------------------- /output/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/1.jpg -------------------------------------------------------------------------------- /output/2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/2.jpg -------------------------------------------------------------------------------- /output/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/3.jpg -------------------------------------------------------------------------------- /output/4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/4.jpg -------------------------------------------------------------------------------- /output/5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/5.jpg -------------------------------------------------------------------------------- /output/6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output/6.jpg -------------------------------------------------------------------------------- /output3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output3.jpg -------------------------------------------------------------------------------- /output31.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output31.jpg -------------------------------------------------------------------------------- /output33.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/output33.jpg -------------------------------------------------------------------------------- /robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/robot.jpg -------------------------------------------------------------------------------- /sample.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/sample.jpg -------------------------------------------------------------------------------- /sample1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/sample1.jpg -------------------------------------------------------------------------------- /save1.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/save1.bmp -------------------------------------------------------------------------------- /save1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/save1.jpg -------------------------------------------------------------------------------- /save_sample_image.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | cap = cv2.VideoCapture(1) 4 | _, im = cap.read() 5 | 6 | cv2.imwrite('sample1.jpg', im) -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vampcoder/Artificial-Potential-Field/972256d94122ee2928eb1fe0af15a57a82a2ff2b/test.py --------------------------------------------------------------------------------