def drive_to_charger(robot): '''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.origin_id == robot.pose.origin_id: 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 transport_cube_III(cube: cozmo.objects.LightCube): global robot,SIDE # Third cube was picked, go place it next to the others charger = go_to_charger() final_adjust(charger,80) robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed() putDown_cube(cube) switch_cube_off(cube) # Get back in front of charger robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed() robot.turn_in_place(degrees(-SIDE*60)).wait_for_completed() return
def transport_cube_I(cube: cozmo.objects.LightCube): global robot,SIDE # First cube was picked, place it next to the charger charger = go_to_charger() final_adjust(charger,60) robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed() putDown_cube(cube) switch_cube_off(cube) # Get back in front of charger robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() return
def custom_objects(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) # define a unique wall (150mm x 120mm (x10mm thick for all walls) # with a 50mm x 30mm Circles2 image on front and back wall_obj = robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles2, 140, 130, 65, 65, False) wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds2, 140, 130, 64, 64, False) # if ((cube_obj is not None) and (big_cube_obj is not None) and # (wall_obj is not None) and (box_obj is not None)): # print("All objects defined successfully!") # else: # print("One or more object definitions failed!") # return print( "Show the above markers to Cozmo and you will see the related objects " "annotated in Cozmo's view window, you will also see print messages " "everytime a custom object enters or exits Cozmo's view.") print("Press CTRL-C to quit") # Move lift down and tilt the head up robot.move_lift(100) robot.set_head_angle(degrees(-25)) while True: global found_wall global found_wall1 if found_wall == True: robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() found_wall = False robot.set_head_angle(degrees(-25)) if found_wall1 == True: robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(-90)).wait_for_completed() found_wall1 = False robot.set_head_angle(degrees(-25)) time.sleep(0.1)
def __handle_drive_forward(self, command, agent): """ Handle a Soar drive-forward action. The Sour output should look like: (I3 ^drive-forward Vx) (Vx ^distance [dist] ^speed [spd]) where [dist] is a real number indicating how far Cozmo should travel (negatives go backwards) and speed is how fast Cozmo should travel. Units are mm and mm/s, respectively. :param command: Soar command object :param agent: Soar Agent object :return: True if successful, False otherwise """ try: distance = distance_mm(float(command.GetParameterValue("distance"))) except ValueError as e: print("Invalid distance format {}".format(command.GetParameterValue("distance"))) return False try: speed = speed_mmps(float(command.GetParameterValue("speed"))) except ValueError as e: print("Invalid speed format {}".format(command.GetParameterValue("speed"))) return False print("Driving forward {}mm at {}mm/s".format(distance.distance_mm, speed.speed_mmps)) drive_forward_action = self.r.drive_straight(distance, speed) self.running_actions.append((command, drive_forward_action)) # callback = self.__handle_action_complete_factory(command) # drive_forward_action.add_event_handler(EvtActionCompleted, callback) return True
def __init__(self, coz): mixer.init() self.soundHappy = mixer.Sound('static/sounds/bg.ogg') self.soundSad = mixer.Sound('static/sounds/boring.ogg') self.soundSad.play(loops=-1) self.cozmo = coz self.arcadeGame = Arcade(self.cozmo, self) self.autonomousInstance = Patrol(self,self.cozmo) self.merrygoround = MerryGoRound(self.cozmo, self) # self.instagram = MemCapture(self.cozmo,self); self.define_custom_objects() self.action_queue = [] self.lift_up = 0 self.lift_down = 0 self.head_up = 0 self.head_down = 0 self.text_to_say = "" self.cozmo_audio_effect_interval = random.randint(200,1000) self.update_count = 0 self.anims_for_keys = ["bored", # 1 "sad", # 2 "happy", # 3 "veryHappy", #4 "laugh", #5 "angry", #6 "impress" #7 ] self.cozmo.set_lift_height(0,in_parallel=True) self.cozmo.set_head_angle(cozmo.util.Angle(degrees=-15),in_parallel=True).wait_for_completed() self.visible_objects = [] self.measuring_dist = False _thread.start_new_thread(self.start_Pizza_Thread, ()) self.cubes = None try: self.cubes = self.cozmo.world.wait_until_observe_num_objects(1, object_type = cozmo.objects.LightCube,timeout=10) except asyncio.TimeoutError: print("Didn't find a cube :-(") return finally: # for cube in self.cubes: # cube.set_lights(Colors.BLUE) if len(self.cubes) > 0: self.cozmo.camera.image_stream_enabled = True self.cubes[0].set_lights_off() self.cozmo.drive_straight(distance_mm(10), speed_mmps(50), in_parallel=True).wait_for_completed() self.cozmo.set_head_angle(cozmo.util.Angle(degrees=30),in_parallel=True) self.cozmo.world.add_event_handler(cozmo.objects.EvtObjectAppeared, self.on_object_appeared) self.cozmo.world.add_event_handler(cozmo.objects.EvtObjectDisappeared, self.on_object_disappeared) else: print("Not found")
def en_drive(self, robot:cozmo.robot.Robot = None, cmd_args = None): usage = "Cozmo drives forward/backwards for X seconds (i.e. 'Cozmo, drive backwards for 3 seconds')." if robot is None: return usage error_message = "" if log: print(cmd_args) #check if the user said "drive to charger" and redirect the command to en_charger() if "charger" in cmd_args: self.en_charger(robot, cmd_args) return drive_duration = extract_next_float(cmd_args)#[0] if drive_duration is not None: if "backward" in cmd_args or "backwards" in cmd_args: drive_speed = speed drive_duration = -drive_duration drive_dir = "backwards" else: drive_speed = speed drive_dir = "forward" #robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration) robot.drive_straight(distance_mm(drive_duration*drive_speed), speed_mmps(drive_speed), should_play_anim=True).wait_for_completed() #time.sleep(drive_duration) return "I drove " + drive_dir + " for " + str(drive_duration) + " seconds!" return "Error: usage = " + usage + error_message
def check_charger(self, robot:cozmo.robot.Robot, distance=150, speed=100): if robot.is_on_charger: if log: print("I am on the charger. Driving off the charger...") robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(distance), speed_mmps(speed)).wait_for_completed() robot.move_lift(-8)
def run(sdk_conn): '''The run method runs once Cozmo is connected.''' robot = sdk_conn.wait_for_robot() for _ in range(4): robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed()
def idle(robot, img_clf): count = 0 while True: robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() data = [] time.sleep(0.5) if count < 10: pass elif count % 4 == 0: robot.turn_in_place(degrees(50)).wait_for_completed() robot.drive_straight(distance_mm(random.uniform(-20, 20)), speed_mmps(80)).wait_for_completed() else: robot.turn_in_place(degrees(-10)).wait_for_completed() count += 1 latestImage = robot.world.latest_image newImage = latestImage.raw_image data.append(np.array(newImage)) feature = img_clf.extract_image_features(data) symbol = img_clf.classifier.predict(feature) if symbol == "drone": drone(robot) elif symbol == "order": order(robot) count = 0 elif symbol == "inspection": inspection(robot) count = 0
async def CozmoPlanning(robot: cozmo.robot.Robot): # Allows access to map and stopevent, which can be used to see if the GUI # has been closed by checking stopevent.is_set() global cmap, stopevent ######################################################################## # TODO: please enter your code below. # Set start node # cmap.set_start(start_node) # Start RRT RRT(cmap, cmap.get_start()) path = cmap.get_smooth_path() index = 1 # Path Planning for RRT while index < len(path): curr_node = path[index] dx = curr_node.x - path[index - 1].x dy = curr_node.y - path[index - 1].y # turn_angle = diff_heading_deg(cozmo.util, np.arctan2(dy, dx)) # print("dx=", dx) # print("dy=", dy) # print("tan=", np.arctan2(dy, dx)) await robot.turn_in_place( radians(np.arctan2(dy, dx)), angle_tolerance=Angle(1)).wait_for_completed() await robot.drive_straight( distance_mm(np.sqrt((dx**2) + (dy**2))), speed=cozmo.util.speed_mmps(50)).wait_for_completed() index += 1 Visualizer.update(cmap)
def inspection(robot: cozmo.robot.Robot): # Use a "for loop" to repeat the indented code 4 times # Note: the _ variable name can be used when you don't need the value # drive in a square with side 20cm robot.set_lift_height(0.0, in_parallel=False, duration=1.0).wait_for_completed() robot.abort_all_actions(log_abort_messages=True) for i in range(4): # raise the arm for 3 seconds, at a speed of 1/3 = 0.333 robot.move_lift(0.33) robot.drive_straight(distance_mm(200), speed_mmps(40), should_play_anim=False, in_parallel=True, num_retries=0) time.sleep(3) # lower the arm for 2 seconds, at a speed of 1/2 = 0.5 robot.move_lift(-0.50) time.sleep(2) # abort all ongoing actions robot.abort_all_actions(log_abort_messages=True) # perform the turn: 90 degree robot.turn_in_place(degrees(90)).wait_for_completed() # stop all actions, if there are any robot.abort_all_actions(log_abort_messages=True) # return to idle state idle(robot)
def follow_cube(robot): target_id = LightCube1Id colors = [ cozmo.lights.red_light, cozmo.lights.blue_light, cozmo.lights.green_light ] cube = search_and_go_to_cube(robot, target_id) while cube: cube.set_lights(random.choice(colors)) action = robot.go_to_object(cube, distance_mm(70.0)) action.wait_for_completed() cube.set_lights_off() target_in_view = False for visible_object in robot.world.visible_objects: if isinstance(visible_object, cozmo.objects.LightCube ) and visible_object.cube_id == target_id: target_in_view = True if not target_in_view: cube = search_and_go_to_cube(robot, target_id) else: time.sleep(2)
def example3_abort_one_action(robot: cozmo.robot.Robot): cozmo.logger.info("----------------------------------------") cozmo.logger.info("Example 3: Abort some parallel actions.") cozmo.logger.info("Start multiple parallel actions:") action1 = robot.set_lift_height(0.0, in_parallel=True) action2 = robot.set_head_angle(cozmo.robot.MIN_HEAD_ANGLE, duration=6.0, in_parallel=True) action3 = robot.drive_straight(distance_mm(75), speed_mmps(25), should_play_anim=False, in_parallel=True) action4 = robot.display_oled_face_image( face_image, 30000.0) # Note: face image is in_parallel by default # Lift-lowering is aborted immediately action1.abort(log_abort_messages=True) # Head-lowering is aborted shortly afterwards time.sleep(0.1) action2.abort(log_abort_messages=True) # Image on Cozmo's face is aborted another 2 seconds later time.sleep(2) action4.abort(log_abort_messages=True) # We wait for the one remaining action to complete action3.wait_for_completed() # Only action3 should succeed (as long as Cozmo had enough space to drive) cozmo.logger.info("action1 = %s", action1) cozmo.logger.info("action2 = %s", action2) cozmo.logger.info("action3 = %s", action3) cozmo.logger.info("action4 = %s", action4)
def go_to_charger(): # Driving towards charger without much precision global robot charger = None ''' cf. 08_drive_to_charger_test.py ''' # see if Cozmo already knows where the charger is if robot.world.charger: # make sure Cozmo was not delocalised after observing the 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: charger = find_charger() action = robot.go_to_object(charger, distance_from_object=distance_mm(80), in_parallel=False, num_retries=5) #action = robot.go_to_pose(charger.pose) action.wait_for_completed() return charger
def cozmo_program(robot: cozmo.robot.Robot): # Use a "for loop" to repeat the indented code 4 times # Note: the _ variable name can be used when you don't need the value for _ in range(4): robot.drive_straight(distance_mm(320), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): global isTakingPicture global targetObject targetObject = sys.argv[1] if os.path.exists('photos'): shutil.rmtree('photos') if not os.path.exists('photos'): os.makedirs('photos') robot.say_text(f"Somebody lost the {targetObject}. Don't worry, I'll find it.").wait_for_completed() # reset Cozmo's arms and head robot.set_head_angle(degrees(10.0)).wait_for_completed() robot.set_lift_height(0.0).wait_for_completed() robot.add_event_handler(cozmo.world.EvtNewCameraImage, on_new_camera_image) while not discoveredObject: isTakingPicture = False robot.turn_in_place(degrees(45)).wait_for_completed() isTakingPicture = True time.sleep(2) isTakingPicture = False if discoveredObject: robot.drive_straight(distance_mm(200), speed_mmps(300)).wait_for_completed() robot.say_text(f"Oh yay! I've found the {targetObject}").wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin).wait_for_completed()
def __handle_go_to_object(self, command, agent): """ Handle a Soar go-to-object action. The Sour output should look like: (I3 ^go-to-object Vx) (Vx ^target_object_id [id]) where [id] is the object id of the object to go to. Cozmo will stop 150mm from the object. :param command: Soar command object :param agent: Soar Agent object :return: True if successful, False otherwise """ try: target_id = int(command.GetParameterValue("target_object_id")) except ValueError as e: print( "Invalid target-object-id format {}".format( command.GetParameterValue("target_object_id") ) ) return False if target_id not in self.objects.keys(): print("Couldn't find target object") return False print("Going to object {}".format(target_id)) target_obj = self.objects[target_id] go_to_object_action = self.robot.go_to_object(target_obj, distance_mm(100)) self.running_actions.append((command, go_to_object_action)) # callback = self.__handle_action_complete_factory(command) # go_to_object_action.add_event_handler(EvtActionCompleted, callback) return True
def __init__(self, robot): self.robot = robot print('Current state:', str(self)) """ Do action here """ count_turns = 0 while count_turns <= 3: action1 = robot.drive_straight(distance_mm(200), speed_mmps(30), in_parallel=True) action2 = robot.set_lift_height(1.0, in_parallel=True, duration=3.0) action2.wait_for_completed() action2 = robot.set_lift_height(0.0, in_parallel=True, duration=3.0) action2.wait_for_completed() action1.wait_for_completed() action1 = robot.turn_in_place( degrees(90), in_parallel=True).wait_for_completed() count_turns = count_turns + 1 action2 = robot.set_lift_height(0.0, in_parallel=True, duration=3.0).wait_for_completed() self.return_to_idle()
async def get_sad(self): #Cozmo behavior - get sad on seeing marker on photo of his pirate father await self.robot.go_to_object(self.objects[0], distance_mm(100)).wait_for_completed() await self.robot.play_anim_trigger( cozmo.anim.Triggers.RequestGameKeepAwayDeny1).wait_for_completed() x = random.randrange(4) if x == 0: await self.robot.say_text("I miss you father", duration_scalar=2, voice_pitch=0).wait_for_completed() elif x == 1: await self.robot.say_text("You will always be in my heart, father", duration_scalar=1.5, voice_pitch=0).wait_for_completed() elif x == 2: await self.robot.say_text("Father", duration_scalar=2, voice_pitch=-1).wait_for_completed() await self.robot.say_text("Oh, father", duration_scalar=2, voice_pitch=0).wait_for_completed() elif x == 3: await self.robot.say_text("What is dead may never die", duration_scalar=2, voice_pitch=-1).wait_for_completed() await asyncio.sleep(2) await self.robot.turn_in_place(cozmo.util.Angle(degrees=180) ).wait_for_completed()
def go_to_object_test(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 drone(robot): robot.say_text('drone').wait_for_completed() time.sleep(0.5) lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = robot.world.wait_until_observe_num_objects( num=1, object_type=cozmo.objects.LightCube, timeout=60) lookaround.stop() if len(cubes) > 0: robot.pickup_object(cubes[0], num_retries=5).wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(80)).wait_for_completed() robot.place_object_on_ground_here(cubes[0]).wait_for_completed() robot.drive_straight(distance_mm(-80), speed_mmps(80)).wait_for_completed()
def moveToGoal(robot: cozmo.robot.Robot, cmap, stack, cube): print("Move To Goal State") cube = None goals = cmap.get_goals(); goal = goals[0] if stack: try: cube = robot.world.wait_for_observed_light_cube(timeout = 4) print(cube) except asyncio.TimeoutError: print("no obstacles found") pass if cube is not None and cube.object_id != cubeID: print("Tadah") robot.foundObstacle() else: # Keep moving curr = stack.pop() ro = (robot.pose.position.x, robot.pose.position.y) if ((goal[0] - 90 < ro[0] < goal[0] + 90) and (goal[1] - 90 < ro[1] < goal[1] + 90)): print("Yay"); robot.reachedGoal() dist = get_dist(Node(ro), Node(curr)) #get distance angle = np.arctan2(curr[1]-ro[1],curr[0]-ro[0])*180/np.pi - robot.pose_angle.degrees print(angle) robot.turn_in_place(degrees(angle)).wait_for_completed() robot.drive_straight(distance_mm(dist), speed_mmps(50)).wait_for_completed() else: print("Yay") robot.reachedGoal() return robot, stack, cube # returns cozmo
def docking_cozmo(cozmo_robot: cozmo.robot.Robot): global robot init_robot(cozmo_robot) if robot.is_on_charger : robot.drive_straight(distance_mm(200),speed_mmps(80)).wait_for_completed() get_on_charger() return
def cozmo_go_to_object(sdk_conn): print("SENDING COZMO TO OBJECT %s" % cube_to_go) robot = sdk_conn.wait_for_robot() # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube_detination = None 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) < 1: print("Error: Cube not found") else: for cube in cubes: print("Comparing cubes: %s" % cube.object_id) if (str(cube.object_id) == str(cube_to_go)): cube_detination = cube print("CUBE FOUND!") if cube_detination: # Drive to 70mm away from the cube (much closer and Cozmo # will likely hit the cube) and then stop. cube_detination.set_lights(cozmo.lights.green_light) action = robot.go_to_object(cube_detination, distance_mm(70.0)) action.wait_for_completed() print("Completed action: result = %s" % action) print("Done.") else: print("CUBE NOT FOUND!!!!")
async def execute_directions(robot, directions): print("Robot is at: ", robot.pose) await robot.turn_in_place(angle=directions.rotation.angle_z ).wait_for_completed() print("Robot is at after turning to be parrael to X: ", robot.pose) await robot.drive_straight(distance=distance_mm(directions.position.x * grid.scale), speed=speed_mmps(80)).wait_for_completed() print("Robot is at after driving in the X dirrection: ", robot.pose) await robot.turn_in_place(angle=cozmo.util.Angle(degrees=90) ).wait_for_completed() print("Robot is at after turning to the Y dirrection: ", robot.pose) await robot.drive_straight(distance=distance_mm(directions.position.y * grid.scale), speed=speed_mmps(80)).wait_for_completed() print("Robot is at after driving in the Y dirreciton: ", robot.pose)
async def go_to_cube(self, cube): await self._robot.set_lift_height(1, duration=0.5).wait_for_completed() await self._robot.go_to_object( cube, distance_mm(30.0) ).wait_for_completed() self._wait_for_tap_flag = True await self.coz_tap(cube)
def returnToCharger(robot): robot.turn_in_place(degrees(-180)).wait_for_completed() #looks for 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: pass # look for charger here if not charger: look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=5) except asyncio.TimeoutError: print("Didn't see the charger") finally: look_around.stop() # if charger, dock with it 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() robot.turn_in_place(cozmo.util.degrees(180)).wait_for_completed() robot.drive_straight(cozmo.util.distance_mm(-175), cozmo.util.speed_mmps(50)).wait_for_completed() print("Done.")
def forward(self, robot: cozmo.robot.Robot = None, cmd_args=None, invert=False): if self.log: print(cmd_args) drive_duration = extract_next_float(cmd_args) #[0] if drive_duration is not None: if invert: drive_speed = speed drive_duration = -drive_duration drive_dir = "backwards" else: drive_speed = speed drive_dir = "forward" #robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration) robot.drive_straight(distance_mm(drive_duration * drive_speed), speed_mmps(drive_speed), should_play_anim=True).wait_for_completed() #time.sleep(drive_duration) return "I drove " + drive_dir + " for " + str( drive_duration) + " seconds!" return "Error: bad drive duration!"
def robot_step(self, robot: cozmo.robot.Robot): if not robot.is_cliff_detected: robot.drive_straight(distance_mm(50), speed_mmps(70)).wait_for_completed() else: print('cliff detected')
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) print("--------------------------") print("Battery (below 3.5V is low)") print(robot.world.robot.battery_voltage) print("--------------------------") robot.world.add_event_handler(cozmo.objects.EvtObjectTapped, object_tapped) # Initialization 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.start_freeplay_behaviors() while True: time.sleep(1) # Run mini games run = random.randint(0, 100) global interaction if run == 1: interaction = 1 useless(robot) interaction = 0
async def _final_adjust_async(self, charger: Charger, dist_charger=40, speed=40, critical=False) -> None: # Final adjustement to properly face the charger. # The position can be adjusted several times if # the precision is critical, i.e. when climbing # back onto the charger. while (True): # Calculate positions r_coord = [0, 0, 0] c_coord = [0, 0, 0] # Coordonates of robot and charger # .x .y .z, .rotation otherwise r_coord[0] = self._robot.pose.position.x r_coord[1] = self._robot.pose.position.y r_coord[2] = self._robot.pose.position.z r_zRot = self._robot.pose_angle.radians # .degrees or .radians c_coord[0] = charger.pose.position.x c_coord[1] = charger.pose.position.y c_coord[2] = charger.pose.position.z c_zRot = charger.pose.rotation.angle_z.radians # Create target position # dist_charger in mm, distance if front of charger c_coord[0] -= dist_charger * math.cos(c_zRot) c_coord[1] -= dist_charger * math.sin(c_zRot) # Direction and distance to target position (in front of charger) distance = math.sqrt((c_coord[0] - r_coord[0])**2 + (c_coord[1] - r_coord[1])**2 + (c_coord[2] - r_coord[2])**2) vect = [ c_coord[0] - r_coord[0], c_coord[1] - r_coord[1], c_coord[2] - r_coord[2] ] # Angle of vector going from robot's origin to target's position theta_t = math.atan2(vect[1], vect[0]) print('CHECK: Adjusting position') # Face the target position angle = self._clip_angle(theta_t - r_zRot) await self._robot.turn_in_place(radians(angle) ).wait_for_completed() # Drive toward the target position await self._robot.drive_straight( distance_mm(distance), speed_mmps(speed)).wait_for_completed() # Face the charger angle = self._clip_angle(c_zRot - theta_t) await self._robot.turn_in_place(radians(angle) ).wait_for_completed() # In case the robot does not need to climb onto the charger if not critical: break elif (await self._check_tol_async(charger, dist_charger)): print('CHECK: Robot aligned relativ to the charger.') break return
async def get_off_charger_async(self): print("Getting off charger") if self._robot.is_on_charger: await self._robot.drive_off_charger_contacts( self._robot).wait_for_completed() await self._robot.drive_straight( distance_mm(100), speed_mmps(100)).wait_for_completed()
async def run(self): '''Program runs until typing CRTL+C into Terminal/Command Prompt, or by closing the viewer window. ''' if not self.cubes_connected(): print('Cubes did not connect successfully - check that they are nearby. You may need to replace the batteries.') return self.turn_on_cubes() await self.robot.drive_straight(distance_mm(100), speed_mmps(50), should_play_anim = False).wait_for_completed() # Updates self.state and resets self.amount_turned_recently every 1 second. # MODIFICATION FROM ORIGINAL SDK = while loop would run contiously until an abort signal was dectected. # variable running remains True until Cozmo detects the color passed and changes to False # if False, then run returns giving control back to main.py while running: await asyncio.sleep(1) print(self.color_to_find) print(self.state) print(running) if self.state == LOOK_AROUND_STATE: await self.start_lookaround() print("In look around state") if self.state == FOUND_COLOR_STATE and self.amount_turned_recently < self.moving_threshold: self.state = DRIVING_STATE self.amount_turned_recently = radians(0) if running == False: self.robot.abort_all_actions() return
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(cozmo_robot: cozmo.robot.Robot): global robot init_robot(cozmo_robot) # Get off charger if on it if(robot.is_on_charger): robot.drive_off_charger_contacts().wait_for_completed() robot.drive_straight(distance_mm(110),speed_mmps(80)).wait_for_completed() execute_procedure() return
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False): # Final adjustement to properly face the charger. # The position can be adjusted several times if # the precision is critical, i.e. when climbing # back onto the charger. global robot,PI while(True): # Calculate positions r_coord = [0,0,0] c_coord = [0,0,0] # Coordonates of robot and charger r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise r_coord[1] = robot.pose.position.y r_coord[2] = robot.pose.position.z r_zRot = robot.pose_angle.radians # .degrees or .radians c_coord[0] = charger.pose.position.x c_coord[1] = charger.pose.position.y c_coord[2] = charger.pose.position.z c_zRot = charger.pose.rotation.angle_z.radians # Create target position # dist_charger in mm, distance if front of charger c_coord[0] -= dist_charger*math.cos(c_zRot) c_coord[1] -= dist_charger*math.sin(c_zRot) # Direction and distance to target position (in front of charger) distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2) vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]] # Angle of vector going from robot's origin to target's position theta_t = math.atan2(vect[1],vect[0]) print('CHECK: Adjusting position') # Face the target position angle = clip_angle((theta_t-r_zRot)) robot.turn_in_place(radians(angle)).wait_for_completed() # Drive toward the target position robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed() # Face the charger angle = clip_angle((c_zRot-theta_t)) robot.turn_in_place(radians(angle)).wait_for_completed() # In case the robot does not need to climb onto the charger if not critical: break elif(check_tol(charger,dist_charger)): print('CHECK: Robot aligned relativ to the charger.') break return
def run(sdk_conn): '''The run method runs once Cozmo is connected.''' robot = sdk_conn.wait_for_robot() robot.set_all_backpack_lights(cozmo.lights.red_light) robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed() # make cozmo reverse 100mm (negative distance == reverse) robot.drive_straight(distance=distance_mm(-100), speed=speed_mmps(50)).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.ReactToCliff).wait_for_completed() robot.say_text("Hello").wait_for_completed()
def go_to_charger(): # Driving towards charger without much precision global robot charger = None ''' cf. 08_drive_to_charger_test.py ''' # see if Cozmo already knows where the charger is if robot.world.charger: # make sure Cozmo was not delocalised after observing the 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: charger = find_charger() action = robot.go_to_object(charger,distance_from_object=distance_mm(80), in_parallel=False, num_retries=5) #action = robot.go_to_pose(charger.pose) action.wait_for_completed() return charger
def knock_cubes_over(): # This function allows to detect if the three cubes are stacked on each others. # If it is the case, Cozmo will try to make them fall. global robot cube1 = robot.world.get_light_cube(LightCube1Id) cube2 = robot.world.get_light_cube(LightCube2Id) cube3 = robot.world.get_light_cube(LightCube3Id) retries = 5 i = 0 for i in range(0,5): # Try to see at least 1 cube behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: seen_cube = robot.world.wait_for_observed_light_cube(timeout=10,include_existing=True) except: seen_cube = None behavior.stop() # Try to observe possible stacked cubes robot.set_head_angle(degrees(10)).wait_for_completed() time.sleep(.5) robot.set_head_angle(degrees(30)).wait_for_completed() time.sleep(.5) robot.set_head_angle(degrees(0)).wait_for_completed() num_observed_cubes = 0 if(cube1.pose.is_comparable(robot.pose)): num_observed_cubes += 1 if(cube2.pose.is_comparable(robot.pose)): num_observed_cubes += 1 if(cube3.pose.is_comparable(robot.pose)): num_observed_cubes += 1 if(num_observed_cubes == 3): # All cubes were observed, check if stacked ref = [] ref.append(cube1.pose.position.x) ref.append(cube1.pose.position.y) tol = 20 # Less than 20 mm means that the cubes are stacked delta2 = math.sqrt((ref[0]-cube2.pose.position.x)**2 + (ref[1]-cube2.pose.position.y)**2) delta3 = math.sqrt((ref[0]-cube3.pose.position.x)**2 + (ref[1]-cube3.pose.position.y)**2) if(delta2 < tol and delta3 < tol): try: behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.KnockOverCubes) behavior.wait_for_started(timeout=10) behavior.wait_for_completed(timeout=None) except asyncio.TimeoutError: print('WARNING: Timeout exception raised from behavior type KnockOverCubes.') except: print('WARNING: An exception was raised from behavior type KnockOverCubes.') else: robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed() return 1 else: robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed() return 1 if(i >= 4): return 0
async def desk_security_guard(robot): '''The core of the desk_security_guard program''' # Turn on image receiving by the camera robot.camera.image_stream_enabled = True # Connect Twitter, run async in the background twitter_api, twitter_auth = twitter_helpers.init_twitter(twitter_keys) stream_to_app_comms = TwitterStreamToAppCommunication() stream_listener = SecurityGuardStreamListener(twitter_api, stream_to_app_comms) twitter_stream = twitter_helpers.CozmoStream(twitter_auth, stream_listener) twitter_stream.async_userstream(_with='user') # Create our security guard dsg = DeskSecurityGuard(twitter_api) # Make sure Cozmo is clear of the charger if robot.is_on_charger: # Drive fully clear of charger (not just off the contacts) await robot.drive_off_charger_contacts().wait_for_completed() await robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() # Tilt head up to look for people await robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed() initial_pose_angle = robot.pose_angle patrol_offset = 0 # middle max_pose_angle = 45 # offset from initial pose_angle (up to +45 or -45 from this) # Time to wait between each turn and patrol, in seconds time_between_turns = 2.5 time_between_patrols = 20 time_for_next_turn = time.time() + time_between_turns time_for_next_patrol = time.time() + time_between_patrols while True: # Handle any external requests to arm or disarm Cozmo if stream_to_app_comms.has_arm_request: stream_to_app_comms.has_arm_request = False if not dsg.is_armed: print("Alarm Armed") dsg.is_armed = True if stream_to_app_comms.has_disarm_request: stream_to_app_comms.has_disarm_request = False if dsg.is_armed: print("Alarm Disarmed") dsg.is_armed = False stream_to_app_comms.is_armed = dsg.is_armed # Turn head every few seconds to cover a wider field of view # Only do this if not currently investigating an intruder if (time.time() > time_for_next_turn) and not dsg.is_investigating_intruder(): # pick a random amount to turn angle_to_turn = randint(10,40) # 50% chance of turning in either direction if randint(0,1) > 0: angle_to_turn = -angle_to_turn # Clamp the amount to turn face_angle = (robot.pose_angle - initial_pose_angle).degrees face_angle += angle_to_turn if face_angle > max_pose_angle: angle_to_turn -= (face_angle - max_pose_angle) elif face_angle < -max_pose_angle: angle_to_turn -= (face_angle + max_pose_angle) # Turn left/right await robot.turn_in_place(degrees(angle_to_turn)).wait_for_completed() # Tilt head up/down slightly await robot.set_head_angle(degrees(randint(30,44))).wait_for_completed() # Queue up the next time to look around time_for_next_turn = time.time() + time_between_turns # Every now and again patrol left and right between 3 patrol points if (time.time() > time_for_next_patrol) and not dsg.is_investigating_intruder(): # Check which way robot is facing vs initial pose, pick a new patrol point face_angle = (robot.pose_angle - initial_pose_angle).degrees drive_right = (patrol_offset < 0) or ((patrol_offset == 0) and (face_angle > 0)) # Turn to face the new patrol point if drive_right: await robot.turn_in_place(degrees(90 - face_angle)).wait_for_completed() patrol_offset += 1 else: await robot.turn_in_place(degrees(-90 - face_angle)).wait_for_completed() patrol_offset -= 1 # Drive to the patrol point, playing animations along the way await robot.drive_wheels(20, 20) for i in range(1,4): await robot.play_anim("anim_hiking_driving_loop_0" + str(i)).wait_for_completed() # Stop driving robot.stop_all_motors() # Turn to face forwards again face_angle = (robot.pose_angle - initial_pose_angle).degrees if face_angle > 0: await robot.turn_in_place(degrees(-90)).wait_for_completed() else: await robot.turn_in_place(degrees(90)).wait_for_completed() # Queue up the next time to patrol time_for_next_patrol = time.time() + time_between_patrols # look for intruders await check_for_intruder(robot, dsg) # Sleep to allow other things to run await asyncio.sleep(0.05)
def clean_up_cubes(): global cubeIDs,SIDE cubeIDs = [] switch_cube_off(robot.world.get_light_cube(LightCube1Id)) switch_cube_off(robot.world.get_light_cube(LightCube2Id)) switch_cube_off(robot.world.get_light_cube(LightCube3Id)) go_to_charger() turn_around() # I. Find first cube and put it next to charger cube = look_for_next_cube() if not cube: return switch_cube_on(cube) print(cubeIDs) success = pickUp_cube(cube) if not success: switch_cube_off(cube) return charger = go_to_charger() final_adjust(charger,60) robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed() putDown_cube(cube) switch_cube_off(cube) # Get back in front of charger robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() # II. Find next cube and stack on first placed cube cube = look_for_next_cube() if(cube == None): return switch_cube_on(cube) print(cubeIDs) success = pickUp_cube(cube) if not success: switch_cube_off(cube) return charger = go_to_charger() robot.turn_in_place(degrees(SIDE*70)).wait_for_completed() #SIDE*180 stack_cube(cubes[0]) switch_cube_off(cube) # Get back in front of charger #robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed() charger = go_to_charger() final_adjust(charger,100,80) robot.turn_in_place(degrees(SIDE*180)).wait_for_completed() #-SIDE*70 robot.set_lift_height(height=0,max_speed=10,in_parallel=False).wait_for_completed() #robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed() # III. Go get the last cube and lay it in front of the others cube = look_for_next_cube() if(cube == None): return switch_cube_on(cube) print(cubeIDs) success = pickUp_cube(cube) if not success: switch_cube_off(cube) return turn_around() charger = go_to_charger() final_adjust(charger,80) robot.turn_in_place(degrees(SIDE*60)).wait_for_completed() robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed() putDown_cube(cube) switch_cube_off(cube) # Get back in front of charger robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed() robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed() robot.drive_straight(distance_mm(50),speed_mmps(40)).wait_for_completed() turn_around() return