def map_slp_to_rand_angles(self, original_pose, alter_angles=True): R_root = libKinematics.matrix_from_dir_cos_angles(original_pose[0:3]) flip_root_euler = np.pi flip_root_R = libKinematics.eulerAnglesToRotationMatrix( [flip_root_euler, 0.0, 0.0]) root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix( [0.0, 0.0, 0.0]) # randomize the root rotation # root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix([0.0, 0.0, 0.0]) #randomize the root rotation dir_cos_root = libKinematics.dir_cos_angles_from_matrix( np.matmul(root_rot_rand_R, np.matmul(R_root, flip_root_R))) # R_root2 = libKinematics.matrix_from_dir_cos_angles([original_pose[0]-4*np.pi, original_pose[1], original_pose[2]]) # dir_cos_root2 = libKinematics.dir_cos_angles_from_matrix(R_root2) # print('eulers2', libKinematics.rotationMatrixToEulerAngles(R_root2)) self.m.pose[0] = dir_cos_root[0] self.m.pose[1] = dir_cos_root[1] self.m.pose[2] = dir_cos_root[2] for i in range(3, 72): self.m.pose[i] = original_pose[i] from capsule_body import get_capsules, joint2name, rots0 capsules = get_capsules(self.m) joint2name = joint2name rots0 = rots0 print len(capsules) return self.m, capsules, joint2name, rots0
def generate_prechecked_pose(self, gender, posture, stiffness, filename): prechecked_pose_list = np.load(filename, allow_pickle = True).tolist() print len(prechecked_pose_list) shuffle(prechecked_pose_list) pyRender = libRender.pyRenderMesh() for shape_pose_vol in prechecked_pose_list[6:]: #print shape_pose_vol #print shape_pose_vol[0] #print shape_pose_vol[1] #print shape_pose_vol[2] for idx in range(len(shape_pose_vol[0])): #print shape_pose_vol[0][idx] self.m.betas[idx] = shape_pose_vol[0][idx] print 'init' print shape_pose_vol[2][0],shape_pose_vol[2][1],shape_pose_vol[2][2] self.m.pose[:] = np.array(72 * [0.]) for idx in range(len(shape_pose_vol[1])): #print shape_pose_vol[1][idx] #print self.m.pose[shape_pose_vol[1][idx]] #print shape_pose_vol[2][idx] pose_index = shape_pose_vol[1][idx]*1 self.m.pose[pose_index] = shape_pose_vol[2][idx]*1. #print idx, pose_index, self.m.pose[pose_index], shape_pose_vol[2][idx] print self.m.pose[0:3] init_root = np.array(self.m.pose[0:3])+0.000001 init_rootR = libKinematics.matrix_from_dir_cos_angles(init_root) root_rot = libKinematics.eulerAnglesToRotationMatrix([np.pi, 0.0, np.pi/2]) #print root_rot trans_root = libKinematics.dir_cos_angles_from_matrix(np.matmul(root_rot, init_rootR)) self.m.pose[0] = trans_root[0] self.m.pose[1] = trans_root[1] self.m.pose[2] = trans_root[2] print root_rot print init_rootR print trans_root print init_root, trans_root #print self.m.J_transformed[1, :], self.m.J_transformed[4, :] # self.m.pose[51] = selection_r pyRender.mesh_render_pose_bed(self.m, self.point_cloud_array, self.pc_isnew, self.pressure, self.markers) self.point_cloud_array = None
def estimate_real_time(self, gender, filename): pyRender = libRender.pyRenderMesh() mat_size = (64, 27) if torch.cuda.is_available(): # Use for GPU GPU = True dtype = torch.cuda.FloatTensor print '######################### CUDA is available! #############################' else: # Use for CPU GPU = False dtype = torch.FloatTensor print '############################## USING CPU #################################' import convnet as convnet from torch.autograd import Variable if GPU == True: model = torch.load(filename) model = model.cuda() else: model = torch.load(filename, map_location='cpu') while not rospy.is_shutdown(): pmat = np.fliplr(np.flipud(np.clip(self.pressure.reshape(mat_size)*5.0, a_min=0, a_max=100))) pmat = gaussian_filter(pmat, sigma= 0.5) pmat_stack = PreprocessingLib().preprocessing_create_pressure_angle_stack_realtime(pmat, self.bedangle, mat_size) pmat_stack = torch.Tensor(pmat_stack) images_up_non_tensor = np.array(PreprocessingLib().preprocessing_pressure_map_upsample(pmat_stack.numpy(), multiple=2)) images_up = Variable(torch.Tensor(images_up_non_tensor).type(dtype), requires_grad=False) betas_est, root_shift_est, angles_est = model.forward_kinematic_angles_realtime(images_up) print betas_est, root_shift_est, angles_est angles_est = angles_est.reshape(72) for idx in range(10): #print shape_pose_vol[0][idx] self.m.betas[idx] = betas_est[idx] for idx in range(72): self.m.pose[idx] = angles_est[idx] init_root = np.array(self.m.pose[0:3])+0.000001 init_rootR = libKinematics.matrix_from_dir_cos_angles(init_root) root_rot = libKinematics.eulerAnglesToRotationMatrix([np.pi, 0.0, np.pi/2]) #print root_rot trans_root = libKinematics.dir_cos_angles_from_matrix(np.matmul(root_rot, init_rootR)) self.m.pose[0] = trans_root[0] self.m.pose[1] = trans_root[1] self.m.pose[2] = trans_root[2] #print self.m.J_transformed[1, :], self.m.J_transformed[4, :] # self.m.pose[51] = selection_r pyRender.mesh_render_pose_bed(self.m, root_shift_est, self.point_cloud_array, self.pc_isnew, pmat, self.markers, self.bedangle) self.point_cloud_array = None
def estimate_real_time(self, filename1, filename2 = None): pyRender = libRender.pyRenderMesh() mat_size = (64, 27) from unpack_batch_lib import UnpackBatchLib if torch.cuda.is_available(): # Use for GPU GPU = True dtype = torch.cuda.FloatTensor print '######################### CUDA is available! #############################' else: # Use for CPU GPU = False dtype = torch.FloatTensor print '############################## USING CPU #################################' from torch.autograd import Variable if GPU == True: for i in range(0, 8): try: model = torch.load(filename1, map_location={'cuda:'+str(i):'cuda:0'}) model = model.cuda().eval() break except: pass if filename2 is not None: for i in range(0, 8): try: model2 = torch.load(filename2, map_location={'cuda:'+str(i):'cuda:0'}) model2 = model2.cuda().eval() break except: pass else: model2 = None else: model = torch.load(filename1, map_location='cpu').eval() if filename2 is not None: model2 = torch.load(filename2, map_location='cpu').eval() else: model2 = None pub = rospy.Publisher('meshTopic', MeshAttr) #rospy.init_node('talker', anonymous=False) while not rospy.is_shutdown(): pmat = np.fliplr(np.flipud(np.clip(self.pressure.reshape(mat_size)*float(self.CTRL_PNL['pmat_mult']*4), a_min=0, a_max=100))) #pmat = np.fliplr(np.flipud(np.clip(self.pressure.reshape(mat_size)*float(1), a_min=0, a_max=100))) #print "max is : ", np.max(pmat) #print "sum is : ", np.sum(pmat) if self.CTRL_PNL['cal_noise'] == False: pmat = gaussian_filter(pmat, sigma= 0.5) pmat_stack = PreprocessingLib().preprocessing_create_pressure_angle_stack_realtime(pmat, self.bedangle, mat_size) if self.CTRL_PNL['cal_noise'] == False: pmat_stack = np.clip(pmat_stack, a_min=0, a_max=100) pmat_stack = np.array(pmat_stack) if self.CTRL_PNL['incl_pmat_cntct_input'] == True: pmat_contact = np.copy(pmat_stack[:, 0:1, :, :]) pmat_contact[pmat_contact > 0] = 100 pmat_stack = np.concatenate((pmat_contact, pmat_stack), axis = 1) weight_input = WEIGHT_LBS/2.20462 height_input = (HEIGHT_IN*0.0254 - 1)*100 batch1 = np.zeros((1, 162)) if GENDER == 'f': batch1[:, 157] += 1 elif GENDER == 'm': batch1[:, 158] += 1 batch1[:, 160] += weight_input batch1[:, 161] += height_input if self.CTRL_PNL['normalize_input'] == True: self.CTRL_PNL['depth_map_input_est'] = False pmat_stack = self.TPL.normalize_network_input(pmat_stack, self.CTRL_PNL) batch1 = self.TPL.normalize_wt_ht(batch1, self.CTRL_PNL) pmat_stack = torch.Tensor(pmat_stack) batch1 = torch.Tensor(batch1) batch = [] batch.append(pmat_stack) batch.append(batch1) self.CTRL_PNL['adjust_ang_from_est'] = False scores, INPUT_DICT, OUTPUT_DICT = UnpackBatchLib().unpackage_batch_kin_pass(batch, False, model, self.CTRL_PNL) self.CTRL_PNL['first_pass'] = False mdm_est_pos = OUTPUT_DICT['batch_mdm_est'].clone().unsqueeze(1) / 16.69545796387731 mdm_est_neg = OUTPUT_DICT['batch_mdm_est'].clone().unsqueeze(1) / 45.08513083167194 mdm_est_pos[mdm_est_pos < 0] = 0 mdm_est_neg[mdm_est_neg > 0] = 0 mdm_est_neg *= -1 cm_est = OUTPUT_DICT['batch_cm_est'].clone().unsqueeze(1) * 100 / 43.55800622930469 #1. / 16.69545796387731, # pos est depth #1. / 45.08513083167194, # neg est depth #1. / 43.55800622930469, # cm est if model2 is not None: batch_cor = [] batch_cor.append(torch.cat((pmat_stack[:, 0:1, :, :], mdm_est_pos.type(torch.FloatTensor), mdm_est_neg.type(torch.FloatTensor), cm_est.type(torch.FloatTensor), pmat_stack[:, 1:, :, :]), dim=1)) if self.CTRL_PNL['full_body_rot'] == False: batch_cor.append(torch.cat((batch1, OUTPUT_DICT['batch_betas_est'].cpu(), OUTPUT_DICT['batch_angles_est'].cpu(), OUTPUT_DICT['batch_root_xyz_est'].cpu()), dim = 1)) elif self.CTRL_PNL['full_body_rot'] == True: batch_cor.append(torch.cat((batch1, OUTPUT_DICT['batch_betas_est'].cpu(), OUTPUT_DICT['batch_angles_est'].cpu(), OUTPUT_DICT['batch_root_xyz_est'].cpu(), OUTPUT_DICT['batch_root_atan2_est'].cpu()), dim = 1)) self.CTRL_PNL['adjust_ang_from_est'] = True scores, INPUT_DICT, OUTPUT_DICT = UnpackBatchLib().unpackage_batch_kin_pass(batch_cor, False, model2, self.CTRL_PNL) betas_est = np.squeeze(OUTPUT_DICT['batch_betas_est_post_clip'].cpu().numpy()) angles_est = np.squeeze(OUTPUT_DICT['batch_angles_est_post_clip']) root_shift_est = np.squeeze(OUTPUT_DICT['batch_root_xyz_est_post_clip'].cpu().numpy()) #print betas_est.shape, root_shift_est.shape, angles_est.shape #print betas_est, root_shift_est, angles_est angles_est = angles_est.reshape(72) for idx in range(10): #print shape_pose_vol[0][idx] self.m.betas[idx] = betas_est[idx] for idx in range(72): self.m.pose[idx] = angles_est[idx] init_root = np.array(self.m.pose[0:3])+0.000001 init_rootR = libKinematics.matrix_from_dir_cos_angles(init_root) root_rot = libKinematics.eulerAnglesToRotationMatrix([np.pi, 0.0, np.pi/2]) #print root_rot trans_root = libKinematics.dir_cos_angles_from_matrix(np.matmul(root_rot, init_rootR)) self.m.pose[0] = trans_root[0] self.m.pose[1] = trans_root[1] self.m.pose[2] = trans_root[2] #print self.m.J_transformed[1, :], self.m.J_transformed[4, :] # self.m.pose[51] = selection_r print self.m.r #print OUTPUT_DICT['verts'] pyRender.mesh_render_pose_bed_orig(self.m, root_shift_est, self.point_cloud_array, self.pc_isnew, pmat, self.markers, self.bedangle) self.point_cloud_array = None
def map_shuffled_yash_to_smpl_angles(self, posture, shiftUD, alter_angles=True): angle_type = "angle_axis" #angle_type = "euler" #limits are relative to left side of body. some joints for right side need flip ''' dircos_limit_lay = {} dircos_limit_lay['hip0_L'] = -1.3597488648299256 dircos_limit_lay['hip0_U'] = 0.07150907780435195 dircos_limit_lay['hip1_L'] = -1.1586383564647427 dircos_limit_lay['hip1_U'] = 0.9060547654972676 dircos_limit_lay['hip2_L'] = -0.22146805085504204 dircos_limit_lay['hip2_U'] = 0.7070043169765721 dircos_limit_lay['knee_L'] = 0.0 dircos_limit_lay['knee_U'] = 2.6656374963467337 dircos_limit_lay['shd0_L'] = -1.3357882970793375 dircos_limit_lay['shd0_U'] = 1.580579714602293 dircos_limit_lay['shd1_L'] = -1.545407583212041 dircos_limit_lay['shd1_U'] = 1.0052006799247593 dircos_limit_lay['shd2_L'] = -1.740132642542865 dircos_limit_lay['shd2_U'] = 2.1843225295849584 dircos_limit_lay['elbow_L'] = -2.359569981489685 dircos_limit_lay['elbow_U'] = 0.0 dircos_limit_sit = {} dircos_limit_sit['hip0_L'] = -2.655719256295683 dircos_limit_sit['hip0_U'] = -1.0272376612196454 dircos_limit_sit['hip1_L'] = -0.9042010441370677 dircos_limit_sit['hip1_U'] = 0.9340170076795724 dircos_limit_sit['hip2_L'] = -0.37290106435540205 dircos_limit_sit['hip2_U'] = 0.9190375825276169 dircos_limit_sit['knee_L'] = 0.0 dircos_limit_sit['knee_U'] = 2.6869091212979197 dircos_limit_sit['shd0_L'] = -1.5525790052790263 dircos_limit_sit['shd0_U'] = 0.6844121077167088 dircos_limit_sit['shd1_L'] = -0.7791830995457885 dircos_limit_sit['shd1_U'] = 1.0243644544117376 dircos_limit_sit['shd2_L'] = -1.5283559130498783 dircos_limit_sit['shd2_U'] = 1.1052120105774226 dircos_limit_sit['elbow_L'] = -2.4281310593630705 dircos_limit_sit['elbow_U'] = 0.0 ''' ''' dircos_limit = {} dircos_limit['hip0_L'] = -2.655719256295683 dircos_limit['hip0_U'] = 0.07150907780435195 dircos_limit['hip1_L'] = -1.1586383564647427 dircos_limit['hip1_U'] = 0.9340170076795724 dircos_limit['hip2_L'] = -0.37290106435540205 dircos_limit['hip2_U'] = 0.9190375825276169 dircos_limit['knee_L'] = 0.0 dircos_limit['knee_U'] = 2.6869091212979197 dircos_limit['shd0_L'] = -1.5525790052790263 dircos_limit['shd0_U'] = 1.580579714602293 dircos_limit['shd1_L'] = -1.545407583212041 dircos_limit['shd1_U'] = 1.0243644544117376 dircos_limit['shd2_L'] = -1.740132642542865 dircos_limit['shd2_U'] = 2.1843225295849584 dircos_limit['elbow_L'] = -2.4281310593630705 dircos_limit['elbow_U'] = 0.0 ''' dircos_limit = {} dircos_limit['hip0_L'] = -2.7443260550003967 dircos_limit['hip0_U'] = -0.14634814003149707 dircos_limit['hip1_L'] = -1.0403111466710133 dircos_limit['hip1_U'] = 1.1185343875601006 dircos_limit['hip2_L'] = -0.421484532214729 dircos_limit['hip2_U'] = 0.810063927501682 dircos_limit['knee_L'] = 0.0 dircos_limit['knee_U'] = 2.7020409229712863 dircos_limit['shd0_L'] = -1.8674195346872975 dircos_limit['shd0_U'] = 1.410545172086535 dircos_limit['shd1_L'] = -1.530112726921327 dircos_limit['shd1_U'] = 1.2074724617209949 dircos_limit['shd2_L'] = -1.9550515937478927 dircos_limit['shd2_U'] = 1.7587935205169856 dircos_limit['elbow_L'] = -2.463868908637374 dircos_limit['elbow_U'] = 0.0 #if posture == "lay": # dircos_limit = dircos_limit_lay # elif posture == "sit": # dircos_limit = dircos_limit_sit if alter_angles == True: try: entry = self.angles_data.pop() except: print "############################# RESAMPLING !! #################################" if posture == "sit": filename = self.filepath_prefix + '/data/init_ik_solutions/all_sit_angles_side_up.p' else: filename = self.filepath_prefix + '/data/init_ik_solutions/all_lay_angles_side_up.p' with open(filename, 'rb') as fp: self.angles_data = pickle.load(fp) shuffle(self.angles_data) entry = self.angles_data.pop() if posture == "sit": if shiftUD >= 0.1: self.m.pose[0] = np.pi / 3 self.m.pose[9] = 0.0 self.m.pose[18] = 0.0 self.m.pose[27] = 0.0 elif 0.0 <= shiftUD < 0.1: self.m.pose[0] = np.pi / 6 self.m.pose[9] = np.pi / 6 self.m.pose[18] = 0.0 self.m.pose[27] = 0.0 elif -0.1 <= shiftUD < 0.0: self.m.pose[0] = np.pi / 9 self.m.pose[9] = np.pi / 9 self.m.pose[18] = np.pi / 9 self.m.pose[27] = 0.0 elif shiftUD < -0.1: self.m.pose[0] = np.pi / 12 self.m.pose[9] = np.pi / 12 self.m.pose[18] = np.pi / 12 self.m.pose[27] = np.pi / 12 self.m.pose[36] = np.pi / 12 self.m.pose[45] = np.pi / 12 R_root = libKinematics.eulerAnglesToRotationMatrix( [-float(self.m.pose[0]), 0.0, 0.0]) R_l_hip_rod = libKinematics.matrix_from_dir_cos_angles( entry['l_hip_' + angle_type]) R_r_hip_rod = libKinematics.matrix_from_dir_cos_angles( entry['r_hip_' + angle_type]) R_l = np.matmul(R_root, R_l_hip_rod) R_r = np.matmul(R_root, R_r_hip_rod) new_left_hip = libKinematics.dir_cos_angles_from_matrix(R_l) new_right_hip = libKinematics.dir_cos_angles_from_matrix(R_r) flip_leftright = random.choice([True, False]) while True: self.reset_pose = False print R_root, self.m.pose[0], shiftUD print entry['l_hip_' + angle_type], new_left_hip print entry['r_hip_' + angle_type], new_right_hip if flip_leftright == False: #print "WORKING WITH:, ", new_left_hip[0], dircos_limit['hip0_L'], dircos_limit['hip0_U'] #time.sleep(2) self.m.pose[3] = generator.get_noisy_angle( new_left_hip[0], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[4] = generator.get_noisy_angle( new_left_hip[1], dircos_limit['hip1_L'], dircos_limit['hip1_U']) self.m.pose[5] = generator.get_noisy_angle( new_left_hip[2], dircos_limit['hip2_L'], dircos_limit['hip2_U']) self.m.pose[12] = generator.get_noisy_angle( entry['l_knee'][0], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[6] = generator.get_noisy_angle( new_right_hip[0], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[7] = generator.get_noisy_angle( new_right_hip[1], -dircos_limit['hip1_U'], -dircos_limit['hip1_L']) self.m.pose[8] = generator.get_noisy_angle( new_right_hip[2], -dircos_limit['hip2_U'], -dircos_limit['hip2_L']) self.m.pose[15] = generator.get_noisy_angle( entry['r_knee'][0], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[39] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][0] * 1 / 3, dircos_limit['shd0_L'] * 1 / 3, dircos_limit['shd0_U'] * 1 / 3) self.m.pose[40] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][1] * 1 / 3, dircos_limit['shd1_L'] * 1 / 3, dircos_limit['shd1_U'] * 1 / 3) self.m.pose[41] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][2] * 1 / 3, dircos_limit['shd2_L'] * 1 / 3, dircos_limit['shd2_U'] * 1 / 3) self.m.pose[48] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][0] * 2 / 3, dircos_limit['shd0_L'] * 2 / 3, dircos_limit['shd0_U'] * 2 / 3) self.m.pose[49] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][1] * 2 / 3, dircos_limit['shd1_L'] * 2 / 3, dircos_limit['shd1_U'] * 2 / 3) self.m.pose[50] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][2] * 2 / 3, dircos_limit['shd2_L'] * 2 / 3, dircos_limit['shd2_U'] * 2 / 3) self.m.pose[55] = generator.get_noisy_angle( entry['l_elbow'][1], dircos_limit['elbow_L'], dircos_limit['elbow_U']) self.m.pose[42] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][0] * 1 / 3, dircos_limit['shd0_L'] * 1 / 3, dircos_limit['shd0_U'] * 1 / 3) self.m.pose[43] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][1] * 1 / 3, -dircos_limit['shd1_U'] * 1 / 3, -dircos_limit['shd1_L'] * 1 / 3) self.m.pose[44] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][2] * 1 / 3, -dircos_limit['shd2_U'] * 1 / 3, -dircos_limit['shd2_L'] * 1 / 3) self.m.pose[51] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][0] * 2 / 3, dircos_limit['shd0_L'] * 2 / 3, dircos_limit['shd0_U'] * 2 / 3) self.m.pose[52] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][1] * 2 / 3, -dircos_limit['shd1_U'] * 2 / 3, -dircos_limit['shd1_L'] * 2 / 3) self.m.pose[53] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][2] * 2 / 3, -dircos_limit['shd2_U'] * 2 / 3, -dircos_limit['shd2_L'] * 2 / 3) self.m.pose[58] = generator.get_noisy_angle( entry['r_elbow'][1], -dircos_limit['elbow_U'], -dircos_limit['elbow_L']) elif flip_leftright == True: self.m.pose[3] = generator.get_noisy_angle( new_right_hip[0], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[4] = generator.get_noisy_angle( -new_right_hip[1], dircos_limit['hip1_L'], dircos_limit['hip1_U']) self.m.pose[5] = generator.get_noisy_angle( -new_right_hip[2], dircos_limit['hip2_L'], dircos_limit['hip2_U']) self.m.pose[12] = generator.get_noisy_angle( entry['r_knee'][0], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[6] = generator.get_noisy_angle( new_left_hip[0], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[7] = generator.get_noisy_angle( -new_left_hip[1], -dircos_limit['hip1_U'], -dircos_limit['hip1_L']) self.m.pose[8] = generator.get_noisy_angle( -new_left_hip[2], -dircos_limit['hip2_U'], -dircos_limit['hip2_L']) self.m.pose[15] = generator.get_noisy_angle( entry['l_knee'][0], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[39] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][0] * 1 / 3, dircos_limit['shd0_L'] * 1 / 3, dircos_limit['shd0_U'] * 1 / 3) self.m.pose[40] = generator.get_noisy_angle( -entry['r_shoulder_' + angle_type][1] * 1 / 3, dircos_limit['shd1_L'] * 1 / 3, dircos_limit['shd1_U'] * 1 / 3) self.m.pose[41] = generator.get_noisy_angle( -entry['r_shoulder_' + angle_type][2] * 1 / 3, dircos_limit['shd2_L'] * 1 / 3, dircos_limit['shd2_U'] * 1 / 3) self.m.pose[48] = generator.get_noisy_angle( entry['r_shoulder_' + angle_type][0] * 2 / 3, dircos_limit['shd0_L'] * 2 / 3, dircos_limit['shd0_U'] * 2 / 3) self.m.pose[49] = generator.get_noisy_angle( -entry['r_shoulder_' + angle_type][1] * 2 / 3, dircos_limit['shd1_L'] * 2 / 3, dircos_limit['shd1_U'] * 2 / 3) self.m.pose[50] = generator.get_noisy_angle( -entry['r_shoulder_' + angle_type][2] * 2 / 3, dircos_limit['shd2_L'] * 2 / 3, dircos_limit['shd2_U'] * 2 / 3) self.m.pose[55] = generator.get_noisy_angle( -entry['r_elbow'][1], dircos_limit['elbow_L'], dircos_limit['elbow_U']) self.m.pose[42] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][0] * 1 / 3, dircos_limit['shd0_L'] * 1 / 3, dircos_limit['shd0_U'] * 1 / 3) self.m.pose[43] = generator.get_noisy_angle( -entry['l_shoulder_' + angle_type][1] * 1 / 3, -dircos_limit['shd1_U'] * 1 / 3, -dircos_limit['shd1_L'] * 1 / 3) self.m.pose[44] = generator.get_noisy_angle( -entry['l_shoulder_' + angle_type][2] * 1 / 3, -dircos_limit['shd2_U'] * 1 / 3, -dircos_limit['shd2_L'] * 1 / 3) self.m.pose[51] = generator.get_noisy_angle( entry['l_shoulder_' + angle_type][0] * 2 / 3, dircos_limit['shd0_L'] * 2 / 3, dircos_limit['shd0_U'] * 2 / 3) self.m.pose[52] = generator.get_noisy_angle( -entry['l_shoulder_' + angle_type][1] * 2 / 3, -dircos_limit['shd1_U'] * 2 / 3, -dircos_limit['shd1_L'] * 2 / 3) self.m.pose[53] = generator.get_noisy_angle( -entry['l_shoulder_' + angle_type][2] * 2 / 3, -dircos_limit['shd2_U'] * 2 / 3, -dircos_limit['shd2_L'] * 2 / 3) self.m.pose[58] = generator.get_noisy_angle( -entry['l_elbow'][1], -dircos_limit['elbow_U'], -dircos_limit['elbow_L']) print "stuck in loop", self.reset_pose, flip_leftright if self.reset_pose == True: pass else: break from capsule_body import get_capsules, joint2name, rots0 capsules = get_capsules(self.m) joint2name = joint2name rots0 = rots0 print len(capsules) return self.m, capsules, joint2name, rots0
def fix_root(self): import os for gender in ["f", "m"]: #for i in range(51,103): for i in range(1, 103): if i == 7: continue some_subject = '%05d' % (i) onlyfiles = next( os.walk('/home/henry/git/smplify_public/output_' + some_subject))[2] print(len(onlyfiles)) number_files = str(int(len(onlyfiles) * 1 / 2)) new_valid_shape_pose_vol_list = [] try: subject_new_samples = np.load( self.filepath_prefix + "/data/init_poses/slp_identical/identical_bins/identical_shape_pose_vol_" + some_subject + "_" + gender + "_" + number_files + ".npy", allow_pickle=True) except: continue print(len(subject_new_samples)) #for pose_num in range(1, 46):#45 pose per participant. for pose_num in range(0, 45): #45 pose per participant. #here load some subjects joint angle data within danaLab and found by SMPLIFY try: root_angles = np.array( subject_new_samples[pose_num][2][0:3]) * 1. R_root = libKinematics.matrix_from_dir_cos_angles( root_angles) flip_root_euler = np.pi flip_root_R = libKinematics.eulerAnglesToRotationMatrix( [flip_root_euler, 0.0, 0.0]) root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix( [0.0, 0.0, 0.0]) # randomize the root rotation # root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix([0.0, 0.0, 0.0]) #randomize the root rotation dir_cos_root = libKinematics.dir_cos_angles_from_matrix( np.matmul(root_rot_rand_R, np.matmul(R_root, flip_root_R))) print(subject_new_samples[pose_num][2][0:3], dir_cos_root) subject_new_samples[pose_num][2][0] = dir_cos_root[0] subject_new_samples[pose_num][2][1] = dir_cos_root[1] subject_new_samples[pose_num][2][2] = dir_cos_root[2] #print(subject_new_samples[pose_num][2][0:3]) except: #pass print(i, pose_num, ' is missing for subject:', some_subject)
def random_bag_yash_data(self, posture="lay", verbose=True): if posture == "sit": movements = [ 'LL_sitting', 'RL_sitting', 'LH_sitting', 'RH_sitting' ] movement_ct = [150, 150, 200, 200] elif posture == "lay": movements = ['LL', 'RL', 'LH1', 'LH2', 'LH3', 'RH1', 'RH2', 'RH3'] movement_ct = [150, 150, 200, 150, 100, 200, 150, 100] else: movements = [ 'LL', 'RL', 'LH1', 'LH2', 'LH3', 'RH1', 'RH2', 'RH3', 'LL_sitting', 'RL_sitting', 'LH_sitting', 'RH_sitting' ] movement_ct = [ 150, 150, 200, 150, 100, 200, 150, 100, 150, 150, 200, 200 ] #subjects = ['40ESJ', 'GRTJK', 'TX887', 'WFGW9', 'WM9KJ', 'ZV7TE', 'FMNGQ'] subjects = [ '4ZZZQ', '5LDJG', 'A4G4Y', 'G55Q1', 'GF5Q3', 'RQCLC', 'TSQNA', 'WCNOM', 'WE2SZ' ] #'GRTJK', bag = [] for subject in subjects: for movement in range(len(movements)): print "subject: ", subject, " movement: ", movements[movement] filename = "/home/henry/data/init_ik_solutions/subject_" + subject + "/side_up/" + movements[ movement] + "_angles_offset_side_up.p" with open(filename, 'rb') as fp: angles_data = pickle.load(fp) print len(angles_data) num_appended = 0 shuffle(angles_data) while num_appended < movement_ct[movement]: print "beginning!" for entry in angles_data: #print entry['r_hip_euler'] #if num_appended >= movement_ct[movement]: # #print "breaking!" # break if entry['r_shoulder_euler'][0] < -1.8: pass elif entry['r_shoulder_euler'][0] > 2.9: pass elif posture == "sit" and entry['r_shoulder_euler'][ 2] < -1.05: pass elif entry['l_shoulder_euler'][0] < -1.8: pass elif posture == "lay" and entry['l_shoulder_euler'][ 2] > 1.8: pass elif entry['l_shoulder_euler'][0] > 2.9: pass elif posture == "sit" and entry['r_hip_euler'][ 2] < -1.0: pass elif posture == "sit" and entry['l_hip_euler'][ 0] < -2.0: pass elif posture == "sit" and entry['l_hip_euler'][2] > 2.5: pass elif posture == "sit" and entry['r_hip_angle_axis'][ 1] > 0.95: pass elif posture == "lay" and entry['l_hip_euler'][ 0] < -2.0: pass elif posture == "lay" and entry['r_hip_euler'][ 0] < -2.0: pass else: bag.append(entry) num_appended += 1 #print "subject: ", subject, " movement: ", movements[movement], " ct: ", num_appended r_hip_angle_axis_0 = [] r_hip_angle_axis_1 = [] r_hip_angle_axis_2 = [] r_hip_angle_axis_sit_0 = [] r_hip_angle_axis_sit_1 = [] r_hip_angle_axis_sit_2 = [] r_knee_angle_axis_0 = [] l_hip_angle_axis_0 = [] l_hip_angle_axis_1 = [] l_hip_angle_axis_2 = [] l_hip_angle_axis_sit_0 = [] l_hip_angle_axis_sit_1 = [] l_hip_angle_axis_sit_2 = [] l_knee_angle_axis_0 = [] r_shoulder_angle_axis_0 = [] r_shoulder_angle_axis_1 = [] r_shoulder_angle_axis_2 = [] r_elbow_angle_axis_1 = [] l_shoulder_angle_axis_0 = [] l_shoulder_angle_axis_1 = [] l_shoulder_angle_axis_2 = [] l_elbow_angle_axis_1 = [] r_hip_euler_0 = [] r_hip_euler_1 = [] r_hip_euler_2 = [] r_hip_euler_sit_0 = [] r_hip_euler_sit_1 = [] r_hip_euler_sit_2 = [] r_knee_0 = [] l_hip_euler_0 = [] l_hip_euler_1 = [] l_hip_euler_2 = [] l_hip_euler_sit_0 = [] l_hip_euler_sit_1 = [] l_hip_euler_sit_2 = [] l_knee_0 = [] r_shoulder_euler_0 = [] r_shoulder_euler_1 = [] r_shoulder_euler_2 = [] r_elbow_1 = [] l_shoulder_euler_0 = [] l_shoulder_euler_1 = [] l_shoulder_euler_2 = [] l_elbow_1 = [] R_root = libKinematics.eulerAnglesToRotationMatrix( [-np.pi / 3, 0.0, 0.0]) for entry in bag: if posture == "lay": r_hip_angle_axis_0.append(entry['r_hip_angle_axis'][0]) r_hip_angle_axis_1.append(entry['r_hip_angle_axis'][1]) r_hip_angle_axis_2.append(entry['r_hip_angle_axis'][2]) r_hip_euler_0.append(entry['r_hip_euler'][0]) r_hip_euler_1.append(entry['r_hip_euler'][1]) r_hip_euler_2.append(entry['r_hip_euler'][2]) l_hip_angle_axis_0.append(entry['l_hip_angle_axis'][0]) l_hip_angle_axis_1.append(entry['l_hip_angle_axis'][1]) l_hip_angle_axis_2.append(entry['l_hip_angle_axis'][2]) l_hip_euler_0.append(entry['l_hip_euler'][0]) l_hip_euler_1.append(entry['l_hip_euler'][1]) l_hip_euler_2.append(entry['l_hip_euler'][2]) if posture == "sit": #now we have to rotate the limits for the sitting type poses R_r = np.matmul(R_root, entry['r_hip_R']) new_right_hip = libKinematics.dir_cos_angles_from_matrix(R_r) r_hip_angle_axis_sit_0.append(new_right_hip[0]) r_hip_angle_axis_sit_1.append(new_right_hip[1]) r_hip_angle_axis_sit_2.append(new_right_hip[2]) R = libKinematics.matrix_from_dir_cos_angles([ r_hip_angle_axis_sit_0[-1], r_hip_angle_axis_sit_1[-1], r_hip_angle_axis_sit_2[-1] ]) eulers = libKinematics.rotationMatrixToEulerAngles(R) r_hip_euler_sit_0.append(eulers[0]) r_hip_euler_sit_1.append(eulers[1]) r_hip_euler_sit_2.append(eulers[2]) #R_l_hip_rod = libKinematics.matrix_from_dir_cos_angles(entry['l_hip_angle_axis']) R_l = np.matmul(R_root, entry['l_hip_R']) new_left_hip = libKinematics.dir_cos_angles_from_matrix(R_l) l_hip_angle_axis_sit_0.append(new_left_hip[0]) l_hip_angle_axis_sit_1.append(new_left_hip[1]) l_hip_angle_axis_sit_2.append(new_left_hip[2]) R = libKinematics.matrix_from_dir_cos_angles([ l_hip_angle_axis_sit_0[-1], l_hip_angle_axis_sit_1[-1], l_hip_angle_axis_sit_2[-1] ]) eulers = libKinematics.rotationMatrixToEulerAngles(R) l_hip_euler_sit_0.append(eulers[0]) l_hip_euler_sit_1.append(eulers[1]) l_hip_euler_sit_2.append(eulers[2]) r_knee_0.append(entry['r_knee'][0]) l_knee_0.append(entry['l_knee'][0]) r_shoulder_angle_axis_0.append(entry['r_shoulder_angle_axis'][0]) r_shoulder_angle_axis_1.append(entry['r_shoulder_angle_axis'][1]) r_shoulder_angle_axis_2.append(entry['r_shoulder_angle_axis'][2]) r_shoulder_euler_0.append(entry['r_shoulder_euler'][0]) r_shoulder_euler_1.append(entry['r_shoulder_euler'][1]) r_shoulder_euler_2.append(entry['r_shoulder_euler'][2]) #if r_shoulder_euler_0[-1] > 3: print "greater than 3!" r_shoulder_euler_0[-1] l_shoulder_angle_axis_0.append(entry['l_shoulder_angle_axis'][0]) l_shoulder_angle_axis_1.append(entry['l_shoulder_angle_axis'][1]) l_shoulder_angle_axis_2.append(entry['l_shoulder_angle_axis'][2]) l_shoulder_euler_0.append(entry['l_shoulder_euler'][0]) l_shoulder_euler_1.append(entry['l_shoulder_euler'][1]) l_shoulder_euler_2.append(entry['l_shoulder_euler'][2]) r_elbow_1.append(entry['r_elbow'][1]) l_elbow_1.append(entry['l_elbow'][1]) print r_hip_angle_axis_sit_1 print "lower body axis angle: " if posture == "lay": print min(r_hip_angle_axis_0), max(r_hip_angle_axis_0), 'rhip' print min(r_hip_angle_axis_1), max(r_hip_angle_axis_1), 'rhip' print min(r_hip_angle_axis_2), max(r_hip_angle_axis_2), 'rhip' print min(l_hip_angle_axis_0), max(l_hip_angle_axis_0), 'lhip' print min(l_hip_angle_axis_1), max(l_hip_angle_axis_1), 'lhip' print min(l_hip_angle_axis_2), max(l_hip_angle_axis_2), 'lhip' elif posture == "sit": print min(r_hip_angle_axis_sit_0), max( r_hip_angle_axis_sit_0), 'rhip sit' print min(r_hip_angle_axis_sit_1), max( r_hip_angle_axis_sit_1), 'rhip sit' print min(r_hip_angle_axis_sit_2), max( r_hip_angle_axis_sit_2), 'rhip sit' print min(l_hip_angle_axis_sit_0), max( l_hip_angle_axis_sit_0), 'lhip sit' print min(l_hip_angle_axis_sit_1), max( l_hip_angle_axis_sit_1), 'lhip sit' print min(l_hip_angle_axis_sit_2), max( l_hip_angle_axis_sit_2), 'lhip sit' print min(r_knee_0), max(r_knee_0), 'rknee' print min(l_knee_0), max(l_knee_0), 'lknee' print "upper body axis angle: " print min(r_shoulder_angle_axis_0), max( r_shoulder_angle_axis_0), 'rshould' print min(r_shoulder_angle_axis_1), max( r_shoulder_angle_axis_1), 'rshould' print min(r_shoulder_angle_axis_2), max( r_shoulder_angle_axis_2), 'rshould' print min(l_shoulder_angle_axis_0), max( l_shoulder_angle_axis_0), 'lshould' print min(l_shoulder_angle_axis_1), max( l_shoulder_angle_axis_1), 'lshould' print min(l_shoulder_angle_axis_2), max( l_shoulder_angle_axis_2), 'lshould' print min(r_elbow_1), max(r_elbow_1), 'relbow' print min(l_elbow_1), max(l_elbow_1), 'lelbow' print "lower body euler: " if posture == "lay": print min(r_hip_euler_0), max(r_hip_euler_0), 'rhip' print min(r_hip_euler_1), max(r_hip_euler_1), 'rhip' print min(r_hip_euler_2), max(r_hip_euler_2), 'rhip' print min(l_hip_euler_0), max(l_hip_euler_0), 'lhip' print min(l_hip_euler_1), max(l_hip_euler_1), 'lhip' print min(l_hip_euler_2), max(l_hip_euler_2), 'lhip' elif posture == "sit": print min(r_hip_euler_sit_0), max(r_hip_euler_sit_0), 'rhip sit' print min(r_hip_euler_sit_1), max(r_hip_euler_sit_1), 'rhip sit' print min(r_hip_euler_sit_2), max(r_hip_euler_sit_2), 'rhip sit' print min(l_hip_euler_sit_0), max(l_hip_euler_sit_0), 'lhip sit' print min(l_hip_euler_sit_1), max(l_hip_euler_sit_1), 'lhip sit' print min(l_hip_euler_sit_2), max(l_hip_euler_sit_2), 'lhip sit' print "upper body euler: " print min(r_shoulder_euler_0), max(r_shoulder_euler_0), 'rshould' print min(r_shoulder_euler_1), max(r_shoulder_euler_1), 'rshould' print min(r_shoulder_euler_2), max(r_shoulder_euler_2), 'rshould' print min(l_shoulder_euler_0), max(l_shoulder_euler_0), 'lshould' print min(l_shoulder_euler_1), max(l_shoulder_euler_1), 'lshould' print min(l_shoulder_euler_2), max(l_shoulder_euler_2), 'lshould' import matplotlib.pyplot as plt #print r_hip_angle_axis_1 plt.plot(np.arange(len(r_shoulder_angle_axis_1)), r_shoulder_angle_axis_1, 'ko') plt.show() pickle.dump( bag, open( "/home/henry/data/init_ik_solutions/all_test_" + posture + "_angles_side_up.p", "wb"))
def estimate_real_time(self, filename1, filename2=None): pyRender = libRender.pyRenderMesh() mat_size = (64, 27) import sys # insert at 1, 0 is the script path (or '' in REPL) sys.path.insert(1, '../sim_camera_resting_scene/lib_py') sys.path.insert(1, '../sim_camera_resting_scene/DPNet') from unpack_batch_lib_br import UnpackBatchLib import convnet_br as convnet if torch.cuda.is_available(): # Use for GPU GPU = True dtype = torch.cuda.FloatTensor print '######################### CUDA is available! #############################' else: # Use for CPU GPU = False dtype = torch.FloatTensor print '############################## USING CPU #################################' from torch.autograd import Variable model = torch.load(filename1) if GPU == True: for i in range(0, 8): try: model = torch.load( filename1, map_location={'cuda:' + str(i): 'cuda:0'}) model = model.cuda().eval() break except: pass if filename2 is not None: for i in range(0, 8): try: model2 = torch.load( filename2, map_location={'cuda:' + str(i): 'cuda:0'}) model2 = model2.cuda().eval() break except: pass else: model2 = None else: model = torch.load(filename1, map_location='cpu').eval() if filename2 is not None: model2 = torch.load(filename2, map_location='cpu').eval() else: model2 = None pub = rospy.Publisher('meshTopic', MeshAttr) #rospy.init_node('talker', anonymous=False) while not rospy.is_shutdown(): #pmat = np.fliplr(np.flipud(np.clip(self.pressure.reshape(mat_size)*float(self.CTRL_PNL['pmat_mult']*4), a_min=0, a_max=100))) pmat = np.fliplr( np.flipud( np.clip(self.pressure.reshape(mat_size) * float(4), a_min=0, a_max=100))) #print "max is : ", np.max(pmat) #print "sum is : ", np.sum(pmat) if self.CTRL_PNL['cal_noise'] == False: pmat = gaussian_filter(pmat, sigma=0.5) pmat_stack = PreprocessingLib( ).preprocessing_create_pressure_angle_stack_realtime( pmat, self.bedangle, mat_size, return_height=False) print np.shape(pmat_stack) if self.CTRL_PNL['cal_noise'] == False: pmat_stack = np.clip(pmat_stack, a_min=0, a_max=100) pmat_stack = np.array(pmat_stack) if self.CTRL_PNL['incl_pmat_cntct_input'] == True: pmat_contact = np.copy(pmat_stack[:, 0:1, :, :]) pmat_contact[pmat_contact > 0] = 100 pmat_stack = np.concatenate((pmat_contact, pmat_stack), axis=1) weight_input = WEIGHT_LBS / 2.20462 height_input = (HEIGHT_IN * 0.0254 - 1) * 100 batch1 = np.zeros((1, 162)) if GENDER == 'f': batch1[:, 157] += 1 elif GENDER == 'm': batch1[:, 158] += 1 batch1[:, 160] += weight_input batch1[:, 161] += height_input if self.CTRL_PNL['normalize_std'] == True: self.CTRL_PNL['depth_map_input_est'] = False pmat_stack = self.TPL.normalize_network_input( pmat_stack, self.CTRL_PNL) batch1 = self.TPL.normalize_wt_ht(batch1, self.CTRL_PNL) pmat_std_from_mult = [ 'N/A', 11.70153502792190, 19.90905848383454, 23.07018866032369, 0.0, 25.50538629767412 ] if self.CTRL_PNL['cal_noise'] == False: sobel_std_from_mult = [ 'N/A', 29.80360490415032, 33.33532963163579, 34.14427844692501, 0.0, 34.86393494050921 ] else: sobel_std_from_mult = [ 'N/A', 45.61635847182483, 77.74920396659292, 88.89398421073700, 0.0, 97.90075708182506 ] self.CTRL_PNL['norm_std_coeffs'] = [ 1. / 41.80684362163343, # contact 1. / 45.08513083167194, # neg est depth 1. / 43.55800622930469, # cm est 1. / pmat_std_from_mult[int(self.CTRL_PNL['pmat_mult'])], # pmat x5 1. / sobel_std_from_mult[int( self.CTRL_PNL['pmat_mult'])], # pmat sobel 1. / 1.0, # OUTPUT DO NOTHING 1. / 1.0, # OUTPUT DO NOTHING 1. / 30.216647403350, # weight 1. / 14.629298141231 ] # height if self.CTRL_PNL['normalize_std'] == False: for i in range(9): self.CTRL_PNL['norm_std_coeffs'][i] *= 0. self.CTRL_PNL['norm_std_coeffs'][i] += 1. pmat_stack = torch.Tensor(pmat_stack) batch1 = torch.Tensor(batch1) batch = [] batch.append(pmat_stack) batch.append(batch1) self.CTRL_PNL['adjust_ang_from_est'] = False self.CTRL_PNL['recon_map_output'] = True print self.CTRL_PNL['num_input_channels'], batch[0].size( ), 'inputs and batch size' scores, INPUT_DICT, OUTPUT_DICT = UnpackBatchLib().unpack_batch( batch, False, model, self.CTRL_PNL) self.CTRL_PNL['first_pass'] = False mdm_est_pos = OUTPUT_DICT['batch_mdm_est'].clone().unsqueeze( 1) #/ 16.69545796387731 mdm_est_neg = OUTPUT_DICT['batch_mdm_est'].clone().unsqueeze( 1) #/ 45.08513083167194 mdm_est_pos[mdm_est_pos < 0] = 0 mdm_est_neg[mdm_est_neg > 0] = 0 mdm_est_neg *= -1 cm_est = OUTPUT_DICT['batch_cm_est'].clone().unsqueeze( 1) * 100 #/ 43.55800622930469 #1. / 16.69545796387731, # pos est depth #1. / 45.08513083167194, # neg est depth #1. / 43.55800622930469, # cm est if model2 is not None: batch_cor = [] batch_cor.append( torch.cat( (pmat_stack[:, 0:1, :, :], mdm_est_neg.type( torch.FloatTensor), cm_est.type( torch.FloatTensor), pmat_stack[:, 1:, :, :]), dim=1)) if self.CTRL_PNL['full_body_rot'] == False: batch_cor.append( torch.cat( (batch1, OUTPUT_DICT['batch_betas_est'].cpu(), OUTPUT_DICT['batch_angles_est'].cpu(), OUTPUT_DICT['batch_root_xyz_est'].cpu()), dim=1)) elif self.CTRL_PNL['full_body_rot'] == True: batch_cor.append( torch.cat( (batch1, OUTPUT_DICT['batch_betas_est'].cpu(), OUTPUT_DICT['batch_angles_est'].cpu(), OUTPUT_DICT['batch_root_xyz_est'].cpu(), OUTPUT_DICT['batch_root_atan2_est'].cpu()), dim=1)) self.CTRL_PNL['adjust_ang_from_est'] = True self.CTRL_PNL['recon_map_output'] = True scores, INPUT_DICT, OUTPUT_DICT = UnpackBatchLib( ).unpack_batch(batch_cor, False, model2, self.CTRL_PNL) betas_est = np.squeeze( OUTPUT_DICT['batch_betas_est_post_clip'].cpu().numpy()) angles_est = np.squeeze(OUTPUT_DICT['batch_angles_est_post_clip']) root_shift_est = np.squeeze( OUTPUT_DICT['batch_root_xyz_est_post_clip'].cpu().numpy()) #print betas_est.shape, root_shift_est.shape, angles_est.shape #print betas_est, root_shift_est, angles_est angles_est = angles_est.reshape(72) for idx in range(10): #print shape_pose_vol[0][idx] self.m.betas[idx] = betas_est[idx] for idx in range(72): self.m.pose[idx] = angles_est[idx] init_root = np.array(self.m.pose[0:3]) + 0.000001 init_rootR = libKinematics.matrix_from_dir_cos_angles(init_root) root_rot = libKinematics.eulerAnglesToRotationMatrix( [np.pi, 0.0, np.pi / 2]) #print root_rot trans_root = libKinematics.dir_cos_angles_from_matrix( np.matmul(root_rot, init_rootR)) self.m.pose[0] = trans_root[0] self.m.pose[1] = trans_root[1] self.m.pose[2] = trans_root[2] #print self.m.J_transformed[1, :], self.m.J_transformed[4, :] # self.m.pose[51] = selection_r print self.m.r #print OUTPUT_DICT['verts'] pyRender.mesh_render_pose_bed_orig(self.m, root_shift_est, self.point_cloud_array, self.pc_isnew, pmat * 4, self.markers, self.bedangle) self.point_cloud_array = None
def map_slp_to_rand_angles(self, original_pose, alter_angles = True): angle_type = "angle_axis" #angle_type = "euler" dircos_limit = {} dircos_limit['hip0_L'] = -1.8620680158061373 dircos_limit['hip0_U'] = 0.3928991379790361 dircos_limit['hip1_L'] = -0.3002682177448498 dircos_limit['hip1_U'] = 0.4312268766210817 dircos_limit['hip2_L'] = -0.5252221939422439 dircos_limit['hip2_U'] = 0.9361197251810272 dircos_limit['knee_L'] = -0.00246317350132912 dircos_limit['knee_U'] = 2.5615940356353004 dircos_limit['shd00_L'] = -0.44788772883633016 dircos_limit['shd00_U'] = 0.20496654360962563 dircos_limit['shd01_L'] = -1.0991885726395296 dircos_limit['shd01_U'] = 0.6828417105678483 dircos_limit['shd02_L'] = -0.6177946204522845 dircos_limit['shd02_U'] = 0.7288264054368747 dircos_limit['shd10_L'] = -0.8121612985394123 dircos_limit['shd10_U'] = 0.7203413993648013 dircos_limit['shd11_L'] = -1.3427142449685556 dircos_limit['shd11_U'] = 0.4865822031689829 dircos_limit['shd12_L'] = -1.292990223497471 dircos_limit['shd12_U'] = 0.7428419209098167 dircos_limit['elbow_L'] = -2.656264518131647 dircos_limit['elbow_U'] = 0.1873184747130497 print('alter angs',alter_angles) while True: self.reset_pose = False R_root = libKinematics.matrix_from_dir_cos_angles(original_pose[0:3]) flip_root_euler = np.pi flip_root_R = libKinematics.eulerAnglesToRotationMatrix([flip_root_euler, 0.0, 0.0]) root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix([0.0, random.normalvariate(0.0, np.pi/12), random.normalvariate(0.0, np.pi/12)]) #randomize the root rotation #root_rot_rand_R = libKinematics.eulerAnglesToRotationMatrix([0.0, 0.0, 0.0]) #randomize the root rotation dir_cos_root = libKinematics.dir_cos_angles_from_matrix(np.matmul(root_rot_rand_R, np.matmul(R_root, flip_root_R))) #R_root2 = libKinematics.matrix_from_dir_cos_angles([original_pose[0]-4*np.pi, original_pose[1], original_pose[2]]) #dir_cos_root2 = libKinematics.dir_cos_angles_from_matrix(R_root2) #print('eulers2', libKinematics.rotationMatrixToEulerAngles(R_root2)) self.m.pose[0] = dir_cos_root[0] self.m.pose[1] = -dir_cos_root[1] self.m.pose[2] = dir_cos_root[2] #print(original_pose[0:3]) #print(dir_cos_root) #print(dir_cos_root2) self.m.pose[3] = generator.get_noisy_angle_hard_limit(original_pose[3], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[4] = generator.get_noisy_angle_hard_limit(original_pose[4], dircos_limit['hip1_L'], dircos_limit['hip1_U']) self.m.pose[5] = generator.get_noisy_angle_hard_limit(original_pose[5], dircos_limit['hip2_L'], dircos_limit['hip2_U']) self.m.pose[12] = generator.get_noisy_angle_hard_limit(original_pose[12], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[13] = original_pose[13] self.m.pose[14] = original_pose[14] self.m.pose[6] = generator.get_noisy_angle_hard_limit(original_pose[6], dircos_limit['hip0_L'], dircos_limit['hip0_U']) self.m.pose[7] = generator.get_noisy_angle_hard_limit(original_pose[7], -dircos_limit['hip1_U'], -dircos_limit['hip1_L']) self.m.pose[8] = generator.get_noisy_angle_hard_limit(original_pose[8], -dircos_limit['hip2_U'], -dircos_limit['hip2_L']) self.m.pose[15] = generator.get_noisy_angle_hard_limit(original_pose[15], dircos_limit['knee_L'], dircos_limit['knee_U']) self.m.pose[16] = original_pose[16] self.m.pose[17] = original_pose[17] self.m.pose[9] = original_pose[9] #stomach self.m.pose[10] = original_pose[10] #stomach self.m.pose[11] = original_pose[11] #stomach self.m.pose[18] = original_pose[18]#chest self.m.pose[19] = original_pose[19]#chest self.m.pose[20] = original_pose[20]#chest self.m.pose[21] = original_pose[21]#l ankle self.m.pose[22] = original_pose[22]#l ankle self.m.pose[23] = original_pose[23]#l ankle self.m.pose[24] = original_pose[24]#r ankle self.m.pose[25] = original_pose[25]#r ankle self.m.pose[26] = original_pose[26]#r ankle self.m.pose[27] = original_pose[27]#sternum self.m.pose[28] = original_pose[28]#sternum self.m.pose[29] = original_pose[29]#stermum self.m.pose[30] = original_pose[30]#l foot self.m.pose[31] = original_pose[31]#l foot self.m.pose[32] = original_pose[32]#l foot self.m.pose[33] = original_pose[33]#r foot self.m.pose[34] = original_pose[34]#r foot self.m.pose[35] = original_pose[35]#r foot self.m.pose[36] = original_pose[36]#neck self.m.pose[37] = original_pose[37]#neck self.m.pose[38] = original_pose[38]#neck self.m.pose[45] = original_pose[45]#head self.m.pose[46] = original_pose[46]#head self.m.pose[47] = original_pose[47]#head self.m.pose[39] = generator.get_noisy_angle_hard_limit(original_pose[39], dircos_limit['shd00_L'], dircos_limit['shd00_U']) self.m.pose[40] = generator.get_noisy_angle_hard_limit(original_pose[40], dircos_limit['shd01_L'], dircos_limit['shd01_U']) self.m.pose[41] = generator.get_noisy_angle_hard_limit(original_pose[41], dircos_limit['shd02_L'], dircos_limit['shd02_U']) self.m.pose[42] = generator.get_noisy_angle_hard_limit(original_pose[42], dircos_limit['shd00_L'], dircos_limit['shd00_U']) self.m.pose[43] = generator.get_noisy_angle_hard_limit(original_pose[43], -dircos_limit['shd01_U'], -dircos_limit['shd01_L']) self.m.pose[44] = generator.get_noisy_angle_hard_limit(original_pose[44], -dircos_limit['shd02_U'], -dircos_limit['shd02_L']) self.m.pose[54] = original_pose[54] self.m.pose[55] = generator.get_noisy_angle_hard_limit(original_pose[55], dircos_limit['elbow_L'], dircos_limit['elbow_U']) self.m.pose[56] = original_pose[56] self.m.pose[48] = generator.get_noisy_angle_hard_limit(original_pose[48], dircos_limit['shd10_L'], dircos_limit['shd10_U']) self.m.pose[49] = generator.get_noisy_angle_hard_limit(original_pose[49], dircos_limit['shd11_L'], dircos_limit['shd11_U']) self.m.pose[50] = generator.get_noisy_angle_hard_limit(original_pose[50], dircos_limit['shd12_L'], dircos_limit['shd12_U']) self.m.pose[51] = generator.get_noisy_angle_hard_limit(original_pose[51], dircos_limit['shd10_L'], dircos_limit['shd10_U']) self.m.pose[52] = generator.get_noisy_angle_hard_limit(original_pose[52], -dircos_limit['shd11_U'], -dircos_limit['shd11_L']) self.m.pose[53] = generator.get_noisy_angle_hard_limit(original_pose[53], -dircos_limit['shd12_U'], -dircos_limit['shd12_L']) self.m.pose[57] = original_pose[57] self.m.pose[58] = generator.get_noisy_angle_hard_limit(original_pose[58], -dircos_limit['elbow_U'], -dircos_limit['elbow_L']) self.m.pose[59] = original_pose[59] for i in range(60, 72): self.m.pose[i] = original_pose[i] print "stuck in loop", self.reset_pose if self.reset_pose == True: pass else: break from capsule_body import get_capsules, joint2name, rots0 capsules = get_capsules(self.m) joint2name = joint2name rots0 = rots0 print len(capsules) return self.m, capsules, joint2name, rots0