def drive_to_charger(robot):
    '''The core of the drive_to_charger program'''

    # If the robot was on the charger, drive them forward and clear of the charger
    if robot.is_on_charger:
        # drive off the charger
        robot.drive_off_charger_contacts().wait_for_completed()
        robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
        # Start moving the lift down
        robot.move_lift(-3)
        # turn around to look at the charger
        robot.turn_in_place(degrees(180)).wait_for_completed()
        # Tilt the head to be level
        robot.set_head_angle(degrees(0)).wait_for_completed()
        # wait half a second to ensure Cozmo has seen the charger
        time.sleep(0.5)
        # drive backwards away from the charger
        robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed()

    # try to find the charger
    charger = None

    # see if Cozmo already knows where the charger is
    if robot.world.charger:
        if robot.world.charger.pose.origin_id == robot.pose.origin_id:
            print("Cozmo already knows where the charger is!")
            charger = robot.world.charger
        else:
            # Cozmo knows about the charger, but the pose is not based on the
            # same origin as the robot (e.g. the robot was moved since seeing
            # the charger) so try to look for the charger first
            pass

    if not charger:
        # Tell Cozmo to look around for the charger
        look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
        try:
            charger = robot.world.wait_for_observed_charger(timeout=30)
            print("Found charger: %s" % charger)
        except asyncio.TimeoutError:
            print("Didn't see the charger")
        finally:
            # whether we find it or not, we want to stop the behavior
            look_around.stop()

    if charger:
        # Attempt to drive near to the charger, and then stop.
        action = robot.go_to_object(charger, distance_mm(65.0))
        action.wait_for_completed()
        print("Completed action: result = %s" % action)
        print("Done.")
Example #2
0
def transport_cube_III(cube: cozmo.objects.LightCube):
    global robot,SIDE

    # Third cube was picked, go place it next to the others
    charger = go_to_charger()
    final_adjust(charger,80)
    robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
    robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed()
    putDown_cube(cube)
    switch_cube_off(cube)
    # Get back in front of charger
    robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed()
    robot.turn_in_place(degrees(-SIDE*60)).wait_for_completed()
    return
Example #3
0
def transport_cube_I(cube: cozmo.objects.LightCube):
    global robot,SIDE

    # First cube was picked, place it next to the charger
    charger = go_to_charger()
    final_adjust(charger,60)
    robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
    robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed()
    putDown_cube(cube)
    switch_cube_off(cube)
    # Get back in front of charger
    robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
    robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
    robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()
    return
def custom_objects(robot: cozmo.robot.Robot):
    # Add event handlers for whenever Cozmo sees a new object
    robot.add_event_handler(cozmo.objects.EvtObjectAppeared,
                            handle_object_appeared)
    #robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)

    # define a unique wall (150mm x 120mm (x10mm thick for all walls)
    # with a 50mm x 30mm Circles2 image on front and back
    wall_obj = robot.world.define_custom_wall(CustomObjectTypes.CustomType02,
                                              CustomObjectMarkers.Circles2,
                                              140, 130, 65, 65, False)
    wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03,
                                               CustomObjectMarkers.Diamonds2,
                                               140, 130, 64, 64, False)

    #    if ((cube_obj is not None) and (big_cube_obj is not None) and
    #           (wall_obj is not None) and (box_obj is not None)):
    #       print("All objects defined successfully!")
    #    else:
    #        print("One or more object definitions failed!")
    #       return

    print(
        "Show the above markers to Cozmo and you will see the related objects "
        "annotated in Cozmo's view window, you will also see print messages "
        "everytime a custom object enters or exits Cozmo's view.")

    print("Press CTRL-C to quit")

    # Move lift down and tilt the head up
    robot.move_lift(100)
    robot.set_head_angle(degrees(-25))
    while True:
        global found_wall
        global found_wall1
        if found_wall == True:
            robot.drive_straight(distance_mm(150),
                                 speed_mmps(50)).wait_for_completed()
            robot.turn_in_place(degrees(90)).wait_for_completed()
            found_wall = False
            robot.set_head_angle(degrees(-25))
        if found_wall1 == True:
            robot.drive_straight(distance_mm(150),
                                 speed_mmps(50)).wait_for_completed()
            robot.turn_in_place(degrees(-90)).wait_for_completed()
            found_wall1 = False
            robot.set_head_angle(degrees(-25))
        time.sleep(0.1)
Example #5
0
    def __handle_drive_forward(self, command, agent):
        """
        Handle a Soar drive-forward action.

        The Sour output should look like:
        (I3 ^drive-forward Vx)
          (Vx ^distance [dist]
              ^speed [spd])
        where [dist] is a real number indicating how far Cozmo should travel (negatives go
        backwards) and speed is how fast Cozmo should travel. Units are mm and mm/s, respectively.

        :param command: Soar command object
        :param agent: Soar Agent object
        :return: True if successful, False otherwise
        """
        try:
            distance = distance_mm(float(command.GetParameterValue("distance")))
        except ValueError as e:
            print("Invalid distance format {}".format(command.GetParameterValue("distance")))
            return False
        try:
            speed = speed_mmps(float(command.GetParameterValue("speed")))
        except ValueError as e:
            print("Invalid speed format {}".format(command.GetParameterValue("speed")))
            return False

        print("Driving forward {}mm at {}mm/s".format(distance.distance_mm, speed.speed_mmps))
        drive_forward_action = self.r.drive_straight(distance, speed)
        self.running_actions.append((command, drive_forward_action))
        # callback = self.__handle_action_complete_factory(command)
        # drive_forward_action.add_event_handler(EvtActionCompleted, callback)
        return True
