def grade_core(grid, Robot_init_pose, Dh_circular, Robot_speed):
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid, Dh_circular,
Robot_speed)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update()
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 50
elif steps_built_track < 100:
score = 100 - steps_built_track
else:
score = 0
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
score_per_track = 50.0 / Steps_stable_tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update()
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
score += score_per_track
return score
threading.Thread.__init__(self, daemon=True)
self.filter = particle_filter
self.gui = gui
def run(self):
while True:
estimated = self.filter.update()
self.gui.show_particles(self.filter.particles)
self.gui.show_mean(estimated[0], estimated[1], estimated[2],
estimated[3])
self.gui.show_robot(self.filter.robbie)
self.gui.updated.set()
if __name__ == "__main__":
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
if Use_GUI:
gui = GUIWindow(grid)
filter_thread = ParticleFilterThread(particlefilter, gui)
filter_thread.start()
gui.start()
else:
while True:
particlefilter.update()
def run_test_case(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(setting.PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update()
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 45
elif steps_built_track < Steps_build_tracking:
score = Steps_build_tracking - steps_built_track
else:
score = 0
print("\nPhase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
score_per_track = 45.0 / Steps_stable_tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update()
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
score += score_per_track
print("\nPhase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :",
acc_err_trans / Steps_stable_tracking)
print("Average rotational error :", acc_err_rot / Steps_stable_tracking,
"deg")
print("Max translational error :", max_err_trans)
print("Max rotational error :", max_err_rot, "deg")
return score
def grade(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 50
elif steps_built_track < 100:
score = 100 - steps_built_track
else:
score = 0
print("\nPhrase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
if step_track > 90:
score += 50
elif step_track > 40:
score += step_track - 40
else:
score += 0
print("\nPhrase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :{:.2f}".format(acc_err_trans /
Steps_stable_tracking))
print("Average rotational error :{:.2f} deg".format(acc_err_rot /
Steps_stable_tracking))
print("Max translational error :{:.2f}".format(max_err_trans))
print("Max rotational error :{:.2f} deg".format(max_err_rot))
print("\nexample score =", score)
return score
async def run(robot: cozmo.robot.Robot):
global last_pose
global grid, gui
# global pf, state
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
await robot.set_head_angle(degrees(0)).wait_for_completed()
state = "In motion"
############################################################################
######################### YOUR CODE HERE####################################
turn_num = 0
while True:
robo_odom = compute_odometry(robot.pose)
print(robo_odom)
vis_markers = await image_processing(robot)
markers_2d = cvt_2Dmarker_measurements(vis_markers)
print(markers_2d)
m_x, m_y, m_h, m_confident = ParticleFilter.update(
pf, robo_odom, markers_2d)
gui.show_mean(m_x, m_y, m_h, m_confident)
gui.show_particles(pf.particles)
robbie = Robot(robot.pose.position.x, robot.pose.position.y,
robot.pose_angle.degrees)
gui.show_robot(robbie)
gui.updated.set()
if robot.is_picked_up:
await robot.say_text("Hey put me down!!").wait_for_completed()
pf = ParticleFilter(grid)
time.sleep(5)
state = "In motion"
await robot.set_head_angle(degrees(0)).wait_for_completed()
elif m_confident and state == "In motion":
print("Going to the goal pose")
print("X: {}, Y:{}".format(m_x, m_y))
# Part 1: Figure out Robot's origin + theta offset
# For consistency, the global frame is A, the one used by the bot is B
# need tp find angle theta_b_a (radians) that represents angle FROM X_b TO X_a
theta_a_b = math.radians(m_h - robot.pose_angle.degrees)
theta_b_a = math.radians(robot.pose_angle.degrees - m_h)
# need to find vector t (in frame A) that represents the origin of B in A
# Solution: We have robot pos in both B and A. a_to_r, b_to_r, and t make a triangle.
# First we must get R_b in the coordinate system of A, then a_to_r - b_to_r = t.
a_to_r_in_A = (m_x, m_y)
b_to_r_in_B = (robot.pose.position.x / 10.,
robot.pose.position.y / 10.) # in mm, right?
b_to_r_in_A_x = (math.cos(theta_a_b)* b_to_r_in_B[0] \
- math.sin(theta_a_b)*b_to_r_in_B[1])
b_to_r_in_A_y = (math.sin(theta_a_b) * b_to_r_in_B[0] \
+ math.cos(theta_a_b) * b_to_r_in_B[1])
b_to_r_in_A = (b_to_r_in_A_x, b_to_r_in_A_y)
t = (a_to_r_in_A[0] - b_to_r_in_B[0],
a_to_r_in_A[1] - b_to_r_in_B[1])
# Part 2: Find goal_in_B
goal_in_A = (goal[0], goal[1])
goal_in_B_x = (math.cos(theta_b_a) * goal_in_A[0] \
- math.sin(theta_b_a) *goal_in_A[1] \
+ t[0])
goal_in_B_y = (math.sin(theta_b_a) * goal_in_A[0] \
+ math.cos(theta_b_a) * goal_in_A[1] \
+ t[1])
r_to_goal_in_B = (goal_in_B_x - b_to_r_in_B[0],
goal_in_B_y - b_to_r_in_B[1])
head = math.degrees(
math.atan2(r_to_goal_in_B[1],
r_to_goal_in_B[0])) - robot.pose_angle.degrees
actual_head = math.degrees(math.atan2(goal[1], goal[0])) - m_h
print("Calculated Head: " + str(head) + " Actual Head: " +
str(actual_head))
print("Observed theta: " + str(m_h) + "Calculated theta: " +
str(robot.pose_angle.degrees + theta_a_b))
dist = (goal[0] - m_x, goal[1] - m_y)
print(dist)
delta_t = math.degrees(math.atan2(dist[1], dist[0])) - m_h
dist = math.sqrt(dist[0]**2 + dist[1]**2)
print("M_H: " + str(m_h))
print("M_X: " + str(m_x))
print("M_Y: " + str(m_y))
print("Delta_t: " + str(delta_t))
print("Dist: " + str(dist))
await robot.turn_in_place(degrees(delta_t)).wait_for_completed()
#dist = math.sqrt(r_to_goal_in_B[0]**2 + r_to_goal_in_B[1]**2)
await robot.drive_straight(distance_inches(dist),
speed_mmps(40)).wait_for_completed()
# print("Vectors:")
# print(m_x,m_y)
# print(str(math.degrees(theta_a_b)) + " + " + str(robot.pose_angle.degrees))
# print(b_to_r_in_B)
# print(goal_in_A)
# print((goal_in_B_x,goal_in_B_y))
# print(t)
# print(r_to_goal_in_B)
# h_offset *= -1
# local_x = math.cos(h_offset)*(goal[0]-x_offset) - math.sin(h_offset)*(goal[1]-y_offset)
# local_y = math.sin(h_offset)*(goal[0]-x_offset) + math.cos(h_offset)*(goal[1]-y_offset)
# goal_pose = cozmo.util.Pose(10*goal_in_B_x, 10*goal_in_B_y, goal[2], angle_z=degrees(goal[2]))
# print(robot.pose)
# await robot.go_to_pose(goal_pose, relative_to_robot=True, in_parallel=False).wait_for_completed()
await robot.say_text("I did it!!").wait_for_completed()
print(robot.pose)
state = "Wait to be picked up"
elif state == "In motion":
last_pose = robot.pose
if abs(m_x - 130) < 20 and abs(m_y - 90) < 10:
await robot.turn_in_place(degrees(15)).wait_for_completed()
else:
await robot.turn_in_place(degrees(15)).wait_for_completed()
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
reqConfidentFrames = 10
reqUnconfidentFrames = 10
############YOUR CODE HERE#################
condition = True
estimated = [0,0,0,False]
ballObj = Robot(0, 0, 0)
STATE = RobotStates.LOCALIZING
trueVal = 0
falseVal = 0
dist_to_ball = None
robotLen = 1
while (condition):
curr_pose = robot.pose
img = image_processing(robot)
(markers, ball) = cvt_2Dmarker_measurements(img)
dist_to_ball = calcDistance(ball)
if(dist_to_ball is not None and estimated[3]):
ballx = estimated[0] + math.cos(estimated[2]) * dist_to_ball
bally = estimated[1] + math.sin(estimated[2]) * dist_to_ball
ballObj.set_pos(ballx, bally)
#thinkg about false positives
#go to pose calculation
#How to make a pose??
#rotation plus position
#once localized we can tell what directino we are looking in and what position we are in
#add to cozmo's pose robot.Pose.pose.position in the direction of robot.Pose.pose.rotation
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_robot(ballObj)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
if STATE == RobotStates.LOCALIZING:
robot.drive_wheels(-5, -5)
if estimated[3]:
trueVal += 1
else:
trueVal = 0
if trueVal > reqConfidentFrames && dist_to_ball is not None:
robot.stop_all_motors()
STATE = RobotStates.TRAVELING
if STATE == RobotStates.TRAVELING:
h = math.degrees(math.atan2(goal[1] - estimated[1], goal[0] - estimated[0]))
dx = ballObj.x - estimated[0] - (math.cos(h) * robotLen)
dy = ballObj.y - estimated[1] - (math.sin(h) * robotLen)
dh = h - estimated[2]
our_go_to_pose(robot, curr_pose, dx, dy, dh)
if not estimated[3]:
falseVal += 1
else:
falseVal = 0
if trueVal > reqUnconfidentFrames:
robot.stop_all_motors()
STATE = RobotStates.LOCALIZING
STATE = RobotStates.SHOOT
if STATE == RobotStates.SHOOT:
pass
STATE == RobotStates.RESET
if STATE == RobotStates.RESET:
pass
STATE = RobotStates.TRAVELING
last_pose = curr_pose
async def run(robot: cozmo.robot.Robot):
global last_pose
global grid, gui
# global pf, state
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
await robot.set_head_angle(degrees(0)).wait_for_completed()
await robot.set_lift_height(0).wait_for_completed()
state = "unknown"
role = ""
storage_cube_mult = 1
############################################################################
######################### YOUR CODE HERE####################################
while True:
robo_odom = compute_odometry(robot.pose)
vis_markers = await image_processing(robot)
markers_2d = cvt_2Dmarker_measurements(vis_markers)
m_x, m_y, m_h, m_confident = ParticleFilter.update(
pf, robo_odom, markers_2d)
gui.show_mean(m_x, m_y, m_h, m_confident)
gui.show_particles(pf.particles)
robbie = Robot(robot.pose.position.x, robot.pose.position.y,
robot.pose_angle.degrees)
gui.show_robot(robbie)
gui.updated.set()
if m_confident:
state = "known"
#TODO: relax the constraints on m_confident
if state == "known":
if role == "":
if m_x < 13:
role = "pickup"
else:
role = "storage"
print(role)
print(m_x, m_y, m_h)
time.sleep(3)
h_offset = m_h - robot.pose_angle.degrees
h_offset_rad = math.radians(m_h - robot.pose_angle.degrees)
cube = None
print("wheels driven")
init_rx = robot.pose.position.x
init_ry = robot.pose.position.y
init_rh = robot.pose_angle.degrees
while cube is None:
try:
cube = await robot.world.wait_for_observed_light_cube(
timeout=.5, include_existing=False)
after_rx = robot.pose.position.x
after_ry = robot.pose.position.y
after_rh = robot.pose_angle.degrees
dx = .03937 * (after_rx - init_rx)
dy = .03937 * (after_ry - init_ry)
dh = after_rh - init_rh
new_x = m_x + (dx * math.cos(h_offset_rad)) - (
dy * math.sin(h_offset_rad))
new_y = m_y + (dx * math.sin(h_offset_rad)) + (
dy * math.cos(h_offset_rad))
new_h = m_h + dh
#init_rx = after_rx
#init_ry = after_ry
#init_rh = after_rh
print("rotated pose: " + str(new_x) + ", " + str(new_y) +
", " + str(new_h))
cube_pose = get_cube_global_pose(
robot, new_x, new_y, new_h,
cube.pose.position.x * .03937,
cube.pose.position.y * .03937)
#start if-else for diff areas:
if role == "pickup":
if cube_pose[0] >= 9 or cube_pose[1] <= 4:
print("Cube out of bounds! " + str(cube_pose))
cube = None
await robot.turn_in_place(degrees(15)
).wait_for_completed()
continue
else:
print("In bound cube pose! " + str(cube_pose))
else:
if cube_pose[0] <= 9 or cube_pose[
0] >= 17 or cube_pose[1] <= 4:
cube = None
print("Cube out of bounds! " + str(cube_pose))
await robot.turn_in_place(degrees(15)
).wait_for_completed()
continue
except:
await robot.turn_in_place(degrees(15)).wait_for_completed()
print(cube.pose)
time.sleep(3)
##Cube found
await robot.pickup_object(cube,
use_pre_dock_pose=False,
num_retries=6).wait_for_completed()
#TODO: drive to correct destination
#TODO: drop object
#TODO: return to starting position, look in right direction
after_rx = robot.pose.position.x
after_ry = robot.pose.position.y
after_rh = robot.pose_angle.degrees
dx = .03937 * (after_rx - init_rx)
dy = .03937 * (after_ry - init_ry)
dh = after_rh - init_rh
new_x = m_x + (dx * math.cos(h_offset_rad)) - (
dy * math.sin(h_offset_rad))
new_y = m_y + (dx * math.sin(h_offset_rad)) + (
dy * math.cos(h_offset_rad))
new_h = m_h + dh
print(new_x, new_y, new_h)
time.sleep(3)
if role == "pickup":
await move_dist_in_global_frame(robot, new_x, new_y, new_h, 12,
9)
await robot.place_object_on_ground_here(
cube, num_retries=5).wait_for_completed()
await robot.set_lift_height(0).wait_for_completed()
await move_dist_in_global_frame(
robot, 11, 9, robot.pose_angle.degrees - h_offset, 9, 9)
pf = ParticleFilter(grid)
state = "unknown"
else:
await move_dist_in_global_frame(robot, new_x, new_y, new_h, 23,
15 * storage_cube_mult)
await robot.place_object_on_ground_here(
cube, num_retries=5).wait_for_completed(
) #TODO: put second cube somewhere else
await robot.set_lift_height(0).wait_for_completed()
await move_dist_in_global_frame(
robot, 23, 15 * storage_cube_mult,
robot.pose_angle.degrees - h_offset, 23, 9)
storage_cube_mult -= .25
pf = ParticleFilter(grid)
state = "unknown"
elif state == "unknown":
last_pose = robot.pose
if abs(m_x - 130) < 20 and abs(m_y - 90) < 10:
await robot.turn_in_place(degrees(15)).wait_for_completed()
else:
await robot.turn_in_place(degrees(15)).wait_for_completed()
