async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
# Obtain odometry information
last_pose = robot.last_pose
current_odom = compute_odometry(robot.pose)
robot.last_pose = robot.pose
# Obtain list of currently seen markers and their poses
images = await image_processing(robot)
r_marker_list = cvt_2Dmarker_measurements(images)
# Update the particle filter using the above information
result = robot.pf.update(current_odom, r_marker_list)
# Determine the robot's action based on current state of localization system
# Then do the below.
converged = result[3]
if robot.played_goal_animation:
robot.stop_all_motors()
elif converged:
# Have the robot drive to the goal.
# Goal is defined w/ position & orientation
position = result[0:2]
rounded_position = tuple(
int(position[i]) for i in range(len(position)))
angle = math.radians(result[2])
if robot.found_goal:
target_angle = 0
else:
target_angle = math.degrees(
getTurnDirection(math.cos(angle), math.sin(angle), position,
goal[0:2]))
at_goal = True
goal_position = tuple(goal[0:2])
for i in range(len(rounded_position)):
if abs(rounded_position[i] - goal_position[i]) > 1:
at_goal = False
break
if at_goal:
robot.stop_all_motors()
await robot.turn_in_place(degrees(-result[2]),
num_retries=3).wait_for_completed()
robot.found_goal = True
if not robot.played_goal_animation:
# Then robot should play a happy animation, and stand still.
await robot.say_text("Yay",
play_excited_animation=True,
duration_scalar=0.5,
voice_pitch=1).wait_for_completed()
robot.played_goal_animation = True
elif abs(target_angle) > robot.TURN_TOLERANCE and abs(
2 * math.pi - abs(target_angle)) > robot.TURN_TOLERANCE:
robot.stop_all_motors()
await robot.turn_in_place(degrees(target_angle),
num_retries=3).wait_for_completed()
else:
robot.found_goal = False
await robot.drive_wheels(robot.ROBOT_SPEED, robot.ROBOT_SPEED,
robot.ROBOT_ACCELERATION,
robot.ROBOT_ACCELERATION)
else:
# Have robot actively look around if localization has not converged.
await robot.drive_wheels(robot.TURN_SPEED, -robot.TURN_SPEED,
robot.ROBOT_ACCELERATION,
robot.ROBOT_ACCELERATION)
# Make code robust to "kidnapped robot problem"
# Reset localization if robot is picked up.
# Make robot unhappy.
if robot.is_picked_up:
robot.pf.__init__(robot.grid)
robot.found_goal = False
robot.played_goal_animation = False
if not robot.played_angry_animation:
await play_angry(robot)
robot.played_angry_animation = True
await robot.set_head_angle(cozmo.util.degrees(robot.HEAD_ANGLE)
).wait_for_completed()
else:
robot.played_angry_animation = False
# Update the particle filter GUI for debugging
robot.gui.show_particles(robot.pf.particles)
robot.gui.show_mean(*result)
robot.gui.updated.set()
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
global grid, gui, pf
robot.played_goal_animation = False
robot.found_goal = False
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
await robot.set_head_angle(cozmo.util.degrees(7)).wait_for_completed()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
camera_settings = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
dtype=np.float)
###################
at_goal = False
while True:
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text("Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(1, 10000).wait_for_completed()
await robot.set_lift_height(0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
currPose = robot.pose
odom = compute_odometry(currPose)
markerlist, camera_image = await marker_processing(
robot, camera_settings)
gui.show_camera_image(camera_image)
mean_estimate = pf.update(odom, markerlist)
gui.show_camera_image(camera_image)
last_pose = currPose
estimatex = mean_estimate[0]
estimatey = mean_estimate[1]
estimateh = mean_estimate[2]
converged = mean_estimate[3]
# angle = math.radians(mean_estimate[2])
# forward = (math.cos(angle), math.sin(angle))
# target_direction = np.subtract(goal[0:2], mean_estimate[0:2])
# target_angle = math.atan2(target_direction[1], target_direction[0]) - math.atan2(forward[1], forward[0])
distx = goal[0] - mean_estimate[0]
disty = goal[1] - mean_estimate[1]
angle = math.degrees(math.atan2(disty, distx))
target_angle = diff_heading_deg(angle, mean_estimate[2])
if robot.played_goal_animation:
robot.stop_all_motors()
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text("daddy stop helping me. senpai ",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(1, 10000).wait_for_completed()
await robot.set_lift_height(0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
elif converged:
#robot.found_goal = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
print("converged")
pos = mean_estimate[0:2]
rPos = tuple(int(pos[x]) for x in range(len(pos)))
#robot.found_goal =True
if robot.found_goal:
print("foundgoal")
target_angle = 0
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(
1, 10000).wait_for_completed()
await robot.set_lift_height(
0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
#dist = math.sqrt((goal[0] - pos[0])**2 + (goal[1] - pos[1])**2)
# robot.stop_all_motors()
# robot.drive_straight(distance=dist, speed=10).wait_for_completed()
else:
print('foundgoal else')
# angle = math.radians(mean_estimate[2])
# forward = (math.cos(angle), math.sin(angle))
# target_direction = np.subtract(goal[0:2], mean_estimate[0:2])
# target_angle = math.atan2(target_direction[1], target_direction[0]) - math.atan2(forward[1], forward[0])
distx = goal[0] - mean_estimate[0]
disty = goal[1] - mean_estimate[1]
angle = math.degrees(math.atan2(disty, distx))
target_angle = diff_heading_deg(angle, mean_estimate[2])
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(
1, 10000).wait_for_completed()
await robot.set_lift_height(
0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
# robot.stop_all_motors()
# robot.turn_in_place(degrees(target_angle), num_retries=3).wait_for_completed()
# robot.found_goal=True
at_goal = True
pickupinloop = False
gPos = tuple(goal[0:2])
for x in range(len(rPos)):
if abs(rPos[x] - gPos[x]) > 1:
at_goal = False
print("forloop at goal")
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(
1, 10000).wait_for_completed()
await robot.set_lift_height(
0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)
).wait_for_completed()
pickupinloop = True
break
#break
if pickupinloop:
continue
if at_goal:
print("at goal")
robot.stop_all_motors()
await robot.turn_in_place(degrees(-mean_estimate[2]),
num_retries=3).wait_for_completed()
robot.found_goal = True
#if not robot.played_goal_animation:
robot.say_text("yeet yeet yeet I DID IT DADDY",
play_excited_animation=True,
duration_scalar=0.75,
voice_pitch=1)
robot.played_goal_animation = True
#await robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin)
elif abs(target_angle) > 2 and abs(2 * math.pi -
abs(target_angle)) > 2:
print("elif abs(target)angle)")
robot.stop_all_motors()
await robot.turn_in_place(degrees(target_angle),
num_retries=2).wait_for_completed()
robot.found_goal = True
robot.stop_all_motors()
await robot.set_head_angle(degrees(7)).wait_for_completed()
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(
1, 10000).wait_for_completed()
await robot.set_lift_height(
0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
else:
print("else of at goal")
robot.found_goal = False
dist = math.sqrt((goal[0] - pos[0])**2 + (goal[1] - pos[1])**2)
# if not robot.found_goal:
# robot.stop_all_motors()
# await robot.turn_in_place(angle=degrees(target_angle),num_retries=2).wait_for_completed()
#robot.found_goal = True
robot.stop_all_motors()
await robot.drive_straight(
distance=cozmo.util.distance_mm(dist * 25),
speed=cozmo.util.speed_mmps(75)).wait_for_completed()
#await robot.drive_wheels(20, 20, duration=6)
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"daddy stop helping me. senpai ",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(
1, 10000).wait_for_completed()
await robot.set_lift_height(
0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
else:
await robot.set_head_angle(degrees(7)).wait_for_completed()
await robot.drive_wheels(-10, 10, duration=0)
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
# robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text(
"Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(1, 10000).wait_for_completed()
await robot.set_lift_height(0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
print("turning")
if robot.is_picked_up:
robot.stop_all_motors()
print("picked up")
pf = ParticleFilter(grid)
#robot.pf = pf
robot.found_goal = False
robot.played_goal_animation = False
robot.at_goal = False
if not robot.played_angry_animation:
await robot.say_text("Stop holding me daddy uhhh daddy please",
duration_scalar=0.75,
voice_pitch=1,
num_retries=2).wait_for_completed()
await robot.set_lift_height(1, 10000).wait_for_completed()
await robot.set_lift_height(0, 10000).wait_for_completed()
robot.played_angry_animation = True
await robot.set_head_angle(degrees(7)).wait_for_completed()
continue
else:
robot.played_angry_animation = False
gui.show_particles(pf.particles)
gui.show_mean(*mean_estimate)
gui.updated.set()
