コード例 #1
0
def setup():
    robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
    startTheta = io.SensorInput().odometry.theta
    # static plot of robot angle (from start) vs time
    robot.gfx.addStaticPlotFunction(
        y=('angle', lambda inp: util.fixAnglePlusMinusPi(
                (inp.odometry.theta-startTheta))))
コード例 #2
0
ファイル: move.py プロジェクト: luxuriance19/development
def actionToPoint(goalPoint, robotPose, forwardGain, rotationGain, maxVel,
                  angleEps):
    """
    Internal procedure that returns an action to take to drive
    toward a specified goal point.
    """
    rvel = 0
    fvel = 0
    robotPoint = robotPose.point()

    # Compute the angle between the robot and the goal point
    headingTheta = robotPoint.angleTo(goalPoint)

    # Difference between the way the robot is currently heading and
    # the angle to the goal.  This is an angular error relative to the
    # robot's current heading, in the range +pi to -pi.
    headingError = util.fixAnglePlusMinusPi(headingTheta - robotPose.theta)

    if util.nearAngle(robotPose.theta, headingTheta, angleEps):
        # The robot is pointing toward the goal, so it's okay to move
        # forward.  Note that we are actually doing two proportional
        # controllers simultaneously:  one to reduce angular error
        # and one to reduce distance error.
        distToGoal = robotPoint.distance(goalPoint)
        fvel = distToGoal * forwardGain
        rvel = headingError * rotationGain
    else:
        # The robot is not pointing close enough to the goal, so don't
        # start moving foward yet.  This is a proportional controller
        # to reduce angular error.
        rvel = headingError * rotationGain
    return io.Action(fvel=util.clip(fvel, -maxVel, maxVel),
                     rvel=util.clip(rvel, -maxVel, maxVel))
コード例 #3
0
def step():
    global confident, targetX, targetTheta
    inp = io.SensorInput()
    sonars = inp.sonars

    # current discretized sonar reading
    left = discretize(sonars[0], sonarMax / numObservations,
                      numObservations - 1)

    if not confident:
        # GRAPHICS
        if robot.g is not None:
            # update observation model graph
            robot.g.updateObsLabel(left)
            robot.g.updateObsGraph(
                [obsModel(s).prob(left) for s in xrange(numStates)])

        if DO_ESTIMATION:
            # update belief state
            robot.estimator.update(left)

        (location, confident) = confidentLocation(robot.estimator.belief)

        # GRAPHICS
        if robot.g is not None:
            # update belief graph
            robot.g.updateBeliefGraph(
                [robot.estimator.belief.prob(s) for s in xrange(numStates)])
        if confident:
            targetX = (parkingSpot - location) * (
                xMax - xMin) / float(numStates) + inp.odometry.x
            print 'I am at x =', location, ': proceeding to parking space'

        # DL6 Angle Controller
        (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars)
        if not theta:
            theta = 0
            print 'Angle too large!'
        e = desiredRight - distanceRight
        ROTATIONAL_VELOCITY = Kp * e - Ka * theta

        io.setForward(FORWARD_VELOCITY)
        io.setRotational(ROTATIONAL_VELOCITY)
    else:
        inp.odometry.theta = util.fixAnglePlusMinusPi(inp.odometry.theta)
        if inp.odometry.x > targetX + .05 and abs(
                inp.odometry.theta) < math.pi / 4:
            io.Action(fvel=-0.2,
                      rvel=0).execute()  #drive backwards if necessary
        elif inp.odometry.x < targetX and abs(
                inp.odometry.theta) < math.pi / 4:
            io.Action(fvel=0.2, rvel=0).execute()  #drive to desired x location
        elif inp.odometry.theta < targetTheta - .05:
            io.Action(fvel=0, rvel=0.2).execute()  #rotate
        elif inp.sonars[3] > .3:
            io.Action(fvel=0.2, rvel=0).execute()  #drive into spot
        else:
            io.Action(fvel=0, rvel=0).execute(
            )  #congratulate yourself (or call insurance company)
