def our_go_to_pose(robot, curr_pose, dx, dy, dh):
# goto = curr_pose.define_pose_relative_this(Pose(distance_inches(dx), distance_inches(dy), distance_inches(0), angle_z=degrees(dh)))
# robot.go_to_pose(goto)
goto_x = curr_pose.position.x + distance_inches(dx)
goto_y = curr_pose.position.y + distance_inches(dy)
goto_h = curr_pose.rotation.angle_z + degrees(dh)
robot.go_to_pose(Pose(goto_x, goto_y, distance_inches(0), angle_z=goto_h))
def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose, confident
global grid, gui
# start streaming
robot.camera.image_stream_enabled = True
#start particle filter
pf = ParticleFilter(grid)
###################
############YOUR CODE HERE#################
###################
robot.set_head_angle(radians(0)).wait_for_completed()
estimated = [0, 0, 0, False]
trueVal = 0
while True:
# print(robot.is_picked_up)
if risingEdge(flag_odom_init, robot.is_picked_up):
pf = ParticleFilter(grid)
elif robot.is_picked_up:
robot.drive_wheels(0, 0)
elif not robot.is_picked_up:
if trueVal < 35:
robot.drive_wheels(5, 20)
else:
robot.stop_all_motors()
gh = math.degrees(
math.atan2(goal[1] - estimated[1], goal[0] - estimated[0]))
ch = estimated[2]
dh = gh - ch
print("gh:", gh % 360, "ch:", ch % 360, "dh:", dh % 360)
tol = 0.01
if dh > tol or dh < -tol:
print("hi")
robot.turn_in_place(degrees(dh)).wait_for_completed()
robot.drive_straight(
distance_inches(
grid_distance(goal[0], goal[1],
estimated[0], estimated[1]) - 1.75),
speed_mmps(25)).wait_for_completed()
dh = goal[2] - estimated[2] - dh
print("dh2", dh % 360)
robot.turn_in_place(degrees(dh)).wait_for_completed()
return 0
curr_pose = robot.pose
img = image_processing(robot)
markers = cvt_2Dmarker_measurements(img)
# print(markers)
odom = compute_odometry(curr_pose)
estimated = pf.update(odom, markers)
gui.show_particles(pf.particles)
gui.show_mean(estimated[0], estimated[1], estimated[2], estimated[3])
gui.updated.set()
last_pose = curr_pose
if estimated[3]:
trueVal += 1
async def run(self):
await self.robot.drive_straight(distance_inches(1), speed_mmps(50), should_play_anim = False).wait_for_completed()
while True:
await asyncio.sleep(1)
print (self.robot.camera.config.fov_y)
print (self.robot.camera.config.fov_x)
if self.state == LOOK_AROUND_STATE:
await self.start_lookaround()
def action_on_seeing_object(robot: cozmo.robot.Robot):
'''This block uses a full size hexagon 5 printout to initiate the demo. Cozmo will drive straight for 20.5 inches,
in which times he will hit the wall. It'll then do some actions, then turn. Once cozmo has turned, it should see
the diamond 2 QR, and begin those actions upon the trigger'''
if isHex5[0]:
robot.drive_straight(distance_inches(20.5), speed_mmps(70)).wait_for_completed()
robot.say_text("What the????").wait_for_completed()
robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed()
robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed()
robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed()
robot.say_text("You gotta be kidding me!").wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
'''Once cozmo notices the diamond 2 image, it will begin the actions stated below. It will eventually do a full
180 degree turn, and head towards the hexagon 4 image. This will trigger cozmo's final set of instructions.'''
if isDiamond2[0]:
robot.drive_straight(distance_inches(10), speed_mmps(70)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession, ignore_body_track=True).wait_for_completed()
robot.say_text("Ugh.").wait_for_completed()
robot.say_text("Careful Cozmo....").wait_for_completed()
robot.drive_straight(distance_inches(2), speed_mmps(90)).wait_for_completed()
robot.drive_straight(distance_inches(-2), speed_mmps(65)).wait_for_completed()
robot.turn_in_place(degrees(180)).wait_for_completed()
'''Here cozmo will notice the hexagon 4 image, and should find his way out of the "maze". To signal that cozmo
has made it out, the event 'CubePounceWinSession is triggered.'''
if isHex4[0]:
robot.drive_straight(distance_inches(22), speed_mmps(70)).wait_for_completed()
robot.turn_in_place(degrees(-90)).wait_for_completed()
robot.say_text("How do I get out of here!?").wait_for_completed()
robot.drive_straight(distance_inches(2), speed_mmps(25)).wait_for_completed()
robot.turn_in_place(degrees(-180)).wait_for_completed()
robot.say_text("It's gotta be around here somewhere").wait_for_completed()
robot.drive_straight(distance_inches(8), speed_mmps(35)).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceWinSession, ignore_body_track=True).wait_for_completed()
async def move_dist_in_global_frame(robot, m_x, m_y, m_h, dest_x, dest_y):
#help
goal = [dest_x, dest_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 * .03937,
robot.pose.position.y * .03937) # 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]) #should be negative?
goal_in_B_y = (math.sin(theta_b_a) * goal_in_A[0] +
math.cos(theta_b_a) * goal_in_A[1] - t[1]
) #should be negative?
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
dist = (goal[0] - m_x, goal[1] - m_y)
delta_t = math.degrees(math.atan2(dist[1], dist[0])) - m_h
dist = math.sqrt(dist[0]**2 + dist[1]**2)
print("DISTANCE TO TARGET: {}", 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()
return m_h + delta_t
def action_on_seeing_object(robot: cozmo.robot.Robot):
if isDiamond2[0]:
rand = random.randint(0, 1)
#print("This is the random value")
#print(rand)
#_action_1 = _clad_to_engine_cozmo.RobotActionType.SAY_TEXT
wall = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.CustomObject, timeout=1)
if wall:
#action1[0]= robot.go_to_pose(wall[0].pose, relative_to_robot=False, in_parallel=False, num_retries=0).wait_for_completed()
if not action1:
print("I saw the wall, but am I moving")
#drive_action = _clad_to_engine_cozmo.RobotActionType.DRIVE_STRAIGHT
action1.append(
robot.drive_straight(distance_inches(14.5),
speed_mmps(30),
in_parallel=True))
#time.sleep(2.0)
#robot.say_text("What the????").wait_for_completed()
#action2 = robot.drive_straight(distance_inches(-3.5), speed_mmps(30)).wait_for_completed()
#if(rand == 0):
#action3 = robot.turn_in_place(degrees(90)).wait_for_completed()
#else:
#action3= robot.turn_in_place(degrees(-90)).wait_for_completed()
#robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed()
#robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed()
#robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed()
#robot.say_text("You gotta be kidding me!").wait_for_completed()
#robot.turn_in_place(degrees(90)).wait_for_completed()
#default_position_upon_start(robot);
if isCircle5[0]:
print("I saw the circle")
#action1._abort_action(self, action1, "Oh my message")
action1[0].abort()
action1[0].wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot, DEGREE):
if DEGREE != 0 :
robot.turn_in_place(DEGREEs(DEGREE)).wait_for_completed()
robot.drive_straight(distance_inches(4), speed_mmps(50)).wait_for_completed()
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()
def driveInches(robot, inches):
robot.drive_straight(distance_inches(inches), speed_mmps(50)).wait_for_completed()
async def run(robot: cozmo.robot.Robot):
global flag_odom_init, last_pose
global grid, gui, pf
# 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(0)).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
picked_up = False
count = 0
""" This requires a file to be in the same directory, functionality was demoed """
#angry_img = cozmo.oled_face.convert_image_to_screen_data(Image.open("angery.jpg").resize(cozmo.oled_face.dimensions(), Image.NEAREST))
while (True):
#robot.enable_stop_on_cliff(False)
await robot.say_text("").wait_for_completed()
if (robot.is_picked_up):
if (not picked_up):
at_goal = False
pf.particles = Particle.create_random(PARTICLE_COUNT, grid)
#await robot.display_oled_face_image(angry_img, 3000.0).wait_for_completed()
await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
picked_up = True
continue
if (at_goal):
continue
if (not flag_odom_init):
last_pose = robot.pose
flag_odom_init = True
continue
picked_up = False
curr_odom = compute_odometry(robot.pose)
last_pose = robot.pose
markers, camera_image = await marker_processing(robot, camera_settings, show_diagnostic_image=False)
m_x, m_y, m_h, m_confident = pf.update(curr_odom, markers)
#robot.pose = cozmo.util.Pose(m_x,m_y,0,angle_z=cozmo.util.Angle(degrees=m_h))
gui.show_particles(pf.particles)
gui.show_mean(0,0,0)
gui.show_camera_image(camera_image)
gui.updated.set()
count += 1
if (m_confident):
#robot.go_to_pose(cozmo.util.Pose(goal[0], goal[1], 0, angle_z=cozmo.util.Angle(degrees=goal[2])))
robot.stop_all_motors()
dx = goal[0] - m_x
dy = goal[1] - m_y
angle = math.atan2(dy,dx)
distance = grid_distance(dx,dy,0,0)
print(diff_heading_deg(math.degrees(angle), m_h))
await robot.turn_in_place(degrees(diff_heading_deg(math.degrees(angle), m_h))).wait_for_completed()
robot.wait_for_all_actions_completed()
await robot.drive_straight(distance_inches(distance), speed_mmps(140)).wait_for_completed()
robot.wait_for_all_actions_completed()
await robot.turn_in_place(degrees(-1*diff_heading_deg(-5,math.degrees(angle)))).wait_for_completed()
at_goal = True
print("Done")
else:
"""
if (len(markers) > 0):
await robot.turn_in_place(degrees(5)).wait_for_completed()
else:
await robot.turn_in_place(degrees(20)).wait_for_completed()
if (count % 20 == 0):
robot.stop_all_motors()
await robot.drive_straight(distance_inches(3), speed_mmps(100)).wait_for_completed()
"""
if (count % 5 == 0):
await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
if (count > 30 and len(markers) > 0):
robot.stop_all_motors()
await robot.drive_straight(distance_inches(markers[0][0] * 0.5), speed_mmps(100)).wait_for_completed()
await robot.turn_in_place(degrees(100)).wait_for_completed()
await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
count -= 30
"""
if (len(markers) > 0):
robot.drive_wheel_motors(-10,10,-1000,1000)
else:
robot.drive_wheel_motors(-20,20,-1000,1000)
"""
if (len(markers) > 0):
await robot.turn_in_place(degrees(10)).wait_for_completed()
else:
await robot.turn_in_place(degrees(20)).wait_for_completed()
def action_on_seeing_object(robot: cozmo.robot.Robot):
if isHex4[0]:
rand = random.randint(0, 2)
print("This is the random value")
print(rand)
wall = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.CustomObject, timeout=1)
if wall:
robot.go_to_pose(wall[0].pose,
relative_to_robot=False,
in_parallel=False,
num_retries=0).wait_for_completed()
robot.drive_straight(distance_inches(2.5),
speed_mmps(30)).wait_for_completed()
robot.say_text("What the????").wait_for_completed()
robot.drive_straight(distance_inches(-3.5),
speed_mmps(30)).wait_for_completed()
if (rand == 0):
robot.turn_in_place(degrees(90)).wait_for_completed()
else:
robot.turn_in_place(degrees(-90)).wait_for_completed()
#robot.drive_straight(distance_inches(-3), speed_mmps(70)).wait_for_completed()
#robot.drive_straight(distance_inches(3), speed_mmps(35)).wait_for_completed()
#robot.drive_straight(distance_inches(1), speed_mmps(85)).wait_for_completed()
#robot.say_text("You gotta be kidding me!").wait_for_completed()
#robot.turn_in_place(degrees(90)).wait_for_completed()
default_position_upon_start(robot)
if isDiamond2[0]:
wall = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.CustomObject, timeout=1)
if wall:
robot.go_to_pose(wall[0].pose,
relative_to_robot=False,
in_parallel=False,
num_retries=0).wait_for_completed()
robot.drive_straight(distance_inches(2.5),
speed_mmps(30)).wait_for_completed()
robot.play_anim_trigger(
cozmo.anim.Triggers.CubePounceLoseSession,
ignore_body_track=True).wait_for_completed()
#robot.say_text("Ugh.").wait_for_completed()
#robot.say_text("Careful Cozmo....").wait_for_completed()
#robot.turn_in_place(degrees(180)).wait_for_completed()
default_position_upon_start(robot)
if isHex5[0]:
wall = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.CustomObject, timeout=1)
if wall:
robot.go_to_pose(wall[0].pose,
relative_to_robot=False,
in_parallel=False,
num_retries=0).wait_for_completed()
robot.drive_straight(distance_inches(2.5),
speed_mmps(30)).wait_for_completed()
#robot.turn_in_place(degrees(-90)).wait_for_completed()
#robot.say_text("How do I get out of here!?").wait_for_completed()
#robot.drive_straight(distance_inches(2), speed_mmps(25)).wait_for_completed()
#robot.turn_in_place(degrees(-180)).wait_for_completed()
#robot.say_text("It's gotta be around here somewhere").wait_for_completed()
#robot.drive_straight(distance_inches(8), speed_mmps(35)).wait_for_completed()
#robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceWinSession, ignore_body_track=True).wait_for_completed()
default_position_upon_start(robot)
