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))))
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))
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)
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]))
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]))
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
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)
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)
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)
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)
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)
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()
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
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)
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()
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()