def resample(state): first = True stack = [] for i, w in enumerate(state.weights): if first: stack.append(w) first = False else: stack.append(stack[i - 1] + w) new_state = motion_predict.State(particles=[], weights=[]) for _ in range(NUMBER_OF_PARTICLES): i = bisect.bisect(stack, random.random()) new_state.weights.append(1.0 / NUMBER_OF_PARTICLES) new_state.particles.append(state.particles[i]) return new_state
def updateMeasurement(state, z): """ param: z: sonar measurement """ unsensible_readings = 0 new_weights = [] for p, w in zip(state.particles, state.weights): likelihood = calculate_likelihood(p.x, p.y, p.theta, z) if likelihood < 0: unsensible_readings += 1 new_weights.append(SONAR_CONSTANT_LIKELIHOOD * w) else: new_weights.append(likelihood * w) if unsensible_readings > UNSENSIBLE_READINNGS_THRESHOLD: print "I am here" raise UnsensibleReadings return motion_predict.State(particles=state.particles, weights=new_weights)
def main(): ultrasound.setup() walls.wallmap.draw() WAYPOINTS = [ walls.Point(84, 30), walls.Point(180, 30), walls.Point(180, 54), walls.Point(138, 54), walls.Point(138, 168), walls.Point(114, 168), walls.Point(114, 84), walls.Point(84, 84), walls.Point(84, 30) ] state = motion_predict.State(particles=[ motion_predict.Particle(x=WAYPOINTS[0].x, y=WAYPOINTS[0].y, theta=0) ] * NUMBER_OF_PARTICLES, weights=[ 1.0 / NUMBER_OF_PARTICLES for _ in range(NUMBER_OF_PARTICLES) ]) draw_particles(state) for waypoint in WAYPOINTS[1:]: # waypoint refers to the next destination while True: x_is_close = abs(state.x - waypoint.x) <= 1.0 y_is_close = abs(state.y - waypoint.y) <= 1.0 if x_is_close and y_is_close: # We have reached our destination, rotate! break else: state = motion_predict.navigateToWaypoint(state, waypoint) state = MCLStep(state) print state.x, state.y, state.theta
def normalize(state): total_weight = sum(w for w in state.weights) return motion_predict.State( particles=state.particles, weights=[w / total_weight for w in state.weights])
def main(): walls.wallmap.draw() state = motion_predict.State( particles=[motion_predict.Particle( x=BOTTLES[3][1][0].x, y=BOTTLES[3][1][0].y, theta=0)] * NUMBER_OF_PARTICLES, weights=[1.0 / NUMBER_OF_PARTICLES for _ in range(NUMBER_OF_PARTICLES)]) mcl.draw_particles(state) raw_input("Click enter to begin") # visitpoints is a list of points that we need to visit for key, visitpoints in BOTTLES: # print "Going into key = ", key # print "Entering state is ", state # mcl_points is a list of lists area_mcl_points = MCL_POINTS[key] # Bottles if key != "FINAL": # waypoint refers to the next destination for waypoint, mcl_points in zip(visitpoints, area_mcl_points): distance = 0.0 # Navigate properly for i in range(1,3): #while True: x_is_close = abs(state.x - waypoint.x) <= WAYPOINT_MIN_OFFSET y_is_close = abs(state.y - waypoint.y) <= WAYPOINT_MIN_OFFSET if x_is_close and y_is_close: # We have reached our destination break else: # TO DO: Smart navigation with not many rotations # print "state = ", state state = uncertainNavigate(state, waypoint) # print "Navigating to ", waypoint # Run MCL if key!= "A": if i != 2: state = sigPointMCLStep(state, mcl_points) # print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta) # Make sure your orientation is the same as the orientation a signature must be taken at state = uncertainRotate(state, waypoint) # Compare signatures bottle_loc = get_bottle(waypoint) # We did not find a bottle, go to the next waypoint # TO DO: What to do if we went to all waypoints and we still did not hit a bottle if bottle_loc is None: # print "BOTTLE NOT DETECTED" continue # We have a possible bottle location else: # print "bottle_loc = ", bottle_loc # 1. Try navigating to the bottle. motor_params.rotate(bottle_loc.angle) state = state.rotate(math.radians(bottle_loc.angle)) # print "State right before moving towards bottle:" # print state nearest_wall_dist = walls.getWallDist( motion_predict.Particle(x=state.x, y=state.y, theta=state.theta), incidence_angle=False) if nearest_wall_dist == ultrasound.GARBAGE: overshoot = 5.0 else: # bottle.distance + overshoot == nearest_wall_dist - 10.0 overshoot = nearest_wall_dist - 10.0 - bottle_loc.distance distance, hit_bottle = motor_params.slow_down_forward( bottle_loc.distance, place_rec.bump_termination_callback, overshoot=overshoot) # Don't perform MCL here, we are fairly sure that it will # screw up. (Due to the bottle). state = state.move_forward(distance) if hit_bottle: # print "State right after touching bottle:" # print state # 2. If we hit the bottle, we will want to reverse. motor_params.forward(-distance * 0.8) state = state.move_forward(-distance * 0.8) # print "State right after reversing from bottle:" # print state # 3. Break if we hit the bottle. Then we go on to # handle the next bottle area. state = sigPointMCLStep(state, mcl_points) # print "State right after performing MCL" # print state break else: # 4. if we did not hit a bottle, we continue the loop. # Going to the next waypoint in the area. motor_params.forward(-distance * 0.8) state = state.move_forward(-distance * 0.8) continue # Final endpoint else: waypoint = visitpoints[0] mcl_points = area_mcl_points[0] # Navigate properly for i in range(1,3): #while True: x_is_close = abs(state.x - waypoint.x) <= FINAL_WAYPOINT_MIN_OFFSET y_is_close = abs(state.y - waypoint.y) <= FINAL_WAYPOINT_MIN_OFFSET if x_is_close and y_is_close: # We have reached our destination break else: # TO DO: Make sure you are tunning MCL facing to the proper walls state = uncertainNavigate(state, waypoint) state = sigPointMCLStep(state, mcl_points) print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta)