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)
async def go_to_ar_cube(robot: cozmo.robot.Robot, fsm):
'''The core of the go to object test program'''
# look around and try to find a cube
# look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
cube = None
#loop = asyncio.get_event_loop()
#loop.run_until_complete(robot.drive_wheels(-0.1,0.1,duration=5))
print("driving wheels")
robot.drive_wheel_motors(-15,15)
print("wheels driven")
while cube is None:
cube = await robot.world.wait_for_observed_light_cube()
robot.stop_all_motors()
fsm.found_cube()
if cube:
# Drive to 70mm away from the cube (much closer and Cozmo
# will likely hit the cube) and then stop.
# print(cube.pose)
# fsm.found_cube()
# robot.say_text("Found cube, going to cube!")
cube_x = cube.pose.position.x
cube_y = cube.pose.position.y
cube_angle = cube.pose.rotation.angle_z.degrees
pose_x = cube_x - robot.pose.position.x
pose_y = cube_y - robot.pose.position.y
pose_x += (-60 + (abs(cube_angle) / 1.5))
if cube_angle < -90:
pose_y += ((abs(abs(cube_angle) - 180)) / 1.5)
elif cube_angle < 0:
pose_y += ((abs(cube_angle)) / 1.5)
elif cube_angle > 90:
pose_y -= ((abs(abs(cube_angle) - 180)) / 1.5)
else:
pose_y -= cube_angle / 1.5
print("robot vals : x-val: %s, y-val: %s", robot.pose.position.x, robot.pose.position.y)
print("cube vals : x-val: %d, y-val: %d", cube_x, cube_y)
await robot.go_to_pose(Pose(pose_x, pose_y, 0,
angle_z=cube.pose.rotation.angle_z), relative_to_robot=False).wait_for_completed()
# print(robot.pose)
if robot.pose.position.x - cube_x < -60 or robot.pose.position.x - cube_x > 60:
# fsm.lost_cube()
await fsmlib.trigger(fsm, "lost_cube", robot)
# robot.say_text("Lost Cube")
await go_to_ar_cube(robot, fsm)
if robot.pose.position.y - cube_y < -60 or robot.pose.position.y - cube_y > 60:
# fsm.lost_cube()
await fsmlib.trigger(fsm, "lost_cube", robot)
# robot.say_text("Lost Cube")
await go_to_ar_cube(robot, fsm)
# robot.say_text("At the cube")
await fsmlib.trigger(fsm, "at_cube", robot)
def correct_turn(robot: cozmo.robot.Robot, turn, mode):
""" Performs the turn for the correction. In case of
'drive' mode, wheels are stopped and started again,
in case of 'step' mode, only turn is performed
"""
log.info('Turn for correction...')
if mode == MODE_DRIVE:
robot.stop_all_motors()
robot.turn_in_place(degrees(turn)).wait_for_completed()
if mode == MODE_DRIVE:
robot.drive_wheel_motors(speed, speed)
async def CozmoPID(robot: cozmo.robot.Robot):
global stopevent
with open("./config.json") as file:
config = json.load(file)
kp = config["kp"]
ki = config["ki"]
kd = config["kd"]
###############################
# PLEASE ENTER YOUR CODE BELOW
# initialize the variables and objects in memory
cube = None
cubefound = False
rt = 0
et = 0
yt = 0 # the desired distance you want to travel
ut = 0
while not cubefound:
await robot.world.wait_for_observed_light_cube(timeout=30)
for obj in robot.world.visible_objects:
if obj is not None:
cube = obj
# print in cmd to indicate a cube found
print("cube found!")
# print the distance of the cube in front of
# the robot
print(cube.pose.position.x)
# prepare to break the while loop
cubefound = True
# break the for loop to get rid of duplicates
break
# calculate rt, the desired distance to travel
rt = cube.pose.position.x - robot.pose.position.x - 125 + 5
# print rt in case something went wrong
print(rt)
'''
PID control loop
'''
while True:
# calculate the error associated with time t
print(et)
et = rt - yt
# update the ut using PID
ut = kp * et + ki * yt + kd * (-ut)
# inject the input to the wheel motors
robot.drive_wheel_motors(ut, ut)
# travel for about 0.1 second
time.sleep(0.1)
# update yt after setting new speed
yt += 0.1 * ut
# breaking condition for debugging, not needed
'''
def cozmo_drive_forward(robot: cozmo.robot.Robot):
""" Follows line forever (continuously)
"""
global show_img
log.info('Entering Cozmo drive_forward...')
show_img = False
robot.set_head_light(True)
robot.camera.image_stream_enabled = True
robot.set_lift_height(1.0).wait_for_completed()
robot.drive_wheel_motors(speed, speed)
while True:
if drive_forward(robot):
continue
else:
break
def cozmo_program(robot: cozmo.robot.Robot):
"robot.enable_stop_on_cliff(True)"
clockwise = True
for edge in range(4):
while not robot.is_cliff_detected:
robot.drive_wheel_motors(50.0, 50.0, l_wheel_acc=0, r_wheel_acc=0)
robot.stop_all_motors()
#dimensions[edge] = distance_mm()#
"back up"
time.sleep(0.5)
robot.drive_straight(distance_mm(-20),
speed_mmps(50)).wait_for_completed()
if clockwise:
"Check left turn"
robot.turn_in_place(degrees(90)).wait_for_completed()
time.sleep(0.5)
robot.drive_straight(distance_mm(10),
speed_mmps(50)).wait_for_completed()
"left side is edge"
if robot.is_cliff_detected:
"back up"
time.sleep(0.5)
robot.drive_straight(distance_mm(-10),
speed_mmps(50)).wait_for_completed()
robot.turn_in_place(degrees(-180)).wait_for_completed()
time.sleep(0.5)
robot.drive_straight(distance_mm(10),
speed_mmps(50)).wait_for_completed()
if robot.is_cliff_detected:
break
else:
clockwise = False
else:
robot.turn_in_place(degrees(-90)).wait_for_completed()
time.sleep(0.5)
robot.drive_straight(distance_mm(10),
speed_mmps(50)).wait_for_completed()
"left side is edge"
if robot.is_cliff_detected:
break
"checking the dimensions to determine whether it is a rectangle"
'''print(dimensions[0])
print(dimensions[1])
print(dimensions[2])
print(dimensions[3])
if dimensions[2] - dimensions[0] <= distance_mm(25) and dimensions[3] - dimensions[1] <= distance_mm(25):
'''
robot.say_text("The environment is a rectangle").wait_for_completed()
def cozmo_drive_to_target(robot: cozmo.robot.Robot):
global currentPose
didhut = False
while True:
relativeTarget = Frame2D() # TODO determine current position of target relative to robot
relativeTarget = currentPose.inverse().mult(targetPose)
print("relativeTarget"+str(relativeTarget))
velocity = target_pose_to_velocity_linear(relativeTarget)
print("velocity"+str(velocity))
trackSpeed = velocity_to_track_speed(velocity[0],velocity[1])
print("trackSpeedCommand"+str(trackSpeed))
lspeed = robot.left_wheel_speed.speed_mmps
rspeed = robot.right_wheel_speed.speed_mmps
print("trackSpeed"+str([lspeed, rspeed]))
delta = track_speed_to_pose_change(lspeed, rspeed,interval)
currentPose = delta.mult(currentPose)
print("currentPose"+str(currentPose))
print()
robot.drive_wheel_motors(l_wheel_speed=trackSpeed[0],r_wheel_speed=trackSpeed[1])
time.sleep(interval)
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)
async def run(robot: cozmo.robot.Robot):
# Move lift down and tilt the head up
robot.move_lift(-3)
await robot.set_head_angle(degrees(-10)).wait_for_completed()
robot.world.image_annotator.annotation_enabled = False
robot.world.image_annotator.add_annotator('box', BoxAnnotator)
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
gain, exposure, mode = 390, 3, 1
fsm = fsmlib.init_fsm()
try:
positions = []
last_angle = 0
nones = 0
while True:
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30) # get camera image
# i = Image.new('RGBA', (cozmo.oled_face.SCREEN_WIDTH, cozmo.oled_face.SCREEN_HEIGHT), (0,0,0,0))
# d = ImageDraw.Draw(i)
# # draw text, full opacity
# d.text((10,60), fsm.current, fill=(255,255,255,255))
#
# image_data = cozmo.oled_face.convert_image_to_screen_data(i)
# action = robot.display_oled_face_image(image_data, in_parallel=True)
#
# action.wait_for_completed()
if event.image is not None:
image = cv2.cvtColor(np.asarray(event.image), cv2.COLOR_BGR2RGB)
if mode == 1:
robot.camera.enable_auto_exposure = True
else:
robot.camera.set_manual_exposure(exposure, fixed_gain)
if fsm.current == 'search_for_AR_cube':
# robot.say_text("Searching for AR cube!")
await go_to_ar_cube(robot, fsm)
await fsmlib.trigger(fsm, "switch_to_color", robot)
elif fsm.current == 'go_to_colored_cube':
# find the cube
cube = find_cube(image, YELLOW_LOWER, YELLOW_UPPER)
BoxAnnotator.cube = cube
################################################################
# Todo: Add Motion Here
################################################################
# need estimated diameter of cube from 5cm away
# need to turn until x is in the middle of the range. Dimensions are 320x240
if cube == None:
nones = nones + 1
if nones > 7:
nones = 0
robot.stop_all_motors()
direction = -1 if last_angle > 0 else 1
await robot.turn_in_place(degrees(40*direction)).wait_for_completed()
continue
nones = 0
angle = cube[0] - 160
last_angle = angle
positions.append((angle, cube[2]))
if len(positions) > 10:
positions.pop(0)
if len(positions) < 10:
continue
# get avg. position
avg_x = sum(x[0] for x in positions) / float(len(positions))
avg_size = sum(x[1] for x in positions) / float(len(positions))
outliers = sum(0 if abs(x[0] - avg_x) < 20 and abs(x[1] - avg_size) < 20 else 1 for x in positions)
print(avg_x, avg_size)
if outliers >= 3:
continue
if (avg_size > 100):
robot.stop_all_motors()
if (avg_x > 40 or avg_x < -40):
await robot.turn_in_place(degrees(10 * (-1 if avg_x > 0 else 1))).wait_for_completed()
positions = []
continue
if (avg_size < 120):
dist = 50 if avg_size < 80 else 20
await robot.drive_straight(distance_mm(20), speed_mmps(40), False, False,
0).wait_for_completed()
positions = []
continue
else:
print("driving wheels")
l_speed = 20
r_speed = 20
if (positions[-1][0] > 40):
l_speed += 20
if (positions[-1][0] < -40):
r_speed += 20
robot.drive_wheel_motors(l_speed,r_speed)
await asyncio.sleep(0.1)
elif fsm.current == "at_colored_cube":
continue
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
def cozmo_program(robot: cozmo.robot.Robot):
global posiblePos
timeInterval = 0.1
threading.Thread(target=runMCLLoop, args=(robot,)).start()
threading.Thread(target=runPlotLoop, args=(robot,)).start()
robot.enable_stop_on_cliff(True)
# main loop
# TODO insert driving and navigation behavior HERE
# t = 0
# targetPose=Frame2D.fromXYA(400,700,0.5*3.1416) # this is the target position
# relativeTarget = targetPose
differentTarget = []
targetPose = Frame2D.fromXYA(180, 320, 0.5 * 3.1416)
differentTarget.append(targetPose)
targetPose = Frame2D.fromXYA(400, 700, 0.5 * 3.1416)
differentTarget.append(targetPose)
# explore, move
action = "move1"
timer = 0
while True:
if robot.is_cliff_detected:
robot.drive_wheel_motors(l_wheel_speed=-25, r_wheel_speed=-25)
time.sleep(5)
robot.drive_wheel_motors(l_wheel_speed=15, r_wheel_speed=-15)
time.sleep(10.5)
if (action == "move1"):
#robot.turn_in_place(degrees(360), speed=degrees(13)).wait_for_completed()
robot.drive_wheel_motors(l_wheel_speed= 15, r_wheel_speed=-15)
time.sleep(15.6)
print("yeszzzz")
action = "move"
elif (action == "move"):
if (timer < 300):
timer += 1
print(timer ,"timer")
else:
timer = 0
action = "move1"
# spline approach
currentPose = pos()
relativeTarget = currentPose.inverse().mult(targetPose)
#print("relativeTarget" + str(relativeTarget))
velocity = target_pose_to_velocity_spline(
relativeTarget) # target pose to velocity is the method for spline driving
#print("velocity" + str(velocity))
trackSpeed = velocity_to_track_speed(velocity[0], velocity[1])
#print("trackSpeedCommand" + str(trackSpeed))
robot.drive_wheel_motors(l_wheel_speed=trackSpeed[0], r_wheel_speed=trackSpeed[1])
#print("currentPose" + str(currentPose))
print()
else:
print("error")
exit(-1)
print(action)
print()
time.sleep(timeInterval)
async def run(robot: cozmo.robot.Robot):
'''The run method runs once the Cozmo SDK is connected.'''
#add annotators for battery level and ball bounding box
robot.world.image_annotator.add_annotator('battery', BatteryAnnotator)
robot.world.image_annotator.add_annotator('ball', BallAnnotator)
try:
looking_around = None
robot.set_lift_height(0)
while True:
#get camera image
event = await robot.world.wait_for(
cozmo.camera.EvtNewRawCameraImage, timeout=30)
#convert camera image to opencv format
opencv_image = cv2.cvtColor(np.asarray(event.image),
cv2.COLOR_RGB2GRAY)
h = opencv_image.shape[0]
w = opencv_image.shape[1]
#find the ball
ball = find_ball.find_ball(opencv_image)
#set annotator ball
BallAnnotator.ball = ball
if np.array_equal(ball, [0, 0, 0]):
if not looking_around:
robot.stop_all_motors()
looking_around = robot.start_behavior(
cozmo.behavior.BehaviorTypes.LookAroundInPlace)
else:
# Stop moving around
if looking_around:
looking_around.stop()
looking_around = None
# If reached the ball (by checking the visible radius), lift it
if ball[2] > 95:
robot.stop_all_motors()
# Drive the final distance in a straight line (we'll lose sight of the ball when it's too close)
await robot.drive_straight(
cozmo.util.distance_mm(30),
cozmo.util.speed_mmps(10)).wait_for_completed()
await robot.set_lift_height(1.0).wait_for_completed()
return
# Move head to center the ball along the y axis
# Ball on the top -> head_speed = 0.5
# Ball on the bottom -> head_speed = -0.5
head_speed = 0.5 - ball[1] / h
print("Ball = {0}, Head speed = {1}".format(ball, head_speed))
robot.move_head(head_speed)
# Drive towards the ball like a Braitenberg vehicle.
# Larger radius -> slower
speed = 300 / ball[2]
# Ball on the left -> diff = -0.5
# Ball on the right -> diff = 0.5
diff = ball[0] / w - 0.5
left_speed = speed * (1 + diff)
right_speed = speed * (1 - diff)
print(
"Speed = {0}, Diff = {1}, Left = {2}, Right = {3}".format(
speed, diff, left_speed, right_speed))
robot.drive_wheel_motors(left_speed, right_speed)
except KeyboardInterrupt:
print("")
print("Exit requested by user")
except cozmo.RobotBusy as e:
print(e)
def cozmo_program(robot: cozmo.robot.Robot):
global execute
global move
global cameraActive
global cameraButton
global currentLiftPos
global currentHeadAng
execute = True
run = True
while run:
# Sets global var move to False when button_2 is released.
button_F.when_released = stopCozmo
button_F.when_held = moveCozmo
button_B.when_released = stopCozmo
button_B.when_held = moveCozmo
button_L.when_released = stopCozmo
button_L.when_held = moveCozmo
button_R.when_released = stopCozmo
button_R.when_held = moveCozmo
# Joystick Controls
if LJoyPos.value not in joystickDZ or RJoyPos.value not in joystickDZ:
while True:
##print("LJoyPos : {} RJoyPos : {}".format(LJoyPos.value,RJoyPos.value))
LTracSpd, RTracSpd = JoyCalc(LJoyPos.value, RJoyPos.value)
if LTracSpd == 0 and RTracSpd == 0:
break
robot.drive_wheel_motors(LTracSpd, RTracSpd)
time.sleep(.05)
robot.stop_all_motors()
# Determines Lift Position
PosLift = liftPosCheck(liftPos.value)
if PosLift != currentLiftPos:
currentLiftPos = PosLift
robot.set_lift_height(currentLiftPos).wait_for_completed()
print("Lift Position : {}".format(currentLiftPos))
# Determines Head Angle
angleHead = headAngCheck(headAng.value)
if angleHead != currentHeadAng:
currentHeadAng = angleHead
robot.set_head_angle(degrees(currentHeadAng)).wait_for_completed()
print("Head Angle : {}".format(currentHeadAng))
# Move Forward
if button_F.is_held == True:
while move:
robot.drive_wheel_motors(75, 75)
robot.stop_all_motors()
# Move Backwards
if button_B.is_held == True:
while move:
robot.drive_wheel_motors(-75, -75)
robot.stop_all_motors()
# Turn Left
if button_L.is_held == True:
while move:
robot.drive_wheel_motors(-75, 75)
robot.stop_all_motors()
# Turn Right
if button_R.is_held == True:
while move:
robot.drive_wheel_motors(75, -75)
robot.stop_all_motors()
# Take a Picture on with Button Press and Activate/Deactivate Camera with a button hold.
if button_1.is_pressed == True:
ctr = 0
# Condition to seperate a button press vs button held.
while button_1.is_active:
if button_1.is_held == True and ctr == 0:
tempHumChk(robot)
ctr = ctr + 1
if ctr == 0:
distanceChk(robot)
# Take a picture when button is held down and performs when button is Pressed.
if button_2.is_pressed == True:
ctrCam = 0
while button_2.is_active:
if button_2.is_held == True and ctrCam == 0:
ImageCapture(robot)
ctrCam = ctrCam + 1
if ctrCam == 0:
robot.play_anim_trigger(
cozmo.anim.Triggers.CodeLabDog).wait_for_completed()
if button_3.is_pressed == True:
ctrExit = 0
while button_3.is_active:
if button_3.is_held and ctrExit == 0:
print("\nSystem Shutting Down\n")
GPIO.cleanup()
sys.exit(0)
if ctrExit == 0:
robot.drive_wheel_motors(75, -75)
robot.say_text("You spin me right round baby right round.",
in_parallel=True).wait_for_completed()
robot.stop_all_motors()
def drive_right(robot: cozmo.robot.Robot):
""" Drive to right
"""
log.info('Drive right...')
robot.drive_wheel_motors(speed, speed - 20)
def drive_left(robot: cozmo.robot.Robot):
""" Drive to left
"""
log.info('Drive left...')
robot.drive_wheel_motors(speed - 15, speed)
def drive(robot: cozmo.robot.Robot):
""" Start driving
"""
log.info('Drive...')
robot.drive_wheel_motors(speed, speed)
async def CozmoPID(robot: cozmo.robot.Robot):
global stopevent
with open("./config.json") as file:
config = json.load(file)
kp = config["kp"]
ki = config["ki"]
kd = config["kd"]
###############################
# PLEASE ENTER YOUR CODE BELOW
await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
# find cube
cube_position_x = 0
cube_position_y = 0
time.sleep(1)
for obj in robot.world.visible_objects:
cube_position_x = obj.pose.position.x
cube_position_y = obj.pose.position.y
break
# print(robot.pose.position.y)
# print("X:{0} Y:{1}".format(cube_position_x, cube_position_y))
# PID
prev_x = 0
prev_y = 0
y_error_sum = 0
previousTime = time.time()
iteration_count = 0
reference_x = 130
reference_y = 0
while True:
if iteration_count > 1:
time_elapsed = time.time() - previousTime
previousTime = time.time()
else:
time_elapsed = 1
x_distance_from_cube = (cube_position_x - robot.pose.position.x)
# print("\nCube Position: {0}\nRobot Pose: {1}\nX Distance: {2}\n".format(
# cube_position_x, robot.pose.position.x, x_distance_from_cube))
y_distance_from_cube = (cube_position_y - robot.pose.position.y)
# if cube_position_x == 0: # Too close to the cube
# reference_x = 20
error_x = x_distance_from_cube - reference_x
error_y = y_distance_from_cube - reference_y
y_error_sum += error_y
px = kp * error_x
ix = ki * 0
dx = kd * (error_x - prev_x) / time_elapsed
pidx = px + ix + dx
py = kp * error_y
iy = ki * y_error_sum
dy = kd * (error_y - prev_y) / time_elapsed
pidy = py + iy + dy
prev_x = error_x
prev_y = error_y
# print("ITERATION #{5}\nX PID: {0}\nY PID: {6}\n\nX from Cube: {1}\nY from Cube: {2}\n\nX Error: {3}\nY error: {4}\n\nTime Elapsed: {7}\nPx: {8}\nPy: {9}\nDx: {10}\nDy: {11}\n\n".format(
# str(pidx), str(x_distance_from_cube), str(y_distance_from_cube), str(error_x), str(error_y), str(iteration_count), str(pidy), str(time_elapsed), str(px), str(py), str(dx), str(dy)))
robot.drive_wheel_motors(pidx, pidx)
iteration_count += 1
await robot.wait_for(cozmo.robot.EvtRobotStateUpdated)
def cube_com(robot: cozmo.robot.Robot):
# Initialize light cubes
cube = [None, None, None]
cube[0] = robot.world.get_light_cube(LightCube1Id) # FWD cube
cube[1] = robot.world.get_light_cube(LightCube2Id) # LEFT cube
cube[2] = robot.world.get_light_cube(LightCube3Id) # RIGHT cube
if cube[0] is not None:
cube[0].set_lights(cozmo.lights.red_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")
if cube[1] is not None:
cube[1].set_lights(cozmo.lights.green_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.")
if cube[2] is not None:
cube[2].set_lights(cozmo.lights.blue_light)
else:
cozmo.logger.warning("Cozmo is not connected to a LightCube3Id cube - check the battery.")
# Robot setup
timer = 0
timestep = 0.01
cube_side = 0 # Ranges 0-3 for cube lights
state = "FWD" # State variable
last_com = "FWD" # Event handler to update next state
last_com_time = 0 # Last event received
tap_tracker = [0, 0, 0] # Last time cube was tapped
tap_com = ["FWD", "LEFT", "RIGHT"] # List of states
# Robot loop
while True:
# Update cube tap times
for n in range(3):
if cube[n].last_tapped_time is not None:
tap_tracker[n] = cube[n].last_tapped_time
# Find most recent cube tap
last_tap = max(tap_tracker)
if last_tap > last_com_time:
last_com_time = last_tap
last_com = tap_com[tap_tracker.index(last_tap)]
# State Machine
timer += timestep
if timer >= 0.25:
timer = 0
cube_side = (cube_side + 1) % 4
# Forward State
if state == "FWD" and last_com == "LEFT":
state = "LEFT"
cube[0].set_lights(cozmo.lights.red_light)
elif state == "FWD" and last_com == "RIGHT":
state = "RIGHT"
cube[0].set_lights(cozmo.lights.red_light)
elif state == "FWD":
robot.drive_wheel_motors(50, 50, 200, 200)
rotate_lights(cube[0], cube_side)
# Left State
elif state == "LEFT" and last_com == "FWD":
state = "FWD"
cube[1].set_lights(cozmo.lights.green_light)
elif state == "LEFT" and last_com == "RIGHT":
state = "RIGHT"
cube[1].set_lights(cozmo.lights.green_light)
elif state == "LEFT":
robot.drive_wheel_motors(-50, 50, 200, 200)
rotate_lights(cube[1], cube_side)
# Right State
elif state == "RIGHT" and last_com == "FWD":
state = "FWD"
cube[2].set_lights(cozmo.lights.blue_light)
elif state == "RIGHT" and last_com == "LEFT":
state = "LEFT"
cube[2].set_lights(cozmo.lights.blue_light)
elif state == "RIGHT":
robot.drive_wheel_motors(50, -50, 200, 200)
rotate_lights(cube[2], cube_side)
time.sleep(timestep)
def cozmo_program(robot: cozmo.robot.Robot):
global angle
angle = 25.
robot.set_head_angle(degrees(angle)).wait_for_completed()
robot.set_lift_height(0.0).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = True
robot.camera.enable_auto_exposure = True
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'
frame = os.path.join(directory, "current.jpeg")
print("Starting Tensorflow...")
with tf.Session() as sess:
print("Session successfully started")
model = load_model('modelv1.07-0.96.hdf5')
while True:
global X, Y, W, H
global result
X = 245.
Y = 165.
W = 150.
H = 150.
gt = [X, Y, W, H]
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
frame = os.path.join(directory, "current.jpeg")
result = 0
dog_counter = 0
cat_counter = 0
background_counter = 0
next_state = 0
current_state = 0 #Background: 0, Cat:1, Dog:2
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
show_frame(np.asarray(Image.open(frame)), [900.,900.,900.,900.], 1)
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
next_state = np.argmax(result)
print('Arg max: ',next_state)
# Initial Current State is Background
if current_state == 0:
print('Background')
if next_state == 1: # Detected a Cat
current_state = 1 # Transition to Cat State
background_counter = 0
cat_counter = 1
dog_counter = 0
elif next_state == 2: # Detected a Dog
current_state = 2 # Transition to Dog state
background_counter = 0
cat_counter = 0
dog_counter = 1
# Current State is Cat
elif current_state == 1:
print('\t\t\t\t\t\tCat')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
cat_counter = 0
elif next_state == 1: # Detected Cat itself
cat_counter +=1
if cat_counter >= 30:
print('Cozmo sees a cat')
dense = model.get_layer('dense').get_weights()
weights = dense[0].T
testing_counter = 0
detected_centroid = 0
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
frame_average = 2
frame_count = 0
while True:
latest_img = robot.world.latest_image
if latest_img is not None:
pilImage = latest_img.raw_image
pilImage.resize((640,480), Image.ANTIALIAS).save(os.path.join(directory, "current.jpeg"), "JPEG")
img = load_image(frame)
[result,out_relu,global_average_pooling2d] = sess.run([model.outputs,model.get_layer('out_relu').output\
,model.get_layer('global_average_pooling2d').output ], feed_dict={model.input.name:img})
kernels = out_relu.reshape(7,7,1280)
final = np.dot(kernels,weights[result[0].argmax()])
final1 = array_to_img(final.reshape(7,7,1))
final1 = final1.resize((224,224), Image.ANTIALIAS)
box = img_to_array(final1).reshape(224,224)
#box = cv2.blur(box,(30,30))
temp = (box > box.max()*.8) *1
temp_adjusted = np.ndarray(shape=np.shape(temp), dtype=np.dtype(np.uint8))
temp_adjusted[:,:] = np.asarray(temp)*255
contours, hierarchy = cv2.findContours(temp_adjusted, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
contours = np.array(contours)
max_area = [0,0] # contours index and area
for index, contour in enumerate(contours):
if(max_area[1]< len(contour)):
max_area = [index,len(contour)]
contours_adjusted = contours[max_area[0]].squeeze(axis=1).T
xmin = contours_adjusted[0].min() * (640./224.)
ymin = contours_adjusted[1].min() * (480./224.)
xmax = contours_adjusted[0].max() * (640./224.)
ymax = contours_adjusted[1].max() * (480./224.)
if result[0].argmax() == 1:
# Frame smoothing
frame_count = frame_count + 1
xmin_avg = xmin_avg + xmin
xmax_avg = xmax_avg + xmax
ymin_avg = ymin_avg + ymin
ymax_avg = ymax_avg + ymax
if frame_count % frame_average == 0:
frame_count = 0
xmin_avg = xmin_avg/frame_average
xmax_avg = xmax_avg/frame_average
ymin_avg = ymin_avg/frame_average
ymax_avg = ymax_avg/frame_average
print(xmin_avg, end=",")
print(ymin_avg, end=",")
print(xmax_avg, end=",")
print(ymax_avg, end="\n")
ymin_avg = ymin_avg + (ymax_avg - ymin_avg)/2. - H/2.
xmin_avg = xmin_avg + (xmax_avg - xmin_avg)/2. - W/2.
print("150: ",xmin_avg, end=",")
print("150: ",ymin_avg, end="\n")
gt = [xmin_avg, ymin_avg, W, H]
xmin_avg = 0
xmax_avg = 0
ymin_avg = 0
ymax_avg = 0
pos_x, pos_y, target_w, target_h = region_to_bbox(gt)
bboxes = np.zeros((1, 4))
#bboxes[0,:] = pos_x-target_w/2, pos_y-target_h/2, target_w, target_h
bboxes[0,:] = pos_x-W/2, pos_y-H/2, W, H
print(len(contours))
testing_counter = testing_counter + 1
print("Testing_counter: ",testing_counter)
show_frame(np.asarray(Image.open(frame)), gt, 1)
print("Cat is detected")
print("Starting the tracker ...")
if (bboxes[0,1] + bboxes[0,3]/2) < (Y + H/2 - 40):
print("Command: Raise the head")
angle = angle + 0.5
if angle > 44.5:
angle = 44.5
elif (bboxes[0,1] + bboxes[0,3]/2) > (Y + H/2 + 40):
print("Command: Lower the head")
angle = angle - 0.5
if angle < 0:
angle = 0
else:
pass
set_head_angle_action = robot.set_head_angle(degrees(angle), max_speed=20, in_parallel=True)
if straight(bboxes[0,:])[0] != 0 and turn(bboxes[0,:])[0] != 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0] + turn(bboxes[0,:])[0], straight(bboxes[0,:])[1] + turn(bboxes[0,:])[1])
detected_centroid = 0
elif straight(bboxes[0,:])[0] == 0 and turn(bboxes[0,:])[0] == 0:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
elif straight(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(turn(bboxes[0,:])[0], turn(bboxes[0,:])[1])
detected_centroid = 0
elif turn(bboxes[0,:])[0] == 0:
robot.drive_wheel_motors(straight(bboxes[0,:])[0], straight(bboxes[0,:])[1])
detected_centroid = 0
else:
robot.stop_all_motors()
detected_centroid = detected_centroid + 1
if detected_centroid > 20//frame_average:
detected_centroid = 0
print("Reached a stable state.........\t\t\t\t\t\t\t\t STABLE")
# Go near the object
set_head_angle_action.wait_for_completed()
robot.abort_all_actions(log_abort_messages=True)
robot.wait_for_all_actions_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
target_frame_count = 1
while True:
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame1 = latest_img.raw_image
target_frame1 = target_frame1.resize((640,480), Image.ANTIALIAS)
#target_frame1 = target_frame1.convert('L')
target_frame1 = np.asarray(target_frame1)
#orb1 = cv2.ORB_create(500)
#kp1 = orb1.detect(target_frame1,None)
#kp1, des1 = orb1.compute(target_frame1, kp1)
#features_img1 = cv2.drawKeypoints(target_frame1, kp1, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",features_img1)
plt.imsave("target_frame1_"+str(target_frame_count)+".jpeg",target_frame1)
drive_straight_action = robot.drive_straight(distance=cozmo.util.distance_mm(distance_mm=10),speed=cozmo.util.speed_mmps(10), in_parallel=True)
drive_straight_action.wait_for_completed()
robot.set_head_angle(degrees(0.5)).wait_for_completed()
print("Robot's head angle: ",robot.head_angle)
latest_img = None
while latest_img is None:
latest_img = robot.world.latest_image
target_frame2 = latest_img.raw_image
target_frame2 = target_frame2.resize((640,480), Image.ANTIALIAS)
#target_frame2 = target_frame2.convert('L')
target_frame2 = np.asarray(target_frame2)
#orb2 = cv2.ORB_create(500)
#kp2 = orb2.detect(target_frame2,None)
#kp2, des2 = orb2.compute(target_frame2, kp2)
#features_img2 = cv2.drawKeypoints(target_frame2, kp2, None, color=(255,0,0), flags=0)
#plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",features_img2)
plt.imsave("target_frame2_"+str(target_frame_count)+".jpeg",target_frame2)
target_frame_count = target_frame_count + 1
'''
matcher = cv2.DescriptorMatcher_create(cv2.DESCRIPTOR_MATCHER_BRUTEFORCE_HAMMING)
matches = matcher.match(des1, des2, None)
matches.sort(key=lambda x: x.distance, reverse=False)
matches = matches[:10]
imMatches = cv2.drawMatches(target_frame1, kp1, target_frame2, kp2, matches, None)
cv2.imwrite("matches_tf1_tf2.jpg", imMatches)
points1 = np.zeros((len(matches), 2), dtype=np.float32)
points2 = np.zeros((len(matches), 2), dtype=np.float32)
for i, match in enumerate(matches):
points1[i, :] = kp1[match.queryIdx].pt
points2[i, :] = kp2[match.trainIdx].pt
print("Points1 [{}]: {}".format(i,points1[i][0]), points1[i][1],"\tPoints2: ",points2[i][0], points2[i][1])
index = None
dist1_x = []
dist2_x = []
for index in range(len(points1)):
dist1_x.append((W/2.)-points1[index][0]) # Extract only the x-coordinate
dist2_x.append((W/2.)-points2[index][0]) # Extract only the x-coordinate
fw_x = 1./((1./np.array(dist2_x)) - (1./np.array(dist1_x))) # Calculate the image plane to obj plane mapping in x direction
pt1_x = []
pt2_x = []
for index in range(len(points1)):
pt1_x.append(fw_x[index]/(W/2. - points1[index][0]))
pt2_x.append(fw_x[index]/(W/2. - points2[index][0]))
print("Approx. distance[{}]: {}".format(index, pt1_x[index]))
if len(pt2_x) < 10:
break
'''
sys.exit(0)
else: # Detected Dog
dog_counter += 1
if dog_counter >= 6: # Transition to Dog only if Dog appeared for more than 6 times
cat_counter = 0
current_state = 2
# Current State is Dog
elif current_state == 2:
print('\t\t\t\t\t\t\t\t\t\t\t\tDog')
if next_state == 0: # Detected Background
background_counter += 1
if background_counter >= 6: # Transition to Background only if Background appeared for more than 6 times
background_counter = 0
current_state = 0
dog_counter = 0
elif next_state == 2: # Detected Dog itself
dog_counter +=1
if dog_counter >= 30:
print('Cozmo sees a Dog')
robot.drive_wheels(-50, -50)
time.sleep(3)
robot.drive_wheels(70, -70)
time.sleep(2.8)
robot.drive_wheels(0, 0)
break
else: # Detected Cat
cat_counter += 1
if cat_counter >= 6: # Transition to Cat only if Cat appeared for more than 6 times
dog_counter = 0
current_state = 1
def run2(img_cols, img_rows, prev_label, model, robot: cozmo.robot.Robot):
"""
まっすぐ, 右の緩急, 左の緩急, バックの６パターン
TODO: 右に行くときの認識が甘く、まっすぐと誤認識している。要改善。
TODO: 学習画像の枚数が不揃いなので、揃える。
TODO: Cozmoの画像上に進む方向をオーバーレイで表示する。
TODO: 何もないところをバックと認識することがある。
"""
for _ in range(100000):
start = time.time()
new_image = robot.world.wait_for(cozmo.world.EvtNewCameraImage)
new_image.image.raw_image.save('image.png')
image = cv2.imread('image.png', cv2.IMREAD_COLOR)
image = get_binary_image(image)
if K.image_data_format() == 'channels_first':
image = image.reshape(1, 3, img_rows, img_cols)
else:
image = image.reshape(1, img_rows, img_cols, 3)
pred = model.predict(image, batch_size=1, verbose=0)
pred_label = np.argmax(pred)
prev_label = np.roll(prev_label, 1)
prev_label[0] = pred_label
print(prev_label, end=", ")
# 緩やかに右に動く
if pred_label == 1:
print('rightゆっくり→')
robot.drive_wheel_motors(25, 10)
# 緩やかに左に動く
elif pred_label == 0:
print('←leftゆっくり')
robot.drive_wheel_motors(10, 25)
# 急に左に動く
elif pred_label == 4:
print('←leftきゅうカーブ')
robot.drive_wheel_motors(5, 25)
# 急に右に動く
elif pred_label == 5:
print('rightきゅうカーブ→')
robot.drive_wheel_motors(25, 5)
# まっすぐ進む
elif pred_label == 3:
print('まっすぐ')
robot.drive_wheel_motors(25, 25)
# 戻ってやり直し
elif pred_label == 2:
print("------------------")
robot.drive_wheel_motors(0, 0)
time.sleep(3)
robot.drive_wheel_motors(-10, -10)
time.sleep(5)
robot.drive_wheel_motors(0, 0)
time.sleep(3)
count0 = count1 = 0
for i in range(30):
if prev_label[i] == 0 or prev_label[i] == 4:
count0 += 1
elif prev_label[i] == 1 or prev_label[i] == 5:
count1 += 1
print(prev_label)
print("Loop1 → count0: " + str(count0) + ", " + "count1: " + str(count1))
if count1 < count0:
print("左に行きます！")
robot.drive_wheel_motors(5, 25)
elif count0 < count1:
print("右に行きます！")
robot.drive_wheel_motors(25, 5)
else:
count0 = count1 = 0
for j in range(29):
if prev_label[j] == 0 or prev_label[j] == 4:
count0 += 1
elif prev_label[j] == 1 or prev_label[j] == 5:
count1 += 1
print("Loop2 → count0: " + str(count0) + ", " + "count1: " + str(count1))
if count1 < count0:
print("左に行きます！")
robot.drive_wheel_motors(5, 25)
elif count0 < count1:
print("右に行きます！")
robot.drive_wheel_motors(25, 5)
else:
print("どれでもない")
time.sleep(2)
print(time.time() - start)
