def cozmo_program(robot: cozmo.robot.Robot): from cozmo.util import degrees, Pose, distance_mm, speed_mmps from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id cube1 = robot.world.get_light_cube(LightCube1Id) cube2 = robot.world.get_light_cube(LightCube2Id) cube3 = robot.world.get_light_cube(LightCube3Id) lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=60) lookaround.stop() robot.go_to_object(cube2, distance_mm(120)).wait_for_completed() robot.roll_cube(cube2, check_for_object_on_top=True, num_retries=9).wait_for_completed()
def go_to_object_test(robot: cozmo.robot.Robot): '''The core of the go to object test program''' # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() # look around and try to find a cube look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube = None try: cube = robot.world.wait_for_observed_light_cube(timeout=30) print("Found cube: %s" % cube) except asyncio.TimeoutError: print("Didn't find a cube") finally: # whether we find it or not, we want to stop the behavior look_around.stop() if cube: # Drive to 70mm away from the cube (much closer and Cozmo # will likely hit the cube) and then stop. action = robot.go_to_object(cube, distance_mm(70.0)) action.wait_for_completed() print("Completed action: result = %s" % action) print("Done.")
def cozmo_program(robot: cozmo.robot.Robot): print("--------------------------") print("Battery (below 3.5V is low)") print(robot.world.robot.battery_voltage) print("--------------------------") new_color = cozmo.lights.Color(rgb=(0, 255, 0)) green = cozmo.lights.Light(on_color=new_color) cubes = [ robot.world.light_cubes.get(cozmo.objects.LightCube1Id), robot.world.light_cubes.get(cozmo.objects.LightCube2Id), robot.world.light_cubes.get(cozmo.objects.LightCube3Id) ] for cube in cubes: cube.set_lights(green) time.sleep(1) robot.world.add_event_handler(cozmo.objects.EvtObjectTapped, tap_handler) if robot.is_on_charger: robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.say_text("Where are my cubes?").wait_for_completed() look = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube_vision = robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=30, include_existing=True) look.stop() print("Cubes found") robot.say_text("My favourite color is green!").wait_for_completed() global id_cube while True: time.sleep(1) if id_cube: robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession ).wait_for_completed() print("Going to cube ", id_cube) for mycube in cube_vision: if mycube.object_id == id_cube: robot.go_to_object(mycube, distance_mm(60)).wait_for_completed() robot.play_anim(name="ID_pokedB").wait_for_completed() mycube.set_lights(green) id_cube = None
def drive_to_charger(robot: cozmo.robot.Robot): robot.say_text( "You're welcome. I'm done. Now I am going to take a nap. Bye bye." ).wait_for_completed() '''The core of the drive_to_charger program''' # If the robot was on the charger, drive them forward and clear of the charger if robot.is_on_charger: # drive off the charger robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() # Start moving the lift down robot.move_lift(-3) # turn around to look at the charger robot.turn_in_place(degrees(180)).wait_for_completed() # Tilt the head to be level robot.set_head_angle(degrees(0)).wait_for_completed() # wait half a second to ensure Cozmo has seen the charger time.sleep(0.5) # drive backwards away from the charger robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed() # try to find the charger charger = None # see if Cozmo already knows where the charger is if robot.world.charger: if robot.world.charger.pose.is_comparable(robot.pose): print("Cozmo already knows where the charger is!") charger = robot.world.charger else: # Cozmo knows about the charger, but the pose is not based on the # same origin as the robot (e.g. the robot was moved since seeing # the charger) so try to look for the charger first pass if not charger: # Tell Cozmo to look around for the charger look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=30) print("Found charger: %s" % charger) except asyncio.TimeoutError: print("Didn't see the charger") finally: # whether we find it or not, we want to stop the behavior look_around.stop() if charger: # Attempt to drive near to the charger, and then stop. action = robot.go_to_object(charger, distance_mm(65.0)) action.wait_for_completed() print("Completed action: result = %s" % action) print("Done.")
def follow_qrs(robot: cozmo.robot.Robot): '''The core of the follow_faces program''' # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(cozmo.robot.util.degrees(0)).wait_for_completed() code_to_follow = None print("Press CTRL-C to quit") while True: if code_to_follow: #turn = random.choice(-90, 90) robot.say_text("I found a cube!").wait_for_completed() # start turning towards the face robot.go_to_object(code_to_follow, cozmo.robot.util.distance_mm(50)).wait_for_completed() robot.say_text("I made it to the code").wait_for_completed() if code_to_follow.cube_id is 1: robot.say_text("This is Cube 1").wait_for_completed() robot.turn_in_place(cozmo.robot.util.Angle(degrees=90)).wait_for_completed() robot.say_text("I'm turning").wait_for_completed() if code_to_follow.cube_id is 2: robot.say_text("This is Cube 2").wait_for_completed() robot.turn_in_place(cozmo.robot.util.Angle(degrees=-90)).wait_for_completed() robot.say_text("I'm turning").wait_for_completed() if code_to_follow.cube_id is 3: robot.say_text("This is Cube 3").wait_for_completed() robot.turn_in_place(cozmo.robot.util.Angle(degrees=180)).wait_for_completed() robot.say_text("I'm turning").wait_for_completed() robot.set_head_angle(cozmo.robot.util.degrees(0)).wait_for_completed() code_to_follow = None if not (code_to_follow and code_to_follow.is_visible): # find a visible face, timeout if nothing found after a short while try: code_to_follow = robot.world.wait_for_observed_light_cube(timeout=None) except asyncio.TimeoutError: print("I fail") time.sleep(.1)
async def explore_sides(robot: cozmo.robot.Robot, found_cubes): await robot.turn_in_place(degrees(-45)).wait_for_completed() await robot.drive_straight(distance_mm(140), speed_mmps(200), False, False, 0).wait_for_completed() await robot.turn_in_place(degrees(75)).wait_for_completed() go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(100.0)) await go_to_cube.wait_for_completed() # await robot.drive_straight(distance_mm(140), speed_mmps(100), False, False, 0).wait_for_completed() await robot.drive_wheels(-200, -200, l_wheel_acc=4000, r_wheel_acc=4000, duration=1)
def cozmo_program(robot: cozmo.robot.Robot): robot.world.image_annotator.add_static_text('text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT) robot.world.image_annotator.add_annotator('clock', clock) robot.world.image_annotator.add_annotator('battery', Battery) lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=None) cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart cube3 = robot.world.get_light_cube(LightCube3Id) lookaround.stop() robot.pickup_object(cube3).wait_for_completed() d = cozmo.util.Distance(distance_mm = 40) robot.place_on_object(cube1).wait_for_completed() robot.set_lift_height(height = 0.0, max_speed = 5).wait_for_completed() robot.go_to_object(cube1,d).wait_for_completed() robot.set_lift_height(height = 1.0, max_speed = 5).wait_for_completed() d2 = cozmo.util.Distance(distance_inches=-2.0) d3= cozmo.util.Distance(distance_inches=2.0) d4 = cozmo.util.Distance(distance_inches = 3.0) s = cozmo.util.Speed(speed_mmps = 10.0) robot.drive_straight(d2,s, False).wait_for_completed() robot.drive_wheels(10.0, -10.0, duration = 5.0) robot.drive_straight(d4,s,False).wait_for_completed() robot.drive_wheels(-10.0, 10.0, duration = 5.0) robot.drive_straight(d3,s,False).wait_for_completed() robot.set_lift_height (height = 0.0).wait_for_completed() # Shutdown the program after 200 seconds time.sleep(200)
async def cozmo_program(robot: cozmo.robot.Robot): # Add event handlers for whenever Cozmo sees a new object robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared) robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared) # Connect to the pre-defined cubes await robot.world.connect_to_cubes() # Make sure Cozmo's head and arm are at reasonable levels await robot.set_head_angle(degrees(5.0)).wait_for_completed() await robot.set_lift_height(0.0).wait_for_completed() # Searches for the cubes for a defined time in seconds search_cubes = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) found_cubes = await robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.LightCube, timeout=6) search_cubes.stop() found_cubes[0] = robot.world.get_light_cube(LightCube2Id) # Approach the object (pre-defined function used since its a defined object) go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(100.0)) await go_to_cube.wait_for_completed() # Code for objects that weren't pre-defined (we drive manually) # await robot.drive_straight(distance_mm(140), speed_mmps(100), False, False, 0).wait_for_completed() await robot.drive_wheels(-200, -200, l_wheel_acc=4000, r_wheel_acc=4000, duration=1) # Explore all sides of the object for sides in range(15): await explore_sides(robot, found_cubes) await robot.say_text("Exploration complete!").wait_for_completed() # Disconnects from the cubes robot.world.disconnect_from_cubes()
def cozmo_program(robot: cozmo.robot.Robot): state = "findcube1" cubefound = robot.world.get_light_cube(3) while True: if state == "findcube1" and cubefound.cube_id != cozmo.objects.LightCubeIDs[0]: robot.drive_wheels(40, 10) cubefound = robot.world.wait_for_observed_light_cube() elif state == "findcube1" and cubefound.cube_id == cozmo.objects.LightCubeIDs[0]: state = "gotocube1" robot.stop_all_motors() elif state == "gotocube1": skip_check = random.randrange(1, 3) if skip_check == 1: state = "findcube2" else: state = "findcube3" robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed() robot.set_head_angle(degrees(0)) elif state == "findcube2" and cubefound.cube_id != cozmo.objects.LightCubeIDs[1]: robot.drive_wheels(40, 10) cubefound = robot.world.wait_for_observed_light_cube() elif state == "findcube2" and cubefound.cube_id == cozmo.objects.LightCubeIDs[1]: state = "gotocube2" robot.stop_all_motors() elif state == "gotocube2": state = "findcube3" robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed() robot.set_head_angle(degrees(0)) elif state == "findcube3" and cubefound.cube_id != cozmo.objects.LightCubeIDs[2]: robot.drive_wheels(40, 10) cubefound = robot.world.wait_for_observed_light_cube() elif state == "findcube3" and cubefound.cube_id == cozmo.objects.LightCubeIDs[2]: state = "gotocube3" robot.stop_all_motors() elif state == "gotocube3": state = "findcube1" robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed() robot.set_head_angle(degrees(0))
def cozmo_tap_game(robot: cozmo.robot.Robot): # Initialize all the game engines screens and listners speed_tap_game = SpeedTapEngine(robot) robot_game_action = CozmoPlayerActions() display_screen = Screen() display_screen.setup(robot_game_action.score_plan, singleScreen=robot_game_action.singleScreen) game_screen = display_screen.gameScreen #display_screen.start() display_screen.root.mainloop(1) game_screen.show_play_screen(0, 0) time.sleep(0.25) # sleep to give the cozmo cube to stop flashing robot_cube, player_coop_cube, player_defect_cube = speed_tap_game.cozmo_setup_game( robot_game_action.score_plan) if robot_cube in [player_coop_cube, player_defect_cube]: print("Participant cannot play on the same cube as cozmo") game_screen.master.destroy() exit(0) monitor_player_tap = Human_Listener(robot, player_coop_cube, player_defect_cube, speed_tap_game) # initialise variables correctChoice = -1 # Correct choice is for practice round track_correct_practice = 0 game_complete = False winner = 0 score_to = 5 pass_criteria = 8 # Only applies for practice rounds goal_statement = "" preset_goals = [P_R, P_O, O_R, O_O] cozmo_fixture = COZMO_CHOICE[randint(0, 2)] # Now all decided so lets suffle it up shuffle(cozmo_fixture) monitor_player_tap.game_on = True monitor_player_tap.start() deal_count = 1 if robot_game_action.practice: cozmo.logger.info("PD : Playing practice round") else: cozmo.logger.info("PD : Playing experiment round") cozmo.logger.info("PD : Cozmo initial plan fixture") log_deal_plan(cozmo_fixture) cozmo.logger.info("PD : Score set: %s" % robot_game_action.score_plan) try: while deal_count <= robot_game_action.rounds_to_play: #print("%s" % cozmo_fixture) cozmo.logger.info("PD : Deal started") if robot_game_action.practice: if track_correct_practice % 4 == 0: shuffle(preset_goals) #cozmo.logger.info("PD : Preset Goals :%s" % preset_goals) correctChoice = preset_goals[(track_correct_practice - 1) % 4] cozmo.logger.info( "PD : Practice Goal : %s" % robot_game_action.goals_statment_list[correctChoice]) goal_statement = robot_game_action.goals_statment_list[ correctChoice] game_screen.show_play_screen(speed_tap_game.player_score, speed_tap_game.robot_score) game_screen.show_goal_statement(goal_statement) # Cozmo get in ready position robot_game_action.act_out(robot, "wait") # Deal the hand speed_tap_game.deal_hand() cozmo.logger.info("PD : Hand delt : %s" % deal_count) #print("Pre: %s" % datetime.now()) monitor_player_tap.listen = True if robot_game_action.practice: cozmo_goal = correctChoice else: cozmo_goal = cozmo_fixture[deal_count - 1] # Get Cozmo to decide whether it is going to tap tapped = robot_game_action.cozmo_tap_decision( robot, speed_tap_game, cozmo_goal) # If player has tapped it would be registered by now monitor_player_tap.listen = False speed_tap_game.deactivate_current_deal() cozmo.logger.info("PD : Hand deactivated : %s" % deal_count) speed_tap_game.score_last_deal() result = speed_tap_game.result_track[-1] cozmo.logger.info("PD : Result : %s" % RESULT_STATEMENT[result]) game_screen.show_play_screen(speed_tap_game.player_score, speed_tap_game.robot_score) cozmo.logger.info("PD : After hand %s player score : %s" % (deal_count, speed_tap_game.player_score)) cozmo.logger.info("PD : After hand %s cozmo score : %s" % (deal_count, speed_tap_game.robot_score)) game_screen.show_selection(result, correctChoice) if robot_game_action.practice: game_screen.show_goal_statement(goal_statement) else: game_screen.show_goal_statement("") if robot_game_action.practice: # We need to track practice round so that # we know the player has correctly understood the game if result == correctChoice: track_correct_practice += 1 cozmo.logger.info("PD : CORRECT for %s times" % track_correct_practice) else: cozmo.logger.info( "PD : INCORRECT choice. Chances left: %s " % (robot_game_action.rounds_to_play - deal_count - 1)) # One wrong implies all wrong track_correct_practice = 0 # We are not tracking scores across games for # practice so reset deal speed_tap_game.reset_deals() deal_count += 1 #time.sleep(2) # Cozmo check out score robot_game_action.act_out(robot, "stand_back") robot_game_action.act_out(robot, "check_score") robot_cube.stop_light_chaser() player_coop_cube.stop_light_chaser() player_defect_cube.stop_light_chaser() robot_cube.set_lights_off() player_coop_cube.set_lights_off() player_defect_cube.set_lights_off() if robot_game_action.practice and track_correct_practice >= pass_criteria: cozmo.logger.info("PD : Practice Passed") print("PRACTICE PASSED") break elif not robot_game_action.practice and result == X_X: if robot_game_action.rounds_to_play >= 35: cozmo.logger.info( "PD : Over 15 rounds of missing data. We will stop.") break else: cozmo.logger.info( "PD : Rounds incremented to compensate for missing data" ) robot_game_action.rounds_to_play += 1 # We missed a even paced tap/no tap decision by cozmo so append it # at the end to maintain balance cozmo_fixture.append(cozmo_goal) cozmo_fixture[deal_count - 2] = -1 cozmo.logger.info("PD : Updated cozmo plan") log_deal_plan(cozmo_fixture) if deal_count <= robot_game_action.rounds_to_play: robot.move_lift(3) if robot_game_action.practice or deal_count % 5 == 0: #robot.drive_wheels(-50, -50, duration=0.5) robot.go_to_object(robot_cube, distance_mm(35.0)).wait_for_completed() else: robot.drive_wheels(100, 100, duration=1.5) time.sleep(1.5) cozmo.logger.info("PD : Ready for next deal") # clear up games to show result robot_cube.stop_light_chaser() player_coop_cube.stop_light_chaser() player_defect_cube.stop_light_chaser() robot_cube.set_lights_off() player_coop_cube.set_lights_off() player_defect_cube.set_lights_off() cozmo.logger.info("PD : Done playing") robot_game_action.act_out(robot, "stand_back") time.sleep(2) robot.go_to_object(robot_cube, distance_mm(60.0)).wait_for_completed() #display_screen.root.mainloop() if robot_game_action.practice and deal_count >= robot_game_action.rounds_to_play: cozmo.logger.info("PD : Practice Failed") print("PRACTICE FAILED") finally: monitor_player_tap.game_on = False robot_cube.stop_light_chaser() player_coop_cube.stop_light_chaser() player_defect_cube.stop_light_chaser() robot_cube.set_lights_off() player_coop_cube.set_lights_off() player_defect_cube.set_lights_off() monitor_player_tap.join() del speed_tap_game del player_coop_cube del player_defect_cube del robot_cube game_screen.master.destroy()
def cozmo_program(robot: cozmo.robot.Robot): from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id from cozmo.util import degrees, Pose, distance_mm, speed_mmps robot.set_head_angle(degrees(0.0)).wait_for_completed() robot.set_lift_height(0.0).wait_for_completed() cube1 = robot.world.get_light_cube(LightCube1Id) cube2 = robot.world.get_light_cube(LightCube2Id) cube3 = robot.world.get_light_cube(LightCube3Id) robot.set_all_backpack_lights(cozmo.lights.white_light) cube1.set_lights(cozmo.lights.white_light) cube2.set_lights(cozmo.lights.white_light) cube3.set_lights(cozmo.lights.white_light) lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=60) lookaround.stop() robot.set_all_backpack_lights(cozmo.lights.blue_light) cube1.set_lights(cozmo.lights.blue_light) cube2.set_lights(cozmo.lights.blue_light) cube3.set_lights(cozmo.lights.blue_light) robot.pickup_object(cube1, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.green_light.flash()) cube1.set_lights(cozmo.lights.green_light.flash()) robot.say_text(Cozmo_Says1, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.place_on_object(cube2, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.blue_light) cube1.set_lights(cozmo.lights.green_light) cube2.set_lights(cozmo.lights.green_light) robot.say_text(Cozmo_Says2, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.pickup_object(cube3, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.green_light.flash()) cube3.set_lights(cozmo.lights.green_light.flash()) robot.say_text(Cozmo_Says3, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.place_on_object(cube2, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.blue_light) cube3.set_lights(cozmo.lights.green_light) robot.place_object_on_ground_here(cube3, num_retries=5).wait_for_completed() robot.go_to_object(cube3, distance_mm(3.0)).wait_for_completed() robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed() robot.say_text(Cozmo_Says4, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.pickup_object(cube1, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.red_light.flash()) cube1.set_lights(cozmo.lights.red_light.flash()) cube2.set_lights(cozmo.lights.red_light) robot.say_text(Cozmo_Says4, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.place_on_object(cube3, use_pre_dock_pose=True, in_parallel=True, num_retries=5).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.blue_light) cube1.set_lights(cozmo.lights.red_light) cube3.set_lights(cozmo.lights.red_light) robot.say_text(Cozmo_Says5, play_excited_animation=False, use_cozmo_voice=False, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.set_all_backpack_lights(cozmo.lights.white_light) cube1.set_lights(cozmo.lights.white_light) cube2.set_lights(cozmo.lights.white_light) cube3.set_lights(cozmo.lights.white_light) robot.say_text(Cozmo_Says6, play_excited_animation=True, use_cozmo_voice=True, duration_scalar=1.0, voice_pitch=0.0).wait_for_completed() robot.play_anim(name=Cozmo_Says7).wait_for_completed() robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed() anim = robot.play_anim_trigger(cozmo.anim.Triggers.AudioTestAnim) exec(Redund_Code)
async def cozmo_program(robot: cozmo.robot.Robot): #connects to the three pre-defined cubes await robot.world.connect_to_cubes() #(assuming starting point is the charger) moves off the charger then turns towards cubes await robot.drive_straight(distance_mm(100), speed_mmps(250)).wait_for_completed() await robot.turn_in_place(degrees(45)).wait_for_completed() #searches for three cubes for 10 seconds search_cubes = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) found_cubes = await robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=10) search_cubes.stop() #checks to see if all three cubes are found - if they are, then we assign cubes if len(found_cubes) < 3: print("Need 3 cubes but found", len(found_cubes)) sys.exit else: found_cubes[0] = robot.world.get_light_cube(LightCube1Id) found_cubes[1] = robot.world.get_light_cube(LightCube2Id) found_cubes[2] = robot.world.get_light_cube(LightCube3Id) #lights up cubes in custom colors if found_cubes[0] is not None: found_cubes[0].set_light_corners(cozmo.lights.red_light, cozmo.lights.green_light, cozmo.lights.red_light, cozmo.lights.green_light) else: cozmo.logger.warning("Not connected to cube 1!") if found_cubes[1] is not None: found_cubes[1].set_light_corners(cozmo.lights.white_light, cozmo.lights.blue_light, cozmo.lights.white_light, cozmo.lights.blue_light) else: cozmo.logger.warning("Not connected to cube 2!") if found_cubes[2] is not None: found_cubes[2].set_lights(cozmo.lights.red_light) else: cozmo.logger.warning("Not connected to cube 3!") #goes to first cube go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(70.0)) await go_to_cube.wait_for_completed() #picks up the first cube within three tries - if it fails then a message is printed pick_up_cube = robot.pickup_object(found_cubes[0], num_retries=3) await pick_up_cube.wait_for_completed() if pick_up_cube.has_failed: code, reason = pick_up_cube.failure_reason result = pick_up_cube.result print("Picking up cube failed: code=%s reason='%s' result=%s" % (code, reason, result)) return #places the first cube on top of second cube once it reaches it - if it fails then a message is printed place_cube = robot.place_on_object(found_cubes[1], num_retries=3) await place_cube.wait_for_completed() if place_cube.has_failed: code, reason = place_cube.failure_reason result = place_cube.result print("Placing cube has failed: code=%s reason='%s' result=%s" % (code, reason, result)) return #does a wheelie on the third object once it reaches it - if it fails then a message is printed do_a_wheelie = robot.pop_a_wheelie(found_cubes[2], num_retries=2) await do_a_wheelie.wait_for_completed() if do_a_wheelie.has_failed: code, reason = do_a_wheelie.failure_reason result = do_a_wheelie.result print("Doing a wheelie has failed: code=%s reason='%s' result=%s" % (code, reason, result)) return #drives backwards to get back into its regular position from the wheelie position await robot.drive_wheels(-200, -200, l_wheel_acc=4000, r_wheel_acc=4000, duration=0.6) #plays a victory dance to celebrate sucessful completeion await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabVictory ).wait_for_completed() await robot.say_text("I did it!").wait_for_completed() #disconnects all three cubes robot.world.disconnect_from_cubes()
def cozmo_idle(robot: cozmo.robot.Robot): robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() img_clf = ImageClassifier() (train_raw, train_labels) = img_clf.load_data_from_folder('./train/') train_data = img_clf.extract_image_features(train_raw) img_clf.train_classifier(train_data, train_labels) robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() while True: time.sleep(1.0) latest_im = robot.world.latest_image new_im = np.asarray(latest_im.raw_image, dtype="uint8") f = feature.hog(new_im, orientations=10, pixels_per_cell=(48, 48), cells_per_block=(4, 4), feature_vector=True, block_norm='L2-Hys') type = img_clf.predict_labels([f])[10] if type == "drone": robot.say_text(type).wait_for_completed() look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube = None try: cube = robot.world.wait_for_observed_light_cube(timeout=20) except asyncio.TimeoutError: print("There is no cube") finally: look_around.stop() if cube is not None: robot.go_to_object(cube, distance_mm(40)).wait_for_completed() robot.pickup_object(cube, num_retries=2).wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(55)).wait_for_completed() robot.place_object_on_ground_here(cube).wait_for_completed() robot.drive_straight(distance_mm(-100), speed_mmps(55)).wait_for_completed() elif type == "order": robot.say_text(type).wait_for_completed() robot.drive_wheels(50, 23.5, duration=21.55) elif type == "inspection": robot.say_text(type).wait_for_completed() for times in range(4): robot.set_lift_height(1, duration=2, in_parallel=True) robot.drive_straight(distance_mm(200), speed_mmps(50), in_parallel=True).wait_for_completed() robot.set_lift_height(0, duration=2, in_parallel=True).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() elif type == "plane": robot.say_text(type).wait_for_completed() elif type == "truck": robot.say_text(type).wait_for_completed() elif type == "hands": robot.say_text(type).wait_for_completed() elif type == "place": robot.say_text(type).wait_for_completed() time.sleep(1)
def cozmo_program(robot: cozmo.robot.Robot): success = True #see what Cozmo sees robot.camera.image_stream_enabled = True #connect to cubes (in case Cozmo was disconnected from the cubes) robot.world.connect_to_cubes() #identify cubes cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart cube3 = robot.world.get_light_cube(LightCube3Id) # looks like the letters 'ab' over 'T' if cube1 is not None: cube1.set_lights(cozmo.lights.red_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.") if cube2 is not None: cube2.set_lights(cozmo.lights.green_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.") if cube3 is not None: cube3.set_lights(cozmo.lights.blue_light) else: cozmo.logger.warning("Cozmo is not connected to a LightCube3Id cube - check the battery.") #have the user tap each of the cubes, in order try: robot.say_text("Tap the red cube to make me move.").wait_for_completed() cube1.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The red cube was not tapped").wait_for_completed() success = False finally: cube1.set_lights_off() if (success): robot.go_to_object(cube1, distance_mm(20.0)).wait_for_completed() robot.drive_straight(distance_mm(-100.0), speed_mmps(150)).wait_for_completed() else: robot.say_text("You didn't tap the cube properly.").wait_for_completed() success = True try: robot.say_text("Tap the green cube so I can take a picture.").wait_for_completed() cube2.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The green cube was not tapped").wait_for_completed() success = False finally: cube2.set_lights_off() if (success): robot.say_text("I see that you were paying attention. I will take a picture.").wait_for_completed() else: robot.say_text("Do you know how to tap a cube? I will take a picture anyway.").wait_for_completed() success = True new_im = robot.world.wait_for(cozmo.world.EvtNewCameraImage) new_im.image.raw_image.show() #save the raw image as a bmp file img_latest = robot.world.latest_image.raw_image img_convert = img_latest.convert('L') img_convert.save("aPhoto.bmp") #save the raw image data as a png file, named imageName imageName = "myPhoto.png" img = Image.open("aPhoto.bmp") width, height = img.size new_img = img.resize( (width, height) ) new_img.save( imageName, 'png') try: robot.say_text("Tap the blue cube and make me do something.").wait_for_completed() cube3.wait_for_tap(timeout=10) except asyncio.TimeoutError: robot.say_text("The blue cube was not tapped").wait_for_completed() success = False finally: cube3.set_lights_off() cube = robot.world.wait_for_observed_light_cube() if (success): robot.play_anim("anim_cozmosays_badword_01").wait_for_completed() else: robot.say_text("Do you know how to tap a cube? I will pop a wheelie.").wait_for_completed() success = True action = robot.pop_a_wheelie(cube, num_retries=2) action.wait_for_completed() robot.say_text("I treat my body right. Help me.").wait_for_completed() return
def cozmo_program(robot: cozmo.robot.Robot): # create an origin point where Cozmo's charger is. When he picks up objects he will return here. # If the robot was on the charger, drive them forward and clear of the charger if robot.is_on_charger: robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.move_lift(-3) robot.turn_in_place(degrees(180)).wait_for_completed() robot.set_head_angle(degrees(0)).wait_for_completed() time.sleep(0.5) # try to find the charger charger = None if robot.world.charger: if robot.world.charger.pose.is_comparable(robot.pose): # Cozmo knows where the charger is charger = robot.world.charger else: pass if not charger: # Tell Cozmo to look around for the charger look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=30) except asyncio.TimeoutError: print("Didn't see the charger") finally: look_around.stop() origin = charger robot.go_to_object(origin, 70) # on boot up show loading screen # locate all cubes look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) look_around.stop() # define light colours red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0))) blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255))) yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0))) # tag each cube found as a different colour red_cube = robot.world.get_light_cube(LightCube1Id) blue_cube = robot.world.get_light_cube(LightCube2Id) yellow_cube = robot.world.get_light_cube(LightCube3Id) red_cube.set_lights(red_light) blue_cube.set_lights(blue_light) yellow_cube.set_lights(yellow__light) # Pass found objects to GUI for selection robot.say_text("Ready when you are").wait_for_completed() # user selects which cube they want robot.say_text("When you are sure that's the one you want. Press the tick, if you want to select another, " "just press on another object").wait_for_completed() # Wait for conformation robot.say_text("OK, I'll be right back.").wait_for_completed() # cozmo goes and gets cube action = robot.go_to_object(red_cube, distance_mm(70.0)) action.wait_for_completed() action = robot.dock_with_cube(red_cube, approach_angle=cozmo.util.degrees(0), num_retries=2) action.wait_for_completed() action = robot.pickup_object(red_cube, num_retries=3) action.wait_for_completed() robot.say_text("Got it").wait_for_completed() # Cozmo returns cube to user action = robot.go_to_object(origin, 70) action.wait_for_completed() robot.say_text("Is this one the right one?").wait_for_completed() # Object is confirmed robot.say_text("Yay") # get dat fist bump robot.say_text("Do you want me to fetch anything else") # user says no # cozmo returns to base # wait for five minutes of inactivity robot.say_text("I'm going to have a nap now, let me know if you need anything?")
def cozmo_program(robot: cozmo.robot.Robot): # create an origin point where Cozmo's charger is. When he picks up objects he will return here. # If the robot was on the charger, drive them forward and clear of the charger if robot.is_on_charger: robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.move_lift(-3) robot.turn_in_place(degrees(180)).wait_for_completed() robot.set_head_angle(degrees(0)).wait_for_completed() time.sleep(0.5) # try to find the charger charger = None if robot.world.charger: if robot.world.charger.pose.is_comparable(robot.pose): # Cozmo knows where the charger is charger = robot.world.charger else: pass if not charger: # Tell Cozmo to look around for the charger look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=30) except asyncio.TimeoutError: print("Didn't see the charger") finally: look_around.stop() # on boot up show loading screen # define light colours red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0))) blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255))) yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0))) # tag each cube found as a different colour red_cube = robot.world.get_light_cube(LightCube1Id) blue_cube = robot.world.get_light_cube(LightCube2Id) yellow_cube = robot.world.get_light_cube(LightCube3Id) red_cube.set_lights(red_light) blue_cube.set_lights(blue_light) yellow_cube.set_lights(yellow__light) # Pass found objects to GUI for selection robot.say_text("Ready").wait_for_completed() # user selects which cube they want cube_selected_input = input("Which cube do you want. Options: red_cube, yellow_cube, blue_cube: ") cube_selected = cube_selected_input print(cube_selected) robot.say_text("press").wait_for_completed() # Wait for conformation confirmation = input("Are you sure Y/N: ") if confirmation == "Y": robot.say_text("OK.").wait_for_completed() cube_wanted = cube_selected else: print("Fine") cube_wanted = "" # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() # look around and try to find a cube look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) look_around.start() cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=60) look_around.stop() # cozmo goes and gets cube robot.pickup_object(yellow_cube, num_retries=3).wait_for_completed robot.say_text("Got it").wait_for_completed() # Cozmo returns cube to user robot.go_to_object(charger, distance_mm(100)).wait_for_completed() robot.say_text("Is this one the right one?").wait_for_completed() # Object is confirmed confirmation1 = input("Is this the right one? Y/N: ") if confirmation1 == "Y": robot.say_text("Yay") else: robot.say_text("Well you're getting it anyway.") action = robot.place_object_on_ground_here(cube_wanted) action.wait_for_completed() # get dat fist bump robot.say_text("Do you want me to fetch anything else") # user says no # cozmo returns to base # wait for five minutes of inactivity robot.say_text("I'm going to have a nap now, let me know if you need anything?")
def cozmo_program(robot: cozmo.robot.Robot): # Lookaround until Cozmo knows where at least 3 cubes are: lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = robot.world.wait_until_observe_num_objects( num=3, object_type=cozmo.objects.LightCube, timeout=60) lookaround.stop() if len(cubes) < 3: print("Error: need 3 Cubes but only found", len(cubes), "Cube(s)") else: print("Found 3 cubes, seting random lights") # Light up cubes randomly lights = [ cozmo.lights.red_light, cozmo.lights.blue_light, cozmo.lights.green_light ] i = 0 for cube in cubes: cube.set_lights(lights[i]) i = i + 1 # choose a number chosen = random.randint(0, 3) time.sleep(5) if chosen < 3: cube = cubes[chosen] print("Chose " + str(chosen)) # Drive to the cube action = robot.go_to_object(cube, distance_mm(20.0)) action.wait_for_completed() print("Completed action: result = %s" % action) # take a step back, lift fork and drive to the tube robot.drive_straight(distance_mm(-10), speed_mmps(50)).wait_for_completed() robot.set_lift_height(10.0).wait_for_completed() robot.set_head_angle(degrees(0.0)).wait_for_completed() robot.drive_straight(distance_mm(20), speed_mmps(50)).wait_for_completed() # hit the tube for _ in range(2): robot.move_lift(-5) time.sleep(0.05) robot.move_lift(5) time.sleep(0.05) # blink the cube for _ in range(0, 10): cube.set_lights(cozmo.lights.off_light) time.sleep(0.2) cube.set_lights(lights[chosen]) time.sleep(0.2) # Say the color if chosen == 0: robot.say_text("Rot").wait_for_completed() if chosen == 1: robot.say_text("Blau").wait_for_completed() if chosen == 2: robot.say_text("GrĂ¼n").wait_for_completed() # Be happy robot.drive_straight(distance_mm(-50), speed_mmps(150)).wait_for_completed() robot.play_anim(name="anim_poked_giggle").wait_for_completed() else: # Cozmo chose black robot.say_text("Nein!").wait_for_completed() robot.turn_in_place(degrees(180)).wait_for_completed() robot.say_text("Ich will doch lieber bei schwarz bleiben!" ).wait_for_completed()
def cozmo_tap_game(robot: cozmo.robot.Robot): """ The main game """ #Setup speed_tap_game = SpeedTapEngine(robot) robot_game_action = CozmoPlayerActions() #emotion manipulation if robot_game_action.sad_not_angry: reaction_bias = "sad" else: reaction_bias = "angry" cozmo.logger.info( "Starting tap game with reaction time %s and game reaction bias %s" % (robot_game_action.reaction_time, reaction_bias)) cozmo.logger.info("Player %s : Cozmo " % COZMO_ID) cozmo.logger.info("Player %s : Human" % PLAYER_ID) # Robot selects cube robot_cube, player_cube = speed_tap_game.cozmo_setup_game() time.sleep(0.25) # Participant select cube monitor_player_tap = Human_Listener(robot, player_cube, speed_tap_game) game_complete = False winner = 0 score_to = 5 monitor_player_tap.game_on = True monitor_player_tap.start() deal_count = 1 try: while not winner: # robot wait for the light to change on cube i.e. light to be dealt robot_game_action.act_out(robot, "wait") deal_type = speed_tap_game.deal_hand() cozmo.logger.info("Hand %s delt" % deal_count) deal_count += 1 # Cozmo's tap decision tapped = robot_game_action.cozmo_tap_decision( robot, deal_type, speed_tap_game) # Give player time to take decision time.sleep(1) # Current deal is now deactivated speed_tap_game.deactivate_current_deal() #Check who scored and do the right emotion display for winning/losing hand if speed_tap_game.deal_score[-1] == PLAYER_ID: if speed_tap_game.deal_score.count(PLAYER_ID) >= score_to: winner = PLAYER_ID robot_game_action.act_out(robot, "lose_hand") elif speed_tap_game.deal_score[-1] == COZMO_ID: if speed_tap_game.deal_score.count(COZMO_ID) >= score_to: winner = COZMO_ID robot_game_action.act_out(robot, "win_hand") # stop light chaser and prep for next round robot_cube.stop_light_chaser() player_cube.stop_light_chaser() robot_cube.set_lights_off() player_cube.set_lights_off() cozmo.logger.info("Score Board : %s" % speed_tap_game.deal_score) cozmo.logger.info("Cozmo : %s" % speed_tap_game.deal_score.count(COZMO_ID)) cozmo.logger.info("Player : %s" % speed_tap_game.deal_score.count(PLAYER_ID)) # clear up games to show final gameresult robot_cube.stop_light_chaser() player_cube.stop_light_chaser() robot_cube.set_lights_off() player_cube.set_lights_off() monitor_player_tap.game_on = False robot_game_action.act_out(robot, "stand_back") # Indicate win/loss to player if winner == COZMO_ID: robot_cube.set_lights(cozmo.lights.green_light.flash()) player_cube.set_lights(cozmo.lights.red_light.flash()) elif winner == PLAYER_ID: player_cube.set_lights(cozmo.lights.green_light.flash()) robot_cube.set_lights(cozmo.lights.red_light.flash()) robot.go_to_object(robot_cube, distance_mm(60.0)).wait_for_completed() cozmo.logger.info("Final Score Cozmo : %s" % speed_tap_game.deal_score.count(COZMO_ID)) cozmo.logger.info("Final Score Player : %s" % speed_tap_game.deal_score.count(PLAYER_ID)) # Game end emotion act display if winner == COZMO_ID: robot_game_action.act_out(robot, "win_game") else: robot_game_action.act_out(robot, "lose_game") finally: monitor_player_tap.game_on = False robot_cube.stop_light_chaser() player_cube.stop_light_chaser() robot_cube.set_lights_off() player_cube.set_lights_off() monitor_player_tap.join() del speed_tap_game del player_cube del robot_cube