def run(robot: cozmo.robot.Robot): global flag_odom_init, last_pose, confident global grid, gui # start streaming robot.camera.image_stream_enabled = True #start particle filter pf = ParticleFilter(grid) ################### ############YOUR CODE HERE################# ################### robot.set_head_angle(radians(0)).wait_for_completed() estimated = [0, 0, 0, False] trueVal = 0 while True: # print(robot.is_picked_up) if risingEdge(flag_odom_init, robot.is_picked_up): pf = ParticleFilter(grid) elif robot.is_picked_up: robot.drive_wheels(0, 0) elif not robot.is_picked_up: if trueVal < 35: robot.drive_wheels(5, 20) else: robot.stop_all_motors() gh = math.degrees( math.atan2(goal[1] - estimated[1], goal[0] - estimated[0])) ch = estimated[2] dh = gh - ch print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360) tol = 0.01 if dh > tol or dh < -tol: print("hi") robot.turn_in_place(degrees(dh)).wait_for_completed() robot.drive_straight( distance_inches( grid_distance(goal[0], goal[1], estimated[0], estimated[1]) - 1.75), speed_mmps(25)).wait_for_completed() dh = goal[2] - estimated[2] - dh print("dh2", dh % 360) robot.turn_in_place(degrees(dh)).wait_for_completed() return 0 curr_pose = robot.pose img = image_processing(robot) markers = cvt_2Dmarker_measurements(img) # print(markers) odom = compute_odometry(curr_pose) estimated = pf.update(odom, markers) gui.show_particles(pf.particles) gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3]) gui.updated.set() last_pose = curr_pose if estimated[3]: trueVal += 1
def move_in_circle(robot: cozmo.robot.Robot, speed, seconds): robot.say_text("I will spin in three circles").wait_for_completed() # the first value is the speed for one of the treads, and the second value # is the speed for the other tread (left? right?). They can both be # the same sign, or have opposite signs. the last value is the duration of the # movement (measured in seconds?) robot.drive_wheels(speed, -1 * speed, None, None, seconds)
def cozmo_program(robot: cozmo.robot.Robot): # dire_bonjour+setup robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed() cute = CuteCozmo(robot) cute.face_cube(1) # demo gamme for i in range(8): cute.play_note(i) # Animation content, je t'invite a continuer robot.play_anim('anim_fistbump_success_01').wait_for_completed() cute.face_cube(1) #Partie jouer mélodie simple melodie = [1,4,7] print('playing simple melody') mini_jeu_jouer_melodie(cute, melodie) mini_jeu_enregistrer_melodie(cute) robot.play_anim('anim_hiking_observe_01').wait_for_completed() mini_jeu_jouer_melodie(cute) robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed() robot.turn_in_place(Angle(degrees=180)).wait_for_completed() robot.drive_wheels(500, 500) time.sleep(5)
def run(self, robot: cozmo.robot.Robot): robot.set_head_angle(degrees(-15)).wait_for_completed() robot.move_lift(-5) goLeft = False showImage(robot, "RIGHT.PNG") while True: event = robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) if event.image is not None: image = np.asarray(event.image) cube = None try: cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER) print(cube) # time.sleep(1) except asyncio.TimeoutError: print("Cube not found") if cube: return "colorTrack", robot else: robot.drive_wheels(25, -25) time.sleep(1) robot.stop_all_motors()
def cozmo_program(robot: cozmo.robot.Robot): lw = 0 rw = 0 count = 0 set_init_pose(robot) demo_camera_exposure(robot) while True: start = time.time() key = getKey(robot) cozmo.logger.info("'{}' key pressed".format(key)) if key in moveBindings.keys(): lw = moveBindings[key][0] rw = moveBindings[key][1] count = 0 else: count = count + 1 if count > 4: lw = 0 rw = 0 if key == 'q' or key == '\x03': data = [('img', img_list), ('key', key_list)] df = pd.DataFrame.from_items(data) df.to_csv('driving_log.csv') cozmo.logger.info('Writing to csv file, Exiting') break robot.drive_wheels(lw, rw) ls = robot.left_wheel_speed rs = robot.right_wheel_speed cozmo.logger.info('cmd left, right speed: {}, {}'.format(lw, rw)) cozmo.logger.info('fb left,right speed: {}, {}'.format(ls, rs))
def followComposedTrajectory(robot: cozmo.robot.Robot, pathPositions, pPosition, vl, vr, angle): ''' Path Following Algorithm ''' stateVector = [vr, vl, pPosition[0], pPosition[1], angle] subPath = 0 nbSubPath = len(pathPositions) - 1 i = 0 #While the robot is not close enough of the last position of the trajectory while np.linalg.norm( alg.euclideanVector([robot.pose.position.x, robot.pose.position.y], pathPositions[nbSubPath])) > scale / 10: #The robot drive with the instructions computed with newWheelsSpeed robot.drive_wheels(stateVector[1], stateVector[0]) time.sleep(deltaT) #During a certain time plt.scatter( [robot.pose.position.x], [robot.pose.position.y]) #Plot the real position of the robot stateVector = newWheelsSpeed( stateVector[0], stateVector[1], [robot.pose.position.x, robot.pose.position.y], pathPositions[subPath], pathPositions[subPath + 1], robot.pose.rotation.angle_z.radians, pathPositions) subPath = predic.closestPath( subPath, pathPositions, [robot.pose.position.x, robot.pose.position.y]) i += 1
def drone(robot: cozmo.robot.Robot): # Make robot drive in an S formation. Show animation of choice on robot face robot.drive_wheels(50.0, 120.0, duration=3.5) robot.drive_wheels(120.0, 50.0, duration=4.0) robot.set_head_angle(degrees(30.0)).wait_for_completed() robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabWin).wait_for_completed() idle(robot)
def cozmo_program(robot: cozmo.robot.Robot): time.sleep(2) robot.play_anim_trigger(cozmo.anim.Triggers.FacePlantRollArmUp).wait_for_completed() #tries to get up, fails robot.play_anim_trigger(cozmo.anim.Triggers.FacePlantRollArmUp).wait_for_completed() robot.drive_wheels(-1000, 1000, l_wheel_acc=250, r_wheel_acc=250, duration=2) #spin robot.set_lift_height(1.0, accel=100.0, max_speed=100.0).wait_for_completed() #slam lift robot.set_lift_height(0.0, accel=100.0, max_speed=100.0).wait_for_completed() robot.drive_wheels(-200, -200, l_wheel_acc=9999, r_wheel_acc=9999, duration=0.5) #sucessfully gets up robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabCelebrate).wait_for_completed() #celebrate
def cozmo_program(robot: cozmo.robot.Robot): 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() while True: latest_image = robot.world.latest_image if latest_image is not None: new_image = latest_image.raw_image new_image = np.array(new_image) image = img_clf.extract_image_features([new_image]) s = str(img_clf.predict_labels(image)) if "order" in s: for i in range(1): robot.say_text(s).wait_for_completed() robot.drive_wheels(2,5) time.sleep(18) if "drone" in s: robot.say_text(s).wait_for_completed() cube = robot.world.wait_for_observed_light_cube(timeout=30) robot.pickup_object(cube).wait_for_completed() robot.drive_straight(distance_mm(100), speed_mmps(30)).wait_for_completed() robot.place_object_on_ground_here(cube).wait_for_completed() robot.drive_straight(distance_mm(-100), speed_mmps(30)).wait_for_completed() robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() if "inspection" in s: robot.say_text(s).wait_for_completed() #time.sleep(5) for i in range(4): robot.move_lift(.2) robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed() robot.move_lift(-.4) robot.turn_in_place(degrees(90)).wait_for_completed() robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() if "plane" in s: robot.say_text(s).wait_for_completed() if "truck" in s: robot.say_text(s).wait_for_completed() if "hands" in s: robot.say_text(s).wait_for_completed() if "place" in s: robot.say_text(s).wait_for_completed() if "none" in s: time.sleep(5)
def do_drive(robot: cozmo.robot.Robot): drive_duration = extract_float(duration) if drive_duration is not None: drive_speed = 50 drive_dir = "forwards" if drive_duration < 0: drive_speed = -drive_speed drive_duration = -drive_duration drive_dir = "backwards" robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration)
def cozmo_program(robot: cozmo.robot.Robot): # robot.say_text("I'm a gangstah robot!").wait_for_completed() # robot.move_head(-5) # Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second), # and the right wheel at 50 mmps (so Cozmo will drive Forwards while also # turning to the left robot.drive_wheels(20, 80) # wait for 3 seconds (the head, lift and wheels will move while we wait) time.sleep(10)
def cozmo_program(robot: cozmo.robot.Robot): robot.move_head(-1) robot.move_lift(-1) robot.drive_wheels(50, 25) time.sleep(3) robot.move_head(1) robot.move_lift(1) robot.drive_wheels(50, -50) time.sleep(3)
def run(robot: cozmo.robot.Robot): print("***** Front wheel radius: " + str(get_front_wheel_radius(robot))) print("***** Distance between wheels: " + str(get_distance_between_wheels())) ## Example tests of the functions robot.drive_wheels(50, 25, duration=6.4) """cozmo_drive_straight(robot, 85, 10)
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) custom_box = robot.world.define_custom_box(custom_object_type=cozmo.objects.CustomObjectTypes.CustomType00, \ marker_front=cozmo.objects.CustomObjectMarkers.Circles2, \ marker_back=cozmo.objects.CustomObjectMarkers.Circles3, \ marker_top=cozmo.objects.CustomObjectMarkers.Circles4, \ marker_bottom=cozmo.objects.CustomObjectMarkers.Circles5, \ marker_left=cozmo.objects.CustomObjectMarkers.Diamonds2, \ marker_right=cozmo.objects.CustomObjectMarkers.Diamonds3, \ depth_mm=60, \ width_mm=60, \ height_mm=45, \ marker_width_mm=24.892, \ marker_height_mm=24.892, \ is_unique=True) if (custom_box 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") cubes = lookForCubes(robot, 45, 0) old_pose = robot.pose print("looking for cubes!") if len(cubes) == 1: print("found object") robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed() robot.go_to_pose(cubes[0].pose,relative_to_robot=False).wait_for_completed() pickupObject(robot) robot.drive_wheels(-50,-50) time.sleep(3) robot.drive_wheels(0,0) robot.go_to_pose(old_pose,relative_to_robot=False).wait_for_completed() robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed() else: print("Cannot locate custom box") while True: time.sleep(0.1)
def handle_control_input_for_acceleration(robot: cozmo.robot.Robot, pad_events): for pad_event in pad_events: # Check for direction # Go forward or go backward if pad_event.ev_type == 'Absolute' and pad_event.code == 'ABS_RZ': speed = pad_event.state * acceleration_factor robot.drive_wheels(speed, speed) elif pad_event.ev_type == 'Absolute' and pad_event.code == 'ABS_Z': speed = pad_event.state * acceleration_factor * -1 robot.drive_wheels(speed, speed)
def drone(robot: cozmo.robot.Robot): # Have the robot drive in an “S” formation. Show an animation of your choice on the # robot’s face. Then return to the Idle state. robot.say_text(str('drone')).wait_for_completed() trigger = cozmo.anim.Triggers.CodeLabAmazed robot.play_anim_trigger(trigger, in_parallel=True) robot.drive_wheels(l_wheel_speed=10, r_wheel_speed=30, duration=10) robot.play_anim_trigger(trigger, in_parallel=True) robot.drive_wheels(l_wheel_speed=30, r_wheel_speed=10, duration=10) robot.play_anim_trigger(trigger, in_parallel=True).wait_for_completed()
def order(robot: cozmo.robot.Robot): # code for state order # drive in a circle with radius of approximately 10 cm robot.drive_wheels(80, 40) # drive for about 13 seconds, approximately one cycle time.sleep(13) # stop driving after 13 sec robot.drive_wheels(0, 0) # end all actions robot.abort_all_actions(log_abort_messages=True) idle(robot)
def get_distance_between_wheels_test(robot: cozmo.robot.Robot): """Returns the distance between the wheels of the Cozmo robot in millimeters.""" # #### # TODO: Empirically determine the distance between the wheels of the robot using # robot.drive_wheels() function. Write a comment that explains how you determined # it and any computation you do as part of this function. # #### initialAngleZ = robot.pose.rotation.angle_z robot.drive_wheels(100,-100) time.sleep(3.33623) robot.drive_wheels(0,0)
def cozmo_program(robot: cozmo.robot.Robot): robot.world.request_nav_memory_map(1) N = 4000 w = 1e-2 * np.pi / 4 # Loop following sinusoidal control and printing position and visible cubes. for t in range(N): robot.drive_wheels(20 * np.sin(w * t), 20 * np.cos(w * t)) position = robot.pose.position.x_y_z landmarks = [(i, c.is_visible) for i, c in robot.world.light_cubes.items()] print(position, landmarks) time.sleep(1e-1)
def order(robot: cozmo.robot.Robot ): # Call this function once the robot sees the order object #Drive in a circle. Need to test out to change the configurations to 10cm radius lSpeed = 20.0 rSpeed = 50.0 l_acc = 10.0 r_acc = 10.0 robot.drive_wheels(lSpeed, rSpeed, l_wheel_acc=None, r_wheel_acc=None, duration=19.3).wait_for_completed( ) #take out wait for completed if necessary
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() # H geht hoch robot.move_lift(10) # faehrt einen Kreis rechtsrum robot.drive_wheels(200, 10) time.sleep(0.5) # H geht runter robot.move_lift(-10) # faehrt einen Kreis linkssrum robot.drive_wheels(10, 201) time.sleep(2.3)
def cozmo_program(robot: cozmo.robot.Robot): robot.pop_a_wheelie(robot.world.get_light_cube(3)).wait_for_completed() robot.say_text('Oh oh, nop').wait_for_completed() robot.play_anim_trigger( cozmo.anim.Triggers.CodeLabSurprise).wait_for_completed() 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.play_anim_trigger( cozmo.anim.Triggers.CodeLabWhee1).wait_for_completed() robot.set_lift_height(1.0, accel=100.0, max_speed=100.0).wait_for_completed() robot.set_lift_height(0.0, accel=100.0, max_speed=100.0).wait_for_completed() robot.drive_wheels(-200, -200, l_wheel_acc=9999, r_wheel_acc=9999, duration=0.5) time.sleep(1) robot.say_text('Ooo YES').wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): try: s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) except socket_error as msg: robot.say_text("socket failed" + msg).wait_for_completed() ip = "10.0.1.10" port = 5000 try: s.connect((ip, port)) except socket_error as msg: robot.say_text("socket failed to bind").wait_for_completed() cont = True robot.say_text("ready").wait_for_completed() # keys cozmo responds to movement = {'w', 's'} turning = {'a', 'd'} while cont: bytedata = s.recv(4048) # data = str(bytedata) data = bytedata.decode('utf-8') if not data: cont = False s.close() quit() else: print(data) instructions = data.split(';') print(instructions) # check the name to Stop movement, move forward or back, or turn R/L: if instructions[0] == "stop": robot.stop_all_motors() if instructions[0] in movement: # Movement is based on F forwards or B backwards and the speed at which both wheels move if instructions[1] == 'F': robot.drive_wheels(100, 100, 0, 0) elif instructions[1] == 'B': robot.drive_wheels(-100, -100, 0, 0) # We want to turn left or right if instructions[0] in turning: if instructions[0] == 'a': robot.turn_in_place(degrees(90)).wait_for_completed() elif instructions[0] == 'd': robot.turn_in_place(degrees(-90)).wait_for_completed() print(instructions)
def order(robot: cozmo.robot.Robot ): # Call this function once the robot sees the order object #Drive in a circle. Need to test out to change the configurations to 10cm radius robot.say_text("order", play_excited_animation=False, use_cozmo_voice=True, duration_scalar=1.0, voice_pitch=5.0, in_parallel=False, num_retries=0).wait_for_completed() lSpeed = 20.0 rSpeed = 50.0 l_acc = 10.0 r_acc = 10.0 robot.drive_wheels( lSpeed, rSpeed, l_wheel_acc=None, r_wheel_acc=None, duration=19.18) #take out wait for completed if necessary
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 cozmo_program(robot: cozmo.robot.Robot): robot.set_head_angle(degrees(0)) robot.set_lift_height(0, in_parallel=True).wait_for_completed() #turn in circles until you see the charger robot.drive_wheels(SPIN_SPEED, -SPIN_SPEED) charger = robot.world.wait_for_observed_charger(timeout=30) robot.drive_wheels(0.0, 0.0) if charger: if robot.pose.is_comparable(charger.pose): #they're both on the same origin (cozmo) print('going to charger pose') robot.go_to_pose(charger.pose, in_parallel=True).wait_for_completed() robot.set_lift_height(.2, in_parallel=True).wait_for_completed() robot.turn_in_place(degrees(180)).wait_for_completed() wiggle_for_charger(robot) else: print('charger not found.')
def handle_control_input(robot: cozmo.robot.Robot): ''' Méthode permettant de faire les actions en fonction des inputs de gamepad ''' stop_all_movement = False while True and not stop_all_movement: events = get_gamepad() for event in events: #print(event.ev_type, event.code, event.state) handle_control_input_for_lift(robot, event) handle_control_input_for_head(robot, event) if robot.is_cliff_detected: robot.move_lift(5) robot.drive_wheels(-800, -800, duration=0.5) robot.move_lift(-5) stop_all_movement = True break handle_control_input_for_acceleration(robot, events)
def cozmo_program(robot: cozmo.robot.Robot): robot.world.image_annotator.add_static_text('text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT) robot.world.image_annotator.add_annotator('clock', clock) robot.world.image_annotator.add_annotator('battery', Battery) lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=None) cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart cube3 = robot.world.get_light_cube(LightCube3Id) lookaround.stop() robot.pickup_object(cube3).wait_for_completed() d = cozmo.util.Distance(distance_mm = 40) robot.place_on_object(cube1).wait_for_completed() robot.set_lift_height(height = 0.0, max_speed = 5).wait_for_completed() robot.go_to_object(cube1,d).wait_for_completed() robot.set_lift_height(height = 1.0, max_speed = 5).wait_for_completed() d2 = cozmo.util.Distance(distance_inches=-2.0) d3= cozmo.util.Distance(distance_inches=2.0) d4 = cozmo.util.Distance(distance_inches = 3.0) s = cozmo.util.Speed(speed_mmps = 10.0) robot.drive_straight(d2,s, False).wait_for_completed() robot.drive_wheels(10.0, -10.0, duration = 5.0) robot.drive_straight(d4,s,False).wait_for_completed() robot.drive_wheels(-10.0, 10.0, duration = 5.0) robot.drive_straight(d3,s,False).wait_for_completed() robot.set_lift_height (height = 0.0).wait_for_completed() # Shutdown the program after 200 seconds time.sleep(200)
def cozmo_program(robot: cozmo.robot.Robot): # command cozmo to talk # 'wait_for_completed()' syntax complete the statement before moving to the next line robot.say_text("Hello World").wait_for_completed() # A "for loop" runs for each value i in the given range - in this example # starting from 0, while i is less than 5 (so 0,1,2,3,4). for i in range(5): # Add 1 to the number (so that we count from 1 to 5, not 0 to 4), # then convert the number to a string and make Cozmo say it. robot.say_text(str(i+1)).wait_for_completed() # Drive forwards for X millimeters at X millimeters-per-second. # X = any integer 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() # 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() # Tell the head motor to start lowering the head (at 5 radians per second) robot.move_head(-5) # Tell the lift motor to start lowering the lift (at 5 radians per second) robot.move_lift(-5) # Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second), # and the right wheel at 50 mmps (so Cozmo will drive Forwards while also # turning to the left robot.drive_wheels(25, 50) # wait for 3 seconds (the head, lift and wheels will move while we wait) time.sleep(3)
def cozmo_program(robot: cozmo.robot.Robot): # Tell the head motor to start lowering the head (at 5 radians per second) robot.move_head(-5) # Tell the lift motor to start lowering the lift (at 5 radians per second) robot.move_lift(-5) # Tell Cozmo to drive the left wheel at 25 mmps (millimeters per second), # and the right wheel at 50 mmps (so Cozmo will drive Forwards while also # turning to the left robot.drive_wheels(25, 50) # wait for 3 seconds (the head, lift and wheels will move while we wait) time.sleep(3) # Tell the head motor to start raising the head (at 5 radians per second) robot.move_head(5) # Tell the lift motor to start raising the lift (at 5 radians per second) robot.move_lift(5) # Tell Cozmo to drive the left wheel at 50 mmps (millimeters per second), # and the right wheel at -50 mmps (so Cozmo will turn in-place to the right) robot.drive_wheels(50, -50) # wait for 3 seconds (the head, lift and wheels will move while we wait) time.sleep(3)