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 cozmo_program(robot: cozmo.robot.Robot):
cozmo.logger.setLevel('WARN')
hx = []
hu = []
# Start a first action (this one is trivial, just to start).
a = robot.go_to_pose(cozmo.util.Pose(x=0,
y=0,
z=0,
angle_z=cozmo.util.radians(0)),
relative_to_robot=True)
# Let's run the loop for 10 seconds.
for i in range(10):
x = env.reset(cozmo2net(robot.pose)) # Get current state from sensors
xn = preview(env, 10) # Predict next state
hx.append(x)
hu.append(xn) # Log ...
print(x, xn) # ... and print
if a.is_running: a.abort() # Abort preview action if not achieved
a = robot.go_to_pose(
net2cozmo(xn)) # Send the robot to next passing point
time.sleep(1)
# \endfor main action loop
# Just in case, stop the robot motors.
if a.is_running: a.abort()
robot.drive_wheel_motors(0, 0)
def custom_objects(robot: cozmo.robot.Robot):
# Add event handlers for whenever Cozmo sees a new object
robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)
custom_box = robot.world.define_custom_box(custom_object_type=cozmo.objects.CustomObjectTypes.CustomType00, \
marker_front=cozmo.objects.CustomObjectMarkers.Circles2, \
marker_back=cozmo.objects.CustomObjectMarkers.Circles3, \
marker_top=cozmo.objects.CustomObjectMarkers.Circles4, \
marker_bottom=cozmo.objects.CustomObjectMarkers.Circles5, \
marker_left=cozmo.objects.CustomObjectMarkers.Diamonds2, \
marker_right=cozmo.objects.CustomObjectMarkers.Diamonds3, \
depth_mm=60, \
width_mm=60, \
height_mm=45, \
marker_width_mm=24.892, \
marker_height_mm=24.892, \
is_unique=True)
if (custom_box is not None):
print("All objects defined successfully!")
else:
print("One or more object definitions failed!")
return
print("Show the above markers to Cozmo and you will see the related objects "
"annotated in Cozmo's view window, you will also see print messages "
"everytime a custom object enters or exits Cozmo's view.")
print("Press CTRL-C to quit")
cubes = lookForCubes(robot, 45, 0)
old_pose = robot.pose
print("looking for cubes!")
if len(cubes) == 1:
print("found object")
robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed()
robot.go_to_pose(cubes[0].pose,relative_to_robot=False).wait_for_completed()
pickupObject(robot)
robot.drive_wheels(-50,-50)
time.sleep(3)
robot.drive_wheels(0,0)
robot.go_to_pose(old_pose,relative_to_robot=False).wait_for_completed()
robot.set_lift_height(height = 0, accel = 6, max_speed = 500, duration = 1, in_parallel = False, num_retries = 3).wait_for_completed()
else:
print("Cannot locate custom box")
while True:
time.sleep(0.1)
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 mov_w2h(robot: cozmo.robot.Robot):
print("Robot mov_w2h action execution ...")
try:
robot.go_to_pose(exit_home, relative_to_robot=False).wait_for_completed(20)
robot.go_to_pose(home, relative_to_robot=False).wait_for_completed(20)
robot.say_text(text="I visited way point two and now I'm in my home").wait_for_completed(10)
robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10)
return "s2"
except Exception as e:
print("ERROR: %sn" + str(e))
return "err"
def mov_w1w2(robot: cozmo.robot.Robot):
print("Robot mov_w1w2 action execution ...")
try:
robot.go_to_pose(w2, relative_to_robot=False).wait_for_completed(20)
cube2 = robot.world.get_light_cube(LightCube2Id)
cube2.set_lights(cozmo.lights.Light(cozmo.lights.Color(rgb=(255, 255, 102), name="yellow")))
robot.say_text(text="I visited way point one and now I'm in way point two").wait_for_completed(10)
robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10)
return "s3"
except Exception as e:
print("ERROR: %sn" + str(e))
return "err"
def mov_hw1(robot: cozmo.robot.Robot):
print("Robot mov_hw1 action execution ...")
try:
robot.go_to_pose(exit_home, relative_to_robot=False).wait_for_completed(20)
robot.go_to_pose(w1, relative_to_robot=False).wait_for_completed(20)
cube1 = robot.world.get_light_cube(LightCube1Id)
cube1.set_lights(cozmo.lights.Light(cozmo.lights.Color(rgb=(102, 255, 102), name="green")))
robot.say_text(text="I'm at way point one").wait_for_completed(10)
robot.play_anim(name="anim_memorymatch_successhand_cozmo_01").wait_for_completed(10)
return "s5"
except Exception as e:
print("ERROR: %sn" + str(e))
return "err"
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 cozmo_program(robot: cozmo.robot.Robot):
# cube = None
fixed_object = custom_objects(robot)
time.sleep(60)
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
# try to find a block
xpose = 500
cube = None
while not cube:
try:
cube = robot.world.wait_for_observed_light_cube(timeout=30)
print("Found it", cube)
except asyncio.TimeoutError:
print("Didn't find it")
look_around.stop()
robot.go_to_pose(Pose(xpose, 0, 0, angle_z=degrees(0)),
relative_to_robot=False).wait_for_completed()
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
xpose = 1000
print("Yay")
look_around.stop()
cube.set_lights(cozmo.lights.green_light.flash())
anim = robot.play_anim_trigger(cozmo.anim.Triggers.BlockReact)
anim.wait_for_completed()
action = robot.pickup_object(cube)
print("got action", action)
result = action.wait_for_completed(timeout=30)
print("got action result", result)
robot.turn_in_place(degrees(90)).wait_for_completed()
action = robot.place_object_on_ground_here(cube)
print("got action", action)
result = action.wait_for_completed(timeout=30)
print("got action result", result)
anim = robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin)
cube.set_light_corners(None, None, None, None)
anim.wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
robot.set_head_angle(degrees(0))
robot.set_lift_height(0, in_parallel=True).wait_for_completed()
#turn in circles until you see the charger
robot.drive_wheels(SPIN_SPEED, -SPIN_SPEED)
charger = robot.world.wait_for_observed_charger(timeout=30)
robot.drive_wheels(0.0, 0.0)
if charger:
if robot.pose.is_comparable(charger.pose):
#they're both on the same origin (cozmo)
print('going to charger pose')
robot.go_to_pose(charger.pose,
in_parallel=True).wait_for_completed()
robot.set_lift_height(.2, in_parallel=True).wait_for_completed()
robot.turn_in_place(degrees(180)).wait_for_completed()
wiggle_for_charger(robot)
else:
print('charger not found.')
def cozmo_program(robot: cozmo.robot.Robot):
""" global coordinates from Cozmo SDK """
# trajectory = [Pose(400, 0, 0, angle_z=degrees(90)), Pose(400, 250, 0, angle_z=degrees(180)),
# Pose(0, 250, 0, angle_z=degrees(270)), Pose(0, 0, 0, angle_z=degrees(0))]
# # goes around a block four times
# for i in range(4):
# for coord in trajectory:
# robot.go_to_pose(coord).wait_for_completed()
''' relative motion '''
trajectory = [
Pose(100, 0, 0, angle_z=degrees(0)),
Pose(0, 100, 0, angle_z=degrees(90)),
Pose(0, 100, 0, angle_z=degrees(180)),
Pose(0, 100, 0, angle_z=degrees(270))
]
# goes around a block four times
for i in range(4):
for coord in trajectory:
robot.go_to_pose(coord,
relative_to_robot=True).wait_for_completed()
def find_relative_cube_pose(robot: cozmo.robot.Robot):
'''Looks for a cube while sitting still, prints the pose of the detected cube
in world coordinate frame and relative to the robot coordinate frame.'''
robot.move_lift(-3)
robot.set_head_angle(degrees(0)).wait_for_completed()
cube = None
while True:
try:
cube = robot.world.wait_for_observed_light_cube(timeout=30)
if cube:
print("Robot pose: %s" % robot.pose)
print("Cube pose: %s" % cube.pose)
print("Cube pose: %s" % cube.pose)
final = get_relative_pose(cube.pose, robot.pose)
print("Cube pose in the robot coordinate frame: %s" % final)
robot.go_to_pose((final),
relative_to_robot=True).wait_for_completed()
except asyncio.TimeoutError:
print("Didn't find a cube")
def workloop(robot: cozmo.robot.Robot):
robot.set_head_angle(degrees(7)).wait_for_completed()
MyObjectArchetype = robot.world.define_custom_cube(
CustomObjectTypes.CustomType00, CustomObjectMarkers.Circles2, 50, 50,
50, True)
zeroOffset = 80
inputOffset = 100
# wait until we see a custom object
while True:
offset = zeroOffset + inputOffset
my_object_instance = None
while my_object_instance is None:
evt = robot.wait_for(cozmo.objects.EvtObjectObserved, timeout=None)
if isinstance(evt.obj, CustomObject):
my_object_instance = evt.obj
# find the vector from the object to the robot
object_to_robot_vec = robot.pose.position - my_object_instance.pose.position
# normalize the vector (so it's length 1.0)
object_to_robot_vec_dist = math.sqrt(
(object_to_robot_vec.x * object_to_robot_vec.x) +
(object_to_robot_vec.y * object_to_robot_vec.y) +
(object_to_robot_vec.z * object_to_robot_vec.z))
normalized_object_to_robot_vec = object_to_robot_vec * (
1.0 / object_to_robot_vec_dist)
# we can now add X times this vector to the objects position and it will push us X mm towards the robot
# e.g. lets push it 50mm back (about 1 Cozmo length)
offset_vec = (normalized_object_to_robot_vec * offset)
target_pos = my_object_instance.pose.position + offset_vec
target_pose = my_object_instance.pose
# change the position (set it to the new target_pos we calculated above)
target_pose._position = target_pos
robot.go_to_pose(target_pose).wait_for_completed()
time.sleep(0.5)
def find_face(robot: cozmo.robot.Robot):
global pose
print ("cozmo is looking for a human face")
face = None
# two cubes spotted, the robot will find the human face and approach it.
while True:
if face and face.is_visible:
robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
robot.say_text("Hello Human.How are you today? I am excited to play a game with you. Are u ready? Let's start").wait_for_completed()
robot.go_to_pose(Pose(0.0, 0.0, 0.0, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed()
break
else:
robot.turn_in_place(degrees(30)).wait_for_completed()
# Wait until we we can see another face
try:
face = robot.world.wait_for_observed_face(timeout=2)
print(face)
except asyncio.TimeoutError:
print("Didn't find a face.")
# return
time.sleep(1)
return face
def intent_solve_maze(robot: cozmo.robot.Robot):
print(str(world))
world.create_objects(robot, (2, 2))
robot.go_to_pose(Pose(2 * 95, 2 * 95, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
pass
def charger(self, robot: cozmo.robot.Robot = None, cmd_args=None):
trial = 1
# try to find the charger
charger = None
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.origin_id == robot.pose.origin_id:
robot.say_text("I know where the charger is",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0).wait_for_completed()
charger = robot.world.charger
else:
pass
if not charger:
robot.say_text("I am trying to find the charger",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=None)
robot.say_text("Found it!",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
except asyncio.TimeoutError:
robot.say_text("I couldn't find the charger",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
robot.say_text("I will now lift my arms to maneuver to the base",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
robot.move_lift(10)
robot.say_text("I am going to align myself, I got this!",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
action = robot.go_to_pose(charger.pose)
action.wait_for_completed()
print("Completed action: result = %s" % action)
robot.drive_straight(distance_mm(-100),
speed_mmps(5000)).wait_for_completed()
robot.say_text("I think I am aligned!",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
# Turn 180 (and 5) degrees, then goes backwards at full speed
robot.say_text("And now for the finale, I will turn around",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
#this value needs to be tweaked (90 or 95)
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.turn_in_place(degrees(90)).wait_for_completed()
# time.sleep( 1 )
robot.say_text("Almost there!",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
robot.drive_straight(distance_mm(-200),
speed_mmps(5000)).wait_for_completed()
robot.say_text("Am I on the base? I will check.",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
if robot.is_on_charger:
robot.move_lift(-8)
robot.say_text("I did it! Yay",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
else:
robot.say_text("I did not make it, but I am not discouraged",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
robot.world.charger = None
robot.say_text("I will try again",
play_excited_animation=True,
use_cozmo_voice=True,
duration_scalar=0.7,
voice_pitch=-1.0,
in_parallel=False).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(5000)).wait_for_completed()
trial += 1
if trial < 4:
self.charger(robot)
else:
print("I am tired of trying, I will stop trying for now")
return
def explore_the_world(robot: cozmo.robot.Robot):
global pose
robot.say_text(
"Hello Welcome to my world. I am going to explore my world please bear with me"
).wait_for_completed()
#initialise the variables
cube = None
cube1 = None
look_around_2 = None
look_around_3 = None
# stores the starting location of the cozmo robot
pose = robot.pose
print('Pose: Pos = <%.1f, %.1f, %.1f>' % pose.position.x_y_z)
print('Pose: Rot quat = <%.1f, %.1f, %.1f, %.1f>' %
pose.rotation.q0_q1_q2_q3)
print('Pose: angle_z = %.1f' % pose.rotation.angle_z.degrees)
print('Pose: origin_id: %s' % pose.origin_id)
# look around to find the first cube
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
while True:
try:
cube = robot.world.wait_for_observed_light_cube(timeout=10)
if cube:
print(cube)
look_around.stop()
break
else:
robot.go_to_pose(Pose(pose.position.x,
pose.position.y,
pose.position.z,
angle_z=degrees(
pose.rotation.angle_z.degrees)),
relative_to_robot=True).wait_for_completed()
robot.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
except asyncio.TimeoutError:
print("Didn't find a cube :-(")
#finally:
#look_around.stop()
print("the type of the cube is ", type(cube.cube_id))
robot.say_text("the first cube is spotted").wait_for_completed()
cube.set_lights(cozmo.lights.green_light.flash())
cube1 = cube
id = cube.cube_id
robot.turn_in_place(degrees(30)).wait_for_completed()
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
print(id)
while True:
try:
cube1 = robot.world.wait_for_observed_light_cube(timeout=10)
if cube1.cube_id != id:
print(cube1)
else:
id = cube1.cube_id
look_around_2 = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
except asyncio.TimeoutError:
print("Didn't find a cube :-(")
finally:
look_around_3 = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
#robot.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed()
if id != cube1.cube_id:
break
print("the new id is ", id)
look_around.stop()
if look_around_2:
look_around_2.stop()
if look_around_3:
look_around_3.stop()
# set the head position for the cozmo robot
print("both cubes have been spotted ")
robot.say_text("both cubes have been spotted").wait_for_completed()
cube.set_lights(cozmo.lights.green_light.flash())
cube1.set_lights(cozmo.lights.green_light.flash())
robot.say_text("now i am looking for a pretty face").wait_for_completed()
cube.set_lights_off()
cube1.set_lights_off()
return cube, cube1
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, motion
robot.set_lift_height(0).wait_for_completed()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
# Useful vars for the local functions.
origin = robot.pose
start = grid.getStart()
goal_relative_to_cube = cozmo.util.Pose(-100,
0,
0,
angle_z=cozmo.util.degrees(0))
center_of_arena = (grid.width / 2, grid.height / 2)
motion = None # Current motion of the robot, if the robot is moving.
goal_pose = None
def pose_to_coords(pose):
"""Transforms a cozmo pose in world coordinates to the grid coordinates.
Assuming that the robot starts at the given start location on the map and facing +x direction."""
pose_relative_to_origin = get_relative_pose(pose, origin)
(x0, y0) = start
return round(
pose_relative_to_origin.position.x / grid.scale) + x0, round(
pose_relative_to_origin.position.y / grid.scale) + y0
def coords_to_pose(coords, angle):
(x, y) = coords
(x0, y0) = start
return origin.define_pose_relative_this(
cozmo.util.pose_z_angle((x - x0) * grid.scale,
(y - y0) * grid.scale, 0, angle))
def stop_motion():
global motion
if motion is not None and motion.is_running:
motion.abort()
motion = None
def direction(p1, p2):
"""Returns the direction vector from p1 to p2."""
return p2[0] - p1[0], p2[1] - p1[1]
def build_plan(path):
"""Given a path (list of coords), build a plan that goes through each key point on the path.
Returns a list of poses."""
plan = deque()
last_point = None
last_direction = None
for coords in path:
# The first point is the starting location. Don't need to add movement.
if last_point is None:
last_point = coords
continue
dir = direction(last_point, coords)
if dir != last_direction:
# Move to last point, facing current direction
plan.append(
coords_to_pose(
last_point,
cozmo.util.radians(math.atan2(dir[1], dir[0]))))
last_direction = dir
# Otherwise, we're continuing to the same direction
last_point = coords
if goal_pose is not None:
plan.append(goal_pose)
else:
plan.append(coords_to_pose(last_point, cozmo.util.radians(0)))
return plan
last_known_coords = [None, None,
None] # Last known coordinates for the 3 cubes.
plan = None # A list of poses to go through in order to reach the goal.
while not stopevent.is_set():
robot_coords = pose_to_coords(robot.pose)
cubes = [
robot.world.get_light_cube(id) for id in cozmo.objects.LightCubeIDs
]
update_map = False # Only update map in case cubes are moved.
for i, cube in enumerate(cubes):
if cube.is_visible:
coords = pose_to_coords(cube.pose)
if coords != last_known_coords[i]:
last_known_coords[i] = coords
update_map = True
if i == 0:
goal_pose = cube.pose.define_pose_relative_this(
goal_relative_to_cube)
if update_map or plan is None:
print("Cubes moved, updating map. Cube locations: " +
str(last_known_coords))
stop_motion()
grid.clearObstacles()
grid.clearGoals()
grid.setStart(robot_coords)
for coords in last_known_coords:
if coords:
# Mark each cube as a 3x3 square to account for the radius of the robot.
grid.addObstacle(coords)
for neighbor, _ in grid.getNeighbors(coords):
grid.addObstacle(neighbor)
if goal_pose:
grid.addGoal(pose_to_coords(goal_pose))
elif robot_coords != center_of_arena:
# Drive to the center of the arena.
grid.addGoal(center_of_arena)
else:
# Turn in place, 30 degrees at a time, and re-evaluate.
robot.turn_in_place(
cozmo.util.degrees(30)).wait_for_completed()
continue
# Now that we have a goal, replan the path.
astar(grid, heuristic)
plan = build_plan(grid.getPath())
print("Plan: " + str(plan))
# Follow the plan.
if len(plan) == 0:
if motion is not None:
motion.wait_for_completed()
stopevent.set()
return
if motion is None or motion.is_completed:
nextpose = plan.popleft()
print("Following plan to next pose: " + str(nextpose))
motion = robot.go_to_pose(nextpose)
time.sleep(0.1)
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
while not stopevent.is_set():
grid.clearStart()
grid.setStart((math.ceil(robot.pose.position.x / grid.scale),
math.ceil(robot.pose.position.y / grid.scale)))
cube1 = robot.world.get_light_cube(1)
cube2 = robot.world.get_light_cube(2)
cube3 = robot.world.get_light_cube(3)
print("cube1=" + str(cube1.is_visible) + str(cube1))
print("cube2=" + str(cube2.is_visible) + str(cube2))
print("cube3=" + str(cube3.is_visible) + str(cube3))
if cube1.is_visible:
cube1x = math.ceil(cube1.pose.position.x / grid.scale)
cube1y = math.ceil(cube1.pose.position.y / grid.scale)
cube1angle = cube1.pose.rotation.angle_z.degrees
for x in range(cube1x - 2, cube1x + 3):
for y in range(cube1y - 2, cube1y + 3):
grid.addObstacle((x, y))
rx = math.ceil(
(cube1.pose.position.x +
(100 * abs(math.cos(cube1.pose.rotation.angle_z.radians)))) /
grid.scale)
ry = math.ceil(
(cube1.pose.position.y +
(100 * abs(math.sin(cube1.pose.rotation.angle_z.radians)))) /
grid.scale)
grid.addGoal((rx, ry))
if cube2.is_visible:
cube2x = math.ceil(cube2.pose.position.x / grid.scale)
cube2y = math.ceil(cube2.pose.position.y / grid.scale)
for x in range(cube2x - 2, cube2x + 3):
for y in range(cube2y - 2, cube2y + 3):
grid.addObstacle((x, y))
if cube3.is_visible:
cube3x = math.ceil(cube3.pose.position.x / grid.scale)
cube3y = math.ceil(cube3.pose.position.y / grid.scale)
for x in range(cube3x - 2, cube3x + 3):
for y in range(cube3y - 2, cube3y + 3):
grid.addObstacle((x, y))
astar(grid, heuristic)
for step in grid.getPath():
print(step[0] * grid.scale, step[1] * grid.scale)
robot.go_to_pose(
Pose(step[0] * grid.scale,
step[1] * grid.scale,
0,
angle_z=degrees(0))).wait_for_completed()
robot.turn_in_place(degrees(180 +
cube1angle % 90)).wait_for_completed()
time.sleep(10)
def program_cozmo_action(robot: cozmo.robot.Robot):
#paramètres et initialisations
#pour l'url du serveur faire attention à la modifification faite dans le fonction "checkpicture", un rework sera peut-être nécessaire pour viser autre chose que l'API de démo
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
cube=None
numero_cube = 0
answer_json = 0
url = "http://localhost:3000/v1/images"
nbrVotesMin = 3 # Nombre de votes minimaux pour qu'une réponse soit jugée acceptable
# Tant que COZMO n'a pas vu de cube
while(cube == None):
#permet de voir l'environnement qu'entour cozmo
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
# scanne l’environnement à la recherche d’un cube.
cube = robot.world.wait_for_observed_light_cube(timeout=30)
look_around.stop()
# aller jusqu'a la position du cube identifié
robot.go_to_pose(cozmo.util.Pose(cube.pose.position.x, cube.pose.position.y, cube.pose.position.z, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
# prend en photo le cube (si COZMO a bien réussi à se placer, ce qui n'est pas gagné)
# c'est une boucle pour être sûr qu'il n'y à pas de problèmes
pictureIsTaken = True
while pictureIsTaken:
time.sleep(0.1)
latest_image = robot.world.latest_image
if latest_image is not None:
image = latest_image.raw_image
pictureIsTaken = False
# On génère une uuid pour l'image prise, qui servira d'identifiant
numero_cube = str(uuid.uuid4())
#save la photo prise pour pouvoir l'exposer au crowsourcing
image.save("CubeAction" + str(numero_cube) + ".bmp")
image = image.convert("RGB") # On supprime le channel alpha de l'image, qui pourraît poser des problèmes dans certains cas
# On utilise un buffer pour capturer les données brutes de l'image
buffered = BytesIO()
image.save(buffered, format="JPEG") # On convertit l'image
img_base64 = "data:image/jpeg;base64," # Cette chaine de caractères doit être rajouté pour que les navigateur reconnaissance que c'est une image
encode64 = img_base64 + str(base64.b64encode(buffered.getvalue())).replace("b'","").replace("'","") # Les navigateurs arrivent sans problèmes à afficher des images encodé en base64, mais pas quand Python rajoute ses " b' ... '" autour des valeurs brutes
response_send = sendPicture(encode64, url, numero_cube) #On envoie l'image
if response_send.status_code is not 201 : # gestion d'erreur
error_string = "Error, can't POST the picture to the server API : " + response_send.status_code + "\n" + response_send.text
raise IOError(error_string)
else : # Si l'image est posté avec succès
done = False
while not done : # Boucle de check si le nombre de vote dépasse le minimum requis
color = cozmo.lights.Light(cozmo.lights.Color(rgb=(178,128,1)))
robot.set_all_backpack_lights(color)
time.sleep(3)
answer_text = checkPicture(url, numero_cube) # On check la réponse sur l'API
answer_json = answer_text.json()
if answer_json.get("nbrVotes") is not None : # Si une réponse existe
if answer_json.get("nbrVotes") > nbrVotesMin: # Vérification du nombre de vote
done = True
color = cozmo.lights.Light(cozmo.lights.Color(rgb=(1,128,128)))
robot.set_all_backpack_lights(color)
# Réaliser les actions demandés
action = answer_json.get("answer")
# Enregistrement de couleurs de base de la librairie COZMO
red, green, blue, white=cozmo.lights.red_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.white_light
colors=[red, green, blue, white]
if(action== "red"):
color_light(cube, red)
elif(action== "white"):
color_light(cube, white)
elif(action== "blue"):
color_light(cube, blue)
elif(action== "green"):
color_light(cube, green)
elif(action== "crazy_color_1"):
crazy_color_1(cube, colors)
elif(action== "crazy_color_2"):
crazy_color_2(cube)
elif(action == "off"):
light_off(cube)
elif "say" in action :
robot.say_text(action.replace("say ", ""),True,use_cozmo_voice=True, in_parallel=True).wait_for_completed()
else:
error_string = "This action isn't a valide answer : " + action + "\nThe answer can be : red, blue, green, white, off, crazy_color_1, crazy_color_2"
raise ValueError(error_string)
return 1
def cozmo_program(robot: cozmo.robot.Robot):
# Move lift down and tilt the head up
robot.move_lift(-3)
robot.set_head_angle(degrees(0)).wait_for_completed()
# intrinsics from calibration.cfg
cameraParam = [288.15418237, 285.12686892, 197.31722863,
120.45748409] # [fx, fy, cx, cy]
extrinsic = np.array(
[[-9.97469913e-02, -5.61812756e-02, 9.93425489e-01, -2.40474116e-02],
[-9.94999768e-01, 5.15631732e-04, -9.98758996e-02, -4.49931548e-03],
[5.09891373e-03, -9.98420452e-01, -5.59517889e-02, 3.75532293e-02],
[0., 0., 0., 1.]])
detector = Detector("tagStandard41h12",
quad_decimate=2.0,
quad_sigma=1.0,
debug=False)
# create main window
cv2.namedWindow("AprilTag Detection", 1)
''' relative motion '''
trajectory = [[400, 0, 0], [0, 250, 90], [0, 400, 180], [0, 250, 270]]
dx = 0
dy = 0
# goes around a block four times
for i in range(4):
for coord in trajectory:
target_pose = Pose(coord[0] + dx,
coord[1] + dy,
0,
angle_z=degrees(coord[2]))
robot.go_to_pose(target_pose,
relative_to_robot=True).wait_for_completed()
robot.set_head_angle(degrees(0)).wait_for_completed()
# convert Bayer GB to RGB for display
image = robot.world.latest_image.raw_image
img = np.array(image)
gray = cv2.cvtColor(
img, cv2.COLOR_BGR2GRAY
) # convert Bayer BG to Grayscale for corner detections
tags = detector.detect(gray,
estimate_tag_pose=True,
camera_params=cameraParam,
tag_size=0.0127)
while len(tags) == 0:
image = robot.world.latest_image.raw_image
img = np.array(image)
gray = cv2.cvtColor(
img, cv2.COLOR_BGR2GRAY
) # convert Bayer BG to Grayscale for corner detections
tags = detector.detect(gray,
estimate_tag_pose=True,
camera_params=cameraParam,
tag_size=0.0127)
# visualize the detection
tag = tags[0]
for idx in range(len(tag.corners)):
cv2.line(gray, tuple(tag.corners[idx - 1, :].astype(int)),
tuple(tag.corners[idx, :].astype(int)), (255, 0, 0))
rot_mat = np.array([[
tag.pose_R[0][0], tag.pose_R[0][1], tag.pose_R[0][2],
tag.pose_t[0]
],
[
tag.pose_R[1][0], tag.pose_R[1][1],
tag.pose_R[1][2], tag.pose_t[1]
],
[
tag.pose_R[2][0], tag.pose_R[2][1],
tag.pose_R[2][2], tag.pose_t[2]
], [0.0, 0.0, 0.0, 1.0]],
dtype='float')
tag_pose = np.matmul(rot_mat, np.array([0, 0, 0.0125, 1]))
pose_in_Cozmo = np.matmul(extrinsic, tag_pose)
# label the id of AprilTag on the image.
cv2.putText(gray,
str(pose_in_Cozmo),
org=(tag.corners[0, 0].astype(int) + 10,
tag.corners[0, 1].astype(int) + 10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.3,
color=(255, 0, 0))
cv2.imshow('AprilTags', gray)
dx = pose_in_Cozmo[0] * 1000 - 100
dy = pose_in_Cozmo[1] * 1000
def cozmoBehavior(robot: cozmo.robot.Robot):
"""Cozmo search behavior. See assignment document 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
foundGoal = False
setPath = False
while not stopevent.is_set():
if grid.getGoals() == []:
# find goal
look_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
try:
cube = robot.world.wait_for_observed_light_cube(timeout=30)
print("Found cube: %s" % cube)
foundGoal = True
pos = cube.pose.position
x, y, z = pos.x, pos.y, pos.z
cube_angle = cube.pose.rotation.angle_z.degrees
a, b = 0, 0
if cube_angle > -22.5 and cube_angle < 22.5:
a, b = -1, 0
if cube_angle >= 22.5 and cube_angle < 67.5:
a, b = -1, 1
if cube_angle >= 67.5 and cube_angle < 112.5:
a, b = 0, 1
if cube_angle >= 112.5 and cube_angle < 157.5:
a, b = 1, 1
if cube_angle >= 157.5 or cube_angle <= -157.5:
a, b = 1, 0
if cube_angle > -67.5 and cube_angle <= -22.5:
a, b = -1, -1
if cube_angle > -112.5 and cube_angle <= -67.5:
a, b = 0, -1
if cube_angle > -157.5 and cube_angle <= -112.5:
a, b = 1, -1
obs1 = int(x / grid.scale + grid.getStart()[0] +
0.5), int(y / grid.scale + grid.getStart()[1] + 0.5)
add_obs = []
for i in range(-1, 2, 1):
for j in range(-1, 2, 1):
ob = obs1[0] + i, obs1[1] + j
add_obs.append(ob)
grid.addObstacles(add_obs)
goal = obs1[0] + a * 2, obs1[1] + b * 2
print(goal)
grid.addGoal(goal)
except asyncio.TimeoutError:
print("Didn't find a cube")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
else:
# astar
# astar(grid)
# get path
# path = grid.getPath() #a list of cells
robot_pose = robot.pose
print(robot_pose)
rx, ry = robot_pose.position.x, robot_pose.position.y
cx = int(rx / grid.scale + grid.getStart()[0] + 0.5)
cy = int(ry / grid.scale + grid.getStart()[1] + 0.5)
# update start
grid.clearStart()
grid.setStart((cx, cy))
astar(grid, heuristic)
path = grid.getPath()
print(path)
prev = path[0]
for p in path[1:]:
diff = p[0] - prev[0], p[1] - prev[1]
angle = getAngle(diff)
posX, posY = grid.scale * (p[0] - 1), grid.scale * (p[1] - 1)
# posX, posY = 25 * p[0] - rx, 25 * p[1] - ry
# pose = cozmo.util.Pose(posX, posY, 0.0, q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0)
# Everytime we move, check to see if there's an obstacle in our vision
pose = cozmo.util.Pose(posX,
posY,
0.0,
angle_z=cozmo.util.degrees(angle))
robot.go_to_pose(pose, in_parallel=True).wait_for_completed()
# robot.go_to_pose(pose)
# time.sleep(0.1)
print(p)
print(robot.pose.position.x, robot.pose.position.y)
print(posX, posY)
prev = p
ca = -1.0 * cube_angle
pose = cozmo.util.Pose(robot.pose.position.x,
robot.pose.position.y,
robot.pose.position.z,
angle_z=cozmo.util.degrees(ca))
robot.go_to_pose(pose).wait_for_completed()
break # Your code here
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
width = grid.width
height = grid.height
scale = grid.scale
startX = grid.getStart()[0]
startY = grid.getStart()[1]
#Find Goal
robot.move_lift(-3)
robot.set_head_angle(degrees(0)).wait_for_completed()
# Boolean that determines if Cozmo is already in the center of the grid
inCenter = False
cube1 = None
#The intial goal is set to the center of the grid, in case the cube is not found initially
grid.addGoal((width / 2, height / 2))
while not stopevent.is_set():
#Update start coordinate to be current robot's pose
xR = int(robot.pose.position.x / scale) + startX
yR = int(robot.pose.position.y / scale) + startY
grid.setStart((xR, yR))
cubeObstacle = None
try:
cubeObstacle = robot.world.wait_for_observed_light_cube(timeout=2)
except asyncio.TimeoutError:
print("No Cube obstacle was found")
if cubeObstacle is not None:
if cubeObstacle.object_id == 1 and cube1 is None:
cube1 = cubeObstacle
# We found cube 1. Clear the old goal of going to center.
grid.clearGoals()
#Get cube's 1 coordinates and rotation
print("Found Cube 1:", cube1)
#Assumption: Cube's coordinates is global coordiantes (where Robot originally started)
xC = int(cube1.pose.position.x / scale) + startX
yC = int(cube1.pose.position.y / scale) + startY
angle_zC = cube1.pose.rotation.angle_z
# Make cube 1 an obstacle to avoid hitting it
addCubeObstacle(grid, (xC, yC))
#Compute the final goal since we found Cube 1
#This is the final distance where we want to finish from the cube
distanceToCube = 4
goalX = int(
round(xC + distanceToCube * math.cos(angle_zC.radians)))
goalY = int(
round(yC + distanceToCube * math.sin(angle_zC.radians)))
grid.addGoal((goalX, goalY))
print("Goal:", grid.getGoals()[0])
else:
#We found cube 2 or 3. Let's make them obstacles
xObstacle = int(cubeObstacle.pose.position.x / scale) + startX
yObstacle = int(cubeObstacle.pose.position.y / scale) + startY
print("Obstacle found:(", xObstacle, ",", yObstacle, ")")
addCubeObstacle(grid, (xObstacle, yObstacle))
#Stop robot if we are close enough to goal.
numSquaresThreshold = 1
goalX = grid.getGoals()[0][0]
goalY = grid.getGoals()[0][1]
distanceToGoal = math.sqrt(
math.pow(goalX - xR, 2) + math.pow(goalY - yR, 2))
if cube1 is not None:
if distanceToGoal < numSquaresThreshold:
#Rotate the robot to face cube's face and finish
degreesToTurn = normalizeAngle(
cube1.pose.rotation.angle_z.degrees + 180 -
robot.pose.rotation.angle_z.degrees)
print("degrees to turn:", degreesToTurn)
robot.turn_in_place(degrees(degreesToTurn),
speed=degrees(40)).wait_for_completed()
break
elif distanceToGoal < numSquaresThreshold or inCenter:
# We haven't found cube1 yet but we are in the center of the grid. Just rotate to look around
inCenter = True
robot.turn_in_place(degrees(20),
speed=degrees(40)).wait_for_completed()
continue
# Use A* to find the path to cube
astar(grid, heuristic)
path = grid.getPath()
if len(path) >= 2:
movStart = path[0]
movEnd = path[1]
robotAngle = math.degrees(
math.atan2(movEnd[1] - movStart[1], movEnd[0] - movStart[0]))
newRobotX = robot.pose.position.x + (movEnd[0] -
movStart[0]) * scale
newRobotY = robot.pose.position.y + (movEnd[1] -
movStart[1]) * scale
robot.go_to_pose(Pose(newRobotX,
newRobotY,
0,
angle_z=degrees(robotAngle)),
relative_to_robot=False).wait_for_completed()
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
#robot.pose = Pose(0,0,0,angle_z = degrees(0))
scale = grid.getScale()
start = grid.getStart()
#print("Start %i,%i",start[0],start[1])
#print("Scale %i",scale)
#print(robot.pose)
pose = cell_to_pose(start, scale, 0)
#print(pose)
robot.go_to_pose(cell_to_pose(start, scale, 0)).wait_for_completed()
cubes_seen = set()
while not stopevent.is_set():
cube = None
count = 0
while (cube is None):
try:
cube = robot.world.wait_for_observed_light_cube(timeout=1)
except Exception:
robot.turn_in_place(
cozmo.util.degrees(15)).wait_for_completed()
count += 1
if count >= 6:
start = (start[0] + 1, start[1] + 1)
grid.setStart(start)
robot.go_to_pose(cell_to_pose(start, scale,
0)).wait_for_completed()
count = 0
cubes_seen.add(cube.cube_id)
finalanlge = cube.pose.rotation.angle_z.degrees
robot.say_text("I see the cube").wait_for_completed()
#print(cube.pose)
block = pose_to_cell(cube.pose, scale)
grid.addObstacle(block)
finish = get_finish_from_pose(cube.pose, scale)
grid.addGoal(finish)
astar(grid, heuristic)
newpath = True
while newpath:
newpath = False
path = grid.getPath()
last = path[0]
for i in range(len(path)):
angle = 0
if i + 1 < len(path):
angle = get_angle(path[i], path[i + 1])
robot.go_to_pose(cell_to_pose(path[i], scale,
angle)).wait_for_completed()
newcube = None
try:
newcube = robot.world.wait_for_observed_light_cube(
timeout=1)
except Exception:
print("don't do anything")
if newcube is not None and newcube.cube_id not in cubes_seen:
cubes_seen.add(newcube.cube_id)
robot.say_text("I found a new cube").wait_for_completed()
grid.setStart(path[i])
grid.addObstacle(pose_to_cell(newcube.pose, scale))
grid.clearPath()
grid.clearVisited()
astar(grid, heuristic)
newpath = True
break
if not newpath:
robotangle = robot.pose.rotation.angle_z.degrees
robot.turn_in_place(
cozmo.util.degrees(finalanlge -
robotangle)).wait_for_completed()
break
stopevent.set()
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
# Start robot at start location
x = grid.getStart()[0] * grid.scale
y = grid.getStart()[1] * grid.scale
robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(0))).wait_for_completed()
# Add dummy goal
x = int(grid.width / 2)
y = int(grid.height / 2)
grid.addGoal((x, y))
foundCubes = set()
goalRotation = 0
while not stopevent.is_set():
# Clear visited
grid.clearVisited()
# Clear path
grid.clearPath()
# Spin to find cubes until all are found
if len(foundCubes) < 3:
for _ in range(4):
robot.turn_in_place(degrees(90),
speed=degrees(180)).wait_for_completed()
if len(foundCubes) < 3:
start = time.time()
while time.time() - start < 1:
try:
cube = robot.world.wait_for_observed_light_cube(
timeout=0.5, include_existing=False)
if cube and cube.cube_id not in foundCubes:
x, y, _ = cube.pose.position.x_y_z
x = int(round(x / grid.scale))
y = int(round(y / grid.scale))
foundCubes.add(cube.cube_id)
if cube.cube_id == 1:
goalRotation = (round(
cube.pose.rotation.angle_z.degrees /
90) * 90) % 360
print(x, y)
if goalRotation == 0:
x -= 3
elif goalRotation == 90:
y -= 3
elif goalRotation == 180:
x += 3
else:
y += 3
print(x, y)
grid.clearGoals()
grid.addGoal((x, y))
else:
# Add obstacle in 3x3 around cube
for i in range(-1, 2, 1):
for j in range(-1, 2, 1):
grid.addObstacle((x + i, y + j))
except asyncio.TimeoutError:
continue
# Take next step
astar(grid, heuristic)
if len(grid.getPath()) < 2:
if 1 in foundCubes:
robot.turn_in_place(degrees(goalRotation),
speed=degrees(180)).wait_for_completed()
print("COMPLETE")
stopevent.wait()
else:
print("CUBE 1 NOT VISIBLE")
continue # Keep spinning until cube 1 found
if len(foundCubes) == 3:
index = len(grid.getPath()) - 1
else:
index = 1
x = grid.getPath()[index][0] * grid.scale
y = grid.getPath()[index][1] * grid.scale
robot.go_to_pose(Pose(x, y, 0,
angle_z=degrees(0))).wait_for_completed()
# Update start
x, y, _ = robot.pose.position.x_y_z
x = int(round(x / grid.scale))
y = int(round(y / grid.scale))
grid.setStart((x, y))
pass
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 custom_objects(robot: cozmo.robot.Robot):
# Gestionnaires d'évennements à chaque fois que Cozmo vois ou arrète de voir un objet
robot.add_event_handler(cozmo.objects.EvtObjectAppeared,
handle_object_appeared)
robot.add_event_handler(cozmo.objects.EvtObjectDisappeared,
handle_object_disappeared)
# Création des custom objects
objs = objects(robot)
if None not in objs:
print("All objects defined successfully!")
else:
print("One or more object definitions failed!")
return
robot.say_text("À la recherche des objet").wait_for_completed()
setup_camera(robot)
origin = robot.pose
am = ActionManager(robot)
stops = 1
while len(stops_visited) < 6:
# Chercer les objest
lookaround = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
objs = robot.world.wait_until_observe_num_objects(
num=1, object_type=CustomObject, timeout=60)
lookaround.stop()
if objs[0].object_type in stops_visited:
continue
stops_visited.append(objs[0].object_type)
robot.say_text("Objet trouvé").wait_for_completed()
if len(objs) > 0:
photo(robot)
pose = custom_object_pose(robot, objs[0])
robot.go_to_pose(pose,
relative_to_robot=False).wait_for_completed()
photo(robot)
robot.say_text(f"Arrête {stops}").wait_for_completed()
am.launch(objs[0])
print("origin: ", origin)
robot.go_to_pose(origin,
relative_to_robot=False).wait_for_completed()
stops += 1
else:
print("Cannot locate custom box")
robot.play_anim_trigger(
cozmo.anim.Triggers.SparkSuccess).wait_for_completed()
while True:
time.sleep(0.1)
def en_charger(self, robot:cozmo.robot.Robot = None, cmd_args = None):
usage = "Cozmo tries to park on is charger, in 3 tries."
if robot is None:
return usage
trial = 1
# try to find the charger
charger = None
# see if Cozmo already knows where the charger is
if robot.world.charger:
if robot.world.charger.pose.origin_id == robot.pose.origin_id:
if log:
print("Cozmo already knows where the charger is!")
charger = robot.world.charger
else:
# Cozmo knows about the charger, but the pose is not based on the
# same origin as the robot (e.g. the robot was moved since seeing
# the charger) so try to look for the charger first
pass
if not charger:
# Tell Cozmo to look around for the charger
if log:
print("looking for the charger now...")
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
try:
charger = robot.world.wait_for_observed_charger(timeout=60)
print("Found charger: %s" % charger)
except asyncio.TimeoutError:
print("Didn't see the charger")
finally:
# whether we find it or not, we want to stop the behavior
look_around.stop()
if charger:
if log:
print("lifting my arms to manouver...")
robot.move_lift(10)
# Attempt to drive near to the charger, and then stop.
if log:
print("Trial number %s" % trial)
print("Going for the charger!!!")
action = robot.go_to_pose(charger.pose)
action.wait_for_completed()
if log:
print("Completed action: result = %s" % action)
robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
if log:
print("Done.")
# Turn 180 (and 5) degrees, then goes backwards at full speed
if log:
print("Now the grand finalle: turn around and park!")
print("Turning...")
#this value needs to be tweaked (90 or 95)
robot.turn_in_place(degrees(90)).wait_for_completed()
robot.turn_in_place(degrees(95)).wait_for_completed()
time.sleep( 1 )
if log:
print("Get out of the way: here I go!")
robot.drive_straight(distance_mm(-130), speed_mmps(150)).wait_for_completed()
if log:
print("checking if I did it...")
if robot.is_on_charger:
robot.move_lift(-8)
print("I did it! Yay!")
else:
print("I did not manage to dock in the charger =(")
print("Trying again...")
robot.world.charger = None
if log:
print("let me go a little bit further to be easier to see...")
robot.drive_straight(distance_mm(90), speed_mmps(50)).wait_for_completed()
trial += 1
if trial < 4:
self.en_charger(robot)
else:
print("tired of trying. Giving up =(")
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
while not stopevent.is_set():
# Step 1: Update Map: Update the map to correctly represent all cubes detected by the robot.
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
cube1 = robot.world.get_light_cube(LightCube1Id)
if cube1 is not None:
cube1.set_lights(cozmo.lights.red_light)
else:
cozmo.logger.warning(
"Cozmo is not connected to a LightCube1Id cube - check the battery."
)
foundCube1 = False
cubesFound = {}
# from_global_to_grid_position()
# from_grid_position_to_global()
originalPose = robot.pose
while not foundCube1:
visibleObjectCount = robot.world.visible_object_count(
object_type=None)
visibleObjects = robot.world.visible_objects
if visibleObjectCount > 0:
for visibleObject in visibleObjects:
if isinstance(visibleObject, cozmo.objects.LightCube):
cubesFound[
visibleObject.
object_id] = visibleObject.pose.define_pose_relative_this(
originalPose)
print("Found cube")
print(visibleObject.object_id)
print(cubesFound[visibleObject.object_id])
if visibleObject.object_id == 2:
foundCube1 = True
if not foundCube1:
robot.turn_in_place(degrees(10)).wait_for_completed()
# time.sleep(1)
if cube1 is not None:
cube1.set_lights(cozmo.lights.green_light)
grid.clearVisited()
grid.clearGoals()
grid.clearStart()
grid.clearPath()
grid.setStart((0, 0))
cubePositions = [
(cubeId, cubeToAdd.position.x, cubeToAdd.position.y,
cubeToAdd.position.z, cubeToAdd.rotation.angle_z.degrees)
for cubeId, cubeToAdd in cubesFound.items()
]
print(cubePositions)
cubeObstacles = [
(int(cubeX / grid.scale), int(cubeY / grid.scale))
for cubeId, cubeX, cubeY, cubeZ, cubeAngle in cubePositions
if cubeId != 2
]
print(cubeObstacles)
for cubeObstacle in cubeObstacles:
addObstaclesAround(grid, cubeObstacle)
# grid.addObstacles(cubeObstacles)
goalCube = [(int(cubeX / grid.scale), int(cubeY / grid.scale))
for cubeId, cubeX, cubeY, cubeZ, cubeAngle in cubePositions
if cubeId == 2][0]
print("goalCube")
print(goalCube)
grid.addGoal(goalCube)
# cube_1_pose = cubesFound[1].pose
while not closeEnough(grid.getStart(), grid.getGoals()[0]):
newStart = robot.pose.define_pose_relative_this(originalPose)
print("newStart")
print(newStart)
grid.clearStart()
grid.setStart(world_to_grid(newStart, grid))
addObstaclesIfSeen(robot, originalPose, grid)
grid.clearVisited()
grid.clearPath()
astar(grid, heuristic)
if not grid.checkPath():
print("CONFUSED")
return
nextStep = grid.getPath()[1] # go one steps ahead
stepAfter = grid.getPath()[2] # go two steps ahead
print(nextStep)
delta_x = (stepAfter[0] * grid.scale - nextStep[0] * grid.scale)
delta_y = (stepAfter[1] * grid.scale - nextStep[1] * grid.scale)
nextPose = Pose(
nextStep[0] * grid.scale,
nextStep[1] * grid.scale,
originalPose.position.z,
angle_z=degrees(90 if delta_x == 0 else 180.0 *
math.atan(delta_y / delta_x) / math.pi))
robot.go_to_pose(nextPose,
relative_to_robot=False).wait_for_completed()
# Step 2: Navigate to Goal: Successfully navigate the robot from a known start location to Cube 1 and stop within 10cm of Cube 1.
# Step 3: Cube Orientation: Extend your navigation code to consider which side of the cube the robot is approaching. Have the robot approach the side of the cube that looks like Figure 1. The orientation of the cube can be accessed with cube.pose.rotation.angle_z. The robot should stop within 10cm of the cube, facing toward the correct side of the cube but without touching it.
goalPose = Pose(cube1.pose.position.x +
100 * math.cos(cube1.pose.rotation.angle_z.radians),
cube1.pose.position.y +
100 * math.sin(cube1.pose.rotation.angle_z.radians),
cube1.pose.position.z,
angle_z=degrees(
(720 + cube1.pose.rotation.angle_z.degrees - 180) %
360))
robot.go_to_pose(goalPose,
relative_to_robot=False).wait_for_completed()
def cozmo_program(robot: cozmo.robot.Robot):
logging.basicConfig(format='%(asctime)s - %(message)s',
filename="cozmo_program.log",
level=logging.INFO)
reset_sequence()
pose = robot.pose
# explore world
logging.info("Cozmo starts exploring")
cube, cube1 = explore_the_world(robot)
logging.info("Cozmo detected Cubes:", cube, cube1)
get_in_position(robot)
# meet the player
face = find_face(robot)
logging.info("Identified Face:", face)
robot.go_to_pose(pose, relative_to_robot=False).wait_for_completed()
############################################## Game Preparation ###################################
robot.say_text("I will try to stack the cubes first").wait_for_completed()
robot.say_text("Then it will be your turn").wait_for_completed()
# Autonomous play
cube_stack(robot)
logging.info("Victory Condition check - Cozmo game")
# Victory condition check
if abs(cube.pose.position.z -
cube1.pose.position.z) > 40: # Assuming cube height (z) is 40mm.
logging.info("Cube is successfully stacked")
else:
logging.info("Cube stack failed!")
# reset game
reset_game(robot)
############################################# Play the game ######################################
# Explain game rules to the player
robot.say_text("It's your turn now!").wait_for_completed()
robot.say_text(
"You need to show me the cards and i will execute the actions"
).wait_for_completed()
robot.say_text(
"Remember, The goal is to stack the cubes").wait_for_completed()
victory_flag = 1
logging.info("Human interaction with cozmo starts")
while (victory_flag):
reset_sequence()
# Show markers
result = cozmo_action_ar_marker_cards(robot)
logging.info("Marker Cards detected - Action Sequence results: ",
result)
# Execute the actions.
logging.info("Executing Sequence")
execute_sequence(robot, result)
############################################# Game result ########################################
# results of the game
logging.info("Victory condition check - Human game")
if abs(cube.pose.position.z - cube1.pose.position.z
) > 40: # Assuming cube height (z) is 40mm.
logging.info("Cube is successfully stacked")
robot.say_text("Well done").wait_for_completed()
victory_flag = 0
else:
logging.info("Cube stack failed!")
logging.info(
"Reset the game and start again! Press Cntrl + C to exit")
robot.say_text(
"Nice try! do you want to play again?").wait_for_completed()
break