Example #6
0
    def __init__(self, coz):
        mixer.init()
        self.soundHappy = mixer.Sound('static/sounds/bg.ogg')
        self.soundSad = mixer.Sound('static/sounds/boring.ogg')
        self.soundSad.play(loops=-1)

        self.cozmo = coz
        self.arcadeGame = Arcade(self.cozmo, self)
        self.autonomousInstance = Patrol(self,self.cozmo)
        self.merrygoround = MerryGoRound(self.cozmo, self)
        # self.instagram = MemCapture(self.cozmo,self);

        self.define_custom_objects()

        self.action_queue = []

        self.lift_up = 0
        self.lift_down = 0
        self.head_up = 0
        self.head_down = 0

        self.text_to_say = ""
        self.cozmo_audio_effect_interval = random.randint(200,1000)
        self.update_count = 0

        self.anims_for_keys = ["bored",  # 1
                                  "sad",  # 2
                                  "happy",  # 3
                               "veryHappy", #4
                               "laugh", #5
                               "angry", #6
                               "impress" #7
                                 ]

        self.cozmo.set_lift_height(0,in_parallel=True)
        self.cozmo.set_head_angle(cozmo.util.Angle(degrees=-15),in_parallel=True).wait_for_completed()

        self.visible_objects = []
        self.measuring_dist = False

        _thread.start_new_thread(self.start_Pizza_Thread, ())

        self.cubes = None
        try:
            self.cubes = self.cozmo.world.wait_until_observe_num_objects(1, object_type = cozmo.objects.LightCube,timeout=10)
        except asyncio.TimeoutError:
            print("Didn't find a cube :-(")
            return
        finally:
            # for cube in self.cubes:
            #     cube.set_lights(Colors.BLUE)
            if len(self.cubes) > 0:
                self.cozmo.camera.image_stream_enabled = True
                self.cubes[0].set_lights_off()
                self.cozmo.drive_straight(distance_mm(10), speed_mmps(50), in_parallel=True).wait_for_completed()
                self.cozmo.set_head_angle(cozmo.util.Angle(degrees=30),in_parallel=True)
                self.cozmo.world.add_event_handler(cozmo.objects.EvtObjectAppeared, self.on_object_appeared)
                self.cozmo.world.add_event_handler(cozmo.objects.EvtObjectDisappeared, self.on_object_disappeared)
            else:
                print("Not found")
    def en_drive(self, robot:cozmo.robot.Robot = None, cmd_args = None):
        usage = "Cozmo drives forward/backwards for X seconds (i.e. 'Cozmo, drive backwards for 3 seconds')."
        if robot is None:
            return usage

        error_message = ""
        if log:
            print(cmd_args)

        #check if the user said "drive to charger" and redirect the command to en_charger()
        if "charger" in cmd_args:
            self.en_charger(robot, cmd_args)
            return

        drive_duration = extract_next_float(cmd_args)#[0]

        if drive_duration is not None:

            if "backward" in cmd_args or "backwards" in cmd_args:
                drive_speed = speed
                drive_duration = -drive_duration
                drive_dir = "backwards"
            else:
                drive_speed = speed
                drive_dir = "forward"

            #robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration)
            robot.drive_straight(distance_mm(drive_duration*drive_speed), speed_mmps(drive_speed), should_play_anim=True).wait_for_completed()
            #time.sleep(drive_duration)

            return "I drove " + drive_dir + " for " + str(drive_duration) + " seconds!"

        return "Error: usage = " + usage + error_message
 def check_charger(self, robot:cozmo.robot.Robot, distance=150, speed=100):
     if robot.is_on_charger:
         if log:
             print("I am on the charger. Driving off the charger...")
         robot.drive_off_charger_contacts().wait_for_completed()
         robot.drive_straight(distance_mm(distance), speed_mmps(speed)).wait_for_completed()
         robot.move_lift(-8)
Example #9
0
def run(sdk_conn):
    '''The run method runs once Cozmo is connected.'''
    robot = sdk_conn.wait_for_robot()

    for _ in range(4):
        robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
        robot.turn_in_place(degrees(90)).wait_for_completed()
def idle(robot, img_clf):
    count = 0
    while True:
        robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
        data = []
        time.sleep(0.5)

        if count < 10:
            pass
        elif count % 4 == 0:
            robot.turn_in_place(degrees(50)).wait_for_completed()
            robot.drive_straight(distance_mm(random.uniform(-20, 20)),
                                 speed_mmps(80)).wait_for_completed()
        else:
            robot.turn_in_place(degrees(-10)).wait_for_completed()

        count += 1

        latestImage = robot.world.latest_image
        newImage = latestImage.raw_image
        data.append(np.array(newImage))
        feature = img_clf.extract_image_features(data)

        symbol = img_clf.classifier.predict(feature)
        if symbol == "drone":
            drone(robot)
        elif symbol == "order":
            order(robot)
            count = 0
        elif symbol == "inspection":
            inspection(robot)
            count = 0
Example #11
0
async def CozmoPlanning(robot: cozmo.robot.Robot):
    # Allows access to map and stopevent, which can be used to see if the GUI
    # has been closed by checking stopevent.is_set()
    global cmap, stopevent

    ########################################################################
    # TODO: please enter your code below.
    # Set start node
    # cmap.set_start(start_node)
    # Start RRT
    RRT(cmap, cmap.get_start())
    path = cmap.get_smooth_path()
    index = 1
    # Path Planning for RRT
    while index < len(path):
        curr_node = path[index]
        dx = curr_node.x - path[index - 1].x
        dy = curr_node.y - path[index - 1].y
        # turn_angle = diff_heading_deg(cozmo.util, np.arctan2(dy, dx))
        # print("dx=", dx)
        # print("dy=", dy)
        # print("tan=", np.arctan2(dy, dx))
        await robot.turn_in_place(
            radians(np.arctan2(dy, dx)),
            angle_tolerance=Angle(1)).wait_for_completed()
        await robot.drive_straight(
            distance_mm(np.sqrt((dx**2) + (dy**2))),
            speed=cozmo.util.speed_mmps(50)).wait_for_completed()
        index += 1
    Visualizer.update(cmap)
