def __init__(self, robot: cozmo.robot.Robot):
self.robot = robot
self.robot.camera.image_stream_enabled = True
self.robot.camera.color_image_enabled = True
self.fov_x = self.robot.camera.config.fov_x
self.fov_y = self.robot.camera.config.fov_y
self.robot.add_event_handler(cozmo.objects.EvtObjectTapped, self.on_cube_tap)
self.robot.add_event_handler(cozmo.world.EvtNewCameraImage, self.on_new_camera_image)
self.color_selector_cube = None # type: LightCube
self.color_to_find = 'yellow'
self.color_to_find_index = POSSIBLE_COLORS_TO_FIND.index(self.color_to_find)
self.grid_cube = None # type: LightCube
self.robot.world.image_annotator.add_annotator('color_finder', self)
self.robot.world.image_annotator.annotation_enabled = False
self.enabled = True
self.pixel_matrix = MyMatrix(DOWNSIZE_WIDTH, DOWNSIZE_HEIGHT)
self.amount_turned_recently = radians(0)
self.moving_threshold = radians(12)
self.state = LOOK_AROUND_STATE
self.look_around_behavior = None # type: LookAroundInPlace behavior
self.drive_action = None # type: DriveStraight action
self.tilt_head_action = None # type: SetHeadAngle action
self.rotate_action = None # type: TurnInPlace action
self.lift_action = None # type: SetLiftHeight action
self.last_known_blob_center = (DOWNSIZE_WIDTH / 2, DOWNSIZE_HEIGHT / 2) # initially set to center screen
self.white_balance_cube = None # type: LightCube
self.adjustment = None
def on_finding_a_blob(self, blob_center, blob_size):
'''Determines whether Cozmo should continue to look at the blob, or drive towards it.
Args:
blob_center (int, int): coordinates of the blob's center in self.pixel_matrix
blob_size (int): number of pixels in the blob
'''
self.robot.set_center_backpack_lights(
map_color_to_light[self.color_to_find])
if blob_size > (self.pixel_matrix.size / 4):
self.lift_action = self.robot.set_lift_height(0.0,
in_parallel=True)
x, y = blob_center
# 'fov' stands for 'field of view'. This is the angle amount
# that Cozmo can see to the edges of his camera view.
amount_to_move_head = radians(self.fov_y.radians *
(.5 - float(y) / DOWNSIZE_HEIGHT))
amount_to_rotate = radians(self.fov_x.radians *
(.5 - float(x) / DOWNSIZE_WIDTH))
if self.moved_too_far_from_center(amount_to_move_head,
amount_to_rotate):
self.state = FOUND_COLOR_STATE
if self.state != DRIVING_STATE:
self.turn_toward_blob(amount_to_move_head, amount_to_rotate)
else:
self.drive_toward_color_blob()
async def _final_adjust_async(self,
charger: Charger,
dist_charger=40,
speed=40,
critical=False) -> None:
# Final adjustement to properly face the charger.
# The position can be adjusted several times if
# the precision is critical, i.e. when climbing
# back onto the charger.
while (True):
# Calculate positions
r_coord = [0, 0, 0]
c_coord = [0, 0, 0]
# Coordonates of robot and charger
# .x .y .z, .rotation otherwise
r_coord[0] = self._robot.pose.position.x
r_coord[1] = self._robot.pose.position.y
r_coord[2] = self._robot.pose.position.z
r_zRot = self._robot.pose_angle.radians # .degrees or .radians
c_coord[0] = charger.pose.position.x
c_coord[1] = charger.pose.position.y
c_coord[2] = charger.pose.position.z
c_zRot = charger.pose.rotation.angle_z.radians
# Create target position
# dist_charger in mm, distance if front of charger
c_coord[0] -= dist_charger * math.cos(c_zRot)
c_coord[1] -= dist_charger * math.sin(c_zRot)
# Direction and distance to target position (in front of charger)
distance = math.sqrt((c_coord[0] - r_coord[0])**2 +
(c_coord[1] - r_coord[1])**2 +
(c_coord[2] - r_coord[2])**2)
vect = [
c_coord[0] - r_coord[0], c_coord[1] - r_coord[1],
c_coord[2] - r_coord[2]
]
# Angle of vector going from robot's origin to target's position
theta_t = math.atan2(vect[1], vect[0])
print('CHECK: Adjusting position')
# Face the target position
angle = self._clip_angle(theta_t - r_zRot)
await self._robot.turn_in_place(radians(angle)
).wait_for_completed()
# Drive toward the target position
await self._robot.drive_straight(
distance_mm(distance), speed_mmps(speed)).wait_for_completed()
# Face the charger
angle = self._clip_angle(c_zRot - theta_t)
await self._robot.turn_in_place(radians(angle)
).wait_for_completed()
# In case the robot does not need to climb onto the charger
if not critical:
break
elif (await self._check_tol_async(charger, dist_charger)):
print('CHECK: Robot aligned relativ to the charger.')
break
return
def turn_toward_last_known_blob(self):
'''Turns toward the coordinates of the last recorded blob in memory.
amount_to_rotate is multiplied to overshoot the object rather than undershoot it.
'''
x, y = self.last_known_blob_center
amount_to_move_head = radians(self.fov_y.radians*(.5-y/DOWNSIZE_HEIGHT))
amount_to_rotate = radians(self.fov_x.radians*(.5-x/DOWNSIZE_WIDTH)) * 4
self.turn_toward_blob(amount_to_move_head, amount_to_rotate)
def cozmo_program(robot: cozmo.robot.Robot):
robot.camera.image_stream_enabled = True
i = 0
transport, goal = lookAroundBehavior(robot)
print(transport, robot.pose, goal)
#if cube:
# cube.set_lights(cozmo.lights.green_light.flash())
##Robot pose
robot_pos, rangle = get_pose(robot)
##Object pose
object_pos, cangle = get_pose(transport)
x1 = robot_pos[0]
y1 = object_pos[1]
#loc = np.array([x1,y1,[0]])
##Controller initialization
ctrl = PIctrl()
#Set the reference point i.e object position
ctrl.ref_point = object_pos
# Get the direction to the object
robot_pos, rangle = get_pose(robot)
angle = get_direction(robot_pos, object_pos)
angle = angle - rangle
robot.turn_in_place(radians(np.pi + angle)).wait_for_completed()
print('robot turing completed')
exit()
#robot.turn_in_place(radians(np.pi + angle)).wait_for_completed()
#Set the current robot location
#ctrl.pos_update(robot_pos)
#Update the controller i.e get new values of v and w
#ctrl.update()
gotobehavior(robot, object_pos, th=1)
print('gotobehavior for object complte')
##goal pose
# Get the direction to the object
goal_pos, gangle = get_pose(goal)
robot_pos, rangle = get_pose(robot)
angle = get_direction(robot_pos, goal_pos)
angle = angle - rangle
print(angle)
#time.sleep(200)
robot.stop_all_motors()
robot.turn_in_place(radians(np.pi + angle)).wait_for_completed()
print('direction changed towards goal')
print('gotobehavior for goal started')
gotobehavior(robot, goal_pos, th=55)
robot.stop_all_motors()
"""
def cozmo_rotate(robot: cozmo.robot.Robot, scalar, speed=175):
"""
Asynchronous wrapper function that rotates cozmo.
:param robot: cozmo robot object
:param scalar: angle that cozmo should rotate
:param speed: speed that cozmo should rotate at
"""
# TODO: Change speed parameter to do some cool scalar math
robot.turn_in_place(angle=radians(-scalar),
num_retries=2,
speed=radians(speed)).wait_for_completed()
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False):
# Final adjustement to properly face the charger.
# The position can be adjusted several times if
# the precision is critical, i.e. when climbing
# back onto the charger.
global robot,PI
while(True):
# Calculate positions
r_coord = [0,0,0]
c_coord = [0,0,0]
# Coordonates of robot and charger
r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise
r_coord[1] = robot.pose.position.y
r_coord[2] = robot.pose.position.z
r_zRot = robot.pose_angle.radians # .degrees or .radians
c_coord[0] = charger.pose.position.x
c_coord[1] = charger.pose.position.y
c_coord[2] = charger.pose.position.z
c_zRot = charger.pose.rotation.angle_z.radians
# Create target position
# dist_charger in mm, distance if front of charger
c_coord[0] -= dist_charger*math.cos(c_zRot)
c_coord[1] -= dist_charger*math.sin(c_zRot)
# Direction and distance to target position (in front of charger)
distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2)
vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]]
# Angle of vector going from robot's origin to target's position
theta_t = math.atan2(vect[1],vect[0])
print('CHECK: Adjusting position')
# Face the target position
angle = clip_angle((theta_t-r_zRot))
robot.turn_in_place(radians(angle)).wait_for_completed()
# Drive toward the target position
robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed()
# Face the charger
angle = clip_angle((c_zRot-theta_t))
robot.turn_in_place(radians(angle)).wait_for_completed()
# In case the robot does not need to climb onto the charger
if not critical:
break
elif(check_tol(charger,dist_charger)):
print('CHECK: Robot aligned relativ to the charger.')
break
return
def addStaticObject(self, model, x1, y1, x2, y2, depth, height):
print("[Bot] Executing addStaticObject({},{},{},{},{},{})".format(
x1, y1, x2, y2, depth, height))
data = {
'addStaticObject': {
'model': model,
'x1': x1,
'y1': y1,
'x2': x2,
'y2': y2,
'depth': depth,
'height': height
}
}
self._wsClient.send(json.dumps(data))
X1 = x1 * 10
Y1 = y1 * 10
X2 = x2 * 10
Y2 = y2 * 10
HEIGHT = height * 10
DEPTH = depth * 10
WIDTH = math.sqrt(math.pow(X1 - X2, 2) + math.pow(Y1 - Y2, 2))
centerX = (X1 + X2) / 2.0
centerY = (Y1 + Y2) / 2.0
centerZ = HEIGHT / 2.0
angle = math.atan2(Y1 - Y2, X1 - X2)
pose = Pose(centerX, centerY, centerZ, angle_z=radians(angle))
self._robot.world.create_custom_fixed_object(
self._origin.define_pose_relative_this(pose), WIDTH, DEPTH, HEIGHT)
async def update(self, robot):
"""
Executes the state's behavior for the current tick.
Args:
robot: The object to affect behavior for.
Returns:
If the object's state should be changed, returns the class of the new state.
Otherwise, return None.
"""
if not self.turned:
rotation_rad = math.radians(robot.rotation)
turn = planning.getTurnDirection(math.cos(rotation_rad),
math.sin(rotation_rad),
robot.position, robot.ball)
robot.stop_all_motors()
await robot.turn_in_place(radians(turn),
num_retries=3).wait_for_completed()
robot.add_odom_rotation(robot, math.degrees(turn))
robot.ball = None
robot.ball_grid = None
self.turned = True
elif not self.hit and (robot.ball is not None or self.wait <= 0):
robot.stop_all_motors()
await hitBall(robot, self.distance_multiplier)
robot.ball = None
robot.ball_grid = None
robot.localized = False
self.hit = True
elif self.wait <= -HitBall.WAIT_TIME:
return Search()
self.wait -= 1
def __handle_move_head(self, command, agent):
"""
Handle a Soar move-head action.
The Soar output should look like:
(I3 ^move-head Vx)
(Vx ^angle [ang])
where [ang] is a real number in the range [-0.44, 0.78]. This command moves the head to the
the given angle, where 0 is looking straight ahead and the angle is radians from that
position.
:param command: Soar command object
:param agent: Soar Agent object
:return: True if successful, False otherwise
"""
try:
angle = float(command.GetParameterValue("angle"))
except ValueError as e:
print("Invalid angle format {}".format(command.GetParameterValue("angle")))
return False
print("Moving lift {}".format(angle))
set_head_angle_action = self.robot.set_head_angle(radians(angle), in_parallel=True)
self.running_actions.append((command, set_head_angle_action))
# callback = self.__handle_action_complete_factory(command)
# set_head_angle_action.add_event_handler(EvtActionCompleted, callback)
return True
async def run(self):
'''Program runs until typing CRTL+C into Terminal/Command Prompt,
or by closing the viewer window.
'''
if not self.cubes_connected():
print('Cubes did not connect successfully - check that they are nearby. You may need to replace the batteries.')
return
self.turn_on_cubes()
await self.robot.drive_straight(distance_mm(100), speed_mmps(50), should_play_anim = False).wait_for_completed()
# Updates self.state and resets self.amount_turned_recently every 1 second.
# MODIFICATION FROM ORIGINAL SDK = while loop would run contiously until an abort signal was dectected.
# variable running remains True until Cozmo detects the color passed and changes to False
# if False, then run returns giving control back to main.py
while running:
await asyncio.sleep(1)
print(self.color_to_find)
print(self.state)
print(running)
if self.state == LOOK_AROUND_STATE:
await self.start_lookaround()
print("In look around state")
if self.state == FOUND_COLOR_STATE and self.amount_turned_recently < self.moving_threshold:
self.state = DRIVING_STATE
self.amount_turned_recently = radians(0)
if running == False:
self.robot.abort_all_actions()
return
def publish_cube(self, publisher, color=(0, 0.5, 0.5, 1)):
"""
Publishes the object as a cube Marker
Parameters
---------
publisher : ros publisher
color : tuple
(r, g, b, a) to represent the color of the cube
"""
cube_marker = Marker()
cube_marker.header.frame_id = "base_link"
cube_marker.type = Marker.CUBE
cube_marker.pose.position.x = self.pose[0] / 1000
cube_marker.pose.position.y = self.pose[1] / 1000
cube_marker.pose.position.z = self.height / 1000
cube_orientation = angle_z_to_quaternion(radians(self.pose[2]))
cube_marker.pose.orientation.x = cube_orientation[1]
cube_marker.pose.orientation.y = cube_orientation[2]
cube_marker.pose.orientation.z = cube_orientation[3]
cube_marker.pose.orientation.w = cube_orientation[0]
cube_marker.scale.x = self.width / 1000
cube_marker.scale.y = self.length / 1000
cube_marker.scale.z = self.width / 1000
cube_marker.color.r = color[0]
cube_marker.color.g = color[1]
cube_marker.color.b = color[2]
cube_marker.color.a = color[3]
publisher.publish(cube_marker)
async def CozmoPlanning(robot: cozmo.robot.Robot):
# Allows access to map and stopevent, which can be used to see if the GUI
# has been closed by checking stopevent.is_set()
global cmap, stopevent
########################################################################
# TODO: please enter your code below.
# Set start node
# cmap.set_start(start_node)
# Start RRT
RRT(cmap, cmap.get_start())
path = cmap.get_smooth_path()
index = 1
# Path Planning for RRT
while index < len(path):
curr_node = path[index]
dx = curr_node.x - path[index - 1].x
dy = curr_node.y - path[index - 1].y
# turn_angle = diff_heading_deg(cozmo.util, np.arctan2(dy, dx))
# print("dx=", dx)
# print("dy=", dy)
# print("tan=", np.arctan2(dy, dx))
await robot.turn_in_place(
radians(np.arctan2(dy, dx)),
angle_tolerance=Angle(1)).wait_for_completed()
await robot.drive_straight(
distance_mm(np.sqrt((dx**2) + (dy**2))),
speed=cozmo.util.speed_mmps(50)).wait_for_completed()
index += 1
Visualizer.update(cmap)
async def drive_to_goal(cmap, robot, observed_cubes):
found_goal = False
while not found_goal:
print(cmap.get_start().coord)
cmap.reset()
print(cmap.get_start().coord)
RRT(cmap, cmap.get_start())
# 3. follow RRT path to target_cube face
smooth_path = cmap.get_smooth_path()
for i in range(1, len(smooth_path)):
# get current and next node references
current_node = smooth_path[i - 1]
next_node = smooth_path[i]
# get displacement:
# x and y components of difference vector.
dx = next_node.x - current_node.x
dy = next_node.y - current_node.y
# use angle between
angle = angle_from_xaxis(current_node, next_node)
# rotate to see direction to travel in
#await robot.go_to_pose(cozmo.util.Pose(robot.pose.position.x, robot.pose.position.y, 0, angle_z=radians(angle)), relative_to_robot=False).wait_for_completed()
await robot.turn_in_place(
angle=radians(diff_heading_rad(angle,
robot.pose_angle.radians)),
speed=cozmo.util.Angle(90)).wait_for_completed()
#await robot.go_to_pose(cozmo.util.Pose(robot.pose.position.x + dx, robot.pose.position.y + dy, 0, angle_z=radians(angle)), relative_to_robot=False).wait_for_completed()
speed = cozmo.util.speed_mmps(100)
await robot.drive_straight(distance=cozmo.util.distance_mm(
get_dist(current_node, next_node)),
speed=speed).wait_for_completed()
if i == len(smooth_path) - 1:
found_goal = True
def my_go_to_pose3(robot, x, y, angle_z): """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 """ dest_x = x + robot.pose.position.x dest_y = y + robot.pose.position.y dist = 100 while(dist > 20): dx, dy = delta(dest_x, dest_y, robot) theta_r = robot.pose.rotation.angle_z.degrees dist = p = math.sqrt( dx ** 2 + dy ** 2) p = clamp(250, p, 300) a = theta_r - radians(math.atan2(dy,dx)).degrees # Normalize drive to goal to not do a 180 if a > 90: a -= 180 p *= -1 elif a < -90: a += 180 p *= -1 v = 0.5 * p theta = 1.0 * a l,r = v + theta , v - theta robot.drive_wheels(l, r, duration = 0.2) my_turn_in_place(robot, angle_z - robot.pose.rotation.angle_z.degrees, 90)
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
def my_go_to_pose2(robot, x, y, angle_z):
"""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
"""
dest_x = x + robot.pose.position.x
dest_y = y + robot.pose.position.y
while(1):
dx, dy = delta(dest_x, dest_y, robot)
theta_r = robot.pose.rotation.angle_z.degrees
p = math.sqrt( dx ** 2 + dy ** 2)
a = theta_r - radians(math.atan2(dy,dx)).degrees
n = angle_z - theta_r
bp = p < 40
bn = abs(n) < 15
if bp and bn:
return
# Normalize turn to goal angle to the shortest path
if n > 180:
n -= 360
elif n < -180:
n += 360
# Normalize drive to goal to not do a 180
if a > 90:
a -= 180
p *= -1
elif a < -90:
a += 180
p *= -1
t = 100
if (abs(p) < t):
a *= (abs(p)/t)
n *= 1 - (abs(p)/t)
else:
n = 0
v = 0.5 * p
theta = 1.0 * (1.0 * a - 1.0 * n)
l,r = v + theta , v - theta
robot.drive_wheels(l, r, duration = 0.2)
print("%8.2f,%8.2f %7.2f,%7.2f,%7.2f %8.2f,%8.2f %s,%s" % (dx,dy, p,a,n, l,r, bp,bn))
# ####
# TODO: Implement a function that makes the robot move to a desired pose
# using the robot.drive_wheels() function to jointly move and rotate the
# robot to reduce distance between current and desired pose (Approach 2).
# ####
pass
async def loop(self, robot: cozmo.robot.Robot):
if robot.is_on_charger:
await robot.drive_off_charger_contacts().wait_for_completed()
self.initialPose = robot.pose
self.poseTrack = self.track.getPoseTrack(self.forwardSpeed)
self.driveOnRoad = True
self.stopped = False
await robot.drive_wheels(self.forwardSpeed, self.forwardSpeed)
print(self.poseTrack.edge.end.id)
while not self.stopped:
# basic update of straight drive
# picking next edge if the current is finished
if self.driveOnRoad:
self.poseTrack.update(FRAME_DURATION, self.forwardSpeed)
# back to starting point
if self.poseTrack.consumeRouteEndSignal():
self.driveOnRoad = False
# stop before turn
robot.stop_all_motors()
await robot.go_to_pose(self.initialPose).wait_for_completed()
await robot.drive_wheels(self.forwardSpeed, self.forwardSpeed)
# turning is included in go to pose
self.poseTrack.consumeEdgeChangeSignal()
self.driveOnRoad = True
print("fix position at initial pose")
print(self.poseTrack.edge.end.id)
# at any other cross or corner
elif self.poseTrack.consumeEdgeChangeSignal():
self.driveOnRoad = False
diff = self.poseTrack.angleDiff
angleAbs = self.poseTrack.angleAbs
# make sure the picked next
if diff < -0.1 or diff > 0.1:
# stop before turn
robot.stop_all_motors()
# make a turn
angle = radians(angleAbs +
self.initialPose.rotation.angle_z.radians -
robot.pose.rotation.angle_z.radians)
await robot.turn_in_place(angle).wait_for_completed()
print("make a turn")
# restart motion
await robot.drive_wheels(self.forwardSpeed,
self.forwardSpeed)
self.driveOnRoad = True
print(self.poseTrack.edge.end.id)
# let Cozmo drive straight for a short time before updates
await asyncio.sleep(FRAME_DURATION)
robot.stop_all_motors()
async 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))
estimated = [0, 0, 0, False]
cont = True
while cont:
# print(robot.is_picked_up)
if risingEdge(flag_odom_init, robot.is_picked_up):
pf = ParticleFilter(grid)
elif robot.is_picked_up:
await robot.drive_wheels(0, 0)
elif not robot.is_picked_up:
if not estimated[3]:
await robot.drive_wheels(5, 20)
else:
cont = False
# print("confident", confident, estimated[3])
# if risingEdge(confident, estimated[3]):
# print("confident after", confident)
# await robot.drive_wheels(0, 0)
dh = math.degrees(math.atan2(goal[1], goal[0]) - estimated[2])
print("dh", dh)
# tol = 0.01
# if dh > tol and dh < -tol:
robot.turn_in_place(degrees(dh))
await robot.drive_wheels(20, 20)
cont = grid_distance(goal[0], goal[1], estimated[0],
estimated[1]) > 0.1
if not cont:
dh = goal[2] - estimated[2]
robot.turn_in_place(dh)
curr_pose = robot.pose
img = await 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
def _resetHead(self):
action1 = self._cozmo.set_head_angle(radians(0.20),
duration=0.1,
in_parallel=True)
action1.wait_for_completed()
action2 = self._cozmo.set_lift_height(0,
duration=0.2,
in_parallel=True)
action2.wait_for_completed()
def start(self, event=None):
super().start(event)
pf_pose = self.robot.world.particle_filter.pose
current_pose = Pose(pf_pose[0], pf_pose[1], 0, angle_z=radians(pf_pose[2]))
pose_diff = current_pose - self.parent.target_pose
distance = (pose_diff.position.x**2 + pose_diff.position.y**2) ** 0.5
MAX_TARGET_DISTANCE = 50.0 # mm
if distance <= MAX_TARGET_DISTANCE:
self.post_success()
else:
self.post_failure()
def _move_head(self, cmd):
"""
Move head to given angle.
:type cmd: Float64
:param cmd: The message containing angle in degrees. [-25 - 44.5]
"""
action = self._cozmo.set_head_angle(radians(cmd.data * np.pi / 180.), duration=0.1,
in_parallel=True)
action.wait_for_completed()
def initialState(p, robot: cozmo.robot.Robot):
'''
The robot drive to the first position with the right orientation
'''
[vr, vl] = [25, 24]
angle = robot.pose.rotation.angle_z.radians
orientation = predic.orientationTarget(p[0], p[1])
firstAngle = alg.angleBetweenVectors(
[10, 0], orientation) #Robot heading towoard the next posiition
#The robot drive to the first position
robot.go_to_pose(Pose(p[0][0], p[0][1], 0,
angle_z=radians(firstAngle))).wait_for_completed()
return [vr, vl, firstAngle]
def turnTowardCube(self, cube_num):
if not self.getCubeSeen(cube_num):
print(
"[Bot] Ignoring turnTowardCube() as the cube has not been observed yet"
)
return False
print("[Bot] Executing turn toward cube")
cube = self._robot.world.light_cubes[cube_num]
pos = self._robot.pose.position - cube.pose.position
angle = radians(math.atan2(pos.y, pos.x) -
math.pi) - self._robot.pose.rotation.angle_z
res = self._robot.turn_in_place(angle).wait_for_completed()
print("[Bot] turn finished")
return res.state == cozmo.action.ACTION_SUCCEEDED
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
found = 0
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if ball is not None and ball[0] != 0:
found = found + 1
look_around.stop()
b = robot.camera.config.fov_x
amount_to_rotate = radians(b.radians *
(.5 - float(ball[0]) / 320))
await robot.turn_in_place(
amount_to_rotate, in_parallel=True).wait_for_completed()
await robot.drive_straight(
distance_mm(50),
speed_mmps(30),
should_play_anim=False,
in_parallel=True).wait_for_completed()
elif found > 4:
await robot.set_lift_height(
1.0, in_parallel=True).wait_for_completed()
break
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
def my_go_to_pose1(robot, x, y, angle_z): """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 """ angle = radians(math.atan2(y,x)).degrees my_turn_in_place(robot, angle, 60) time.sleep(1) my_drive_straight(robot, math.sqrt(x*x + y*y), 50) time.sleep(1) my_turn_in_place(robot, angle_z - angle, 60) time.sleep(1)
async def run(self):
'''Program runs until typing CRTL+C into Terminal/Command Prompt,
or by closing the viewer window.
'''
if not self.cubes_connected():
print('Cubes did not connect successfully - check that they are nearby. You may need to replace the batteries.')
return
self.turn_on_cubes()
await self.robot.drive_straight(distance_mm(100), speed_mmps(50), should_play_anim=False).wait_for_completed()
# Updates self.state and resets self.amount_turned_recently every 1 second.
while True:
await asyncio.sleep(1)
if self.state == LOOK_AROUND_STATE:
await self.start_lookaround()
if self.state == FOUND_COLOR_STATE and self.amount_turned_recently < self.moving_threshold:
self.state = DRIVING_STATE
self.amount_turned_recently = radians(0)
async def update(self, robot):
"""
Executes the state's behavior for the current tick.
Args:
robot: The object to affect behavior for.
Returns:
If the object's state should be changed, returns the class of the new state.
Otherwise, return None.
"""
if self.first:
robot.was_turning = False
robot.was_driving = False
rotation_rad = math.radians(robot.rotation)
rotation_cos = math.cos(rotation_rad)
rotation_sin = math.sin(rotation_rad)
if robot.was_driving:
speed_delta = robot.delta_time * robot.ROBOT_SPEED
robot.add_odom_position(
robot,
(rotation_cos * speed_delta, rotation_sin * speed_delta))
robot.grid.setStart(robot.grid_position)
else:
robot.drive_timer = robot.DRIVE_COOLDOWN
if robot.was_turning:
robot.add_odom_rotation(robot, robot.TURN_YAW * robot.delta_time)
changed = False
if robot.ball is not None:
if robot.prev_ball is not None:
robot.ball_grid = robot.grid.worldToGridCoords(robot.ball)
robot.ball_prev_grid = robot.grid.worldToGridCoords(
robot.prev_ball)
changed = robot.ball_grid != robot.ball_prev_grid
else:
changed = True
if not changed and robot.prev_grid_position != robot.grid_position:
changed = True
if self.first:
changed = True
self.first = False
rounded_grid = (round(robot.grid_position[0]),
round(robot.grid_position[1]))
if changed:
robot.grid.clearObstacles()
if robot.ball is not None:
grid_points = getGridPoints(robot.ball_grid[0],
robot.ball_grid[1], robot)
for point in grid_points:
if robot.grid.coordInBounds(point):
robot.grid.addObstacle(point)
# Wall obstacles.
for i in range(0, robot.grid.width):
robot.grid.addObstacle((i, 0))
robot.grid.addObstacle((i, robot.grid.height - 1))
for i in range(1, robot.grid.height - 1):
robot.grid.addObstacle((0, i))
robot.grid.addObstacle((robot.grid.width - 1, i))
goal_to_ball = np.subtract(robot.ball, robot.goal_position)
goal_distance = np.linalg.norm(goal_to_ball)
if goal_distance == 0:
return
goal_direction = np.divide(goal_to_ball, goal_distance)
goal_direction = np.multiply(
goal_direction, (robot.RADIUS + robot.BALL_RADIUS) * 1.2)
robot.target_position = np.add(robot.ball, goal_direction)
robot.target_position = robot.grid.worldToGridCoords(
robot.target_position)
if robot.target_position is not None:
robot.grid.clearGoals()
robot.grid.setStart(rounded_grid)
rounded_target = (round(robot.target_position[0]),
round(robot.target_position[1]))
robot.grid.addGoal(rounded_target)
astar(robot.grid, heuristic)
path = robot.grid.getPath()
robot.was_turning = False
if path is not None and len(path) > 1:
robot.next_cell = path[0]
if path[0] == rounded_grid:
robot.next_cell = path[1]
turn = getTurnDirection(rotation_cos, rotation_sin, rounded_grid,
robot.next_cell)
if abs(turn) > robot.TURN_THRESHOLD and abs(
2 * math.pi - abs(turn)) > robot.TURN_THRESHOLD:
robot.stop_all_motors()
await robot.turn_in_place(radians(turn),
num_retries=3).wait_for_completed()
robot.add_odom_rotation(robot, math.degrees(turn))
robot.was_driving = False
else:
await robot.drive_wheels(robot.ROBOT_SPEED, robot.ROBOT_SPEED,
robot.ROBOT_ACCELERATION,
robot.ROBOT_ACCELERATION)
robot.was_driving = True
else:
robot.was_driving = False
turn = getTurnDirection(rotation_cos, rotation_sin,
robot.grid_position, robot.target_position)
robot.stop_all_motors()
if abs(turn) > robot.TURN_THRESHOLD and abs(
2 * math.pi - abs(turn)) > robot.TURN_THRESHOLD:
await robot.turn_in_place(radians(turn),
num_retries=3).wait_for_completed()
robot.add_odom_rotation(robot, math.degrees(turn))
robot.stop_all_motors()
distance = grid_distance(
robot.grid_position[0], robot.grid_position[1],
robot.target_position[0],
robot.target_position[1]) * robot.grid.scale
await robot.drive_straight(
distance_mm(distance),
speed_mmps(robot.HIT_SPEED),
should_play_anim=False).wait_for_completed()
robot.add_odom_forward(robot, distance)
turn = getTurnDirection(rotation_cos, rotation_sin,
robot.grid_position, robot.ball_grid)
robot.stop_all_motors()
if abs(turn) > robot.TURN_THRESHOLD and abs(
2 * math.pi - abs(turn)) > robot.TURN_THRESHOLD:
await robot.turn_in_place(radians(turn),
num_retries=3).wait_for_completed()
robot.add_odom_rotation(robot, math.degrees(turn))
return goto_ball.HitBall()
def matrix_to_pose(rel):
rz = acos(rel[0, 0])
if rel[1, 0] < 0: # sin(theta) < 0 -> theta < 0
rz = -rz
return Pose(rel[0, -1], rel[1, -1], rel[2, -1], angle_z=radians(rz))
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment description for details
Has global access to grid, a CozGrid instance created by the main thread, and
stopevent, a threading.Event instance used to signal when the main thread has stopped.
You can use stopevent.is_set() to check its status or stopevent.wait() to wait for the
main thread to finish.
Arguments:
robot -- cozmo.robot.Robot instance, supplied by cozmo.run_program
"""
global grid, stopevent
#set/import initial grid conditions, initialize variables. set a default initial goal to be the center of the map.
botPoint = (3, 2)
grid.setStart(botPoint)
grid.addGoal((13, 9))
cubes = set()
goalCubePos = None
goalScalar = -1
cubeFootprint = [-2, -1, 0, 1, 2]
scale = 25
width = 26
height = 18
repath = False
#reset the robot for its journey
robot.image_stream_enabled = True
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.move_lift(-3)
speed = 27
previousHeading = 0
while not stopevent.is_set():
#if repath == True:
grid.clearStart()
grid.setStart(botPoint)
# repath = False;
#run astar search
astar(grid, heuristic)
path = grid.getPath()
#where are we now?
if len(path) - path.index(botPoint) == 1:
endPoint = botPoint
else:
endPoint = path[path.index(botPoint) + 1]
print("cur: {0}".format(botPoint))
print("end: {0}".format(endPoint))
#find the cube(s)
for cube in robot.world.visible_objects:
cubeID = cube.object_id
#if this is a 'new' cube...
if cubeID not in cubes:
cubex = cube.pose.position.x
cubey = cube.pose.position.y
#normalize the position to the size of the grid and make sure that any cubes are located INSIDE the grid
cubex = min(math.ceil((cubex) / scale) + 2, width)
cubey = min(math.ceil((cubey) / scale) + 2, height)
#mark grid squares as obstacles, based on the normalized cube position and the footprint of the cube
for xi in cubeFootprint:
for yi in cubeFootprint:
curx = cubex + xi
cury = cubey + yi
#only mark a grid square as containing an obstacle if the square is actually inside the grid
if curx >= 0 and curx <= width and cury >= 0 and cury <= height:
grid.addObstacle((curx, cury))
grid.clearVisited()
grid.setStart(endPoint)
print("Found a cube: {0}".format(cubeID))
#add this cube to the master set of cubes
cubes.add(cubeID)
#special casing if we realize that this is the goal cube
#if robot.world.light_cubes[cozmo.objects.LightCube1Id].object_id == cubeID:
if cubeID == 1:
goalCubePos = (cubex, cubey)
#adjust the bot's final position based on the rotational position of the cube so that the bot can approach it properly
theta = cube.pose.rotation.angle_z.radians + math.pi
goalx = cubex + (round(math.cos(theta)) *
math.floor((len(cubeFootprint) / 2) + 1))
goaly = cubey + (round(math.sin(theta)) *
math.floor((len(cubeFootprint) / 2) + 1))
#get rid of any stale goals (e.g. center of the grid) and add the goal position
print("Found Goal Cube at: " + str(goalx) + ", " +
str(goaly))
grid.clearGoals()
grid.addGoal((goalx, goaly))
print(grid.getGoals())
repath = True
grid.clearStart()
grid.setStart(botPoint)
grid.clearVisited()
# run astar search
astar(grid, heuristic)
path = grid.getPath()
# where are we now?
if len(path) - path.index(botPoint) == 1:
endPoint = botPoint
else:
endPoint = path[path.index(botPoint) + 1]
print("bot: {0}".format(botPoint))
print("end: {0}".format(endPoint))
#if repath == False:
#get the vector of displacement between the endpoint and the bot's current point
dispVector = (endPoint[0] - botPoint[0], endPoint[1] - botPoint[1])
#get a scalar measure of how far the endpoint is from the bot's current point
dispScalar = math.sqrt(
math.pow(dispVector[0], 2) + math.pow(dispVector[1], 2))
#atan2(y,x) gives the absolute angle of a vector, so we can use it to get the best heading
heading = math.atan2(dispVector[1], dispVector[0])
#we turn based on the difference between the desired heading and the bot's previous heading
turn = heading - previousHeading
previousHeading = heading
if dispScalar != 0:
#turn and move the bot
robot.turn_in_place(radians(turn)).wait_for_completed()
robot.drive_wheels(speed, speed, duration=2 * dispScalar)
if goalCubePos is not None:
goalVector = (goalCubePos[0] - botPoint[0],
goalCubePos[1] - botPoint[1])
goalScalar = math.sqrt(
math.pow(goalVector[0], 2) + math.pow(goalVector[1], 2))
#if we've reached the center of the grid and still haven't found the goal cube, slowly turn in place (until we find it)
if dispScalar == 0 and goalCubePos is None:
robot.turn_in_place(degrees(15)).wait_for_completed()
print("Turning in place til we find the cube")
#if we HAVE found the goal cube, and we've reached the endpoint...
elif dispScalar == 0 and goalCubePos is not None and goalScalar == 0:
#get the heading and turn to face the cube
heading = math.atan2(goalCubePos[1] - endPoint[1],
goalCubePos[0] - endPoint[0])
turn = heading - previousHeading
robot.turn_in_place(radians(turn)).wait_for_completed()
print("Reached goal cube")
#and we're done!
robot.set_all_backpack_lights(cozmo.lights.blue_light)
robot.turn_in_place(degrees(720))
robot.play_audio(cozmo.audio.AudioEvents.SfxGameWin)
time.sleep(5)
break
#update the bot's point to be the endpoint it just navigated to
botPoint = endPoint
async def run(self, robot: cozmo.robot.Robot):
self.robot = robot
print("BATTERY LEVEL: %f" % self.robot.battery_voltage)
# add handlers
self.robot.world.add_event_handler(cozmo.objects.EvtObjectObserved,
self.cube_observed_handler)
# setup the audio
pygame.mixer.init()
pygame.mixer.music.load('assets/shuffling_music.wav')
cubes = []
non_friend_cubes = []
friend_cube = None
while True:
print(self.state)
if self.state == States.LOOKING_FOR_CUBES:
await self.robot.play_anim("anim_explorer_driving01_end_01"
).wait_for_completed()
await self.robot.set_head_angle(degrees(0)
).wait_for_completed()
await self.robot.set_lift_height(
0, duration=0.2).wait_for_completed()
cubes = await self.wait_for_three_cubes()
# point to face the center cube
center_cube = find_center_cube(cubes)
angle = radians(angle_to_cube(self.robot.pose, center_cube))
await self.robot.turn_in_place(angle).wait_for_completed()
self.state = States.PICKING_CUBE
elif self.state == States.PICKING_CUBE:
# friend picks the cube we're going to track
tap_events = []
for cube in cubes:
cube.start_light_chaser(cozmo.lights.blue_light)
tap_events.append(cube.wait_for_tap())
tap_event = await cozmo.event.wait_for_first(*tap_events)
friend_cube = tap_event.obj
friend_cube.stop_light_chaser()
# we've got the right one. turn the others off
for cube in cubes:
cube.stop_light_chaser()
cube.set_light(cozmo.lights.off_light)
# make the clubes blink so friend knows to start
await blink_cubes(cubes, cozmo.lights.green_light, 2)
# prep work so guessing correct/incorrect is easy
cubes.remove(friend_cube)
friend_cube.set_light(cozmo.lights.blue_light)
non_friend_cubes = cubes
self.state = States.READY_ANIM
elif self.state == States.READY_ANIM:
# tell friend we're ready with an animation, and by flashing the cubes
await self.robot.play_anim("anim_speedtap_findsplayer_01"
).wait_for_completed()
pygame.mixer.music.play()
self.visible_cube_count = 0
self.state = States.WATCHING
elif self.state == States.WATCHING:
# turn around a bit while we watch
look_around_gen = small_random_angle()
current_angle = 0
for i in range(3):
current_angle, robot_angle = next(look_around_gen)
await self.robot.turn_in_place(degrees(robot_angle)
).wait_for_completed()
await self.robot.play_anim("anim_explorer_driving01_end_01"
).wait_for_completed()
# turn back to center
await self.robot.turn_in_place(degrees(-current_angle)
).wait_for_completed()
shuffle_guess_rate = max(
self.min_guess_rate,
self.visible_cube_count / self.max_visible_cube_count)
self.correct_guess_rate = min(1, shuffle_guess_rate)
print(self.correct_guess_rate, self.visible_cube_count)
self.visible_cube_count = 0
self.state = States.WATCHING
self.state = States.REFINDING_CUBES
elif self.state == States.REFINDING_CUBES:
# try to find the cubes again
# back up so we are more likely to see them
await self.robot.drive_straight(
distance_mm(-40), speed_mmps(90)).wait_for_completed()
cubes = await self.wait_for_three_cubes(show_colors=False)
# determine the order
self.state = States.GUESSING
elif self.state == States.GUESSING:
# show friend we're guessing
await self.robot.play_anim("anim_meetcozmo_lookface_02"
).wait_for_completed()
# pick a cube
if random.random() < self.correct_guess_rate and friend_cube:
guessed_cube = friend_cube
self.state = States.CORRECT
else:
guessed_cube = non_friend_cubes[random.randint(0, 1)]
self.state = States.INCORRECT
# go flip the cube
await flip_cube(self.robot, guessed_cube)
await asyncio.sleep(0.5)
elif self.state == States.INCORRECT:
for cube in cubes:
cube.set_light(cozmo.lights.red_light)
await self.robot.drive_wheels(-200, -200, duration=0.5)
await self.robot.play_anim("anim_reacttocliff_stuckleftside_01"
).wait_for_completed()
self.state = States.DONE
elif self.state == States.CORRECT:
for cube in cubes:
cube.start_light_chaser(cozmo.lights.green_light)
await self.robot.play_anim(
"anim_speedtap_wingame_intensity02_01"
).wait_for_completed()
for cube in cubes:
cube.stop_light_chaser()
self.state = States.DONE
elif self.state == States.DONE:
print("PRESS ENTER TO PLAY AGAIN, press q then enter to quit")
key = input()
if key == 'q':
await asyncio.sleep(1)
return
else:
await asyncio.sleep(1)
self.state = States.LOOKING_FOR_CUBES