コード例 #4
0
def setup():
    robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
    startTheta = io.SensorInput().odometry.theta
    # static plot of robot angle vs eye voltage
    robot.gfx.addStaticPlotFunction(x=('angle',
                                       lambda inp: util.fixAnglePlusMinusPi(
                                           (inp.odometry.theta - startTheta))),
                                    y=('eye voltage',
                                       lambda inp: inp.analogInputs[1]))
コード例 #5
0
def setup():
    robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
    startTheta = io.SensorInput().odometry.theta
    # static plot of robot angle vs left eye, right eye and difference
    # assumes that left is analog input #2, right is analog input #3
    robot.gfx.addStaticPlotFunction(
        x=('angle', lambda inp: util.fixAnglePlusMinusPi(
         (inp.odometry.theta-startTheta))),
        y=('left', lambda inp: inp.analogInputs[1]))
    robot.gfx.addStaticPlotFunction(
        x=('angle', lambda inp: util.fixAnglePlusMinusPi(
         (inp.odometry.theta-startTheta))),
        y=('right', lambda inp: inp.analogInputs[2]))
    robot.gfx.addStaticPlotFunction(
        x=('angle', lambda inp: util.fixAnglePlusMinusPi(
         (inp.odometry.theta-startTheta))),
        y=('diff', lambda inp: inp.analogInputs[1] - \
               inp.analogInputs[2]))
コード例 #6
0
ファイル: angleBrain.py プロジェクト: lasernite/6.01
def computePlotValues(rotationalVelocity):
    robot.rvels.append(rotationalVelocity)
    current = time.time()
    if robot.pTi is not None:
        dT = current-robot.pTi
    else:
        dT = 0.1
    robot.pTi = current
    t = io.getPosition()[2]
    if robot.pTh is not None:
        robot.rvels2.append(util.fixAnglePlusMinusPi(t-robot.pTh)/dT)
    robot.pTh = t
コード例 #7
0
ファイル: mazeBrain.py プロジェクト: udgam/6.01-Code
def step():
    x, y, theta = io.getPosition()
    robot.slimeX.append(x)
    robot.slimeY.append(y)

    # the following lines compute the robot's current position and angle
    currentPoint = util.Point(x,y).add(robot.initialLocation)
    currentAngle = util.fixAnglePlusMinusPi(theta)

    fv, rv = driver.drive(robot.path, currentPoint, currentAngle)
    io.setForward(fv)
    io.setRotational(rv)
コード例 #8
0
ファイル: move.py プロジェクト: luxuriance19/development
def actionToPose(goalPose, robotPose, forwardGain, rotationGain, maxVel,
                 angleEps, distEps):
    """
    Internal procedure that returns an action to take to drive
    toward a specified goal pose.
    """
    if robotPose.distance(goalPose) < distEps:
        finalRotError = util.fixAnglePlusMinusPi(goalPose.theta -
                                                 robotPose.theta)
        return io.Action(rvel=finalRotError * rotationGain)
    else:
        return actionToPoint(goalPose.point(), robotPose, forwardGain,
                             rotationGain, maxVel, angleEps)
