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 True:
try:
robot.say_text('searching').wait_for_completed()
cube = robot.world.wait_for_observed_light_cube(timeout=5)
if cube:
robot.say_text('Found it').wait_for_completed()
break
except asyncio.TimeoutError:
my_turn_in_place(robot, 30, 50, True)
relative_pose = get_relative_pose(cube.pose, robot.pose)
print(f"Found a cube, pose in the robot coordinate frame: {relative_pose}")
my_go_to_pose3(robot, relative_pose.position.x, relative_pose.position.y, relative_pose.rotation.angle_z.degrees)
robot.say_text('Arrived').wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.FistBumpSuccess).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 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 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)