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 map_random_selection_to_smpl_angles(self, alter_angles):
if alter_angles == True:
selection_r_leg = ProcessYashData().sample_angles('r_leg')
self.m.pose[6] = selection_r_leg['rG_ext']
self.m.pose[7] = selection_r_leg['rG_yaw'] #/2
self.m.pose[8] = selection_r_leg['rG_abd']
self.m.pose[15] = selection_r_leg['rK']
#self.m.pose[16] = selection_r_leg['rG_yaw']/2
selection_l_leg = ProcessYashData().sample_angles('l_leg')
self.m.pose[3] = selection_l_leg['lG_ext']
self.m.pose[4] = selection_l_leg['lG_yaw'] #/2
self.m.pose[5] = selection_l_leg['lG_abd']
self.m.pose[12] = selection_l_leg['lK']
#self.m.pose[13] = selection_l_leg['lG_yaw']/2
selection_r_arm = ProcessYashData().sample_angles('r_arm')
self.m.pose[51] = selection_r_arm['rS_roll'] * 2 / 3
self.m.pose[52] = selection_r_arm['rS_yaw'] * 2 / 3
self.m.pose[53] = selection_r_arm['rS_pitch'] * 2 / 3
self.m.pose[42] = selection_r_arm['rS_roll'] * 1 / 3
self.m.pose[43] = selection_r_arm['rS_yaw'] * 1 / 3
self.m.pose[44] = selection_r_arm['rS_pitch'] * 1 / 3
self.m.pose[58] = selection_r_arm['rE']
selection_l_arm = ProcessYashData().sample_angles('l_arm')
self.m.pose[48] = selection_l_arm['lS_roll'] * 2 / 3
self.m.pose[49] = selection_l_arm['lS_yaw'] * 2 / 3
self.m.pose[50] = selection_l_arm['lS_pitch'] * 2 / 3
self.m.pose[39] = selection_l_arm['lS_roll'] * 1 / 3
self.m.pose[40] = selection_l_arm['lS_yaw'] * 1 / 3
self.m.pose[41] = selection_l_arm['lS_pitch'] * 1 / 3
self.m.pose[55] = selection_l_arm['lE']
#self.m.pose[51] = selection_r
from capsule_body import get_capsules, joint2name, rots0
capsules = get_capsules(self.m)
joint2name = joint2name
rots0 = rots0
print len(capsules)
#put these capsules into dart based on these angles. Make Dart joints only as necessary.
#Use the positions found in dart to update positions in FleX. Do not use angles in Flex
#repeat: do not need a forward kinematics model in FleX! Just need the capsule positions and radii. Can potentially get rotation from the Capsule end positions.
#Find IK solution at the very end.
return self.m, capsules, joint2name, rots0
def on_changed(self, which):
if 'regs' in which:
self.length_regs = self.regs['betas2lens']
self.rad_regs = self.regs['betas2rads']
if 'betas' in which:
if not hasattr(self, 'capsules'):
self.capsules = get_capsules(self.model,
wrt_betas=self.betas,
length_regs=self.length_regs,
rad_regs=self.rad_regs)
self.update_capsules_and_centers()
if 'pose' in which:
self.update_pose()
def map_random_selection_to_smpl_angles(self, alter_angles):
if alter_angles == True:
selection_r_leg = ProcessYashData().sample_angles('r_leg')
self.m.pose[6] = selection_r_leg['rG_ext']
self.m.pose[7] = selection_r_leg['rG_yaw'] #/2
self.m.pose[8] = selection_r_leg['rG_abd']
self.m.pose[15] = selection_r_leg['rK']
#self.m.pose[16] = selection_r_leg['rG_yaw']/2
selection_l_leg = ProcessYashData().sample_angles('l_leg')
self.m.pose[3] = selection_l_leg['lG_ext']
self.m.pose[4] = selection_l_leg['lG_yaw'] #/2
self.m.pose[5] = selection_l_leg['lG_abd']
self.m.pose[12] = selection_l_leg['lK']
#self.m.pose[13] = selection_l_leg['lG_yaw']/2
selection_r_arm = ProcessYashData().sample_angles('r_arm')
self.m.pose[51] = selection_r_arm['rS_roll'] * 2 / 3
self.m.pose[52] = selection_r_arm['rS_yaw'] * 2 / 3
self.m.pose[53] = selection_r_arm['rS_pitch'] * 2 / 3
self.m.pose[42] = selection_r_arm['rS_roll'] * 1 / 3
self.m.pose[43] = selection_r_arm['rS_yaw'] * 1 / 3
self.m.pose[44] = selection_r_arm['rS_pitch'] * 1 / 3
self.m.pose[58] = selection_r_arm['rE']
selection_l_arm = ProcessYashData().sample_angles('l_arm')
self.m.pose[48] = selection_l_arm['lS_roll'] * 2 / 3
self.m.pose[49] = selection_l_arm['lS_yaw'] * 2 / 3
self.m.pose[50] = selection_l_arm['lS_pitch'] * 2 / 3
self.m.pose[39] = selection_l_arm['lS_roll'] * 1 / 3
self.m.pose[40] = selection_l_arm['lS_yaw'] * 1 / 3
self.m.pose[41] = selection_l_arm['lS_pitch'] * 1 / 3
self.m.pose[55] = selection_l_arm['lE']
#self.m.pose[51] = selection_r
from capsule_body import get_capsules, joint2name, rots0
capsules = get_capsules(self.m)
joint2name = joint2name
rots0 = rots0
return self.m, capsules, joint2name, rots0
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 map_yash_to_smpl_angles(self, verbose=True):
movements = ['LL', 'RL', 'LH1', 'LH2', 'LH3', 'RH1', 'RH2', 'RH3']
subjects = [
'40ESJ', 'GRTJK', 'TX887', 'WFGW9', 'WM9KJ', 'ZV7TE'
'FMNGQ'
]
for subject in subjects:
for movement in movements:
print "subject: ", subject, " movement: ", movement
filename = "/home/henry/pressure_mat_angles/subject_" + subject + "/" + movement + "_angles.p"
with open(filename, 'rb') as fp:
angles_data = pickle.load(fp)
for entry in angles_data:
hip_dir_cos = (entry['r_hip_angle_axis'])
print hip_dir_cos
print entry['r_hip_angle_axis']
#entry = angles_data[50]
self.m.pose[6] = entry['r_hip_angle_axis'][0]
self.m.pose[7] = entry['r_hip_angle_axis'][1] #/2
self.m.pose[8] = entry['r_hip_angle_axis'][2]
if verbose == True: print 'r hip', self.m.pose[6:9]
self.m.pose[15] = entry['r_knee_angle_axis'][0]
#self.m.pose[16] = entry['r_hip_angle_axis'][1]
if verbose == True: print 'r knee', self.m.pose[15:18]
self.m.pose[3] = entry['l_hip_angle_axis'][0]
self.m.pose[4] = entry['l_hip_angle_axis'][1] #/2
self.m.pose[5] = entry['l_hip_angle_axis'][2]
if verbose == True: print 'l hip', self.m.pose[3:6]
self.m.pose[12] = entry['l_knee_angle_axis'][0]
#self.m.pose[13] = entry['l_hip_angle_axis'][1]
if verbose == True: print 'l knee', self.m.pose[12:15]
self.m.pose[51] = entry['r_shoulder_angle_axis'][0] * 2 / 3
self.m.pose[52] = entry['r_shoulder_angle_axis'][1] * 2 / 3
self.m.pose[53] = entry['r_shoulder_angle_axis'][2] * 2 / 3
self.m.pose[42] = entry['r_shoulder_angle_axis'][0] * 1 / 3
self.m.pose[43] = entry['r_shoulder_angle_axis'][1] * 1 / 3
self.m.pose[44] = entry['r_shoulder_angle_axis'][2] * 1 / 3
if verbose == True:
print 'r shoulder', self.m.pose[51:54] + self.m.pose[
42:45]
self.m.pose[58] = entry['r_elbow_angle_axis'][1]
if verbose == True: print 'r elbow', self.m.pose[57:60]
self.m.pose[48] = entry['l_shoulder_angle_axis'][0] * 2 / 3
self.m.pose[49] = entry['l_shoulder_angle_axis'][1] * 2 / 3
self.m.pose[50] = entry['l_shoulder_angle_axis'][2] * 2 / 3
self.m.pose[39] = entry['l_shoulder_angle_axis'][0] * 1 / 3
self.m.pose[40] = entry['l_shoulder_angle_axis'][1] * 1 / 3
self.m.pose[41] = entry['l_shoulder_angle_axis'][2] * 1 / 3
if verbose == True:
print 'l shoulder', self.m.pose[48:51] + self.m.pose[
39:42]
self.m.pose[55] = entry['l_elbow_angle_axis'][1]
if verbose == True: print 'l elbow', self.m.pose[54:57]
break
break
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 __init__(self,
render,
STARTING_HEIGHT,
input_flex_radius,
input_flex_length,
input_flex_mass,
shiftSIDE=0.0,
shiftUD=0.0):
model_path = '/home/henry/git/SMPL_python_v.1.0.0/smpl/models/basicModel_f_lbs_10_207_0_v1.0.0.pkl'
m = load_model(model_path)
regs = np.load(
'/home/henry/git/smplify_public/code/models/regressors_locked_normalized_male.npz'
)
length_regs = regs['betas2lens']
rad_regs = regs['betas2rads']
betas = m.betas
capsules_median = get_capsules(m, betas * 0, length_regs, rad_regs)
capsules = get_capsules(m, betas, length_regs, rad_regs)
joint_names = joint2name
initial_rots = rots0
self.num_steps = 10000
self.render_dart = render
self.ct = 0
self.num_dart_steps = 4
self.has_reset_velocity1 = False
self.has_reset_velocity2 = False
joint_ref = list(m.kintree_table[1]) #joints
parent_ref = list(m.kintree_table[0]) #parent of each joint
parent_ref[0] = -1
self.capsules = capsules
pydart.init(verbose=True)
print('pydart initialization OK')
self.world = pydart.World(
0.0103 / 4, "EMPTY"
) #0.002 is what works well. 0.0103 is when the velocity aligns. thus flex is 0.0103/0.0020 = 5.15x more fast than dart
self.world.set_gravity([0, 0, GRAVITY]) #([0, 0, -9.81])
self.world.set_collision_detector(detector_type=2)
self.world.add_empty_skeleton(_skel_name="human")
self.force_dir_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
self.pmat_idx_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
self.force_loc_list_prev = [[], [], [], [], [], [], [], [], [], [], [],
[], [], [], [], [], [], [], [], []]
joint_root_loc = np.asarray(np.transpose(capsules[0].t)[0])
self.step_num = 0
joint_locs = []
capsule_locs = []
joint_locs_abs = []
joint_locs_trans_abs = []
capsule_locs_abs = []
mJ = np.asarray(m.J)
mJ_transformed = np.asarray(m.J_transformed)
shift = [shiftSIDE, shiftUD, 0.0]
red_joint_ref = joint_ref[0:20] #joints
red_parent_ref = parent_ref[0:20] #parent of each joint
red_parent_ref[10] = 9 #fix neck
red_parent_ref[11] = 9 #fix l inner shoulder
red_parent_ref[13] = 10 #fix head
red_parent_ref[14] = 11 #fix l outer shoulder
red_parent_ref[15] = 12 #fix r outer shoulder
red_parent_ref[16] = 14 #fix l elbow
red_parent_ref[17] = 15 #fix r elbow
head_ref = [10, 13]
leg_cap_ref = [1, 2, 4, 5]
foot_ref = [7, 8]
l_arm_ref = [11, 14, 16, 18]
r_arm_ref = [12, 15, 17, 19]
self.red_joint_ref = [joint_ref[0]]
self.red_parent_ref = red_parent_ref
#make lists of the locations of the joint locations and the smplify capsule initial ends
for i in range(np.shape(mJ)[0]):
if i == 0:
joint_locs.append(list(mJ[0, :] - mJ[0, :] + shift))
joint_locs_abs.append(list(mJ[0, :] - mJ[0, :]))
joint_locs_trans_abs.append(
list(mJ_transformed[0, :] - mJ_transformed[0, :]))
if i < 20:
capsule_locs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
capsule_locs_abs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
print(capsule_locs_abs, "caps locs abs")
else:
joint_locs.append(list(mJ[i, :] - mJ[parent_ref[i], :]))
joint_locs_abs.append(list(mJ[i, :] - mJ[0, :]))
joint_locs_trans_abs.append(
list(mJ_transformed[i, :] - mJ_transformed[0, :]))
if i < 20:
capsule_locs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
np.asarray(
np.transpose(capsules[red_parent_ref[i]].t)[0])
))
capsule_locs_abs.append(
list(
np.asarray(np.transpose(capsules[i].t)[0]) -
joint_root_loc))
capsule_locs_abs[i][0] += np.abs(
float(capsules[0].length[0])) / 2
if i in [
1, 2
]: #shift over the legs relative to where the pelvis mid capsule is
capsule_locs[i][0] += np.abs(
float(capsules[0].length[0])) / 2
if i in [
3, 6, 9
]: #shift over the torso segments relative to their length and their parents length to match the mid capsule
capsule_locs[i][0] -= (
np.abs(float(capsules[i].length[0])) -
np.abs(float(
capsules[red_parent_ref[i]].length[0]))) / 2
if i in [
10, 11, 12
]: #shift over the inner shoulders and neck to match the middle of the top spine capsule
capsule_locs[i][0] += np.abs(
float(capsules[red_parent_ref[i]].length[0])) / 2
if i in [
3, 6, 9
]: #shift over everything in the abs list to match the root
capsule_locs_abs[i][0] -= np.abs(
float(capsules[i].length[0])) / 2
del (joint_locs[10])
del (joint_locs[10])
del (joint_locs_abs[10])
del (joint_locs_abs[10])
self.joint_locs = joint_locs
count = 0
root_joint_type = "FREE"
self.cap_offsets = []
self.cap_init_rots = []
lowest_points = []
for capsule in capsules:
print "************* Capsule No.", count, joint_names[
count], " joint ref: ", red_joint_ref[
count], " parent_ref: ", red_parent_ref[
count], " ****************"
cap_rad = input_flex_radius
cap_len = input_flex_length
cap_init_rot = list(np.asarray(initial_rots[count]))
joint_loc = joint_locs[count]
joint_loc_abs = joint_locs_abs[count]
capsule_loc = capsule_locs[count]
capsule_loc_abs = capsule_locs_abs[count]
cap_offset = [0., 0., 0.]
if count in leg_cap_ref:
cap_offset[1] = -cap_len / 2
if count in foot_ref: cap_offset[2] = cap_len / 2
if count in l_arm_ref: cap_offset[0] = cap_len / 2
if count in r_arm_ref: cap_offset[0] = -cap_len / 2
#if count in head_ref: cap_offset[1] = cap_len/2
cap_offset[0] += capsule_loc_abs[0] - joint_loc_abs[0]
cap_offset[1] += capsule_loc_abs[1] - joint_loc_abs[1] - .04
cap_offset[2] += capsule_loc_abs[2] - joint_loc_abs[2]
self.cap_offsets.append(np.asarray(cap_offset))
self.cap_init_rots.append(np.asarray(cap_init_rot))
if count == 0:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type=root_joint_type,
joint_name=joint_names[count])
elif count == 4 or count == 5:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_X",
joint_name=joint_names[count])
elif count == 16 or count == 17:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="REVOLUTE_Y",
joint_name=joint_names[count])
else:
self.world.add_capsule(parent=int(red_parent_ref[count]),
radius=cap_rad,
length=cap_len,
cap_rot=cap_init_rot,
cap_offset=cap_offset,
joint_loc=joint_loc,
joint_type="BALL",
joint_name=joint_names[count])
lowest_points.append(
np.asarray(joint_locs_trans_abs)[count, 2] -
np.abs(float(input_flex_radius)))
count += 1
break
#print "pelvis cap",
#print np.asarray(joint_locs_trans_abs)[:, 2]
self.STARTING_HEIGHT = STARTING_HEIGHT - 0.0508
#add a floor-STARTING_HEIGHT / DART_TO_FLEX_CONV
self.world.add_weld_box(
width=10.0,
length=10.0,
height=0.2,
joint_loc=[
0.0, 0.0, -self.STARTING_HEIGHT / DART_TO_FLEX_CONV / 2 - 0.05
],
box_rot=[0.0, 0.0, 0.0],
joint_name="floor") #-0.05
skel = self.world.add_built_skeleton(_skel_id=0, _skel_name="human")
skel.set_self_collision_check(True)
#weight the capsules appropriately
volume = []
volume_median = []
for body_ct in range(NUM_CAPSULES):
#give the capsules a weight propertional to their volume
cap_rad = input_flex_radius
cap_len = input_flex_length
cap_rad_median = np.abs(float(capsules_median[body_ct].rad[0]))
cap_len_median = np.abs(float(capsules_median[body_ct].length[0]))
volume.append(np.pi * np.square(cap_rad) *
(cap_rad * 4 / 3 + cap_len))
volume_median.append(np.pi * np.square(cap_rad_median) *
(cap_rad_median * 4 / 3 + cap_len_median))
skel.bodynodes[0].set_mass(input_flex_mass)
body_mass = 0.0
#set the mass moment of inertia matrices
for body_ct in range(NUM_CAPSULES):
radius = input_flex_radius
length = input_flex_length
radius2 = radius * radius
length2 = length * length
mass = skel.bodynodes[body_ct].m
cap_init_rot = list(np.asarray(initial_rots[body_ct]))
volumeCylinder = np.pi * radius2 * length
volumeSphere = np.pi * radius * radius * radius * 4 / 3
density = mass / (volumeCylinder + volumeSphere)
massCylinder = density * volumeCylinder
massSphere = density * volumeSphere
Ixx = massCylinder * (length2 / 12.0 +
radius2 / 4.0) + massSphere * (
length2 + (3.0 / 8.0) * length * radius +
(2.0 / 5.0) * radius2)
Izz = massCylinder * (radius2 / 2.0) + massSphere * (
(2.0 / 5.0) * radius2)
RotMatInit = LibDartSkel().eulerAnglesToRotationMatrix(
[np.pi / 2, 0.0, 0.0])
RotMat = LibDartSkel().eulerAnglesToRotationMatrix(cap_init_rot)
I = np.matmul(np.matmul(RotMatInit, RotMat),
np.asarray([Ixx, Izz, Ixx]))
Ixx = np.abs(I[0])
Iyy = np.abs(I[1])
Izz = np.abs(I[2])
#print body_ct, I
skel.bodynodes[body_ct].set_inertia_entries(Ixx, Iyy, Izz)
body_mass += skel.bodynodes[body_ct].m
break
print "Body mass is: ", body_mass, "kg"
self.body_node = 9 #need to solve for the body node that corresponds to a force using flex.
self.force = np.asarray([0.0, 100.0, 100.0])
self.offset_from_centroid = np.asarray([-0.15, 0.0, 0.0])
self.pmat_red_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/pmat_red.npy")
self.force_dir_red_dart_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/force_dir_red.npy")
for element in range(len(self.force_dir_red_dart_all)):
self.force_dir_red_dart_all[element] = (np.multiply(
np.asarray(self.force_dir_red_dart_all[element]),
np.expand_dims(np.asarray(self.pmat_red_all[element]),
axis=1)))
self.force_loc_red_dart_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/force_loc_red.npy"
).tolist()
self.nearest_capsule_list_all = np.load(
"/home/henry/git/volumetric_pose_gen/data/nearest_capsule.npy"
).tolist()
print('init pose = %s' % skel.q)
skel.controller = DampingController(skel)
#now setup the open GL window
self.title = "GLUT Window"
self.window_size = (1280, 720)
self.scene = OpenGLScene(*self.window_size)
self.mouseLastPos = None
self.is_simulating = False
self.is_animating = False
self.frame_index = 0
self.capture_index = 0
self.force_application_count = 0
self.count = 0
self.zi = []
self.b = []
self.a = []
for i in range(60):
b, a = signal.butter(1, 0.05, analog=False)
self.b.append(b)
self.a.append(a)
self.zi.append(signal.lfilter_zi(self.b[-1], self.a[-1]))
self.ziF = []
self.bF = []
self.aF = []
for i in range(3):
b, a = signal.butter(1, 0.05, analog=False)
self.bF.append(b)
self.aF.append(a)
self.ziF.append(signal.lfilter_zi(self.bF[-1], self.aF[-1]))
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