コード例 #9
0
def step():
    global confident, targetX, targetTheta
    inp = io.SensorInput()
    sonars = inp.sonars

    # current discretized sonar reading
    left = discretize(sonars[0], sonarMax/numObservations, numObservations-1)
   
    if not confident:
        # GRAPHICS
        if robot.g is not None:
            # update observation model graph
            robot.g.updateObsLabel(left)
            robot.g.updateObsGraph([obsModel(s).prob(left) for s in xrange(numStates)])

        if DO_ESTIMATION:
            # update belief state
            robot.estimator.update(left)

        (location, confident) = confidentLocation(robot.estimator.belief)

        # GRAPHICS
        if robot.g is not None:
            # update belief graph
            robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) for s in xrange(numStates)])
        if confident:
            targetX = (parkingSpot-location)*(xMax-xMin)/float(numStates)+inp.odometry.x
            print 'I am at x =',location,': proceeding to parking space'
        
        # DL6 Angle Controller
        (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars)
        if not theta:
           theta = 0
           print 'Angle too large!'
        e = desiredRight-distanceRight
        ROTATIONAL_VELOCITY = Kp*e - Ka*theta

        io.setForward(FORWARD_VELOCITY)
        io.setRotational(ROTATIONAL_VELOCITY)
    else:
        inp.odometry.theta = util.fixAnglePlusMinusPi(inp.odometry.theta)
        if inp.odometry.x>targetX+.05 and abs(inp.odometry.theta)<math.pi/4:
            io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary
        elif inp.odometry.x<targetX and abs(inp.odometry.theta)<math.pi/4:
            io.Action(fvel=0.2,rvel=0).execute()  #drive to desired x location
        elif inp.odometry.theta<targetTheta-.05:
            io.Action(fvel=0,rvel=0.2).execute()  #rotate
        elif inp.sonars[3]>.3:
            io.Action(fvel=0.2,rvel=0).execute()  #drive into spot
        else:
            io.Action(fvel=0,rvel=0).execute()  #congratulate yourself (or call insurance company)
コード例 #10
0
ファイル: mazeBrain.py プロジェクト: lasernite/6.01
def step():
    global i
    x, y, theta = io.getPosition()
    robot.slimeX.append(x)
    robot.slimeY.append(y)
    
    currentPoint = util.Point(x,y).add(robot.initialLocation)
    currentAngle = theta
    destinationPoint = robot.path[i]
    thetad = currentPoint.angleTo(destinationPoint)
    
    if util.nearAngle(currentAngle,thetad,math.pi/180.0):
        io.setForward(0.1)
        io.setRotational(0)
        if currentPoint.distance(destinationPoint) < 0.02:
            i += 1
            print i
    else:
        theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle)
        io.setRotational(theta_constant)
        io.setForward(0)
コード例 #11
0
def step():
    global i
    x, y, theta = io.getPosition()
    robot.slimeX.append(x)
    robot.slimeY.append(y)

    currentPoint = util.Point(x, y).add(robot.initialLocation)
    currentAngle = theta
    destinationPoint = robot.path[i]
    thetad = currentPoint.angleTo(destinationPoint)

    if util.nearAngle(currentAngle, thetad, math.pi / 180.0):
        io.setForward(0.1)
        io.setRotational(0)
        if currentPoint.distance(destinationPoint) < 0.02:
            i += 1
            print i
    else:
        theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle)
        io.setRotational(theta_constant)
        io.setForward(0)
