def motion_actuator(queue_beat, queue_motion_from_selector):
#print('Process to actuate motion: %s' % os.getpid())
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip, port, True, True, -5000, 5)
# Connect to V-REP
if clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' + ip + ':' +
str(port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
Body = {}
get_first_handles(
clientID, Body) #get first handles of Nao in the virtual environment
motion_base_dir = 'MyNao\\motion_base\\motion_base.json'
with open(motion_base_dir, 'r') as f:
motion_base = json.load(f)
sc = Speed_Controller_and_Motion_Actuator(alpha=0.9,
motion_base=motion_base,
kp=2e-4,
ki=1e-6,
kd=5e-4) #2e-4, 1e-6, 1e-6
#time_init = time.time()
#time.sleep(5)
while (True):
if not queue_motion_from_selector.empty():
current_motion_idx = queue_motion_from_selector.get()
sc.set_current_motion(current_motion_idx)
current_motion = motion_base[str(current_motion_idx)]['frame']
for frame in range(len(current_motion)):
time_1 = time.time()
joint_actuate(clientID, Body, current_motion, frame)
time_transmit = time.time() - time_1
sc.set_current_frame(frame)
sc.control(queue_beat)
time_sleep = sc.get_time_sleep()
if time_sleep < 0.02:
time.sleep(0.033)
else:
time.sleep(max(0.02, time_sleep - time_transmit))
#primitive_revised[:, :, 0] = -primitive_backup[:, :, 1]
#primitive_revised[:, :, 1] = primitive_backup[:, :, 0]
#primitive_revised[:, :, 2] = -primitive_backup[:, :, 2]
#primitive = primitive_revised.reshape(-1, 51)
primitive = primitive_backup
motion = {}
for i in range(primitive.shape[0]):
motion[str(i)] = {}
for j in range(primitive.shape[1]):
#motion[str(i)][str(j)] = [primitive[i][j][0], primitive[i][j][2], primitive[i][j][1]]
motion[str(i)][str(j)] = [
-primitive[i][j][2], primitive[i][j][0],
-primitive[i][j][1]
]
for frame in range(primitive.shape[0]):
joint_actuate(clientID, Body, motion, frame)
returnCode, position = sim.simxGetObjectPosition(
clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer)
time.sleep(0.01)
"""if position[2] < 0.4 and position[2] > 0: #fall down
#print('fall down')
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
time.sleep(.1)
sys.exit()"""
current_state = np.random.choice(
transition_graph[current_state]['next'],
p=np.array(transition_graph[current_state]['p']).ravel())
#print('current:', current_state)
if current_state in sequence_head:
mode = 1
p = np.array(
primitive = np.load('C:\\Users\\lenovo\\Desktop\\AI-Project-Portfolio\\danceprimitives\\'+'{0}'.format(str(abs(current_state)).zfill(4))+'\\dance_motion_'+str(abs(current_state))+'.npy')
primitive_backup = primitive.reshape(-1, 17, 3)
#primitive_revised = np.empty((primitive_backup.shape[0], primitive_backup.shape[1], primitive_backup.shape[2]))
#primitive_revised[:, :, 0] = -primitive_backup[:, :, 1]
#primitive_revised[:, :, 1] = primitive_backup[:, :, 0]
#primitive_revised[:, :, 2] = -primitive_backup[:, :, 2]
#primitive = primitive_revised.reshape(-1, 51)
primitive = primitive_backup
motion = {}
for i in range(primitive.shape[0]):
motion[str(i)] = {}
for j in range(primitive.shape[1]):
#motion[str(i)][str(j)] = [primitive[i][j][0], primitive[i][j][2], primitive[i][j][1]]
motion[str(i)][str(j)] = [-primitive[i][j][2], primitive[i][j][0], -primitive[i][j][1]]
for frame in range(primitive.shape[0]):
joint_actuate(clientID, Body, motion, frame, current_state)
returnCode, position=sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer)
time.sleep(0.03)
"""if position[2] < 0.4 and position[2] > 0: #fall down
#print('fall down')
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
time.sleep(.1)
sys.exit()"""
if count < 3:
count += 1
current_state = np.random.choice(limited_transition_graph[current_state]['next'], p=np.array(limited_transition_graph[current_state]['p']).ravel())
else:
count =0
current_state = np.random.choice(inter_transition_graph[current_state]['next'], p=np.array(inter_transition_graph[current_state]['p']).ravel())
if current_state in sequence_head:
mode = 1
primitive = np.load(
'C:\\Users\\lenovo\\Desktop\\AI-Project-Portfolio\\danceprimitives\\'
+ '{0}'.format(str(abs(primitive_id)).zfill(4)) +
'\\dance_motion_' + str(abs(primitive_id)) + '.npy')
primitive = primitive.reshape(-1, 17, 3)
motion = {}
for i in range(primitive.shape[0]):
motion[str(i)] = {}
for j in range(primitive.shape[1]):
#motion[str(i)][str(j)] = [primitive[i][j][0], primitive[i][j][2], primitive[i][j][1]]
motion[str(i)][str(j)] = [
-primitive[i][j][2], primitive[i][j][0],
-primitive[i][j][1]
]
for frame in range(primitive.shape[0]):
joint_actuate(clientID, Body, motion, frame, primitive_id)
returnCode, position = sim.simxGetObjectPosition(
clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer)
time.sleep(0.03)
#print(position)
if position[2] < 0.2 and position[2] > 0: #fall down
print(position[2], 'fall down in primitive', primitive_id,
'frame', frame)
unstable_sequences.append(idx)
#init_joint(clientID, Body, motion, frame)
#time.sleep(1)
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
time.sleep(1)
continue_flag = 1
break
if continue_flag:
