def __init__(self, robot: cozmo.robot.Robot): self.current_arena_pose = None self.current_robot_pose = robot.pose self.robot = robot # start streaming robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() # Obtain the camera intrinsics matrix fx, fy = robot.camera.config.focal_length.x_y cx, cy = robot.camera.config.center.x_y self.camera_settings = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float) self.pick_up_pose = Pose(x=4.5, y=13.75, z=0, angle_z=degrees(90)) self.drop_off_pose = Pose(x=21.75, y=13.75, z=0, angle_z=degrees(90)) self.drop_off_directions = [ Pose(x=3, y=4.5, z=0, angle_z=degrees(0)), Pose(x=21.75, y=4.5, z=0, angle_z=degrees(90)), self.drop_off_pose ] self.pick_up_directions = [ Pose(x=21.75, y=4.5, z=0, angle_z=degrees(90)), Pose(x=3, y=4.5, z=0, angle_z=degrees(0)), self.pick_up_pose ] self.drive_speed = speed_mmps(50) print("Robot initialized!") self.grid = CozGrid("map_arena.json") self.pf = ParticleFilter(self.grid) print("Robot initialized!") threading.Thread(target=self.runGUI).start()
def __init__(self, robot: cozmo.robot.Robot): self.current_arena_pose = None self.last_robot_pose = robot.pose self.robot = robot # start streaming await robot.set_head_angle(degrees(3)).wait_for_completed() robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() # Obtain the camera intrinsics matrix fx, fy = robot.camera.config.focal_length.x_y cx, cy = robot.camera.config.center.x_y self.camera_settings = np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ], dtype=np.float) self.grid = CozGrid("map_arena.json") self.pf = ParticleFilter(self.grid) self.gui = GUIWindow(self.grid, show_camera=True) self.pick_up_pose = Pose(x=4.5, y=20, 0, angle_z=degrees(90)) self.drop_off_pose = Pose(x=21.75, y=13.75, 0, angle_z=degrees(90)) self.drop_off_directions = [Pose(x=3, y=4.5, 0, angle_z=degrees(0)), Pose(x=21.75, y=4.5, 0, angle_z=degrees(90)), self.drop_off_pose] self.pick_up_directions = [Pose(x=21.75, y=4.5, 0, angle_z=degrees(90)), Pose(x=3, y=4.5, 0, angle_z=degrees(0)), self.pick_up_pose]
# ---------- Sensor (markers) model update ---------- self.particles = measurement_update(self.particles, r_marker_list, self.grid) # ---------- Show current state ---------- # Try to find current best estimate for display m_x, m_y, m_h, m_confident = compute_mean_pose(self.particles) return (m_x, m_y, m_h, m_confident) # tmp cache last_pose = cozmo.util.Pose(0,0,0,angle_z=cozmo.util.Angle(degrees=0)) flag_odom_init = False # map Map_filename = "map_arena.json" grid = CozGrid(Map_filename) gui = GUIWindow(grid, show_camera=True) pf = ParticleFilter(grid) def compute_odometry(curr_pose, cvt_inch=True): ''' Compute the odometry given the current pose of the robot (use robot.pose) Input: - curr_pose: a cozmo.robot.Pose representing the robot's current location - cvt_inch: converts the odometry into grid units Returns: - 3-tuple (dx, dy, dh) representing the odometry ''' global last_pose, flag_odom_init
def run_test_case(Robot_init_pose, Dh_circular, Robot_speed): grid = CozGrid(Map_filename) # initial distribution assigns each particle an equal probability particles = Particle.create_random(setting.PARTICLE_COUNT, grid) robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2]) particlefilter = ParticleFilter(particles, robbie, grid) score = 0 # 1. steps to build tracking steps_built_track = 9999 for i in range(0, Steps_build_tracking): est_pose = particlefilter.update() if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \ and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \ and i+1 < steps_built_track: steps_built_track = i + 1 #print("steps_built_track =", steps_built_track) if steps_built_track < 50: score = 45 elif steps_built_track < Steps_build_tracking: score = Steps_build_tracking - steps_built_track else: score = 0 print("\nPhase 1") print("Number of steps to build track :", steps_built_track, "/", Steps_build_tracking) acc_err_trans, acc_err_rot = 0.0, 0.0 max_err_trans, max_err_rot = 0.0, 0.0 step_track = 0 # 2. test tracking score_per_track = 45.0 / Steps_stable_tracking for i in range(0, Steps_stable_tracking): est_pose = particlefilter.update() err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) acc_err_trans += err_trans if max_err_trans < err_trans: max_err_trans = err_trans err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h)) acc_err_rot += err_rot if max_err_rot < err_rot: max_err_rot = err_rot if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \ and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot: step_track += 1 score += score_per_track print("\nPhase 2") print("Number of steps error in threshold :", step_track, "/", Steps_stable_tracking) print("Average translational error :", acc_err_trans / Steps_stable_tracking) print("Average rotational error :", acc_err_rot / Steps_stable_tracking, "deg") print("Max translational error :", max_err_trans) print("Max rotational error :", max_err_rot, "deg") return score
def grade(Robot_init_pose, Dh_circular, Robot_speed): grid = CozGrid(Map_filename) # initial distribution assigns each particle an equal probability particles = Particle.create_random(PARTICLE_COUNT, grid) robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2]) particlefilter = ParticleFilter(particles, robbie, grid) score = 0 # 1. steps to build tracking steps_built_track = 9999 for i in range(0, Steps_build_tracking): est_pose = particlefilter.update(Dh_circular, Robot_speed) if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \ and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \ and i+1 < steps_built_track: steps_built_track = i + 1 #print("steps_built_track =", steps_built_track) if steps_built_track < 50: score = 50 elif steps_built_track < 100: score = 100 - steps_built_track else: score = 0 print("\nPhrase 1") print("Number of steps to build track :", steps_built_track, "/", Steps_build_tracking) acc_err_trans, acc_err_rot = 0.0, 0.0 max_err_trans, max_err_rot = 0.0, 0.0 step_track = 0 # 2. test tracking for i in range(0, Steps_stable_tracking): est_pose = particlefilter.update(Dh_circular, Robot_speed) err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) acc_err_trans += err_trans if max_err_trans < err_trans: max_err_trans = err_trans err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h)) acc_err_rot += err_rot if max_err_rot < err_rot: max_err_rot = err_rot if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \ and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot: step_track += 1 if step_track > 90: score += 50 elif step_track > 40: score += step_track - 40 else: score += 0 print("\nPhrase 2") print("Number of steps error in threshold :", step_track, "/", Steps_stable_tracking) print("Average translational error :{:.2f}".format(acc_err_trans / Steps_stable_tracking)) print("Average rotational error :{:.2f} deg".format(acc_err_rot / Steps_stable_tracking)) print("Max translational error :{:.2f}".format(max_err_trans)) print("Max rotational error :{:.2f} deg".format(max_err_rot)) print("\nexample score =", score) return score