コード例 #12
0
ファイル: mazeBrain.py プロジェクト: udgam/6.01-Code
def step():
    global inp
    robot.count += 1
    inp = io.SensorInput(cheat=True)


    # discretize sonar readings
    # each element in discreteSonars is a tuple (d, cells)
    # d is the distance measured by the sonar
    # cells is a list of grid cells (r,c) between the sonar and the point d meters away
    discreteSonars = []
    for (sonarPose, distance) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            distance = noiseModel.noisify(distance, gridSquareSize)
        sensorIndices = robot.map.pointToIndices(inp.odometry.transformPose(sonarPose).point())
        hitIndices = robot.map.pointToIndices(sonarDist.sonarHit(distance, sonarPose, inp.odometry))
        ray = util.lineIndices(sensorIndices, hitIndices)
        discreteSonars.append((distance, ray))


    # figure out where robot is
    startPoint = inp.odometry.point()
    startCell = robot.map.pointToIndices(startPoint)


    # if necessary, make new plan
    if robot.plan is None:
        print 'REPLANNING'
        robot.plan = search.ucSearch(robot.successors,
                              robot.map.pointToIndices(inp.odometry.point()),
                              lambda x: x == robot.goalIndices,
                              lambda x: 0)
    isGood = True
    for i in robot.plan:
        if not robot.map.isPassable(i) and isGood:
            print 'REPLANNING'
            robot.plan = search.ucSearch(robot.successors,
                                  robot.map.pointToIndices(inp.odometry.point()),
                                  lambda x: x == robot.goalIndices,
                                  lambda x: 0)
            isGood = False


    # graphics (draw robot's plan, robot's location, goalPoint)
    # do not change this block
    for w in robot.windows:
        w.redrawWorld()
    robot.windows[-1].markCells(robot.plan,'blue')
    robot.windows[-1].markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
    robot.windows[-1].markCell(robot.map.pointToIndices(goalPoint),'green')


    # update map
    for (d,cells) in discreteSonars:
        if d != 5.0:
            robot.map.sonarHit(cells[-1])


    # if we are within 0.1m of the goal point, we are done!
    if startPoint.distance(goalPoint) <= 0.1:
        io.Action(fvel=0,rvel=0).execute()
        code = checkoff.generate_code(globals())
        raise Exception('Goal Reached!\n\n%s' % code)

    # otherwise, figure out where to go, and drive there
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while startPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    currentAngle = inp.odometry.theta
    angleDiff = startPoint.angleTo(destinationPoint)-currentAngle
    angleDiff = util.fixAnglePlusMinusPi(angleDiff)
    fv = rv = 0
    if startPoint.distance(destinationPoint) > 0.1:
        if abs(angleDiff) > 0.1:
            rv = angleDiff
        else:
            fv = 1.5*startPoint.distance(destinationPoint)
    io.setForward(fv)
    io.setRotational(rv)


    # render the drawing windows
    # do not change this block
    for w in robot.windows:
        w.render()
コード例 #13
0
ファイル: mazeBrain.py プロジェクト: jmptable/mit_6.01_mapbot
def step():
	robot.count += 1
	inp = io.SensorInput(cheat=True)
	for c in ('orange','cyan','blue','red'):
		robot.map.clearColor(c)

	# discretize sonar readings
	# each element in discreteSonars is a tuple (d, cells)
	# d is the distance measured by the sonar
	# cells is a list of grid cells (r,c) between the sonar and the point d meters away
	discreteSonars = []
	for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
		if NOISE_ON:
			d = noiseModel.noisify(d,gridSquareSize)
		discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
	
	# update map
	for (d,cells) in discreteSonars:
		# update probabilities
		if(d<=1.5):
			robot.mentalmap[cells[-1]].update(True)
			for c in cells[:-1]:
				robot.mentalmap[c].update(False)
		else:
			for i in range(0, len(cells)/4):
				robot.mentalmap[cells[i]].update(False)

		# update the map
		for c in cells:
			belief = robot.mentalmap[c].belief
			state = belief.maxProbElt()
			if(state):
				robot.map.sonarHit(c)
			else:
				robot.map.sonarPass(c)

	# null the path if necessary
	if not robot.plan is None:
		for pt in robot.plan:
			if(not robot.map.isPassable(pt)):
				robot.plan = None
				break

	if robot.plan is None:
		print 'REPLANNING'
		robot.dirty = True
		robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
							  robot.map.pointToIndices(inp.odometry.point()),None,0), 
							  lambda x: x == robot.map.pointToIndices(goalPoint), 
							  lambda x: 0)

	# graphics (draw robot's plan, robot's location, goalPoint)
	# do not change this block
	robot.map.markCells(robot.plan,'blue')
	robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
	robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

	# move to target point (similar to driving task in DL2)
	currentPoint = inp.odometry.point()
	currentAngle = inp.odometry.theta

	destinationPoint = robot.map.indicesToPoint(robot.plan[0])
	
	# check if we are ahead in the path
	if(robot.dirty):
		for i in range(0, len(robot.plan)):
			if(robot.map.pointToIndices(currentPoint)==robot.plan[i]):
				destination = robot.map.indicesToPoint(robot.plan[i+1])

	while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
		robot.plan.pop(0)
		destinationPoint = robot.map.indicesToPoint(robot.plan[0])

	if not currentPoint.isNear(destinationPoint,0.1):
		angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
		if abs(angle)<0.1:
			#if close enough to pointing, use proportional control on forward velocity
			fv = SPEED*currentPoint.distance(destinationPoint)
			rv = 0
		else:
			#otherwise, use proportional control on angular velocity
			fv = 0
			rv = ANG_SPEED*angle
	else:
		t = time.time() - robot.startTime;o=inp.odometry.xytTuple();w=THE_WORLD[:1]+[THE_WORLD[1].xyTuple()]+THE_WORLD[2:];n=NOISE_ON
		raise Exception, 'Goal Reached!\n\n%s' % e(repr((t,o,w,n)))
	robot.map.update()
	io.Action(fvel=fv,rvel=rv).execute()

	robot.dirty = True
コード例 #14
0
ファイル: localizeBrain.py プロジェクト: lasernite/6.01
def step():
    global confident, targetX, targetTheta
    sonars = io.getSonars()
    pose = io.getPosition(True)
    (px, py, ptheta) = pose

    if confident:
        ptheta = util.fixAnglePlusMinusPi(ptheta)
        if px>targetX+.05 and abs(ptheta)<math.pi/4:
            io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary
        elif px<targetX and abs(ptheta)<math.pi/4:
            io.Action(fvel=0.2,rvel=0).execute()  #drive to desired x location
        elif ptheta<targetTheta-.05:
            io.Action(fvel=0,rvel=0.2).execute()  #rotate
        elif sonars[3]>.3:
            io.Action(fvel=0.2,rvel=0).execute()  #drive into spot
        else:
            io.Action(fvel=0,rvel=0).execute()  #congratulate yourself (or call insuran
        return

    
    # Quality metric.  Important to do this before we update the belief state, because
    # it is always a prediction
    parkingSpaceSize = .75
    robotWidth = 0.3
    margin = (parkingSpaceSize - robotWidth) / 2
    # Robot is about .3 meters wide.  Parking place is .75
    trueX = io.getPosition(True)[0]
    robot.probMeasures.append(estimateQualityMeasure(robot.estimator.belief,
                                                     xMin, xMax, numStates, margin,
                                                     trueX))
    trueS = discretize(trueX, (xMax - xMin)/numStates, valueMin = xMin)
    n = len(robot.probMeasures)
    
    if n == 80:
        brainStop()
    
    # current discretized sonar reading
    left = discretize(sonars[0], sonarMax/numObservations, numObservations-1)
    robot.data.append((trueS, ideal[trueS], left))
    # obsProb
    obsProb = sum([robot.estimator.belief.prob(s) * OBS_MODEL_TO_USE(s).prob(left) \
                   for s in xrange(numStates)])

    # GRAPHICS
    if robot.g is not None:
        # redraw ideal distances (compensating for tk bug on macs)
        # draw robot's true state
        if trueS < numStates:
            robot.g.updateDist()
            robot.g.updateTrueRobot(trueS)
        # update observation model graph
        robot.g.updateObsLabel(left)
        robot.g.updateObsGraph([OBS_MODEL_TO_USE(s).prob(left) \
                                for s in xrange(numStates)])

    robot.estimator.update(left)
    (location, confident) = confidentLocation(robot.estimator.belief)

    if confident:
        targetX = (parkingSpot-location)*(xMax-xMin)/float(numStates) + px
        print 'I am at x =',location,': proceeding to parking space'

    # GRAPHICS
    if robot.g is not None:
        # update world drawing
        # update belief graph
        robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) \
                                   for s in xrange(numStates)])

    # DL6 Angle Controller
    (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars)
    if not theta:
       theta = 0
    e = desiredRight-distanceRight
    ROTATIONAL_VELOCITY = Kp*e - Ka*theta
    io.setForward(FORWARD_VELOCITY)
    io.setRotational(ROTATIONAL_VELOCITY)
