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)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
# add annotators for angle
robot.world.image_annotator.add_annotator('angle', AngleAnnotator)
print(robot.pose.rotation.angle_z)
await robot.turn_in_place(Angle(degrees=1080),
speed=Angle(degrees=20)).wait_for_completed()
print(robot.pose.rotation.angle_z)
await robot.drive_wheels(20, 0, duration=20)
def normalize_angle_and_minus(angle1: Angle, angle2: Angle):
degrees1 = angle1.degrees
degrees2 = angle2.degrees
if degrees1 < 0:
degrees1 += 360
if degrees2 < 0:
degrees2 += 360
if degrees1 < degrees2:
return Angle(degrees=degrees2 - degrees1)
else:
return Angle(degrees=degrees2 + 360 - degrees1)
def face_cube(self, i, wait=True):
angle = (self.facing - i) * 0.5
action = self.robot.turn_in_place(Angle((self.facing - i) * 0.5))
if wait:
action.wait_for_completed()
self.facing = i
return action
def cozmo_program(robot: cozmo.robot.Robot):
# dire_bonjour+setup
robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed()
cute = CuteCozmo(robot)
cute.face_cube(1)
# demo gamme
for i in range(8):
cute.play_note(i)
# Animation content, je t'invite a continuer
robot.play_anim('anim_fistbump_success_01').wait_for_completed()
cute.face_cube(1)
#Partie jouer mélodie simple
melodie = [1,4,7]
print('playing simple melody')
mini_jeu_jouer_melodie(cute, melodie)
mini_jeu_enregistrer_melodie(cute)
robot.play_anim('anim_hiking_observe_01').wait_for_completed()
mini_jeu_jouer_melodie(cute)
robot.play_anim('anim_freeplay_reacttoface_like_01').wait_for_completed()
robot.turn_in_place(Angle(degrees=180)).wait_for_completed()
robot.drive_wheels(500, 500)
time.sleep(5)
def setup(self):
self.armsDown()
# reconnect to the cubes in case we lost them
self.robot.world.connect_to_cubes()
# make the head horizontal
self.robot.set_head_angle(Angle(0)).wait_for_completed()
# we face each direction (center, left and right) hoping to find a cube
self.cubes = list()
for i in range(3):
self.face(i)
color = self.lights[i]
try:
cube = self.robot.world.wait_for_observed_light_cube(1)
except:
# self.robot.play_anim_trigger(cozmo.anim.Triggers.CubePounceLoseSession).wait_for_completed()
print('no cube detected :(')
self.face(1)
return
self.cubes.append(cube)
cube.set_lights(color)
# self.play_note(i)
cube.set_lights(color)
self.face(1)
for cube in self.cubes:
cube.set_lights(cozmo.lights.off_light)
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)
def take_pics(robot: cozmo.robot.Robot):
robot.move_lift(-3.0)
robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()
face: cozmo.faces.Face = None
while True:
if face and face.is_visible:
print("Found a face.")
robot.camera.color_image_enabled = True
robot.world.latest_image.raw_image.show()
print(face.name)
return
else:
try:
print("Searching a face.")
face = robot.world.wait_for_observed_face(timeout=10)
except asyncio.TimeoutError:
print("Didn't find a face.")
robot.turn_in_place(Angle(degrees=60.0)).wait_for_completed()
time.sleep(.1)
def set_head_angle(self, degrees):
"""
Set the head position of Cozmo
@param degrees:
@type degrees:
@return:
@rtype:
"""
angle = Angle(degrees=degrees)
self.robot.set_head_angle(angle).wait_for_completed()
pass
def __init__(self):
robot = self
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, cv2.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 Draw(self):
sleep(7)
self.Ready()
sleep(0.5)
self.LiftUp()
sleep(1)
self.LiftDown()
sleep(0.1)
for _ in range(6):
self.cozmo.turn_in_place(degrees(360),
speed=Angle(1)).wait_for_completed()
self.LiftUp()
async def get_distance_between_wheels(robot: cozmo.robot.Robot,
speed: int,
debug: bool = False):
robot.move_lift(-3)
await robot.set_head_angle(degrees(0)).wait_for_completed()
bArray = []
last_phi = Angle(degrees=0)
for duration in range(1, 7):
# Turn right with center of wheel as point, b is the distance between wheels
# So
# phi * (b / 2) = speed * time
# And because
# phi = old_robot_angle - new_robot_angle
# ==>
# b = speed * time / (old_robot_angle - new_robot_angle) * 2
oldPose = robot.pose
await robot.drive_wheels(speed, -speed, duration=duration)
newPose = robot.pose
phi = normalize_angle_and_minus(newPose.rotation.angle_z,
oldPose.rotation.angle_z)
# Use the delta of the value to ingore the warm up time or friction
delta_phi = phi - last_phi
last_phi = phi
if duration < 3: # Skip the test data because when duration less than 3, the value is not accurate
continue
if debug:
print(f"phi: {phi}")
print(f"delta_phi: {delta_phi}")
if delta_phi.abs_value.radians < 0.1:
print('Wrong data, ignore')
continue
b = (speed * 1) / delta_phi.abs_value.radians * 2
if b > 100 or b < 40:
print('Wrong data, ignore')
continue
print(
f"With speed {speed} and time {duration}, the distance between wheels: by phi {b}"
)
bArray.append(b)
print(f'Average B: {mean(bArray)} with speed {speed}')
return mean(bArray)
def step(self, action):
if not self.continuous_actions:
if action == 0:
# print('Turning left')
self.robot.turn_in_place(degrees(
self.degrees_rotate)).wait_for_completed()
elif action == 1:
# print('Turning right')
self.robot.turn_in_place(
degrees(-self.degrees_rotate)).wait_for_completed()
else:
# action in range [-1, +1]
speed = abs(action) * MAX_SPEED
neg_pos = 90 if action > 0.0 else -90
print(
'Step_num: {}. continuous action: {}. Speed {}. Direction: {}'.
format(self.step_num, action, speed, neg_pos))
robot_action = self.robot.turn_in_place(degrees(neg_pos),
speed=Angle(speed),
in_parallel=True)
time.sleep(
0.2
) # is any sleep necessary and will continuous move too fast
self.robot.stop_all_motors(
) # this might make the pause too long, todo try one or other
self.robot.abort_all_actions()
raw_state = self.get_raw_image() # todo find best place to resize
reward, done, detections = self.get_reward(raw_state)
self.step_num += 1
if self.render:
bbox_image = create_image_with_bounding_boxes(
raw_state, detections)
cv2.imshow('Current raw image', raw_state)
cv2.waitKey(1)
cv2.imshow('Current bounding box image', bbox_image)
cv2.waitKey(1)
state_preprocessed = cv2.resize(raw_state, IMAGE_DIM_INPUT)
state_preprocessed = np.moveaxis(state_preprocessed, 2, 0)
if self.natural_language:
self.instruction = self.nlp_instructions[self.nlp_instruction_idx]
state_preprocessed = (state_preprocessed, self.instruction)
return state_preprocessed, reward, done, None
def back_up(self):
self.camera_off()
robot = self.robot
turn_choices = [120, -120]
drive_choices = [-25, -100, -50]
random_turn = random.choice(turn_choices)
random_drive = random.choice(drive_choices)
# print("back up: ", random_drive, "... and turn: ", random_turn)
robot.drive_straight(distance_mm(random_drive),
speed=speed_mmps(30),
should_play_anim=False,
in_parallel=True)
robot.wait_for_all_actions_completed()
robot.turn_in_place(degrees(random_turn),
speed=Angle(5),
in_parallel=True)
robot.wait_for_all_actions_completed()
self.camera_on()
def set_head_angle(self, degrees):
angle = Angle(degrees=degrees)
pass
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(5)).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)
###################
# YOUR CODE HERE
###################
converged = False
while True:
# print(robot.is_picked_up)
# obtain odometry information
odo = compute_odometry(robot.pose)
# update odometry
last_pose = robot.pose
if robot.is_picked_up:
robot.stop_all_motors()
print("who kidnapped me ??!")
last_pose = cozmo.util.Pose(0,
0,
0,
angle_z=cozmo.util.Angle(degrees=0))
await robot.say_text("Ass we can").wait_for_completed()
await robot.play_anim_trigger(cozmo.anim.Triggers.CodeLabUnhappy
).wait_for_completed()
pf = ParticleFilter(grid)
converged = False
#obtain markers and poses
markers, camera_image = await marker_processing(
robot, camera_settings, show_diagnostic_image=False)
if converged:
pass
else:
# particle filters update
mean = pf.update(odo, markers)
# gui update??
gui.show_particles(pf.particles)
gui.show_mean(mean[0], mean[1], mean[2], mean[3])
gui.show_camera_image(camera_image)
gui.updated.set()
robot.stop_all_motors()
if mean[3]:
# drive to goal!
# compute odometry to goal
print(mean)
mean_x, mean_y, mean_h = mean[0:3]
goal_x, goal_y, goal_h = goal
await robot.turn_in_place(degrees(-mean_h)
).wait_for_completed()
dx, dy = goal_x - mean_x, goal_y - mean_y
turn_degree = np.arctan2(dy, dx) * 180 / np.pi
turn_degree -= 70
turn_degree %= 360
goal_pose = Pose(dx * 24,
dy * 24,
0,
angle_z=cozmo.util.Angle(degrees=turn_degree))
await robot.go_to_pose(
goal_pose, relative_to_robot=True).wait_for_completed()
await robot.turn_in_place(
degrees(-turn_degree),
speed=Angle(360)).wait_for_completed()
converged = True
print("converged!")
a = robot.pose
if robot.is_picked_up:
print("kidnap!")
robot.stop_all_motors()
last_pose = cozmo.util.Pose(
0, 0, 0, angle_z=cozmo.util.Angle(degrees=0))
await robot.say_text("No").wait_for_completed()
await robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabUnhappy
).wait_for_completed()
pf = ParticleFilter(grid)
converged = False
else:
# actively look around
# random_degree = np.random.randint(low=85, high=93)
# await robot.turn_in_place(degrees(random_degree),speed=Angle(1080)).wait_for_completed()
robot.drive_wheel_motors(30, 0)
def face(self, i):
self.robot.turn_in_place(Angle(
(self.facing - i) * 0.5)).wait_for_completed()
self.facing = i
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
global found_cubes
found_cubes = []
goal_degrees = []
grid.addGoal((grid.width / 2, grid.height / 2))
found_goal = False
astar(grid, heuristic)
path = grid.getPath()
path_index = 0
grid_init_start_pose = grid.getStart()
(robot_grid_x, robot_grid_y) = grid.getStart()
robot.set_head_angle(degrees(0)).wait_for_completed()
robot.set_lift_height(1).wait_for_completed()
robot.say_text('Game is on').wait_for_completed()
while not stopevent.is_set():
new_cube = search_cube(robot, grid)
if not new_cube == None:
grid.clearStart()
grid.clearVisited()
grid.clearPath()
grid.setStart((robot_grid_x, robot_grid_y))
add_obstacle(
grid, new_cube, grid_init_start_pose
) # Add the obstacle for all cubes that had been found
if new_cube.cube_id == LightCube1Id:
new_cube.set_lights(cozmo.lights.blue_light)
goal_degrees = set_goal(
grid, new_cube, grid_init_start_pose
) # Update the goal coordinate while found cube 1
robot.say_text("It's the Goal").wait_for_completed()
found_goal = True
else:
new_cube.set_lights(cozmo.lights.red_light)
robot.say_text("It's an Obstacle").wait_for_completed()
# Replanning the path for
robot.say_text('Replanning').wait_for_completed()
try:
astar(grid, heuristic)
except:
robot.say_text("Cannot go to that place").wait_for_completed()
return
path_index = 0
path = grid.getPath()
path_index += 1
if path_index == len(path): # At the goal position
if not found_goal: # At the center of grid
path_index -= 1
robot.turn_in_place(Angle(degrees=-30)).wait_for_completed()
continue
else: # Arrived the final place
goal_degree = goal_degrees[f"{(robot_grid_x, robot_grid_y)}"]
robot.turn_in_place(
Angle(degrees=normalize_angle(
goal_degree - robot.pose.rotation.angle_z.degrees))
).wait_for_completed()
robot.say_text('Arrived').wait_for_completed()
break
current_pose = path[path_index - 1]
next_pose = path[path_index]
robot_grid_x += int(next_pose[0] - current_pose[0])
robot_grid_y += int(next_pose[1] - current_pose[1])
x = (next_pose[0] - current_pose[0]) * grid.scale
y = (next_pose[1] - current_pose[1]) * grid.scale
degree = ((90 * y / abs(y)) if x == 0 else math.degrees(
math.atan2(y, x))) - robot.pose.rotation.angle_z.degrees
# robot.turn_in_place(Angle(degrees=normalize_angle(degree))).wait_for_completed()
# robot.drive_straight(distance_mm(math.sqrt(x**2 + y**2)), speed_mmps(50), should_play_anim=False).wait_for_completed()
(x, y) = world_to_related(x, y, robot.pose.rotation.angle_z.degrees)
robot.go_to_pose(Pose(x, y, 0, angle_z=Angle(degrees=degree)),
relative_to_robot=True).wait_for_completed()
stopevent.wait()
def move_head_up(self):
"""
Move the head up
"""
self.robot.set_head_angle(Angle(0.9), in_parallel=True)