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 get_max_min_of_init(self):
filepath_prefix = '/home/henry/data/init_poses/'
angles_list = []
eulerangles_list = []
for filename in [
'valid_shape_pose_vol_f_lay_4000_leftside_stiff.npy',
'valid_shape_pose_vol_f_lay_4000_lowerbody_stiff.npy',
'valid_shape_pose_vol_f_lay_4000_none_stiff.npy',
'valid_shape_pose_vol_f_lay_4000_rightside_stiff.npy',
'valid_shape_pose_vol_f_lay_4000_upperbody_stiff.npy',
'valid_shape_pose_vol_f_sit_2000_leftside_stiff.npy',
'valid_shape_pose_vol_f_sit_2000_lowerbody_stiff.npy',
'valid_shape_pose_vol_f_sit_2000_none_stiff.npy',
'valid_shape_pose_vol_f_sit_2000_rightside_stiff.npy',
'valid_shape_pose_vol_f_sit_2000_upperbody_stiff.npy',
'valid_shape_pose_vol_m_lay_4000_leftside_stiff.npy',
'valid_shape_pose_vol_m_lay_4000_lowerbody_stiff.npy',
'valid_shape_pose_vol_m_lay_4000_none_stiff.npy',
'valid_shape_pose_vol_m_lay_4000_rightside_stiff.npy',
'valid_shape_pose_vol_m_lay_4000_upperbody_stiff.npy',
'valid_shape_pose_vol_m_sit_2000_leftside_stiff.npy',
'valid_shape_pose_vol_m_sit_2000_lowerbody_stiff.npy',
'valid_shape_pose_vol_m_sit_2000_none_stiff.npy',
'valid_shape_pose_vol_m_sit_2000_rightside_stiff.npy',
'valid_shape_pose_vol_m_sit_2000_upperbody_stiff.npy'
]:
prechecked_pose_list = np.load(filepath_prefix + filename)
for shape_pose in prechecked_pose_list:
#print shape_pose[2]
#print len(shape_pose[2])
angles = np.zeros((72, 1))
for idx in range(len(shape_pose[1])):
angles[shape_pose[1][idx], 0] = shape_pose[2][idx]
angles_list.append(angles)
angles_reshaped = angles_list[-1].reshape(24, 3)
#print angles_reshaped, 'DIR COS'
angles_euler = []
for joint in range(24):
#print angles_reshaped[joint, :]
R = libKinematics.matrix_from_dir_cos_angles(
angles_reshaped[joint, :] + 0.0000001)
eulers = libKinematics.rotationMatrixToEulerAngles(R)
angles_euler.append(eulers)
angles_euler = np.array(angles_euler).reshape(72)
#print angles_euler.reshape(24, 3), 'EULERS'
eulerangles_list.append(angles_euler)
angles_list = np.array(angles_list)
angles_list[np.abs(angles_list) < 0.00001] = 0.0
print angles_list.shape
print np.amin(angles_list, axis=0).reshape(24, 3)
print np.amax(angles_list, axis=0).reshape(24, 3)
eulerangles_list = np.array(eulerangles_list)
eulerangles_list[np.abs(eulerangles_list) < 0.00001] = 0.0
print eulerangles_list.shape
print np.amin(eulerangles_list, axis=0).reshape(24, 3)
print np.amax(eulerangles_list, axis=0).reshape(24, 3)
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 get_max_min_of_resting(self):
def load_pickle(filename):
with open(filename, 'rb') as f:
return pickle.load(f)
filepath_prefix = '/home/henry/data/synth/'
angles_list = []
eulerangles_list = []
for filename in [
'train_f_lay_3555_upperbody_stiff.p',
'train_f_sit_1513_leftside_stiff.p',
'train_m_lay_3841_none_stiff.p',
'train_f_lay_3681_rightside_stiff.p',
'train_f_sit_1534_rightside_stiff.p',
'train_m_sit_1259_rightside_stiff.p',
'train_f_lay_3722_leftside_stiff.p',
'train_f_sit_1649_none_stiff.p',
'train_m_sit_1275_lowerbody_stiff.p',
'train_f_lay_3808_lowerbody_stiff.p',
'train_m_lay_3573_upperbody_stiff.p',
'train_m_sit_1302_leftside_stiff.p',
'train_f_lay_3829_none_stiff.p',
'train_m_lay_3628_rightside_stiff.p',
'train_m_sit_1302_upperbody_stiff.p',
'train_f_sit_1494_lowerbody_stiff.p',
'train_m_lay_3646_leftside_stiff.p',
'train_m_sit_1414_none_stiff.p',
'train_f_sit_1508_upperbody_stiff.p',
'train_m_lay_3735_lowerbody_stiff.p'
]:
file = load_pickle(filepath_prefix + filename)
for entry in range(len(file['joint_angles'])):
angles_list.append(file['joint_angles'][entry][0:72])
angles_reshaped = angles_list[-1].reshape(24, 3)
#print angles_reshaped, 'DIR COS'
angles_euler = []
for joint in range(24):
#print angles_reshaped[joint, :]
R = libKinematics.matrix_from_dir_cos_angles(
angles_reshaped[joint, :] + 0.0000001)
eulers = libKinematics.rotationMatrixToEulerAngles(R)
angles_euler.append(eulers)
angles_euler = np.array(angles_euler).reshape(72)
#print angles_euler.reshape(24, 3), 'EULERS'
eulerangles_list.append(angles_euler)
angles_list = np.array(angles_list)
print angles_list.shape
print np.amin(angles_list, axis=0).reshape(24, 3)
print np.amax(angles_list, axis=0).reshape(24, 3)
eulerangles_list = np.array(eulerangles_list)
print eulerangles_list.shape
print np.amin(eulerangles_list, axis=0).reshape(24, 3)
print np.amax(eulerangles_list, axis=0).reshape(24, 3)
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 run_sim_step(self,
pmat_red_list=[],
force_loc_red_dart=[],
force_dir_red_dart=[],
pmat_idx_red_dart=[],
nearest_capsule_list=[],
kill_dart_vel=False):
self.world.step()
if kill_dart_vel == True:
self.world.skeletons[0].reset_momentum()
max_vel = 0.0
max_acc = 0.0
skel = self.world.skeletons[0]
force_dir_red_dart = (np.multiply(
np.asarray(force_dir_red_dart),
np.expand_dims(np.asarray(pmat_red_list), axis=1))) / 10
nearest_capsules = nearest_capsule_list
force_dir_list = [[]]
pmat_idx_list = [[]]
force_loc_list = [[]]
force_vel_list = [[]]
for idx in range(len(nearest_capsules)):
force_dir_list[nearest_capsules[idx]].append(
force_dir_red_dart[idx])
pmat_idx_list[nearest_capsules[idx]].append(pmat_idx_red_dart[idx])
force_loc_list[nearest_capsules[idx]].append(
force_loc_red_dart[idx])
#filter the acceleration vectors
accel_vectors = []
for item in range(len(force_dir_list)): \
accel_vectors.append(skel.bodynodes[item].com_linear_acceleration())
accel_vectors = np.array(accel_vectors).flatten()
accel_vectors_filtered = np.copy(accel_vectors)
for i in range(len(force_dir_list) * 3):
accel_vectors_filtered[i], self.zi[i] = signal.lfilter(
self.b[i], self.a[i], [accel_vectors[i]], zi=self.zi[i])
accel_vectors_filtered = accel_vectors_filtered.reshape(
len(accel_vectors.tolist()) / 3, 3)
#time3 = time() - time2 - time1 - time0
print accel_vectors, accel_vectors_filtered
active_bn_list = []
active_force_resultant_COM_list = []
active_moment_at_COM_list = []
for item in range(len(force_dir_list)):
#print "linear v", skel.bodynodes[item].com_linear_velocity()
#if item not in max_vel_withhold and np.linalg.norm(skel.bodynodes[item].com_linear_velocity()) > max_vel:
if np.linalg.norm(
skel.bodynodes[item].com_linear_velocity()) > max_vel:
max_vel = np.linalg.norm(
skel.bodynodes[item].com_linear_velocity())
if np.linalg.norm(accel_vectors_filtered[item]) > max_acc:
max_acc = np.linalg.norm(accel_vectors_filtered[item])
if len(force_dir_list[item]) is not 0:
item, len(force_dir_list[item])
# find the sum of forces and the moment about the center of mass of each capsule
#COM = skel.bodynodes[item].C + [0.96986 / DART_TO_FLEX_CONV, 2.4 / DART_TO_FLEX_CONV, self.STARTING_HEIGHT / DART_TO_FLEX_CONV]
COM = skel.bodynodes[item].C + [
1.185 / DART_TO_FLEX_CONV, 2.55 / DART_TO_FLEX_CONV,
self.STARTING_HEIGHT / DART_TO_FLEX_CONV
]
#print item
#print self.pmat_idx_list_prev[item], pmat_idx_list[item]
#print self.force_dir_list_prev[item]
#print "dir:", len(force_dir_list[item]), force_dir_list[item]
#Calculate the spring force
##PARTICLE BASIS##
f_spring = K * np.asarray(force_dir_list[item]) + np.asarray(
[0.00001, 0.00001, 0.00001])
force_spring_COM = np.sum(f_spring, axis=0)
#Calculate the damping force
##PARTICLE BASIS##
f_damping = LibDartSkel().get_particle_based_damping_force(
pmat_idx_list, self.pmat_idx_list_prev, force_dir_list,
self.force_dir_list_prev, force_vel_list, item, B)
force_damping_COM = np.sum(f_damping, axis=0)
##CAPSULE BASIS##
#force_damping_COM = - B*skel.bodynodes[item].com_linear_velocity()
#Calculate the friction force
##PARTICLE BASIS##
f_normal = f_spring + f_damping
V_capsule = skel.bodynodes[item].com_linear_velocity(
) + np.asarray([0.00001, 0.00001, 0.00001])
f_friction = LibDartSkel().get_particle_based_friction_force(
f_normal, V_capsule, FRICTION_COEFF)
force_friction_COM = np.sum(f_friction, axis=0)
##CAPSULE BASIS##
#force_friction_COM = LibDartSkel().get_capsule_based_friction_force(skel.bodynodes[item], force_spring_COM, force_damping_COM, FRICTION_COEFF)
#
force_resultant_COM = force_spring_COM + force_damping_COM + force_friction_COM
print force_resultant_COM, "F R COM"
force_resultant_COM_filtered = np.copy(force_resultant_COM)
for i in range(3):
force_resultant_COM[i], self.ziF[i] = signal.lfilter(
self.bF[i],
self.aF[i], [force_resultant_COM[i]],
zi=self.ziF[i])
print force_resultant_COM
#Calculate the moment arm
d_forces = force_loc_list[item] - COM
#Calculate the moment
##PARTICLE BASIS##
moments = np.cross(d_forces, f_normal) #+f_friction
##CAPSULE BASIS##
#moments = np.cross(d_forces, f_spring)
moment_at_COM = np.sum(moments, axis=0)
active_bn_list.append(item)
active_force_resultant_COM_list.append(force_resultant_COM)
active_moment_at_COM_list.append(moment_at_COM)
LibDartSkel().impose_force(skel=skel,
body_node=item,
force=force_resultant_COM,
offset_from_centroid=np.asarray(
[0.0, 0.0, 0.0]),
cap_offsets=self.cap_offsets,
render=False,
init=False)
#LibDartSkel().impose_torque(skel=skel, body_node=item, torque=moment_at_COM, init=False)
#time4 = time() - time3 - time2 - time1 - time0
#now apply the forces and step through dart for some repeated number of times. not particularly fast. expect 20 ms for
for step in range(self.num_dart_steps - 1):
self.world.step()
for active_bn in range(len(active_bn_list)):
LibDartSkel().impose_force(
skel=skel,
body_node=active_bn_list[active_bn],
force=active_force_resultant_COM_list[active_bn],
offset_from_centroid=np.asarray([0.0, 0.0, 0.0]),
cap_offsets=self.cap_offsets,
render=False,
init=False)
#LibDartSkel().impose_torque(skel=skel, body_node=active_bn_list[active_bn], torque=active_moment_at_COM_list[active_bn], init=False)
#print "dart timing", time1, time2, time3, time4, time() - time4-time3-time2-time1-time0
#this root joint position will tell us how to shift the root when we remesh the capsule model
#root_joint_pos = [skel.bodynodes[0].C[0] - self.cap_offsets[0][0]*np.cos(skel.q[2]) + self.cap_offsets[0][1]*np.sin(skel.q[2]),
# skel.bodynodes[0].C[1] - self.cap_offsets[0][0]*np.sin(skel.q[2]) - self.cap_offsets[0][1]*np.cos(skel.q[2])]
#print skel.bodynodes[0].C
#print skel.q[0:3], "FIRST 3 ANGLES"
#print root_joint_pos, 'old root pos'
Trans1 = lib_kinematics.matrix_from_dir_cos_angles(skel.q[0:3])
dist = np.matmul(Trans1, np.array([0.0, -0.04, 0.0]))
print skel.bodynodes[0].C, skel.bodynodes[0].m
root_joint_pos = skel.bodynodes[0].C - dist
root_joint_pos[2] += self.STARTING_HEIGHT / DART_TO_FLEX_CONV
print root_joint_pos
#print "appending time", time() - time_orig
#LibDartSkel().impose_force(skel=skel, body_node=self.body_node, force=self.force, offset_from_centroid=self.offset_from_centroid, cap_offsets=self.cap_offsets, render=False, init=False)
self.force_dir_list_prev = force_dir_list
self.pmat_idx_list_prev = pmat_idx_list
self.force_loc_list_prev = force_loc_list
if self.step_num == 0:
self.world.check_collision()
contact_check_bns = [4, 5, 7, 8, 16, 17, 18, 19]
#for contact_set in self.world.collision_result.contact_sets:
# if contact_set[0] in contact_check_bns or contact_set[1] in contact_check_bns: # consider removing spine 3 and upper legs
print self.world.collision_result.contact_sets
#sleep(1)
self.step_num += 1
return skel.q, skel.bodynodes, root_joint_pos, max_vel, max_acc
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
