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=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)