robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
robot.turn_in_place(degrees(135)).wait_for_completed()
# 3
robot.drive_straight(distance_mm(70), speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(-135)).wait_for_completed()
# 4
robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
# 5
robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
# 6
robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
# 7
robot.turn_in_place(degrees(-135)).wait_for_completed()
robot.drive_straight(distance_mm(70), speed_mmps(50)).wait_for_completed()
# 8
robot.turn_in_place(degrees(135)).wait_for_completed()
robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
# Play the ascending notes
robot.play_song(notes, loop_count=1).wait_for_completed()
cozmo.run_program(cozmo_path, use_viewer=True, force_viewer_on_top=True)
if distance < 54 or ball[2] > 98:
print("Previous state: drive, new state: hit ball")
myCozmo.ballInReach()
# print(state)
# print('\a')
else:
await robot.drive_wheels(leftSpeed, rightSpeed)
elif myCozmo.state == "hitball":
await robot.set_lift_height(
0.5, in_parallel=True).wait_for_completed()
await robot.set_lift_height(
0.1, in_parallel=True).wait_for_completed()
print("Previous state: hit ball, new state: drive")
myCozmo.ballDetected()
# print('\a')
# isNotFinished = 0
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
if __name__ == '__main__':
cozmo.run_program(run, use_viewer=True, force_viewer_on_top=True)
def run(self):
cozmo.run_program(cozmoBehavior)
robot.set_head_angle(degrees(ihi)).wait_for_completed() time.sleep(0.25) imageB = robot.world.latest_image.raw_image imageB.save((cpath + "\\outB.png"), quality=imq) robot.turn_in_place(degrees(30)).wait_for_completed() cviA = cv2.imread((cpath + '\\outA.png'),0) cviB = cv2.imread((cpath + '\\outB.png'),0) disparity = cv2.StereoBM_create(numDisparities=16, blockSize=15).compute(cviA,cviB) pyplot.imshow(disparity,'gray') pyplot.show() time.sleep(60) time.sleep(0.25) imageD = robot.world.latest_image.raw_image imageD.save((cpath + "\\outD.png"), quality=imq) robot.set_head_angle(degrees(ilo)).wait_for_completed() time.sleep(0.25) imageC = robot.world.latest_image.raw_image imageC.save((cpath + "\\outC.png"), quality=imq) # Run Function cozmo.robot.Robot.drive_off_charger_on_connect = False cozmo.run_program(map3d)
return final
def find_relative_cube_pose(robot: cozmo.robot.Robot):
'''Looks for a cube while sitting still, prints the pose of the detected cube
in world coordinate frame and relative to the robot coordinate frame.'''
robot.move_lift(-3)
robot.set_head_angle(degrees(0)).wait_for_completed()
cube = None
while True:
try:
cube = robot.world.wait_for_observed_light_cube(timeout=30)
if cube:
print("Robot pose: %s" % robot.pose)
print("Cube pose: %s" % cube.pose)
print("Cube pose: %s" % cube.pose)
final = get_relative_pose(cube.pose, robot.pose)
print("Cube pose in the robot coordinate frame: %s" % final)
robot.go_to_pose((final),
relative_to_robot=True).wait_for_completed()
except asyncio.TimeoutError:
print("Didn't find a cube")
if __name__ == '__main__':
cozmo.run_program(find_relative_cube_pose)
cube2.set_lights(cozmo.lights.red_light)
cube3.set_lights(cozmo.lights.red_light)
for i in range(5):
robot.say_text("Power Cube").wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.red_light)
cube1.set_lights(cozmo.lights.red_light)
cube2.set_lights(cozmo.lights.green_light)
cube3.set_lights(cozmo.lights.green_light)
robot.move_head(1)
robot.move_lift(1)
robot.say_text(str(i + 1)).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.white_light)
cube1.set_lights(cozmo.lights.white_light)
cube2.set_lights(cozmo.lights.blue_light)
cube3.set_lights(cozmo.lights.blue_light)
robot.move_head(-1)
robot.move_lift(-3)
time.sleep(2)
cozmo.run_program(light)
# robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
# robot.move_head(1) Moves robot head all the way up 45 degrees
# robot.move_head(-1) Moves robot head all the way down -45 degrees
# robot.move_lift(1) All the way up
# robot.move_lift(-1) all the way down
# set_light_corners(light1, light2, light3, light4)
tagBuffer = []
def on_key(event):
global tagBuffer
keycode = event.GetKeyCode()
tagBuffer.append(chr(keycode))
if len(tagBuffer) == 10:
printTag(''.join(tagBuffer))
tagBuffer = []
# Draw the interface and display it on the screen
panel.Bind(wx.EVT_PAINT, on_paint)
panel.Bind(wx.EVT_KEY_DOWN, on_key)
def new_cozmo_pgm(czmo):
frame.Show(True)
global robot
rbt = CozmoRobot(czmo)
robot = rbt
app.MainLoop()
print('exiting')
print('hi')
cozmo.run_program(new_cozmo_pgm)
import asyncio
import time
import cozmo
def light_when_face(robot: cozmo.robot.Robot):
robot.move_lift(-3)
robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
face = None
while True:
if face and face.is_visible:
robot.set_all_backpack_lights(cozmo.lights.green_light)
robot.say_text("Hello, I can see you!").wait_for_completed()
else:
robot.set_backpack_lights_off()
try:
face = robot.world.wait_for_observed_face(timeout=30)
robot.say_text("I cannot see you now!").wait_for_completed()
except asyncio.TimeoutError:
return
cozmo.run_program(light_when_face, use_viewer=True, force_viewer_on_top=True)
def inspection(robot: cozmo.robot.Robot):
# Have the robot drive in a square, where each side of the square is approximately 20 cm.
# While driving, the robot must continuously raise and lower the lift, but do so slowly (2-3 seconds
# to complete lowering or raising the lift). Simultaneously, the robot must say, “I am not a spy”.
# Lower the lift at the end of the behavior, and return to the Idle state.
robot.say_text(str('inspection')).wait_for_completed()
i = 0
while i < 4:
if robot.lift_ratio > .5:
robot.move_lift(-0.33)
else:
robot.move_lift(0.33)
robot.say_text(str('I am not a spy'))
robot.drive_straight(distance_mm(200),
speed_mmps(77),
in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(90), in_parallel=True).wait_for_completed()
i = i + 1
def main(robot: cozmo.robot.Robot):
run(robot)
idle(robot)
cozmo.run_program(main)
import cozmo
async def pop_a_wheelie(robot: cozmo.robot.Robot):
print("Cozmo is waiting until he sees a cube")
cube = await robot.world.wait_for_observed_light_cube()
print("Cozmo found a cube, and will now attempt to pop a wheelie on it")
current_action = robot.pop_a_wheelie(cube, num_retries=2)
await current_action.wait_for_completed()
if current_action.has_failed:
print("Error")
else:
from cozmo.util import degrees, distance_mm, speed_mmps
def cozmo_program(robot: cozmo.robot.Robot):
# Drive forwards for 150 millimeters at 50 millimeters-per-second.
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
# Turn 90 degrees to the left.
# Note: To turn to the right, just use a negative number.
robot.turn_in_place(degrees(90)).wait_for_completed()
cozmo.run_program(pop_a_wheelie)
cozmo_action.setup_ScorePlan(SCORE_SETS["score_set%s" % (args.scoreSet)])
configParam["participantID"] = args.participantID
if args.participantID <= 0:
print("ERROR! Cannot proceed without participant ID.")
exit(0)
if args.practice:
configParam["practice"] = True
cozmo_action.set_practice(True)
else:
configParam["practice"] = False
if args.singleScreen:
cozmo_action.set_singleScreen()
if configParam and log_path:
add_file_logger(log_path, cozmo_action, args.practice)
return configParam
if __name__ == "__main__":
cozmo_action = CozmoPlayerActions()
if handle_selection(cozmo_action):
cozmo.run_program(cozmo_tap_game)
del cozmo_action
exit(0)
async def run(robot: cozmo.robot.Robot):
await look_around_until_converge(robot)
print("LAST POSE IS:", last_pose)
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()
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()
cozmo.run_program(cozmo_program, use_viewer=True, force_viewer_on_top=True)
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, goal_pose
global grid, gui, pf
global camera_settings
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
await robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
camera_settings = np.array([
[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]
], dtype=np.float)
###################
# pickup point
#pickup_node = Node((153, 240))
# dropoff_node = Node((544, 344))
# localize the robot
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("LAST POSE IS:", last_pose)
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
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=cozmo.util.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()
drop_off_directions = [(3, 4.5, 0), (21.75, 4.5, 90), (21.75, 13.75, 90)]
pick_up_directions = [(21.75, 4.5, 90), (3, 4.5, 0), (4.5, 20)]
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)
def CozmoWarehouseWorker:
def __init__(self, robot, current_arena_pose, ):
self.current_arena_pose = current_arena_pose
self.last_robot_pose = robot.pose
self.robot = robot
async def execute_directions(directions):
print("Robot is at: ", self.robot.pose)
await self.robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to X: ", self.robot.pose)
await self.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 direction: ", self.robot.pose)
await self.robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to Y: ", self.robot.pose)
await self.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 direction: ", self.robot.pose)
async def localize(self):
# globals
global grid, gui, pf
global camera_settings
# reset variables
conf = False
last_pose = cozmo.util.Pose(0,0,0,angle_z=cozmo.util.Angle(degrees=0))
pf = ParticleFilter(grid)
# reset lift and head
await self.robot.set_lift_height(0.0).wait_for_completed()
await self.robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
while not conf:
# move a little
last_pose = self.robot.pose
await self.robot.turn_in_place(angle=cozmo.util.Angle(degrees=20)).wait_for_completed()
odometry = compute_odometry(last_pose, self.robot.pose)
detected_markers, camera_image = await marker_processing(self.robot, camera_settings)
# update, motion, and measurment with the odometry and marker data
curr_x, curr_y, curr_h, conf = pf.update(odometry, detected_markers)
# update gui
gui.show_particles(pf.particles)
gui.show_mean(curr_x, curr_y, curr_h)
gui.show_camera_image(camera_image)
gui.updated.set()
self.current_arena_pose = cozmo.util.Pose(curr_x , curr_y, 0, angle_z=cozmo.util.Angle(degrees=curr_h))
def compute_odometry(self, cvt_inch=True):
'''
Compute the odometry given the current pose of the robot (use robot.pose)
Input:
- curr_pose: a cozmo.robot.Pose representing the robot's current location
- cvt_inch: converts the odometry into grid units
Returns:
- 3-tuple (dx, dy, dh) representing the odometry
'''
last_x, last_y, last_h = self.last_robot_pose.position.x, self.last_robot_pose.position.y, \
self.last_robot_pose.rotation.angle_z.degrees
curr_x, curr_y, curr_h = self.robot.pose.position.x, self.robot.pose.position.y, \
self.robot.pose.rotation.angle_z.degrees
dx, dy = rotate_point(curr_x-last_x, curr_y-last_y, -last_h)
if cvt_inch:
dx, dy = dx / grid.scale, dy / grid.scale
return (dx, dy, diff_heading_deg(curr_h, last_h))
async def marker_processing(self, show_diagnostic_image=False):
'''
Obtain the visible markers from the current frame from Cozmo's camera.
Since this is an async function, it must be called using await, for example:
markers, camera_image = await marker_processing(robot, camera_settings, show_diagnostic_image=False)
Input:
- robot: cozmo.robot.Robot object
- camera_settings: 3x3 matrix representing the camera calibration settings
- show_diagnostic_image: if True, shows what the marker detector sees after processing
Returns:
- a list of detected markers, each being a 3-tuple (rx, ry, rh)
(as expected by the particle filter's measurement update)
- a PIL Image of what Cozmo's camera sees with marker annotations
'''
global grid
# Wait for the latest image from Cozmo
image_event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# Convert the image to grayscale
image = np.array(image_event.image)
image = color.rgb2gray(image)
# Detect the markers
markers, diag = detect.detect_markers(image, camera_settings, include_diagnostics=True)
# Measured marker list for the particle filter, scaled by the grid scale
marker_list = [marker['xyh'] for marker in markers]
marker_list = [(x/grid.scale, y/grid.scale, h) for x,y,h in marker_list]
# Annotate the camera image with the markers
if not show_diagnostic_image:
annotated_image = image_event.image.resize((image.shape[1] * 2, image.shape[0] * 2))
annotator.annotate_markers(annotated_image, markers, scale=2)
else:
diag_image = color.gray2rgb(diag['filtered_image'])
diag_image = Image.fromarray(np.uint8(diag_image * 255)).resize((image.shape[1] * 2, image.shape[0] * 2))
annotator.annotate_markers(diag_image, markers, scale=2)
annotated_image = diag_image
return marker_list, annotated_image
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()
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cozmo
from cozmo.util import degrees, distance_mm, speed_mmps
def cozmo_move(robot: cozmo.robot.Robot):
robot.say_text("Halo, allemaal!").wait_for_completed()
robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
for _ in range(12):
if _ % 2 == 0:
robot.set_all_backpack_lights(cozmo.lights.green_light)
else:
robot.set_all_backpack_lights(cozmo.lights.red_light)
robot.turn_in_place(degrees(30)).wait_for_completed()
robot.set_all_backpack_lights(cozmo.lights.off_light)
robot.drive_straight(distance_mm(-150),
speed_mmps(50)).wait_for_completed()
robot.say_text("Show is done!").wait_for_completed()
robot.play_anim(name="anim_poked_giggle").wait_for_completed()
cozmo.run_program(cozmo_move)
########################## Main ##########################
robot = None
def init_robot(cozmo_robot: cozmo.robot.Robot):
global robot
robot = cozmo_robot
def execute_procedure():
low_battery = check_battery()
if not low_battery:
clean_up_cubes()
get_on_charger()
return
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
if __name__ == "__main__":
cozmo.robot.Robot.drive_off_charger_on_connect = False
cozmo.run_program(cozmo_program,use_viewer=True,use_3d_viewer=False)
# Cozmo returns cube to user
robot.go_to_object(charger, distance_mm(100)).wait_for_completed()
robot.say_text("Is this one the right one?").wait_for_completed()
# Object is confirmed
confirmation1 = input("Is this the right one? Y/N: ")
if confirmation1 == "Y":
robot.say_text("Yay")
else:
robot.say_text("Well you're getting it anyway.")
action = robot.place_object_on_ground_here(cube_wanted)
action.wait_for_completed()
# get dat fist bump
robot.say_text("Do you want me to fetch anything else")
# user says no
# cozmo returns to base
# wait for five minutes of inactivity
robot.say_text(
"I'm going to have a nap now, let me know if you need anything?")
# cozmo returns to cradle and sleeps
cozmo.run_program(cozmo_program, use_viewer=True)
m = re.search("_", f)
labels[i] = f[len(dir):m.start()]
return(data,labels)
def extract_image_features(self, data):
feature_data = []
for i in data:
feature_data.append(feature.hog(i, orientations = 9, pixels_per_cell = (32, 32),
cells_per_block = (6, 6), transform_sqrt = True,
block_norm="L1-sqrt"))
return(feature_data)
def train_classifier(self, train_data, train_labels):
self.classifer = svm.SVC(kernel = 'linear')
self.classifer.fit(train_data,train_labels)
def predict_labels(self, data):
# Please do not modify the header
# predict labels of test data using trained model in self.classifier
# the code below expects output to be stored in predicted_labels
predicted_labels = self.classifer.predict(data)
return predicted_labels
cozmo.run_program(cozmo_idle)
def run(self):
self._import_requirement_or_import()
cozmo.run_program(self.cozmo_program)
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, goal_pose
global grid, gui, pf
global camera_settings
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
await robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
camera_settings = np.array([
[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]
], dtype=np.float)
###################
# pickup point
#pickup_node = Node((153, 240))
# dropoff_node = Node((544, 344))
# localize the robot
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("LAST POSE IS:", last_pose)
#print("COZMO CONVERTED THAT TO A START AT:", cosimo.last_arena_pose)
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=cozmo.util.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()
drop_off_directions = [(3, 4.5, 0), (21.75, 4.5, 90), (21.75, 13.75, 90)]
pick_up_directions = [(21.75, 4.5, 90), (3, 4.5, 0), (4.5, 20)]
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)
def CozmoWarehouseWorker:
def __init__(self, robot, current_arena_pose, ):
self.current_arena_pose = current_arena_pose
self.last_robot_pose = robot.pose
self.robot = robot
async def execute_directions(directions):
print("Robot is at: ", self.robot.pose)
await self.robot.turn_in_place(angle=directions.rotation.angle_z).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to X: ", self.robot.pose)
await self.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 direction: ", self.robot.pose)
await self.robot.turn_in_place(angle=cozmo.util.Angle(degrees=90)).wait_for_completed()
print("ROBOT is at AFTER TURNING to be parallel to Y: ", self.robot.pose)
await self.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 direction: ", self.robot.pose)
async def localize(self):
# globals
global grid, gui, pf
# reset variables
conf = False
last_pose = cozmo.util.Pose(0,0,0,angle_z=cozmo.util.Angle(degrees=0))
pf = ParticleFilter(grid)
# reset lift and head
await self.robot.set_lift_height(0.0).wait_for_completed()
await self.robot.set_head_angle(cozmo.util.degrees(3)).wait_for_completed()
while not conf:
# move a little
last_pose = self.robot.pose
await self.robot.turn_in_place(angle=cozmo.util.Angle(degrees=20)).wait_for_completed()
odometry = compute_odometry(last_pose, self.robot.pose)
detected_markers, camera_image = await marker_processing(self.robot, camera_settings)
# update, motion, and measurment with the odometry and marker data
curr_x, curr_y, curr_h, conf = pf.update(odometry, detected_markers)
# update gui
gui.show_particles(pf.particles)
gui.show_mean(curr_x, curr_y, curr_h)
gui.show_camera_image(camera_image)
gui.updated.set()
self.current_arena_pose = cozmo.util.Pose(curr_x , curr_y, 0, angle_z=cozmo.util.Angle(degrees=curr_h))
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 main():
cozmo.run_program(program)
CustomObjectMarkers.Circles3, # back
CustomObjectMarkers.Circles4, # top
CustomObjectMarkers.Circles5, # bottom
CustomObjectMarkers.Triangles2, # left
CustomObjectMarkers.Triangles3, # right
60,
140,
100,
30,
50,
True)
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")
while True:
time.sleep(0.1)
cozmo.run_program(custom_objects, use_3d_viewer=True, use_viewer=True)
# You may obtain a copy of the License in the file LICENSE.txt or at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
'''Tell Cozmo to roll a cube that is placed in front of him.
This example demonstrates Cozmo driving to and rolling a cube.
You must place a cube in front of Cozmo so that he can see it.
'''
import cozmo
async def roll_a_cube(robot: cozmo.robot.Robot):
print("Cozmo is waiting until he sees a cube")
cube = await robot.world.wait_for_observed_light_cube()
# Cozmo will approach the cube he has seen and roll it
# check_for_object_on_top=True enforces that Cozmo will not roll cubes with anything on top
await robot.roll_cube(cube, check_for_object_on_top=True,
num_retries=2).wait_for_completed()
cozmo.run_program(roll_a_cube)
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)
def run(self):
# Please refrain from enabling use_viewer since it uses tk, which must be in main thread
cozmo.run_program(CozmoPlanning,use_3d_viewer=False, use_viewer=False)
stopevent.set()
import sys
import time
import cozmo
from PIL import Image
from cozmo.util import degrees, distance_mm, speed_mmps
def setup_position(robot: cozmo.robot.Robot):
if robot.lift_height.distance_mm > 45:
with robot.perform_off_charger():
robot.set_lift_height(0,20).wait_for_completed()
resampling_mode = Image.NEAREST
image_name= "C:/Users/MI700T/Documents/cozmo_sdk_examples_1.2.1/face_images/Cozmo_Primary_Gradient_Logo-2.png"
image = Image.open(image_name)
resized_image = image.resize(cozmo.oled_face.dimensions(), resampling_mode)
face_image = cozmo.oled_face.convert_image_to_screen_data(resized_image,invert_image=False)
duration_s = 3.0
robot.display_oled_face_image(face_image, duration_s * 1000.0)
time.sleep(duration_s)
cozmo.run_program(setup_position)
elif action == DONKEY_COZMO_ACTION_HOLD:
print('*** HOLD ***')
cozmo_hold(robot)
elif action == DONKEY_COZMO_ACTION_SHORT_FORWARD:
print('*** Go Short FORWARD ***')
cozmo_go_short_forward(robot)
prev_action = decided_action
# time.sleep(duration_s)
except KeyboardInterrupt:
print('Keyboard Interrupt!!')
print('Exit Cozmo SDK')
cv2.destroyAllWindows()
pass
# Initialize Neural Network
model = czCnn()
#optimizer = optimizers.Adam()
#optimizer.setup(model)
chainer.config.train = False
serializers.load_npz(DONKEY_COZMO_MDLFILE, model)
# GPU setup
gpu_device = 0
cuda.get_device(gpu_device).use()
model.to_gpu(gpu_device)
# Run main code
cozmo.run_program(cozmo_donkey_run)
for i in range(5):
cols = [white_light] * (4 - i) + [off_light] * i
self.set_light_corners(*cols)
await asyncio.sleep(.5)
async def flair_correct_tap(self):
'''Runs a fast _chaser when the player taps correctly.'''
self.start_light_chaser(0.1)
await asyncio.sleep(2)
self.stop_light_chaser()
async def flair_incorrect_tap(self):
'''Blinks red when the player taps incorrectly.'''
for _ in range(4):
self.set_lights(red_light)
await asyncio.sleep(.2)
self.set_lights(off_light)
await asyncio.sleep(.2)
# Make sure World knows how to instantiate the BlinkyCube subclass
cozmo.world.World.light_cube_factory = BlinkyCube
async def cozmo_program(robot: cozmo.robot.Robot):
game = QuickTapGame(robot)
await game.run()
cozmo.run_program(cozmo_program)
linearspeed = 55
my_drive_straight(robot, distance, linearspeed)
def run(robot: cozmo.robot.Robot):
print("***** Front wheel radius: " + str(get_front_wheel_radius(robot)))
print("***** Distance between wheels: " +
str(get_distance_between_wheels(robot)))
## Example tests of the functions
cozmo_drive_straight(robot, 62, 50)
cozmo_turn_in_place(robot, 60, 30)
cozmo_go_to_pose(robot, 100, 100, 45)
rotate_front_wheel(robot, 90)
my_drive_straight(robot, 62, 50)
my_turn_in_place(robot, 90, 30)
my_go_to_pose1(robot, 100, 100, 45)
my_go_to_pose2(robot, 100, 100, 45)
my_go_to_pose3(robot, 100, 100, 45)
if __name__ == '__main__':
cozmo.run_program(run)
def run():
cozmo.run_program(mainLoop, use_viewer=False, force_viewer_on_top=False)
line = f.readline()
# check if line is not empty
if not line:
break
#strip out the \n at the end
l1 = line.rstrip('\n')
#split by ":"
l2 = l1.split(":")
#print(l2)
if l2[0] == 'human':
key = l2[1]
#print('human : ',key)
else:
pair = l2[1]
#print('key : ',pair)
dictionary[key] = pair
print(dictionary)
###
addEntry(log, "Conversation started.")
print("######################")
print("#Type 'quit' to exit.#")
print("######################")
cozmo.run_program(mainLoop)
offset = zeroOffset + inputOffset
my_object_instance = None
while my_object_instance is None:
evt = robot.wait_for(cozmo.objects.EvtObjectObserved, timeout=None)
if isinstance(evt.obj, CustomObject):
my_object_instance = evt.obj
# find the vector from the object to the robot
object_to_robot_vec = robot.pose.position - my_object_instance.pose.position
# normalize the vector (so it's length 1.0)
object_to_robot_vec_dist = math.sqrt(
(object_to_robot_vec.x * object_to_robot_vec.x) +
(object_to_robot_vec.y * object_to_robot_vec.y) +
(object_to_robot_vec.z * object_to_robot_vec.z))
normalized_object_to_robot_vec = object_to_robot_vec * (
1.0 / object_to_robot_vec_dist)
# we can now add X times this vector to the objects position and it will push us X mm towards the robot
# e.g. lets push it 50mm back (about 1 Cozmo length)
offset_vec = (normalized_object_to_robot_vec * offset)
target_pos = my_object_instance.pose.position + offset_vec
target_pose = my_object_instance.pose
# change the position (set it to the new target_pos we calculated above)
target_pose._position = target_pos
robot.go_to_pose(target_pose).wait_for_completed()
time.sleep(0.5)
cozmo.run_program(workloop)
print('Check detected!')
break
time.sleep(.1)
robot.set_backpack_lights_off()
return
def init(r: cozmo.robot.Robot):
global robot,delta
robot = r
setMaxAngle()
robot.set_head_angle(cozmo.util.Angle(degrees=0), accel=10.0, max_speed=10.0,
duration=0.0, warn_on_clamp=True, in_parallel=False, num_retries=0).wait_for_completed()
robot.pose.invalidate()
delta = robot.pose_pitch.degrees
return
def level(robot: cozmo.robot.Robot):
global LIFT_POSITION
init(robot)
robot.set_lift_height(0.8).wait_for_completed()
LIFT_POSITION = robot._lift_position.ratio
setBackpackColors()
robot.set_lift_height(0.0).wait_for_completed()
celebrate(robot)
return
if __name__ == '__main__':
cozmo.run_program(level,use_viewer=False,use_3d_viewer=False)
