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" %
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=5)
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))
destX = desired_pose_relative_to_cube.position.x + cube.pose.position.x
destY = desired_pose_relative_to_cube.position.y + cube.pose.position.y
destT = degrees(desired_pose_relative_to_cube.rotation.angle_z.degrees +
cube.pose.rotation.angle_z.degrees)
dest = Pose(destX, destY, 0, angle_z=destT)
trans = get_relative_pose(dest, robot.pose)
#my_go_to_pose1(robot, trans.position.x, trans.position.y, trans.rotation.angle_z.degrees)
my_go_to_pose2(robot, trans.position.x, trans.position.y,
trans.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" % 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 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.'''
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 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 my_drive_straight(robot, dist, speed, debug=False):
"""Drives the robot straight.
Arguments:
robot -- the Cozmo robot instance passed to the function
dist -- Desired distance of the movement in millimeters
speed -- Desired speed of the movement in millimeters per second
"""
debug_print(f"[Drive Straight] Dist to {dist}, speed to {speed}", debug)
if speed < 0:
robot.say_text('Cannot do that').wait_for_completed()
return
if dist < 0 and speed > 0:
speed = -speed
dist = abs(dist)
old_position = robot.pose
new_position = old_position
related_pose = get_relative_pose(new_position, old_position)
rest = dist - abs(
math.sqrt(related_pose.position.x**2 + related_pose.position.y**2))
while rest > 0:
debug_print(f"[Drive Straight] old_position {old_position}", debug)
debug_print(f"[Drive Straight] new_position {new_position}", debug)
debug_print(f"[Drive Straight] rest {rest}", debug)
if abs(rest) < 5:
break
if rest < abs(speed):
speed = get_number_signal(speed) * rest
debug_print(f"[Drive Straight] lower speed to {speed}", debug)
elif rest - abs(speed) < 10: # Cannot move when distance is small
speed = get_number_signal(speed) * (rest + 10)
debug_print(f"[Drive Straight] higher speed to {speed}", debug)
elif rest - abs(speed) < 30: # Cannot move when distance is small
speed = get_number_signal(speed) * rest
debug_print(f"[Drive Straight] higher speed to {speed}", debug)
robot.drive_wheels(speed,
speed,
0,
0,
duration=max(rest / abs(speed), 1))
time.sleep(0.2)
new_position = robot.pose
related_pose = get_relative_pose(new_position, old_position)
rest = dist - abs(
math.sqrt(related_pose.position.x**2 + related_pose.position.y**2))
if debug:
debug_print(f"[Drive Straight] rest {rest}", debug)
def pose_to_coords(pose):
"""Transforms a cozmo pose in world coordinates to the grid coordinates.
Assuming that the robot starts at the given start location on the map and facing +x direction."""
pose_relative_to_origin = get_relative_pose(pose, origin)
(x0, y0) = start
return round(
pose_relative_to_origin.position.x / grid.scale) + x0, round(
pose_relative_to_origin.position.y / grid.scale) + y0
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))
# desired_pose = get_relative_pose(desired_pose_relative_to_cube, cube.pose)
# odo.my_go_to_pose1(robot, desired_pose.position.x, desired_pose.position.y, desired_pose.rotation.angle_z.degrees)
# Somehow I got my poses mixed up for go_to_pose 2/3, but this works great!
desired_pose = cube.pose
odo.my_go_to_pose2(robot, desired_pose.position.x, desired_pose.position.y, desired_pose.rotation.angle_z.degrees)
def run(robot: cozmo.robot.Robot):
print(f"***** Front wheel radius : {get_front_wheel_radius()}")
print(f"***** Distance between wheels: {get_distance_between_wheels()}")
# Example tests of the functions
for angle in range(15, 181, 15):
old_position = robot.pose
rotate_front_wheel(robot, angle)
new_position = robot.pose
related_pose = get_relative_pose(new_position, old_position)
moved = abs(
math.sqrt(related_pose.position.x**2 + related_pose.position.y**2))
distance = get_front_wheel_radius() * math.radians(angle)
if abs(moved - distance) < 3:
print(f'[rotate_front_wheel_test] Good in angle: {angle}')
else:
print(
f'[rotate_front_wheel_test] Wrong in angle: {angle}, delta {abs(moved - distance)}'
)
# cozmo_drive_straight(robot, 62, 50)
for distance in range(50, 100, 15):
for speed in range(20, 50, 10):
for signal in range(-1, 2, 2):
dist = signal * distance
old_position = robot.pose
my_drive_straight(robot, dist, speed)
new_position = robot.pose
related_pose = get_relative_pose(new_position, old_position)
moved = abs(
math.sqrt(related_pose.position.x**2 +
related_pose.position.y**2))
if abs(moved - distance) < 10:
print(
f'[my_drive_straight_test] Good in distance: {dist}, speed: {speed}'
)
else:
print(
f'[my_drive_straight_test] Wrong in distance: {dist}, speed: {speed}, delta {moved - distance}'
)
# cozmo_turn_in_place(robot, 60, 30)
for orignal_angle in range(30, 31, 30):
for speed in range(30, 61, 15):
for signal in range(-1, 2, 2):
angle = signal * orignal_angle
old_angle = robot.pose.rotation.angle_z.degrees
if old_angle < 0:
old_angle += 360
my_turn_in_place(robot, angle, speed)
new_angle = robot.pose.rotation.angle_z.degrees
if new_angle < 0:
new_angle += 360
if (new_angle > old_angle):
delta = new_angle - old_angle
else:
delta = old_angle - new_angle
if delta > 180:
delta -= 360
if abs(abs(delta) - abs(angle)) < 10:
print(
f'[my_turn_in_place_test] Good in angle: {angle}, speed: {speed}'
)
else:
print(
f'[my_turn_in_place_test] Wrong in angle: {angle}, speed: {speed}, delta {abs(delta) - abs(angle)}'
)
for x in range(-100, 101, 100):
for y in range(-100, 101, 100):
for angle in range(-45, 90, 45):
old_pose = robot.pose
my_go_to_pose1(robot, x, y, angle)
new_pose = robot.pose
related_pose = get_relative_pose(new_pose, old_pose)
print(
f'[Go to Pose 1 Test] Moved x {related_pose.position.x}), y: {related_pose.position.y}, angle: {related_pose.rotation.angle_z.degrees})'
)
delta_x = related_pose.position.x - x
delta_y = related_pose.position.y - y
delta_angle = related_pose.rotation.angle_z.degrees - angle
if abs(delta_x) < 15 and abs(delta_y) < 15 and abs(
delta_angle) < 5:
print(
f'[Go to Pose 1 Test] Good in x: {x}, y: {y}, angle: {angle}'
)
else:
print(
f'[Go to Pose 1 Test] Wrong in x: {x} (delta {delta_x}), y: {y} (delta {delta_y}), angle: {angle} (delta {delta_angle})'
)
for x in range(-100, 101, 100):
for y in range(-100, 101, 100):
for angle in range(-45, 90, 45):
old_pose = robot.pose
my_go_to_pose2(robot, x, y, angle)
new_pose = robot.pose
related_pose = get_relative_pose(new_pose, old_pose)
print(
f'[Go to Pose 2 Test] Moved x {related_pose.position.x}), y: {related_pose.position.y}, angle: {related_pose.rotation.angle_z.degrees})'
)
delta_x = related_pose.position.x - x
delta_y = related_pose.position.y - y
delta_angle = related_pose.rotation.angle_z.degrees - angle
if abs(delta_x) < 10 and abs(delta_y) < 10 and abs(
delta_angle) < 10:
print(
f'[Go to Pose 2 Test] Good in x: {x}, y: {y}, angle: {angle}'
)
else:
print(
f'[Go to Pose 2 Test] Wrong in x: {x} (delta {delta_x}), y: {y} (delta {delta_y}), angle: {angle} (delta {delta_angle})'
)
# cozmo_go_to_pose(robot, 100, 0, 45)
# cozmo_go_to_pose(robot, 100, 100, 45)
# cozmo_go_to_pose(robot, 100, -100, 45)
# cozmo_go_to_pose(robot, -100, -100, 45)
# cozmo_go_to_pose(robot, 0, -150, 45)
for x in range(-100, 101, 100):
for y in range(-100, 101, 100):
for angle in range(-45, 90, 45):
old_pose = robot.pose
my_go_to_pose3(robot, x, y, angle)
new_pose = robot.pose
related_pose = get_relative_pose(new_pose, old_pose)
print(
f'[Go to Pose 3 Test] Moved x {related_pose.position.x}), y: {related_pose.position.y}, angle: {related_pose.rotation.angle_z.degrees})'
)
delta_x = related_pose.position.x - x
delta_y = related_pose.position.y - y
delta_angle = related_pose.rotation.angle_z.degrees - angle
if abs(delta_x) < 15 and abs(delta_y) < 15 and abs(
delta_angle) < 10:
print(
f'[Go to Pose 3 Test] Good in x: {x}, y: {y}, angle: {angle}'
)
else:
print(
f'[Go to Pose 3 Test] Wrong in x: {x} (delta {delta_x}), y: {y} (delta {delta_y}), angle: {angle} (delta {delta_angle})'
)
def my_go_to_pose2(robot, x, y, angle_z, debug=False):
"""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
"""
debug_print(
f"[Go to Pose2] Go to position ({x}, {y}), angle degree: {angle_z}",
debug)
if x == 0 and y == 0:
my_turn_in_place(robot, angle_z, angle_z, debug)
return
world_old_position = robot.pose
distance = math.sqrt(x**2 + y**2)
while True:
debug_print("======================================================",
debug)
world_new_position = robot.pose
robot_pose = get_relative_pose(world_new_position, world_old_position)
debug_print(
f"[Go to Pose2] Robot at ({robot_pose.position.x}, {robot_pose.position.y}), angle degree: {robot_pose.rotation.angle_z.degrees}",
debug)
delta_x = x - robot_pose.position.x
delta_y = y - robot_pose.position.y
rho = math.sqrt(delta_x**2 + delta_y**2)
if delta_x == 0:
alpha = get_number_signal(delta_y) * math.pi
else:
alpha = normalize_angle(
math.atan2(delta_y, delta_x) -
robot_pose.rotation.angle_z.radians)
eta = normalize_angle(
math.radians(angle_z) - robot_pose.rotation.angle_z.radians)
debug_print("[Go to Pose2] Errors:", debug)
debug_print(f"[Go to Pose2] rho: {rho}", debug)
debug_print(
f"[Go to Pose2] alpha: {alpha}, degrees: {math.degrees(alpha)}",
debug)
debug_print(f"[Go to Pose2] eta: {eta}, degrees: {math.degrees(eta)}",
debug)
if abs(rho) < 10 and abs(math.degrees(eta)) < 10:
robot.stop_all_motors()
debug_print("[Go to Pose2] Stop", debug)
break
elif abs(rho) < 10:
# Stop the movement and just turn to the right angle.
# If not stop at this time, due Cozmo's motor and slip, might run into too long time
# Also, if every parameter (p1,p2,p3) is good enough, this should not been used
debug_print("[Go to Pose2] Stop", debug)
robot.stop_all_motors()
debug_print(f"[Go to Pose2] Turn {math.degrees(eta)}", debug)
my_turn_in_place(robot, math.degrees(eta), abs(math.degrees(eta)),
debug)
break
p1 = 0.2
# more focus on direction when far from goal
# more focus on heading when near the goal
if rho > distance / 5 * 3:
p2 = 0.3
p3 = 0.1
elif rho < distance / 5:
p2 = 0.1
p3 = 0.3
else:
p2 = 0.2
p3 = 0.2
debug_print(f"[Go to Pose2] p1: {p1}, p2: {p2}, p3: {p3}", debug)
move_speed = p1 * rho
rotation_speed = p2 * alpha + p3 * eta
debug_print(
f"[Go to Pose2] Move Speed: {move_speed}, Rotation Degrees: {math.degrees(rotation_speed)}",
debug)
rotation_speed_mm = rotation_speed * get_distance_between_wheels() / 2
left_speed = move_speed - rotation_speed_mm
right_speed = move_speed + rotation_speed_mm
debug_print(
f"[Go to Pose2] Left Speed: {left_speed}, Right Speed: {right_speed}",
debug)
robot.drive_wheels(left_speed, right_speed)
if abs(left_speed) < 5 and abs(right_speed) < 5:
# When speed is not that farest, don't change the speed to often
# due Cozmo's motor and slip, might run into too long time and not stop
time.sleep(1)
else:
time.sleep(0.1)