Ejemplo n.º 1
0
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
Ejemplo n.º 2
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
Ejemplo n.º 3
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
Ejemplo n.º 4
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
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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
Ejemplo n.º 7
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()