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 run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
fixed_object = robot.world.create_custom_fixed_object(Pose(100, 0, 0, angle_z=degrees(0)),
10, 100, 100, relative_to_robot=True)
if fixed_object:
print("fixed_object created successfully")
robot.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
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_II(cube: cozmo.objects.LightCube):
global robot,SIDE
# Second cube was picked, place it on the first cube
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
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()
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 try_picking_up_cube(): # cube: cozmo.objects.LightCube
# Look for a cube and try picking it up. If another cube is located
# on the one Cozmo is trying to pick up, Cozmo will switch targets
# and pick the upper cube.
# 0: action failed
# 1: action succeeded
global robot, cubeIDs, cubes
num_fail = 5
for failures in range(1,num_fail+1):
cube = look_for_next_cube()
switch_cube_on(cube)
action = robot.pickup_object(cube,use_pre_dock_pose=True, in_parallel=False, num_retries=1)
action.wait_for_completed()
if(action.has_failed):
print('The cube cannot be picked, checking for stacked cube.')
switch_cube_off(cube)
# Look up to try and find a stacked cube
robot.set_head_angle(degrees(20),in_parallel=False).wait_for_completed()
# Pop last cube from memory that raised an exception
cubes.pop()
cubeIDs.pop()
# If no exception is raised, check for action failure
else:
print(cubeIDs)
break
if(failures >= num_fail):
print('ERROR: Picking cube ' + str(cube.cube_id) + ' has failed.')
print('MESSAGE: Aborting clean up, engaging docking procedure.')
return 0, None
else:
return 1, cube
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 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 run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
# Turn 90 degrees, play an animation, exit.
robot.turn_in_place(degrees(90)).wait_for_completed()
anim = robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin)
anim.wait_for_completed()
def do_turn(self, cmd_args, kw_args):
usage = "'turn X' where X is a number of degrees to turn"
drive_angle = extract_float(cmd_args)
if drive_angle is not None:
self.cozmo.turn_in_place(degrees(drive_angle)).wait_for_completed()
return "I turned " + str(drive_angle) + " degrees!"
return "Error: usage = " + usage
def do_head(self, cmd_args, kw_args):
usage = "'head X' where X is desired angle for head" #-25 (down) to 44.5 degrees (up)
head_angle = extract_float(cmd_args)
if head_angle is not None:
head_angle_action = self.cozmo.set_head_angle(degrees(head_angle))
clamped_head_angle = head_angle_action.angle.degrees
head_angle_action.wait_for_completed()
resultString = "I moved head to " + "{0:.1f}".format(clamped_head_angle)
if abs(head_angle - clamped_head_angle) > 0.01:
resultString += " (clamped to range)"
return resultString
return "Error: usage = " + usage
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
# try to find a block
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=30)
print("Found cube", 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 is None:
robot.play_anim_trigger(cozmo.anim.Triggers.MajorFail)
return
print("Yay, found cube")
cube.set_lights(cozmo.lights.green_light.flash())
anim = robot.play_anim_trigger(cozmo.anim.Triggers.BlockReact)
anim.wait_for_completed()
action = robot.pickup_object(cube)
print("got action", action)
result = action.wait_for_completed(timeout=30)
print("got action result", result)
robot.turn_in_place(degrees(90)).wait_for_completed()
action = robot.place_object_on_ground_here(cube)
print("got action", action)
result = action.wait_for_completed(timeout=30)
print("got action result", result)
anim = robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin)
cube.set_light_corners(None, None, None, None)
anim.wait_for_completed()
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
scales = ["Doe", "Ray", "Mi", "Fa", "So", "La", "Ti", "Doe"]
# Find voice_pitch_delta value that will range the pitch from -1 to 1 over all of the scales
voice_pitch = -1.0
voice_pitch_delta = 2.0 / (len(scales) -1)
# Move head and lift down to the bottom, and wait until that's achieved
robot.move_head(-5) # start moving head down so it mostly happens in parallel with lift
robot.set_lift_height(0.0).wait_for_completed()
robot.set_head_angle(degrees(-25.0)).wait_for_completed()
# Start slowly raising lift and head
robot.move_lift(0.15)
robot.move_head(0.15)
# "Sing" each note of the scale at increasingly high pitch
for note in scales:
robot.say_text(note, voice_pitch=voice_pitch, duration_scalar=0.3).wait_for_completed()
voice_pitch += voice_pitch_delta
def findCube(grid, robot: cozmo.robot.Robot):
startRobot = front_robot(robot)
start = grid.getStart()
robot.set_head_angle(degrees(0)).wait_for_completed()
grid.clearGoals()
grid.addGoal((grid.width / 2, grid.height / 2))
goal = None
angle = None
reach_center = False
while True:
print()
print("1. Another loop")
curRobot = front_robot(robot)
rCoorX, rCoorY = xyToCoor(grid, start, startRobot, curRobot[0],
curRobot[1])
reach_center = reach_center or (
(rCoorX == grid.width / 2 or rCoorX == grid.width / 2 + 1
or rCoorX == grid.width / 2 - 1) and
(rCoorY == grid.height / 2 or rCoorY == grid.height / 2 + 1
or rCoorY == grid.height / 2 - 1))
print("reach_center = ", reach_center)
grid.clearStart()
grid.setStart((rCoorX, rCoorY))
try:
# Update visible objects
print("2. Update objects")
update_obstacles(grid, start, startRobot, robot)
# Find the goal (cube1), update the grid and drive toward it
if goal:
print("8. Found goals")
update_obstacles(grid, start, startRobot, robot)
grid.clearPath()
print("9. Finding path")
astar(grid, heuristic)
path = grid.getPath()
print("10. Found path ", path)
if grid.checkPath() and len(path) > 2:
if turn_to(grid, start, startRobot, robot, path[1][0],
path[1][1]):
continue
drive_straight_to(grid, start, startRobot, robot,
path[1][0], path[1][1])
# Reach goal:
if len(path) <= 2:
turn_to(grid, start, startRobot, robot, goal[0], goal[1])
stopevent.set()
return
continue
# Found the goal (cube1)
objects = robot.world.visible_objects
for obj in objects:
if obj.cube_id == cozmo.objects.LightCube1Id:
cornersCoors = lightCubeXYToBorderCoors(
grid, start, startRobot, obj.pose.position.x,
obj.pose.position.y)
for corner in cornersCoors:
grid.addObstacle((corner[0], corner[1]))
coorX, coorY = xyToCoor(grid, start, startRobot,
obj.pose.position.x,
obj.pose.position.y)
angle = obj.pose.rotation.angle_z.degrees
if angle < 45 and angle > -45:
coorX -= 3
elif angle > 45 and angle < 135:
coorY -= 3
elif angle > 135 or angle < -135:
coorX += 3
else:
coorY += 3
grid.clearGoals()
goal = (coorX, coorY)
grid.addGoal((coorX, coorY))
continue
if reach_center:
print("4. Reach center")
grid.clearPath()
grid.clearGoals()
# Once reach center, keep turning in place
robot.turn_in_place(degrees(20)).wait_for_completed()
else:
print("5. Drive toward center")
update_obstacles(grid, start, startRobot, robot)
# If not reach center, try to reach center
# Update current robot position as start and recalculate path to center and drive toward it, 1 cell at a time
grid.clearPath()
print("6. Finding path")
astar(grid, heuristic)
path = grid.getPath()
print("7. Found path ", path)
if grid.checkPath() and len(path) > 1:
if turn_to(grid, start, startRobot, robot, path[1][0],
path[1][1]):
continue
drive_straight_to(grid, start, startRobot, robot,
path[1][0], path[1][1])
except Exception as e:
print("Exception")
print(e)
return
async def run(robot: cozmo.robot.Robot):
await look_around_until_converge(robot)
cosimo = CozmoWarehouseWorker()
cosimo.localize()
directions = goal_pose - last_pose
current_pose = last_pose
last_robot_pose = robot.pose
print("SETTING LAST ROBOT POSE TO: ", last_robot_pose)
print("SO WE GOING TO FOLLOW THIS TO PICKUP ZONE:", directions)
await execute_directions(robot, directions)
await robot.turn_in_place(angle=degrees(45)).wait_for_completed()
print("LAST ROBOT POSE IS: ", last_robot_pose)
print("CURRENT POSE IS:", robot.pose)
print("WE THINK WE MOVED THIS MUCH TO GO TO PICKUP ZONE: ", convertPoseToInches(robot.pose - last_robot_pose))
current_pose = current_pose + convertPoseToInches(rotate_point(robot.pose, - last_robot_pose)
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER DRIVING TO PICKUPZONE: ", current_pose)
# await robot.say_text('Ready for pick up!').wait_for_completed()
while True:
cube = await robot.world.wait_for_observed_light_cube(timeout=30)
print("Found cube: %s" % cube)
await robot.pickup_object(cube, num_retries=5).wait_for_completed()
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO PICK UP CUBE: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
#cosimo.update_pose()
print("COZMO THINKS IT IS AT AFTER PICKING UP CUBE: ", current_pose)
#await look_around_until_converge(robot)
# intialize an explorer after localized
#cosimo = CozmoExplorer(robot, x_0=last_pose.position.x, y_0=last_pose.position.y, theta_0=last_pose.rotation.angle_z.radians)
# move robot to pickup zone once localized
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
#current_pose = last_pose
# rrt to drop zone and drop off cube
for destination in drop_off_directions:
directions = convertInchesToPose(destination) - current_pose
await execute_directions(robot,directions)
current_pose = current_pose + convertPoseToInches(robot.pose - last_robot_pose)
print("WE THINK WE MOVED THIS MUCH TO FOLLOW DIRECTIONS: ", convertPoseToInches(robot.pose - last_robot_pose))
last_robot_pose = robot.pose
print("COZMO THINKS IT IS AT AFTER FOLLOWING DIRECTIONS: ", current_pose)
#await cosimo.go_to_goal(goal_node=dropoff_node)
await robot.set_lift_height(0.0).wait_for_completed()
# rrt to just in front of pick up zone
# await cosimo.go_to_goal(goal_node=pickup_node)
class CozmoThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self, daemon=False)
def run(self):
cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on his charger
cozmo.run_program(run, use_viewer=False)
if __name__ == '__main__':
# cozmo thread
cozmo_thread = CozmoThread()
cozmo_thread.start()
# init
gui.show_particles(pf.particles)
gui.show_mean(0, 0, 0)
gui.start()
def cozmo_program(robot: cozmo.robot.Robot):
robot.drive_off_charger_contacts().wait_for_completed()
robot.drive_straight(cozmo.util.distance_mm(200),
cozmo.util.speed_mmps(50)).wait_for_completed()
robot.say_text("please Place Go Pro on my lift",
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=0.6,
voice_pitch=0.3,
in_parallel=False,
num_retries=0).wait_for_completed()
time.sleep(4)
#parse input for separate actions
commandList = commands.split('%')
for command in commandList:
instructions = command.split(';')
#PHOTO SECTION
#step 1 = "photo"
#step 2 = "front, back, left or right"
#return to original position in front of charger
if instructions[0] == "photo":
robot.say_text("photo program initiated",
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=0.6,
voice_pitch=0.3,
in_parallel=False,
num_retries=0).wait_for_completed()
driveStraight(robot)
if instructions[1] == "front":
takePhoto(robot)
backUp(robot)
if instructions[1] == "back":
turnAround(robot)
takePhoto(robot)
turnAround(robot)
backUp(robot)
if instructions[1] == "left":
robot.turn_in_place(degrees(90)).wait_for_completed()
driveStraight(robot)
takePhoto(robot)
backUp(robot)
robot.turn_in_place(degrees(-90)).wait_for_completed()
backUp(robot)
if instructions[1] == "right":
robot.turn_in_place(degrees(-90)).wait_for_completed()
driveStraight(robot)
takePhoto(robot)
backUp(robot)
robot.turn_in_place(degrees(90)).wait_for_completed()
backUp(robot)
# VIDEO SECTION
# step 1 = "video"
# step 2 = "stationary or pan"
# step 3 = "front, back or 360"
# return to original position in front of charger
if instructions[0] == "video":
robot.say_text("video program initiated",
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=0.6,
voice_pitch=0.3,
in_parallel=False,
num_retries=0).wait_for_completed()
driveStraight(robot)
if instructions[1] == "stationary":
startVideo(robot)
time.sleep(videoTime)
stopVideo(robot)
backUp(robot)
if instructions[1] == "pan":
if instructions[2] == "front":
robot.turn_in_place(degrees(90)).wait_for_completed()
startVideo(robot)
robot.turn_in_place(degrees(-180), None, 0, None,
degrees(3)).wait_for_completed()
stopVideo(robot)
robot.turn_in_place(degrees(90)).wait_for_completed()
backUp(robot)
if instructions[2] == "back":
robot.turn_in_place(degrees(-90)).wait_for_completed()
startVideo(robot)
robot.turn_in_place(degrees(-180), None, 0, None,
degrees(3)).wait_for_completed()
stopVideo(robot)
robot.turn_in_place(degrees(-90)).wait_for_completed()
backUp(robot)
if instructions[2] == "360":
startVideo(robot)
robot.turn_in_place(degrees(-360), None, 0, None,
degrees(3)).wait_for_completed()
stopVideo(robot)
backUp(robot)
#TIME LAPSE SECTION
# step 1 = "timelapse"
# step 2 = "front, back, left or right"
# return to original position in front of charger
if instructions[0] == "timelapse":
robot.say_text("time lapse program initiated",
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=0.6,
voice_pitch=0.3,
in_parallel=False,
num_retries=0).wait_for_completed()
driveStraight(robot)
if instructions[1] == "front":
takeTimeLapse(robot)
backUp(robot)
if instructions[1] == "back":
turnAround(robot)
takeTimeLapse(robot)
turnAround(robot)
backUp(robot)
if instructions[1] == "left":
robot.turn_in_place(degrees(90)).wait_for_completed()
driveStraight(robot)
takeTimeLapse(robot)
backUp(robot)
robot.turn_in_place(degrees(-90)).wait_for_completed()
backUp(robot)
if instructions[1] == "right":
robot.turn_in_place(degrees(-90)).wait_for_completed()
driveStraight(robot)
takeTimeLapse(robot)
backUp(robot)
robot.turn_in_place(degrees(90)).wait_for_completed()
backUp(robot)
#AFTER ALL COMMANDS
robot.say_text("Please take Go Pro off my lift",
play_excited_animation=False,
use_cozmo_voice=False,
duration_scalar=0.6,
voice_pitch=0.3,
in_parallel=False,
num_retries=0).wait_for_completed()
time.sleep(4)
returnToCharger(robot)
async def run(robot: cozmo.robot.Robot):
global last_pose
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
await robot.set_head_angle(degrees(0)).wait_for_completed()
await robot.set_lift_height(0).wait_for_completed()
pf = ParticleFilter(grid)
############################################################################
######################### YOUR CODE HERE####################################
############################################################################
while True:
if state == "LOCALIZE":
direction = 1
m_confident = False
while m_confident == False:
curr_pose = robot.pose
odom = compute_odometry(curr_pose)
markers = await image_processing(robot)
marker2d_list = cvt_2Dmarker_measurements(markers)
m_x, m_y, m_h, m_confident = pf.update(odom, marker2d_list)
gui.show_particles(pf.particles)
gui.show_mean(m_x, m_y, m_h, m_confident)
gui.updated.set()
last_pose = curr_pose
#
if m_confident == False:
numMarkers = len(markers)
if direction == -1 or (numMarkers > 0
and marker2d_list[0][0] > 2.0):
await robot.drive_straight(
distance_mm(direction * DISTANCE),
speed_mmps(DISTANCE)).wait_for_completed()
direction *= -1
if direction == 1:
await robot.turn_in_place(degrees(TURNING_ANGLE)
).wait_for_completed()
else:
await robot.turn_in_place(degrees(TURNING_ANGLE)
).wait_for_completed()
state = "MOVING"
elif state == "MOVING":
#
m_x, m_y, m_h, m_confident = compute_mean_pose(pf.particles)
#
region = "DOCKING" if m_x <= 13 else "WAREHOUSE"
x_goal, y_goal, h_goal = regionGoals[region]
x_diff, y_diff = x_goal * .95 - m_x, y_goal * .95 - m_y
tan = math.degrees(atan2(y_diff, x_diff))
rot = diff_heading_deg(tan, m_h)
await robot.turn_in_place(degrees(rot)).wait_for_completed()
#Move toward goal
dist_to_goal = math.sqrt(y_diff**2 + x_diff**2) * 25
dist = 0.0
while dist < dist_to_goal:
min_dist = min(DISTANCE, dist_to_goal - dist)
await robot.drive_straight(
distance_mm(min_dist),
speed_mmps(40)).wait_for_completed()
dist = dist + min_dist
# Reoriented
await robot.turn_in_place(degrees(-1 * tan)).wait_for_completed()
reoriented_angle = 145 if region == "DOCKING" else 180
await robot.turn_in_place(degrees(reoriented_angle)
).wait_for_completed()
cube = None
try:
cube = await robot.world.wait_for_observed_light_cube(timeout=1
)
except:
print("Cube not found")
pass
async def email_security_guard(robot):
'''The core of the email_security_guard program'''
# Turn on image receiving by the camera
robot.camera.image_stream_enabled = True
# Create our security guard
dsg = EmailSecurityGuard()
# 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:
# 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 get_on_charger():
global robot,pitch_threshold
robot.set_head_angle(degrees(0),in_parallel=False).wait_for_completed()
pitch_threshold = math.fabs(robot.pose_pitch.degrees)
pitch_threshold += 1 # Add 1 degree to threshold
print('Pitch threshold: ' + str(pitch_threshold))
# Drive towards charger
go_to_charger()
# Let Cozmo first look for the charger once again. The coordinates
# tend to be too unprecise if an old coordinate system is kept.
if robot.world.charger is not None and robot.world.charger.pose.is_comparable(robot.pose):
robot.world.charger.pose.invalidate()
charger = find_charger()
# Adjust position in front of the charger
final_adjust(charger,critical=True)
# Turn around and start going backward
turn_around()
robot.drive_wheel_motors(-120,-120)
robot.set_lift_height(height=0.5,max_speed=10,in_parallel=True).wait_for_completed()
robot.set_head_angle(degrees(0),in_parallel=True).wait_for_completed()
# This section allow to wait for Cozmo to arrive on its charger
# and detect eventual errors. The whole procedure will be restarted
# in case something goes wrong.
timeout = 1 # seconds before timeout
t = 0
# Wait for back wheels to climb on charger
while(True):
time.sleep(.1)
t += 0.1
if(t >= timeout):
print('ERROR: robot timed out before climbing on charger.')
restart_procedure(charger)
return
elif(math.fabs(robot.pose_pitch.degrees) >= pitch_threshold):
print('CHECK: backwheels on charger.')
break
# Wait for front wheels to climb on charger
timeout = 2
t = 0
while(True):
time.sleep(.1)
t += 0.1
if(math.fabs(robot.pose_pitch.degrees) > 20 or t >= timeout):
# The robot is climbing on charger's wall -> restart
print('ERROR: robot climbed on charger\'s wall or timed out.')
restart_procedure(charger)
return
elif(math.fabs(robot.pose_pitch.degrees) < pitch_threshold):
print('CHECK: robot on charger, backing up on pins.')
robot.stop_all_motors()
break
# Final backup onto charger's contacts
robot.set_lift_height(height=0,max_speed=10,in_parallel=True).wait_for_completed()
robot.backup_onto_charger(max_drive_time=3)
if(robot.is_on_charger):
print('PROCEDURE SUCCEEDED')
else:
restart_procedure(charger)
return
# Celebrate success
robot.drive_off_charger_contacts().wait_for_completed()
celebrate(robot) # A small celebration where only the head moves
robot.backup_onto_charger(max_drive_time=3)
return
def excited(robot: cozmo.robot.Robot):
trigger = cozmo.anim.Triggers.CodeLabExcited
play_animation(robot,trigger)
robot.turn_in_place(degrees(-130)).wait_for_completed()
async def turn_counterclockwise(self):
"""
Turn counterclockwise in place (left)
"""
await self.robot.turn_in_place(degrees(90)).wait_for_completed()
async def turn_clockwise(self):
"""
Turn clockwise in place (right)
"""
await self.robot.turn_in_place(degrees(-90)).wait_for_completed()
async def turn_in_place(self):
await self.robot.turn_in_place(degrees(180)).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
global angle
angle = 25.
robot.set_head_angle(degrees(angle)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'
frame = os.path.join(directory, "current.jpeg")
print("Starting Tensorflow...")
with tf.Session() as sess:
print("Session successfully started")
model = load_model('modelv1.07-0.96.hdf5')
while True:
global X, Y, W, H
global result
X = 245.
Y = 165.
W = 150.
H = 150.
gt = [X, Y, W, H]
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
frame = os.path.join(directory, "current.jpeg")
result = 0
dog_counter = 0
cat_counter = 0
background_counter = 0
next_state = 0
current_state = 0 #Background: 0, Cat:1, Dog:2
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
show_frame(np.asarray(Image.open(frame)), [900.,900.,900.,900.], 1)
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
next_state = np.argmax(result)
print('Arg max: ',next_state)
# Initial Current State is Background
if current_state == 0:
print('Background')
if next_state == 1: # Detected a Cat
current_state = 1 # Transition to Cat State
background_counter = 0
cat_counter = 1
dog_counter = 0
elif next_state == 2: # Detected a Dog
current_state = 2 # Transition to Dog state
background_counter = 0
cat_counter = 0
dog_counter = 1
# Current State is Cat
elif current_state == 1:
print('\t\t\t\t\t\tCat')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
cat_counter = 0
elif next_state == 1: # Detected Cat itself
cat_counter +=1
if cat_counter >= 30:
print('Cozmo sees a cat')
dense = model.get_layer('dense').get_weights()
weights = dense[0].T
testing_counter = 0
detected_centroid = 0
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
frame_average = 2
frame_count = 0
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
kernels = out_relu.reshape(7,7,1280)
final = np.dot(kernels,weights[result[0].argmax()])
final1 = array_to_img(final.reshape(7,7,1))
final1 = final1.resize((224,224), Image.ANTIALIAS)
box = img_to_array(final1).reshape(224,224)
#box = cv2.blur(box,(30,30))
temp = (box > box.max()*.8) *1
temp_adjusted = np.ndarray(shape=np.shape(temp), dtype=np.dtype(np.uint8))
temp_adjusted[:,:] = np.asarray(temp)*255
contours, hierarchy = cv2.findContours(temp_adjusted, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
contours = np.array(contours)
max_area = [0,0] # contours index and area
for index, contour in enumerate(contours):
if(max_area[1]< len(contour)):
max_area = [index,len(contour)]
contours_adjusted = contours[max_area[0]].squeeze(axis=1).T
xmin = contours_adjusted[0].min() * (640./224.)
ymin = contours_adjusted[1].min() * (480./224.)
xmax = contours_adjusted[0].max() * (640./224.)
ymax = contours_adjusted[1].max() * (480./224.)
if result[0].argmax() == 1:
# Frame smoothing
frame_count = frame_count + 1
xmin_avg = xmin_avg + xmin
xmax_avg = xmax_avg + xmax
ymin_avg = ymin_avg + ymin
ymax_avg = ymax_avg + ymax
if frame_count % frame_average == 0:
frame_count = 0
xmin_avg = xmin_avg/frame_average
xmax_avg = xmax_avg/frame_average
ymin_avg = ymin_avg/frame_average
ymax_avg = ymax_avg/frame_average
print(xmin_avg, end=",")
print(ymin_avg, end=",")
print(xmax_avg, end=",")
print(ymax_avg, end="\n")
ymin_avg = ymin_avg + (ymax_avg - ymin_avg)/2. - H/2.
xmin_avg = xmin_avg + (xmax_avg - xmin_avg)/2. - W/2.
print("150: ",xmin_avg, end=",")
print("150: ",ymin_avg, end="\n")
gt = [xmin_avg, ymin_avg, W, H]
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
bboxes = np.zeros((1, 4))
#bboxes[0,:] = pos_x-target_w/2, pos_y-target_h/2, target_w, target_h
bboxes[0,:] = pos_x-W/2, pos_y-H/2, W, H
print(len(contours))
testing_counter = testing_counter + 1
print("Testing_counter: ",testing_counter)
show_frame(np.asarray(Image.open(frame)), gt, 1)
print("Cat is detected")
print("Starting the tracker ...")
if (bboxes[0,1] + bboxes[0,3]/2) < (Y + H/2 - 40):
print("Command: Raise the head")
angle = angle + 0.5
if angle > 44.5:
angle = 44.5
elif (bboxes[0,1] + bboxes[0,3]/2) > (Y + H/2 + 40):
print("Command: Lower the head")
angle = angle - 0.5
if angle < 0:
angle = 0
else:
pass
set_head_angle_action = robot.set_head_angle(degrees(angle), max_speed=20, in_parallel=True)
if straight(bboxes[0,:])[0] != 0 and turn(bboxes[0,:])[0] != 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0] + turn(bboxes[0,:])[0], straight(bboxes[0,:])[1] + turn(bboxes[0,:])[1])
detected_centroid = 0
elif straight(bboxes[0,:])[0] == 0 and turn(bboxes[0,:])[0] == 0:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
elif straight(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(turn(bboxes[0,:])[0], turn(bboxes[0,:])[1])
detected_centroid = 0
elif turn(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0], straight(bboxes[0,:])[1])
detected_centroid = 0
else:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
if detected_centroid > 20//frame_average:
detected_centroid = 0
print("Reached a stable state.........\t\t\t\t\t\t\t\t STABLE")
# Go near the object
set_head_angle_action.wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
robot.wait_for_all_actions_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
target_frame_count = 1
while True:
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame1 = latest_img.raw_image
target_frame1 = target_frame1.resize((640,480), Image.ANTIALIAS)
#target_frame1 = target_frame1.convert('L')
target_frame1 = np.asarray(target_frame1)
#orb1 = cv2.ORB_create(500)
#kp1 = orb1.detect(target_frame1,None)
#kp1, des1 = orb1.compute(target_frame1, kp1)
#features_img1 = cv2.drawKeypoints(target_frame1, kp1, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",features_img1)
plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",target_frame1)
drive_straight_action = robot.drive_straight(distance=cozmo.util.distance_mm(distance_mm=10),speed=cozmo.util.speed_mmps(10), in_parallel=True)
drive_straight_action.wait_for_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame2 = latest_img.raw_image
target_frame2 = target_frame2.resize((640,480), Image.ANTIALIAS)
#target_frame2 = target_frame2.convert('L')
target_frame2 = np.asarray(target_frame2)
#orb2 = cv2.ORB_create(500)
#kp2 = orb2.detect(target_frame2,None)
#kp2, des2 = orb2.compute(target_frame2, kp2)
#features_img2 = cv2.drawKeypoints(target_frame2, kp2, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",features_img2)
plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",target_frame2)
target_frame_count = target_frame_count + 1
'''
matcher = cv2.DescriptorMatcher_create(cv2.DESCRIPTOR_MATCHER_BRUTEFORCE_HAMMING)
matches = matcher.match(des1, des2, None)
matches.sort(key=lambda x: x.distance, reverse=False)
matches = matches[:10]
imMatches = cv2.drawMatches(target_frame1, kp1, target_frame2, kp2, matches, None)
cv2.imwrite("matches_tf1_tf2.jpg", imMatches)
points1 = np.zeros((len(matches), 2), dtype=np.float32)
points2 = np.zeros((len(matches), 2), dtype=np.float32)
for i, match in enumerate(matches):
points1[i, :] = kp1[match.queryIdx].pt
points2[i, :] = kp2[match.trainIdx].pt
print("Points1 [{}]: {}".format(i,points1[i][0]), points1[i][1],"\tPoints2: ",points2[i][0], points2[i][1])
index = None
dist1_x = []
dist2_x = []
for index in range(len(points1)):
dist1_x.append((W/2.)-points1[index][0]) # Extract only the x-coordinate
dist2_x.append((W/2.)-points2[index][0]) # Extract only the x-coordinate
fw_x = 1./((1./np.array(dist2_x)) - (1./np.array(dist1_x))) # Calculate the image plane to obj plane mapping in x direction
pt1_x = []
pt2_x = []
for index in range(len(points1)):
pt1_x.append(fw_x[index]/(W/2. - points1[index][0]))
pt2_x.append(fw_x[index]/(W/2. - points2[index][0]))
print("Approx. distance[{}]: {}".format(index, pt1_x[index]))
if len(pt2_x) < 10:
break
'''
sys.exit(0)
else: # Detected Dog
dog_counter += 1
if dog_counter >= 6: # Transition to Dog only if Dog appeared for more than 6 times
cat_counter = 0
current_state = 2
# Current State is Dog
elif current_state == 2:
print('\t\t\t\t\t\t\t\t\t\t\t\tDog')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
dog_counter = 0
elif next_state == 2: # Detected Dog itself
dog_counter +=1
if dog_counter >= 30:
print('Cozmo sees a Dog')
robot.drive_wheels(-50, -50)
time.sleep(3)
robot.drive_wheels(70, -70)
time.sleep(2.8)
robot.drive_wheels(0, 0)
break
else: # Detected Cat
cat_counter += 1
if cat_counter >= 6: # Transition to Cat only if Cat appeared for more than 6 times
dog_counter = 0
current_state = 1
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment description for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
global found_cubes
found_cubes = []
goal_degrees = []
grid.addGoal((grid.width / 2, grid.height / 2))
found_goal = False
astar(grid, heuristic)
path = grid.getPath()
path_index = 0
grid_init_start_pose = grid.getStart()
(robot_grid_x, robot_grid_y) = grid.getStart()
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.set_lift_height(1).wait_for_completed()
robot.say_text('Game is on').wait_for_completed()
while not stopevent.is_set():
new_cube = search_cube(robot, grid)
if not new_cube == None:
grid.clearStart()
grid.clearVisited()
grid.clearPath()
grid.setStart((robot_grid_x, robot_grid_y))
add_obstacle(
grid, new_cube, grid_init_start_pose
) # Add the obstacle for all cubes that had been found
if new_cube.cube_id == LightCube1Id:
new_cube.set_lights(cozmo.lights.blue_light)
goal_degrees = set_goal(
grid, new_cube, grid_init_start_pose
) # Update the goal coordinate while found cube 1
robot.say_text("It's the Goal").wait_for_completed()
found_goal = True
else:
new_cube.set_lights(cozmo.lights.red_light)
robot.say_text("It's an Obstacle").wait_for_completed()
# Replanning the path for
robot.say_text('Replanning').wait_for_completed()
try:
astar(grid, heuristic)
except:
robot.say_text("Cannot go to that place").wait_for_completed()
return
path_index = 0
path = grid.getPath()
path_index += 1
if path_index == len(path): # At the goal position
if not found_goal: # At the center of grid
path_index -= 1
robot.turn_in_place(Angle(degrees=-30)).wait_for_completed()
continue
else: # Arrived the final place
goal_degree = goal_degrees[f"{(robot_grid_x, robot_grid_y)}"]
robot.turn_in_place(
Angle(degrees=normalize_angle(
goal_degree - robot.pose.rotation.angle_z.degrees))
).wait_for_completed()
robot.say_text('Arrived').wait_for_completed()
break
current_pose = path[path_index - 1]
next_pose = path[path_index]
robot_grid_x += int(next_pose[0] - current_pose[0])
robot_grid_y += int(next_pose[1] - current_pose[1])
x = (next_pose[0] - current_pose[0]) * grid.scale
y = (next_pose[1] - current_pose[1]) * grid.scale
degree = ((90 * y / abs(y)) if x == 0 else math.degrees(
math.atan2(y, x))) - robot.pose.rotation.angle_z.degrees
# robot.turn_in_place(Angle(degrees=normalize_angle(degree))).wait_for_completed()
# robot.drive_straight(distance_mm(math.sqrt(x**2 + y**2)), speed_mmps(50), should_play_anim=False).wait_for_completed()
(x, y) = world_to_related(x, y, robot.pose.rotation.angle_z.degrees)
robot.go_to_pose(Pose(x, y, 0, angle_z=Angle(degrees=degree)),
relative_to_robot=True).wait_for_completed()
stopevent.wait()
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment document for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
foundGoal = False
foudnCenter = False
setPath = False
while not stopevent.is_set():
if foundGoal == False:
# find goal
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=5)
print("Found cube: %s" % cube)
foundGoal = True
pos = cube.pose.position
x, y, z = pos.x, pos.y, pos.z
cube_angle = cube.pose.rotation.angle_z.degrees
print("cube angle is: ", cube_angle)
a, b = 0, 0
if cube_angle > -22.5 and cube_angle < 22.5:
a, b = -1, 0
if cube_angle >= 22.5 and cube_angle < 67.5:
a, b = -1, -1
if cube_angle >= 67.5 and cube_angle < 112.5:
a, b = 0, -1
if cube_angle >= 112.5 and cube_angle < 157.5:
a, b = 1, -1
if cube_angle >= 157.5 or cube_angle <= -157.5:
a, b = 1, 0
if cube_angle > -67.5 and cube_angle <= -22.5:
a, b = -1, 1
if cube_angle > -112.5 and cube_angle <= -67.5:
a, b = 0, 1
if cube_angle > -157.5 and cube_angle <= -112.5:
a, b = 1, 1
obs1 = int(x / grid.scale + grid.getStart()[0] +
0.5), int(y / grid.scale + grid.getStart()[1] + 0.5)
add_obs = []
for i in range(-1, 2, 1):
for j in range(-1, 2, 1):
ob = obs1[0] + i, obs1[1] + j
add_obs.append(ob)
grid.addObstacles(add_obs)
goal = obs1[0] + a * 2, obs1[1] + b * 2
print(goal)
grid.clearGoals()
grid.addGoal(goal)
except asyncio.TimeoutError:
print("Didn't find a cube")
goal = int(grid.width / 2), int(grid.height / 2)
grid.addGoal(goal)
foundGoal = True
foundCenter = True
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
else:
# astar
# astar(grid)
# get path
# path = grid.getPath() #a list of cells
robot_pose = robot.pose
print(robot_pose)
rx, ry = robot_pose.position.x, robot_pose.position.y
cx = int(rx / grid.scale + grid.getStart()[0] + 0.5)
cy = int(ry / grid.scale + grid.getStart()[1] + 0.5)
# update start
grid.clearStart()
grid.setStart((cx, cy))
astar(grid, heuristic)
path = grid.getPath()
print(path)
prev = path[0]
for p in path[1:]:
diff = p[0] - prev[0], p[1] - prev[1]
# Needs to verify
curr_angle = robot.pose.rotation.angle_z
angle = getAngle(diff)
angle_to_turn = angle - curr_angle.degrees
robot.turn_in_place(
degrees(angle_to_turn)).wait_for_completed()
d = math.sqrt(math.pow(diff[0], 2) + math.pow(diff[1], 2))
robot.drive_straight(distance_mm(d * 50),
speed_mmps(40)).wait_for_completed()
posX, posY = grid.scale * (p[0] - 1), grid.scale * (p[1] - 1)
# posX, posY = 25 * p[0] - rx, 25 * p[1] - ry
# pose = cozmo.util.Pose(posX, posY, 0.0, q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0)
# Everytime we move, check to see if there's an obstacle in our vision
# pose = cozmo.util.Pose(posX, posY, 0.0, angle_z = cozmo.util.degrees(angle))
# robot.go_to_pose(pose, in_parallel = True).wait_for_completed()
# robot.go_to_pose(pose)
# time.sleep(0.1)
print(p)
print(robot.pose.position.x, robot.pose.position.y)
print(posX, posY)
prev = p
# ca = -1.0*cube_angle
# pose = cozmo.util.Pose(robot.pose.position.x, robot.pose.position.y, robot.pose.position.z, angle_z = degrees(ca))
# robot.go_to_pose(pose).wait_for_completed()
if foundCenter == True: #not in the goal yet
foundGoal = False
foundCenter = False
else:
curr_angle = robot.pose.rotation.angle_z
angle = cube_angle
angle_to_turn = angle - curr_angle.degrees
robot.turn_in_place(
degrees(angle_to_turn)).wait_for_completed()
break # Your code here
def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, confident
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
############YOUR CODE HERE#################
###################
robot.set_head_angle(radians(0)).wait_for_completed()
estimated = [0, 0, 0, False]
trueVal = 0
leaveTurningPhase = False
complete = False
doneSearching = False
while not complete:
# print(robot.is_picked_up)
if risingEdge(robot.is_picked_up):
pf = ParticleFilter(grid)
elif robot.is_picked_up:
# print("got picked up")
trueVal = 0
estimated = [0, 0, 0, False]
leaveTurningPhase = False
robot.drive_wheels(0, 0)
doneSearching = False
elif not robot.is_picked_up:
if trueVal < 5:
print("in searching phase")
robot.drive_wheels(5, 25)
else:
doneSearching = True
if not leaveTurningPhase:
robot.stop_all_motors()
print("in turning phase")
gh = math.degrees(
math.atan2(goal[1] - estimated[1],
goal[0] - estimated[0]))
ch = estimated[2]
dh = gh - ch
# print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
leaveTurningPhase = True
# print("est h 1", estimated[2])
else:
print("in driving phase")
robot.drive_wheels(25, 25)
if (grid_distance(goal[0], goal[1], estimated[0],
estimated[1])) < 0.1:
robot.stop_all_motors()
# print("est h 2", estimated[2])
dh = goal[2] - estimated[2]
# print("dh2", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
robot.play_anim_trigger(cozanim.Triggers.SparkSuccess
).wait_for_completed()
complete = True
curr_pose = robot.pose
img = image_processing(robot)
markers = cvt_2Dmarker_measurements(img)
# print(markers)
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
last_pose = curr_pose
if estimated[3]:
trueVal += 1
else:
trueVal = 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 cozmo_program(robot: cozmo.robot.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 cozmo_rigth_turn(robot: cozmo.robot.Robot):
global DONKEY_COZMO_TURN_ANGLE
robot.turn_in_place(degrees(-1.0 * DONKEY_COZMO_TURN_ANGLE)).wait_for_completed()
def getCube(robot,cube):
robot.set_head_angle(degrees(7)).wait_for_completed()
robot.pickup_object(cube, num_retries=3).wait_for_completed()
print("cozmo hat Cube aufgeladen")
def main_loop(robot: Robot):
global model
global cortex
global test
global sequence_length
sequence = np.zeros((sequence_length, 70), 'float32')
last_command_time = datetime.now()
last_command = None
# confs = {}
while True:
if not test:
frame = np.log10(get_data(cortex))
else:
frame = np.zeros((70, ), 'float32')
sequence = sequence[1:]
sequence = np.concatenate([sequence, frame.reshape(1, -1)], axis=0)
inferred = model.predict_on_batch(sequence[np.newaxis, :])[0]
command = list(Command)[int(np.argmax(inferred))]
conf = float(inferred[int(np.argmax(inferred))])
print(f"{command} ({last_command}) - {conf}")
# if command not in confs:
# confs[command] = []
#
# confs[command].append(1)
if conf <= 0.8:
command = Command.Nothing
if last_command is None or last_command != command:
last_command = command
command = Command.Nothing
else:
last_command = None
# pass
if not robot.has_in_progress_actions and datetime.now(
) > last_command_time:
# confs = {k: np.array(v).sum() for k, v in confs.items()}
# print("Confs:", confs)
# command = max(confs.items(), key=operator.itemgetter(1))[0]
if command is Command.Nothing:
pass
elif command is Command.Forward:
robot.drive_straight(distance_mm(100), speed_mmps(200))
last_command_time = datetime.now() + timedelta(
seconds=DELAY_TIME)
elif command is Command.Backward:
robot.drive_straight(distance_mm(-100), speed_mmps(200))
last_command_time = datetime.now() + timedelta(
seconds=DELAY_TIME)
elif command is Command.Left:
robot.turn_in_place(degrees(90))
last_command_time = datetime.now() + timedelta(
seconds=DELAY_TIME)
elif command is Command.Right:
robot.turn_in_place(degrees(-90))
last_command_time = datetime.now() + timedelta(
seconds=DELAY_TIME)
# confs = {}
else:
# robot is already doing command, just wait and record signal
pass
def cozmo_turn_in_place(robot, angle, speed):
robot.turn_in_place(degrees(angle),
speed=degrees(speed)).wait_for_completed()
def idle_state(robot : cozmo.robot.Robot = None):
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
image_classifier = joblib.load('image_classifier.joblib')
while(True):
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
robot.set_lift_height(0.0, duration=0.5).wait_for_completed()
states = []
for counter in range(4):
time.sleep(0.1)
latest_image = robot.world.latest_image
new_image = latest_image.raw_image
data = np.array(new_image)
data = np.asarray(data.astype(np.uint8))
data = np.expand_dims(data, axis=0)
data = cozmo_classifier.extract_image_features(0, data)
current_state = image_classifier.predict(data);
states = states + [current_state]
sliding_window_one = (states[-1])
sliding_window_two = (states[-2])
sliding_window_three = (states[-3])
if(sliding_window_one == sliding_window_two):
state = sliding_window_one
elif(sliding_window_two == sliding_window_three):
state = sliding_window_two
elif(sliding_window_one == sliding_window_three):
state = sliding_window_one
else:
state = sliding_window_one
print(state)
if state == 'drone':
print('Commencing Drone Actions')
robot.say_text('Drone', duration_scalar=0.7).wait_for_completed()
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()
robot.pickup_object(cubes[0], num_retries=3).wait_for_completed()
robot.drive_straight(distance_mm(100.0), speed_mmps(500.0)).wait_for_completed()
robot.place_object_on_ground_here(cubes[0], num_retries=3).wait_for_completed()
robot.drive_straight(distance_mm(-100.0), speed_mmps(500.0)).wait_for_completed()
continue
elif state == 'inspection':
print('Commencing Inspection Actions')
robot.say_text('Inspection', duration_scalar=0.7).wait_for_completed()
for counter in range(4):
action1 = robot.drive_straight(distance_mm(200), speed_mmps(40), 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()
robot.turn_in_place(degrees(90)).wait_for_completed()
continue
elif state == 'order':
print('Commencing Order Actions')
robot.say_text('Order', duration_scalar=0.7).wait_for_completed()
robot.drive_wheels(50.0, 120.0, duration=8.2)
continue
# elif state == 'plane':
# robot.say_text('Plane State').wait_for_completed()
# continue
#
# elif state == 'truck':
# robot.say_text('Truck State').wait_for_completed()
# continue
#
# elif state == 'hands':
# robot.say_text('Hands State').wait_for_completed()
# continue
#
# elif state == 'place':
# robot.say_text('Place State').wait_for_completed()
# continue
#
elif state == 'none':
continue
def cozmo_right_turn(robot: cozmo.robot.Robot):
# Move and take a left
robot.turn_in_place(degrees(-90)).wait_for_completed()
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
robot.go_to_pose(Pose(100, 100, 0, angle_z=degrees(45)),
relative_to_robot=True).wait_for_completed()
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
def turnAround(robot):
robot.turn_in_place(degrees(180)).wait_for_completed()
async def look(self, angle):
handle = self.robot.set_head_angle(degrees(angle), in_parallel=True)
await handle.wait_for_completed()
pf = self.robot.world.particle_filter
self.robot.loop.call_later(0.1, pf.look_for_new_landmarks)
self.report_pose()
async def set_down_cube(self):
print("-" * 10 + "SETTING DOWN CUBE" + "-" * 10)
await self.robot.set_lift_height(0.0).wait_for_completed()
await self.robot.set_head_angle(degrees(3)).wait_for_completed()
print("-" * 20)
def cozmo_program(robot: cozmo.robot.Robot):
global face_images
global gaze_type
global gaze_side
get_in_position(robot)
# load some images and convert them for display cozmo's face
setup_images("eyes_image/%s.png" % (gaze_type))
setup_images("eyes_image/%s.png" % (gaze_type))
for i in range(1, 7, 1):
#print("close %d" %i)
setup_images("%s/%s%d.png" % (gaze_type, gaze_side, i))
for i in range(6, 0, -1):
#print("open %d" %i)
setup_images("%s/%s%d.png" % (gaze_type, gaze_side, i))
# display each image on Cozmo's face for duration_s seconds (Note: this
# is clamped at 30 seconds max within the engine to prevent burn-in)
# repeat this num_loops times
num_loops = 2 # Increase the number of blinks here. This is 5 blinks in a loop
duration_s = 0.02 # Increase time here to make it slower
#robot.play_audio(cozmo.audio.AudioEvents.Sfx_Flappy_Increase)
print("Press CTRL-C to quit (or wait %s seconds to complete)" %
int(num_loops * duration_s))
ts = time.time()
print(ts)
#for _ in range(num_loops):
face_to_follow = None
while True:
turn_action = None
if face_to_follow:
# start turning towards the face
turn_action = robot.turn_towards_face(face_to_follow,
in_parallel=True)
robot.display_oled_face_image(face_images[1],
5000.0,
in_parallel=True)
if not (face_to_follow and face_to_follow.is_visible):
# find a visible face, timeout if nothing found after a short while
robot.display_oled_face_image(face_images[1],
10000.0,
in_parallel=True)
try:
robot.display_oled_face_image(face_images[1],
10000.0,
in_parallel=True)
face_to_follow = robot.world.wait_for_observed_face(timeout=30)
except asyncio.TimeoutError:
print("Didn't find a face")
robot.display_oled_face_image(face_images[1],
10000.0,
in_parallel=True)
#return
if turn_action:
# Complete the turn action if one was in progress
turn_action.wait_for_completed()
time.sleep(.1)
for image in face_images:
robot.display_oled_face_image(image, duration_s * 1000.0)
time.sleep(duration_s)
robot.display_oled_face_image(face_images[-1], 2000.0)
time.sleep(2)
if keyboard.is_pressed("t"):
print("You pressed t")
ts = time.time()
print(ts)
if keyboard.is_pressed("w"):
print("You pressed w")
ts = time.time()
print(ts)
action1 = robot.say_text("Yeaaaaaaaaaaaaaahhh",
voice_pitch=1,
in_parallel=True)
action2 = robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE,
in_parallel=True)
action3 = robot.set_lift_height(0.0, in_parallel=True)
action4 = robot.display_oled_face_image(face_images[6],
5000.0,
in_parallel=True)
action5 = robot.turn_in_place(degrees(360), in_parallel=True)
break
if keyboard.is_pressed("l"):
print("You pressed l")
ts = time.time()
print(ts)
action1 = robot.say_text("Nooooooooh",
voice_pitch=-1,
in_parallel=True)
action2 = robot.set_head_angle(cozmo.robot.MIN_HEAD_ANGLE,
in_parallel=True)
action3 = robot.set_lift_height(1.0, in_parallel=True)
action4 = robot.display_oled_face_image(face_images[6],
5000.0,
in_parallel=True)
action5 = robot.drive_straight(distance_mm(-50),
speed_mmps(10),
in_parallel=True)
break
robot.display_oled_face_image(face_images[6], 5000.0)
while True:
for image in face_images:
robot.set_lift_height(0.0, in_parallel=True)
robot.display_oled_face_image(image, duration_s * 1000.0)
time.sleep(duration_s)
robot.display_oled_face_image(face_images[-1], 2000.0)
time.sleep(2)
if keyboard.is_pressed("q"):
print("You pressed q")
ts = time.time()
print(ts)
break
async def run(robot: cozmo.robot.Robot):
robot.world.image_annotator.annotation_enabled = False
robot.world.image_annotator.add_annotator('box', BoxAnnotator)
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
fixed_gain, exposure, mode = 3.90, 67, 0
try:
await robot.set_head_angle(degrees(-5)).wait_for_completed()
while True:
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage,
timeout=30) #get camera image
if event.image is not None:
image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_BGR2RGB)
if mode == 1:
robot.camera.enable_auto_exposure = True
else:
robot.camera.set_manual_exposure(exposure, fixed_gain)
#find the cube
cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER)
print(cube)
i = 0
while not cube and i < 5:
cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER)
# print(cube)
BoxAnnotator.cube = cube
i += 1
################################################################
# Todo: Add Motion Here
################################################################
if cube and cube[0] < 120:
action = robot.turn_in_place(radians(0.1))
await action.wait_for_completed()
elif cube and cube[0] > 180:
action = robot.turn_in_place(radians(-0.1))
await action.wait_for_completed()
elif cube and cube[2] < 120:
action = robot.drive_straight(distance_mm(30),
Speed(1000),
should_play_anim=False)
await action.wait_for_completed()
elif cube and cube[2] >= 120:
print("stop")
else:
action = robot.turn_in_place(radians(0.3))
await action.wait_for_completed()
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
def default_position_upon_start(robot: cozmo.robot.Robot):
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.set_lift_height(height=0).wait_for_completed()
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
def inspection_helper(robot):
robot.set_lift_height(1.0, duration=2.5, in_parallel=True)
robot.drive_straight(distance_mm(250), speed_mmps(75), in_parallel=True).wait_for_completed()
robot.set_lift_height(0, duration=2.5, in_parallel=True)
robot.turn_in_place(degrees(90), in_parallel=True).wait_for_completed()
def hesitate_long(robot: cozmo.robot.Robot):
trigger = cozmo.anim.Triggers.MeetCozmoFirstEnrollmentSayName #PatternGuessThinking #CodeLabThinking
play_animation(robot,trigger)
robot.turn_in_place(degrees(10)).wait_for_completed()
def turn(robot, step_number):
robot.turn_in_place(degrees(turns[step_number - 1])).wait_for_completed()
def run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
robot.go_to_pose(Pose(100, 100, 0, angle_z=degrees(45)), relative_to_robot=True).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
robot.turn_in_place(degrees(180), speed=degrees(5)).wait_for_completed()
def cozmo_drive_square(robot: cozmo.robot.Robot):
# Make a small square
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 turn_around():
global robot
robot.turn_in_place(degrees(-180)).wait_for_completed()
return