Example #12
0
def inspection(robot: cozmo.robot.Robot):
    # Use a "for loop" to repeat the indented code 4 times
    # Note: the _ variable name can be used when you don't need the value
    # drive in a square with side 20cm
    robot.set_lift_height(0.0, in_parallel=False, duration=1.0).wait_for_completed()
    robot.abort_all_actions(log_abort_messages=True)
    for i in range(4):

        # raise the arm for 3 seconds, at a speed of 1/3 = 0.333
        robot.move_lift(0.33)
        robot.drive_straight(distance_mm(200), speed_mmps(40), should_play_anim=False, in_parallel=True, num_retries=0)
        time.sleep(3)

        # lower the arm for 2 seconds, at a speed of 1/2 = 0.5
        robot.move_lift(-0.50)
        time.sleep(2)

        # abort all ongoing actions
        robot.abort_all_actions(log_abort_messages=True)

        # perform the turn: 90 degree
        robot.turn_in_place(degrees(90)).wait_for_completed()
    # stop all actions, if there are any
    robot.abort_all_actions(log_abort_messages=True)
    
    # return to idle state
    idle(robot)
Example #13
0
def follow_cube(robot):
    target_id = LightCube1Id
    colors = [
        cozmo.lights.red_light, cozmo.lights.blue_light,
        cozmo.lights.green_light
    ]

    cube = search_and_go_to_cube(robot, target_id)

    while cube:
        cube.set_lights(random.choice(colors))
        action = robot.go_to_object(cube, distance_mm(70.0))
        action.wait_for_completed()
        cube.set_lights_off()

        target_in_view = False
        for visible_object in robot.world.visible_objects:
            if isinstance(visible_object, cozmo.objects.LightCube
                          ) and visible_object.cube_id == target_id:
                target_in_view = True

        if not target_in_view:
            cube = search_and_go_to_cube(robot, target_id)
        else:
            time.sleep(2)
Example #14
0
def example3_abort_one_action(robot: cozmo.robot.Robot):
    cozmo.logger.info("----------------------------------------")
    cozmo.logger.info("Example 3: Abort some parallel actions.")
    cozmo.logger.info("Start multiple parallel actions:")
    action1 = robot.set_lift_height(0.0, in_parallel=True)
    action2 = robot.set_head_angle(cozmo.robot.MIN_HEAD_ANGLE,
                                   duration=6.0,
                                   in_parallel=True)
    action3 = robot.drive_straight(distance_mm(75),
                                   speed_mmps(25),
                                   should_play_anim=False,
                                   in_parallel=True)
    action4 = robot.display_oled_face_image(
        face_image, 30000.0)  # Note: face image is in_parallel by default
    # Lift-lowering is aborted immediately
    action1.abort(log_abort_messages=True)
    # Head-lowering is aborted shortly afterwards
    time.sleep(0.1)
    action2.abort(log_abort_messages=True)
    # Image on Cozmo's face is aborted another 2 seconds later
    time.sleep(2)
    action4.abort(log_abort_messages=True)
    # We wait for the one remaining action to complete
    action3.wait_for_completed()
    # Only action3 should succeed (as long as Cozmo had enough space to drive)
    cozmo.logger.info("action1 = %s", action1)
    cozmo.logger.info("action2 = %s", action2)
    cozmo.logger.info("action3 = %s", action3)
    cozmo.logger.info("action4 = %s", action4)
Example #15
0
def go_to_charger():
    # Driving towards charger without much precision
    global robot

    charger = None
    ''' cf. 08_drive_to_charger_test.py '''
    # see if Cozmo already knows where the charger is
    if robot.world.charger:
        # make sure Cozmo was not delocalised after observing the charger
        if robot.world.charger.pose.is_comparable(robot.pose):
            print("Cozmo already knows where the charger is!")
            charger = robot.world.charger
        else:
            # Cozmo knows about the charger, but the pose is not based on the
            # same origin as the robot (e.g. the robot was moved since seeing
            # the charger) so try to look for the charger first
            pass
    if not charger:
        charger = find_charger()

    action = robot.go_to_object(charger,
                                distance_from_object=distance_mm(80),
                                in_parallel=False,
                                num_retries=5)
    #action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    return charger
Example #16
0
def cozmo_program(robot: cozmo.robot.Robot):
    # Use a "for loop" to repeat the indented code 4 times
    # Note: the _ variable name can be used when you don't need the value
    for _ in range(4):
        robot.drive_straight(distance_mm(320),
                             speed_mmps(50)).wait_for_completed()
        robot.turn_in_place(degrees(90)).wait_for_completed()
Example #17
0
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()
Example #18
0
    def __handle_go_to_object(self, command, agent):
        """
        Handle a Soar go-to-object action.

        The Sour output should look like:
        (I3 ^go-to-object Vx)
          (Vx ^target_object_id [id])
        where [id] is the object id of the object to go to. Cozmo will stop 150mm from the object.

        :param command: Soar command object
        :param agent: Soar Agent object
        :return: True if successful, False otherwise
        """
        try:
            target_id = int(command.GetParameterValue("target_object_id"))
        except ValueError as e:
            print(
                "Invalid target-object-id format {}".format(
                    command.GetParameterValue("target_object_id")
                )
            )
            return False
        if target_id not in self.objects.keys():
            print("Couldn't find target object")
            return False

        print("Going to object {}".format(target_id))
        target_obj = self.objects[target_id]
        go_to_object_action = self.robot.go_to_object(target_obj, distance_mm(100))
        self.running_actions.append((command, go_to_object_action))
        # callback = self.__handle_action_complete_factory(command)
        # go_to_object_action.add_event_handler(EvtActionCompleted, callback)
        return True
