def play_animation(robot: cozmo.robot.Robot, anim_trig, body = False, para = False): # anim_trig = cozmo.anim.Triggers."Name of trigger" (this is an object) # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers" for animations' triggers # Refer to "http://cozmosdk.anki.com/docs/generated/cozmo.robot.html#cozmo.robot.Robot.play_anim_trigger" for playing the animation robot.play_anim_trigger(anim_trig,loop_count = 1, in_parallel = para, num_retries = 0, use_lift_safe = False, ignore_body_track = body, ignore_head_track=False, ignore_lift_track=False).wait_for_completed()
def level(robot: cozmo.robot.Robot): global LIFT_POSITION init(robot) robot.set_lift_height(0.8).wait_for_completed() LIFT_POSITION = robot._lift_position.ratio setBackpackColors() robot.set_lift_height(0.0).wait_for_completed() celebrate(robot) return
def find_face(robot: cozmo.robot.Robot): # Look around for a face behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.FindFaces) try: face_to_follow = robot.world.wait_for_observed_face(timeout=5) except: face_to_follow = None # Stop looking around behavior.stop() if(face_to_follow): turn_action = robot.turn_towards_face(face_to_follow).wait_for_completed()
def init_cubes(robot: cozmo.robot.Robot): global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light 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' # Make sure each cube is off switch_cubes_off() # Add listener to control initialisation handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tap_init) seq = ['red','green','blue'] light_seq = [cozmo.lights.red_light,cozmo.lights.green_light,cozmo.lights.blue_light] cube_seq = [cube1,cube2,cube3] comm_seq = ['g','c','s'] # greater,correct,smaller for i in range(0,3): print('Tap the ',seq[i],' cube') switch_cube_on(cube_seq[i],light_seq[i]) while(True): if(ev_id): # Map event to cube ev_cube[ev_id] = cube_seq[i] ev_command[ev_id] = comm_seq[i] ev_light[ev_id] = light_seq[i] make_selected_cube_blink(ev_cube[ev_id],light_seq[i]) switch_cubes_off() ev_id = 0 break handler.disable()
def get_answer_from_cubes(robot: cozmo.robot.Robot): global cube1,cube2,cube3,ev_cube,ev_id,ev_command,ev_light timeOut = 30 timer = 0 switch_cubes_on() handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped, handle_tapped) while(True): time.sleep(0.5) timer += 0.5 if(ev_id): switch_cubes_off() make_selected_cube_blink(ev_cube[ev_id],ev_light[ev_id]) local_id = ev_id ev_id = 0 # Deactivate selection handler.disable() return ev_command[local_id] elif(timer > timeOut): handler.disable() return 'timeOut'
def cozmo_program(robot: cozmo.robot.Robot): # Play an animation via a Trigger - see: # http://cozmosdk.anki.com/docs/generated/cozmo.anim.html#cozmo.anim.Triggers # for a list of available triggers. # A trigger can pick from several appropriate animations for variety. # robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionSoft).wait_for_completed() # robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionMedium).wait_for_completed() # robot.play_anim_trigger(cozmo.anim.Triggers.DizzyReactionHard).wait_for_completed() robot.start_behavior(cozmo.behavior.BehaviorTypes.StackBlocks).wait_for_completed(); robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpIdle).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpLeftHanging).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpSuccess).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake).wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed() # Play an animation via its Name. # Warning: Future versions of the app might change these, so for future-proofing # we recommend using play_anim_trigger above instead. # See the remote_control_cozmo.py example in apps for an easy way to see # the available animations. robot.play_anim(name="id_poked_giggle").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()
async def loopPath(self, robot: cozmo.robot.Robot): if robot.is_on_charger: await robot.drive_off_charger_contacts().wait_for_completed() self.initialPose = robot.pose self.pathPoseTrack = self.track.getPathPoseTrack(self.forwardSpeed) self.driveOnRoad = True self.stopped = False self.waitForOrder = False self.waitForAnimation = False self.offsetPixel = 0.0 self.deliveryCount = 0 self.attentionCount = 0 print("start drive") ## await robot.drive_wheels(FORWARD_SPEED, FORWARD_SPEED) print(self.pathPoseTrack.distance) await robot.drive_straight( distance_mm(self.pathPoseTrack.distance), speed_mmps(self.forwardSpeed)).wait_for_completed() print(self.pathPoseTrack.edge.end.id) while not self.stopped: # basic update of straight drive # picking next edge if the current is finished if self.driveOnRoad: ## self.pathPoseTrack.update(FRAME_DURATION, FORWARD_SPEED) # finish this path, because drive_straight is used and waited to finish self.pathPoseTrack.update(999, self.forwardSpeed) # did the last auto delivery if self.deliveryCount > self.maxDelivery: await robot.play_anim_trigger(random.choice(ATTENTION_TRIGGERS) ).wait_for_completed() self.attentionCount = self.attentionCount + 1 # end of the path elif self.pathPoseTrack.consumeRouteEndSignal(): self.driveOnRoad = False # stop before turn robot.stop_all_motors() # turning is included in go to pose self.pathPoseTrack.consumeEdgeChangeSignal() # go to loading area and drop cube # lift cube, go back to road bldgId = self.pathPoseTrack.edge.start.id if bldgId != "GA": await self.deliverItem( robot, self.pathPoseTrack.edge.start, self.pathPoseTrack.path.lastTurnRight) else: await self.backInGarage(robot, False) self.stopped = True if self.stopped: break # continue auto delivery if self.deliveryCount <= self.maxDelivery: await self.depart(robot) print("Move towards: %s" % self.pathPoseTrack.edge.end.id) # at any road intersection elif self.pathPoseTrack.consumeEdgeChangeSignal(): self.driveOnRoad = False diff = self.pathPoseTrack.angleDiff angleAbs = self.pathPoseTrack.angleAbs # a turn is about to be made if diff < -0.1 or diff > 0.1: # stop before turn robot.stop_all_motors() # make a turn angle = radians(angleAbs + self.initialPose.rotation.angle_z.radians - robot.pose.rotation.angle_z.radians) await robot.turn_in_place(angle).wait_for_completed() print("turn of angle: ", angle.degrees) # restart motion ## await robot.drive_wheels(FORWARD_SPEED, FORWARD_SPEED) anim_done = False while anim_done is False: try: await robot.drive_straight( distance_mm(self.pathPoseTrack.distance), speed_mmps( self.forwardSpeed)).wait_for_completed() anim_done = True except cozmo.exceptions.RobotBusy: await asyncio.sleep(0.1) self.driveOnRoad = True print("Move towards: %s" % self.pathPoseTrack.edge.end.id) # let Cozmo drive straight for a short time before updates await asyncio.sleep(FRAME_DURATION) robot.stop_all_motors()
def cozmo_program(robot: cozmo.robot.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 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 cozmo_program(robot: cozmo.robot.Robot): global angle angle = 25. robot.set_head_angle(degrees(angle)).wait_for_completed() robot.set_lift_height(0.0).wait_for_completed() robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = True robot.camera.enable_auto_exposure = True os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3' frame = os.path.join(directory, "current.jpeg") print("Starting Tensorflow...") with tf.Session() as sess: print("Session successfully started") model = load_model('modelv1.07-0.96.hdf5') while True: global X, Y, W, H global result X = 245. Y = 165. W = 150. H = 150. gt = [X, Y, W, H] pos_x, pos_y, target_w, target_h = region_to_bbox(gt) frame = os.path.join(directory, "current.jpeg") result = 0 dog_counter = 0 cat_counter = 0 background_counter = 0 next_state = 0 current_state = 0 #Background: 0, Cat:1, Dog:2 while True: latest_img = robot.world.latest_image if latest_img is not None: pilImage = latest_img.raw_image pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG") show_frame(np.asarray(Image.open(frame)), [900.,900.,900.,900.], 1) img = load_image(frame) [result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\ ,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img}) next_state = np.argmax(result) print('Arg max: ',next_state) # Initial Current State is Background if current_state == 0: print('Background') if next_state == 1: # Detected a Cat current_state = 1 # Transition to Cat State background_counter = 0 cat_counter = 1 dog_counter = 0 elif next_state == 2: # Detected a Dog current_state = 2 # Transition to Dog state background_counter = 0 cat_counter = 0 dog_counter = 1 # Current State is Cat elif current_state == 1: print('\t\t\t\t\t\tCat') if next_state == 0: # Detected Background background_counter += 1 if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times background_counter = 0 current_state = 0 cat_counter = 0 elif next_state == 1: # Detected Cat itself cat_counter +=1 if cat_counter >= 30: print('Cozmo sees a cat') dense = model.get_layer('dense').get_weights() weights = dense[0].T testing_counter = 0 detected_centroid = 0 xmin_avg = 0 xmax_avg = 0 ymin_avg = 0 ymax_avg = 0 frame_average = 2 frame_count = 0 while True: latest_img = robot.world.latest_image if latest_img is not None: pilImage = latest_img.raw_image pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG") img = load_image(frame) [result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\ ,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img}) kernels = out_relu.reshape(7,7,1280) final = np.dot(kernels,weights[result[0].argmax()]) final1 = array_to_img(final.reshape(7,7,1)) final1 = final1.resize((224,224), Image.ANTIALIAS) box = img_to_array(final1).reshape(224,224) #box = cv2.blur(box,(30,30)) temp = (box > box.max()*.8) *1 temp_adjusted = np.ndarray(shape=np.shape(temp), dtype=np.dtype(np.uint8)) temp_adjusted[:,:] = np.asarray(temp)*255 contours, hierarchy = cv2.findContours(temp_adjusted, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:] contours = np.array(contours) max_area = [0,0] # contours index and area for index, contour in enumerate(contours): if(max_area[1]< len(contour)): max_area = [index,len(contour)] contours_adjusted = contours[max_area[0]].squeeze(axis=1).T xmin = contours_adjusted[0].min() * (640./224.) ymin = contours_adjusted[1].min() * (480./224.) xmax = contours_adjusted[0].max() * (640./224.) ymax = contours_adjusted[1].max() * (480./224.) if result[0].argmax() == 1: # Frame smoothing frame_count = frame_count + 1 xmin_avg = xmin_avg + xmin xmax_avg = xmax_avg + xmax ymin_avg = ymin_avg + ymin ymax_avg = ymax_avg + ymax if frame_count % frame_average == 0: frame_count = 0 xmin_avg = xmin_avg/frame_average xmax_avg = xmax_avg/frame_average ymin_avg = ymin_avg/frame_average ymax_avg = ymax_avg/frame_average print(xmin_avg, end=",") print(ymin_avg, end=",") print(xmax_avg, end=",") print(ymax_avg, end="\n") ymin_avg = ymin_avg + (ymax_avg - ymin_avg)/2. - H/2. xmin_avg = xmin_avg + (xmax_avg - xmin_avg)/2. - W/2. print("150: ",xmin_avg, end=",") print("150: ",ymin_avg, end="\n") gt = [xmin_avg, ymin_avg, W, H] xmin_avg = 0 xmax_avg = 0 ymin_avg = 0 ymax_avg = 0 pos_x, pos_y, target_w, target_h = region_to_bbox(gt) bboxes = np.zeros((1, 4)) #bboxes[0,:] = pos_x-target_w/2, pos_y-target_h/2, target_w, target_h bboxes[0,:] = pos_x-W/2, pos_y-H/2, W, H print(len(contours)) testing_counter = testing_counter + 1 print("Testing_counter: ",testing_counter) show_frame(np.asarray(Image.open(frame)), gt, 1) print("Cat is detected") print("Starting the tracker ...") if (bboxes[0,1] + bboxes[0,3]/2) < (Y + H/2 - 40): print("Command: Raise the head") angle = angle + 0.5 if angle > 44.5: angle = 44.5 elif (bboxes[0,1] + bboxes[0,3]/2) > (Y + H/2 + 40): print("Command: Lower the head") angle = angle - 0.5 if angle < 0: angle = 0 else: pass set_head_angle_action = robot.set_head_angle(degrees(angle), max_speed=20, in_parallel=True) if straight(bboxes[0,:])[0] != 0 and turn(bboxes[0,:])[0] != 0: robot.drive_wheel_motors(straight(bboxes[0,:])[0] + turn(bboxes[0,:])[0], straight(bboxes[0,:])[1] + turn(bboxes[0,:])[1]) detected_centroid = 0 elif straight(bboxes[0,:])[0] == 0 and turn(bboxes[0,:])[0] == 0: robot.stop_all_motors() detected_centroid = detected_centroid + 1 elif straight(bboxes[0,:])[0] == 0: robot.drive_wheel_motors(turn(bboxes[0,:])[0], turn(bboxes[0,:])[1]) detected_centroid = 0 elif turn(bboxes[0,:])[0] == 0: robot.drive_wheel_motors(straight(bboxes[0,:])[0], straight(bboxes[0,:])[1]) detected_centroid = 0 else: robot.stop_all_motors() detected_centroid = detected_centroid + 1 if detected_centroid > 20//frame_average: detected_centroid = 0 print("Reached a stable state.........\t\t\t\t\t\t\t\t STABLE") # Go near the object set_head_angle_action.wait_for_completed() robot.abort_all_actions(log_abort_messages=True) robot.wait_for_all_actions_completed() robot.set_head_angle(degrees(0.5)).wait_for_completed() print("Robot's head angle: ",robot.head_angle) target_frame_count = 1 while True: latest_img = None while latest_img is None: latest_img = robot.world.latest_image target_frame1 = latest_img.raw_image target_frame1 = target_frame1.resize((640,480), Image.ANTIALIAS) #target_frame1 = target_frame1.convert('L') target_frame1 = np.asarray(target_frame1) #orb1 = cv2.ORB_create(500) #kp1 = orb1.detect(target_frame1,None) #kp1, des1 = orb1.compute(target_frame1, kp1) #features_img1 = cv2.drawKeypoints(target_frame1, kp1, None, color=(255,0,0), flags=0) #plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",features_img1) plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",target_frame1) drive_straight_action = robot.drive_straight(distance=cozmo.util.distance_mm(distance_mm=10),speed=cozmo.util.speed_mmps(10), in_parallel=True) drive_straight_action.wait_for_completed() robot.set_head_angle(degrees(0.5)).wait_for_completed() print("Robot's head angle: ",robot.head_angle) latest_img = None while latest_img is None: latest_img = robot.world.latest_image target_frame2 = latest_img.raw_image target_frame2 = target_frame2.resize((640,480), Image.ANTIALIAS) #target_frame2 = target_frame2.convert('L') target_frame2 = np.asarray(target_frame2) #orb2 = cv2.ORB_create(500) #kp2 = orb2.detect(target_frame2,None) #kp2, des2 = orb2.compute(target_frame2, kp2) #features_img2 = cv2.drawKeypoints(target_frame2, kp2, None, color=(255,0,0), flags=0) #plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",features_img2) plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",target_frame2) target_frame_count = target_frame_count + 1 ''' matcher = cv2.DescriptorMatcher_create(cv2.DESCRIPTOR_MATCHER_BRUTEFORCE_HAMMING) matches = matcher.match(des1, des2, None) matches.sort(key=lambda x: x.distance, reverse=False) matches = matches[:10] imMatches = cv2.drawMatches(target_frame1, kp1, target_frame2, kp2, matches, None) cv2.imwrite("matches_tf1_tf2.jpg", imMatches) points1 = np.zeros((len(matches), 2), dtype=np.float32) points2 = np.zeros((len(matches), 2), dtype=np.float32) for i, match in enumerate(matches): points1[i, :] = kp1[match.queryIdx].pt points2[i, :] = kp2[match.trainIdx].pt print("Points1 [{}]: {}".format(i,points1[i][0]), points1[i][1],"\tPoints2: ",points2[i][0], points2[i][1]) index = None dist1_x = [] dist2_x = [] for index in range(len(points1)): dist1_x.append((W/2.)-points1[index][0]) # Extract only the x-coordinate dist2_x.append((W/2.)-points2[index][0]) # Extract only the x-coordinate fw_x = 1./((1./np.array(dist2_x)) - (1./np.array(dist1_x))) # Calculate the image plane to obj plane mapping in x direction pt1_x = [] pt2_x = [] for index in range(len(points1)): pt1_x.append(fw_x[index]/(W/2. - points1[index][0])) pt2_x.append(fw_x[index]/(W/2. - points2[index][0])) print("Approx. distance[{}]: {}".format(index, pt1_x[index])) if len(pt2_x) < 10: break ''' sys.exit(0) else: # Detected Dog dog_counter += 1 if dog_counter >= 6: # Transition to Dog only if Dog appeared for more than 6 times cat_counter = 0 current_state = 2 # Current State is Dog elif current_state == 2: print('\t\t\t\t\t\t\t\t\t\t\t\tDog') if next_state == 0: # Detected Background background_counter += 1 if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times background_counter = 0 current_state = 0 dog_counter = 0 elif next_state == 2: # Detected Dog itself dog_counter +=1 if dog_counter >= 30: print('Cozmo sees a Dog') robot.drive_wheels(-50, -50) time.sleep(3) robot.drive_wheels(70, -70) time.sleep(2.8) robot.drive_wheels(0, 0) break else: # Detected Cat cat_counter += 1 if cat_counter >= 6: # Transition to Cat only if Cat appeared for more than 6 times dog_counter = 0 current_state = 1
def light_when_face(robot: cozmo.robot.Robot): '''The core of the light_when_face program''' # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed() face = None print("Press CTRL-C to quit") while True: if face and face.is_visible: robot.set_all_backpack_lights(cozmo.lights.blue_light) else: robot.set_backpack_lights_off() # Wait until we we can see another face try: face = robot.world.wait_for_observed_face(timeout=30) if face.name.strip() != "": robot.say_text("I see " + face.name).wait_for_completed() else: robot.say_text( "I see a face. Who is this?").wait_for_completed() except asyncio.TimeoutError: print("Didn't find a face.") return time.sleep(.1)
def cozmoBehavior(robot: cozmo.robot.Robot): """Cozmo search behavior. See assignment description for details Has global access to grid, a CozGrid instance created by the main thread, and stopevent, a threading.Event instance used to signal when the main thread has stopped. You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the main thread to finish. Arguments: robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program """ global grid, stopevent, scale robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() while not stopevent.is_set(): grid.clearObstacles() grid.setStart(convert_pose_to_coords(robot.pose)) # Find up to three light cubes using built-in functions cubes = [ robot.world.get_light_cube(cozmo.objects.LightCube1Id), robot.world.get_light_cube(cozmo.objects.LightCube2Id), robot.world.get_light_cube(cozmo.objects.LightCube3Id) ] desired_angle = cubes[0].pose.rotation.angle_z.degrees # Set the location of the goal in the grid, or set it to the middle if isFound(cubes[0]): grid.clearGoals() target_coords = convert_pose_to_coords(cubes[0].pose) approach_x = approach_y = 0 grid.addObstacles(expand_obstacle(target_coords)) approach_angle = cubes[0].pose.rotation.angle_z approach_x = 3 * round(math.cos(approach_angle.radians), 0) approach_y = 3 * round(math.sin(approach_angle.radians), 0) grid.addGoal( (target_coords[0] - approach_x, target_coords[1] - approach_y)) else: while not isFound(cubes[0]): robot.turn_in_place( cozmo.util.degrees(10)).wait_for_completed() cubes[0] = robot.world.get_light_cube( cozmo.objects.LightCube1Id) if isFound(cubes[1]): grid.addObstacles( expand_obstacle(convert_pose_to_coords(cubes[1].pose))) if isFound(cubes[2]): grid.addObstacles( expand_obstacle(convert_pose_to_coords(cubes[2].pose))) # Run Astar astar(grid, heuristic) # Move to next dx = (grid.getPath()[1][0] - grid.getPath()[0][0]) * scale dy = (grid.getPath()[1][1] - grid.getPath()[0][1]) * scale dh = cozmo.util.radians(math.atan2(dy, dx)) - robot.pose.rotation.angle_z robot.turn_in_place(dh).wait_for_completed() robot.drive_straight( cozmo.util.distance_mm(math.sqrt(dx * dx + dy * dy)), cozmo.util.speed_mmps(25)).wait_for_completed() if math.sqrt( (robot.pose.position.x - cubes[0].pose.position.x)**2 + (robot.pose.position.y - cubes[0].pose.position.y)**2) < 100: robot.turn_in_place( cozmo.util.degrees( desired_angle - robot.pose.rotation.angle_z.degrees)).wait_for_completed() stopevent.set()
def move_to_next_side(robot: cozmo.robot.Robot): robot.turn_in_place(degrees(-45)).wait_for_completed() robot.drive_straight(distance_mm(400), speed_mmps(200), False, False, 0).wait_for_completed() robot.turn_in_place(degrees(130)).wait_for_completed() take_photos(robot)
def cozmo_program(robot: cozmo.robot.Robot): robot.say_text("variation 4").wait_for_completed()
def move_straight(robot: cozmo.robot.Robot, unit): robot.drive_straight(cozmo.util.distance_mm(unit), cozmo.util.speed_mmps(150)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): print("Cozmo SDK has behavior control...") robot.world.image_annotator.add_annotator('robotState', RobotStateDisplay) robot.enable_device_imu(True, True, True) robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() robot.move_lift(-1) robot.say_text("Hello World!").wait_for_completed() time.sleep(1) robot.say_text("Let's take pictures!").wait_for_completed() time.sleep(1) # take pictures # myargs = ["1", "order", "drone", "inspection"] myargs = sys.argv[1:] if len(myargs) <= 1: sys.exit("Incorrect arguments") num_images_per_type = int( myargs[0]) # number of images to take of each type of object print("Taking ", num_images_per_type, "images each of ", myargs[1:]) for type in myargs[1:]: for i in range(num_images_per_type): time.sleep(.5) robot.say_text(type).wait_for_completed() time.sleep(3) latest_image = robot.world.latest_image new_image = latest_image.raw_image timestamp = datetime.datetime.now().strftime("%dT%H%M%S%f") new_image.save("./outputs/" + str(type) + "_" + timestamp + ".bmp") time.sleep(3) time.sleep(0.5) robot.say_text("I will drive now!").wait_for_completed() time.sleep(0.5) robot.drive_straight(distance_mm(1000), speed_mmps(1000)).wait_for_completed() robot.say_text("Check out my arms!").wait_for_completed() robot.move_lift(1) time.sleep(1) robot.move_lift(-1) time.sleep(1) robot.say_text("Pick me up and shake me around!").wait_for_completed() time.sleep(2) robot.say_text("You can put me down now!").wait_for_completed() f = open("./outputs/test_output.txt", 'r+') f.seek(0) f.write("Min Gyro: " + str(min(gyro)) + "\n") f.write("Max Gyro: " + str(max(gyro)) + "\n") f.write("Min Acc: " + str(min(acc)) + "\n") f.write("Max Acc: " + str(max(acc))) f.truncate() f.close() if (all(i > -10 for i in min(gyro)) and all(i < 10 for i in max(gyro))) and (all( i > -25000 for i in min(acc)) and all(i < 25000 for i in max(acc))): robot.say_text("Sensors look good!").wait_for_completed() else: robot.say_text("Sensors look bad!").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
def cozmo_program(robot: cozmo.robot.Robot): time.sleep(2) #go left and right robot.drive_wheels(100, -100, l_wheel_acc=999, r_wheel_acc=999, duration=0.3) robot.drive_wheels(-100, 100, l_wheel_acc=999, r_wheel_acc=999, duration=0.6) robot.drive_wheels(100, -100, l_wheel_acc=999, r_wheel_acc=999, duration=0.3) robot.move_head(1) time.sleep(1) robot.drive_wheels(100, -100, l_wheel_acc=999, r_wheel_acc=999, duration=0.3) robot.drive_wheels(-100, 100, l_wheel_acc=999, r_wheel_acc=999, duration=0.6) robot.drive_wheels(100, -100, l_wheel_acc=999, r_wheel_acc=999, duration=0.4) time.sleep(1) robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabChatty).wait_for_completed() #chat robot.play_anim_trigger( cozmo.anim.Triggers.VC_Alrighty).wait_for_completed() #happy time.sleep(2)
def cozmoBehavior(robot: cozmo.robot.Robot): """Cozmo search behavior. See assignment document for details Has global access to grid, a CozGrid instance created by the main thread, and stopevent, a threading.Event instance used to signal when the main thread has stopped. You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the main thread to finish. Arguments: robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program """ global grid, stopevent foundGoal = False foudnCenter = False setPath = False while not stopevent.is_set(): if foundGoal == False: # find goal look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube = None try: cube = robot.world.wait_for_observed_light_cube(timeout=5) print("Found cube: %s" % cube) foundGoal = True pos = cube.pose.position x, y, z = pos.x, pos.y, pos.z cube_angle = cube.pose.rotation.angle_z.degrees print("cube angle is: ", cube_angle) a, b = 0, 0 if cube_angle > -22.5 and cube_angle < 22.5: a, b = -1, 0 if cube_angle >= 22.5 and cube_angle < 67.5: a, b = -1, -1 if cube_angle >= 67.5 and cube_angle < 112.5: a, b = 0, -1 if cube_angle >= 112.5 and cube_angle < 157.5: a, b = 1, -1 if cube_angle >= 157.5 or cube_angle <= -157.5: a, b = 1, 0 if cube_angle > -67.5 and cube_angle <= -22.5: a, b = -1, 1 if cube_angle > -112.5 and cube_angle <= -67.5: a, b = 0, 1 if cube_angle > -157.5 and cube_angle <= -112.5: a, b = 1, 1 obs1 = int(x / grid.scale + grid.getStart()[0] + 0.5), int(y / grid.scale + grid.getStart()[1] + 0.5) add_obs = [] for i in range(-1, 2, 1): for j in range(-1, 2, 1): ob = obs1[0] + i, obs1[1] + j add_obs.append(ob) grid.addObstacles(add_obs) goal = obs1[0] + a * 2, obs1[1] + b * 2 print(goal) grid.clearGoals() grid.addGoal(goal) except asyncio.TimeoutError: print("Didn't find a cube") goal = int(grid.width / 2), int(grid.height / 2) grid.addGoal(goal) foundGoal = True foundCenter = True finally: # whether we find it or not, we want to stop the behavior look_around.stop() else: # astar # astar(grid) # get path # path = grid.getPath() #a list of cells robot_pose = robot.pose print(robot_pose) rx, ry = robot_pose.position.x, robot_pose.position.y cx = int(rx / grid.scale + grid.getStart()[0] + 0.5) cy = int(ry / grid.scale + grid.getStart()[1] + 0.5) # update start grid.clearStart() grid.setStart((cx, cy)) astar(grid, heuristic) path = grid.getPath() print(path) prev = path[0] for p in path[1:]: diff = p[0] - prev[0], p[1] - prev[1] # Needs to verify curr_angle = robot.pose.rotation.angle_z angle = getAngle(diff) angle_to_turn = angle - curr_angle.degrees robot.turn_in_place( degrees(angle_to_turn)).wait_for_completed() d = math.sqrt(math.pow(diff[0], 2) + math.pow(diff[1], 2)) robot.drive_straight(distance_mm(d * 50), speed_mmps(40)).wait_for_completed() posX, posY = grid.scale * (p[0] - 1), grid.scale * (p[1] - 1) # posX, posY = 25 * p[0] - rx, 25 * p[1] - ry # pose = cozmo.util.Pose(posX, posY, 0.0, q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0) # Everytime we move, check to see if there's an obstacle in our vision # pose = cozmo.util.Pose(posX, posY, 0.0, angle_z = cozmo.util.degrees(angle)) # robot.go_to_pose(pose, in_parallel = True).wait_for_completed() # robot.go_to_pose(pose) # time.sleep(0.1) print(p) print(robot.pose.position.x, robot.pose.position.y) print(posX, posY) prev = p # ca = -1.0*cube_angle # pose = cozmo.util.Pose(robot.pose.position.x, robot.pose.position.y, robot.pose.position.z, angle_z = degrees(ca)) # robot.go_to_pose(pose).wait_for_completed() if foundCenter == True: #not in the goal yet foundGoal = False foundCenter = False else: curr_angle = robot.pose.rotation.angle_z angle = cube_angle angle_to_turn = angle - curr_angle.degrees robot.turn_in_place( degrees(angle_to_turn)).wait_for_completed() break # Your code here
def cozmo_program(robot: cozmo.robot.Robot): # Make sure Cozmo's head and arm are at reasonable levels robot.set_head_angle(degrees(10.0)).wait_for_completed() robot.set_lift_height(0.0).wait_for_completed() robot.say_text( f"I'm going to take photos of {sys.argv[1]}").wait_for_completed() # Set directory to the Category that Cozmo is going to photograph global directory directory = sys.argv[1] if not os.path.exists('data'): os.makedirs('data') if not os.path.exists(f'data/{directory}'): os.makedirs(f'data/{directory}') # Anytime Cozmo sees a "new" image, take a photo robot.add_event_handler(cozmo.world.EvtNewCameraImage, on_new_camera_image) # Initial photo sesh robot.drive_straight(distance_mm(-200), speed_mmps(100), False, False, 0).wait_for_completed() take_photos(robot) # Get all the angles #for i in range(3): # move_to_next_side(robot) # And we're done here robot.say_text("All done!").wait_for_completed() robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin).wait_for_completed()
def cozmoBehavior(robot: cozmo.robot.Robot): """Cozmo search behavior. See assignment description for details Has global access to grid, a CozGrid instance created by the main thread, and stopevent, a threading.Event instance used to signal when the main thread has stopped. You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the main thread to finish. Arguments: robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program """ global grid, stopevent global found_cubes found_cubes = [] goal_degrees = [] grid.addGoal((grid.width / 2, grid.height / 2)) found_goal = False astar(grid, heuristic) path = grid.getPath() path_index = 0 grid_init_start_pose = grid.getStart() (robot_grid_x, robot_grid_y) = grid.getStart() robot.set_head_angle(degrees(0)).wait_for_completed() robot.set_lift_height(1).wait_for_completed() robot.say_text('Game is on').wait_for_completed() while not stopevent.is_set(): new_cube = search_cube(robot, grid) if not new_cube == None: grid.clearStart() grid.clearVisited() grid.clearPath() grid.setStart((robot_grid_x, robot_grid_y)) add_obstacle( grid, new_cube, grid_init_start_pose ) # Add the obstacle for all cubes that had been found if new_cube.cube_id == LightCube1Id: new_cube.set_lights(cozmo.lights.blue_light) goal_degrees = set_goal( grid, new_cube, grid_init_start_pose ) # Update the goal coordinate while found cube 1 robot.say_text("It's the Goal").wait_for_completed() found_goal = True else: new_cube.set_lights(cozmo.lights.red_light) robot.say_text("It's an Obstacle").wait_for_completed() # Replanning the path for robot.say_text('Replanning').wait_for_completed() try: astar(grid, heuristic) except: robot.say_text("Cannot go to that place").wait_for_completed() return path_index = 0 path = grid.getPath() path_index += 1 if path_index == len(path): # At the goal position if not found_goal: # At the center of grid path_index -= 1 robot.turn_in_place(Angle(degrees=-30)).wait_for_completed() continue else: # Arrived the final place goal_degree = goal_degrees[f"{(robot_grid_x, robot_grid_y)}"] robot.turn_in_place( Angle(degrees=normalize_angle( goal_degree - robot.pose.rotation.angle_z.degrees)) ).wait_for_completed() robot.say_text('Arrived').wait_for_completed() break current_pose = path[path_index - 1] next_pose = path[path_index] robot_grid_x += int(next_pose[0] - current_pose[0]) robot_grid_y += int(next_pose[1] - current_pose[1]) x = (next_pose[0] - current_pose[0]) * grid.scale y = (next_pose[1] - current_pose[1]) * grid.scale degree = ((90 * y / abs(y)) if x == 0 else math.degrees( math.atan2(y, x))) - robot.pose.rotation.angle_z.degrees # robot.turn_in_place(Angle(degrees=normalize_angle(degree))).wait_for_completed() # robot.drive_straight(distance_mm(math.sqrt(x**2 + y**2)), speed_mmps(50), should_play_anim=False).wait_for_completed() (x, y) = world_to_related(x, y, robot.pose.rotation.angle_z.degrees) robot.go_to_pose(Pose(x, y, 0, angle_z=Angle(degrees=degree)), relative_to_robot=True).wait_for_completed() stopevent.wait()
def cozmo_say(robot: cozmo.robot.Robot): if (msg is not None) and (len(msg) > 0): robot.say_text(msg).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): time.sleep(2) robot.move_lift(50) robot.drive_straight(distance_mm(-50), speed_mmps(150)).wait_for_completed() #drive forward robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabVampire).wait_for_completed() #grossed out robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabSquint1).wait_for_completed() #squint robot.move_lift(-50) #put arms down robot.drive_wheels(-1000, 1000, l_wheel_acc=-250, r_wheel_acc=250, duration=1) #drive away time.sleep(.5) robot.drive_straight(distance_mm(200), speed_mmps(150)).wait_for_completed()
def do_turn(robot: cozmo.robot.Robot): drive_angle = extract_float(turn) if drive_angle is not None: robot.turn_in_place(degrees(drive_angle)).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()
async def run(robot: cozmo.robot.Robot): #disable all annotations from the interface robot.world.image_annotator.annotation_enabled = True #add your own annotation called 'box' robot.world.image_annotator.add_annotator('box', BoxAnnotator) #make sure the camera receives image data (enable the camera) robot.camera.image_stream_enabled = True #make sure you receive color images from the robot robot.camera.color_image_enabled = True #enabling auto_exposure to constantly update the exposure time and gain values on recent images robot.camera.enable_auto_exposure = True #current camera gain setting, current camera exposure in ms, and...? gain, exposure, mode = 390, 3, 1 try: while True: #current event is the new raw image from the camera that lasts 30ms event = await robot.world.wait_for( cozmo.camera.EvtNewRawCameraImage, timeout=30) #get camera image #if we got an image (of course we will) if event.image is not None: #convert the image into into an array and change the color scheme image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_BGR2RGB) #make sure we refresh the image if mode == 1: robot.camera.enable_auto_exposure = True else: #force the specified exposure time and gain values to the camera robot.camera.set_manual_exposure(exposure, fixed_gain) #find the cube (using the current image, and the lower and upper bounds of 'yellow' cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER) #print the coords in the terminal or None if no cube is detected print(cube) #set the returned cube value to BoxAnnotator's cube BoxAnnotator.cube = cube ################################################################ # Todo: Add Motion Here ################################################################ #Want to center the cube in cozmo's vision, move so it is close to the center of the image #Once cozmo's camera is centered on the cube, move forward. #if cozmo doesn't see the cube, rotate 45 degrees if BoxAnnotator.cube is None: await robot.turn_in_place(degrees(45)).wait_for_completed() if BoxAnnotator.cube is not None: #check to see if we are close enough to the cube if (BoxAnnotator.cube[2] > 90): #was 90 for quarter of the screen print("I am close to the object") robot.stop_all_motors() #cozmo detects cube on the left of the screen, reorient itself elif (BoxAnnotator.cube[0] < 103): print( "Cube on left half of screen, reorienting myself") #orient yourself so cube is near the center of the screen await robot.turn_in_place(degrees(15) ).wait_for_completed() #cozmo detects cube on right half of screen, reorient itself elif (BoxAnnotator.cube[0] > 206): print( "Cube on right half of screen, reorienting myself") #orient yourself so cube is in center of the screen await robot.turn_in_place(degrees(-15) ).wait_for_completed() #cube is centered, move towards it elif (BoxAnnotator.cube[0] > 103 and BoxAnnotator.cube[0] < 206): #cube is in center of screen, move forward print( "Cube is near the center of the screen, moving forward" ) await robot.drive_straight( distance_mm(25.4), speed_mmps(25.4), in_parallel=True).wait_for_completed() #We can also use cozmo.go_to_object()? #no? CUstomObject instances are not supported #any keyboard interrupt will exit the program except KeyboardInterrupt: print("") print("Exit requested by user") #if cozmo is doing something, let him finish before doing the next task except cozmo.RobotBusy as e: print(e)
async def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose global grid, gui # start streaming robot.camera.image_stream_enabled = True #start particle filter pf = ParticleFilter(grid) ################### reqConfidentFrames = 10 reqUnconfidentFrames = 10 ############YOUR CODE HERE################# condition = True estimated = [0,0,0,False] ballObj = Robot(0, 0, 0) STATE = RobotStates.LOCALIZING trueVal = 0 falseVal = 0 dist_to_ball = None robotLen = 1 while (condition): curr_pose = robot.pose img = image_processing(robot) (markers, ball) = cvt_2Dmarker_measurements(img) dist_to_ball = calcDistance(ball) if(dist_to_ball is not None and estimated[3]): ballx = estimated[0] + math.cos(estimated[2]) * dist_to_ball bally = estimated[1] + math.sin(estimated[2]) * dist_to_ball ballObj.set_pos(ballx, bally) #thinkg about false positives #go to pose calculation #How to make a pose?? #rotation plus position #once localized we can tell what directino we are looking in and what position we are in #add to cozmo's pose robot.Pose.pose.position in the direction of robot.Pose.pose.rotation odom = compute_odometry(curr_pose) estimated = pf.update(odom, markers) gui.show_particles(pf.particles) gui.show_robot(ballObj) gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3]) gui.updated.set() if STATE == RobotStates.LOCALIZING: robot.drive_wheels(-5, -5) if estimated[3]: trueVal += 1 else: trueVal = 0 if trueVal > reqConfidentFrames && dist_to_ball is not None: robot.stop_all_motors() STATE = RobotStates.TRAVELING if STATE == RobotStates.TRAVELING: h = math.degrees(math.atan2(goal[1] - estimated[1], goal[0] - estimated[0])) dx = ballObj.x - estimated[0] - (math.cos(h) * robotLen) dy = ballObj.y - estimated[1] - (math.sin(h) * robotLen) dh = h - estimated[2] our_go_to_pose(robot, curr_pose, dx, dy, dh) if not estimated[3]: falseVal += 1 else: falseVal = 0 if trueVal > reqUnconfidentFrames: robot.stop_all_motors() STATE = RobotStates.LOCALIZING STATE = RobotStates.SHOOT if STATE == RobotStates.SHOOT: pass STATE == RobotStates.RESET if STATE == RobotStates.RESET: pass STATE = RobotStates.TRAVELING last_pose = curr_pose
def excited(robot: cozmo.robot.Robot): trigger = cozmo.anim.Triggers.CodeLabExcited play_animation(robot,trigger) robot.turn_in_place(degrees(-130)).wait_for_completed()
def cozmo_rigth_turn(robot: cozmo.robot.Robot): global DONKEY_COZMO_TURN_ANGLE robot.turn_in_place(degrees(-1.0 * DONKEY_COZMO_TURN_ANGLE)).wait_for_completed()
def move_backward(robot: cozmo.robot.Robot): # Drive forwards for 150 millimeters at 50 millimeters-per-second. robot.drive_straight(distance_mm(-100), speed_mmps(50)).wait_for_completed() #cozmo.run_program(move_backward)
async def robot_main(robot: cozmo.robot.Robot): 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.move_lift(-3) await robot.set_head_angle(cozmo.util.degrees(10)).wait_for_completed() robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = True images = [] while True: event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) #convert camera image to opencv format image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_RGB2GRAY) data = img_clf.extract_image_features([image]) labels = img_clf.predict_labels(data) images.append(labels[0]) print(labels[0]) if len(images ) < 3 or images[-1] != images[-2] or images[-2] != images[-3]: continue if labels[0] == 'none': continue await robot.say_text(labels[0], use_cozmo_voice=True, in_parallel=True, num_retries=5).wait_for_completed() if labels[0] == 'order': pass elif labels[0] == 'drone': pass elif labels[0] == 'inspection': await robot.drive_wheels(25, -25) time.sleep(1) await robot.drive_wheels(-25, 25) time.sleep(2) await robot.drive_wheels(25, -25) time.sleep(1) await robot.drive_wheels(0, 0) elif labels[0] == 'truck': await robot.drive_wheels(25, 25) time.sleep(2) await robot.drive_wheels(-25, -25) time.sleep(2) await robot.drive_wheels(0, 0) elif labels[0] == 'hands': pass elif labels[0] == 'place': robot.move_lift(3) time.sleep(.25) robot.move_lift(-3) time.sleep(.25) elif labels[0] == 'plane': pass time.sleep(.25)
async def main(robot: cozmo.robot.Robot): #initialize the machine state state_machine = CozmoStates() state_machine.set_cozmo_coords(robot.pose.position) #event handlers robot.add_event_handler(cozmo.objects.EvtObjectAppeared, state_machine.handle_object_appeared) robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, state_machine.handle_object_disappeared) #are we looking for cube 2? look_for_cube_2 = False #cube1 definition await robot.world.define_custom_cube( CustomObjectTypes.CustomType00, state_machine.cube_arr[state_machine.current_cube], 44, 30, 30, False) while True: print('######looping#####') #get camera image event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) #refresh the coords state_machine.set_cozmo_coords(robot.pose) #triggers when we want the first cube if state_machine.current_cube == 1: print('cube = 1') if state_machine.cube_coords == False: state_machine.cube_not_found() else: state_machine.calc_goto_coords() if (state_machine.calc_dist_from_cozmo() > state_machine.proximity_distance): if state_machine.goto_y >= 20: state_machine.found_cube_left() elif state_machine.goto_y < -20: state_machine.found_cube_right() else: print('Cube ahead!') state_machine.found_cube_ahead() else: robot.stop_all_motors() state_machine.on_enter_near_cube() #triggers when we want cube 2 if state_machine.current_cube == 2: print('cube = 2') #reset the data if this is the first instance if look_for_cube_2 == False: robot.stop_all_motors() print('\nPART 1 DONE! GO TO CUBE 2\n') await robot.say_text('Look for cube 2', in_parallel=True).wait_for_completed() state_machine.set_cozmo_coords(robot.pose) state_machine.calc_goto_coords() print(state_machine.calc_dist_from_cozmo()) look_for_cube_2 = True state_machine.cube_not_found() state_machine.cube_coords = False #define the second cube await robot.world.define_custom_cube( CustomObjectTypes.CustomType00, state_machine.cube_arr[state_machine.current_cube], 44, 30, 30, False) #where does cozmo need to move? if (state_machine.cube_coords == False) or (state_machine.see_cube == False): if state_machine.get_current_state() == 'drive_straight': if state_machine.goto_y < state_machine.mid_screen: state_machine.found_cube_left() elif state_machine.goto_y < -20: state_machine.found_cube_right() else: print('Cube ahead!') state_machine.found_cube_ahead() else: state_machine.cube_not_found() #cozmo sees cube else: state_machine.calc_goto_coords() if (state_machine.calc_dist_from_cozmo() > state_machine.proximity_distance): if state_machine.goto_y < -20: state_machine.found_cube_right() elif state_machine.goto_y > 20: state_machine.found_cube_left() else: state_machine.found_cube_ahead() else: robot.stop_all_motors() state_machine.on_enter_near_cube() #update states last_state = state_machine.get_last_state() current_state = state_machine.get_current_state() #was there a state transition? WIll only trigger if a cube was detected if last_state is not current_state: #audio cue, takes a long time, need to comment out #await robot.say_text('b', in_parallel = True).wait_for_completed() #print the state transition to terminal & reset the state print('Last state: ' + last_state) print('Current state: ' + current_state) print('Coords to cube: ') #some reason, cant print this with the Coords to cube: print statement print(state_machine.calc_goto_coords()) #update the state state_machine.set_last_state(current_state) # the anki victor text stuff, taken and modified # faceImage = Image.new('RGBA', (128, 64), (0, 0, 0, 255)) dc = ImageDraw.Draw(faceImage) try: font_file = ImageFont.truetype('arial.ttf', 20) except IOError: print('IoError') try: font_file = ImageFont.truetype( '/usr/share/fonts/noto/NotoSans-Medium.ttf', 20) except IOError: print('IoError 2') pass dc.text((0, 0), current_state, fill=(255, 255, 255, 255), font=font_file) screen_data = cozmo.oled_face.convert_image_to_screen_data( faceImage) robot.display_oled_face_image(screen_data, 600000000.0, in_parallel=True) #update how robot should move await robot.drive_wheels(state_machine.get_l_motor_speed(), state_machine.get_r_motor_speed())
def do_lift(robot: cozmo.robot.Robot): lift_height = extract_float(lift) if lift_height is not None: robot.set_lift_height(height=lift_height).wait_for_completed()
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) for i in range(10): robot.set_all_backpack_lights(cozmo.lights.blue_light) cube1.set_light_corners(cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light) cube2.set_light_corners(cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light) cube3.set_light_corners(cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light) time.sleep(.1) robot.set_all_backpack_lights(cozmo.lights.green_light) cube1.set_light_corners(cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light) cube2.set_light_corners(cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light) cube3.set_light_corners(cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.blue_light) time.sleep(.1) robot.set_all_backpack_lights(cozmo.lights.blue_light) cube1.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light) cube2.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light) cube3.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light, cozmo.lights.blue_light) time.sleep(.1) robot.set_all_backpack_lights(cozmo.lights.green_light) cube1.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light) cube2.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light) cube3.set_light_corners(cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.blue_light, cozmo.lights.green_light) time.sleep(.1)
def hesitate_long(robot: cozmo.robot.Robot): trigger = cozmo.anim.Triggers.MeetCozmoFirstEnrollmentSayName #PatternGuessThinking #CodeLabThinking play_animation(robot,trigger) robot.turn_in_place(degrees(10)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): robot.drive_straight(distance_mm(5), speed_mmps(10)).wait_for_completed()
def propose_guess(robot: cozmo.robot.Robot, guess = ''): robot.say_text(guess, voice_pitch = 0.5,play_excited_animation=False).wait_for_completed()
async 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: state = State() robot.display_oled_face_image(image(state.cur), 20000, in_parallel=True) trigger = True isBallFound = False isRobotAtBall = False left = False right = False robot.set_head_angle(radians(-0.23), in_parallel=True) # prevPos = None # direction = None while trigger: #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) h, w = opencv_image.shape # print(w) #find the ball ball = find_ball.find_ball(opencv_image) distance = calcDistance(ball) # if prevPos #set annotator ball BallAnnotator.ball = ball BallAnnotator.distance = distance # BallAnnotator.direction = direction if state.isCurState("START"): robot.set_lift_height(0, in_parallel=True) # robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True) #spin around and search for ball #Make a sound and print something on screen # display for 1 second if ball is None: await robot.drive_wheels(17, -17) else: await robot.drive_wheels(0, 0, 0.5) state.next() robot.display_oled_face_image(image(state.cur), 20000, in_parallel=True) if state.isCurState("TRAVELING"): # robot.display_oled_face_image(image(state.cur), 10.0, in_parallel=True) #Print and sound off # move towards ball if distance is None: if left: lspeed = 5 rspeed = 2 * base if right: lspeed = 2 * base rspeed = 5 await robot.drive_wheels(lspeed, rspeed) if distance is not None: if distance > 85: base = 25 adj = (ball[0] - (w / 2)) / (distance**0.5) # print(distance) # print("adj:", adj) left = adj < -0.75 right = adj > 0.75 if left: lspeed = base rspeed = base - adj # print("LEFT") elif right: lspeed = base + adj rspeed = base # print("RIGHT") else: lspeed = base + 20 rspeed = base + 20 await robot.drive_wheels(lspeed, rspeed) else: state.next() robot.display_oled_face_image(image(state.cur), 20000, in_parallel=True) if state.isCurState("END"): # robot.display_oled_face_image(image(state.cur), 10.0, in_parallel = True) #tap ball #Screen and sound off robot.set_lift_height(1, in_parallel=True).wait_for_completed() robot.set_lift_height(0, in_parallel=True).wait_for_completed() if distance is None: if left or right: state.next() robot.display_oled_face_image(image(state.cur), 20000, in_parallel=True) elif distance > 85: state.next() robot.display_oled_face_image(image(state.cur), 20000, in_parallel=True) if state.isCurState("PAUSE"): #pause for a moment await robot.drive_wheels(0, 0, 0.05) state.next() robot.display_oled_face_image(image(state.cur), 100, in_parallel=True) except KeyboardInterrupt: print("") print("Exit requested by user") except cozmo.RobotBusy as e: print(e)