def our_go_to_pose(robot, curr_pose, dx, dy, dh): # goto = curr_pose.define_pose_relative_this(Pose(distance_inches(dx), distance_inches(dy), distance_inches(0), angle_z=degrees(dh))) # robot.go_to_pose(goto) goto_x = curr_pose.position.x + distance_inches(dx) goto_y = curr_pose.position.y + distance_inches(dy) goto_h = curr_pose.rotation.angle_z + degrees(dh) robot.go_to_pose(Pose(goto_x, goto_y, distance_inches(0), angle_z=goto_h))
def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose, confident global grid, gui # start streaming robot.camera.image_stream_enabled = True #start particle filter pf = ParticleFilter(grid) ################### ############YOUR CODE HERE################# ################### robot.set_head_angle(radians(0)).wait_for_completed() estimated = [0, 0, 0, False] trueVal = 0 while True: # print(robot.is_picked_up) if risingEdge(flag_odom_init, robot.is_picked_up): pf = ParticleFilter(grid) elif robot.is_picked_up: robot.drive_wheels(0, 0) elif not robot.is_picked_up: if trueVal < 35: robot.drive_wheels(5, 20) else: robot.stop_all_motors() gh = math.degrees( math.atan2(goal[1] - estimated[1], goal[0] - estimated[0])) ch = estimated[2] dh = gh - ch print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360) tol = 0.01 if dh > tol or dh < -tol: print("hi") robot.turn_in_place(degrees(dh)).wait_for_completed() robot.drive_straight( distance_inches( grid_distance(goal[0], goal[1], estimated[0], estimated[1]) - 1.75), speed_mmps(25)).wait_for_completed() dh = goal[2] - estimated[2] - dh print("dh2", dh % 360) robot.turn_in_place(degrees(dh)).wait_for_completed() return 0 curr_pose = robot.pose img = image_processing(robot) markers = cvt_2Dmarker_measurements(img) # print(markers) odom = compute_odometry(curr_pose) estimated = pf.update(odom, markers) gui.show_particles(pf.particles) gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3]) gui.updated.set() last_pose = curr_pose if estimated[3]: trueVal += 1
async def run(self): await self.robot.drive_straight(distance_inches(1), speed_mmps(50), should_play_anim = False).wait_for_completed() while True: await asyncio.sleep(1) print (self.robot.camera.config.fov_y) print (self.robot.camera.config.fov_x) if self.state == LOOK_AROUND_STATE: await self.start_lookaround()
def action_on_seeing_object(robot: cozmo.robot.Robot): '''This block uses a full size hexagon 5 printout to initiate the demo. Cozmo will drive straight for 20.5 inches, in which times he will hit the wall. It'll then do some actions, then turn. Once cozmo has turned, it should see the diamond 2 QR, and begin those actions upon the trigger''' if isHex5[0]: robot.drive_straight(distance_inches(20.5), speed_mmps(70)).wait_for_completed() robot.say_text("What the????").wait_for_completed() robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed() robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed() robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed() robot.say_text("You gotta be kidding me!").wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() '''Once cozmo notices the diamond 2 image, it will begin the actions stated below. It will eventually do a full 180 degree turn, and head towards the hexagon 4 image. This will trigger cozmo's final set of instructions.''' if isDiamond2[0]: robot.drive_straight(distance_inches(10), speed_mmps(70)).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession, ignore_body_track=True).wait_for_completed() robot.say_text("Ugh.").wait_for_completed() robot.say_text("Careful Cozmo....").wait_for_completed() robot.drive_straight(distance_inches(2), speed_mmps(90)).wait_for_completed() robot.drive_straight(distance_inches(-2), speed_mmps(65)).wait_for_completed() robot.turn_in_place(degrees(180)).wait_for_completed() '''Here cozmo will notice the hexagon 4 image, and should find his way out of the "maze". To signal that cozmo has made it out, the event 'CubePounceWinSession is triggered.''' if isHex4[0]: robot.drive_straight(distance_inches(22), speed_mmps(70)).wait_for_completed() robot.turn_in_place(degrees(-90)).wait_for_completed() robot.say_text("How do I get out of here!?").wait_for_completed() robot.drive_straight(distance_inches(2), speed_mmps(25)).wait_for_completed() robot.turn_in_place(degrees(-180)).wait_for_completed() robot.say_text("It's gotta be around here somewhere").wait_for_completed() robot.drive_straight(distance_inches(8), speed_mmps(35)).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceWinSession, ignore_body_track=True).wait_for_completed()
async def move_dist_in_global_frame(robot, m_x, m_y, m_h, dest_x, dest_y): #help goal = [dest_x, dest_y] # Part 1: Figure out Robot's origin + theta offset # For consistency, the global frame is A, the one used by the bot is B # need tp find angle theta_b_a (radians) that represents angle FROM X_b TO X_a theta_a_b = math.radians(m_h - robot.pose_angle.degrees) theta_b_a = math.radians(robot.pose_angle.degrees - m_h) # need to find vector t (in frame A) that represents the origin of B in A # Solution: We have robot pos in both B and A. a_to_r, b_to_r, and t make a triangle. # First we must get R_b in the coordinate system of A, then a_to_r - b_to_r = t. a_to_r_in_A = (m_x, m_y) b_to_r_in_B = (robot.pose.position.x * .03937, robot.pose.position.y * .03937) # in mm, right? b_to_r_in_A_x = (math.cos(theta_a_b)* b_to_r_in_B[0] \ - math.sin(theta_a_b)*b_to_r_in_B[1]) b_to_r_in_A_y = (math.sin(theta_a_b) * b_to_r_in_B[0] \ + math.cos(theta_a_b) * b_to_r_in_B[1]) b_to_r_in_A = (b_to_r_in_A_x, b_to_r_in_A_y) t = (a_to_r_in_A[0] - b_to_r_in_B[0], a_to_r_in_A[1] - b_to_r_in_B[1]) # Part 2: Find goal_in_B goal_in_A = (goal[0], goal[1]) goal_in_B_x = (math.cos(theta_b_a) * goal_in_A[0] \ - math.sin(theta_b_a) *goal_in_A[1] \ - t[0]) #should be negative? goal_in_B_y = (math.sin(theta_b_a) * goal_in_A[0] + math.cos(theta_b_a) * goal_in_A[1] - t[1] ) #should be negative? r_to_goal_in_B = (goal_in_B_x - b_to_r_in_B[0], goal_in_B_y - b_to_r_in_B[1]) head = math.degrees(math.atan2( r_to_goal_in_B[1], r_to_goal_in_B[0])) - robot.pose_angle.degrees actual_head = math.degrees(math.atan2(goal[1], goal[0])) - m_h dist = (goal[0] - m_x, goal[1] - m_y) delta_t = math.degrees(math.atan2(dist[1], dist[0])) - m_h dist = math.sqrt(dist[0]**2 + dist[1]**2) print("DISTANCE TO TARGET: {}", dist) await robot.turn_in_place(degrees(delta_t)).wait_for_completed() #dist = math.sqrt(r_to_goal_in_B[0]**2 + r_to_goal_in_B[1]**2) await robot.drive_straight(distance_inches(dist), speed_mmps(40)).wait_for_completed() return m_h + delta_t
def action_on_seeing_object(robot: cozmo.robot.Robot): if isDiamond2[0]: rand = random.randint(0, 1) #print("This is the random value") #print(rand) #_action_1 = _clad_to_engine_cozmo.RobotActionType.SAY_TEXT wall = robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.CustomObject, timeout=1) if wall: #action1[0]= robot.go_to_pose(wall[0].pose, relative_to_robot=False, in_parallel=False, num_retries=0).wait_for_completed() if not action1: print("I saw the wall, but am I moving") #drive_action = _clad_to_engine_cozmo.RobotActionType.DRIVE_STRAIGHT action1.append( robot.drive_straight(distance_inches(14.5), speed_mmps(30), in_parallel=True)) #time.sleep(2.0) #robot.say_text("What the????").wait_for_completed() #action2 = robot.drive_straight(distance_inches(-3.5), speed_mmps(30)).wait_for_completed() #if(rand == 0): #action3 = robot.turn_in_place(degrees(90)).wait_for_completed() #else: #action3= robot.turn_in_place(degrees(-90)).wait_for_completed() #robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed() #robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed() #robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed() #robot.say_text("You gotta be kidding me!").wait_for_completed() #robot.turn_in_place(degrees(90)).wait_for_completed() #default_position_upon_start(robot); if isCircle5[0]: print("I saw the circle") #action1._abort_action(self, action1, "Oh my message") action1[0].abort() action1[0].wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot, DEGREE): if DEGREE != 0 : robot.turn_in_place(DEGREEs(DEGREE)).wait_for_completed() robot.drive_straight(distance_inches(4), speed_mmps(50)).wait_for_completed()
async def run(robot: cozmo.robot.Robot): global last_pose global grid, gui # global pf, state # start streaming robot.camera.image_stream_enabled = True #start particle filter pf = ParticleFilter(grid) await robot.set_head_angle(degrees(0)).wait_for_completed() state = "In motion" ############################################################################ ######################### YOUR CODE HERE#################################### turn_num = 0 while True: robo_odom = compute_odometry(robot.pose) print(robo_odom) vis_markers = await image_processing(robot) markers_2d = cvt_2Dmarker_measurements(vis_markers) print(markers_2d) m_x, m_y, m_h, m_confident = ParticleFilter.update( pf, robo_odom, markers_2d) gui.show_mean(m_x, m_y, m_h, m_confident) gui.show_particles(pf.particles) robbie = Robot(robot.pose.position.x, robot.pose.position.y, robot.pose_angle.degrees) gui.show_robot(robbie) gui.updated.set() if robot.is_picked_up: await robot.say_text("Hey put me down!!").wait_for_completed() pf = ParticleFilter(grid) time.sleep(5) state = "In motion" await robot.set_head_angle(degrees(0)).wait_for_completed() elif m_confident and state == "In motion": print("Going to the goal pose") print("X: {}, Y:{}".format(m_x, m_y)) # Part 1: Figure out Robot's origin + theta offset # For consistency, the global frame is A, the one used by the bot is B # need tp find angle theta_b_a (radians) that represents angle FROM X_b TO X_a theta_a_b = math.radians(m_h - robot.pose_angle.degrees) theta_b_a = math.radians(robot.pose_angle.degrees - m_h) # need to find vector t (in frame A) that represents the origin of B in A # Solution: We have robot pos in both B and A. a_to_r, b_to_r, and t make a triangle. # First we must get R_b in the coordinate system of A, then a_to_r - b_to_r = t. a_to_r_in_A = (m_x, m_y) b_to_r_in_B = (robot.pose.position.x / 10., robot.pose.position.y / 10.) # in mm, right? b_to_r_in_A_x = (math.cos(theta_a_b)* b_to_r_in_B[0] \ - math.sin(theta_a_b)*b_to_r_in_B[1]) b_to_r_in_A_y = (math.sin(theta_a_b) * b_to_r_in_B[0] \ + math.cos(theta_a_b) * b_to_r_in_B[1]) b_to_r_in_A = (b_to_r_in_A_x, b_to_r_in_A_y) t = (a_to_r_in_A[0] - b_to_r_in_B[0], a_to_r_in_A[1] - b_to_r_in_B[1]) # Part 2: Find goal_in_B goal_in_A = (goal[0], goal[1]) goal_in_B_x = (math.cos(theta_b_a) * goal_in_A[0] \ - math.sin(theta_b_a) *goal_in_A[1] \ + t[0]) goal_in_B_y = (math.sin(theta_b_a) * goal_in_A[0] \ + math.cos(theta_b_a) * goal_in_A[1] \ + t[1]) r_to_goal_in_B = (goal_in_B_x - b_to_r_in_B[0], goal_in_B_y - b_to_r_in_B[1]) head = math.degrees( math.atan2(r_to_goal_in_B[1], r_to_goal_in_B[0])) - robot.pose_angle.degrees actual_head = math.degrees(math.atan2(goal[1], goal[0])) - m_h print("Calculated Head: " + str(head) + " Actual Head: " + str(actual_head)) print("Observed theta: " + str(m_h) + "Calculated theta: " + str(robot.pose_angle.degrees + theta_a_b)) dist = (goal[0] - m_x, goal[1] - m_y) print(dist) delta_t = math.degrees(math.atan2(dist[1], dist[0])) - m_h dist = math.sqrt(dist[0]**2 + dist[1]**2) print("M_H: " + str(m_h)) print("M_X: " + str(m_x)) print("M_Y: " + str(m_y)) print("Delta_t: " + str(delta_t)) print("Dist: " + str(dist)) await robot.turn_in_place(degrees(delta_t)).wait_for_completed() #dist = math.sqrt(r_to_goal_in_B[0]**2 + r_to_goal_in_B[1]**2) await robot.drive_straight(distance_inches(dist), speed_mmps(40)).wait_for_completed() # print("Vectors:") # print(m_x,m_y) # print(str(math.degrees(theta_a_b)) + " + " + str(robot.pose_angle.degrees)) # print(b_to_r_in_B) # print(goal_in_A) # print((goal_in_B_x,goal_in_B_y)) # print(t) # print(r_to_goal_in_B) # h_offset *= -1 # local_x = math.cos(h_offset)*(goal[0]-x_offset) - math.sin(h_offset)*(goal[1]-y_offset) # local_y = math.sin(h_offset)*(goal[0]-x_offset) + math.cos(h_offset)*(goal[1]-y_offset) # goal_pose = cozmo.util.Pose(10*goal_in_B_x, 10*goal_in_B_y, goal[2], angle_z=degrees(goal[2])) # print(robot.pose) # await robot.go_to_pose(goal_pose, relative_to_robot=True, in_parallel=False).wait_for_completed() await robot.say_text("I did it!!").wait_for_completed() print(robot.pose) state = "Wait to be picked up" elif state == "In motion": last_pose = robot.pose if abs(m_x - 130) < 20 and abs(m_y - 90) < 10: await robot.turn_in_place(degrees(15)).wait_for_completed() else: await robot.turn_in_place(degrees(15)).wait_for_completed()
def driveInches(robot, inches): robot.drive_straight(distance_inches(inches), speed_mmps(50)).wait_for_completed()
async def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose global grid, gui, pf # start streaming robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() # Obtain the camera intrinsics matrix fx, fy = robot.camera.config.focal_length.x_y cx, cy = robot.camera.config.center.x_y camera_settings = np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ], dtype=np.float) ################### at_goal = False picked_up = False count = 0 """ This requires a file to be in the same directory, functionality was demoed """ #angry_img = cozmo.oled_face.convert_image_to_screen_data(Image.open("angery.jpg").resize(cozmo.oled_face.dimensions(), Image.NEAREST)) while (True): #robot.enable_stop_on_cliff(False) await robot.say_text("").wait_for_completed() if (robot.is_picked_up): if (not picked_up): at_goal = False pf.particles = Particle.create_random(PARTICLE_COUNT, grid) #await robot.display_oled_face_image(angry_img, 3000.0).wait_for_completed() await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() picked_up = True continue if (at_goal): continue if (not flag_odom_init): last_pose = robot.pose flag_odom_init = True continue picked_up = False curr_odom = compute_odometry(robot.pose) last_pose = robot.pose markers, camera_image = await marker_processing(robot, camera_settings, show_diagnostic_image=False) m_x, m_y, m_h, m_confident = pf.update(curr_odom, markers) #robot.pose = cozmo.util.Pose(m_x,m_y,0,angle_z=cozmo.util.Angle(degrees=m_h)) gui.show_particles(pf.particles) gui.show_mean(0,0,0) gui.show_camera_image(camera_image) gui.updated.set() count += 1 if (m_confident): #robot.go_to_pose(cozmo.util.Pose(goal[0], goal[1], 0, angle_z=cozmo.util.Angle(degrees=goal[2]))) robot.stop_all_motors() dx = goal[0] - m_x dy = goal[1] - m_y angle = math.atan2(dy,dx) distance = grid_distance(dx,dy,0,0) print(diff_heading_deg(math.degrees(angle), m_h)) await robot.turn_in_place(degrees(diff_heading_deg(math.degrees(angle), m_h))).wait_for_completed() robot.wait_for_all_actions_completed() await robot.drive_straight(distance_inches(distance), speed_mmps(140)).wait_for_completed() robot.wait_for_all_actions_completed() await robot.turn_in_place(degrees(-1*diff_heading_deg(-5,math.degrees(angle)))).wait_for_completed() at_goal = True print("Done") else: """ if (len(markers) > 0): await robot.turn_in_place(degrees(5)).wait_for_completed() else: await robot.turn_in_place(degrees(20)).wait_for_completed() if (count % 20 == 0): robot.stop_all_motors() await robot.drive_straight(distance_inches(3), speed_mmps(100)).wait_for_completed() """ if (count % 5 == 0): await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() if (count > 30 and len(markers) > 0): robot.stop_all_motors() await robot.drive_straight(distance_inches(markers[0][0] * 0.5), speed_mmps(100)).wait_for_completed() await robot.turn_in_place(degrees(100)).wait_for_completed() await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() count -= 30 """ if (len(markers) > 0): robot.drive_wheel_motors(-10,10,-1000,1000) else: robot.drive_wheel_motors(-20,20,-1000,1000) """ if (len(markers) > 0): await robot.turn_in_place(degrees(10)).wait_for_completed() else: await robot.turn_in_place(degrees(20)).wait_for_completed()
def action_on_seeing_object(robot: cozmo.robot.Robot): if isHex4[0]: rand = random.randint(0, 2) print("This is the random value") print(rand) wall = robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.CustomObject, timeout=1) if wall: robot.go_to_pose(wall[0].pose, relative_to_robot=False, in_parallel=False, num_retries=0).wait_for_completed() robot.drive_straight(distance_inches(2.5), speed_mmps(30)).wait_for_completed() robot.say_text("What the????").wait_for_completed() robot.drive_straight(distance_inches(-3.5), speed_mmps(30)).wait_for_completed() if (rand == 0): robot.turn_in_place(degrees(90)).wait_for_completed() else: robot.turn_in_place(degrees(-90)).wait_for_completed() #robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed() #robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed() #robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed() #robot.say_text("You gotta be kidding me!").wait_for_completed() #robot.turn_in_place(degrees(90)).wait_for_completed() default_position_upon_start(robot) if isDiamond2[0]: wall = robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.CustomObject, timeout=1) if wall: robot.go_to_pose(wall[0].pose, relative_to_robot=False, in_parallel=False, num_retries=0).wait_for_completed() robot.drive_straight(distance_inches(2.5), speed_mmps(30)).wait_for_completed() robot.play_anim_trigger( cozmo.anim.Triggers.CubePounceLoseSession, ignore_body_track=True).wait_for_completed() #robot.say_text("Ugh.").wait_for_completed() #robot.say_text("Careful Cozmo....").wait_for_completed() #robot.turn_in_place(degrees(180)).wait_for_completed() default_position_upon_start(robot) if isHex5[0]: wall = robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.CustomObject, timeout=1) if wall: robot.go_to_pose(wall[0].pose, relative_to_robot=False, in_parallel=False, num_retries=0).wait_for_completed() robot.drive_straight(distance_inches(2.5), speed_mmps(30)).wait_for_completed() #robot.turn_in_place(degrees(-90)).wait_for_completed() #robot.say_text("How do I get out of here!?").wait_for_completed() #robot.drive_straight(distance_inches(2), speed_mmps(25)).wait_for_completed() #robot.turn_in_place(degrees(-180)).wait_for_completed() #robot.say_text("It's gotta be around here somewhere").wait_for_completed() #robot.drive_straight(distance_inches(8), speed_mmps(35)).wait_for_completed() #robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceWinSession, ignore_body_track=True).wait_for_completed() default_position_upon_start(robot)