def obsModel(s): if numObservations < 10: return dist.mixture(dist.deltaDist(ideal[s]), dist.uniformDist(range(numObservations)), float(numObservations) / 15.) return dist.mixture( dist.triangleDist(ideal[s], int(numObservations / 6.0) + 2), dist.uniformDist(range(numObservations)), 0.8)
def setup(): global ideal ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY, numStates, numObservations) if not (hasattr(robot,'g') and robot.g.winfo_exists()): robot.g = graph.Grapher(ideal) robot.nS = numStates if robot.nS != numStates: robot.g.destroy() robot.g = graph.Grapher(ideal) robot.nS = numStates robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)), transModel, obsModel) robot.g.updateObsGraph([0 for s in xrange(numStates)]) robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) for s in xrange(numStates)])
def setup(): global ideal ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY, numStates, numObservations) if not (hasattr(robot, 'g') and robot.g.winfo_exists()): robot.g = graph.Grapher(ideal) robot.nS = numStates if robot.nS != numStates: robot.g.destroy() robot.g = graph.Grapher(ideal) robot.nS = numStates robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)), transModel, obsModel) robot.g.updateObsGraph([0 for s in xrange(numStates)]) robot.g.updateBeliefGraph( [robot.estimator.belief.prob(s) for s in xrange(numStates)])
def setup(): global ideal, confident, parkingSpot, targetTheta, targetX ideal = idealReadings.computeIdealReadings(WORLD_FILE, xMin, xMax, robotY, numStates, numObservations) parkingSpot = getParkingSpot(ideal) targetX = None targetTheta = math.pi / 2 confident = False if not (hasattr(robot, 'g') and robot.g.winfo_exists()): robot.g = graph.Grapher(ideal) robot.nS = numStates if robot.nS != numStates: robot.g.destroy() robot.g = graph.Grapher(ideal) robot.nS = numStates robot.estimator = markov.StateEstimator(dist.uniformDist(range(numStates)), transModel, obsModel) robot.g.updateObsGraph([0 for s in xrange(numStates)]) robot.g.updateBeliefGraph( [robot.estimator.belief.prob(s) for s in xrange(numStates)]) robot.distances = []
def teleportModel(state): right_d = dist.deltaDist(min(state+1, numStates-1)) random_d = dist.uniformDist(range(numStates)) return dist.mixture(right_d, random_d, 0.7)
def obsModelA(state): zero_dist = dist.uniformDist([0]) return dist.mixture(dist.deltaDist(ideal[state]), zero_dist, 0.7)
return dist.mixture(right_d, random_d, 0.7) def resetModel(state): right_d = dist.deltaDist(min(state+1, numStates-1)) zero_d = dist.deltaDist(0) return dist.mixture(right_d, zero_d, 0.7) def teleportModel2(state): current_d = dist.deltaDist(state) random_d = dist.uniformDist(range(numStates)) return dist.mixture(current_d, random_d, 0.7) def resetModel2(state): current_d = dist.deltaDist(state) zero_d = dist.deltaDist(0) return dist.mixture(current_d, zero_d, 0.7) ## STARTING DISTRIBUTIONS uniformPrior = dist.uniformDist(range(len(ideal))) alwaysLeftPrior = dist.DDist({0:1.0}) ## SIMULATION CODE def simulate(transDist, obsDist): testSE = StateEstimator(uniformPrior, transDist, obsDist) testHMM = HMM(alwaysLeftPrior, transDist, obsDist) Simulator(testHMM, testSE, ideal).simulate() simulate(moveRightModel, obsModelB)
def teleportModel(state): nominal = moveRightModel(state) return dist.mixture(dist.uniformDist(range(numStates)), nominal, 0.3)
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 brainStop(): pass def shutdown(): pass if __name__ == '__main__': prior = dist.uniformDist(range(200)) se = StateEstimator(prior,transModel,obsModel) se.reset() for i in xrange(len(C)): ix,s,obs = C[i] #timestep, state, observation se.update(obs) loc,done = confidentLocation(se.belief) if done: break estimatedState = loc realState = s
def teleportModel2(state): current_d = dist.deltaDist(state) random_d = dist.uniformDist(range(numStates)) return dist.mixture(current_d, random_d, 0.7)
def teleportModel(state): right_d = dist.deltaDist(min(state + 1, numStates - 1)) random_d = dist.uniformDist(range(numStates)) return dist.mixture(right_d, random_d, 0.7)
def obsModelA(state): zero_dist = dist.uniformDist([0]) return dist.mixture(dist.deltaDist(ideal[state]), zero_dist, 0.7)
return dist.mixture(right_d, zero_d, 0.7) def teleportModel2(state): current_d = dist.deltaDist(state) random_d = dist.uniformDist(range(numStates)) return dist.mixture(current_d, random_d, 0.7) def resetModel2(state): current_d = dist.deltaDist(state) zero_d = dist.deltaDist(0) return dist.mixture(current_d, zero_d, 0.7) ## STARTING DISTRIBUTIONS uniformPrior = dist.uniformDist(range(len(ideal))) alwaysLeftPrior = dist.DDist({0: 1.0}) ## SIMULATION CODE def simulate(transDist, obsDist): testSE = StateEstimator(uniformPrior, transDist, obsDist) testHMM = HMM(alwaysLeftPrior, transDist, obsDist) Simulator(testHMM, testSE, ideal).simulate() simulate(moveRightModel, obsModelB)
def uniformObsModel(s): return dist.uniformDist(range(numObservations))
def teleportModel2(state): nominal = dist.deltaDist(state) return dist.mixture(dist.uniformDist(range(numStates)), nominal, 0.3)
def teleportModel2(state): current_d = dist.deltaDist(state) random_d = dist.uniformDist(range(numStates)) return dist.mixture(current_d, random_d, 0.7)
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 brainStop(): pass def shutdown(): pass if __name__ == '__main__': prior = dist.uniformDist(range(200)) se = StateEstimator(prior, transModel, obsModel) se.reset() for i in xrange(len(C)): ix, s, obs = C[i] #timestep, state, observation se.update(obs) loc, done = confidentLocation(se.belief) if done: break estimatedState = loc realState = s
def obsModelB(state): return dist.mixture(dist.deltaDist(ideal[state]),dist.uniformDist(range(10)),0.7)
def teleportModel(state): nominal = moveRightModel(state) return dist.mixture(dist.uniformDist(range(numStates)), nominal,0.3)
def teleportModel2(state): nominal = dist.deltaDist(state) return dist.mixture(dist.uniformDist(range(numStates)), nominal,0.3)
def obsModel(s): if numObservations < 10: return dist.mixture(dist.deltaDist(ideal[s]), dist.uniformDist(range(numObservations)), float(numObservations) / 15.) return dist.mixture(dist.triangleDist(ideal[s], int(numObservations / 6.0) + 2), dist.uniformDist(range(numObservations)), 0.8)
def obsModelB(state): return dist.mixture(dist.deltaDist(ideal[state]), dist.uniformDist(range(10)), 0.7)