def cozmo_program(robot: cozmo.robot.Robot):
from cozmo.util import degrees, Pose, distance_mm, speed_mmps
from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id
cube1 = robot.world.get_light_cube(LightCube1Id)
cube2 = robot.world.get_light_cube(LightCube2Id)
cube3 = robot.world.get_light_cube(LightCube3Id)
lookaround = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
robot.world.wait_until_observe_num_objects(
num=3, object_type=cozmo.objects.LightCube, timeout=60)
lookaround.stop()
robot.go_to_object(cube2, distance_mm(120)).wait_for_completed()
robot.roll_cube(cube2, check_for_object_on_top=True,
num_retries=9).wait_for_completed()
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.")
def cozmo_program(robot: cozmo.robot.Robot):
print("--------------------------")
print("Battery (below 3.5V is low)")
print(robot.world.robot.battery_voltage)
print("--------------------------")
new_color = cozmo.lights.Color(rgb=(0, 255, 0))
green = cozmo.lights.Light(on_color=new_color)
cubes = [
robot.world.light_cubes.get(cozmo.objects.LightCube1Id),
robot.world.light_cubes.get(cozmo.objects.LightCube2Id),
robot.world.light_cubes.get(cozmo.objects.LightCube3Id)
]
for cube in cubes:
cube.set_lights(green)
time.sleep(1)
robot.world.add_event_handler(cozmo.objects.EvtObjectTapped, tap_handler)
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.say_text("Where are my cubes?").wait_for_completed()
look = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube_vision = robot.world.wait_until_observe_num_objects(
num=3,
object_type=cozmo.objects.LightCube,
timeout=30,
include_existing=True)
look.stop()
print("Cubes found")
robot.say_text("My favourite color is green!").wait_for_completed()
global id_cube
while True:
time.sleep(1)
if id_cube:
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession
).wait_for_completed()
print("Going to cube ", id_cube)
for mycube in cube_vision:
if mycube.object_id == id_cube:
robot.go_to_object(mycube,
distance_mm(60)).wait_for_completed()
robot.play_anim(name="ID_pokedB").wait_for_completed()
mycube.set_lights(green)
id_cube = None
def drive_to_charger(robot: cozmo.robot.Robot):
robot.say_text(
"You're welcome. I'm done. Now I am going to take a nap. Bye bye."
).wait_for_completed()
'''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.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:
# 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.")
def follow_qrs(robot: cozmo.robot.Robot):
'''The core of the follow_faces program'''
# Move lift down and tilt the head up
robot.move_lift(-3)
robot.set_head_angle(cozmo.robot.util.degrees(0)).wait_for_completed()
code_to_follow = None
print("Press CTRL-C to quit")
while True:
if code_to_follow:
#turn = random.choice(-90, 90)
robot.say_text("I found a cube!").wait_for_completed()
# start turning towards the face
robot.go_to_object(code_to_follow, cozmo.robot.util.distance_mm(50)).wait_for_completed()
robot.say_text("I made it to the code").wait_for_completed()
if code_to_follow.cube_id is 1:
robot.say_text("This is Cube 1").wait_for_completed()
robot.turn_in_place(cozmo.robot.util.Angle(degrees=90)).wait_for_completed()
robot.say_text("I'm turning").wait_for_completed()
if code_to_follow.cube_id is 2:
robot.say_text("This is Cube 2").wait_for_completed()
robot.turn_in_place(cozmo.robot.util.Angle(degrees=-90)).wait_for_completed()
robot.say_text("I'm turning").wait_for_completed()
if code_to_follow.cube_id is 3:
robot.say_text("This is Cube 3").wait_for_completed()
robot.turn_in_place(cozmo.robot.util.Angle(degrees=180)).wait_for_completed()
robot.say_text("I'm turning").wait_for_completed()
robot.set_head_angle(cozmo.robot.util.degrees(0)).wait_for_completed()
code_to_follow = None
if not (code_to_follow and code_to_follow.is_visible):
# find a visible face, timeout if nothing found after a short while
try:
code_to_follow = robot.world.wait_for_observed_light_cube(timeout=None)
except asyncio.TimeoutError:
print("I fail")
time.sleep(.1)
async def explore_sides(robot: cozmo.robot.Robot, found_cubes):
await robot.turn_in_place(degrees(-45)).wait_for_completed()
await robot.drive_straight(distance_mm(140), speed_mmps(200), False, False,
0).wait_for_completed()
await robot.turn_in_place(degrees(75)).wait_for_completed()
go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(100.0))
await go_to_cube.wait_for_completed()
# await robot.drive_straight(distance_mm(140), speed_mmps(100), False, False, 0).wait_for_completed()
await robot.drive_wheels(-200,
-200,
l_wheel_acc=4000,
r_wheel_acc=4000,
duration=1)
def cozmo_program(robot: cozmo.robot.Robot):
robot.world.image_annotator.add_static_text('text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT)
robot.world.image_annotator.add_annotator('clock', clock)
robot.world.image_annotator.add_annotator('battery', Battery)
lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=None)
cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip
cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart
cube3 = robot.world.get_light_cube(LightCube3Id)
lookaround.stop()
robot.pickup_object(cube3).wait_for_completed()
d = cozmo.util.Distance(distance_mm = 40)
robot.place_on_object(cube1).wait_for_completed()
robot.set_lift_height(height = 0.0, max_speed = 5).wait_for_completed()
robot.go_to_object(cube1,d).wait_for_completed()
robot.set_lift_height(height = 1.0, max_speed = 5).wait_for_completed()
d2 = cozmo.util.Distance(distance_inches=-2.0)
d3= cozmo.util.Distance(distance_inches=2.0)
d4 = cozmo.util.Distance(distance_inches = 3.0)
s = cozmo.util.Speed(speed_mmps = 10.0)
robot.drive_straight(d2,s, False).wait_for_completed()
robot.drive_wheels(10.0, -10.0, duration = 5.0)
robot.drive_straight(d4,s,False).wait_for_completed()
robot.drive_wheels(-10.0, 10.0, duration = 5.0)
robot.drive_straight(d3,s,False).wait_for_completed()
robot.set_lift_height (height = 0.0).wait_for_completed()
# Shutdown the program after 200 seconds
time.sleep(200)
async def cozmo_program(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)
# Connect to the pre-defined cubes
await robot.world.connect_to_cubes()
# Make sure Cozmo's head and arm are at reasonable levels
await robot.set_head_angle(degrees(5.0)).wait_for_completed()
await robot.set_lift_height(0.0).wait_for_completed()
# Searches for the cubes for a defined time in seconds
search_cubes = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
found_cubes = await robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.LightCube, timeout=6)
search_cubes.stop()
found_cubes[0] = robot.world.get_light_cube(LightCube2Id)
# Approach the object (pre-defined function used since its a defined object)
go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(100.0))
await go_to_cube.wait_for_completed()
# Code for objects that weren't pre-defined (we drive manually)
# await robot.drive_straight(distance_mm(140), speed_mmps(100), False, False, 0).wait_for_completed()
await robot.drive_wheels(-200,
-200,
l_wheel_acc=4000,
r_wheel_acc=4000,
duration=1)
# Explore all sides of the object
for sides in range(15):
await explore_sides(robot, found_cubes)
await robot.say_text("Exploration complete!").wait_for_completed()
# Disconnects from the cubes
robot.world.disconnect_from_cubes()
def cozmo_program(robot: cozmo.robot.Robot):
state = "findcube1"
cubefound = robot.world.get_light_cube(3)
while True:
if state == "findcube1" and cubefound.cube_id != cozmo.objects.LightCubeIDs[0]:
robot.drive_wheels(40, 10)
cubefound = robot.world.wait_for_observed_light_cube()
elif state == "findcube1" and cubefound.cube_id == cozmo.objects.LightCubeIDs[0]:
state = "gotocube1"
robot.stop_all_motors()
elif state == "gotocube1":
skip_check = random.randrange(1, 3)
if skip_check == 1:
state = "findcube2"
else:
state = "findcube3"
robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed()
robot.set_head_angle(degrees(0))
elif state == "findcube2" and cubefound.cube_id != cozmo.objects.LightCubeIDs[1]:
robot.drive_wheels(40, 10)
cubefound = robot.world.wait_for_observed_light_cube()
elif state == "findcube2" and cubefound.cube_id == cozmo.objects.LightCubeIDs[1]:
state = "gotocube2"
robot.stop_all_motors()
elif state == "gotocube2":
state = "findcube3"
robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed()
robot.set_head_angle(degrees(0))
elif state == "findcube3" and cubefound.cube_id != cozmo.objects.LightCubeIDs[2]:
robot.drive_wheels(40, 10)
cubefound = robot.world.wait_for_observed_light_cube()
elif state == "findcube3" and cubefound.cube_id == cozmo.objects.LightCubeIDs[2]:
state = "gotocube3"
robot.stop_all_motors()
elif state == "gotocube3":
state = "findcube1"
robot.go_to_object(cubefound, distance_mm(80)).wait_for_completed()
robot.set_head_angle(degrees(0))
def cozmo_tap_game(robot: cozmo.robot.Robot):
# Initialize all the game engines screens and listners
speed_tap_game = SpeedTapEngine(robot)
robot_game_action = CozmoPlayerActions()
display_screen = Screen()
display_screen.setup(robot_game_action.score_plan,
singleScreen=robot_game_action.singleScreen)
game_screen = display_screen.gameScreen
#display_screen.start()
display_screen.root.mainloop(1)
game_screen.show_play_screen(0, 0)
time.sleep(0.25) # sleep to give the cozmo cube to stop flashing
robot_cube, player_coop_cube, player_defect_cube = speed_tap_game.cozmo_setup_game(
robot_game_action.score_plan)
if robot_cube in [player_coop_cube, player_defect_cube]:
print("Participant cannot play on the same cube as cozmo")
game_screen.master.destroy()
exit(0)
monitor_player_tap = Human_Listener(robot, player_coop_cube,
player_defect_cube, speed_tap_game)
# initialise variables
correctChoice = -1 # Correct choice is for practice round
track_correct_practice = 0
game_complete = False
winner = 0
score_to = 5
pass_criteria = 8 # Only applies for practice rounds
goal_statement = ""
preset_goals = [P_R, P_O, O_R, O_O]
cozmo_fixture = COZMO_CHOICE[randint(0, 2)]
# Now all decided so lets suffle it up
shuffle(cozmo_fixture)
monitor_player_tap.game_on = True
monitor_player_tap.start()
deal_count = 1
if robot_game_action.practice:
cozmo.logger.info("PD : Playing practice round")
else:
cozmo.logger.info("PD : Playing experiment round")
cozmo.logger.info("PD : Cozmo initial plan fixture")
log_deal_plan(cozmo_fixture)
cozmo.logger.info("PD : Score set: %s" % robot_game_action.score_plan)
try:
while deal_count <= robot_game_action.rounds_to_play:
#print("%s" % cozmo_fixture)
cozmo.logger.info("PD : Deal started")
if robot_game_action.practice:
if track_correct_practice % 4 == 0:
shuffle(preset_goals)
#cozmo.logger.info("PD : Preset Goals :%s" % preset_goals)
correctChoice = preset_goals[(track_correct_practice - 1) % 4]
cozmo.logger.info(
"PD : Practice Goal : %s" %
robot_game_action.goals_statment_list[correctChoice])
goal_statement = robot_game_action.goals_statment_list[
correctChoice]
game_screen.show_play_screen(speed_tap_game.player_score,
speed_tap_game.robot_score)
game_screen.show_goal_statement(goal_statement)
# Cozmo get in ready position
robot_game_action.act_out(robot, "wait")
# Deal the hand
speed_tap_game.deal_hand()
cozmo.logger.info("PD : Hand delt : %s" % deal_count)
#print("Pre: %s" % datetime.now())
monitor_player_tap.listen = True
if robot_game_action.practice:
cozmo_goal = correctChoice
else:
cozmo_goal = cozmo_fixture[deal_count - 1]
# Get Cozmo to decide whether it is going to tap
tapped = robot_game_action.cozmo_tap_decision(
robot, speed_tap_game, cozmo_goal)
# If player has tapped it would be registered by now
monitor_player_tap.listen = False
speed_tap_game.deactivate_current_deal()
cozmo.logger.info("PD : Hand deactivated : %s" % deal_count)
speed_tap_game.score_last_deal()
result = speed_tap_game.result_track[-1]
cozmo.logger.info("PD : Result : %s" % RESULT_STATEMENT[result])
game_screen.show_play_screen(speed_tap_game.player_score,
speed_tap_game.robot_score)
cozmo.logger.info("PD : After hand %s player score : %s" %
(deal_count, speed_tap_game.player_score))
cozmo.logger.info("PD : After hand %s cozmo score : %s" %
(deal_count, speed_tap_game.robot_score))
game_screen.show_selection(result, correctChoice)
if robot_game_action.practice:
game_screen.show_goal_statement(goal_statement)
else:
game_screen.show_goal_statement("")
if robot_game_action.practice:
# We need to track practice round so that
# we know the player has correctly understood the game
if result == correctChoice:
track_correct_practice += 1
cozmo.logger.info("PD : CORRECT for %s times" %
track_correct_practice)
else:
cozmo.logger.info(
"PD : INCORRECT choice. Chances left: %s " %
(robot_game_action.rounds_to_play - deal_count - 1))
# One wrong implies all wrong
track_correct_practice = 0
# We are not tracking scores across games for
# practice so reset deal
speed_tap_game.reset_deals()
deal_count += 1
#time.sleep(2)
# Cozmo check out score
robot_game_action.act_out(robot, "stand_back")
robot_game_action.act_out(robot, "check_score")
robot_cube.stop_light_chaser()
player_coop_cube.stop_light_chaser()
player_defect_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_coop_cube.set_lights_off()
player_defect_cube.set_lights_off()
if robot_game_action.practice and track_correct_practice >= pass_criteria:
cozmo.logger.info("PD : Practice Passed")
print("PRACTICE PASSED")
break
elif not robot_game_action.practice and result == X_X:
if robot_game_action.rounds_to_play >= 35:
cozmo.logger.info(
"PD : Over 15 rounds of missing data. We will stop.")
break
else:
cozmo.logger.info(
"PD : Rounds incremented to compensate for missing data"
)
robot_game_action.rounds_to_play += 1
# We missed a even paced tap/no tap decision by cozmo so append it
# at the end to maintain balance
cozmo_fixture.append(cozmo_goal)
cozmo_fixture[deal_count - 2] = -1
cozmo.logger.info("PD : Updated cozmo plan")
log_deal_plan(cozmo_fixture)
if deal_count <= robot_game_action.rounds_to_play:
robot.move_lift(3)
if robot_game_action.practice or deal_count % 5 == 0:
#robot.drive_wheels(-50, -50, duration=0.5)
robot.go_to_object(robot_cube,
distance_mm(35.0)).wait_for_completed()
else:
robot.drive_wheels(100, 100, duration=1.5)
time.sleep(1.5)
cozmo.logger.info("PD : Ready for next deal")
# clear up games to show result
robot_cube.stop_light_chaser()
player_coop_cube.stop_light_chaser()
player_defect_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_coop_cube.set_lights_off()
player_defect_cube.set_lights_off()
cozmo.logger.info("PD : Done playing")
robot_game_action.act_out(robot, "stand_back")
time.sleep(2)
robot.go_to_object(robot_cube, distance_mm(60.0)).wait_for_completed()
#display_screen.root.mainloop()
if robot_game_action.practice and deal_count >= robot_game_action.rounds_to_play:
cozmo.logger.info("PD : Practice Failed")
print("PRACTICE FAILED")
finally:
monitor_player_tap.game_on = False
robot_cube.stop_light_chaser()
player_coop_cube.stop_light_chaser()
player_defect_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_coop_cube.set_lights_off()
player_defect_cube.set_lights_off()
monitor_player_tap.join()
del speed_tap_game
del player_coop_cube
del player_defect_cube
del robot_cube
game_screen.master.destroy()
def cozmo_program(robot: cozmo.robot.Robot):
from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id
from cozmo.util import degrees, Pose, distance_mm, speed_mmps
robot.set_head_angle(degrees(0.0)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
cube1 = robot.world.get_light_cube(LightCube1Id)
cube2 = robot.world.get_light_cube(LightCube2Id)
cube3 = robot.world.get_light_cube(LightCube3Id)
robot.set_all_backpack_lights(cozmo.lights.white_light)
cube1.set_lights(cozmo.lights.white_light)
cube2.set_lights(cozmo.lights.white_light)
cube3.set_lights(cozmo.lights.white_light)
lookaround = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
robot.world.wait_until_observe_num_objects(
num=3, object_type=cozmo.objects.LightCube, timeout=60)
lookaround.stop()
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube1.set_lights(cozmo.lights.blue_light)
cube2.set_lights(cozmo.lights.blue_light)
cube3.set_lights(cozmo.lights.blue_light)
robot.pickup_object(cube1,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.green_light.flash())
cube1.set_lights(cozmo.lights.green_light.flash())
robot.say_text(Cozmo_Says1,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.place_on_object(cube2,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube1.set_lights(cozmo.lights.green_light)
cube2.set_lights(cozmo.lights.green_light)
robot.say_text(Cozmo_Says2,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.pickup_object(cube3,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.green_light.flash())
cube3.set_lights(cozmo.lights.green_light.flash())
robot.say_text(Cozmo_Says3,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.place_on_object(cube2,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube3.set_lights(cozmo.lights.green_light)
robot.place_object_on_ground_here(cube3,
num_retries=5).wait_for_completed()
robot.go_to_object(cube3, distance_mm(3.0)).wait_for_completed()
robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed()
robot.say_text(Cozmo_Says4,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.pickup_object(cube1,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.red_light.flash())
cube1.set_lights(cozmo.lights.red_light.flash())
cube2.set_lights(cozmo.lights.red_light)
robot.say_text(Cozmo_Says4,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.place_on_object(cube3,
use_pre_dock_pose=True,
in_parallel=True,
num_retries=5).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.blue_light)
cube1.set_lights(cozmo.lights.red_light)
cube3.set_lights(cozmo.lights.red_light)
robot.say_text(Cozmo_Says5,
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.white_light)
cube1.set_lights(cozmo.lights.white_light)
cube2.set_lights(cozmo.lights.white_light)
cube3.set_lights(cozmo.lights.white_light)
robot.say_text(Cozmo_Says6,
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=1.0,
voice_pitch=0.0).wait_for_completed()
robot.play_anim(name=Cozmo_Says7).wait_for_completed()
robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed()
anim = robot.play_anim_trigger(cozmo.anim.Triggers.AudioTestAnim)
exec(Redund_Code)
async def cozmo_program(robot: cozmo.robot.Robot):
#connects to the three pre-defined cubes
await robot.world.connect_to_cubes()
#(assuming starting point is the charger) moves off the charger then turns towards cubes
await robot.drive_straight(distance_mm(100),
speed_mmps(250)).wait_for_completed()
await robot.turn_in_place(degrees(45)).wait_for_completed()
#searches for three cubes for 10 seconds
search_cubes = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
found_cubes = await robot.world.wait_until_observe_num_objects(
num=3, object_type=cozmo.objects.LightCube, timeout=10)
search_cubes.stop()
#checks to see if all three cubes are found - if they are, then we assign cubes
if len(found_cubes) < 3:
print("Need 3 cubes but found", len(found_cubes))
sys.exit
else:
found_cubes[0] = robot.world.get_light_cube(LightCube1Id)
found_cubes[1] = robot.world.get_light_cube(LightCube2Id)
found_cubes[2] = robot.world.get_light_cube(LightCube3Id)
#lights up cubes in custom colors
if found_cubes[0] is not None:
found_cubes[0].set_light_corners(cozmo.lights.red_light,
cozmo.lights.green_light,
cozmo.lights.red_light,
cozmo.lights.green_light)
else:
cozmo.logger.warning("Not connected to cube 1!")
if found_cubes[1] is not None:
found_cubes[1].set_light_corners(cozmo.lights.white_light,
cozmo.lights.blue_light,
cozmo.lights.white_light,
cozmo.lights.blue_light)
else:
cozmo.logger.warning("Not connected to cube 2!")
if found_cubes[2] is not None:
found_cubes[2].set_lights(cozmo.lights.red_light)
else:
cozmo.logger.warning("Not connected to cube 3!")
#goes to first cube
go_to_cube = robot.go_to_object(found_cubes[0], distance_mm(70.0))
await go_to_cube.wait_for_completed()
#picks up the first cube within three tries - if it fails then a message is printed
pick_up_cube = robot.pickup_object(found_cubes[0], num_retries=3)
await pick_up_cube.wait_for_completed()
if pick_up_cube.has_failed:
code, reason = pick_up_cube.failure_reason
result = pick_up_cube.result
print("Picking up cube failed: code=%s reason='%s' result=%s" %
(code, reason, result))
return
#places the first cube on top of second cube once it reaches it - if it fails then a message is printed
place_cube = robot.place_on_object(found_cubes[1], num_retries=3)
await place_cube.wait_for_completed()
if place_cube.has_failed:
code, reason = place_cube.failure_reason
result = place_cube.result
print("Placing cube has failed: code=%s reason='%s' result=%s" %
(code, reason, result))
return
#does a wheelie on the third object once it reaches it - if it fails then a message is printed
do_a_wheelie = robot.pop_a_wheelie(found_cubes[2], num_retries=2)
await do_a_wheelie.wait_for_completed()
if do_a_wheelie.has_failed:
code, reason = do_a_wheelie.failure_reason
result = do_a_wheelie.result
print("Doing a wheelie has failed: code=%s reason='%s' result=%s" %
(code, reason, result))
return
#drives backwards to get back into its regular position from the wheelie position
await robot.drive_wheels(-200,
-200,
l_wheel_acc=4000,
r_wheel_acc=4000,
duration=0.6)
#plays a victory dance to celebrate sucessful completeion
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabVictory
).wait_for_completed()
await robot.say_text("I did it!").wait_for_completed()
#disconnects all three cubes
robot.world.disconnect_from_cubes()
def cozmo_idle(robot: cozmo.robot.Robot):
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
img_clf = ImageClassifier()
(train_raw, train_labels) = img_clf.load_data_from_folder('./train/')
train_data = img_clf.extract_image_features(train_raw)
img_clf.train_classifier(train_data, train_labels)
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
while True:
time.sleep(1.0)
latest_im = robot.world.latest_image
new_im = np.asarray(latest_im.raw_image, dtype="uint8")
f = feature.hog(new_im,
orientations=10,
pixels_per_cell=(48, 48),
cells_per_block=(4, 4),
feature_vector=True,
block_norm='L2-Hys')
type = img_clf.predict_labels([f])[10]
if type == "drone":
robot.say_text(type).wait_for_completed()
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=20)
except asyncio.TimeoutError:
print("There is no cube")
finally:
look_around.stop()
if cube is not None:
robot.go_to_object(cube, distance_mm(40)).wait_for_completed()
robot.pickup_object(cube, num_retries=2).wait_for_completed()
robot.drive_straight(distance_mm(100),
speed_mmps(55)).wait_for_completed()
robot.place_object_on_ground_here(cube).wait_for_completed()
robot.drive_straight(distance_mm(-100),
speed_mmps(55)).wait_for_completed()
elif type == "order":
robot.say_text(type).wait_for_completed()
robot.drive_wheels(50, 23.5, duration=21.55)
elif type == "inspection":
robot.say_text(type).wait_for_completed()
for times in range(4):
robot.set_lift_height(1, duration=2, in_parallel=True)
robot.drive_straight(distance_mm(200),
speed_mmps(50),
in_parallel=True).wait_for_completed()
robot.set_lift_height(0, duration=2,
in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
elif type == "plane":
robot.say_text(type).wait_for_completed()
elif type == "truck":
robot.say_text(type).wait_for_completed()
elif type == "hands":
robot.say_text(type).wait_for_completed()
elif type == "place":
robot.say_text(type).wait_for_completed()
time.sleep(1)
def cozmo_program(robot: cozmo.robot.Robot):
success = True
#see what Cozmo sees
robot.camera.image_stream_enabled = True
#connect to cubes (in case Cozmo was disconnected from the cubes)
robot.world.connect_to_cubes()
#identify cubes
cube1 = robot.world.get_light_cube(LightCube1Id) # looks like a paperclip
cube2 = robot.world.get_light_cube(LightCube2Id) # looks like a lamp / heart
cube3 = robot.world.get_light_cube(LightCube3Id) # looks like the letters 'ab' over 'T'
if cube1 is not None:
cube1.set_lights(cozmo.lights.red_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")
if cube2 is not None:
cube2.set_lights(cozmo.lights.green_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.")
if cube3 is not None:
cube3.set_lights(cozmo.lights.blue_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube3Id cube - check the battery.")
#have the user tap each of the cubes, in order
try:
robot.say_text("Tap the red cube to make me move.").wait_for_completed()
cube1.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The red cube was not tapped").wait_for_completed()
success = False
finally:
cube1.set_lights_off()
if (success):
robot.go_to_object(cube1, distance_mm(20.0)).wait_for_completed()
robot.drive_straight(distance_mm(-100.0), speed_mmps(150)).wait_for_completed()
else:
robot.say_text("You didn't tap the cube properly.").wait_for_completed()
success = True
try:
robot.say_text("Tap the green cube so I can take a picture.").wait_for_completed()
cube2.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The green cube was not tapped").wait_for_completed()
success = False
finally:
cube2.set_lights_off()
if (success):
robot.say_text("I see that you were paying attention. I will take a picture.").wait_for_completed()
else:
robot.say_text("Do you know how to tap a cube? I will take a picture anyway.").wait_for_completed()
success = True
new_im = robot.world.wait_for(cozmo.world.EvtNewCameraImage)
new_im.image.raw_image.show()
#save the raw image as a bmp file
img_latest = robot.world.latest_image.raw_image
img_convert = img_latest.convert('L')
img_convert.save("aPhoto.bmp")
#save the raw image data as a png file, named imageName
imageName = "myPhoto.png"
img = Image.open("aPhoto.bmp")
width, height = img.size
new_img = img.resize( (width, height) )
new_img.save( imageName, 'png')
try:
robot.say_text("Tap the blue cube and make me do something.").wait_for_completed()
cube3.wait_for_tap(timeout=10)
except asyncio.TimeoutError:
robot.say_text("The blue cube was not tapped").wait_for_completed()
success = False
finally:
cube3.set_lights_off()
cube = robot.world.wait_for_observed_light_cube()
if (success):
robot.play_anim("anim_cozmosays_badword_01").wait_for_completed()
else:
robot.say_text("Do you know how to tap a cube? I will pop a wheelie.").wait_for_completed()
success = True
action = robot.pop_a_wheelie(cube, num_retries=2)
action.wait_for_completed()
robot.say_text("I treat my body right. Help me.").wait_for_completed()
return
def cozmo_program(robot: cozmo.robot.Robot):
# create an origin point where Cozmo's charger is. When he picks up objects he will return here.
# If the robot was on the charger, drive them forward and clear of the charger
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.move_lift(-3)
robot.turn_in_place(degrees(180)).wait_for_completed()
robot.set_head_angle(degrees(0)).wait_for_completed()
time.sleep(0.5)
# try to find the charger
charger = None
if robot.world.charger:
if robot.world.charger.pose.is_comparable(robot.pose):
# Cozmo knows where the charger is
charger = robot.world.charger
else:
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)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
look_around.stop()
origin = charger
robot.go_to_object(origin, 70)
# on boot up show loading screen
# locate all cubes
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
look_around.stop()
# define light colours
red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0)))
blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255)))
yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0)))
# tag each cube found as a different colour
red_cube = robot.world.get_light_cube(LightCube1Id)
blue_cube = robot.world.get_light_cube(LightCube2Id)
yellow_cube = robot.world.get_light_cube(LightCube3Id)
red_cube.set_lights(red_light)
blue_cube.set_lights(blue_light)
yellow_cube.set_lights(yellow__light)
# Pass found objects to GUI for selection
robot.say_text("Ready when you are").wait_for_completed()
# user selects which cube they want
robot.say_text("When you are sure that's the one you want. Press the tick, if you want to select another, "
"just press on another object").wait_for_completed()
# Wait for conformation
robot.say_text("OK, I'll be right back.").wait_for_completed()
# cozmo goes and gets cube
action = robot.go_to_object(red_cube, distance_mm(70.0))
action.wait_for_completed()
action = robot.dock_with_cube(red_cube, approach_angle=cozmo.util.degrees(0), num_retries=2)
action.wait_for_completed()
action = robot.pickup_object(red_cube, num_retries=3)
action.wait_for_completed()
robot.say_text("Got it").wait_for_completed()
# Cozmo returns cube to user
action = robot.go_to_object(origin, 70)
action.wait_for_completed()
robot.say_text("Is this one the right one?").wait_for_completed()
# Object is confirmed
robot.say_text("Yay")
# get dat fist bump
robot.say_text("Do you want me to fetch anything else")
# user says no
# cozmo returns to base
# wait for five minutes of inactivity
robot.say_text("I'm going to have a nap now, let me know if you need anything?")
def cozmo_program(robot: cozmo.robot.Robot):
# create an origin point where Cozmo's charger is. When he picks up objects he will return here.
# If the robot was on the charger, drive them forward and clear of the charger
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.move_lift(-3)
robot.turn_in_place(degrees(180)).wait_for_completed()
robot.set_head_angle(degrees(0)).wait_for_completed()
time.sleep(0.5)
# try to find the charger
charger = None
if robot.world.charger:
if robot.world.charger.pose.is_comparable(robot.pose):
# Cozmo knows where the charger is
charger = robot.world.charger
else:
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)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
look_around.stop()
# on boot up show loading screen
# define light colours
red_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 0, 0)))
blue_light = cozmo.lights.Light(cozmo.lights.Color(rgb=(0, 0, 255)))
yellow__light = cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 0)))
# tag each cube found as a different colour
red_cube = robot.world.get_light_cube(LightCube1Id)
blue_cube = robot.world.get_light_cube(LightCube2Id)
yellow_cube = robot.world.get_light_cube(LightCube3Id)
red_cube.set_lights(red_light)
blue_cube.set_lights(blue_light)
yellow_cube.set_lights(yellow__light)
# Pass found objects to GUI for selection
robot.say_text("Ready").wait_for_completed()
# user selects which cube they want
cube_selected_input = input("Which cube do you want. Options: red_cube, yellow_cube, blue_cube: ")
cube_selected = cube_selected_input
print(cube_selected)
robot.say_text("press").wait_for_completed()
# Wait for conformation
confirmation = input("Are you sure Y/N: ")
if confirmation == "Y":
robot.say_text("OK.").wait_for_completed()
cube_wanted = cube_selected
else:
print("Fine")
cube_wanted = ""
# 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)
look_around.start()
cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=60)
look_around.stop()
# cozmo goes and gets cube
robot.pickup_object(yellow_cube, num_retries=3).wait_for_completed
robot.say_text("Got it").wait_for_completed()
# Cozmo returns cube to user
robot.go_to_object(charger, distance_mm(100)).wait_for_completed()
robot.say_text("Is this one the right one?").wait_for_completed()
# Object is confirmed
confirmation1 = input("Is this the right one? Y/N: ")
if confirmation1 == "Y":
robot.say_text("Yay")
else:
robot.say_text("Well you're getting it anyway.")
action = robot.place_object_on_ground_here(cube_wanted)
action.wait_for_completed()
# get dat fist bump
robot.say_text("Do you want me to fetch anything else")
# user says no
# cozmo returns to base
# wait for five minutes of inactivity
robot.say_text("I'm going to have a nap now, let me know if you need anything?")
def cozmo_program(robot: cozmo.robot.Robot):
# Lookaround until Cozmo knows where at least 3 cubes are:
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) < 3:
print("Error: need 3 Cubes but only found", len(cubes), "Cube(s)")
else:
print("Found 3 cubes, seting random lights")
# Light up cubes randomly
lights = [
cozmo.lights.red_light, cozmo.lights.blue_light,
cozmo.lights.green_light
]
i = 0
for cube in cubes:
cube.set_lights(lights[i])
i = i + 1
# choose a number
chosen = random.randint(0, 3)
time.sleep(5)
if chosen < 3:
cube = cubes[chosen]
print("Chose " + str(chosen))
# Drive to the cube
action = robot.go_to_object(cube, distance_mm(20.0))
action.wait_for_completed()
print("Completed action: result = %s" % action)
# take a step back, lift fork and drive to the tube
robot.drive_straight(distance_mm(-10),
speed_mmps(50)).wait_for_completed()
robot.set_lift_height(10.0).wait_for_completed()
robot.set_head_angle(degrees(0.0)).wait_for_completed()
robot.drive_straight(distance_mm(20),
speed_mmps(50)).wait_for_completed()
# hit the tube
for _ in range(2):
robot.move_lift(-5)
time.sleep(0.05)
robot.move_lift(5)
time.sleep(0.05)
# blink the cube
for _ in range(0, 10):
cube.set_lights(cozmo.lights.off_light)
time.sleep(0.2)
cube.set_lights(lights[chosen])
time.sleep(0.2)
# Say the color
if chosen == 0:
robot.say_text("Rot").wait_for_completed()
if chosen == 1:
robot.say_text("Blau").wait_for_completed()
if chosen == 2:
robot.say_text("Grün").wait_for_completed()
# Be happy
robot.drive_straight(distance_mm(-50),
speed_mmps(150)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
else:
# Cozmo chose black
robot.say_text("Nein!").wait_for_completed()
robot.turn_in_place(degrees(180)).wait_for_completed()
robot.say_text("Ich will doch lieber bei schwarz bleiben!"
).wait_for_completed()
def cozmo_tap_game(robot: cozmo.robot.Robot):
"""
The main game
"""
#Setup
speed_tap_game = SpeedTapEngine(robot)
robot_game_action = CozmoPlayerActions()
#emotion manipulation
if robot_game_action.sad_not_angry:
reaction_bias = "sad"
else:
reaction_bias = "angry"
cozmo.logger.info(
"Starting tap game with reaction time %s and game reaction bias %s" %
(robot_game_action.reaction_time, reaction_bias))
cozmo.logger.info("Player %s : Cozmo " % COZMO_ID)
cozmo.logger.info("Player %s : Human" % PLAYER_ID)
# Robot selects cube
robot_cube, player_cube = speed_tap_game.cozmo_setup_game()
time.sleep(0.25)
# Participant select cube
monitor_player_tap = Human_Listener(robot, player_cube, speed_tap_game)
game_complete = False
winner = 0
score_to = 5
monitor_player_tap.game_on = True
monitor_player_tap.start()
deal_count = 1
try:
while not winner:
# robot wait for the light to change on cube i.e. light to be dealt
robot_game_action.act_out(robot, "wait")
deal_type = speed_tap_game.deal_hand()
cozmo.logger.info("Hand %s delt" % deal_count)
deal_count += 1
# Cozmo's tap decision
tapped = robot_game_action.cozmo_tap_decision(
robot, deal_type, speed_tap_game)
# Give player time to take decision
time.sleep(1)
# Current deal is now deactivated
speed_tap_game.deactivate_current_deal()
#Check who scored and do the right emotion display for winning/losing hand
if speed_tap_game.deal_score[-1] == PLAYER_ID:
if speed_tap_game.deal_score.count(PLAYER_ID) >= score_to:
winner = PLAYER_ID
robot_game_action.act_out(robot, "lose_hand")
elif speed_tap_game.deal_score[-1] == COZMO_ID:
if speed_tap_game.deal_score.count(COZMO_ID) >= score_to:
winner = COZMO_ID
robot_game_action.act_out(robot, "win_hand")
# stop light chaser and prep for next round
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
cozmo.logger.info("Score Board : %s" % speed_tap_game.deal_score)
cozmo.logger.info("Cozmo : %s" %
speed_tap_game.deal_score.count(COZMO_ID))
cozmo.logger.info("Player : %s" %
speed_tap_game.deal_score.count(PLAYER_ID))
# clear up games to show final gameresult
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
monitor_player_tap.game_on = False
robot_game_action.act_out(robot, "stand_back")
# Indicate win/loss to player
if winner == COZMO_ID:
robot_cube.set_lights(cozmo.lights.green_light.flash())
player_cube.set_lights(cozmo.lights.red_light.flash())
elif winner == PLAYER_ID:
player_cube.set_lights(cozmo.lights.green_light.flash())
robot_cube.set_lights(cozmo.lights.red_light.flash())
robot.go_to_object(robot_cube, distance_mm(60.0)).wait_for_completed()
cozmo.logger.info("Final Score Cozmo : %s" %
speed_tap_game.deal_score.count(COZMO_ID))
cozmo.logger.info("Final Score Player : %s" %
speed_tap_game.deal_score.count(PLAYER_ID))
# Game end emotion act display
if winner == COZMO_ID:
robot_game_action.act_out(robot, "win_game")
else:
robot_game_action.act_out(robot, "lose_game")
finally:
monitor_player_tap.game_on = False
robot_cube.stop_light_chaser()
player_cube.stop_light_chaser()
robot_cube.set_lights_off()
player_cube.set_lights_off()
monitor_player_tap.join()
del speed_tap_game
del player_cube
del robot_cube
