def markers(args, must_print=True):
global nao_motion
data.IP = args['--ip']
data.PORT = int(args['--port'])
next_img_func = get_nao_image
try:
if args['--no-robot']:
next_img_func = get_webcam_image
except KeyError:
pass
end_reached = False
nao_motion = MotionController()
nao_motion.stand()
while not end_reached:
img = next_img_func(int(args['--cam-no']), res=2)
detected_markers = detect_markers(img)
for m in detected_markers:
m.draw_contour(img)
cv2.putText(img, str(m.id), tuple(int(p) for p in m.center),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 3)
if must_print:
print "ID: ", str(m.id), "\n", "Contours: ", str(m.contours.ravel().reshape(-1, 2).tolist())
print
else:
end_reached = True
print
if not args['--no-image']:
cv2.imshow('Markers', img)
if cv2.waitKey(1) == 27:
print "Esc pressed : exit"
close_camera()
return 0
def board(args):
global nao_motion
data.IP = args['--ip']
data.PORT = int(args['--port'])
next_img_func = get_nao_image
if args['--no-robot']:
next_img_func = get_webcam_image
myc4 = Connect4Handler(next_img_func, cam_no=int(args['--cam-no']))
dist = float(args['--dist'])
sloped = args['--sloped']
tries = int(args['--min-detections'])
nao_motion = MotionController()
nao_motion.lookAtGameBoard(dist)
while True:
try:
myc4.detectFrontHoles(dist, sloped, tries=tries)
cv2.imshow("Connect 4 Perspective", myc4.front_hole_detector.getPerspective())
except FrontHolesGridNotFoundException:
pass
img2 = draw_circles(myc4.img, myc4.circles)
cv2.imshow("Circles detected", img2)
cv2.imshow("Original picture", myc4.img)
if cv2.waitKey(10) == 27:
print "Esc pressed : exit"
close_camera()
break
return 0
def coordinates(args):
data.IP = args['--ip']
data.PORT = int(args['--port'])
global nao_motion
nao_motion = MotionController()
camera_position_func = nao_motion.getCameraBottomPositionFromTorso
if int(args['--cam-no']) == 0:
camera_position_func = nao_motion.getCameraTopPositionFromTorso
myc4 = Connect4Handler(get_nao_image, cam_no=int(args['--cam-no']))
tries = int(args['--min-detections'])
if args['--board']:
dist = float(args['--dist'])
sloped = args['--sloped']
while True:
try:
coords = myc4.getUpperHoleCoordinatesUsingFrontHoles(dist, sloped, int(args['--hole']),
camera_position_func(),
camera_matrix=data.CAM_MATRIX,
camera_dist=data.CAM_DISTORSION,
tries=tries, debug=not(args['--no-image']))
# We want the coordinates of the center of the hole, not the hand ideal position
coords[2] -= 0.12
coords[5] -= 0.505
coords[1] -= 0.028
coords[0] += 0.25
print coords
except FrontHolesGridNotFoundException:
print
if not(args['--no-image']):
if cv2.waitKey(50) == 27:
print "Esc pressed : exit"
close_camera()
return 0
else:
nao_motion.stand()
while True:
try:
coords = myc4.getUpperHoleCoordinatesUsingMarkers(int(args['--hole']),
camera_position_func(),
data.CAM_MATRIX, data.CAM_DISTORSION,
res=640,
debug=False)
if not args['--no-image']:
markers(args, must_print=False)
# We want the coordinates of the center of the hole, not the hand ideal position
coords[2] -= 0.12
coords[5] -= 0.505
coords[1] -= 0.028
coords[0] += 0.01
print coords
sleep(0.5)
except NotEnoughLandmarksException:
pass
if cv2.waitKey(50) == 27:
print "Esc pressed : exit"
close_camera()
return 0
return 0
def __init__(self, robot_ip=nao.IP, robot_port=nao.PORT):
"""
:param robot_ip: The IP address of the robot
:type robot_ip: str
:param robot_port: The port of the robot
:type robot_port: int
Creates a new learning module for NAO
"""
self.nao_motion = MotionController(robot_ip=robot_ip, robot_port=robot_port)
self.nao_video = VideoController(robot_ip=robot_ip, robot_port=robot_port)
self.nao_video.connectToCamera(res=2, fps=30, camera_num=1, subscriber_id="C4Learning")
self.left_arm_angles = []
self.min_head_pitch = 0.
self.max_head_pitch = 0.
self.min_head_yaw = 0.
self.max_head_yaw = 0.
self.selected_hole = 0
self.shoulder_pitch_array = []
self.shoulder_pitch_file = None
self.shoulder_roll_array = []
self.shoulder_roll_file = None
self.elbow_yaw_array = []
self.elbow_yaw_file = None
self.elbow_roll_array = []
self.elbow_roll_file = None
self.wrist_yaw_array = []
self.wrist_yaw_file = None
def state(args):
global nao_motion
print
print "-1 = Empty, 0 = Red, 1 = Green"
print
data.IP = args['--ip']
data.PORT = int(args['--port'])
next_img_func = get_nao_image
if args['--no-robot']:
next_img_func = get_webcam_image
myc4 = Connect4Handler(next_img_func, cam_no=int(args['--cam-no']))
dist = float(args['--dist'])
sloped = args['--sloped']
tries = int(args['--min-detections'])
ref_mapping = DefaultConnect4Image().generate2DReference()
strategy = NAOVision(ref_mapping, lambda: perspective)
strategy.player_id = disc.GREEN
test_state = C4State()
nao_motion = MotionController()
nao_motion.lookAtGameBoard(dist)
while True:
try:
myc4.detectFrontHoles(dist, sloped, tries=tries)
perspective = myc4.front_hole_detector.getPerspective()
if not args['--no-image']:
cv2.imshow("Perspective", perspective)
game_state = strategy.analyseFullImage(test_state, img=perspective, debug=True)
print game_state
print
except BaseException:
pass
if cv2.waitKey(1) == 27:
print "Esc pressed : exit"
close_camera()
break
sleep(0.5)
def ik(args):
global broker, nao_motion, nao_video
data.IP = args['--ip']
data.PORT = int(args['--port'])
broker = ALBroker("myBroker", "0.0.0.0", 0, data.IP, data.PORT)
nao_motion = MotionController()
nao_video = VideoController()
nao_motion.stand()
nao_tts = ALProxy("ALTextToSpeech", data.IP, data.PORT)
try:
loop = LogicalLoop(nao_motion=nao_motion, nao_video=nao_video, nao_tts=nao_tts,
ppA=float(args['--ppA']), cA=float(args['--cA']), rA=float(args['--rA']),
min_detections=1, wait_disc_func=lambda: None)
if not args['--no-grab']:
wait_for_disc()
else:
nao_motion.motion_proxy.closeHand("LHand")
nao_motion.setLeftArmRaised(secure=True)
loop.inverseKinematicsConvergence(int(args['--hole']))
nao_motion.crouch()
except KeyboardInterrupt:
print "Keyboard interrupt"
broker.shutdown()
return 0
class Connect4Learning(object):
"""
Class that is made to allow NAO to learn where
to place its hand to play a disc into the game board of the Connect 4
"""
def __init__(self, robot_ip=nao.IP, robot_port=nao.PORT):
"""
:param robot_ip: The IP address of the robot
:type robot_ip: str
:param robot_port: The port of the robot
:type robot_port: int
Creates a new learning module for NAO
"""
self.nao_motion = MotionController(robot_ip=robot_ip, robot_port=robot_port)
self.nao_video = VideoController(robot_ip=robot_ip, robot_port=robot_port)
self.nao_video.connectToCamera(res=2, fps=30, camera_num=1, subscriber_id="C4Learning")
self.left_arm_angles = []
self.min_head_pitch = 0.
self.max_head_pitch = 0.
self.min_head_yaw = 0.
self.max_head_yaw = 0.
self.selected_hole = 0
self.shoulder_pitch_array = []
self.shoulder_pitch_file = None
self.shoulder_roll_array = []
self.shoulder_roll_file = None
self.elbow_yaw_array = []
self.elbow_yaw_file = None
self.elbow_roll_array = []
self.elbow_roll_file = None
self.wrist_yaw_array = []
self.wrist_yaw_file = None
def openFiles(self):
"""
Open the data files
"""
self.shoulder_pitch_array = []
self.shoulder_pitch_file = open("../../values/learning/shoulder_pitch", 'a')
self.shoulder_roll_array = []
self.shoulder_roll_file = open("../../values/learning/shoulder_roll", 'a')
self.elbow_yaw_array = []
self.elbow_yaw_file = open("../../values/learning/shoulder_yaw", 'a')
self.elbow_roll_array = []
self.elbow_roll_file = open("../../values/learning/shoulder_roll", 'a')
self.wrist_yaw_array = []
self.wrist_yaw_file = open("../../values/learning/wrist_yaw", 'a')
def waitingNaoPosition(self):
"""
Wait for the user to place NAO's arm in the good position
"""
raw_input("Please place NAO's left arm in the perfect position and press Enter")
self.left_arm_angles = self.nao_motion.getLeftArmAngles()
self.selected_hole = int(raw_input("Enter the index of the selected hole, [0, 6]: "))
self.min_head_yaw = float(raw_input("Enter the min head yaw in degrees: "))
self.max_head_yaw = float(raw_input("Enter the max head yaw in degrees: "))
self.min_head_pitch = float(raw_input("Enter the min head pitch in degrees: "))
self.max_head_pitch = float(raw_input("Enter the max head pitch in degrees: "))
self.openFiles()
def learningRoutine(self):
"""
Move NAO's head and detect the marker corresponding to the wanted hole
"""
self.nao_motion.setLeftArmRaised()
for current_pitch in np.arange(self.min_head_pitch, self.max_head_pitch, 0.5):
for current_yaw in np.arange(self.min_head_yaw, self.max_head_yaw, 0.5):
self.nao_motion.moveHead(current_pitch, current_yaw, radians=False)
self.detectMarker(current_pitch, current_yaw)
self.writeInFile()
def detectMarker(self, current_pitch, current_yaw):
"""
:param current_pitch: current pitch of NAO's head to stock as data
:param current_yaw: current yaw of NAO's head to stock as data
Detect the marker of the hole in the picture
"""
marker_found = False, None
tries = 0
while not marker_found[0] and tries < 10:
img = self.nao_video.getImageFromCamera()
markers = detect_markers(img)
tries += 1
for marker in markers:
if marker.id == (self.selected_hole + 1) * 1000:
marker_found = True, marker
if marker_found[0]:
self.record(marker_found[1], current_pitch, current_yaw)
def record(self, marker, pitch, yaw):
"""
:param marker: the detected marker for the hole
:type: hampy's Hamming Marker
:param pitch: the pitch of NAO's head
:param yaw: the yaw of NAO's head
record the result into the lists
"""
# Variables set
corners = np.array(geom.sort_rectangle_corners(marker.contours)).ravel().tolist()
variables = [pitch, yaw]
variables.extend(corners)
variables = np.array(variables)
# Result set
self.shoulder_pitch_array.append(np.append(variables, self.left_arm_angles[0]))
self.shoulder_roll_array.append(np.append(variables, self.left_arm_angles[1]))
self.elbow_yaw_array.append(np.append(variables, self.left_arm_angles[2]))
self.elbow_roll_array.append(np.append(variables, self.left_arm_angles[3]))
self.wrist_yaw_array.append(np.append(variables, self.left_arm_angles[4]))
def writeInFile(self):
"""
Write the results in different files
"""
np.savetxt(self.shoulder_pitch_file, np.array(self.shoulder_pitch_array), delimiter=",")
self.shoulder_pitch_file.close()
self.shoulder_pitch_array = []
np.savetxt(self.shoulder_roll_file, np.array(self.shoulder_roll_array), delimiter=",")
self.shoulder_roll_file.close()
self.shoulder_roll_array = []
np.savetxt(self.elbow_yaw_file, np.array(self.elbow_yaw_array), delimiter=",")
self.elbow_yaw_file.close()
self.elbow_yaw_array = []
np.savetxt(self.elbow_roll_file, np.array(self.elbow_roll_array), delimiter=",")
self.elbow_roll_file.close()
self.elbow_roll_array = []
np.savetxt(self.wrist_yaw_file, np.array(self.wrist_yaw_array), delimiter=",")
self.wrist_yaw_file.close()
self.wrist_yaw_array = []
def learn(self):
"""
Launch the learning module
"""
while True:
self.waitingNaoPosition()
self.learningRoutine()