Ejemplo n.º 1
0
    def buildPlay():
        if master.needToBuild:
            print 'building'
            #if master.setted_frame[0] != 0:
             #   master.setted_frame.insert(0, 0)
                #master.movement['position'][str(0)]['Left_hand'] = [200] * 9
                #master.movement['position'][str(0)]['Right_hand'] = [200] * 9
            if master.setted_frame[len(master.setted_frame) - 1] != master.movement['frame_number']:
                master.setted_frame.append(master.movement['frame_number'])
            index = 1
            while index < len(master.setted_frame):
                start = master.setted_frame[index - 1]
                stop = master.setted_frame[index]
                #        if (not master.movement['position'][str(0)]['Left_hand'])|(not master.movement['position'][str(0)]['Right_hand']):
                #            master.movement['position'][str(0)]['Left_hand'] = [200]*9
                #            master.movement['position'][str(0)]['Right_hand'] = [200]*9

                for frame in range(start + 1, stop, 1):
                    master.movement['position'][str(frame)]['Left_hand'] = master.movement['position'][str(start)]['Left_hand']
                    master.movement['position'][str(frame)]['Right_hand'] = master.movement['position'][str(start)]['Right_hand']
                index += 1
            print 'Done Building'

            print 'Smoothing the motion'

            win = find_smth_win()/int(2) if master.autoSmth.get() == 1 else master.smooth.get()
            for frame in range(master.movement['frame_number']):
                master.movement['position'][str(frame)]['Left_hand'] = np.mean([master.movement['position'][str(frame + i)]['Left_hand'] for i in range(min(master.movement['frame_number'] - frame-1, win)+1)], 0)
                master.movement['position'][str(frame)]['Right_hand'] = np.mean([master.movement['position'][str(frame + i)]['Right_hand'] for i in range(min(master.movement['frame_number'] - frame-1, win)+1)], 0)
                master.movement['position'][str(frame)]['Left_hand'] = list(np.int_(master.movement['position'][str(frame)]['Left_hand']))
                master.movement['position'][str(frame)]['Right_hand'] = list(np.int_(master.movement['position'][str(frame)]['Right_hand']))
            print 'Done Smoothing'
            master.needToBuild = False
            save.config(state=NORMAL)

        time.sleep(0.5)
        print 'Playing'
        goal_position = master.movement["position"]["0"]
        tools.go_to_pos(active_motors, motor, goal_position, pub, L, R)



        if list(master.emotion_list.curselection()):
            if master.emotion_list.get(master.emotion_list.curselection())=='show text':
                print 'text'
                HEAD_PUB = HEAD_TXT
                show.name = master.text.get()
            else:
                print 'emotion'
                HEAD_PUB = HEAD_EMO
                show.name = master.emotion_list.get(master.emotion_list.curselection())
            show.time = float(master.emotion_time.get())
            print show
        else:
            HEAD_PUB = None

        tools.do_seq(active_motors, master.movement["freq"], master.movement["position"],pub, L, R, HEAD_PUB, show)
        master.recSlider.set(master.movement["frame_number"])
        print 'Done'
Ejemplo n.º 2
0
    def PLAY(self):

        print 'Playing'
        goal_position = self.movement['position']['0']
        tools.go_to_pos(self.movement['actors_NAME'], motor, goal_position,
                        pub)
        tools.do_seq(self.movement['actors_NAME'], self.movement['freq'],
                     self.movement['position'], pub)
        print 'Done'
Ejemplo n.º 3
0
    def setframe(event):
        master.frm = master.recSlider.get()

        tools.do_seq(active_motors, 100, {'0':master.movement['position'][str(master.frm)]}, pub)

        if master.Lrec.get() == 0:
            tools.multi_servo_set(master.movement['position'][str(master.frm)]['Left_hand'], None, L)

        if master.Rrec.get() == 0:
            tools.multi_servo_set(None, master.movement['position'][str(master.frm)]['Right_hand'], None, R)