Example #19
0
    def __init__(self, robot):
        self.robot = robot
        print('Current state:', str(self))
        """
        Do action here
        """
        count_turns = 0
        while count_turns <= 3:
            action1 = robot.drive_straight(distance_mm(200),
                                           speed_mmps(30),
                                           in_parallel=True)
            action2 = robot.set_lift_height(1.0,
                                            in_parallel=True,
                                            duration=3.0)
            action2.wait_for_completed()
            action2 = robot.set_lift_height(0.0,
                                            in_parallel=True,
                                            duration=3.0)
            action2.wait_for_completed()

            action1.wait_for_completed()
            action1 = robot.turn_in_place(
                degrees(90), in_parallel=True).wait_for_completed()

            count_turns = count_turns + 1

        action2 = robot.set_lift_height(0.0, in_parallel=True,
                                        duration=3.0).wait_for_completed()
        self.return_to_idle()
Example #20
0
 async def get_sad(self):
     #Cozmo behavior - get sad on seeing marker on photo of his pirate father
     await self.robot.go_to_object(self.objects[0],
                                   distance_mm(100)).wait_for_completed()
     await self.robot.play_anim_trigger(
         cozmo.anim.Triggers.RequestGameKeepAwayDeny1).wait_for_completed()
     x = random.randrange(4)
     if x == 0:
         await self.robot.say_text("I miss you father",
                                   duration_scalar=2,
                                   voice_pitch=0).wait_for_completed()
     elif x == 1:
         await self.robot.say_text("You will always be in my heart, father",
                                   duration_scalar=1.5,
                                   voice_pitch=0).wait_for_completed()
     elif x == 2:
         await self.robot.say_text("Father",
                                   duration_scalar=2,
                                   voice_pitch=-1).wait_for_completed()
         await self.robot.say_text("Oh, father",
                                   duration_scalar=2,
                                   voice_pitch=0).wait_for_completed()
     elif x == 3:
         await self.robot.say_text("What is dead may never die",
                                   duration_scalar=2,
                                   voice_pitch=-1).wait_for_completed()
     await asyncio.sleep(2)
     await self.robot.turn_in_place(cozmo.util.Angle(degrees=180)
                                    ).wait_for_completed()
def go_to_object_test(robot):
    '''The core of the go to object test program'''

    # Move lift down and tilt the head up
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()

    # look around and try to find a cube
    look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)

    cube = None

    try:
        cube = robot.world.wait_for_observed_light_cube(timeout=30)
        print("Found cube: %s" % cube)
    except asyncio.TimeoutError:
        print("Didn't find a cube")
    finally:
        # whether we find it or not, we want to stop the behavior
        look_around.stop()

    if cube:
        # Drive to 70mm away from the cube (much closer and Cozmo
        # will likely hit the cube) and then stop.
        action = robot.go_to_object(cube, distance_mm(70.0))
        action.wait_for_completed()
        print("Completed action: result = %s" % action)
        print("Done.")
def drone(robot):
    robot.say_text('drone').wait_for_completed()
    time.sleep(0.5)
    lookaround = robot.start_behavior(
        cozmo.behavior.BehaviorTypes.LookAroundInPlace)
    cubes = robot.world.wait_until_observe_num_objects(
        num=1, object_type=cozmo.objects.LightCube, timeout=60)
    lookaround.stop()

    if len(cubes) > 0:
        robot.pickup_object(cubes[0], num_retries=5).wait_for_completed()
        robot.drive_straight(distance_mm(100),
                             speed_mmps(80)).wait_for_completed()
        robot.place_object_on_ground_here(cubes[0]).wait_for_completed()
        robot.drive_straight(distance_mm(-80),
                             speed_mmps(80)).wait_for_completed()
Example #23
0
def moveToGoal(robot: cozmo.robot.Robot, cmap, stack, cube):
    print("Move To Goal State")
    cube = None
    goals = cmap.get_goals();
    goal = goals[0]
    if stack:
        try:
            cube = robot.world.wait_for_observed_light_cube(timeout = 4)
            print(cube)
        except asyncio.TimeoutError:
            print("no obstacles found")
            pass
        if cube is not None and cube.object_id != cubeID:
            print("Tadah")
            robot.foundObstacle()
        else:
            # Keep moving
            curr = stack.pop()
            ro = (robot.pose.position.x, robot.pose.position.y)
            if ((goal[0] - 90 < ro[0] < goal[0] + 90) and (goal[1] - 90 < ro[1] < goal[1] + 90)):
                print("Yay");
                robot.reachedGoal()
            dist = get_dist(Node(ro), Node(curr)) #get distance
            angle = np.arctan2(curr[1]-ro[1],curr[0]-ro[0])*180/np.pi - robot.pose_angle.degrees
            print(angle)
            robot.turn_in_place(degrees(angle)).wait_for_completed()
            robot.drive_straight(distance_mm(dist), speed_mmps(50)).wait_for_completed()
    else:
        print("Yay")
        robot.reachedGoal()

    return robot, stack, cube # returns cozmo
Example #24
0
def docking_cozmo(cozmo_robot: cozmo.robot.Robot):
    global robot
    init_robot(cozmo_robot)
    if robot.is_on_charger :
        robot.drive_straight(distance_mm(200),speed_mmps(80)).wait_for_completed()
    get_on_charger()
    return
