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 cozmo_program(robot: cozmo.robot.Robot): # Drive forwards for 150 millimeters at 50 millimeters-per-second. robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() # Turn 90 degrees to the left. # Note: To turn to the right, just use a negative number. robot.turn_in_place(degrees(90)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): if robot.is_on_charger: robot.stop_all_motors else: robot.say_text(name + "made me move" + distance + "millimeters").wait_for_completed() robot.drive_straight(distance_mm(true_distance), speed_mmps(50)).wait_for_completed()
def move_left(robot: cozmo.robot.Robot, move): """ Moves the robot to the left, in case the path is too far on the right """ log.info('Move left...') robot.turn_in_place(degrees(45)).wait_for_completed() robot.drive_straight(distance_mm(move), speed_mmps(speed)).wait_for_completed() robot.turn_in_place(degrees(-45)).wait_for_completed()
def check_charger(self, robot: cozmo.robot.Robot, distance=150, speed=100): if robot.is_on_charger: if self.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 drive_in_square(robot: cozmo.robot.Robot): print("Driving in square") # 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(150), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed()
def run(robot: cozmo.robot.Robot): '''The run method runs once the Cozmo SDK is connected.''' #add annotators for battery level and ball bounding box robot.world.image_annotator.add_annotator('battery', BatteryAnnotator) robot.world.image_annotator.add_annotator('ball', BallAnnotator) try: robot.set_head_angle(degrees(0)).wait_for_completed() while True: #get camera image event = robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) #convert camera image to opencv format opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY) #find the ball ball = None for i in range(10): ball = find_ball.find_ball(opencv_image) if ball is not None: break if ball is None: robot.turn_in_place(degrees(10)).wait_for_completed() continue #set annotator ball BallAnnotator.ball = ball # Turn toward the ball # If the ball still far away, going toward it while ball[2] < 130: robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed() ball = None turn = 1 # Adjust the angel to turn toward the ball while ball is None: event = robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY) ball = find_ball.find_ball(opencv_image) # Do some angle adjustment in case the ball is a bit off to the side robot.turn_in_place(degrees(turn)).wait_for_completed() turn = turn * (-2) time.sleep(0.1) horizontal_turn = int(float(-ball[0] + len(opencv_image[0])/2) / len(opencv_image[0]) * 25) robot.turn_in_place(degrees(horizontal_turn)).wait_for_completed() # Once the ball is big enough, drive a little bit toward it and hit it robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed() robot.move_lift(5) time.sleep(2) robot.move_lift(-5) except KeyboardInterrupt: print("Exit requested by user") except cozmo.RobotBusy as e: print('here') print(e)
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 move_cozmo(robot: cozmo.robot.Robot, movement, direction): if direction < 0: turn_right(robot) elif direction > 0: turn_left(robot) if movement > 0: robot.drive_straight(distance_mm(movement), speed_mmps(200)).wait_for_completed()
def run_cozmo(coz: cozmo.robot.Robot): reset_lift_position(coz) reset_head_position(coz, False, True) coz.drive_straight(distance_mm(-50), speed_mmps(100)).wait_for_completed() coz.drive_straight(distance_mm(80), speed_mmps(100)).wait_for_completed() while True: time.sleep(1)
async def run(robot: cozmo.robot.Robot): '''The run method runs once the Cozmo SDK is connected.''' label_old = "none" duration_s = 5 img_clf = imgclassification.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) print("model trained") count = 0 try: while True: #get camera image event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) #convert camera image to opencv format opencv_image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY) image = np.array(event.image) cv2.imwrite("./robot/drone_10T094634322772.bmp", cv2.cvtColor(image, cv2.COLOR_RGB2BGR)) (test_raw, test_labels) = img_clf.load_data_from_folder('./robot/') test_data = img_clf.extract_image_features(test_raw) label_new = img_clf.predict_labels(test_data) print("here") print(label_new[0]) if(label_old == label_new[0]): count = count + 1 else: count = 0 label_old = label_new[0] if(label_old == "plane" and count == 2): robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() time.sleep(duration_s) robot.say_text(label_old, voice_pitch=-1.0, duration_scalar=0.3).wait_for_completed() time.sleep(duration_s) elif(label_old == "truck" and count == 2): robot.drive_straight(distance_mm(-150), speed_mmps(50)).wait_for_completed() time.sleep(duration_s) robot.say_text(label_old, voice_pitch=-1.0, duration_scalar=0.3).wait_for_completed() time.sleep(duration_s) elif(label_old == "order" and count == 2): robot.move_head(-0.15) time.sleep(duration_s) robot.say_text(label_old, voice_pitch=-1.0, duration_scalar=0.3).wait_for_completed() time.sleep(duration_s) time.sleep(1) except KeyboardInterrupt: print("") print("Exit requested by user") except cozmo.RobotBusy as e: print(e)
def action_on_seeing_object(robot: cozmo.robot.Robot): if (isLeft[0] == True): robot.drive_straight(distance_mm(100), speed_mmps(70)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() if (isRight[0] == True): robot.drive_straight(distance_mm(100), speed_mmps(70)).wait_for_completed() robot.turn_in_place(degrees(-90)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): # uncomment if you want Cozmo to say that he is getting towels #robot.say_text(f"I am going to go get towels").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.drive_straight(distance_mm(300), speed_mmps(100)).wait_for_completed()
def take_photos(robot: cozmo.robot.Robot): global liveCamera # Start photo sesh liveCamera = True time.sleep(1) robot.drive_straight(distance_mm(100), speed_mmps(100), False, False, 0).wait_for_completed() robot.drive_straight(distance_mm(-100), speed_mmps(100), False, False, 0).wait_for_completed() # Stop photo sesh liveCamera = False time.sleep(1)
def cozmo_program(robot: cozmo.robot.Robot): time.sleep(2) robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed() #go forward time.sleep(1) for x in range(0, 1): #lift head robot.move_head(1) robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabAmazed).wait_for_completed() #amazed
def workloop(robot: cozmo.robot.Robot): global ZielmaschVar global Zielmaschine robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared) robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared) Maschine1 = robot.world.define_custom_cube(CustomObjectTypes.CustomType00, CustomObjectMarkers.Circles2, 50, 50, 50, True) Maschine2 = robot.world.define_custom_cube(CustomObjectTypes.CustomType01, CustomObjectMarkers.Circles3, 50, 50, 50, True) Maschine3 = robot.world.define_custom_cube(CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles4, 50, 50, 50, True) Maschine4 = robot.world.define_custom_cube(CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles5, 50, 50, 50, True) Maschine5 = robot.world.define_custom_cube(CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds2, 50, 50, 50, True) robot.set_head_angle(degrees(7)).wait_for_completed() # dreh dich und such nach der maschine lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) targ = robot.world.wait_for_observed_light_cube() lookaround.stop() robot.pickup_object(targ, num_retries=3).wait_for_completed() robot.set_head_angle(degrees(7)).wait_for_completed() lookForMachine(robot) robot.turn_in_place(degrees(5)).wait_for_completed() #Masch = robot.world.wait_until_observe_num_objects(num=1, object_type=cozmo.objects.CustomObject, timeout=5) robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() robot.place_object_on_ground_here(targ).wait_for_completed() MAschinenwechsler() # such den würfel # lade den Würfel auf # fahr zutrück zur Mitte # dreh und scanne nach der nächsten maschine # positioniere dich zur maschine # lad den würfel ab while True: time.sleep(0.5)
def order(robot: cozmo.robot.Robot): # Robot at D, cube at C, pick up cube, drive forward to end of arena (A), put down cube, drive backward to D robot.set_head_angle(degrees(-5.0)).wait_for_completed() robot.set_lift_height(height=0).wait_for_completed() cube = robot.world.wait_for_observed_light_cube(timeout=30) robot.pickup_object(cube, num_retries=5).wait_for_completed() robot.drive_straight(distance_mm(300), speed_mmps(50)).wait_for_completed() robot.place_object_on_ground_here(cube).wait_for_completed() robot.drive_straight(distance_mm(-355), speed_mmps(50)).wait_for_completed() idle(robot)
def cozmo_movement(robot: cozmo.robot.Robot, scalar, speed=150): """ Asynchronous wrapper function that moves cozmo linearly. :param robot: cozmo robot object :param scalar: scalar of distance to move cozmo :param speed: speed that cozmo should move at """ # TODO: Change speed parameter to do some cool scalar math robot.drive_straight(cozmo.util.distance_mm(scalar), cozmo.util.speed_mmps(speed)).wait_for_completed()
def cozmo_update_position(robot: cozmo.robot.Robot): for i in range(6): acon = robot.drive_straight(distance_mm(50), speed_mmps(50)) recod(acon, robot) acon = robot.turn_in_place(degrees(60)) recod(acon, robot) acon = robot.drive_straight(distance_mm(340), speed_mmps(50)) recod(acon, robot) data = np.array(arr) print(arr) np.save("hextest10", data)
def cozmo_program(robot: cozmo.robot.Robot): counter = 0 while counter != 100: # Drive forwards for 150 millimeters at 50 millimeters-per-second. robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(90), speed=cozmo.util.Angle(0.5)).wait_for_completed() counter += 1
def cozmo_program(robot: cozmo.robot.Robot): # falls der Lift oben ist wird er runter geholt robot.set_lift_height(0.0).wait_for_completed() # dreht sich um 360° nach rechts robot.turn_in_place(degrees(-360)).wait_for_completed() # dreht sich um 360° nach links robot.turn_in_place(degrees(360)).wait_for_completed() # faehrt 100 millimeters vorwaerts mit 150 millimeters-per-second. robot.drive_straight(distance_mm(100), speed_mmps(150)).wait_for_completed() # faehrt 100 millimeters rueckwaerts mit 150 millimeters-per-second. robot.drive_straight(distance_mm(-100), speed_mmps(150)).wait_for_completed()
def inspection(robot: cozmo.robot.Robot): for i in range(0, 4): if i % 2 == 0: robot.move_lift(0.25) else: robot.move_lift(-0.25) robot.say_text("I am not a spy", in_parallel=True) robot.drive_straight(distance_mm(200), speed_mmps(50), in_parallel=True).wait_for_completed() robot.turn_in_place(degrees(90), in_parallel=True).wait_for_completed() robot.move_lift(-1.0) idle(robot)
def cozmo_program(robot: cozmo.robot.Robot): # falls der Lift oben ist wird er runter geholt robot.set_lift_height(0.0).wait_for_completed() # dreht sich um 45° nach links robot.turn_in_place(degrees(45)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # dreht sich um 90° nach rechts robot.turn_in_place(degrees(-90)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # dreht sich um 45° nach links robot.turn_in_place(degrees(45)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # faehrt 150 millimeters rueckwaerts mit 50 millimeters-per-second. robot.drive_straight(distance_mm(-150), speed_mmps(150)).wait_for_completed() # dreht sich um 45° nach links robot.turn_in_place(degrees(45)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # dreht sich um 90° nach rechts robot.turn_in_place(degrees(-90)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # dreht sich um 45° nach links robot.turn_in_place(degrees(45)).wait_for_completed() # Lift geht hoch und wieder runter (at 5 radians per second) robot.move_lift(5) time.sleep(0.5) robot.move_lift(-5) time.sleep(0.5) # faehrt 150 millimeters vorwaerts mit 50 millimeters-per-second. robot.drive_straight(distance_mm(150), speed_mmps(150)).wait_for_completed()
def drive_to(robot: cozmo.robot.Robot, pos): nx, ny = pos rx, ry = convertToGridCoord(robot.pose) dx = nx - rx dy = ny - ry rz = robot.pose.rotation.angle_z rotz = math.degrees(math.atan2(dy + .6, dx + .6)) robot.turn_in_place(degrees(rotz) - rz).wait_for_completed() rx, ry = convertToGridCoord(robot.pose) dx = nx - rx dy = ny - ry rotd = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) robot.drive_straight(distance_mm(rotd * 25), speed_mmps(50)).wait_for_completed()
def drone(robot: cozmo.robot.Robot): # The robot should locate one of the cubes #(one will be placed in front of it within view), #pick up the cube, drive forward with the cube for 10cm, #put down the cube, and drive backward 10cm robot.say_text(str('drone')).wait_for_completed() robot.set_lift_height(height=0).wait_for_completed() cube = robot.world.wait_for_observed_light_cube(timeout=30) robot.pickup_object(cube, num_retries=5).wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.place_object_on_ground_here(cube).wait_for_completed() robot.drive_straight(distance_mm(-100), speed_mmps(50)).wait_for_completed() idle(robot)
def light(robot: cozmo.robot.Robot): cube1 = robot.world.get_light_cube(LightCube1Id) cube2 = robot.world.get_light_cube(LightCube2Id) cube3 = robot.world.get_light_cube(LightCube3Id) robot.move_head(1) robot.move_lift(-1) cube1.set_lights(cozmo.lights.green_light) robot.drive_straight(distance_mm(270), speed_mmps(40)).wait_for_completed() cube1.set_lights(cozmo.lights.red_light) robot.move_lift(1) robot.drive_straight(distance_mm(25), speed_mmps(20)).wait_for_completed() robot.move_lift(-1) cube1.set_lights(cozmo.lights.blue_light) time.sleep(10)
def cozmo_program(robot: cozmo.robot.Robot): robot.drive_straight(distance_mm(150), speed_mmps(30)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() robot.drive_straight(distance_mm(280), speed_mmps(30)).wait_for_completed() robot.turn_in_place(degrees(180)).wait_for_completed() robot.drive_straight(distance_mm(280), speed_mmps(30)).wait_for_completed() robot.turn_in_place(degrees(-90)).wait_for_completed() robot.drive_straight(distance_mm(150), speed_mmps(30)).wait_for_completed()
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 robot.say_text("Path one").wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabRooster, in_parallel=True) robot.drive_straight(distance_mm(100), speed_mmps(50), should_play_anim=True, in_parallel=True).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() robot.say_text("Path two").wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabPartyTime, in_parallel=True) robot.drive_straight(distance_mm(100), speed_mmps(50)) should_play_anim=True, in_parallel=True).wait_for_completed()
def main(robot: cozmo.robot.Robot): while 1: events = get_gamepad() for event in events: print(event.code, event.state) if event.code == "ABS_RZ" and event.state > 150: robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() if event.code == "ABS_Z" and event.state > 150: robot.drive_straight(distance_mm(-150), speed_mmps(50)).wait_for_completed() if event.code == "ABS_HAT0Y" and event.state == -1: robot.move_lift(5) if event.code == "ABS_HAT0Y" and event.state == 1: robot.move_lift(-5)
def order(robot: cozmo.robot.Robot): # Place a cube at point C on the arena. Start your robot at point D on the arena and directly # face the cube. The robot should locate the cube (any cube if you have more than one), pick up the # cube, drive forward with the cube to the end of the arena (point A), put down the cube, and drive # backward to the robot’s starting location. Then return to the Idle state. robot.say_text(str('order')).wait_for_completed() robot.set_lift_height(height=0).wait_for_completed() cube = robot.world.wait_for_observed_light_cube(timeout=5) robot.pickup_object(cube, num_retries=2).wait_for_completed() robot.drive_straight(distance_mm(275), speed_mmps(77)).wait_for_completed() robot.place_object_on_ground_here(cube).wait_for_completed() robot.drive_straight(distance_mm(-275), speed_mmps(77)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): "robot.enable_stop_on_cliff(True)" clockwise = True for edge in range(4): while not robot.is_cliff_detected: robot.drive_wheel_motors(50.0, 50.0, l_wheel_acc=0, r_wheel_acc=0) robot.stop_all_motors() #dimensions[edge] = distance_mm()# "back up" time.sleep(0.5) robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() if clockwise: "Check left turn" robot.turn_in_place(degrees(90)).wait_for_completed() time.sleep(0.5) robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed() "left side is edge" if robot.is_cliff_detected: "back up" time.sleep(0.5) robot.drive_straight(distance_mm(-10), speed_mmps(50)).wait_for_completed() robot.turn_in_place(degrees(-180)).wait_for_completed() time.sleep(0.5) robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed() if robot.is_cliff_detected: break else: clockwise = False else: robot.turn_in_place(degrees(-90)).wait_for_completed() time.sleep(0.5) robot.drive_straight(distance_mm(10), speed_mmps(50)).wait_for_completed() "left side is edge" if robot.is_cliff_detected: break "checking the dimensions to determine whether it is a rectangle" '''print(dimensions[0]) print(dimensions[1]) print(dimensions[2]) print(dimensions[3]) if dimensions[2] - dimensions[0] <= distance_mm(25) and dimensions[3] - dimensions[1] <= distance_mm(25): ''' robot.say_text("The environment is a rectangle").wait_for_completed()