コード例 #15
0
ファイル: mazeBrain 2.py プロジェクト: walterm/6.S078-A1
def step():
    robot.count += 1
    inp = io.SensorInput(cheat=True)
    for c in ('orange','cyan','blue','red'):
        robot.map.clearColor(c)

    # discretize sonar readings
    # each element in discreteSonars is a tuple (d, cells)
    # d is the distance measured by the sonar
    # cells is a list of grid cells (r,c) between the sonar and the point d meters away
    discreteSonars = []
    for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            r = random.random()
            if d != 5:
                if r > .99:
                    d = 5
                elif r > .97:
                    d = random.uniform(gridSquareSize,d)
            else:
                if r > .98:
                    d = random.uniform(gridSquareSize,1.5)
        discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
    
    # update map
    for (d,cells) in discreteSonars:
        if d != 5:
            robot.map.set(cells[-1])
            for index in range(0, len(cells)-1):
                robot.map.clear(cells[index])
        

    # if necessary, make new plan
    if robot.plan is None or not all(robot.map.isPassable(i) for i in robot.plan):
        print 'REPLANNING'
        robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
                              robot.map.pointToIndices(inp.odometry.point()),None,0), 
                              lambda x: x == robot.map.pointToIndices(goalPoint), 
                              lambda x: 0)
        

    # graphics (draw robot's plan, robot's location, goalPoint)
    robot.map.markCells(robot.plan,'blue')
    robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
    robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

    # move to target point (similar to driving task in DL2)
    currentPoint = inp.odometry.point()
    currentAngle = inp.odometry.theta
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    if not currentPoint.isNear(destinationPoint,0.1):
        angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
        if abs(angle)<0.1:
            #if close enough to pointing, use proportional control on forward velocity
            fv = 2*currentPoint.distance(destinationPoint)
            rv = 0
        else:
            #otherwise, use proportional control on angular velocity
            fv = 0
            rv = 2*angle
    else:
        raise Exception, 'Goal Reached!'

    robot.map.update()
    io.Action(fvel=fv,rvel=rv).execute()
コード例 #16
0
def step():
    global inp
    robot.count += 1
    inp = io.SensorInput(cheat=True)
    
    for c in ('orange','cyan','blue','red','gold'):
        robot.map.clearColor(c)

    if robot.map.doHeatMap:
        robot.map.heatMap()
        
    # discretize sonar readings
    # each element in discreteSonars is a tuple (d, cells)
    # d is the distance measured by the sonar
    # cells is a list of grid cells (r,c) between the sonar and the point d meters away
    discreteSonars = []
    for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            d = noiseModel.noisify(d,gridSquareSize)
        if d < 1.5:
            discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))


    # update map
    for (d,cells) in discreteSonars:
        robot.map.sonarHit(cells[-1])

    # if necessary, make new plan
    secondary_sonars = discreteSonars[:3] + discreteSonars[5:]
    if robot.plan is None or tooCloseToWall(discreteSonars[3:5], secondary_sonars):
        print 'REPLANNING'
        robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
                              robot.map.pointToIndices(inp.odometry.point()),None,0), 
                              lambda x: x == robot.map.pointToIndices(goalPoint), 
                              lambda x: 0)

    # graphics (draw robot's plan, robot's location, goalPoint)
    # do not change this block
    if robot.map.showPassable:
        robot.map.markNotPassable()
    if robot.plan is not None:
        robot.map.markCells(robot.plan,'blue')
    robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
    robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

    # move to target point (similar to driving task in DL2)
    currentPoint = inp.odometry.point()
    currentAngle = inp.odometry.theta
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    if not currentPoint.isNear(destinationPoint,0.1):
        angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
        if abs(angle)<0.1:
            #if close enough to pointing, use proportional control on forward velocity
            fv = 2*currentPoint.distance(destinationPoint)
            rv = 0
        else:
            #otherwise, use proportional control on angular velocity
            fv = 0
            rv = 2*angle
    else:
        raise Exception,'Goal Reached!\n\n%s' % checkoff.generate_code(globals())
            
    robot.map.render()
    io.Action(fvel=fv,rvel=rv).execute()