def cozmo_go_to_object(sdk_conn):
    print("SENDING COZMO TO OBJECT %s" % cube_to_go)
    robot = sdk_conn.wait_for_robot()

    # Move lift down and tilt the head up
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()

    cube_detination = None

    lookaround = robot.start_behavior(
        cozmo.behavior.BehaviorTypes.LookAroundInPlace)
    cubes = robot.world.wait_until_observe_num_objects(
        num=3, object_type=cozmo.objects.LightCube, timeout=60)
    lookaround.stop()

    if len(cubes) < 1:
        print("Error: Cube not found")
    else:
        for cube in cubes:
            print("Comparing cubes: %s" % cube.object_id)
            if (str(cube.object_id) == str(cube_to_go)):
                cube_detination = cube
                print("CUBE FOUND!")
    if cube_detination:
        # Drive to 70mm away from the cube (much closer and Cozmo
        # will likely hit the cube) and then stop.
        cube_detination.set_lights(cozmo.lights.green_light)
        action = robot.go_to_object(cube_detination, distance_mm(70.0))
        action.wait_for_completed()
        print("Completed action: result = %s" % action)
        print("Done.")
    else:
        print("CUBE NOT FOUND!!!!")
async def execute_directions(robot, directions):
    print("Robot is at: ", robot.pose)
    await robot.turn_in_place(angle=directions.rotation.angle_z
                              ).wait_for_completed()
    print("Robot is at after turning to be parrael to X: ", robot.pose)
    await robot.drive_straight(distance=distance_mm(directions.position.x *
                                                    grid.scale),
                               speed=speed_mmps(80)).wait_for_completed()
    print("Robot is at after driving in the X dirrection: ", robot.pose)
    await robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)
                              ).wait_for_completed()
    print("Robot is at after turning to the Y dirrection: ", robot.pose)
    await robot.drive_straight(distance=distance_mm(directions.position.y *
                                                    grid.scale),
                               speed=speed_mmps(80)).wait_for_completed()
    print("Robot is at after driving in the Y dirreciton: ", robot.pose)
Example #27
0
 async def go_to_cube(self, cube):
     await self._robot.set_lift_height(1, duration=0.5).wait_for_completed()
     await self._robot.go_to_object(
         cube, distance_mm(30.0)
     ).wait_for_completed()
     self._wait_for_tap_flag = True
     await self.coz_tap(cube)
def returnToCharger(robot):
    robot.turn_in_place(degrees(-180)).wait_for_completed()
    #looks for charger
    charger = None
    # see if Cozmo already knows where the charger is
    if robot.world.charger:
        if robot.world.charger.pose.is_comparable(robot.pose):
            print("Cozmo already knows where the charger is!")
            charger = robot.world.charger
        else:
            pass
    # look for charger here
    if not charger:
        look_around = robot.start_behavior(
            cozmo.behavior.BehaviorTypes.LookAroundInPlace)
        try:
            charger = robot.world.wait_for_observed_charger(timeout=5)
        except asyncio.TimeoutError:
            print("Didn't see the charger")
        finally:
            look_around.stop()
    # if charger, dock with it
    if charger:
        # Attempt to drive near to the charger, and then stop.
        action = robot.go_to_object(charger, distance_mm(65.0))
        action.wait_for_completed()
        robot.turn_in_place(cozmo.util.degrees(180)).wait_for_completed()
        robot.drive_straight(cozmo.util.distance_mm(-175),
                             cozmo.util.speed_mmps(50)).wait_for_completed()
        print("Done.")
Example #29
0
    def forward(self,
                robot: cozmo.robot.Robot = None,
                cmd_args=None,
                invert=False):

        if self.log:
            print(cmd_args)

        drive_duration = extract_next_float(cmd_args)  #[0]

        if drive_duration is not None:

            if invert:
                drive_speed = speed
                drive_duration = -drive_duration
                drive_dir = "backwards"
            else:
                drive_speed = speed
                drive_dir = "forward"

            #robot.drive_wheels(drive_speed, drive_speed, duration=drive_duration)
            robot.drive_straight(distance_mm(drive_duration * drive_speed),
                                 speed_mmps(drive_speed),
                                 should_play_anim=True).wait_for_completed()
            #time.sleep(drive_duration)

            return "I drove " + drive_dir + " for " + str(
                drive_duration) + " seconds!"

        return "Error: bad drive duration!"
Example #30
0
    def robot_step(self, robot: cozmo.robot.Robot):

        if not robot.is_cliff_detected:
            robot.drive_straight(distance_mm(50),
                                 speed_mmps(70)).wait_for_completed()
        else:
            print('cliff detected')
