def skeleton_motion_data_collect(skeleton_file_list):
train_data = []
total_frame = 0
fc_list = [] # frame count list
index = 0
while index < len(skeleton_file_list):
# -----------------------
# -----[ Data Load ]-----
# -----------------------
print('list number: ', index)
print('skeleton: ', skeleton_file_list[index])
file = open(skeleton_file_list[index])
frameCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0]
skel = Skeleton()
for f in range(frameCount): # frameCount
bodyCount = np.fromstring(file.readline(), sep=' ',
dtype='int32')[0]
body = Body_struct()
for b in range(bodyCount):
bodyInfo = np.fromstring(file.readline(),
sep=' ',
dtype='float64')
jointCount = np.fromstring(file.readline(),
sep=' ',
dtype='int32')[0]
body.set_body_info(bodyInfo, jointCount, f * 1.0 / frameCount)
for j in range(jointCount):
jointInfo = np.fromstring(file.readline(),
sep=' ',
dtype='float64')
joint = Joint_struct(jointInfo)
body.joints.append(joint)
skel.append_body(f, b, body)
# print('phase: ', body.phase)
train_data.append(skel)
file.close()
# print(skel.print_skeleton_info())
index += 1
total_frame += len(skel.iBody)
fc_list.append(len(skel.iBody))
print('total number of frames: ', total_frame)
print('train data len: ', len(train_data))
return train_data, total_frame, fc_list
skel = Skeleton()
for f in range(frameCount): # frameCount
bodyCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0]
for b in range(bodyCount):
body = Body_struct()
bodyInfo = np.fromstring(file.readline(), sep=' ', dtype='float64')
jointCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0]
body.set_body_info(bodyInfo, jointCount, f*1.0/frameCount)
for j in range(jointCount):
jointInfo = np.fromstring(file.readline(), sep=' ', dtype='float64')
joint = Joint_struct(jointInfo)
body.joints.append(joint)
try:
skel.append_body(f, b, body)
except IndexError:
exception_avi_list.append(video_file_list[index])
exception_skel_list.append(skeleton_file_list[index])
print('Skeleton was not found..')
file.close()
# print(skel.print_skeleton_info())
# ----------------------------------------
# -----[ Display Image and Skeleton ]-----
# ----------------------------------------
fCount = 0 # frame count
while(cap.isOpened()):
ret, frame = cap.read()
def run(self):
# -----[ For socket Communication, Server ] -----
if socket_comm:
conn = SocketCom('localhost', tcp_port)
print('Port is Opened and Wait for the connection..')
if retarget_mode == 'Analytic': # direct connect to the NAO agent
nao = NAO_AMR()
conn.socket_connect()
conn.write_socket({
'header': 'start Kinect!',
'data': []
}) # start signal
else: # indirect connect to the NAO by C-3PO model
demo = MR_Demo(target_robot=target_robot,
target_env=target_env,
tcp_port=tcp_port)
if target_env == 'CHREO': conn.socket_connect()
# -------- Main Program Loop -----------
demo_body_idx = -1 # only for the firstly detected human
socket_count = 0
while not self._done:
# --- Main event loop
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
self._done = True # Flag that we are done so we exit this loop
elif event.type == pygame.VIDEORESIZE: # window resized
self._screen = pygame.display.set_mode(
event.dict['size'],
pygame.HWSURFACE | pygame.DOUBLEBUF | pygame.RESIZABLE,
32)
# --- Game logic should go here
# --- Getting frames and drawing
# --- Woohoo! We've got a color frame! Let's fill out back buffer surface with frame's data
if self._kinect.has_new_color_frame():
frame = self._kinect.get_last_color_frame()
self.draw_color_frame(frame, self._frame_surface)
frame = None
# --- Cool! We have a body frame, so can get skeletons
if self._kinect.has_new_body_frame():
self._bodies = self._kinect.get_last_body_frame()
# --- draw skeletons to _frame_surface
if self._bodies is not None:
# Get the index of the first body only
if demo_body_idx < 0: # no body index
# Get new body index
for i in range(0, self._kinect.max_body_count):
body = self._bodies.bodies[i]
if body.is_tracked:
demo_body_idx = i
break
else: # already have a body index
body = self._bodies.bodies[demo_body_idx]
if not body.is_tracked:
demo_body_idx = -1
if demo_body_idx >= 0:
# print('Demo body index: ', demo_body_idx)
body = self._bodies.bodies[demo_body_idx]
joints = body.joints
joint_points = self._kinect.body_joints_to_color_space(
joints)
# get a skeleton frame from Kinect
skel = Skeleton()
body_struct = Body_struct()
bodyInfo = np.array([
demo_body_idx, 0, 0, 0, 0, 0, 0, 0, 0, body.is_tracked
])
jointCount = PyKinectV2.JointType_Count
body_struct.set_body_info(bodyInfo, jointCount, phase=0)
for j in range(jointCount): # 25 body joints
jointInfo = np.array([
joints[j].Position.x, joints[j].Position.y,
joints[j].Position.z, 0, 0, 0, 0, 0, 0, 0, 0,
body.is_tracked
])
joint = Joint_struct(jointInfo)
body_struct.joints.append(joint)
skel.append_body(0, demo_body_idx, body_struct)
if socket_comm:
socket_count += 1
if socket_count >= 1: # 6, for sync of socket delay
socket_count = 0
_from = _to = 'None'
skel_data = skeleton_coordinate_transform(
[skel], 1) # numpy data
if retarget_mode == 'Analytic':
data = skel_data[0].tolist()[:-1]
lMotors = nao.left_arm_solve(data)
rMotors = nao.right_arm_solve(data)
motors = lMotors + rMotors
conn.write_socket({
'header': 'Analytic',
'data': motors,
'from': _from,
'to': _to
})
else:
if target_env == 'CHREO':
retargeted_skel = demo.do_retargeting(
skel_data[0].tolist()[:-1])
conn.write_socket({
'header': 'SetMotor',
'data': retargeted_skel,
'from': _from,
'to': _to
})
elif target_env == 'VREP':
ret = demo.do_retargeting_vrep(
skel_data[0].tolist()[:-1])
conn.flush()
print('Send Success')
self.draw_body(joints, joint_points,
SKELETON_COLORS[demo_body_idx])
# --- copy back buffer surface pixels to the screen, resize it if needed and keep aspect ratio
# --- (screen size may be different from Kinect's color frame size)
h_to_w = float(self._frame_surface.get_height(
)) / self._frame_surface.get_width() # (w: 1920, h: 1080)
target_height = int(h_to_w * self._screen.get_width())
surface_to_draw = pygame.transform.scale(
self._frame_surface, (self._screen.get_width(), target_height))
self._screen.blit(surface_to_draw, (0, 0))
surface_to_draw = None
pygame.display.update()
# --- Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# --- Limit to 60 frames per second
self._clock.tick(60)
# Close our Kinect sensor, close the window and quit.
self._kinect.close()
pygame.quit()
