async def localize(self, turn_angle=20):
print("-" * 20 + "LOCALIZING" + "-" * 20)
# reset our location estimates
conf = False
self.current_arena_pose = Pose(0, 0, 0, angle_z=degrees(0))
self.pf = ParticleFilter(self.grid)
# reset lift and head
await self.robot.set_lift_height(0.0).wait_for_completed()
await self.robot.set_head_angle(degrees(3)).wait_for_completed()
while not conf:
# move a little
self.current_robot_pose = self.robot.pose
await self.robot.turn_in_place(angle=degrees(turn_angle)
).wait_for_completed()
odometry = self.__compute_odometry()
detected_markers, camera_image = await self.__marker_processing()
# update, motion, and measurment with the odometry and marker data
curr_x, curr_y, curr_h, conf = self.pf.update(
odometry, detected_markers)
# update gui
self.gui.show_particles(self.pf.particles)
self.gui.show_mean(curr_x, curr_y, curr_h)
self.gui.show_camera_image(camera_image)
self.gui.updated.set()
self.current_arena_pose = Pose(curr_x,
curr_y,
0,
angle_z=degrees(curr_h))
print("We localized to arena location ", self.current_arena_pose)
def find(self, number):
print(number)
lookaround = self.robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cubes = self.robot.world.wait_until_observe_num_objects(num=self.cube_number,
object_type=cozmo.objects.LightCube,
timeout=10)
print("Found %s cubes" % len(cubes))
lookaround.stop()
if number == 1:
for i in cubes:
if i == self.jacket:
action = self.robot.pickup_object(self.jacket, num_retries=3)
action.wait_for_completed()
self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)),
relative_to_robot=False).wait_for_completed()
self.robot.say_text("here is your jacket").wait_for_completed()
action = self.robot.place_object_on_ground_here(i)
action.wait_for_completed()
break
elif number == 2:
for i in cubes:
if i == self.shorts:
action = self.robot.pickup_object(self.shorts, num_retries=3)
action.wait_for_completed()
self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)),
relative_to_robot=False).wait_for_completed()
self.robot.say_text("here are your shorts").wait_for_completed()
action = self.robot.place_object_on_ground_here(i)
action.wait_for_completed()
elif number == 3:
for i in cubes:
if i == self.shorts:
action = self.robot.pickup_object(self.shorts, num_retries=3)
action.wait_for_completed()
self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(180)),
relative_to_robot=False).wait_for_completed()
self.robot.say_text("here are your shorts").wait_for_completed()
action = self.robot.place_object_on_ground_here(i)
action.wait_for_completed()
self.robot.turn_in_place(degrees(180)).wait_for_completed()
self.find(4)
break
elif number == 4:
for i in cubes:
if i == self.sun_glasses:
action = self.robot.pickup_object(self.sun_glasses, num_retries=3)
action.wait_for_completed()
self.robot.go_to_pose(Pose(-9.3, -173, 0, angle_z=degrees(180)),
relative_to_robot=False).wait_for_completed()
self.robot.say_text("here are your sun glasses").wait_for_completed()
action = self.robot.place_object_on_ground_here(i)
action.wait_for_completed()
break
else:
print("unknown item")
def __init__(self):
landmarks = {
cube1: Pose(55, 160, 0, angle_z=degrees(90)),
cube2: Pose(160, 55, 0, angle_z=degrees(0)),
cube3: Pose(160, -55, 0, angle_z=degrees(0))
}
pf = ParticleFilter(robot,
landmarks=landmarks,
sensor_model=CubeSensorModel(robot))
super().__init__(particle_filter=pf, particle_viewer=True)
def roomcreation(robot: cozmo.robot.Robot):
cubesize = 50
initialPosition = 0
origin1 = robot.world.create_custom_fixed_object(Pose(-100,
initialPosition -
cubesize,
0,
angle_z=degrees(0)),
cubesize,
cubesize,
cubesize,
relative_to_robot=True)
origin2 = robot.world.create_custom_fixed_object(Pose(-100,
0,
initialPosition,
angle_z=degrees(0)),
cubesize,
cubesize,
cubesize,
relative_to_robot=True)
origin3 = robot.world.create_custom_fixed_object(Pose(-100,
initialPosition +
cubesize,
0,
angle_z=degrees(0)),
cubesize,
cubesize,
cubesize,
relative_to_robot=True)
'''
target1 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition-100, 0, angle_z=degrees(0)), 100, 100, 100,
relative_to_robot=True)
target2 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition, 0, angle_z=degrees(0)), 100, 100, 100,
relative_to_robot=True)
target3 = robot.world.create_custom_fixed_object(Pose(-300, initialPosition+100
, 0, angle_z=degrees(0)), 100, 100, 100,
relative_to_robot=True)
'''
if origin1 and origin2 and origin3:
print("Created origins succesfully")
# robot.go_to_pose(Pose(100, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed()
'''
if target1 and target2 and target3:
print("Created targets succesfully")
#robot.go_to_pose(Pose(300, 0, 0, angle_z=degrees(180)), relative_to_robot=True).wait_for_completed()
'''
robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(0)),
relative_to_robot=True).wait_for_completed()
robot.move_lift(3)
time.sleep(5)
robot.go_to_pose(Pose(-100, 0, 0, angle_z=degrees(180)),
relative_to_robot=True).wait_for_completed()
robot.move_lift(-3)
time.sleep(5)
def __init__(self):
landmarks = {
0: Pose(-55, 160, 0, angle_z=degrees(90)),
1: Pose(55, 160, 0, angle_z=degrees(90)),
2: Pose(160, 55, 0, angle_z=degrees(0)),
3: Pose(160, -55, 0, angle_z=degrees(0))
}
pf = ParticleFilter(robot,
landmarks=landmarks,
sensor_model=ArucoCombinedSensorModel(robot))
super().__init__(particle_filter=pf, particle_viewer=True)
def cozmo_program(robot: cozmo.robot.Robot):
fixed_object = robot.world.create_custom_fixed_object(
Pose(100, 0, 0, angle_z=degrees(0)),
10,
100,
100,
relative_to_robot=True)
if fixed_object:
print("fixed_object created successfully")
robot.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)),
relative_to_robot=True).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 = 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 run(sdk_conn):
'''The run method runs once Cozmo is connected.'''
robot = sdk_conn.wait_for_robot()
fixed_object = robot.world.create_custom_fixed_object(
Pose(100, 0, 0, angle_z=degrees(0)),
10,
100,
100,
relative_to_robot=True)
if fixed_object:
print("fixed_object created successfully")
robot.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)),
relative_to_robot=True).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.'''
# ####
# 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 __init__(self, run_in_cloud=False):
robot = self
robot.loop = asyncio.get_event_loop()
if not run_in_cloud:
robot.erouter = EventRouter()
robot.erouter.robot = robot
robot.erouter.start()
robot.head_angle = Angle(radians=0)
robot.shoulder_angle = Angle(radians=0)
robot.lift_height = Distance(distance_mm=0)
robot.pose = Pose(0, 0, 0, angle_z=Angle(degrees=0))
robot.camera = None
robot.carrying = None
robot.world = SimWorld()
robot.world.aruco = Aruco(robot, ARUCO_DICT_4x4_100)
robot.world.light_cubes = dict()
robot.world._faces = dict()
robot.world.charger = None
robot.world.server = SimServer()
robot.world.path_viewer = None
robot.world.particle_filter = SLAMParticleFilter(robot)
robot.kine = CozmoKinematics(robot) # depends on particle filter
robot.world.rrt = RRT(robot) # depends on kine
robot.world.world_map = WorldMap(robot)
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)
def drawE(self, updatePos, offset):
print("E")
self.robot.set_lift_height(0.5).wait_for_completed()
# self.robot.go_to_pose(Pose(updatePos.x, updatePos.y+offset, updatePos.z, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed()
self.robot.drive_wheels(400, 400, duration=0.1)
self.robot.turn_in_place(degrees(-90)).wait_for_completed()
self.robot.set_lift_height(0.1).wait_for_completed()
self.robot.drive_wheels(400, 400, duration=0.6)
self.robot.set_lift_height(0.5).wait_for_completed()
self.robot.go_to_pose(Pose(updatePos.x,
updatePos.y + offset - 40,
updatePos.z,
angle_z=degrees(0)),
relative_to_robot=False).wait_for_completed()
self.robot.set_lift_height(0.1).wait_for_completed()
self.robot.drive_wheels(-400, -400, duration=0.6)
self.robot.set_lift_height(0.1).wait_for_completed()
self.robot.drive_wheels(400, 400, duration=0.8)
self.robot.set_lift_height(0.5).wait_for_completed()
self.robot.turn_in_place(degrees(90)).wait_for_completed()
self.robot.set_lift_height(0.1).wait_for_completed()
self.robot.drive_wheels(-400, -400, duration=0.6)
self.robot.set_lift_height(0.5).wait_for_completed()
self.robot.turn_in_place(degrees(-90)).wait_for_completed()
self.robot.drive_wheels(-400, -400, duration=0.5)
self.robot.turn_in_place(degrees(90)).wait_for_completed()
self.robot.set_lift_height(0.1).wait_for_completed()
self.robot.drive_wheels(400, 400, duration=0.6)
self.robot.set_lift_height(0.5).wait_for_completed()
#self.robot.say_text("This is, E").wait_for_completed()
self.robot.go_to_pose(initPos,
relative_to_robot=False).wait_for_completed()
def update_current_arena_pose(self):
print("-" * 20 + "UPDATING POSE" + "-" * 20)
coordinate_systems_diff = diff_heading_deg(
self.current_robot_pose.rotation.angle_z.degrees,
self.current_arena_pose.rotation.angle_z.degrees)
print("Initial robot pose was: ", self.current_robot_pose)
print("With a diff heading of: ", coordinate_systems_diff)
arena_initial_pose_mm = rotate_point(
self.current_robot_pose.position.x,
self.current_robot_pose.position.y, coordinate_systems_diff)
print("Calculated initial arena pose is:", arena_initial_pose_mm)
arena_final_pose_mm = rotate_point(self.robot.pose.position.x,
self.robot.pose.position.y,
coordinate_systems_diff)
print("Calculated final arena pose is:", arena_final_pose_mm)
d_x = arena_final_pose_mm[0] - arena_initial_pose_mm[0]
d_y = arena_final_pose_mm[1] - arena_initial_pose_mm[1]
d_heading = diff_heading_deg(
self.robot.pose.rotation.angle_z.degrees,
self.current_robot_pose.rotation.angle_z.degrees)
difference_pose = convertPoseFromMmToInches(
Pose(x=d_x, y=d_y, z=0, angle_z=d_heading))
print("We think we moved ", difference_pose, "\n\n")
self.current_arena_pose = self.current_arena_pose + difference_pose
print("Current pose is now ", self.current_arena_pose, "\n\n")
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))
async def init_game(self):
await self.robot.set_lift_height(0, duration=0.5).wait_for_completed()
await self.robot.go_to_pose(Pose(0, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
await self.robot.set_head_angle(cozmo.util.Angle(degrees=10)).wait_for_completed()
# setup cubes
self.cubes = await self.robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube)
self.ori_z = self.cubes[0].pose.position.z
self.ori_seq = None
self.cur_word = None
self.cur_seq = None
self.disp_timer = 0
self.correct_check_enabled = False
self.paused = False
self.word_index = -1
self.score = 0
self.idle_count = 0
self.pos_index = 0
self.neg_index = 0
self.anagram_words = ["apt","tap", "are", "ear", "arm", "mar", "ram", "art", "rat", "tar", "asp",
"pas", "sap", "spa", "ate", "eat", "era", "bat", "tab", "now", "own", "won", "opt",
"pot", "top", "own", "now", "won","zoo", "yet", "yes", "wow","win","who","vow","rug","rub","rip","put","pub",
"pie","pet","pen","pic","pig","out","oil","odd","off","nut","nod","aim","air","awe","ban","big","bio","bin"]
shuffle(self.anagram_words)
if not Anagram.DEBUG:
await self.robot.say_text(text="let's play, anagram", play_excited_animation=False,
duration_scalar=1.5).wait_for_completed()
self.ori_pose = self.robot.pose
await self.disp_word()
def act_out(self, game_robot, act_type):
"""
Acting out the various emotion displays
"""
selected_anim = None
if act_type == "lose_hand":
# Minor emotion when losing hand
selected_anim = "anim_speedtap_losehand_0%s" % randint(1, 3)
elif act_type == "win_hand":
# Minor emotion when winning a hand
selected_anim = "anim_speedtap_winhand_0%s" % randint(1, 3)
elif act_type == "stand_back":
#Roll back to have space to act, away from cube.
game_robot.go_to_pose(Pose(-20, 0, 0, angle_z=degrees(0)),
relative_to_robot=True).wait_for_completed()
game_robot.move_lift(-3)
time.sleep(0.2)
elif act_type == "win_game":
# Major win game emotion
selected_anim = self.select_win_game()
elif act_type == "lose_game":
#Major lose game emotion
selected_anim = self.select_lose_game()
else:
# animation to show waiting for light to change
selected_anim = self.select_wait()
# Play animation
if selected_anim:
game_robot.play_anim(selected_anim).wait_for_completed()
async def __drive_to_pose(self, pose):
translation = (pose - self.current_arena_pose).position
directions = Pose(x=translation.x,
y=translation.y,
z=0,
angle_z=pose.rotation.angle_z)
await self.__execute_directions(directions)
def step4_drop_stack_on_top(self):
"""
Place the stack cube in the middle of the corner cube and right cube
:return:
"""
print('**step 4**')
pickup_posn = self.corner_cube.pose.position
right_posn = self.right_cube.pose.position
# find an x coordinate that places cozmo at a position far enough away from the cubes such that when he
# places the stack cube, it will land on top
robot_x = pickup_posn.x - self.CUBE_SIZE * 1.6
# find y coordinate for position in the middle of blocks
middle_y = pickup_posn.y + (right_posn.y - pickup_posn.y) / 2
# (assume an angle of 0 degrees as I'm not yet accounting for cubes that are rotated
destination_pose = Pose(robot_x,
middle_y,
pickup_posn.z,
angle_z=degrees(0))
go_to_destination_pose = self.robot.go_to_pose(destination_pose, num_retries=3)
go_to_destination_pose.wait_for_completed()
drop_stack = self.robot.place_object_on_ground_here(self.stack_cube, num_retries=3)
drop_stack.wait_for_completed()
self.stack_cube.set_lights(Light(on_color=Color(int_color=0xff00ffff), off_color=Color(int_color=0xff00ffff)))
# I can't check for has_succeeded here because Cozmo won't think he's actually succeeded in dropping the cube
# so let's just assume he was successful
self.increment_step()
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=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" %
cube.pose)
except asyncio.TimeoutError:
print("Didn't find a cube")
desired_pose_relative_to_cube = Pose(0, 100, 0, angle_z=degrees(90))
x, y, z = cube.pose.position.x_y_z
print("Robot pose: %s" % robot.pose)
#my_go_to_pose3(robot, x-100, y, 90)
my_go_to_pose2(
robot, 269, 127, 20
) # found the cordinates of cube and moved robot to desired orientation to calculate final position
my_go_to_pose3(robot, 269, -127, 20)
def update_current_arena_pose(self):
print("-" * 40)
print("-" * 20 + "UPDATING POSE" + "-" * 20)
print("-" * 40)
coordinate_systems_diff = -self.pose_offset.degrees
print("Started at arena pose: ", self.current_arena_pose, "\n\n")
#print("Initial robot pose was: ", self.current_robot_pose)
#print("Current robot pose is: ", self.robot.pose)
#print("With a diff heading of: ", coordinate_systems_diff)
d_x, d_y = rotate_point(
self.robot.pose.position.x - self.current_robot_pose.position.x,
self.robot.pose.position.y - self.current_robot_pose.position.y,
coordinate_systems_diff)
#print("Calculated initial arena pose is:", arena_initial_pose_mm)
#arena_final_pose_mm = rotate_point(, self.robot.pose.position.y, coordinate_systems_diff)
#print("Calculated final arena pose is:", arena_final_pose_mm)
#print("COMPUTE ODOMETRY:" ,self.__compute_odometry())
#d_x = arena_final_pose_mm[0] - arena_initial_pose_mm[0]
#d_y = arena_final_pose_mm[1] - arena_initial_pose_mm[1]
d_heading = diff_heading_deg(
self.robot.pose.rotation.angle_z.degrees,
self.current_robot_pose.rotation.angle_z.degrees)
difference_pose = convertPoseFromMmToInches(
Pose(x=d_x, y=d_y, z=0, angle_z=degrees(d_heading)))
print("We think we moved ", difference_pose, "\n\n")
self.current_arena_pose = self.current_arena_pose + difference_pose
self.current_robot_pose = self.robot.pose
print("Current arena pose is now: ", self.current_arena_pose, "\n\n")
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 cozmo_go_to_pose(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 """ robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(angle_z)), relative_to_robot=True).wait_for_completed()
async def __drive_to_pose(self, pose):
print("We are at ", self.current_arena_pose, " and we are driving to ", pose)
translation = (pose - self.current_arena_pose).position
directions = Pose(x=translation.x, y=translation.y, z=0, angle_z=pose.rotation.angle_z)
print("We will follow these directions: ", directions, "\n\n")
await self.__execute_directions(directions)
print("Directions followed!", "\n\n")
self.update_current_arena_pose()
def start(self, event=None):
x = self.parent.obj.x
y = self.parent.obj.y
theta = self.parent.obj.theta
self.target_pose = Pose(x + 250*cos(theta), y+250*sin(theta), self.robot.pose.position.z,
angle_z=Angle(radians = wrap_angle(theta+pi)))
print('Traveling to',self.target_pose)
super().start(event)
def __init__(self, robot: cozmo.robot.Robot):
self.current_arena_pose = None
self.current_robot_pose = robot.pose
self.robot = robot
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
self.camera_settings = np.array([
[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]
], dtype=np.float)
self.current_drop_off_pose_idx = 0
self.pick_up_pose = Pose(x=4.5, y=11, z=0, angle_z=degrees(90))
self.drop_off_poses = [Pose(x=22, y=11, z=0, angle_z=degrees(90)), Pose(x=20, y=11, z=0, angle_z=degrees(90)), Pose(x=18, y=11, z=0, angle_z=degrees(90))]
self.drop_off_directions = [Pose(x=5, y=6, z=0, angle_z=degrees(0)), Pose(x=22, y=6, z=0, angle_z=degrees(90)), self.drop_off_poses[0]]
self.pick_up_directions = [Pose(x=21.75, y=4.5, z=0, angle_z=degrees(0)), Pose(x=5, y=4.5, z=0, angle_z=degrees(90)), self.pick_up_pose]
self.drive_speed = speed_mmps(100)
self.grid = CozGrid("map_arena.json")
self.pf = ParticleFilter(self.grid)
threading.Thread(target=self.runGUI).start()
self.pose_offset = None
def go_to_coordinate(robot):
global x_coordinate
global y_coordinate
global angledegrees
robot.go_to_pose(Pose(x_coordinate,
y_coordinate,
0,
angle_z=degrees(angledegrees)),
relative_to_robot=True).wait_for_completed()
async def run(self, coz_conn):
asyncio.set_event_loop(coz_conn._loop)
# setup robot
self.coz = await coz_conn.wait_for_robot()
await self.coz.go_to_pose(Pose(0, 0, 0, angle_z=degrees(0)),
relative_to_robot=True).wait_for_completed()
await self.start_game()
while True:
pass
def start(self, event=None):
wall = self.parent.object
print('Selected wall',self.parent.wobj)
(x, y, theta) = self.parent.pick_side(150)
self.target_pose = Pose(x, y, self.robot.pose.position.z,
angle_z=Angle(radians = wrap_angle(theta)))
print('Traveling to',self.target_pose)
super().start(event)