Example #31
0
def cozmo_program(robot: cozmo.robot.Robot):
    robot.world.image_annotator.add_static_text(
        'text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT)
    robot.world.image_annotator.add_annotator('clock', clock)
    robot.world.image_annotator.add_annotator('battery', Battery)
    print("--------------------------")
    print("Battery (below 3.5V is low)")
    print(robot.world.robot.battery_voltage)
    print("--------------------------")

    robot.world.add_event_handler(cozmo.objects.EvtObjectTapped, object_tapped)
    # Initialization
    if robot.is_on_charger:
        robot.drive_off_charger_contacts().wait_for_completed()
        robot.drive_straight(distance_mm(100),
                             speed_mmps(50)).wait_for_completed()

    robot.start_freeplay_behaviors()

    while True:
        time.sleep(1)
        # Run mini games
        run = random.randint(0, 100)
        global interaction
        if run == 1:
            interaction = 1
            useless(robot)
        interaction = 0
Example #32
0
    async def _final_adjust_async(self,
                                  charger: Charger,
                                  dist_charger=40,
                                  speed=40,
                                  critical=False) -> None:
        # Final adjustement to properly face the charger.
        # The position can be adjusted several times if
        # the precision is critical, i.e. when climbing
        # back onto the charger.
        while (True):
            # Calculate positions
            r_coord = [0, 0, 0]
            c_coord = [0, 0, 0]
            # Coordonates of robot and charger
            # .x .y .z, .rotation otherwise
            r_coord[0] = self._robot.pose.position.x
            r_coord[1] = self._robot.pose.position.y
            r_coord[2] = self._robot.pose.position.z
            r_zRot = self._robot.pose_angle.radians  # .degrees or .radians
            c_coord[0] = charger.pose.position.x
            c_coord[1] = charger.pose.position.y
            c_coord[2] = charger.pose.position.z
            c_zRot = charger.pose.rotation.angle_z.radians

            # Create target position
            # dist_charger in mm, distance if front of charger
            c_coord[0] -= dist_charger * math.cos(c_zRot)
            c_coord[1] -= dist_charger * math.sin(c_zRot)

            # Direction and distance to target position (in front of charger)
            distance = math.sqrt((c_coord[0] - r_coord[0])**2 +
                                 (c_coord[1] - r_coord[1])**2 +
                                 (c_coord[2] - r_coord[2])**2)
            vect = [
                c_coord[0] - r_coord[0], c_coord[1] - r_coord[1],
                c_coord[2] - r_coord[2]
            ]
            # Angle of vector going from robot's origin to target's position
            theta_t = math.atan2(vect[1], vect[0])

            print('CHECK: Adjusting position')
            # Face the target position
            angle = self._clip_angle(theta_t - r_zRot)
            await self._robot.turn_in_place(radians(angle)
                                            ).wait_for_completed()
            # Drive toward the target position
            await self._robot.drive_straight(
                distance_mm(distance), speed_mmps(speed)).wait_for_completed()
            # Face the charger
            angle = self._clip_angle(c_zRot - theta_t)
            await self._robot.turn_in_place(radians(angle)
                                            ).wait_for_completed()

            # In case the robot does not need to climb onto the charger
            if not critical:
                break
            elif (await self._check_tol_async(charger, dist_charger)):
                print('CHECK: Robot aligned relativ to the charger.')
                break
        return
Example #33
0
 async def get_off_charger_async(self):
     print("Getting off charger")
     if self._robot.is_on_charger:
         await self._robot.drive_off_charger_contacts(
             self._robot).wait_for_completed()
         await self._robot.drive_straight(
             distance_mm(100), speed_mmps(100)).wait_for_completed()
    async def run(self):

        '''Program runs until typing CRTL+C into Terminal/Command Prompt, 
        or by closing the viewer window.
        '''    
        if not self.cubes_connected():
            print('Cubes did not connect successfully - check that they are nearby. You may need to replace the batteries.')
            return
        self.turn_on_cubes()
        await self.robot.drive_straight(distance_mm(100), speed_mmps(50), should_play_anim = False).wait_for_completed()



        # Updates self.state and resets self.amount_turned_recently every 1 second.
        # MODIFICATION FROM ORIGINAL SDK = while loop would run contiously until an abort signal was dectected.
        # variable running remains True until Cozmo detects the color passed and changes to False
        # if False, then run returns giving control back to main.py
        while running:
            await asyncio.sleep(1)
            print(self.color_to_find)
            print(self.state)
            print(running)
            if self.state == LOOK_AROUND_STATE:
                await self.start_lookaround()
                print("In look around state")
            if self.state == FOUND_COLOR_STATE and self.amount_turned_recently < self.moving_threshold:
                self.state = DRIVING_STATE
            self.amount_turned_recently = radians(0)

        if running == False:
            self.robot.abort_all_actions()
            return
Example #35
0
def go_to_object_test(robot: cozmo.robot.Robot):
    '''The core of the go to object test program'''

    # Move lift down and tilt the head up
    robot.move_lift(-3)
    robot.set_head_angle(degrees(0)).wait_for_completed()

    # look around and try to find a cube
    look_around = robot.start_behavior(
        cozmo.behavior.BehaviorTypes.LookAroundInPlace)

    cube = None

    try:
        cube = robot.world.wait_for_observed_light_cube(timeout=30)
        print("Found cube: %s" % cube)
    except asyncio.TimeoutError:
        print("Didn't find a cube")
    finally:
        # whether we find it or not, we want to stop the behavior
        look_around.stop()

    if cube:
        # Drive to 70mm away from the cube (much closer and Cozmo
        # will likely hit the cube) and then stop.
        action = robot.go_to_object(cube, distance_mm(70.0))
        action.wait_for_completed()
        print("Completed action: result = %s" % action)
        print("Done.")
Example #36
0
def cozmo_program(cozmo_robot: cozmo.robot.Robot):
    global robot
    init_robot(cozmo_robot)

    # Get off charger if on it
    if(robot.is_on_charger):
    	robot.drive_off_charger_contacts().wait_for_completed()
    	robot.drive_straight(distance_mm(110),speed_mmps(80)).wait_for_completed()

    execute_procedure()
    return
Example #37
0
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False):
    # Final adjustement to properly face the charger.
    # The position can be adjusted several times if 
    # the precision is critical, i.e. when climbing
    # back onto the charger.  
    global robot,PI

    while(True):
        # Calculate positions
	    r_coord = [0,0,0]
	    c_coord = [0,0,0]
	    # Coordonates of robot and charger
	    r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise
	    r_coord[1] = robot.pose.position.y
	    r_coord[2] = robot.pose.position.z
	    r_zRot = robot.pose_angle.radians # .degrees or .radians
	    c_coord[0] = charger.pose.position.x
	    c_coord[1] = charger.pose.position.y
	    c_coord[2] = charger.pose.position.z
	    c_zRot = charger.pose.rotation.angle_z.radians

	    # Create target position 
	    # dist_charger in mm, distance if front of charger
	    c_coord[0] -=  dist_charger*math.cos(c_zRot)
	    c_coord[1] -=  dist_charger*math.sin(c_zRot)

	    # Direction and distance to target position (in front of charger)
	    distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2)
	    vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]]
	    # Angle of vector going from robot's origin to target's position
	    theta_t = math.atan2(vect[1],vect[0])

	    print('CHECK: Adjusting position')
	    # Face the target position
	    angle = clip_angle((theta_t-r_zRot))
	    robot.turn_in_place(radians(angle)).wait_for_completed()
	    # Drive toward the target position
	    robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed()
	    # Face the charger
	    angle = clip_angle((c_zRot-theta_t))
	    robot.turn_in_place(radians(angle)).wait_for_completed()

        # In case the robot does not need to climb onto the charger
	    if not critical:
	        break
	    elif(check_tol(charger,dist_charger)):
	    	print('CHECK: Robot aligned relativ to the charger.')
	    	break
    return
