# Planning # Dynamic Window Approach (Local Planning) # Andrew Davison 2017 import pygame, os, math, time, random from pygame.locals import * pygame.init() # Constants and variables # Units here are in metres and radians using our standard coordinate frame BARRIERRADIUS = 0.06 ROBOTRADIUS = 0.15 W = 2 * ROBOTRADIUS # width of robot SAFEDIST = 0.2 # used in the cost function for avoiding obstacles MAXVELOCITY = 0.5 #ms^(-1) max speed of each wheel MAXACCELERATION = 0.5 #ms^(-2) max rate we can change speed of each wheel # The region we will fill with obstacles PLAYFIELDCORNERS = (-3.0, -3.0, 3.0, 3.0) # Set an initial target location which is beyond the obstacles target = (PLAYFIELDCORNERS[2] + 1.0, 0) # Starting pose of robot x = PLAYFIELDCORNERS[0] - 0.5 y = 0.0 theta = 0.0 # Starting wheel velocities vL = 0.00 vR = 0.00 # Timestep delta to run control and simulation at dt = 0.1 # Barrier (obstacle) locations barriers = [] # barrier contents are (bx, by, visibilitymask) # Generate some initial random barriers for i in range(2): (bx, by) = (random.uniform(PLAYFIELDCORNERS[0], PLAYFIELDCORNERS[2]), random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3])) barrier = [bx, by, 0] barriers.append(barrier) # Constants for graphics display # Transformation from metric world frame to graphics frame # k pixels per metre # Horizontal screen coordinate: u = u0 + k * x # Vertical screen coordinate: v = v0 - k * y # set the width and height of the screen (pixels) WIDTH = 1500 HEIGHT = 1000 size = [WIDTH, HEIGHT] black = (20,20,40) lightblue = (0,120,255) darkblue = (0,40,160) red = (255,100,0) white = (255,255,255) blue = (0,0,255) k = 160 # pixels per metre for graphics # Screen centre will correspond to (x, y) = (0, 0) u0 = WIDTH / 2 v0 = HEIGHT / 2 # Initialise Pygame display screen screen = pygame.display.set_mode(size) # This makes the normal mouse pointer invisible in graphics window pygame.mouse.set_visible(0) # Array for path choices use for graphics pathstodraw = [] # Function to predict new robot position based on current pose and velocity controls # Uses time deltat in future # Returns xnew, ynew, thetanew # Also returns path. This is just used for graphics, and returns some complicated stuff # used to draw the possible paths during planning. Don't worry about the details of that. def predictPosition(vL, vR, x, y, theta, deltat): # Simple special cases # Straight line motion if (vL == vR): xnew = x + vL * deltat * math.cos(theta) ynew = y + vL * deltat * math.sin(theta) thetanew = theta path = (0, vL * deltat) # 0 indicates pure translation # Pure rotation motion elif (vL == -vR): xnew = x ynew = y thetanew = theta + ((vR - vL) * deltat / W) path = (1, 0) # 1 indicates pure rotation else: # Rotation and arc angle of general circular motion # Using equations given in Lecture 2 R = W / 2.0 * (vR + vL) / (vR - vL) deltatheta = (vR - vL) * deltat / W xnew = x + R * (math.sin(deltatheta + theta) - math.sin(theta)) ynew = y - R * (math.cos(deltatheta + theta) - math.cos(theta)) thetanew = theta + deltatheta # To calculate parameters for arc drawing (complicated Pygame stuff, don't worry) # We need centre of circle (cx, cy) = (x - R * math.sin(theta), y + R * math.cos (theta)) # Turn this into Rect Rabs = abs(R) ((tlx, tly), (Rx, Ry)) = ((int(u0 + k * (cx - Rabs)), int(v0 - k * (cy + Rabs))), (int(k * (2 * Rabs)), int(k * (2 * Rabs)))) if (R > 0): start_angle = theta - math.pi/2.0 else: start_angle = theta + math.pi/2.0 stop_angle = start_angle + deltatheta path = (2, ((tlx, tly), (Rx, Ry)), start_angle, stop_angle) # 2 indicates general motion return (xnew, ynew, thetanew, path) # Function to calculate the closest obstacle at a position (x, y) # Used during planning def calculateClosestObstacleDistance(x, y): closestdist = 100000.0 # Calculate distance to closest obstacle for barrier in barriers: # Is this a barrier we know about? barrier[2] flag is set when sonar observes it if(barrier[2] == 1): dx = barrier[0] - x dy = barrier[1] - y d = math.sqrt(dx**2 + dy**2) # Distance between closest touching point of circular robot and circular barrier dist = d - BARRIERRADIUS - ROBOTRADIUS if (dist < closestdist): closestdist = dist return closestdist # Draw the barriers on the screen def drawBarriers(barriers): for barrier in barriers: # Dark barriers we haven't seen if(barrier[2] == 0): pygame.draw.circle(screen, darkblue, (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0) # Bright barriers we have seen else: pygame.draw.circle(screen, lightblue, (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0) return # Simulation of forward looking depth sensor; assume cone beam # Which barriers can it see? If a barrier has been seen at least once it becomes known to the planner SENSORRANGE = 1.5 SENSORHALFANGLE = 15.0 * math.pi / 180.0 def observeBarriers(x, y, theta): for i, barrier in enumerate(barriers): vector = (barrier[0] - x, barrier[1] - y) vectorangle = math.atan2(vector[1], vector[0]) vectorlength = math.sqrt(vector[0]**2 + vector[1]**2) anglediff = vectorangle - theta while(anglediff < -math.pi): anglediff = anglediff + 2 * math.pi while(anglediff > math.pi): anglediff = anglediff - 2 * math.pi # compare anglediff with arc length barrier presents at the distance it is away barrierangularhalfwidth = BARRIERRADIUS / vectorlength if(abs(anglediff) - SENSORHALFANGLE < barrierangularhalfwidth and vectorlength < SENSORRANGE): barriers[i][2] = 1 # Main loop while(1): # Check if any new barriers are visible from current pose observeBarriers(x, y, theta) # Planning # We want to find the best benefit where we have a positive component for closeness to target, # and a negative component for closeness to obstacles, for each of a choice of possible actions bestBenefit = -100000 FORWARDWEIGHT = 12 OBSTACLEWEIGHT = 16 # Range of possible motions: each of vL and vR could go up or down a bit vLpossiblearray = (vL - MAXACCELERATION * dt, vL, vL + MAXACCELERATION * dt) vRpossiblearray = (vR - MAXACCELERATION * dt, vR, vR + MAXACCELERATION * dt) #print "New action" pathstodraw = [] # We will store path details here for plotting later newpositionstodraw = [] # Also for possible plotting of robot end positions for vLpossible in vLpossiblearray: for vRpossible in vRpossiblearray: # We can only choose an action if it's within velocity limits if (vLpossible <= MAXVELOCITY and vRpossible <= MAXVELOCITY and vLpossible >= -MAXVELOCITY and vRpossible >= -MAXVELOCITY): # Predict new position in TAU seconds TAU = 1.5 (xpredict, ypredict, thetapredict, path) = predictPosition(vLpossible, vRpossible, x, y, theta, TAU) pathstodraw.append(path) newpositionstodraw.append((xpredict, ypredict)) # What is the distance to the closest obstacle from this possible position? distanceToObstacle = calculateClosestObstacleDistance(xpredict, ypredict) # Calculate how much close we've moved to target location previousTargetDistance = math.sqrt((x - target[0])**2 + (y - target[1])**2) newTargetDistance = math.sqrt((xpredict - target[0])**2 + (ypredict - target[1])**2) distanceForward = previousTargetDistance - newTargetDistance # Alternative: how far have I moved forwards? # distanceForward = xpredict - x # Positive benefit distanceBenefit = FORWARDWEIGHT * distanceForward # Negative cost: once we are less than SAFEDIST from collision, linearly increasing cost if (distanceToObstacle < SAFEDIST): obstacleCost = OBSTACLEWEIGHT * (SAFEDIST - distanceToObstacle) else: obstacleCost = 0.0 # Total benefit function to optimise benefit = distanceBenefit - obstacleCost if (benefit > bestBenefit): vLchosen = vLpossible vRchosen = vRpossible bestBenefit = benefit vL = vLchosen vR = vRchosen # Planning is finished; now do graphics screen.fill(black) drawBarriers(barriers) # Target location pygame.draw.circle(screen, red, (int(u0 + k * target[0]), int(v0 - k * target[1])), int(k * ROBOTRADIUS), 0) # Draw robot u = u0 + k * x v = v0 - k * y pygame.draw.circle(screen, white, (int(u), int(v)), int(k * ROBOTRADIUS), 3) # Draw wheels as little blobs so you can see robot orientation # left wheel centre wlx = x - (W/2.0) * math.sin(theta) wly = y + (W/2.0) * math.cos(theta) ulx = u0 + k * wlx vlx = v0 - k * wly WHEELBLOB = 0.04 pygame.draw.circle(screen, blue, (int(ulx), int(vlx)), int(k * WHEELBLOB)) # right wheel centre wrx = x + (W/2.0) * math.sin(theta) wry = y - (W/2.0) * math.cos(theta) urx = u0 + k * wrx vrx = v0 - k * wry pygame.draw.circle(screen, blue, (int(urx), int(vrx)), int(k * WHEELBLOB)) # Draw paths: little arcs which show the different paths the robot is selecting between # A bit complicated so don't worry about the details! for path in pathstodraw: #if path[0] = 1: # Pure rotation: nothing to draw if path[0] == 0: # Straight line straightpath = path[1] linestart = (u0 + k * x, v0 - k * y) lineend = (u0 + k * (x + straightpath * math.cos(theta)), v0 - k * (y + straightpath * math.sin(theta))) pygame.draw.line(screen, (0, 200, 0), linestart, lineend, 1) if path[0] == 2: # General case: circular arc # path[2] and path[3] are start and stop angles for arc but they need to be in the right order to pass if (path[3] > path[2]): startangle = path[2] stopangle = path[3] else: startangle = path[3] stopangle = path[2] # Pygame arc doesn't draw properly unless angles are positive if (startangle < 0): startangle += 2*math.pi stopangle += 2*math.pi if (path[1][1][0] != 0): pygame.draw.arc(screen, (0, 200, 0), path[1], startangle, stopangle, 1) # Uncomment to also draw circles for predicted end positions of robot #for newposition in newpositionstodraw: #u = u0 + k * newposition[0] #v = v0 - k * newposition[1] #pygame.draw.circle(screen, (0,100,0), (int(u), int(v)), int(k * ROBOTRADIUS), 3) #time.sleep(1.0) #pygame.display.flip() # Update display pygame.display.flip() # Actually now move robot based on chosen vL and vR (x, y, theta, tmppath) = predictPosition(vL, vR, x, y, theta, dt) # Wraparound: check if robot has reached target; if so reset it to the other side, randomise # target position and add some more barriers to go again disttotarget = math.sqrt((x - target[0])**2 + (y - target[1])**2) if (disttotarget < 0.05): # Reset robot pose x = PLAYFIELDCORNERS[0] - 0.5 y = random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3]) theta = 0 # Reset barrier visibility for i, barrier in enumerate(barriers): barriers[i][2] = 0 # Add new barriers for i in range(2): (bx, by) = (random.uniform(PLAYFIELDCORNERS[0], PLAYFIELDCORNERS[2]), random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3])) barrier = [bx, by, 0] barriers.append(barrier) # Randomise target target = (target[0], random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3])) # Sleeping dt here runs simulation in real-time time.sleep(dt / 5)