Ejemplo n.º 4
0
def do_sign(buffer):
    time.sleep(5)
    start = time.time()
    print "reseting positions"
    tools.reset(torso + head, present_pos, pub)
    robot_stat = 'ready'
    right_hand = [200, 200, 100, 100, 200, 100, 100, 100, 100]
    left_hand  = [200, 200, 100, 100, 200, 100, 100, 100, 100]
    old_motors = []
    print "BEGIN"
    while True:
        if buffer:
            start = time.time()
            if robot_stat == 'sleep':
                tools.reset(torso, present_pos, pub)
                robot_stat = 'ready'
            try:
                SIGN = buffer.pop(0)
                print 'Doing sign: ' + SIGN
                with open("/home/odroid/catkin_ws/src/robot_body/recording/data_base/" + SIGN + ".json", "r") as sign:
                    movement = json.load(sign)
            except Exception, err:
                print 'Robot can not do sign, error is '
                print  err
                show.name = SIGN
                show.time = 3
                HEAD_TXT.publish(show)
                time.sleep(3)
                continue
            playingPub.publish(1)
            names = movement["actors_NAME"]
            frames = movement["frame_number"]
            freq = float(movement["freq"])
            emotion = movement["emotion_name"]
            text = movement["text"]
            time_to_show = [3]#movement["emotion_time"]
            word_to_speak = movement["speak"]
            sign = movement["position"]
            motor_to_release = [mtr for mtr in old_motors if (mtr not in names) & (mtr not in torso)]
            if motor_to_release:
                goal_position = {"Robot": sign['0']['Robot']+[0 for name in motor_to_release],
                    "Right_hand": [200, 200, 100, 100, 200, 100, 100, 100, 100] if [mtr for mtr in motor_to_release if mtr in right_hand_motors]
                    else sign['0']['Right_hand'],
                    "Left_hand": [200, 200, 100, 100, 200, 100, 100, 100, 100] if [mtr for mtr in motor_to_release if mtr in left_hand_motors]
                    else sign['0']['Left_hand']}
            else:
                goal_position = sign['0']
            tools.go_to_pos(names+motor_to_release, present_pos, goal_position, pub, L, R, left_hand, right_hand)
            tools.releas(motor_to_release, pub, present_pos)
            if emotion != '':
                HEAD_PUB = HEAD_EMO
                show.name = emotion
            elif text != '':
                HEAD_PUB = HEAD_TXT
                show.name = text
            else:
                HEAD_PUB = None
            print "time: " + str(time_to_show)
            if time_to_show:
                show.time = time_to_show[0]
            SPEAKER.publish(word_to_speak)
            tools.do_seq(names, freq, sign, pub, L, R, HEAD_PUB, show)
            right_hand = sign[str(frames - 1)]["Right_hand"]
            left_hand  = sign[str(frames - 1)]["Left_hand"]
            old_motors = names
            
            playingPub.publish(0)


            if not buffer:
                goal_position = {'Robot': [0 for name in names],
                                 'Right_hand': [200, 200, 100, 100, 200, 100, 100, 100, 100],
                                 'Left_hand': [200, 200, 100, 100, 200, 100, 100, 100, 100]}
                tools.go_to_pos(names, present_pos, goal_position, pub, L, R, left_hand, right_hand)
                right_hand = [200, 200, 100, 100, 200, 100, 100, 100, 100]
                left_hand = [200, 200, 100, 100, 200, 100, 100, 100, 100]
                tools.releas([name for name in names if name not in torso], pub, None, L, R)
                old_motors = []


        else:
            if (time.time() - start > time_to_sleep) & (robot_stat == 'ready'):
                tools.releas([name for name in list  if present_pos[name].compliant == False], pub, present_pos, L, R)
                robot_stat = 'sleep'
                print 'robot going to sleep'
            time.sleep(1)