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
Example #2
0
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=10)
            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 = get_relative_pose(
        cube.pose, robot.pose)  #Pose(0, 100, 0, angle_z=degrees(90))

    #desired_pose_relative_to_cube = cube.pose #get_relative_pose(cube.pose, robot.pose) #Pose(0, 100, 0, angle_z=degrees(90))

    # ####
    # 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.
    # ####

    #cozmo_go_to_pose(robot, desired_pose_relative_to_cube.position.x, desired_pose_relative_to_cube.position.y, desired_pose_relative_to_cube.rotation.angle_z.degrees)

    my_go_to_pose1(robot, desired_pose_relative_to_cube.position.x,
                   desired_pose_relative_to_cube.position.y,
                   desired_pose_relative_to_cube.rotation.angle_z.degrees)