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 #set/import initial grid conditions, initialize variables. set a default initial goal to be the center of the map. botPoint = (3, 2) grid.setStart(botPoint) grid.addGoal((13, 9)) cubes = set() goalCubePos = None goalScalar = -1 cubeFootprint = [-2, -1, 0, 1, 2] scale = 25 width = 26 height = 18 repath = False #reset the robot for its journey robot.image_stream_enabled = True robot.set_head_angle(degrees(0)).wait_for_completed() robot.move_lift(-3) speed = 27 previousHeading = 0 while not stopevent.is_set(): #if repath == True: grid.clearStart() grid.setStart(botPoint) # repath = False; #run astar search astar(grid, heuristic) path = grid.getPath() #where are we now? if len(path) - path.index(botPoint) == 1: endPoint = botPoint else: endPoint = path[path.index(botPoint) + 1] print("cur: {0}".format(botPoint)) print("end: {0}".format(endPoint)) #find the cube(s) for cube in robot.world.visible_objects: cubeID = cube.object_id #if this is a 'new' cube... if cubeID not in cubes: cubex = cube.pose.position.x cubey = cube.pose.position.y #normalize the position to the size of the grid and make sure that any cubes are located INSIDE the grid cubex = min(math.ceil((cubex) / scale) + 2, width) cubey = min(math.ceil((cubey) / scale) + 2, height) #mark grid squares as obstacles, based on the normalized cube position and the footprint of the cube for xi in cubeFootprint: for yi in cubeFootprint: curx = cubex + xi cury = cubey + yi #only mark a grid square as containing an obstacle if the square is actually inside the grid if curx >= 0 and curx <= width and cury >= 0 and cury <= height: grid.addObstacle((curx, cury)) grid.clearVisited() grid.setStart(endPoint) print("Found a cube: {0}".format(cubeID)) #add this cube to the master set of cubes cubes.add(cubeID) #special casing if we realize that this is the goal cube #if robot.world.light_cubes[cozmo.objects.LightCube1Id].object_id == cubeID: if cubeID == 1: goalCubePos = (cubex, cubey) #adjust the bot's final position based on the rotational position of the cube so that the bot can approach it properly theta = cube.pose.rotation.angle_z.radians + math.pi goalx = cubex + (round(math.cos(theta)) * math.floor((len(cubeFootprint) / 2) + 1)) goaly = cubey + (round(math.sin(theta)) * math.floor((len(cubeFootprint) / 2) + 1)) #get rid of any stale goals (e.g. center of the grid) and add the goal position print("Found Goal Cube at: " + str(goalx) + ", " + str(goaly)) grid.clearGoals() grid.addGoal((goalx, goaly)) print(grid.getGoals()) repath = True grid.clearStart() grid.setStart(botPoint) grid.clearVisited() # run astar search astar(grid, heuristic) path = grid.getPath() # where are we now? if len(path) - path.index(botPoint) == 1: endPoint = botPoint else: endPoint = path[path.index(botPoint) + 1] print("bot: {0}".format(botPoint)) print("end: {0}".format(endPoint)) #if repath == False: #get the vector of displacement between the endpoint and the bot's current point dispVector = (endPoint[0] - botPoint[0], endPoint[1] - botPoint[1]) #get a scalar measure of how far the endpoint is from the bot's current point dispScalar = math.sqrt( math.pow(dispVector[0], 2) + math.pow(dispVector[1], 2)) #atan2(y,x) gives the absolute angle of a vector, so we can use it to get the best heading heading = math.atan2(dispVector[1], dispVector[0]) #we turn based on the difference between the desired heading and the bot's previous heading turn = heading - previousHeading previousHeading = heading if dispScalar != 0: #turn and move the bot robot.turn_in_place(radians(turn)).wait_for_completed() robot.drive_wheels(speed, speed, duration=2 * dispScalar) if goalCubePos is not None: goalVector = (goalCubePos[0] - botPoint[0], goalCubePos[1] - botPoint[1]) goalScalar = math.sqrt( math.pow(goalVector[0], 2) + math.pow(goalVector[1], 2)) #if we've reached the center of the grid and still haven't found the goal cube, slowly turn in place (until we find it) if dispScalar == 0 and goalCubePos is None: robot.turn_in_place(degrees(15)).wait_for_completed() print("Turning in place til we find the cube") #if we HAVE found the goal cube, and we've reached the endpoint... elif dispScalar == 0 and goalCubePos is not None and goalScalar == 0: #get the heading and turn to face the cube heading = math.atan2(goalCubePos[1] - endPoint[1], goalCubePos[0] - endPoint[0]) turn = heading - previousHeading robot.turn_in_place(radians(turn)).wait_for_completed() print("Reached goal cube") #and we're done! robot.set_all_backpack_lights(cozmo.lights.blue_light) robot.turn_in_place(degrees(720)) robot.play_audio(cozmo.audio.AudioEvents.SfxGameWin) time.sleep(5) break #update the bot's point to be the endpoint it just navigated to botPoint = endPoint
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 robot.image_stream_enabled = True cube1pos = None curHeading = 0 # obstacle_size = [-1, 0, 1] obstacle_size = [-2, -1, 0, 1, 2] # obstacle_size = [-3, -2, -1, 0, 1, 2, 3] gridscale = 25 scale = 27 a = (2, 2) grid.setStart(a) grid.addGoal((13, 9)) objects = set() robot.set_head_angle(radians(-0.23)).wait_for_completed() while not stopevent.is_set(): # print([ e.pose.position for e in robot.world.visible_objects]) # if world.light_cubes[cozmo.objects.LightCube1Id].is_visible astar(grid, heuristic) path = grid.getPath() # print([e for e in grid.getGoals()]) path_ind = path.index(a) if len(path) - path_ind == 1: b = a print("GOOOOOOOOAAAL") else: b = path[path_ind + 1] print("a", a, "b", b) print(path, path_ind) for e in robot.world.visible_objects: if e.object_id not in objects: xr = a[0] yr = a[1] xc = e.pose.position.x yc = e.pose.position.y # yc = yr + ((e.pose.position.x * math.sin(curHeading)) + (e.pose.position.y * math.cos(curHeading)))/scale print(e.pose.position) xc = math.ceil((xc)/gridscale) + 2 yc = math.ceil((yc)/gridscale) + 2 if xc == 27: xc = 26 if yc == 27: yc = 26 print("xr: %d, yr: %d" % (xr, yr)) print("xc: %d, yc: %d" % (xc, yc)) print(curHeading/math.pi) if xc <= 26 and yc <= 18: for i in obstacle_size: for j in obstacle_size: xci = xc + i ycj = yc + j if xci <= 26 and xci >= 0 and ycj <= 18 and ycj >= 0: grid.addObstacle((xci,ycj)) grid.clearVisited() grid.setStart(b) print(grid.getGoals()) print("Obstacle pos", xc,yc) objects.add(e.object_id) #you ge tthe idea if robot.world.light_cubes[cozmo.objects.LightCube1Id].object_id == e.object_id: cube1pos = (xc,yc) print("e id ",e.object_id) angelz = e.pose.rotation.angle_z.radians print("angelz+mod",angelz,angelz+math.pi) angelz = math.pi + angelz xc += round(math.cos(angelz)) * (len(obstacle_size)//2 + 1) yc += round(math.sin(angelz)) * (len(obstacle_size)//2 + 1) grid.clearGoals() grid.addGoal((xc, yc)) # else: # make it an obstacle # if not cubeFound: # sleep(.4) step = (b[0]-a[0], b[1]-a[1]) step_len = math.sqrt(math.pow(step[0],2) + math.pow(step[1],2)) heading = math.atan2(step[1], step[0]) turnHeading = heading - curHeading curHeading = heading robot.turn_in_place(radians(turnHeading)).wait_for_completed() # print(step_len) robot.drive_wheels(scale, scale, duration=1.4*step_len if step_len == 1 else 1.4*step_len) # grid.setStart(b) if step_len == 0 and cube1pos is None: robot.turn_in_place(radians(math.pi/16)).wait_for_completed() elif step_len == 0 and cube1pos is not None: robot.drive_wheels(0, 0, duration=1) print("cube1pos", cube1pos) heading = math.atan2(cube1pos[1] - b[1], cube1pos[0] - b[0]) print("heading", heading) print("curHeading", curHeading) turnHeading = heading - curHeading curHeading = heading robot.turn_in_place(radians(turnHeading)).wait_for_completed() break a = b