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()