def cozmo_program(robot: cozmo.robot.Robot): fixed_object = robot.world.create_custom_fixed_object(Pose(100, 0, 0, angle_z=degrees(0)), 10, 100, 100, relative_to_robot=True) if fixed_object: print("fixed_object created successfully") robot.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): cozmo.logger.setLevel('WARN') hx = [] hu = [] # Start a first action (this one is trivial, just to start). a = robot.go_to_pose(cozmo.util.Pose(x=0, y=0, z=0, angle_z=cozmo.util.radians(0)), relative_to_robot=True) # Let's run the loop for 10 seconds. for i in range(10): x = env.reset(cozmo2net(robot.pose)) # Get current state from sensors xn = preview(env, 10) # Predict next state hx.append(x) hu.append(xn) # Log ... print(x, xn) # ... and print if a.is_running: a.abort() # Abort preview action if not achieved a = robot.go_to_pose( net2cozmo(xn)) # Send the robot to next passing point time.sleep(1) # \endfor main action loop # Just in case, stop the robot motors. if a.is_running: a.abort() robot.drive_wheel_motors(0, 0)
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 roomcreation(robot: cozmo.robot.Robot): cubesize = 50 initialPosition = 0 origin1 = robot.world.create_custom_fixed_object(Pose(-100, initialPosition - cubesize, 0, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) origin2 = robot.world.create_custom_fixed_object(Pose(-100, 0, initialPosition, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) origin3 = robot.world.create_custom_fixed_object(Pose(-100, initialPosition + cubesize, 0, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) ''' target1 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition-100, 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) target2 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition, 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) target3 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition+100 , 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) ''' if origin1 and origin2 and origin3: print("Created origins succesfully") # robot.go_to_pose(Pose(100, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() ''' if target1 and target2 and target3: print("Created targets succesfully") #robot.go_to_pose(Pose(300, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() ''' robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() robot.move_lift(3) time.sleep(5) robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() robot.move_lift(-3) time.sleep(5)
def mov_w2h(robot: cozmo.robot.Robot): print("Robot mov_w2h action execution ...") try: robot.go_to_pose(exit_home, relative_to_robot=False).wait_for_completed(20) robot.go_to_pose(home, relative_to_robot=False).wait_for_completed(20) robot.say_text(text="I visited way point two and now I'm in my home").wait_for_completed(10) robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10) return "s2" except Exception as e: print("ERROR: %sn" + str(e)) return "err"
def mov_w1w2(robot: cozmo.robot.Robot): print("Robot mov_w1w2 action execution ...") try: robot.go_to_pose(w2, relative_to_robot=False).wait_for_completed(20) cube2 = robot.world.get_light_cube(LightCube2Id) cube2.set_lights(cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 102), name="yellow"))) robot.say_text(text="I visited way point one and now I'm in way point two").wait_for_completed(10) robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10) return "s3" except Exception as e: print("ERROR: %sn" + str(e)) return "err"
def mov_hw1(robot: cozmo.robot.Robot): print("Robot mov_hw1 action execution ...") try: robot.go_to_pose(exit_home, relative_to_robot=False).wait_for_completed(20) robot.go_to_pose(w1, relative_to_robot=False).wait_for_completed(20) cube1 = robot.world.get_light_cube(LightCube1Id) cube1.set_lights(cozmo.lights.Light(cozmo.lights.Color(rgb=(102, 255, 102), name="green"))) robot.say_text(text="I'm at way point one").wait_for_completed(10) robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10) return "s5" except Exception as e: print("ERROR: %sn" + str(e)) return "err"
def initialState(p, robot: cozmo.robot.Robot): ''' The robot drive to the first position with the right orientation ''' [vr, vl] = [25, 24] angle = robot.pose.rotation.angle_z.radians orientation = predic.orientationTarget(p[0], p[1]) firstAngle = alg.angleBetweenVectors( [10, 0], orientation) #Robot heading towoard the next posiition #The robot drive to the first position robot.go_to_pose(Pose(p[0][0], p[0][1], 0, angle_z=radians(firstAngle))).wait_for_completed() return [vr, vl, firstAngle]
def cozmo_program(robot: cozmo.robot.Robot): # cube = None fixed_object = custom_objects(robot) time.sleep(60) look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) # try to find a block xpose = 500 cube = None while not cube: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) print("Found it", cube) except asyncio.TimeoutError: print("Didn't find it") look_around.stop() robot.go_to_pose(Pose(xpose, 0, 0, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed() look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) xpose = 1000 print("Yay") look_around.stop() cube.set_lights(cozmo.lights.green_light.flash()) anim = robot.play_anim_trigger(cozmo.anim.Triggers.BlockReact) anim.wait_for_completed() action = robot.pickup_object(cube) print("got action", action) result = action.wait_for_completed(timeout=30) print("got action result", result) robot.turn_in_place(degrees(90)).wait_for_completed() action = robot.place_object_on_ground_here(cube) print("got action", action) result = action.wait_for_completed(timeout=30) print("got action result", result) anim = robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin) cube.set_light_corners(None, None, None, None) anim.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 cozmo_program(robot: cozmo.robot.Robot): """ global coordinates from Cozmo SDK """ # trajectory = [Pose(400, 0, 0, angle_z=degrees(90)), Pose(400, 250, 0, angle_z=degrees(180)), # Pose(0, 250, 0, angle_z=degrees(270)), Pose(0, 0, 0, angle_z=degrees(0))] # # goes around a block four times # for i in range(4): # for coord in trajectory: # robot.go_to_pose(coord).wait_for_completed() ''' relative motion ''' trajectory = [ Pose(100, 0, 0, angle_z=degrees(0)), Pose(0, 100, 0, angle_z=degrees(90)), Pose(0, 100, 0, angle_z=degrees(180)), Pose(0, 100, 0, angle_z=degrees(270)) ] # goes around a block four times for i in range(4): for coord in trajectory: robot.go_to_pose(coord, relative_to_robot=True).wait_for_completed()
def find_relative_cube_pose(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, prints the pose of the detected cube in world coordinate frame and relative to the robot coordinate frame.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None while True: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: print("Robot pose: %s" % robot.pose) print("Cube pose: %s" % cube.pose) print("Cube pose: %s" % cube.pose) final = get_relative_pose(cube.pose, robot.pose) print("Cube pose in the robot coordinate frame: %s" % final) robot.go_to_pose((final), relative_to_robot=True).wait_for_completed() except asyncio.TimeoutError: print("Didn't find a cube")
def workloop(robot: cozmo.robot.Robot): robot.set_head_angle(degrees(7)).wait_for_completed() MyObjectArchetype = robot.world.define_custom_cube( CustomObjectTypes.CustomType00, CustomObjectMarkers.Circles2, 50, 50, 50, True) zeroOffset = 80 inputOffset = 100 # wait until we see a custom object while True: offset = zeroOffset + inputOffset my_object_instance = None while my_object_instance is None: evt = robot.wait_for(cozmo.objects.EvtObjectObserved, timeout=None) if isinstance(evt.obj, CustomObject): my_object_instance = evt.obj # find the vector from the object to the robot object_to_robot_vec = robot.pose.position - my_object_instance.pose.position # normalize the vector (so it's length 1.0) object_to_robot_vec_dist = math.sqrt( (object_to_robot_vec.x * object_to_robot_vec.x) + (object_to_robot_vec.y * object_to_robot_vec.y) + (object_to_robot_vec.z * object_to_robot_vec.z)) normalized_object_to_robot_vec = object_to_robot_vec * ( 1.0 / object_to_robot_vec_dist) # we can now add X times this vector to the objects position and it will push us X mm towards the robot # e.g. lets push it 50mm back (about 1 Cozmo length) offset_vec = (normalized_object_to_robot_vec * offset) target_pos = my_object_instance.pose.position + offset_vec target_pose = my_object_instance.pose # change the position (set it to the new target_pos we calculated above) target_pose._position = target_pos robot.go_to_pose(target_pose).wait_for_completed() time.sleep(0.5)
def find_face(robot: cozmo.robot.Robot): global pose print ("cozmo is looking for a human face") face = None # two cubes spotted, the robot will find the human face and approach it. while True: if face and face.is_visible: robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() robot.say_text("Hello Human.How are you today? I am excited to play a game with you. Are u ready? Let's start").wait_for_completed() robot.go_to_pose(Pose(0.0, 0.0, 0.0, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed() break else: robot.turn_in_place(degrees(30)).wait_for_completed() # Wait until we we can see another face try: face = robot.world.wait_for_observed_face(timeout=2) print(face) except asyncio.TimeoutError: print("Didn't find a face.") # return time.sleep(1) return face
def intent_solve_maze(robot: cozmo.robot.Robot): print(str(world)) world.create_objects(robot, (2, 2)) robot.go_to_pose(Pose(2 * 95, 2 * 95, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() pass
def charger(self, robot: cozmo.robot.Robot = None, cmd_args=None): trial = 1 # try to find the charger charger = None # see if Cozmo already knows where the charger is if robot.world.charger: if robot.world.charger.pose.origin_id == robot.pose.origin_id: robot.say_text("I know where the charger is", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0).wait_for_completed() charger = robot.world.charger else: pass if not charger: robot.say_text("I am trying to find the charger", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=None) robot.say_text("Found it!", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() except asyncio.TimeoutError: robot.say_text("I couldn't find the charger", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() finally: # whether we find it or not, we want to stop the behavior look_around.stop() if charger: robot.say_text("I will now lift my arms to maneuver to the base", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() robot.move_lift(10) robot.say_text("I am going to align myself, I got this!", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() action = robot.go_to_pose(charger.pose) action.wait_for_completed() print("Completed action: result = %s" % action) robot.drive_straight(distance_mm(-100), speed_mmps(5000)).wait_for_completed() robot.say_text("I think I am aligned!", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() # Turn 180 (and 5) degrees, then goes backwards at full speed robot.say_text("And now for the finale, I will turn around", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() #this value needs to be tweaked (90 or 95) robot.turn_in_place(degrees(90)).wait_for_completed() robot.turn_in_place(degrees(90)).wait_for_completed() # time.sleep( 1 ) robot.say_text("Almost there!", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() robot.drive_straight(distance_mm(-200), speed_mmps(5000)).wait_for_completed() robot.say_text("Am I on the base? I will check.", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() if robot.is_on_charger: robot.move_lift(-8) robot.say_text("I did it! Yay", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() else: robot.say_text("I did not make it, but I am not discouraged", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() robot.world.charger = None robot.say_text("I will try again", play_excited_animation=True, use_cozmo_voice=True, duration_scalar=0.7, voice_pitch=-1.0, in_parallel=False).wait_for_completed() robot.drive_straight(distance_mm(150), speed_mmps(5000)).wait_for_completed() trial += 1 if trial < 4: self.charger(robot) else: print("I am tired of trying, I will stop trying for now") return
def explore_the_world(robot: cozmo.robot.Robot): global pose robot.say_text( "Hello Welcome to my world. I am going to explore my world please bear with me" ).wait_for_completed() #initialise the variables cube = None cube1 = None look_around_2 = None look_around_3 = None # stores the starting location of the cozmo robot pose = robot.pose print('Pose: Pos = <%.1f, %.1f, %.1f>' % pose.position.x_y_z) print('Pose: Rot quat = <%.1f, %.1f, %.1f, %.1f>' % pose.rotation.q0_q1_q2_q3) print('Pose: angle_z = %.1f' % pose.rotation.angle_z.degrees) print('Pose: origin_id: %s' % pose.origin_id) # look around to find the first cube look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) while True: try: cube = robot.world.wait_for_observed_light_cube(timeout=10) if cube: print(cube) look_around.stop() break else: robot.go_to_pose(Pose(pose.position.x, pose.position.y, pose.position.z, angle_z=degrees( pose.rotation.angle_z.degrees)), relative_to_robot=True).wait_for_completed() robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() except asyncio.TimeoutError: print("Didn't find a cube :-(") #finally: #look_around.stop() print("the type of the cube is ", type(cube.cube_id)) robot.say_text("the first cube is spotted").wait_for_completed() cube.set_lights(cozmo.lights.green_light.flash()) cube1 = cube id = cube.cube_id robot.turn_in_place(degrees(30)).wait_for_completed() look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) print(id) while True: try: cube1 = robot.world.wait_for_observed_light_cube(timeout=10) if cube1.cube_id != id: print(cube1) else: id = cube1.cube_id look_around_2 = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) except asyncio.TimeoutError: print("Didn't find a cube :-(") finally: look_around_3 = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) #robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() if id != cube1.cube_id: break print("the new id is ", id) look_around.stop() if look_around_2: look_around_2.stop() if look_around_3: look_around_3.stop() # set the head position for the cozmo robot print("both cubes have been spotted ") robot.say_text("both cubes have been spotted").wait_for_completed() cube.set_lights(cozmo.lights.green_light.flash()) cube1.set_lights(cozmo.lights.green_light.flash()) robot.say_text("now i am looking for a pretty face").wait_for_completed() cube.set_lights_off() cube1.set_lights_off() return cube, cube1
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, motion robot.set_lift_height(0).wait_for_completed() robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() # Useful vars for the local functions. origin = robot.pose start = grid.getStart() goal_relative_to_cube = cozmo.util.Pose(-100, 0, 0, angle_z=cozmo.util.degrees(0)) center_of_arena = (grid.width / 2, grid.height / 2) motion = None # Current motion of the robot, if the robot is moving. goal_pose = None def pose_to_coords(pose): """Transforms a cozmo pose in world coordinates to the grid coordinates. Assuming that the robot starts at the given start location on the map and facing +x direction.""" pose_relative_to_origin = get_relative_pose(pose, origin) (x0, y0) = start return round( pose_relative_to_origin.position.x / grid.scale) + x0, round( pose_relative_to_origin.position.y / grid.scale) + y0 def coords_to_pose(coords, angle): (x, y) = coords (x0, y0) = start return origin.define_pose_relative_this( cozmo.util.pose_z_angle((x - x0) * grid.scale, (y - y0) * grid.scale, 0, angle)) def stop_motion(): global motion if motion is not None and motion.is_running: motion.abort() motion = None def direction(p1, p2): """Returns the direction vector from p1 to p2.""" return p2[0] - p1[0], p2[1] - p1[1] def build_plan(path): """Given a path (list of coords), build a plan that goes through each key point on the path. Returns a list of poses.""" plan = deque() last_point = None last_direction = None for coords in path: # The first point is the starting location. Don't need to add movement. if last_point is None: last_point = coords continue dir = direction(last_point, coords) if dir != last_direction: # Move to last point, facing current direction plan.append( coords_to_pose( last_point, cozmo.util.radians(math.atan2(dir[1], dir[0])))) last_direction = dir # Otherwise, we're continuing to the same direction last_point = coords if goal_pose is not None: plan.append(goal_pose) else: plan.append(coords_to_pose(last_point, cozmo.util.radians(0))) return plan last_known_coords = [None, None, None] # Last known coordinates for the 3 cubes. plan = None # A list of poses to go through in order to reach the goal. while not stopevent.is_set(): robot_coords = pose_to_coords(robot.pose) cubes = [ robot.world.get_light_cube(id) for id in cozmo.objects.LightCubeIDs ] update_map = False # Only update map in case cubes are moved. for i, cube in enumerate(cubes): if cube.is_visible: coords = pose_to_coords(cube.pose) if coords != last_known_coords[i]: last_known_coords[i] = coords update_map = True if i == 0: goal_pose = cube.pose.define_pose_relative_this( goal_relative_to_cube) if update_map or plan is None: print("Cubes moved, updating map. Cube locations: " + str(last_known_coords)) stop_motion() grid.clearObstacles() grid.clearGoals() grid.setStart(robot_coords) for coords in last_known_coords: if coords: # Mark each cube as a 3x3 square to account for the radius of the robot. grid.addObstacle(coords) for neighbor, _ in grid.getNeighbors(coords): grid.addObstacle(neighbor) if goal_pose: grid.addGoal(pose_to_coords(goal_pose)) elif robot_coords != center_of_arena: # Drive to the center of the arena. grid.addGoal(center_of_arena) else: # Turn in place, 30 degrees at a time, and re-evaluate. robot.turn_in_place( cozmo.util.degrees(30)).wait_for_completed() continue # Now that we have a goal, replan the path. astar(grid, heuristic) plan = build_plan(grid.getPath()) print("Plan: " + str(plan)) # Follow the plan. if len(plan) == 0: if motion is not None: motion.wait_for_completed() stopevent.set() return if motion is None or motion.is_completed: nextpose = plan.popleft() print("Following plan to next pose: " + str(nextpose)) motion = robot.go_to_pose(nextpose) time.sleep(0.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 while not stopevent.is_set(): grid.clearStart() grid.setStart((math.ceil(robot.pose.position.x / grid.scale), math.ceil(robot.pose.position.y / grid.scale))) cube1 = robot.world.get_light_cube(1) cube2 = robot.world.get_light_cube(2) cube3 = robot.world.get_light_cube(3) print("cube1=" + str(cube1.is_visible) + str(cube1)) print("cube2=" + str(cube2.is_visible) + str(cube2)) print("cube3=" + str(cube3.is_visible) + str(cube3)) if cube1.is_visible: cube1x = math.ceil(cube1.pose.position.x / grid.scale) cube1y = math.ceil(cube1.pose.position.y / grid.scale) cube1angle = cube1.pose.rotation.angle_z.degrees for x in range(cube1x - 2, cube1x + 3): for y in range(cube1y - 2, cube1y + 3): grid.addObstacle((x, y)) rx = math.ceil( (cube1.pose.position.x + (100 * abs(math.cos(cube1.pose.rotation.angle_z.radians)))) / grid.scale) ry = math.ceil( (cube1.pose.position.y + (100 * abs(math.sin(cube1.pose.rotation.angle_z.radians)))) / grid.scale) grid.addGoal((rx, ry)) if cube2.is_visible: cube2x = math.ceil(cube2.pose.position.x / grid.scale) cube2y = math.ceil(cube2.pose.position.y / grid.scale) for x in range(cube2x - 2, cube2x + 3): for y in range(cube2y - 2, cube2y + 3): grid.addObstacle((x, y)) if cube3.is_visible: cube3x = math.ceil(cube3.pose.position.x / grid.scale) cube3y = math.ceil(cube3.pose.position.y / grid.scale) for x in range(cube3x - 2, cube3x + 3): for y in range(cube3y - 2, cube3y + 3): grid.addObstacle((x, y)) astar(grid, heuristic) for step in grid.getPath(): print(step[0] * grid.scale, step[1] * grid.scale) robot.go_to_pose( Pose(step[0] * grid.scale, step[1] * grid.scale, 0, angle_z=degrees(0))).wait_for_completed() robot.turn_in_place(degrees(180 + cube1angle % 90)).wait_for_completed() time.sleep(10)
def program_cozmo_action(robot: cozmo.robot.Robot): #paramètres et initialisations #pour l'url du serveur faire attention à la modifification faite dans le fonction "checkpicture", un rework sera peut-être nécessaire pour viser autre chose que l'API de démo robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = True cube=None numero_cube = 0 answer_json = 0 url = "http://localhost:3000/v1/images" nbrVotesMin = 3 # Nombre de votes minimaux pour qu'une réponse soit jugée acceptable # Tant que COZMO n'a pas vu de cube while(cube == None): #permet de voir l'environnement qu'entour cozmo look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) # scanne l’environnement à la recherche d’un cube. cube = robot.world.wait_for_observed_light_cube(timeout=30) look_around.stop() # aller jusqu'a la position du cube identifié robot.go_to_pose(cozmo.util.Pose(cube.pose.position.x, cube.pose.position.y, cube.pose.position.z, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() # prend en photo le cube (si COZMO a bien réussi à se placer, ce qui n'est pas gagné) # c'est une boucle pour être sûr qu'il n'y à pas de problèmes pictureIsTaken = True while pictureIsTaken: time.sleep(0.1) latest_image = robot.world.latest_image if latest_image is not None: image = latest_image.raw_image pictureIsTaken = False # On génère une uuid pour l'image prise, qui servira d'identifiant numero_cube = str(uuid.uuid4()) #save la photo prise pour pouvoir l'exposer au crowsourcing image.save("CubeAction" + str(numero_cube) + ".bmp") image = image.convert("RGB") # On supprime le channel alpha de l'image, qui pourraît poser des problèmes dans certains cas # On utilise un buffer pour capturer les données brutes de l'image buffered = BytesIO() image.save(buffered, format="JPEG") # On convertit l'image img_base64 = "data:image/jpeg;base64," # Cette chaine de caractères doit être rajouté pour que les navigateur reconnaissance que c'est une image encode64 = img_base64 + str(base64.b64encode(buffered.getvalue())).replace("b'","").replace("'","") # Les navigateurs arrivent sans problèmes à afficher des images encodé en base64, mais pas quand Python rajoute ses " b' ... '" autour des valeurs brutes response_send = sendPicture(encode64, url, numero_cube) #On envoie l'image if response_send.status_code is not 201 : # gestion d'erreur error_string = "Error, can't POST the picture to the server API : " + response_send.status_code + "\n" + response_send.text raise IOError(error_string) else : # Si l'image est posté avec succès done = False while not done : # Boucle de check si le nombre de vote dépasse le minimum requis color = cozmo.lights.Light(cozmo.lights.Color(rgb=(178,128,1))) robot.set_all_backpack_lights(color) time.sleep(3) answer_text = checkPicture(url, numero_cube) # On check la réponse sur l'API answer_json = answer_text.json() if answer_json.get("nbrVotes") is not None : # Si une réponse existe if answer_json.get("nbrVotes") > nbrVotesMin: # Vérification du nombre de vote done = True color = cozmo.lights.Light(cozmo.lights.Color(rgb=(1,128,128))) robot.set_all_backpack_lights(color) # Réaliser les actions demandés action = answer_json.get("answer") # Enregistrement de couleurs de base de la librairie COZMO red, green, blue, white=cozmo.lights.red_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.white_light colors=[red, green, blue, white] if(action== "red"): color_light(cube, red) elif(action== "white"): color_light(cube, white) elif(action== "blue"): color_light(cube, blue) elif(action== "green"): color_light(cube, green) elif(action== "crazy_color_1"): crazy_color_1(cube, colors) elif(action== "crazy_color_2"): crazy_color_2(cube) elif(action == "off"): light_off(cube) elif "say" in action : robot.say_text(action.replace("say ", ""),True,use_cozmo_voice=True, in_parallel=True).wait_for_completed() else: error_string = "This action isn't a valide answer : " + action + "\nThe answer can be : red, blue, green, white, off, crazy_color_1, crazy_color_2" raise ValueError(error_string) return 1
def cozmo_program(robot: cozmo.robot.Robot): # Move lift down and tilt the head up robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() # intrinsics from calibration.cfg cameraParam = [288.15418237, 285.12686892, 197.31722863, 120.45748409] # [fx, fy, cx, cy] extrinsic = np.array( [[-9.97469913e-02, -5.61812756e-02, 9.93425489e-01, -2.40474116e-02], [-9.94999768e-01, 5.15631732e-04, -9.98758996e-02, -4.49931548e-03], [5.09891373e-03, -9.98420452e-01, -5.59517889e-02, 3.75532293e-02], [0., 0., 0., 1.]]) detector = Detector("tagStandard41h12", quad_decimate=2.0, quad_sigma=1.0, debug=False) # create main window cv2.namedWindow("AprilTag Detection", 1) ''' relative motion ''' trajectory = [[400, 0, 0], [0, 250, 90], [0, 400, 180], [0, 250, 270]] dx = 0 dy = 0 # goes around a block four times for i in range(4): for coord in trajectory: target_pose = Pose(coord[0] + dx, coord[1] + dy, 0, angle_z=degrees(coord[2])) robot.go_to_pose(target_pose, relative_to_robot=True).wait_for_completed() robot.set_head_angle(degrees(0)).wait_for_completed() # convert Bayer GB to RGB for display image = robot.world.latest_image.raw_image img = np.array(image) gray = cv2.cvtColor( img, cv2.COLOR_BGR2GRAY ) # convert Bayer BG to Grayscale for corner detections tags = detector.detect(gray, estimate_tag_pose=True, camera_params=cameraParam, tag_size=0.0127) while len(tags) == 0: image = robot.world.latest_image.raw_image img = np.array(image) gray = cv2.cvtColor( img, cv2.COLOR_BGR2GRAY ) # convert Bayer BG to Grayscale for corner detections tags = detector.detect(gray, estimate_tag_pose=True, camera_params=cameraParam, tag_size=0.0127) # visualize the detection tag = tags[0] for idx in range(len(tag.corners)): cv2.line(gray, tuple(tag.corners[idx - 1, :].astype(int)), tuple(tag.corners[idx, :].astype(int)), (255, 0, 0)) rot_mat = np.array([[ tag.pose_R[0][0], tag.pose_R[0][1], tag.pose_R[0][2], tag.pose_t[0] ], [ tag.pose_R[1][0], tag.pose_R[1][1], tag.pose_R[1][2], tag.pose_t[1] ], [ tag.pose_R[2][0], tag.pose_R[2][1], tag.pose_R[2][2], tag.pose_t[2] ], [0.0, 0.0, 0.0, 1.0]], dtype='float') tag_pose = np.matmul(rot_mat, np.array([0, 0, 0.0125, 1])) pose_in_Cozmo = np.matmul(extrinsic, tag_pose) # label the id of AprilTag on the image. cv2.putText(gray, str(pose_in_Cozmo), org=(tag.corners[0, 0].astype(int) + 10, tag.corners[0, 1].astype(int) + 10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.3, color=(255, 0, 0)) cv2.imshow('AprilTags', gray) dx = pose_in_Cozmo[0] * 1000 - 100 dy = pose_in_Cozmo[1] * 1000
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 setPath = False while not stopevent.is_set(): if grid.getGoals() == []: # find goal look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) cube = None try: cube = robot.world.wait_for_observed_light_cube(timeout=30) print("Found cube: %s" % cube) foundGoal = True pos = cube.pose.position x, y, z = pos.x, pos.y, pos.z cube_angle = cube.pose.rotation.angle_z.degrees 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.addGoal(goal) except asyncio.TimeoutError: print("Didn't find a cube") finally: # whether we find it or not, we want to stop the behavior look_around.stop() 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] angle = getAngle(diff) 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=cozmo.util.degrees(ca)) robot.go_to_pose(pose).wait_for_completed() break # Your code here
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 width = grid.width height = grid.height scale = grid.scale startX = grid.getStart()[0] startY = grid.getStart()[1] #Find Goal robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() # Boolean that determines if Cozmo is already in the center of the grid inCenter = False cube1 = None #The intial goal is set to the center of the grid, in case the cube is not found initially grid.addGoal((width / 2, height / 2)) while not stopevent.is_set(): #Update start coordinate to be current robot's pose xR = int(robot.pose.position.x / scale) + startX yR = int(robot.pose.position.y / scale) + startY grid.setStart((xR, yR)) cubeObstacle = None try: cubeObstacle = robot.world.wait_for_observed_light_cube(timeout=2) except asyncio.TimeoutError: print("No Cube obstacle was found") if cubeObstacle is not None: if cubeObstacle.object_id == 1 and cube1 is None: cube1 = cubeObstacle # We found cube 1. Clear the old goal of going to center. grid.clearGoals() #Get cube's 1 coordinates and rotation print("Found Cube 1:", cube1) #Assumption: Cube's coordinates is global coordiantes (where Robot originally started) xC = int(cube1.pose.position.x / scale) + startX yC = int(cube1.pose.position.y / scale) + startY angle_zC = cube1.pose.rotation.angle_z # Make cube 1 an obstacle to avoid hitting it addCubeObstacle(grid, (xC, yC)) #Compute the final goal since we found Cube 1 #This is the final distance where we want to finish from the cube distanceToCube = 4 goalX = int( round(xC + distanceToCube * math.cos(angle_zC.radians))) goalY = int( round(yC + distanceToCube * math.sin(angle_zC.radians))) grid.addGoal((goalX, goalY)) print("Goal:", grid.getGoals()[0]) else: #We found cube 2 or 3. Let's make them obstacles xObstacle = int(cubeObstacle.pose.position.x / scale) + startX yObstacle = int(cubeObstacle.pose.position.y / scale) + startY print("Obstacle found:(", xObstacle, ",", yObstacle, ")") addCubeObstacle(grid, (xObstacle, yObstacle)) #Stop robot if we are close enough to goal. numSquaresThreshold = 1 goalX = grid.getGoals()[0][0] goalY = grid.getGoals()[0][1] distanceToGoal = math.sqrt( math.pow(goalX - xR, 2) + math.pow(goalY - yR, 2)) if cube1 is not None: if distanceToGoal < numSquaresThreshold: #Rotate the robot to face cube's face and finish degreesToTurn = normalizeAngle( cube1.pose.rotation.angle_z.degrees + 180 - robot.pose.rotation.angle_z.degrees) print("degrees to turn:", degreesToTurn) robot.turn_in_place(degrees(degreesToTurn), speed=degrees(40)).wait_for_completed() break elif distanceToGoal < numSquaresThreshold or inCenter: # We haven't found cube1 yet but we are in the center of the grid. Just rotate to look around inCenter = True robot.turn_in_place(degrees(20), speed=degrees(40)).wait_for_completed() continue # Use A* to find the path to cube astar(grid, heuristic) path = grid.getPath() if len(path) >= 2: movStart = path[0] movEnd = path[1] robotAngle = math.degrees( math.atan2(movEnd[1] - movStart[1], movEnd[0] - movStart[0])) newRobotX = robot.pose.position.x + (movEnd[0] - movStart[0]) * scale newRobotY = robot.pose.position.y + (movEnd[1] - movStart[1]) * scale robot.go_to_pose(Pose(newRobotX, newRobotY, 0, angle_z=degrees(robotAngle)), relative_to_robot=False).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 #robot.pose = Pose(0,0,0,angle_z = degrees(0)) scale = grid.getScale() start = grid.getStart() #print("Start %i,%i",start[0],start[1]) #print("Scale %i",scale) #print(robot.pose) pose = cell_to_pose(start, scale, 0) #print(pose) robot.go_to_pose(cell_to_pose(start, scale, 0)).wait_for_completed() cubes_seen = set() while not stopevent.is_set(): cube = None count = 0 while (cube is None): try: cube = robot.world.wait_for_observed_light_cube(timeout=1) except Exception: robot.turn_in_place( cozmo.util.degrees(15)).wait_for_completed() count += 1 if count >= 6: start = (start[0] + 1, start[1] + 1) grid.setStart(start) robot.go_to_pose(cell_to_pose(start, scale, 0)).wait_for_completed() count = 0 cubes_seen.add(cube.cube_id) finalanlge = cube.pose.rotation.angle_z.degrees robot.say_text("I see the cube").wait_for_completed() #print(cube.pose) block = pose_to_cell(cube.pose, scale) grid.addObstacle(block) finish = get_finish_from_pose(cube.pose, scale) grid.addGoal(finish) astar(grid, heuristic) newpath = True while newpath: newpath = False path = grid.getPath() last = path[0] for i in range(len(path)): angle = 0 if i + 1 < len(path): angle = get_angle(path[i], path[i + 1]) robot.go_to_pose(cell_to_pose(path[i], scale, angle)).wait_for_completed() newcube = None try: newcube = robot.world.wait_for_observed_light_cube( timeout=1) except Exception: print("don't do anything") if newcube is not None and newcube.cube_id not in cubes_seen: cubes_seen.add(newcube.cube_id) robot.say_text("I found a new cube").wait_for_completed() grid.setStart(path[i]) grid.addObstacle(pose_to_cell(newcube.pose, scale)) grid.clearPath() grid.clearVisited() astar(grid, heuristic) newpath = True break if not newpath: robotangle = robot.pose.rotation.angle_z.degrees robot.turn_in_place( cozmo.util.degrees(finalanlge - robotangle)).wait_for_completed() break stopevent.set()
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 # Start robot at start location x = grid.getStart()[0] * grid.scale y = grid.getStart()[1] * grid.scale robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0))).wait_for_completed() # Add dummy goal x = int(grid.width / 2) y = int(grid.height / 2) grid.addGoal((x, y)) foundCubes = set() goalRotation = 0 while not stopevent.is_set(): # Clear visited grid.clearVisited() # Clear path grid.clearPath() # Spin to find cubes until all are found if len(foundCubes) < 3: for _ in range(4): robot.turn_in_place(degrees(90), speed=degrees(180)).wait_for_completed() if len(foundCubes) < 3: start = time.time() while time.time() - start < 1: try: cube = robot.world.wait_for_observed_light_cube( timeout=0.5, include_existing=False) if cube and cube.cube_id not in foundCubes: x, y, _ = cube.pose.position.x_y_z x = int(round(x / grid.scale)) y = int(round(y / grid.scale)) foundCubes.add(cube.cube_id) if cube.cube_id == 1: goalRotation = (round( cube.pose.rotation.angle_z.degrees / 90) * 90) % 360 print(x, y) if goalRotation == 0: x -= 3 elif goalRotation == 90: y -= 3 elif goalRotation == 180: x += 3 else: y += 3 print(x, y) grid.clearGoals() grid.addGoal((x, y)) else: # Add obstacle in 3x3 around cube for i in range(-1, 2, 1): for j in range(-1, 2, 1): grid.addObstacle((x + i, y + j)) except asyncio.TimeoutError: continue # Take next step astar(grid, heuristic) if len(grid.getPath()) < 2: if 1 in foundCubes: robot.turn_in_place(degrees(goalRotation), speed=degrees(180)).wait_for_completed() print("COMPLETE") stopevent.wait() else: print("CUBE 1 NOT VISIBLE") continue # Keep spinning until cube 1 found if len(foundCubes) == 3: index = len(grid.getPath()) - 1 else: index = 1 x = grid.getPath()[index][0] * grid.scale y = grid.getPath()[index][1] * grid.scale robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0))).wait_for_completed() # Update start x, y, _ = robot.pose.position.x_y_z x = int(round(x / grid.scale)) y = int(round(y / grid.scale)) grid.setStart((x, y)) pass
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 custom_objects(robot: cozmo.robot.Robot): # Gestionnaires d'évennements à chaque fois que Cozmo vois ou arrète de voir un objet robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared) robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared) # Création des custom objects objs = objects(robot) if None not in objs: print("All objects defined successfully!") else: print("One or more object definitions failed!") return robot.say_text("À la recherche des objet").wait_for_completed() setup_camera(robot) origin = robot.pose am = ActionManager(robot) stops = 1 while len(stops_visited) < 6: # Chercer les objest lookaround = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) objs = robot.world.wait_until_observe_num_objects( num=1, object_type=CustomObject, timeout=60) lookaround.stop() if objs[0].object_type in stops_visited: continue stops_visited.append(objs[0].object_type) robot.say_text("Objet trouvé").wait_for_completed() if len(objs) > 0: photo(robot) pose = custom_object_pose(robot, objs[0]) robot.go_to_pose(pose, relative_to_robot=False).wait_for_completed() photo(robot) robot.say_text(f"Arrête {stops}").wait_for_completed() am.launch(objs[0]) print("origin: ", origin) robot.go_to_pose(origin, relative_to_robot=False).wait_for_completed() stops += 1 else: print("Cannot locate custom box") robot.play_anim_trigger( cozmo.anim.Triggers.SparkSuccess).wait_for_completed() while True: time.sleep(0.1)
def en_charger(self, robot:cozmo.robot.Robot = None, cmd_args = None): usage = "Cozmo tries to park on is charger, in 3 tries." if robot is None: return usage trial = 1 # try to find the charger charger = None # see if Cozmo already knows where the charger is if robot.world.charger: if robot.world.charger.pose.origin_id == robot.pose.origin_id: if log: 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 if log: print("looking for the charger now...") look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: charger = robot.world.wait_for_observed_charger(timeout=60) 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: if log: print("lifting my arms to manouver...") robot.move_lift(10) # Attempt to drive near to the charger, and then stop. if log: print("Trial number %s" % trial) print("Going for the charger!!!") action = robot.go_to_pose(charger.pose) action.wait_for_completed() if log: print("Completed action: result = %s" % action) robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed() if log: print("Done.") # Turn 180 (and 5) degrees, then goes backwards at full speed if log: print("Now the grand finalle: turn around and park!") print("Turning...") #this value needs to be tweaked (90 or 95) robot.turn_in_place(degrees(90)).wait_for_completed() robot.turn_in_place(degrees(95)).wait_for_completed() time.sleep( 1 ) if log: print("Get out of the way: here I go!") robot.drive_straight(distance_mm(-130), speed_mmps(150)).wait_for_completed() if log: print("checking if I did it...") if robot.is_on_charger: robot.move_lift(-8) print("I did it! Yay!") else: print("I did not manage to dock in the charger =(") print("Trying again...") robot.world.charger = None if log: print("let me go a little bit further to be easier to see...") robot.drive_straight(distance_mm(90), speed_mmps(50)).wait_for_completed() trial += 1 if trial < 4: self.en_charger(robot) else: print("tired of trying. Giving up =(")
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 while not stopevent.is_set(): # Step 1: Update Map: Update the map to correctly represent all cubes detected by the robot. robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed() cube1 = robot.world.get_light_cube(LightCube1Id) if cube1 is not None: cube1.set_lights(cozmo.lights.red_light) else: cozmo.logger.warning( "Cozmo is not connected to a LightCube1Id cube - check the battery." ) foundCube1 = False cubesFound = {} # from_global_to_grid_position() # from_grid_position_to_global() originalPose = robot.pose while not foundCube1: visibleObjectCount = robot.world.visible_object_count( object_type=None) visibleObjects = robot.world.visible_objects if visibleObjectCount > 0: for visibleObject in visibleObjects: if isinstance(visibleObject, cozmo.objects.LightCube): cubesFound[ visibleObject. object_id] = visibleObject.pose.define_pose_relative_this( originalPose) print("Found cube") print(visibleObject.object_id) print(cubesFound[visibleObject.object_id]) if visibleObject.object_id == 2: foundCube1 = True if not foundCube1: robot.turn_in_place(degrees(10)).wait_for_completed() # time.sleep(1) if cube1 is not None: cube1.set_lights(cozmo.lights.green_light) grid.clearVisited() grid.clearGoals() grid.clearStart() grid.clearPath() grid.setStart((0, 0)) cubePositions = [ (cubeId, cubeToAdd.position.x, cubeToAdd.position.y, cubeToAdd.position.z, cubeToAdd.rotation.angle_z.degrees) for cubeId, cubeToAdd in cubesFound.items() ] print(cubePositions) cubeObstacles = [ (int(cubeX / grid.scale), int(cubeY / grid.scale)) for cubeId, cubeX, cubeY, cubeZ, cubeAngle in cubePositions if cubeId != 2 ] print(cubeObstacles) for cubeObstacle in cubeObstacles: addObstaclesAround(grid, cubeObstacle) # grid.addObstacles(cubeObstacles) goalCube = [(int(cubeX / grid.scale), int(cubeY / grid.scale)) for cubeId, cubeX, cubeY, cubeZ, cubeAngle in cubePositions if cubeId == 2][0] print("goalCube") print(goalCube) grid.addGoal(goalCube) # cube_1_pose = cubesFound[1].pose while not closeEnough(grid.getStart(), grid.getGoals()[0]): newStart = robot.pose.define_pose_relative_this(originalPose) print("newStart") print(newStart) grid.clearStart() grid.setStart(world_to_grid(newStart, grid)) addObstaclesIfSeen(robot, originalPose, grid) grid.clearVisited() grid.clearPath() astar(grid, heuristic) if not grid.checkPath(): print("CONFUSED") return nextStep = grid.getPath()[1] # go one steps ahead stepAfter = grid.getPath()[2] # go two steps ahead print(nextStep) delta_x = (stepAfter[0] * grid.scale - nextStep[0] * grid.scale) delta_y = (stepAfter[1] * grid.scale - nextStep[1] * grid.scale) nextPose = Pose( nextStep[0] * grid.scale, nextStep[1] * grid.scale, originalPose.position.z, angle_z=degrees(90 if delta_x == 0 else 180.0 * math.atan(delta_y / delta_x) / math.pi)) robot.go_to_pose(nextPose, relative_to_robot=False).wait_for_completed() # Step 2: Navigate to Goal: Successfully navigate the robot from a known start location to Cube 1 and stop within 10cm of Cube 1. # Step 3: Cube Orientation: Extend your navigation code to consider which side of the cube the robot is approaching. Have the robot approach the side of the cube that looks like Figure 1. The orientation of the cube can be accessed with cube.pose.rotation.angle_z. The robot should stop within 10cm of the cube, facing toward the correct side of the cube but without touching it. goalPose = Pose(cube1.pose.position.x + 100 * math.cos(cube1.pose.rotation.angle_z.radians), cube1.pose.position.y + 100 * math.sin(cube1.pose.rotation.angle_z.radians), cube1.pose.position.z, angle_z=degrees( (720 + cube1.pose.rotation.angle_z.degrees - 180) % 360)) robot.go_to_pose(goalPose, relative_to_robot=False).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot): logging.basicConfig(format='%(asctime)s - %(message)s', filename="cozmo_program.log", level=logging.INFO) reset_sequence() pose = robot.pose # explore world logging.info("Cozmo starts exploring") cube, cube1 = explore_the_world(robot) logging.info("Cozmo detected Cubes:", cube, cube1) get_in_position(robot) # meet the player face = find_face(robot) logging.info("Identified Face:", face) robot.go_to_pose(pose, relative_to_robot=False).wait_for_completed() ############################################## Game Preparation ################################### robot.say_text("I will try to stack the cubes first").wait_for_completed() robot.say_text("Then it will be your turn").wait_for_completed() # Autonomous play cube_stack(robot) logging.info("Victory Condition check - Cozmo game") # Victory condition check if abs(cube.pose.position.z - cube1.pose.position.z) > 40: # Assuming cube height (z) is 40mm. logging.info("Cube is successfully stacked") else: logging.info("Cube stack failed!") # reset game reset_game(robot) ############################################# Play the game ###################################### # Explain game rules to the player robot.say_text("It's your turn now!").wait_for_completed() robot.say_text( "You need to show me the cards and i will execute the actions" ).wait_for_completed() robot.say_text( "Remember, The goal is to stack the cubes").wait_for_completed() victory_flag = 1 logging.info("Human interaction with cozmo starts") while (victory_flag): reset_sequence() # Show markers result = cozmo_action_ar_marker_cards(robot) logging.info("Marker Cards detected - Action Sequence results: ", result) # Execute the actions. logging.info("Executing Sequence") execute_sequence(robot, result) ############################################# Game result ######################################## # results of the game logging.info("Victory condition check - Human game") if abs(cube.pose.position.z - cube1.pose.position.z ) > 40: # Assuming cube height (z) is 40mm. logging.info("Cube is successfully stacked") robot.say_text("Well done").wait_for_completed() victory_flag = 0 else: logging.info("Cube stack failed!") logging.info( "Reset the game and start again! Press Cntrl + C to exit") robot.say_text( "Nice try! do you want to play again?").wait_for_completed() break