Last active
December 30, 2017 18:09
-
-
Save acidzebra/536ab30795dd5d9e3341e8d6026d1223 to your computer and use it in GitHub Desktop.
dev version of the cozmo_unleashed script. Rough edges ahead.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python3 | |
| # based on Copyright (c) 2016 Anki, Inc. | |
| # | |
| # Licensed under the Apache License, Version 2.0 (the "License"); | |
| # you may not use this file except in compliance with the License. | |
| # You may obtain a copy of the License in the file LICENSE.txt or at | |
| # | |
| # http://www.apache.org/licenses/LICENSE-2.0 | |
| # | |
| # Unless required by applicable law or agreed to in writing, software | |
| # distributed under the License is distributed on an "AS IS" BASIS, | |
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | |
| # See the License for the specific language governing permissions and | |
| # limitations under the License. | |
| # | |
| # | |
| # cozmo_unleashed | |
| # | |
| # Script based on the Cozmo SDK drive_to_charger example script | |
| # and raproenca's modifications of same. | |
| # rewritten to support battery/charge states and all kinds of bells and whistles by acid zebra | |
| # | |
| import sys | |
| import os | |
| import datetime | |
| import random | |
| import time | |
| import asyncio | |
| import cozmo | |
| from cozmo.util import degrees, distance_mm, speed_mmps, Pose | |
| from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes | |
| global freeplay | |
| global chargerequired | |
| freeplay=0 | |
| chargerequired=0 | |
| # main program and loops | |
| def cozmo_unleashed(robot: cozmo.robot.Robot): | |
| global freeplay | |
| global chargerequired | |
| robot.enable_facial_expression_estimation(enable=True) | |
| robot.world.disconnect_from_cubes() | |
| robot.enable_all_reaction_triggers(False) | |
| robot.enable_stop_on_cliff(True) | |
| robot.set_robot_volume(0.04) | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| while True: | |
| #State 1: on charger, charging - wait for full charge | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
| #reset charge required (I know we're still charging, trust me) | |
| chargerequired=0 | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charging, battery %s" % str(round(robot.battery_voltage, 2))) | |
| i = random.randint(1, 100) | |
| if i == 100: | |
| robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed() | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| elif i >= 85: | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| else: | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| time.sleep(5) | |
| time.sleep(5) | |
| #TODO: figure out how to smoothly cycle lights | |
| robot.set_all_backpack_lights(cozmo.lights.green_light) | |
| time.sleep(5) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| #State 2: on charger, fully charged - check schedule for go/no go | |
| if (robot.is_on_charger == 1) and (robot.is_charging == 0): | |
| #reset charge required | |
| chargerequired=0 | |
| # day and time check - are we okay to play at this time and day? | |
| day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday | |
| ctime = datetime.datetime.now().time() | |
| playokay=0 | |
| #it's weekend! Check for allowed times. | |
| if day_of_week > 4: | |
| if (ctime > datetime.time(9) and ctime < datetime.time(23)): | |
| playokay=1 | |
| #it's a weekday! Check for allowed times. | |
| else: | |
| if (ctime > datetime.time(7,30) and ctime < datetime.time(22)): | |
| playokay=1 | |
| # if the schedule says roll dice to see if we wake up (20 in 100 chance) | |
| if playokay==1: | |
| i = random.randint(1, 100) | |
| # 20 in 100 chance of waking up every 5 minutes | |
| if i >= 80: | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: leaving charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| robot.play_anim("anim_gotosleep_getout_02").wait_for_completed() | |
| robot.drive_off_charger_contacts().wait_for_completed() | |
| robot.drive_off_charger_contacts().wait_for_completed() | |
| robot.drive_off_charger_contacts().wait_for_completed() | |
| time.sleep(2) | |
| robot.move_lift(-3) | |
| robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed() | |
| robot.drive_straight(distance_mm(100), speed_mmps(90)).wait_for_completed() | |
| # we're out of schedule or didn't make the dice roll, back off for ~5 minutes and check again. | |
| x = 1 | |
| while x < 20 and (robot.is_on_charger == 1): | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| if playokay == 1: | |
| print("State: charged, schedule OK but not active, sleep loop %d of 30 before next check." % (x)) | |
| else: | |
| print("State: charged, not active by schedule, sleep loop %d of 30 before next check." % (x)) | |
| i = random.randint(1, 100) | |
| if i == 100: | |
| robot.play_anim("anim_guarddog_fakeout_02").wait_for_completed() | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| elif i >= 90: | |
| robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
| else: | |
| time.sleep(5) | |
| #robot.play_anim("anim_gotosleep_off_01").wait_for_completed() | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| time.sleep(5) | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| robot.set_all_backpack_lights(cozmo.lights.blue_light) | |
| time.sleep(5) | |
| if (robot.is_on_charger == 0): | |
| print("we were taken off charger") | |
| break | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| x += 1 | |
| #State 3: not on charger, good battery - freeplay | |
| if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0): | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
| #initiate freeplay | |
| if freeplay==0 and chargerequired==0: | |
| print("freeplay active") | |
| robot.world.connect_to_cubes() | |
| robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| freeplay=1 | |
| if chargerequired==0: | |
| time.sleep(5) | |
| #State 4: not on charger, low battery, or chargerequired flag is set - seek charger | |
| if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0) or (chargerequired==1) and (robot.is_on_charger == 0): | |
| #back off from whatever we were doing | |
| chargerequired=1 | |
| if freeplay==1: | |
| robot.abort_all_actions(log_abort_messages=False) | |
| robot.wait_for_all_actions_completed() | |
| robot.stop_freeplay_behaviors() | |
| robot.enable_all_reaction_triggers(False) | |
| robot.world.disconnect_from_cubes() | |
| freeplay=0 | |
| robot.play_anim_trigger(cozmo.anim.Triggers.NeedsMildLowEnergyRequest, ignore_body_track=True).wait_for_completed() | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| robot.move_lift(-3) | |
| robot.drive_straight(distance_mm(-30), speed_mmps(90)).wait_for_completed() | |
| robot.set_all_backpack_lights(cozmo.lights.blue_light) | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: finding charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| # charger location search | |
| charger = None | |
| # see if we already know where the charger is | |
| if robot.world.charger: | |
| if robot.world.charger.pose.origin_id == robot.pose.origin_id: | |
| #we know where the charger is | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger position known, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabSurprise, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| robot.move_lift(-3) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| charger = robot.world.charger | |
| else: | |
| # we know where the charger is but we have been delocalized | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: robot delocalized recently, battery %s" % str(round(robot.battery_voltage, 2))) | |
| pass | |
| # we don't know where the charger is | |
| if not charger: | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: looking for charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.SparkIdle, ignore_body_track=True).wait_for_completed() | |
| robot.move_lift(-3) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| # randomly drive around for a bit and see if we can spot the lightcubes or charger | |
| #TODO: better search AI and beter code | |
| loops=2 | |
| while loops>0: | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| for _ in range(11): | |
| robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1) | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| time.sleep(0.3) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| cube1 = robot.world.get_light_cube(cozmo.objects.LightCube1Id) | |
| if cube1: | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| print("going to cube1!") | |
| robot.go_to_object(cube1, distance_mm(80)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed() | |
| print("going to random point +/-100 in X+Y") | |
| x= random.randrange(-100, 101, 100) | |
| y= random.randrange(-100, 101, 100) | |
| robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() | |
| for _ in range(11): | |
| robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1) | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| time.sleep(0.3) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| cube2 = robot.world.get_light_cube(cozmo.objects.LightCube2Id) | |
| if cube2: | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| print("going to cube2!") | |
| robot.go_to_object(cube2, distance_mm(80)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed() | |
| print("going to random point +/-100 in X+Y") | |
| x= random.randrange(-100, 101, 100) | |
| y= random.randrange(-100, 101, 100) | |
| robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() | |
| for _ in range(11): | |
| robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1) | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| time.sleep(0.3) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| cube3 = robot.world.get_light_cube(cozmo.objects.LightCube3Id) | |
| if cube3: | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| print("going to cube3!") | |
| robot.go_to_object(cube3, distance_mm(80)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-70), speed_mmps(90)).wait_for_completed() | |
| print("going to random point +/-100 in X+Y") | |
| x= random.randrange(-100, 101, 100) | |
| y= random.randrange(-100, 101, 100) | |
| robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() | |
| for _ in range(11): | |
| robot.drive_wheels(40, -40, l_wheel_acc=150, r_wheel_acc=150, duration=1) | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| time.sleep(0.3) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| if robot.world.charger: | |
| loops=0 | |
| charger = robot.world.charger | |
| print("breaking charger loop as charger is known") | |
| break | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| loops=loops-1 | |
| print("locator loop complete.") | |
| time.sleep(1) | |
| # Charger location and docking handling here | |
| #TODO: improve this spaghetti code | |
| if charger: | |
| while (robot.is_on_charger == 0): | |
| robot.set_lift_height(0.8,0.8,0.8,0.1).wait_for_completed() | |
| # drive near to the charger, and then stop. | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: moving to charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabChatty, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| robot.move_lift(-3) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| action = robot.go_to_pose(charger.pose) | |
| action.wait_for_completed() | |
| robot.drive_straight(distance_mm(-85), speed_mmps(50)).wait_for_completed() | |
| action = robot.go_to_pose(charger.pose) | |
| action.wait_for_completed() | |
| robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
| # we should be right in front of the charger, can we see it? | |
| if (charger.is_visible==False): | |
| #No we can't see it. Remove charger from navigation map and quit this loop. | |
| robot.world.charger = None | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| print("State: charger not found, clearing map. battery %s" % str(round(robot.battery_voltage, 2))) | |
| break | |
| i = random.randint(1, 100) | |
| if i >= 85: | |
| robot.play_anim_trigger(cozmo.anim.Triggers.FeedingReactToShake_Normal, ignore_body_track=True, ignore_head_track=True).wait_for_completed() | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| # Dock. Turn around and drive backwards | |
| print("State: docking, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.turn_in_place(degrees(95)).wait_for_completed() | |
| robot.turn_in_place(degrees(95)).wait_for_completed() | |
| time.sleep( 1 ) | |
| robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceFake, ignore_body_track=True).wait_for_completed() | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| robot.drive_straight(distance_mm(-145), speed_mmps(150)).wait_for_completed() | |
| time.sleep( 1 ) | |
| # check if we're now docked | |
| if robot.is_on_charger: | |
| # Yes! we're docked! | |
| robot.play_anim("anim_sparking_success_02").wait_for_completed() | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: docked, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.set_all_backpack_lights(cozmo.lights.off_light) | |
| robot.play_anim("anim_gotosleep_getin_01").wait_for_completed() | |
| robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed() | |
| # No, we missed. Back off and try again | |
| else: | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: failed to dock with charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.world.charger = None | |
| robot.play_anim_trigger(cozmo.anim.Triggers.AskToBeRightedRight, ignore_body_track=True).wait_for_completed() | |
| robot.move_lift(-3) | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: backing off to attempt docking, battery %s" % str(round(robot.battery_voltage, 2))) | |
| robot.drive_straight(distance_mm(50), speed_mmps(90)).wait_for_completed() | |
| robot.turn_in_place(degrees(-3)).wait_for_completed() | |
| robot.drive_straight(distance_mm(150), speed_mmps(90)).wait_for_completed() | |
| robot.turn_in_place(degrees(95)).wait_for_completed() | |
| robot.turn_in_place(degrees(96)).wait_for_completed() | |
| robot.set_head_angle(degrees(0)).wait_for_completed() | |
| time.sleep( 1 ) | |
| else: | |
| # we have not managed to find the charger. Falling back to freeplay with occasional checks | |
| #robot.world.charger = None | |
| robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
| #robot.world.connect_to_cubes() | |
| robot.enable_all_reaction_triggers(True) | |
| robot.start_freeplay_behaviors() | |
| freeplay=1 | |
| x=0 | |
| while x<20: | |
| if not robot.world.charger: | |
| time.sleep( 5 ) | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: charger not found, falling back to freeplay for a bit, loop %d of 20." % x) | |
| x+=1 | |
| #after ~100 seconds or spotting the charger end freeplay | |
| robot.enable_all_reaction_triggers(False) | |
| robot.stop_freeplay_behaviors() | |
| freeplay=0 | |
| print("charger loop completed.") | |
| time.sleep(1) | |
| print("state cycle complete, looping.") | |
| time.sleep(1) | |
| os.system('cls' if os.name == 'nt' else 'clear') | |
| print("State: program loop complete (we should not ever reach this state), battery %s" % str(round(robot.battery_voltage, 2))) | |
| time.sleep( 3 ) | |
| cozmo.robot.Robot.drive_off_charger_on_connect = False | |
| #cozmo.run_program(cozmo_unleashed, use_viewer=True) | |
| # if you have freeglut in the same folder as this script you can change the above line to | |
| cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True) | |
| # which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment