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