Last active
November 13, 2020 03:10
-
-
Save acidzebra/85eb266a96fb6b42050ba932d6355898 to your computer and use it in GitHub Desktop.
Autonomy for Cozmo, a script for having Cozmo freeplay until the battery runs low and then find/dock with the charger, with crude scheduler for weekdays/weekend and allowed play times. While in "allowed" play times, Cozmo has a 20% every 5 minutes of waking up and playing. Will attempt to find dock and charge when battery runs low.
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 | |
freeplay=0 | |
# main program and loops | |
def cozmo_unleashed(robot: cozmo.robot.Robot): | |
global freeplay | |
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 | |
if (robot.is_on_charger == 1) and (robot.is_charging == 1): | |
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() | |
robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
elif i >= 85: | |
robot.play_anim("anim_gotosleep_sleeploop_01").wait_for_completed() | |
else: | |
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 | |
if (robot.is_on_charger == 1) and (robot.is_charging == 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(19) 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() | |
for _ in range(3): | |
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(50)).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): | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: we were taken off the charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
time.sleep(5) | |
if (robot.is_on_charger == 0): | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: we were taken off the charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
robot.set_all_backpack_lights(cozmo.lights.blue_light) | |
time.sleep(5) | |
if (robot.is_on_charger == 0): | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: we were taken off the charger, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
robot.set_all_backpack_lights(cozmo.lights.off_light) | |
x += 1 | |
#State 3: not on charger, low battery | |
if (robot.battery_voltage <= 3.7) and (robot.is_on_charger == 0): | |
#back off from whatever we were doing | |
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(50)).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: | |
cube1 = None | |
cube2 = None | |
cube3 = None | |
if robot.world.charger: | |
loops=0 | |
charger = robot.world.charger | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
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 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
time.sleep(0.5) | |
robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
cube1 = robot.world.get_light_cube(cozmo.objects.LightCube1Id) | |
if cube1: | |
if robot.world.charger: | |
loops=0 | |
charger = robot.world.charger | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to lightcube1, battery %s" % str(round(robot.battery_voltage, 2))) | |
robot.go_to_object(cube1, distance_mm(80)).wait_for_completed() | |
robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed() | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to random point +/-100xy of cube, battery %s" % str(round(robot.battery_voltage, 2))) | |
x= random.randrange(-100, 101, 200) | |
y= random.randrange(-100, 101, 200) | |
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 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
time.sleep(0.5) | |
robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
cube2 = robot.world.get_light_cube(cozmo.objects.LightCube2Id) | |
if cube2: | |
if robot.world.charger: | |
loops=0 | |
charger = robot.world.charger | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to lightcube2, battery %s" % str(round(robot.battery_voltage, 2))) | |
robot.go_to_object(cube2, distance_mm(80)).wait_for_completed() | |
robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed() | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to random point +/-100xy of cube, battery %s" % str(round(robot.battery_voltage, 2))) | |
x= random.randrange(-100, 101, 200) | |
y= random.randrange(-100, 101, 200) | |
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 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
time.sleep(0.5) | |
robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
cube3 = robot.world.get_light_cube(cozmo.objects.LightCube3Id) | |
if cube3: | |
if robot.world.charger: | |
loops=0 | |
charger = robot.world.charger | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to lightcube1, battery %s" % str(round(robot.battery_voltage, 2))) | |
robot.go_to_object(cube3, distance_mm(80)).wait_for_completed() | |
robot.drive_straight(distance_mm(-70), speed_mmps(50)).wait_for_completed() | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: traveling to random point +/-100xy of cube, battery %s" % str(round(robot.battery_voltage, 2))) | |
x= random.randrange(-100, 101, 200) | |
y= random.randrange(-100, 101, 200) | |
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 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: breaking charger loop as charger is known, battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
time.sleep(0.5) | |
robot.play_anim_trigger(cozmo.anim.Triggers.HikingInterestingEdgeThought, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed() | |
loops=loops-1 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: locator loop complete. battery %s" % str(round(robot.battery_voltage, 2))) | |
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() | |
# get a little distance and have a look | |
robot.drive_straight(distance_mm(-85), 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() | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: charger not found, clearing map. battery %s" % str(round(robot.battery_voltage, 2))) | |
break | |
# okay it's there, attempt to dock. | |
action = robot.go_to_pose(charger.pose) | |
action.wait_for_completed() | |
robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed() | |
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') | |
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(50)).wait_for_completed() | |
robot.turn_in_place(degrees(-3)).wait_for_completed() | |
robot.drive_straight(distance_mm(150), speed_mmps(50)).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 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: charger program loop complete, battery %s" % str(round(robot.battery_voltage, 2))) | |
time.sleep(1) | |
#State 4: not on charger, good battery | |
if (robot.battery_voltage > 3.7) and (robot.is_on_charger == 0): | |
#initiate freeplay | |
if freeplay==0: | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: freeplay activating, battery %s" % str(round(robot.battery_voltage, 2))) | |
robot.world.connect_to_cubes() | |
robot.enable_all_reaction_triggers(True) | |
robot.start_freeplay_behaviors() | |
freeplay=1 | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: freeplay, battery %s" % str(round(robot.battery_voltage, 2))) | |
time.sleep(10) | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: state program loop complete, battery %s" % str(round(robot.battery_voltage, 2))) | |
time.sleep(1) | |
os.system('cls' if os.name == 'nt' else 'clear') | |
print("State: main 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