async def localize(self, turn_angle=20): print("-" * 20 + "LOCALIZING" + "-" * 20) # reset our location estimates conf = False self.current_arena_pose = Pose(0, 0, 0, angle_z=degrees(0)) self.pf = ParticleFilter(self.grid) # reset lift and head await self.robot.set_lift_height(0.0).wait_for_completed() await self.robot.set_head_angle(degrees(3)).wait_for_completed() while not conf: # move a little self.current_robot_pose = self.robot.pose await self.robot.turn_in_place(angle=degrees(turn_angle) ).wait_for_completed() odometry = self.__compute_odometry() detected_markers, camera_image = await self.__marker_processing() # update, motion, and measurment with the odometry and marker data curr_x, curr_y, curr_h, conf = self.pf.update( odometry, detected_markers) # update gui self.gui.show_particles(self.pf.particles) self.gui.show_mean(curr_x, curr_y, curr_h) self.gui.show_camera_image(camera_image) self.gui.updated.set() self.current_arena_pose = Pose(curr_x, curr_y, 0, angle_z=degrees(curr_h)) print("We localized to arena location ", self.current_arena_pose)
def find(self, number): print(number) lookaround = self.robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace) cubes = self.robot.world.wait_until_observe_num_objects(num=self.cube_number, object_type=cozmo.objects.LightCube, timeout=10) print("Found %s cubes" % len(cubes)) lookaround.stop() if number == 1: for i in cubes: if i == self.jacket: action = self.robot.pickup_object(self.jacket, num_retries=3) action.wait_for_completed() self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() self.robot.say_text("here is your jacket").wait_for_completed() action = self.robot.place_object_on_ground_here(i) action.wait_for_completed() break elif number == 2: for i in cubes: if i == self.shorts: action = self.robot.pickup_object(self.shorts, num_retries=3) action.wait_for_completed() self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() self.robot.say_text("here are your shorts").wait_for_completed() action = self.robot.place_object_on_ground_here(i) action.wait_for_completed() elif number == 3: for i in cubes: if i == self.shorts: action = self.robot.pickup_object(self.shorts, num_retries=3) action.wait_for_completed() self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() self.robot.say_text("here are your shorts").wait_for_completed() action = self.robot.place_object_on_ground_here(i) action.wait_for_completed() self.robot.turn_in_place(degrees(180)).wait_for_completed() self.find(4) break elif number == 4: for i in cubes: if i == self.sun_glasses: action = self.robot.pickup_object(self.sun_glasses, num_retries=3) action.wait_for_completed() self.robot.go_to_pose(Pose(-9.3, -173, 0, angle_z=degrees(180)), relative_to_robot=False).wait_for_completed() self.robot.say_text("here are your sun glasses").wait_for_completed() action = self.robot.place_object_on_ground_here(i) action.wait_for_completed() break else: print("unknown item")
def __init__(self): landmarks = { cube1: Pose(55, 160, 0, angle_z=degrees(90)), cube2: Pose(160, 55, 0, angle_z=degrees(0)), cube3: Pose(160, -55, 0, angle_z=degrees(0)) } pf = ParticleFilter(robot, landmarks=landmarks, sensor_model=CubeSensorModel(robot)) super().__init__(particle_filter=pf, particle_viewer=True)
def roomcreation(robot: cozmo.robot.Robot): cubesize = 50 initialPosition = 0 origin1 = robot.world.create_custom_fixed_object(Pose(-100, initialPosition - cubesize, 0, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) origin2 = robot.world.create_custom_fixed_object(Pose(-100, 0, initialPosition, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) origin3 = robot.world.create_custom_fixed_object(Pose(-100, initialPosition + cubesize, 0, angle_z=degrees(0)), cubesize, cubesize, cubesize, relative_to_robot=True) ''' target1 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition-100, 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) target2 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition, 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) target3 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition+100 , 0, angle_z=degrees(0)), 100, 100, 100, relative_to_robot=True) ''' if origin1 and origin2 and origin3: print("Created origins succesfully") # robot.go_to_pose(Pose(100, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() ''' if target1 and target2 and target3: print("Created targets succesfully") #robot.go_to_pose(Pose(300, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() ''' robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() robot.move_lift(3) time.sleep(5) robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed() robot.move_lift(-3) time.sleep(5)
def __init__(self): landmarks = { 0: Pose(-55, 160, 0, angle_z=degrees(90)), 1: Pose(55, 160, 0, angle_z=degrees(90)), 2: Pose(160, 55, 0, angle_z=degrees(0)), 3: Pose(160, -55, 0, angle_z=degrees(0)) } pf = ParticleFilter(robot, landmarks=landmarks, sensor_model=ArucoCombinedSensorModel(robot)) super().__init__(particle_filter=pf, particle_viewer=True)
def cozmo_program(robot: cozmo.robot.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 move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None while cube is None: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: print("Found a cube, pose in the robot coordinate frame: %s" % get_relative_pose(cube.pose, robot.pose)) except asyncio.TimeoutError: print("Didn't find a cube") desired_pose_relative_to_cube = Pose(0, 100, 0, angle_z=degrees(90)) # #### # TODO: Make the robot move to the given desired_pose_relative_to_cube. # Use the get_relative_pose function your implemented to determine the # desired robot pose relative to the robot's current pose and then use # one of the go_to_pose functions you implemented in Lab 6. # #### # We need to find desired pose relative to cube coordinates in World's frame. # Multiply rotation matrix of cube with relative to cube pose. # Add this to cube's pose to get world's frame. # Then find relative pose of desired pose to the robot's current pose. # Then use gotopose to move to the desired pose relative to cube. alpha = cube.pose.rotation.angle_z.radians rotation = np.array([[math.cos(alpha), -math.sin(alpha), 0], [math.sin(alpha), math.cos(alpha), 0], [0, 0, 1]]) cubeposition = np.array([cube.pose.position.x, cube.pose.position.y, 0]) relativetocubeposition = np.array([desired_pose_relative_to_cube.position.x, desired_pose_relative_to_cube.position.y, 0]) desired_pose_world_frame_position = rotation.T.dot(cubeposition) + relativetocubeposition desired_pose_world_frame_angle_z = cube.pose.rotation.angle_z.degrees + desired_pose_relative_to_cube.rotation.angle_z.degrees desired_pose_world_frame = Pose(desired_pose_world_frame_position[0], desired_pose_world_frame_position[1], 0, angle_z=degrees(desired_pose_world_frame_angle_z)) desired_pose_relative_to_robot = get_relative_pose(desired_pose_world_frame, robot.pose) my_go_to_pose3(robot, desired_pose_relative_to_robot.position.x, desired_pose_relative_to_robot.position.y, desired_pose_relative_to_robot.rotation.angle_z.degrees)
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 move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' # #### # TODO: Make the robot move to the given desired_pose_relative_to_cube. # Use the get_relative_pose function your implemented to determine the # desired robot pose relative to the robot's current pose and then use # one of the go_to_pose functions you implemented in Lab 6. # #### robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None cubeRelToRobot = None while cube is None: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: cubeRelToRobot = get_relative_pose(cube.pose, robot.pose) print("Found a cube, pose in the robot coordinate frame: %s" % cubeRelToRobot) desired_pose_relative_to_cube = Pose( 0, 0, 0, angle_z=degrees(90)) #robot will go where the cube is pose_rel_robot = cubeRelToRobot.define_pose_relative_this( desired_pose_relative_to_cube) #this will always go to 2 as the cube has to be infront to be seen my_go_to_pose3(robot, pose_rel_robot.position.x, pose_rel_robot.position.y, pose_rel_robot.rotation.angle_z.degrees)
def __init__(self, run_in_cloud=False): robot = self robot.loop = asyncio.get_event_loop() if not run_in_cloud: robot.erouter = EventRouter() robot.erouter.robot = robot robot.erouter.start() robot.head_angle = Angle(radians=0) robot.shoulder_angle = Angle(radians=0) robot.lift_height = Distance(distance_mm=0) robot.pose = Pose(0, 0, 0, angle_z=Angle(degrees=0)) robot.camera = None robot.carrying = None robot.world = SimWorld() robot.world.aruco = Aruco(robot, ARUCO_DICT_4x4_100) robot.world.light_cubes = dict() robot.world._faces = dict() robot.world.charger = None robot.world.server = SimServer() robot.world.path_viewer = None robot.world.particle_filter = SLAMParticleFilter(robot) robot.kine = CozmoKinematics(robot) # depends on particle filter robot.world.rrt = RRT(robot) # depends on kine robot.world.world_map = WorldMap(robot)
def addStaticObject(self, model, x1, y1, x2, y2, depth, height): print("[Bot] Executing addStaticObject({},{},{},{},{},{})".format( x1, y1, x2, y2, depth, height)) data = { 'addStaticObject': { 'model': model, 'x1': x1, 'y1': y1, 'x2': x2, 'y2': y2, 'depth': depth, 'height': height } } self._wsClient.send(json.dumps(data)) X1 = x1 * 10 Y1 = y1 * 10 X2 = x2 * 10 Y2 = y2 * 10 HEIGHT = height * 10 DEPTH = depth * 10 WIDTH = math.sqrt(math.pow(X1 - X2, 2) + math.pow(Y1 - Y2, 2)) centerX = (X1 + X2) / 2.0 centerY = (Y1 + Y2) / 2.0 centerZ = HEIGHT / 2.0 angle = math.atan2(Y1 - Y2, X1 - X2) pose = Pose(centerX, centerY, centerZ, angle_z=radians(angle)) self._robot.world.create_custom_fixed_object( self._origin.define_pose_relative_this(pose), WIDTH, DEPTH, HEIGHT)
def drawE(self, updatePos, offset): print("E") self.robot.set_lift_height(0.5).wait_for_completed() # self.robot.go_to_pose(Pose(updatePos.x, updatePos.y+offset, updatePos.z, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed() self.robot.drive_wheels(400, 400, duration=0.1) self.robot.turn_in_place(degrees(-90)).wait_for_completed() self.robot.set_lift_height(0.1).wait_for_completed() self.robot.drive_wheels(400, 400, duration=0.6) self.robot.set_lift_height(0.5).wait_for_completed() self.robot.go_to_pose(Pose(updatePos.x, updatePos.y + offset - 40, updatePos.z, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed() self.robot.set_lift_height(0.1).wait_for_completed() self.robot.drive_wheels(-400, -400, duration=0.6) self.robot.set_lift_height(0.1).wait_for_completed() self.robot.drive_wheels(400, 400, duration=0.8) self.robot.set_lift_height(0.5).wait_for_completed() self.robot.turn_in_place(degrees(90)).wait_for_completed() self.robot.set_lift_height(0.1).wait_for_completed() self.robot.drive_wheels(-400, -400, duration=0.6) self.robot.set_lift_height(0.5).wait_for_completed() self.robot.turn_in_place(degrees(-90)).wait_for_completed() self.robot.drive_wheels(-400, -400, duration=0.5) self.robot.turn_in_place(degrees(90)).wait_for_completed() self.robot.set_lift_height(0.1).wait_for_completed() self.robot.drive_wheels(400, 400, duration=0.6) self.robot.set_lift_height(0.5).wait_for_completed() #self.robot.say_text("This is, E").wait_for_completed() self.robot.go_to_pose(initPos, relative_to_robot=False).wait_for_completed()
def update_current_arena_pose(self): print("-" * 20 + "UPDATING POSE" + "-" * 20) coordinate_systems_diff = diff_heading_deg( self.current_robot_pose.rotation.angle_z.degrees, self.current_arena_pose.rotation.angle_z.degrees) print("Initial robot pose was: ", self.current_robot_pose) print("With a diff heading of: ", coordinate_systems_diff) arena_initial_pose_mm = rotate_point( self.current_robot_pose.position.x, self.current_robot_pose.position.y, coordinate_systems_diff) print("Calculated initial arena pose is:", arena_initial_pose_mm) arena_final_pose_mm = rotate_point(self.robot.pose.position.x, self.robot.pose.position.y, coordinate_systems_diff) print("Calculated final arena pose is:", arena_final_pose_mm) d_x = arena_final_pose_mm[0] - arena_initial_pose_mm[0] d_y = arena_final_pose_mm[1] - arena_initial_pose_mm[1] d_heading = diff_heading_deg( self.robot.pose.rotation.angle_z.degrees, self.current_robot_pose.rotation.angle_z.degrees) difference_pose = convertPoseFromMmToInches( Pose(x=d_x, y=d_y, z=0, angle_z=d_heading)) print("We think we moved ", difference_pose, "\n\n") self.current_arena_pose = self.current_arena_pose + difference_pose print("Current pose is now ", self.current_arena_pose, "\n\n")
def our_go_to_pose(robot, curr_pose, dx, dy, dh): # goto = curr_pose.define_pose_relative_this(Pose(distance_inches(dx), distance_inches(dy), distance_inches(0), angle_z=degrees(dh))) # robot.go_to_pose(goto) goto_x = curr_pose.position.x + distance_inches(dx) goto_y = curr_pose.position.y + distance_inches(dy) goto_h = curr_pose.rotation.angle_z + degrees(dh) robot.go_to_pose(Pose(goto_x, goto_y, distance_inches(0), angle_z=goto_h))
async def init_game(self): await self.robot.set_lift_height(0, duration=0.5).wait_for_completed() await self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() await self.robot.set_head_angle(cozmo.util.Angle(degrees=10)).wait_for_completed() # setup cubes self.cubes = await self.robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube) self.ori_z = self.cubes[0].pose.position.z self.ori_seq = None self.cur_word = None self.cur_seq = None self.disp_timer = 0 self.correct_check_enabled = False self.paused = False self.word_index = -1 self.score = 0 self.idle_count = 0 self.pos_index = 0 self.neg_index = 0 self.anagram_words = ["apt","tap", "are", "ear", "arm", "mar", "ram", "art", "rat", "tar", "asp", "pas", "sap", "spa", "ate", "eat", "era", "bat", "tab", "now", "own", "won", "opt", "pot", "top", "own", "now", "won","zoo", "yet", "yes", "wow","win","who","vow","rug","rub","rip","put","pub", "pie","pet","pen","pic","pig","out","oil","odd","off","nut","nod","aim","air","awe","ban","big","bio","bin"] shuffle(self.anagram_words) if not Anagram.DEBUG: await self.robot.say_text(text="let's play, anagram", play_excited_animation=False, duration_scalar=1.5).wait_for_completed() self.ori_pose = self.robot.pose await self.disp_word()
def act_out(self, game_robot, act_type): """ Acting out the various emotion displays """ selected_anim = None if act_type == "lose_hand": # Minor emotion when losing hand selected_anim = "anim_speedtap_losehand_0%s" % randint(1, 3) elif act_type == "win_hand": # Minor emotion when winning a hand selected_anim = "anim_speedtap_winhand_0%s" % randint(1, 3) elif act_type == "stand_back": #Roll back to have space to act, away from cube. game_robot.go_to_pose(Pose(-20, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() game_robot.move_lift(-3) time.sleep(0.2) elif act_type == "win_game": # Major win game emotion selected_anim = self.select_win_game() elif act_type == "lose_game": #Major lose game emotion selected_anim = self.select_lose_game() else: # animation to show waiting for light to change selected_anim = self.select_wait() # Play animation if selected_anim: game_robot.play_anim(selected_anim).wait_for_completed()
async def __drive_to_pose(self, pose): translation = (pose - self.current_arena_pose).position directions = Pose(x=translation.x, y=translation.y, z=0, angle_z=pose.rotation.angle_z) await self.__execute_directions(directions)
def step4_drop_stack_on_top(self): """ Place the stack cube in the middle of the corner cube and right cube :return: """ print('**step 4**') pickup_posn = self.corner_cube.pose.position right_posn = self.right_cube.pose.position # find an x coordinate that places cozmo at a position far enough away from the cubes such that when he # places the stack cube, it will land on top robot_x = pickup_posn.x - self.CUBE_SIZE * 1.6 # find y coordinate for position in the middle of blocks middle_y = pickup_posn.y + (right_posn.y - pickup_posn.y) / 2 # (assume an angle of 0 degrees as I'm not yet accounting for cubes that are rotated destination_pose = Pose(robot_x, middle_y, pickup_posn.z, angle_z=degrees(0)) go_to_destination_pose = self.robot.go_to_pose(destination_pose, num_retries=3) go_to_destination_pose.wait_for_completed() drop_stack = self.robot.place_object_on_ground_here(self.stack_cube, num_retries=3) drop_stack.wait_for_completed() self.stack_cube.set_lights(Light(on_color=Color(int_color=0xff00ffff), off_color=Color(int_color=0xff00ffff))) # I can't check for has_succeeded here because Cozmo won't think he's actually succeeded in dropping the cube # so let's just assume he was successful self.increment_step()
def move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None while cube is None: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: print("Found a cube, pose in the robot coordinate frame: %s" % get_relative_pose(cube.pose, robot.pose)) except asyncio.TimeoutError: print("Didn't find a cube") desired_pose_relative_to_cube = Pose(0, 0, 0, angle_z=degrees(0)) final_pose = get_relative_pose( robot.pose, get_relative_pose(cube.pose, desired_pose_relative_to_cube)) x, y, z = final_pose.position.x_y_z my_go_to_pose1(robot, x, y, 0) return
def move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None while cube is None: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: print("Found a cube, pose in the robot coordinate frame: %s" % get_relative_pose(cube.pose, robot.pose)) except asyncio.TimeoutError: print("Didn't find a cube") desired_pose_relative_to_cube = Pose(0, 100, 0, angle_z=degrees(90)) # #### # TODO: Make the robot move to the given desired_pose_relative_to_cube. # Use the get_relative_pose function your implemented to determine the # desired robot pose relative to the robot's current pose and then use # one of the go_to_pose functions you implemented in Lab 6. # #### cube_pose_relative_to_robot = get_relative_pose(cube.pose, robot.pose) desired_pose_relative_to_robot = cube_pose_relative_to_robot.define_pose_relative_this( desired_pose_relative_to_cube) # cozmo_go_to_pose(robot, desired_pose_relative_to_robot.position.x, desired_pose_relative_to_robot.position.y, desired_pose_relative_to_robot.rotation.angle_z.degrees) my_go_to_pose3(robot, desired_pose_relative_to_robot.position.x, desired_pose_relative_to_robot.position.y, desired_pose_relative_to_robot.rotation.angle_z.degrees)
def move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube = None while cube is None: try: cube = robot.world.wait_for_observed_light_cube(timeout=30) if cube: print("Found a cube, pose in the robot coordinate frame: %s" % cube.pose) except asyncio.TimeoutError: print("Didn't find a cube") desired_pose_relative_to_cube = Pose(0, 100, 0, angle_z=degrees(90)) x, y, z = cube.pose.position.x_y_z print("Robot pose: %s" % robot.pose) #my_go_to_pose3(robot, x-100, y, 90) my_go_to_pose2( robot, 269, 127, 20 ) # found the cordinates of cube and moved robot to desired orientation to calculate final position my_go_to_pose3(robot, 269, -127, 20)
def update_current_arena_pose(self): print("-" * 40) print("-" * 20 + "UPDATING POSE" + "-" * 20) print("-" * 40) coordinate_systems_diff = -self.pose_offset.degrees print("Started at arena pose: ", self.current_arena_pose, "\n\n") #print("Initial robot pose was: ", self.current_robot_pose) #print("Current robot pose is: ", self.robot.pose) #print("With a diff heading of: ", coordinate_systems_diff) d_x, d_y = rotate_point( self.robot.pose.position.x - self.current_robot_pose.position.x, self.robot.pose.position.y - self.current_robot_pose.position.y, coordinate_systems_diff) #print("Calculated initial arena pose is:", arena_initial_pose_mm) #arena_final_pose_mm = rotate_point(, self.robot.pose.position.y, coordinate_systems_diff) #print("Calculated final arena pose is:", arena_final_pose_mm) #print("COMPUTE ODOMETRY:" ,self.__compute_odometry()) #d_x = arena_final_pose_mm[0] - arena_initial_pose_mm[0] #d_y = arena_final_pose_mm[1] - arena_initial_pose_mm[1] d_heading = diff_heading_deg( self.robot.pose.rotation.angle_z.degrees, self.current_robot_pose.rotation.angle_z.degrees) difference_pose = convertPoseFromMmToInches( Pose(x=d_x, y=d_y, z=0, angle_z=degrees(d_heading))) print("We think we moved ", difference_pose, "\n\n") self.current_arena_pose = self.current_arena_pose + difference_pose self.current_robot_pose = self.robot.pose print("Current arena pose is now: ", self.current_arena_pose, "\n\n")
def move_relative_to_cube(robot: cozmo.robot.Robot): '''Looks for a cube while sitting still, when a cube is detected it moves the robot to a given pose relative to the detected cube pose.''' robot.move_lift(-3) robot.set_head_angle(degrees(0)).wait_for_completed() cube_w = None while cube_w is None: try: cube_w = robot.world.wait_for_observed_light_cube(timeout=30) if cube_w: print("Found a cube, pose in the robot coordinate frame: %s" % pose_transform.get_relative_pose(cube_w.pose, robot.pose)) break except asyncio.TimeoutError: print("Didn't find a cube") desired_pose_relative_to_cube = Pose(0, 100, 0, angle_z=degrees(90)) # #### # TODO: Make the robot move to the given desired_pose_relative_to_cube. # Use the get_relative_pose function you implemented to determine the # desired robot pose relative to the robot's current pose and then use # one of the go_to_pose functions you implemented in Lab 6. # #### pose_relative_to_world = pose_transform.getWorldPoseOfRelativeObject(desired_pose_relative_to_cube, robot.pose) print("destination pose in the world coordinate frame: %s" % pose_transform.format_pose(pose_relative_to_world))
def cozmo_go_to_pose(robot, x, y, angle_z): """Moves the robot to a pose relative to its current pose. Arguments: robot -- the Cozmo robot instance passed to the function x,y -- Desired position of the robot in millimeters angle_z -- Desired rotation of the robot around the vertical axis in degrees """ robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(angle_z)), relative_to_robot=True).wait_for_completed()
async def __drive_to_pose(self, pose): print("We are at ", self.current_arena_pose, " and we are driving to ", pose) translation = (pose - self.current_arena_pose).position directions = Pose(x=translation.x, y=translation.y, z=0, angle_z=pose.rotation.angle_z) print("We will follow these directions: ", directions, "\n\n") await self.__execute_directions(directions) print("Directions followed!", "\n\n") self.update_current_arena_pose()
def start(self, event=None): x = self.parent.obj.x y = self.parent.obj.y theta = self.parent.obj.theta self.target_pose = Pose(x + 250*cos(theta), y+250*sin(theta), self.robot.pose.position.z, angle_z=Angle(radians = wrap_angle(theta+pi))) print('Traveling to',self.target_pose) super().start(event)
def __init__(self, robot: cozmo.robot.Robot): self.current_arena_pose = None self.current_robot_pose = robot.pose self.robot = robot # start streaming robot.camera.image_stream_enabled = True robot.camera.color_image_enabled = False robot.camera.enable_auto_exposure() # Obtain the camera intrinsics matrix fx, fy = robot.camera.config.focal_length.x_y cx, cy = robot.camera.config.center.x_y self.camera_settings = np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ], dtype=np.float) self.current_drop_off_pose_idx = 0 self.pick_up_pose = Pose(x=4.5, y=11, z=0, angle_z=degrees(90)) self.drop_off_poses = [Pose(x=22, y=11, z=0, angle_z=degrees(90)), Pose(x=20, y=11, z=0, angle_z=degrees(90)), Pose(x=18, y=11, z=0, angle_z=degrees(90))] self.drop_off_directions = [Pose(x=5, y=6, z=0, angle_z=degrees(0)), Pose(x=22, y=6, z=0, angle_z=degrees(90)), self.drop_off_poses[0]] self.pick_up_directions = [Pose(x=21.75, y=4.5, z=0, angle_z=degrees(0)), Pose(x=5, y=4.5, z=0, angle_z=degrees(90)), self.pick_up_pose] self.drive_speed = speed_mmps(100) self.grid = CozGrid("map_arena.json") self.pf = ParticleFilter(self.grid) threading.Thread(target=self.runGUI).start() self.pose_offset = None
def go_to_coordinate(robot): global x_coordinate global y_coordinate global angledegrees robot.go_to_pose(Pose(x_coordinate, y_coordinate, 0, angle_z=degrees(angledegrees)), relative_to_robot=True).wait_for_completed()
async def run(self, coz_conn): asyncio.set_event_loop(coz_conn._loop) # setup robot self.coz = await coz_conn.wait_for_robot() await self.coz.go_to_pose(Pose(0, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed() await self.start_game() while True: pass
def start(self, event=None): wall = self.parent.object print('Selected wall',self.parent.wobj) (x, y, theta) = self.parent.pick_side(150) self.target_pose = Pose(x, y, self.robot.pose.position.z, angle_z=Angle(radians = wrap_angle(theta))) print('Traveling to',self.target_pose) super().start(event)