def get_nao_image(camera_num=0):
global nao_video, nao_motion
if nao_video is None:
nao_video = VideoController()
nao_motion = MotionController()
# clean()
ret = nao_video.connectToCamera(res=2, fps=30, camera_num=camera_num)
if ret < 0:
print "Could not open camera"
return None
return nao_video.getImageFromCamera()
def get_nao_image(camera_num=0, res=1):
global nao_video, nao_motion, nao_tracking
if nao_video is None:
nao_video = VideoController()
if nao_motion is None:
nao_motion = MotionController()
nao_video.disconnectFromCamera()
ret = nao_video.connectToCamera(res=res, fps=30, camera_num=camera_num)
if ret < 0:
print "Could not open camera"
return None
return nao_video.getImageFromCamera(camera_num=camera_num)
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 get_images(dist):
global nao_video
nao_video = VideoController()
nao_video.unsubscribeAllCameras()
nao_video.connectToCamera(res=1, fps=5, camera_num=0)
images = []
max_time = 15
start = time.time()
current = time.time()
while current - start < max_time:
images.append(nao_video.getImageFromCamera())
current = time.time()
for i, img in enumerate(images):
cv2.imwrite("../../../latex/img/" + str(dist) + "m/img_" + str(i) + ".png", img)
return images
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()
