def __init__(self, nao_motion, nao_video, nao_tts, wait_disc_func, ppA=0.05, cA=0.005, rA=0.8, min_detections=3,
dist=-1, sloped=True, nao_strategy=Basic, other_strategy=NAOVision):
"""
:param nao_motion: an instance of the motion controller of NAO
:type nao_motion: nao.controller.motion.MotionController
:param nao_video: an instance of the video controller of NAO
:type nao_video: nao.controller.video.VideoController
:param nao_tts: an instance of the TTS proxy of NAO
:type nao_tts: naoqi.ALProxy
:param ppA: the perfect position accuracy in meters. While the robot is not located to the perfect
position, with a sharper accuracy than ppA, the robot continues to move
:param cA: the coordinates accuracy in meters. While the robot's hand has not reached this
accuracy, the robot will continue to move to get its hand to a better place.
:param rA: the rotation accuracy in radians. While the robot's hand is not inclined with this accuracy
compared to the perfect position, the robot will continue to move.
:param min_detections: the minimum number of success before a detection is considered as successful
:param dist: the distance in meters between NAO and the game board, -1 = unknown
:param sloped: True if the game board is sloped from NAO
:param nao_strategy: the class that defines NAO's strategy
:param other_strategy: the class that defines the other player's strategy
"""
self.rA = rA
self.cA = cA
self.ppA = ppA
self.estimated_distance = dist
self.min_detections = min_detections
self.sloped = sloped
self.wait_disc_func = wait_disc_func
# Setting NAO's controllers
self.tts = nao_tts
self.nao_motion = nao_motion
self.nao_video = nao_video
self.camera_subscribed = [False, False]
self.current_yaw = self.nao_motion.DEFAULT_HEAD_YAW
self.current_pitch = self.nao_motion.DEFAULT_HEAD_PITCH
self.yaw_sign = 1
self.pitch_sign = -1
# Connect 4 detectors and models
self.c4_model = DefaultModel()
self.c4_tracker = Connect4Tracker(self.c4_model)
self.c4_handler = Connect4Handler(self.getNaoImage)
self.c4_coords = [0, 0, 0, 0, 0, 0]
# Creating the strategies that will play the game
self.strategy = nao_strategy()
if other_strategy is NAOVision:
self.other_strategy = NAOVision(self.c4_model.image_of_reference.pixel_mapping,
self.c4_handler.front_hole_detector.getPerspective)
else:
self.other_strategy = other_strategy()
# Creating the game and registering the players
self.game = Game()
self.NAO_player = self.game.registerPlayer(self.strategy)
self.human_player = self.game.registerPlayer(self.other_strategy)
print "Lancement du jeu, la couleur de NAO est le {0}".format(disc.color_string(self.NAO_player.color))
next_player = "NAO"
if not self.game.checkPlayerTurn(self.NAO_player):
next_player = "Joueur humain"
print "Le premier joueur est : {0}".format(next_player)
def game(args):
strategies = {'basic': Basic,
'human': Human}
player1 = strategies.get(args['--player1'], None)
player2 = strategies.get(args['--player2'], None)
if player1 is None:
exit("{0} is not a valid strategy. The valid strategies are: basic, human".format(args['--player1']))
if player2 is None:
exit("{0} is not a valid strategy. The valid strategies are: basic, human".format(args['--player2']))
print "A new game is created."
print
print "-1 = Empty, 0 = Red, 1 = Green"
print
new_game = Game()
basic.ALPHA_BETA_MAX_DEPTH = int(args['--max-depth'])
new_game.registerPlayer(player1())
color_int = new_game.players[0].color
print "Player 1: {0} with color {1} ({2})".format(player1.__name__, disc.color_string(color_int), color_int)
new_game.registerPlayer(player2())
color_int = new_game.players[1].color
print "Player 2: {0} with color {1} ({2})".format(player2.__name__, disc.color_string(color_int), color_int)
print
print
new_game.playLoop()
class LogicalLoop(object):
"""
Class that holds the main loop of the project.
"""
def __init__(self, nao_motion, nao_video, nao_tts, wait_disc_func, ppA=0.05, cA=0.005, rA=0.8, min_detections=3,
dist=-1, sloped=True, nao_strategy=Basic, other_strategy=NAOVision):
"""
:param nao_motion: an instance of the motion controller of NAO
:type nao_motion: nao.controller.motion.MotionController
:param nao_video: an instance of the video controller of NAO
:type nao_video: nao.controller.video.VideoController
:param nao_tts: an instance of the TTS proxy of NAO
:type nao_tts: naoqi.ALProxy
:param ppA: the perfect position accuracy in meters. While the robot is not located to the perfect
position, with a sharper accuracy than ppA, the robot continues to move
:param cA: the coordinates accuracy in meters. While the robot's hand has not reached this
accuracy, the robot will continue to move to get its hand to a better place.
:param rA: the rotation accuracy in radians. While the robot's hand is not inclined with this accuracy
compared to the perfect position, the robot will continue to move.
:param min_detections: the minimum number of success before a detection is considered as successful
:param dist: the distance in meters between NAO and the game board, -1 = unknown
:param sloped: True if the game board is sloped from NAO
:param nao_strategy: the class that defines NAO's strategy
:param other_strategy: the class that defines the other player's strategy
"""
self.rA = rA
self.cA = cA
self.ppA = ppA
self.estimated_distance = dist
self.min_detections = min_detections
self.sloped = sloped
self.wait_disc_func = wait_disc_func
# Setting NAO's controllers
self.tts = nao_tts
self.nao_motion = nao_motion
self.nao_video = nao_video
self.camera_subscribed = [False, False]
self.current_yaw = self.nao_motion.DEFAULT_HEAD_YAW
self.current_pitch = self.nao_motion.DEFAULT_HEAD_PITCH
self.yaw_sign = 1
self.pitch_sign = -1
# Connect 4 detectors and models
self.c4_model = DefaultModel()
self.c4_tracker = Connect4Tracker(self.c4_model)
self.c4_handler = Connect4Handler(self.getNaoImage)
self.c4_coords = [0, 0, 0, 0, 0, 0]
# Creating the strategies that will play the game
self.strategy = nao_strategy()
if other_strategy is NAOVision:
self.other_strategy = NAOVision(self.c4_model.image_of_reference.pixel_mapping,
self.c4_handler.front_hole_detector.getPerspective)
else:
self.other_strategy = other_strategy()
# Creating the game and registering the players
self.game = Game()
self.NAO_player = self.game.registerPlayer(self.strategy)
self.human_player = self.game.registerPlayer(self.other_strategy)
print "Lancement du jeu, la couleur de NAO est le {0}".format(disc.color_string(self.NAO_player.color))
next_player = "NAO"
if not self.game.checkPlayerTurn(self.NAO_player):
next_player = "Joueur humain"
print "Le premier joueur est : {0}".format(next_player)
def getNaoImage(self, camera_num=0, res=1):
"""
:param res: The resolution parameter
:param camera_num: 0 : TopCamera, 1 : BottomCamera
:type camera_num: int
:return: An OpenCV readable image that comes from NAO's camera
"""
if not self.camera_subscribed[camera_num]:
ret = self.nao_video.connectToCamera(res=camera_num + 1, fps=30, camera_num=camera_num,
subscriber_id="C4N_Loop" + str(camera_num))
self.camera_subscribed[camera_num] = True
if ret < 0:
print "Could not open camera"
return None
return self.nao_video.getImageFromCamera(camera_num=camera_num)
def findGameBoard(self):
"""
"""
i = 0
distances = [self.estimated_distance]
if self.estimated_distance == -1:
distances = [0.5, 1.0, 1.3, 1.75, 2.2, 2.5, 3.0]
while True:
for dist in distances:
try:
self.nao_motion.lookAtGameBoard(dist)
coords = self.c4_handler \
.getUpperHoleCoordinatesUsingFrontHoles(dist, self.sloped, 3,
self.nao_motion.getCameraTopPositionFromTorso(),
nao.data.CAM_MATRIX, nao.data.CAM_DISTORSION,
debug=True, tries=self.min_detections)
coords[0] += 0.25 # Fix calibration error
self.estimated_distance = coords[0]
self.c4_coords = coords
return 0
except FrontHolesGridNotFoundException:
i += 1
if i > 5:
i = 0
self.tts.say("Je ne trouve pas le Puissance 4...")
self.nao_motion.moveAt(0, 0, 0.8)
continue
def inverseKinematicsConvergence(self, hole_index):
"""
:param hole_index: the number of the hole above which we want to move NAO's hand
:return:
"""
self.nao_motion.moveHead(self.nao_motion.DEFAULT_HEAD_PITCH, self.nao_motion.DEFAULT_HEAD_YAW, True)
max_tries = 4 # If we don't see any marker after 2 tries, we move NAO's head
i = 0
stable = False
while not stable:
while i < max_tries:
try:
hole_coord = self.c4_handler \
.getUpperHoleCoordinatesUsingMarkers(hole_index,
self.nao_motion.getCameraBottomPositionFromTorso(),
data.CAM_MATRIX, data.CAM_DISTORSION,
tries=self.min_detections)
self.resetHead()
if abs(hole_coord[5] + 0.505) > self.rA: # If the board is sloped from NAO, we need to rotate NAO
self.nao_motion.moveAt(0, 0, (hole_coord[5] + 0.505)/3)
continue
dist_from_optimal = geom.vectorize((0.161, 0.113), (hole_coord[0], hole_coord[1]))
if abs(dist_from_optimal[0]) > self.ppA or abs(dist_from_optimal[1]) > 2 * self.ppA:
self.nao_motion.moveAt(dist_from_optimal[0], dist_from_optimal[1], hole_coord[5] + 0.505)
continue
self.estimated_distance = hole_coord[0]
i = 0
self.nao_motion.setLeftHandPosition(hole_coord, mask=63)
diff = self.nao_motion.compareToLeftHandPosition(hole_coord)
if abs(diff[0]) < self.cA and abs(diff[1]) < 2 * self.cA:
stable = True
self.nao_motion.playDisc(hole_coord)
break
else:
self.nao_motion.setLeftArmRaised()
self.nao_motion.moveAt(diff[0], diff[1], hole_coord[5] + 0.505)
i += 1
except NotEnoughLandmarksException:
i += 1
if not stable:
# self.tts.say("Je ne trouve pas les marqueurs dans mon champ de vision")
self.moveHeadToNextPosition()
time.sleep(0.500)
i = 0
def loop(self):
self.findGameBoard()
self.walkTowardConnect4(analysis=not self.game.checkPlayerTurn(self.NAO_player))
finished = 0
while not finished:
if not self.game.checkPlayerTurn(self.NAO_player):
finished = self.analyseGameState()
if not finished:
self.playingRoutine()
def playingRoutine(self):
action = self.strategy.chooseNextAction(self.game.game_state)
self.game.makeMove(action)
if self.estimated_distance > 0.3:
self.walkTowardConnect4()
self.wait_disc_func()
self.inverseKinematicsConvergence(action)
if type(self.other_strategy) is NAOVision:
self.walkBack()
def walkBack(self):
"""
Move NAO back so it can see the game board entirely
"""
self.nao_motion.moveAt(-0.50, 0, 0)
self.estimated_distance = 0.50
self.nao_motion.crouch()
def walkTowardConnect4(self, analysis=False):
"""
Move NAO to the game board
"""
self.nao_motion.motion_proxy.wakeUp()
next_dist = 0.25
if analysis:
next_dist = 0.5
self.nao_motion.moveAt(self.c4_coords[0] - next_dist, self.c4_coords[1], 0)
self.estimated_distance = next_dist
# self.nao_motion.moveAt(coords[0], coords[1], coords[5])
def analyseGameState(self):
self.nao_motion.crouch()
played = False
i = 0
j = 0
unstable_state = 0
self.nao_motion.lookAtGameBoard(self.estimated_distance)
while not played:
try:
if type(self.strategy) is NAOVision:
self.c4_handler.detectFrontHoles(self.estimated_distance, False)
action = self.other_strategy.chooseNextAction(self.game.game_state)
self.game.makeMove(action)
print self.game.game_state.board
if self.NAO_player.won:
self.tts.say("Je gagne !")
return 1
elif self.human_player.won:
self.tts.say("Tu gagnes !")
return 1
elif self.game.draw:
self.tts.say("Match nul!")
return 1
return 0
except ActionNotYetPerformedException:
i += 1
print "Pas d'action jouee..."
except TooManyDifferencesException:
self.tts.say("C'est de la triche, je gagne dans ce cas...")
self.NAO_player.win()
return 1
except FrontHolesGridNotFoundException:
j += 1
if j > 5:
j = 0
self.tts.say("Je n'arrive pas a voir le plateau de jeu...")
except InvalidStateException:
unstable_state += 1
if unstable_state < 3:
self.tts.say("Je dois avoir un probleme de vue... Laisse moi encore essayer un moment")
else:
self.tts.say("Je n'arrive vraiment pas a voir le pion que tu as ajouter. " +
"Veux-tu bien l'indiquer sur l'ordinateur ?")
action = None
while type(action) is not int:
action = input("Derniere colonne jouee")
self.game.makeMove(action)
time.sleep(4)
if i > 10:
i = 0
self.tts.say("Je t'attends pour jouer")
def taunt(self):
pass
# TODO : optionnal, robot interaction with human
def moveHeadToNextPosition(self):
"""
Move NAO's head to the next position of the spiral
"""
if self.pitch_sign == -1:
if self.yaw_sign == 1:
self.yaw_sign = -1
self.current_yaw += HEAD_STEP
else:
if self.yaw_sign == -1:
self.pitch_sign = 1
self.current_pitch += HEAD_STEP
elif self.yaw_sign == -1:
self.yaw_sign = 1
self.current_yaw += HEAD_STEP
else:
self.pitch_sign = -1
self.current_pitch += HEAD_STEP
if self.current_yaw > 40 or self.current_pitch > 40:
self.nao_motion.releaseHead()
self.tts.say("Veuillez placer ma tete au bon endroit s'il vous plait")
time.sleep(5)
else:
self.nao_motion.moveHead(self.current_pitch * self.pitch_sign,
self.current_yaw * self.yaw_sign,
radians=False)
def resetHead(self):
"""
Set the head to the default position
"""
self.nao_motion.moveHead(self.nao_motion.DEFAULT_HEAD_PITCH, self.nao_motion.DEFAULT_HEAD_YAW, True)
self.current_pitch = self.nao_motion.DEFAULT_HEAD_PITCH
self.current_yaw = self.nao_motion.DEFAULT_HEAD_YAW
self.pitch_sign = -1
self.yaw_sign = 1