Example #38
0
def run(sdk_conn):
    '''The run method runs once Cozmo is connected.'''
    robot = sdk_conn.wait_for_robot()

    robot.set_all_backpack_lights(cozmo.lights.red_light)

    robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed()

    # make cozmo reverse 100mm (negative distance == reverse)
    robot.drive_straight(distance=distance_mm(-100),
                         speed=speed_mmps(50)).wait_for_completed()

    robot.play_anim_trigger(cozmo.anim.Triggers.ReactToCliff).wait_for_completed()

    robot.say_text("Hello").wait_for_completed()
Example #39
0
def go_to_charger():
    # Driving towards charger without much precision
    global robot

    charger = None
    ''' cf. 08_drive_to_charger_test.py '''
    # see if Cozmo already knows where the charger is
    if robot.world.charger:
        # make sure Cozmo was not delocalised after observing the charger
        if robot.world.charger.pose.is_comparable(robot.pose):
            print("Cozmo already knows where the charger is!")
            charger = robot.world.charger
        else:
            # Cozmo knows about the charger, but the pose is not based on the
            # same origin as the robot (e.g. the robot was moved since seeing
            # the charger) so try to look for the charger first
            pass
    if not charger:
        charger = find_charger()
    
    action = robot.go_to_object(charger,distance_from_object=distance_mm(80), in_parallel=False, num_retries=5)
    #action = robot.go_to_pose(charger.pose)
    action.wait_for_completed()
    return charger
Example #40
0
def knock_cubes_over():
    # This function allows to detect if the three cubes are stacked on each others.
    # If it is the case, Cozmo will try to make them fall.
    global robot

    cube1 = robot.world.get_light_cube(LightCube1Id)
    cube2 = robot.world.get_light_cube(LightCube2Id)
    cube3 = robot.world.get_light_cube(LightCube3Id)

    retries = 5
    i = 0

    for i in range(0,5):
        # Try to see at least 1 cube
        behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
        try: 
            seen_cube = robot.world.wait_for_observed_light_cube(timeout=10,include_existing=True)
        except:
            seen_cube = None
        behavior.stop()
        # Try to observe possible stacked cubes
        robot.set_head_angle(degrees(10)).wait_for_completed()
        time.sleep(.5)
        robot.set_head_angle(degrees(30)).wait_for_completed()
        time.sleep(.5)
        robot.set_head_angle(degrees(0)).wait_for_completed()

        num_observed_cubes = 0
        if(cube1.pose.is_comparable(robot.pose)):
            num_observed_cubes += 1
        if(cube2.pose.is_comparable(robot.pose)):
            num_observed_cubes += 1
        if(cube3.pose.is_comparable(robot.pose)):
            num_observed_cubes += 1
        
        if(num_observed_cubes == 3):
            # All cubes were observed, check if stacked
            ref = []
            ref.append(cube1.pose.position.x)
            ref.append(cube1.pose.position.y)
            tol = 20 # Less than 20 mm means that the cubes are stacked
            delta2 = math.sqrt((ref[0]-cube2.pose.position.x)**2 + (ref[1]-cube2.pose.position.y)**2)
            delta3 = math.sqrt((ref[0]-cube3.pose.position.x)**2 + (ref[1]-cube3.pose.position.y)**2)

            if(delta2 < tol and delta3 < tol):
                try:
                    behavior = robot.start_behavior(cozmo.behavior.BehaviorTypes.KnockOverCubes)
                    behavior.wait_for_started(timeout=10)
                    behavior.wait_for_completed(timeout=None)
                except asyncio.TimeoutError:
                    print('WARNING: Timeout exception raised from behavior type KnockOverCubes.')
                except:
                    print('WARNING: An exception was raised from behavior type KnockOverCubes.')
            else:
                robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed()
                return 1
        else:
            robot.drive_straight(distance_mm(-50),speed_mmps(80)).wait_for_completed()
            return 1
    if(i >= 4):
        return 0
