def sample_resample(particles, resample_num, landmark_location, measured_angle, measured_distance): print(landmark_location) # Adding uncertainty to the particles 10 # particle.add_uncertainty_von_mises(particles, 0, 0.02) particle.add_uncertainty(particles, 10, 0.08) # Calculating weights for angles particleAngles = [particle.getTheta() for particle in particles] e_l = [((landmark_location[0] - particle.getX()) , (landmark_location[1] - particle.getY())) / np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles] e_theta = [np.array((np.cos(angle),np.sin(angle))) for angle in particleAngles] phi = [np.arccos(np.dot(x,y)) for x, y in zip(e_l, e_theta)] e_hat_theta = [(-np.sin(angle),np.cos(angle)) for angle in particleAngles] particle_angle_diff_temp = [np.dot(x,y) for x,y in zip(e_l,e_hat_theta)] particle_angle_diff = [np.sign(x) * y for x,y in zip(particle_angle_diff_temp, phi)] angleWeights = calcWeights(particle_angle_diff, measured_angle, 0.08) #print ("angle weights after: {}".format(angleWeights)) # Weights for distance particleLengths = [np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles] # Var should be roughly 50 lengthWeights = calcWeights(particleLengths, measured_distance, 20) #print ("length weights after: {}".format(lengthWeights)) # Combining the weight types ''' normWeights = [] for i in range(resample_num): normWeights.append(angleWeights[i] * lengthWeights[i]) ''' normWeights = [x * y for x, y in zip(angleWeights, lengthWeights)] normWeights = normWeights/np.sum(normWeights) # Setting the weight for each particle for i in range(resample_num): particles[i].setWeight(normWeights[i]) # Resampling # Chooses resample_num random particles from the current particles based on the calculated weights. temp = np.random.choice(particles, resample_num, True, normWeights) retVal = [] for elem in temp: retVal.append(copy.deepcopy(elem)) return retVal
def move_robot(oneMeter, particles, direction, amount): distNoise = 0.1 driveAngleNoise = 0.0002 * amount turnAngleNoise = 0.001 * amount if (direction == 'r' or direction == 'l') and amount < 8: amount += 5 if direction == 'f': remaining, stop_dir = mvmt.goForward(amount/100.0, oneMeter) particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise) move_particles(particles, (amount - (remaining*100)), 0) return remaining, stop_dir, particles elif direction == 'b': remaining, stop_dir = mvmt.goBack(amount/100.0, oneMeter) particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise) move_particles(particles, -(amount - (remaining*100)), 0) return remaining, stop_dir, particles elif direction == 'r': mvmt.goRight(amount) particle.add_uncertainty(particles, 5, turnAngleNoise) move_particles(particles, 0, amount) return 0,'n', particles elif direction == 'l': mvmt.goLeft(amount) particle.add_uncertainty(particles, 5, turnAngleNoise) move_particles(particles, 0, -amount) return 0,'n', particles else: print("Not a valid direction") return -1, 'n', particles
def move_robot(particles, direction, amount): distNoise = 0.1 * amount driveAngleNoise = 0.0002 * amount turnAngleNoise = 0.001 * amount if direction == 'f': remaining, stopDir = mvmt.goForward(amount / 100.0) particle.add_uncertainty(particles, distNoise, driveAngleNoise) move_particles(particles, (amount - remaining), 0) return remaining, stopDir elif direction == 'b': remaining, stopDir = mvmt.goBack(amount / 100.0) particle.add_uncertainty(particles, distNoise, driveAngleNoise) move_particles(particles, -(amount - remaining), 0) return remaining, stopDir elif direction == 'r': mvmt.goRight(amount) particle.add_uncertainty(particles, 0, turnAngleNoise) move_particles(particles, 0, -amount) return 0, 'n' elif direction == 'l': mvmt.goLeft(amount) particle.add_uncertainty(particles, 0, turnAngleNoise) move_particles(particles, 0, amount) return 0, 'n' else: print("Not a valid direction") return -1, 'n'
def stop(wait): global nextmove global currentmove global est_pose # send stop command to the adrino # calculate and apply the movemt to the particles msg = 's' serialRead.write(msg.encode('ascii')) runtime = time.time() - movestarttime waittime = time.time() + wait if runtime > 1.0: if currentmove == "F": if runtime < 1.2: distance = 0 else: distance = forward_time_2_cm(runtime) for p in particles: x = p.getX() + (np.cos(p.getTheta()) * distance) y = p.getY() + ((-np.sin(p.getTheta())) * distance) p.setX(x) p.setY(y) add_uncertainty(particles, 5, 0.2) elif currentmove == "H": distance = cw_time_2_deg(runtime) * (np.pi / 180) for p in particles: theta = p.getTheta() p.setTheta(theta - distance) add_uncertainty(particles, 1, 0.2) elif currentmove == "L": distance = ccw_time_2_deg(runtime) * (np.pi / 180) for p in particles: theta = p.getTheta() p.setTheta(theta + distance) add_uncertainty(particles, 1, 0.2) currentmove = "0" est_pose = estimate_pose(particles)
def stop(wait): global nextmove global currentmove global est_pose # send stop command to the adrino # calculate and apply the movemt to the particles msg = 's' serialRead.write(msg.encode('ascii')) runtime = time.time() - movestarttime waittime = time.time() + wait if runtime > 1.0: if currentmove == "F": if runtime < 1.2: distance = 0 else: distance = forward_time_2_cm(runtime) for p in particles: x = p.getX() + (np.cos(p.getTheta()) * distance) y = p.getY() + ((-np.sin(p.getTheta())) * distance) p.setX(x) p.setY(y) add_uncertainty(particles, 5, 0.2) elif currentmove == "H": distance = cw_time_2_deg(runtime) * (np.pi/180) for p in particles: theta = p.getTheta() p.setTheta(theta - distance) add_uncertainty(particles, 1, 0.2) elif currentmove == "L": distance = ccw_time_2_deg(runtime) * (np.pi/180) for p in particles: theta = p.getTheta() p.setTheta(theta + distance) add_uncertainty(particles, 1, 0.2) currentmove = "0" est_pose = estimate_pose(particles)
if sample >= cumsum[i] and sample < cumsum[i + 1]: samples.append(copy.deepcopy(particles[i])) for i in range(len(particles)): particles[i] = samples[i] # Draw detected pattern cam.draw_object(colour) else: LMInSight = False # No observation - reset weights to uniform distribution for p in particles: p.setWeight(1.0 / num_particles) par.add_uncertainty(particles, 5, 0.15) est_pose = par.estimate_pose( particles) # The estimate of the robots current pose print "In sight = {0}".format(LMInSight) print "Mean weight = {0}".format(weightMean) print "Visited lms: ", visitedLM # XXX: Make the robot drive if weightMean > 0.6 and LMInSight and not visitedLM[lastSeenLM]: turn_counter = 0 # Turn towards landmark if lastMeasuredAngle > 0: arlo.go_diff(30, 29, 0, 1) elif lastMeasuredAngle < 0:
def drive_to_middle(particles, pose, landmarks_seen): dest = find_vec_to_mid(pose) pos_vec = (np.cos(pose.getTheta()),np.sin(pose.getTheta())) angle_to_turn, dist_to_drive, _ = get_directions(pos_vec, dest) #print("Drive?:", (len(landmarks_seen) > 1), (abs(angle_to_turn) < 0.18)) #print("Angle to turn:", angle_to_turn) #print("Length to drive:", dist_to_drive) dist_thresh = 10 angle_thresh = 0.15 stdX = [elem.getX() for elem in particles] stdY = [elem.getY() for elem in particles] converged = np.std(stdX) < 80 and np.std(stdY) < 80 if converged and len(landmarks_seen) > 1: if abs(angle_to_turn) < angle_thresh and dist_to_drive > dist_thresh: print("Driving") move_robot(particles, 'f', dist_to_drive) elif angle_to_turn < 0 and abs(angle_to_turn) > angle_thresh and dist_to_drive > dist_thresh: print("Turning Left") move_robot(particles, 'l', -np.degrees(angle_to_turn)) elif angle_to_turn > 0 and abs(angle_to_turn) > angle_thresh and dist_to_drive > dist_thresh: print("Turning Right") move_robot(particles, 'r', np.degrees(angle_to_turn)) if dist_to_drive <= dist_thresh: print("Finding Green Box") pos_vec = (np.cos(pose.getTheta()),np.sin(pose.getTheta())) angle_to_turn, green_dist, _ = get_directions(pos_vec, (300,0)) if abs(angle_to_turn) > angle_thresh: if angle_to_turn < 0: print("Turning Left to Green Box") move_robot(particles, 'l', -np.degrees(angle_to_turn)) return False, 5 elif angle_to_turn > 0: print("Turning Right to Green Box") move_robot(particles, 'r', np.degrees(angle_to_turn)) return False, 5 else: print("Finished") move_robot(particles, 'r', 720) return True, 1 else: return False, 5 elif converged: print("Something went wrong") landmarks_seen = [] particle.add_uncertainty(particles, 200, 1) return False, 5 elif len(landmarks_seen) > 1: print("Not converged, but all landmarks seen") if angle_to_turn < 0: move_robot(particles, 'l', -np.degrees(angle_to_turn)) return False, 5 elif angle_to_turn > 0: move_robot(particles, 'r', np.degrees(angle_to_turn)) return False, 5 else: print("Finding landmarks") move_robot(particles, 'l', 30) return False, 5
former_turn += updated_values[2] angular_velocity = updated_values[1] velocity = updated_values[0] num_particles = len(particles) for p in particles: # calculates new orientation if angular_velocity != 0: curr_angle = add_to_angular(p.getTheta(), angular_velocity) if velocity != 0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) if velocity != 0: particle.add_uncertainty(particles, 12, 15) if velocity == 0 and angular_velocity != 0: particle.add_uncertainty(particles, 0, 15) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) print obs_landmark sum_of_angle_diff = 0.0 list_of_particles = weight_particles(particles, np.degrees(measured_angle),
def update_particles(particles, cam, velocity, angular_velocity, world, WIN_RF1, WIN_World): #print 'update: ' + str(angular_velocity) cv2.waitKey(4) num_particles = len(particles) for p in particles: # calculates new orientation curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity) if velocity > 0.0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) else: particle.move_particle(p, 0.0, 0.0, curr_angle) if velocity != 0.0: particle.add_uncertainty(particles, 12, 5) if velocity == 0.0 and angular_velocity != 0.0: particle.add_uncertainty(particles, 0, 5) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) observed_obj = [ objectType, measured_distance, measured_angle, obs_landmark ] list_of_particles = weight_particles(particles, np.degrees(measured_angle), measured_distance, obs_landmark) particles = resample_particles(list_of_particles[:, [0, 2]]) particle.add_uncertainty(particles, 15, 5) cam.draw_object(colour) else: observed_obj = [None, None, None, None] for p in particles: p.setWeight(1.0 / num_particles) particle.add_uncertainty(particles, 10, 5) est_pose = particle.estimate_pose(particles) draw_world(est_pose, particles, world) #Show frame cv2.imshow(WIN_RF1, colour) #Show world cv2.imshow(WIN_World, world) return { 'est_pos': est_pose, 'obs_obj': observed_obj, 'particles': particles }
def update_particles(particles, cam, velocity, angular_velocity, world, WIN_RF1, WIN_World): raw_input() print 'update: ' + str(angular_velocity) cv2.waitKey(4) num_particles = len(particles) for p in particles: # calculates new orientation curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity) print 'theta_rad: ' + str(p.getTheta()) print 'theta_deg: ' + str(np.degrees(p.getTheta())) print 'cur_ang_deg: ' + str(np.degrees(curr_angle)) if velocity > 0.0: [x, y] = move_vector(p, velocity) particle.move_particle(p, x, y, curr_angle) else: particle.move_particle(p, 0.0, 0.0, curr_angle) print 'cur_ang_rad: ' + str(curr_angle) if velocity != 0.0: particle.add_uncertainty(particles, 12, 15) if velocity == 0.0 and angular_velocity != 0.0: particle.add_uncertainty(particles, 0, 15) # Fetch next frame colour, distorted = cam.get_colour() # Detect objects objectType, measured_distance, measured_angle, colourProb = cam.get_object( colour) if objectType != 'none': obs_landmark = ret_landmark(colourProb, objectType) observed_obj = [objectType, measured_distance, measured_angle, obs_landmark] list_of_particles = weight_particles(particles, np.degrees(measured_angle), measured_distance, obs_landmark) particles = [] for count in range(0, num_particles): rando = np.random.uniform(0.0,1.0) # np.random.normal(0.0, 1.0, 1) # dicto = {'i': 500, # 'n': 2} p = when_in_range(list_of_particles, 0, num_particles, rando) particles.append( particle.Particle(p.getX(), p.getY(), p.getTheta(), 1.0 / num_particles)) print 'list_of_particles: ' + str(list_of_particles) print 'particles: ' + str(particles) particle.add_uncertainty(particles, 5, 2) # new random particles added #for c in range(0, int(math.ceil(num_particles * 0.05))): # p = particle.Particle(500.0 * np.random.ranf() - 100, # 500.0 * np.random.ranf() - 100, # 2.0 * np.pi * np.random.ranf() - np.pi, 0.0) # particles.append(p) # Draw detected pattern cam.draw_object(colour) else: observed_obj = [None, None, None, None] # No observation - reset weights to uniform distribution for p in particles: p.setWeight(1.0 / num_particles) particle.add_uncertainty(particles, 5, 2) # est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # return [est_pose, observed_obj] est_pose = particle.estimate_pose( particles) # The estimate of the robots current pose print 'Updated pose: ' + str([est_pose.getX(), est_pose.getY()]) draw_world(est_pose, particles, world) # Show frame cv2.imshow(WIN_RF1, colour) # Show world cv2.imshow(WIN_World, world) return {'est_pos': est_pose, 'obs_obj': observed_obj, 'particles': particles}
# # 'n': 2} # p = when_in_range(list_of_particles, # 0, # num_particles, # rando) # # rando, # # {'i': num_particles/2, 'n': 2}, # # num_particles) # #weight_sum += p.getWeight() # particles.append(particle.Particle(p.getX(), p.getY(), p.getTheta(), 1.0/num_particles)) print "resampled" particle.add_uncertainty(particles, 15, 10) # 10% new random particles added # for c in range(0,int(math.ceil(num_particles*0.05))): # p = particle.Particle(500.0*np.random.ranf() - 100, 500.0*np.random.ranf() - 100, 2.0*np.pi*np.random.ranf()-np.pi, 0.0) # particles.append(p) # Draw detected pattern cam.draw_object(colour) else: # No observation - reset weights to uniform distribution for p in particles: p.setWeight(1.0/num_particles)
LMInSight = True lastMeasuredAngle = measured_angle weightMean = sampleWeights() # Draw detected pattern cam.draw_object(colour) else: LMInSight = False # No observation - reset weights to uniform distribution for p in particles: p.setWeight(1.0 / num_particles) par.add_uncertainty(particles, 2.5, 0.10) # The estimate of the robots current pose est_pose = par.estimate_pose(particles) print "In sight = {0}".format(LMInSight) print "Mean weight = {0}".format(weightMean) print "Visited lms: ", visitedLM # XXX: Make the robot drive if avoiding_box: avoid_box() elif LMInSight and weightMean < 0.6: print 'waiting for the mean to increase' elif weightMean > 0.6 and LMInSight: