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.")
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
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)
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
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)
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
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)
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)
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)
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)
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
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()
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()
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
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()
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()
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
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)
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.")
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!"
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')
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
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
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
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(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
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
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()
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
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)
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