async def desk_security_guard(robot):
    '''The core of the desk_security_guard program'''

    # Turn on image receiving by the camera
    robot.camera.image_stream_enabled = True

    # Connect Twitter, run async in the background
    twitter_api, twitter_auth = twitter_helpers.init_twitter(twitter_keys)
    stream_to_app_comms = TwitterStreamToAppCommunication()
    stream_listener = SecurityGuardStreamListener(twitter_api, stream_to_app_comms)
    twitter_stream = twitter_helpers.CozmoStream(twitter_auth, stream_listener)
    twitter_stream.async_userstream(_with='user')

    # Create our security guard
    dsg = DeskSecurityGuard(twitter_api)

    # Make sure Cozmo is clear of the charger
    if robot.is_on_charger:
        # Drive fully clear of charger (not just off the contacts)
        await robot.drive_off_charger_contacts().wait_for_completed()
        await robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()

    # Tilt head up to look for people
    await robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()

    initial_pose_angle = robot.pose_angle

    patrol_offset = 0  # middle
    max_pose_angle = 45  # offset from initial pose_angle (up to +45 or -45 from this)

    # Time to wait between each turn and patrol, in seconds
    time_between_turns = 2.5
    time_between_patrols = 20

    time_for_next_turn = time.time() + time_between_turns
    time_for_next_patrol = time.time() + time_between_patrols

    while True:

        # Handle any external requests to arm or disarm Cozmo
        if stream_to_app_comms.has_arm_request:
            stream_to_app_comms.has_arm_request = False
            if not dsg.is_armed:
                print("Alarm Armed")
                dsg.is_armed = True
        if stream_to_app_comms.has_disarm_request:
            stream_to_app_comms.has_disarm_request = False
            if dsg.is_armed:
                print("Alarm Disarmed")
                dsg.is_armed = False

        stream_to_app_comms.is_armed = dsg.is_armed

        # Turn head every few seconds to cover a wider field of view
        # Only do this if not currently investigating an intruder

        if (time.time() > time_for_next_turn) and not dsg.is_investigating_intruder():
            # pick a random amount to turn
            angle_to_turn = randint(10,40)

            # 50% chance of turning in either direction
            if randint(0,1) > 0:
                angle_to_turn = -angle_to_turn

            # Clamp the amount to turn

            face_angle = (robot.pose_angle - initial_pose_angle).degrees

            face_angle += angle_to_turn
            if face_angle > max_pose_angle:
                angle_to_turn -= (face_angle - max_pose_angle)
            elif face_angle < -max_pose_angle:
                angle_to_turn -= (face_angle + max_pose_angle)

            # Turn left/right
            await robot.turn_in_place(degrees(angle_to_turn)).wait_for_completed()

            # Tilt head up/down slightly
            await robot.set_head_angle(degrees(randint(30,44))).wait_for_completed()

            # Queue up the next time to look around
            time_for_next_turn = time.time() + time_between_turns

        # Every now and again patrol left and right between 3 patrol points

        if (time.time() > time_for_next_patrol) and not dsg.is_investigating_intruder():

            # Check which way robot is facing vs initial pose, pick a new patrol point

            face_angle = (robot.pose_angle - initial_pose_angle).degrees
            drive_right = (patrol_offset < 0) or ((patrol_offset == 0) and (face_angle > 0))

            # Turn to face the new patrol point

            if drive_right:
                await robot.turn_in_place(degrees(90 - face_angle)).wait_for_completed()
                patrol_offset += 1
            else:
                await robot.turn_in_place(degrees(-90 - face_angle)).wait_for_completed()
                patrol_offset -= 1

            # Drive to the patrol point, playing animations along the way

            await robot.drive_wheels(20, 20)
            for i in range(1,4):
                await robot.play_anim("anim_hiking_driving_loop_0" + str(i)).wait_for_completed()

            # Stop driving

            robot.stop_all_motors()

            # Turn to face forwards again

            face_angle = (robot.pose_angle - initial_pose_angle).degrees
            if face_angle > 0:
                await robot.turn_in_place(degrees(-90)).wait_for_completed()
            else:
                await robot.turn_in_place(degrees(90)).wait_for_completed()

            # Queue up the next time to patrol
            time_for_next_patrol = time.time() + time_between_patrols

        # look for intruders

        await check_for_intruder(robot, dsg)

        # Sleep to allow other things to run

        await asyncio.sleep(0.05)
Example #42
0
def clean_up_cubes():
    global cubeIDs,SIDE

    cubeIDs = []
    switch_cube_off(robot.world.get_light_cube(LightCube1Id))
    switch_cube_off(robot.world.get_light_cube(LightCube2Id))
    switch_cube_off(robot.world.get_light_cube(LightCube3Id))

    go_to_charger()
    turn_around()
    
    # I. Find first cube and put it next to charger
    cube = look_for_next_cube()
    if not cube:
    	return
    switch_cube_on(cube)
    print(cubeIDs)
    success = pickUp_cube(cube)
    if not success:
    	switch_cube_off(cube) 
    	return
    charger = go_to_charger()
    final_adjust(charger,60)
    robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
    robot.drive_straight(distance_mm(100),speed_mmps(40)).wait_for_completed()
    putDown_cube(cube)
    switch_cube_off(cube)
    # Get back in front of charger
    robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
    robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
    robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()

    # II. Find next cube and stack on first placed cube
    cube = look_for_next_cube()
    if(cube == None):
    	return
    switch_cube_on(cube)
    print(cubeIDs)
    success = pickUp_cube(cube)
    if not success:
    	switch_cube_off(cube)
    	return
    charger = go_to_charger()
    robot.turn_in_place(degrees(SIDE*70)).wait_for_completed() #SIDE*180
    stack_cube(cubes[0])
    switch_cube_off(cube)
    # Get back in front of charger
    #robot.drive_straight(distance_mm(-70),speed_mmps(40)).wait_for_completed()
    charger = go_to_charger()
    final_adjust(charger,100,80)
    robot.turn_in_place(degrees(SIDE*180)).wait_for_completed() #-SIDE*70
    robot.set_lift_height(height=0,max_speed=10,in_parallel=False).wait_for_completed()
    #robot.drive_straight(distance_mm(40),speed_mmps(40)).wait_for_completed()

    # III. Go get the last cube and lay it in front of the others
    cube = look_for_next_cube()
    if(cube == None):
    	return
    switch_cube_on(cube)
    print(cubeIDs)
    success = pickUp_cube(cube)
    if not success:
    	switch_cube_off(cube)
    	return
    turn_around()
    charger = go_to_charger()
    final_adjust(charger,80)
    robot.turn_in_place(degrees(SIDE*60)).wait_for_completed()
    robot.go_to_object(cubes[0], distance_mm(120), in_parallel=False, num_retries=1).wait_for_completed()
    putDown_cube(cube)
    switch_cube_off(cube)
    # Get back in front of charger
    robot.drive_straight(distance_mm(-40),speed_mmps(40)).wait_for_completed()
    robot.turn_in_place(degrees(SIDE*180-SIDE*60)).wait_for_completed()
    robot.drive_straight(distance_mm(50),speed_mmps(40)).wait_for_completed()
    turn_around()
    return