def addJointSO3FromBvhJoint(self,
jointPosture,
bvhJoint,
channelValues,
scale=1.0):
localR = mm.I_SO3()
local_t = mm.O_Vec3()
for channel in bvhJoint.channels:
if channel.channelType == 'XPOSITION':
# jointPosture.rootPos[0] = channelValues[channel.channelIndex]*scale
local_t[0] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'YPOSITION':
# jointPosture.rootPos[1] = channelValues[channel.channelIndex]*scale
local_t[1] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'ZPOSITION':
# jointPosture.rootPos[2] = channelValues[channel.channelIndex]*scale
local_t[2] = channelValues[channel.channelIndex] * scale
elif channel.channelType == 'XROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((1, 0, 0)),
mm.deg2Rad(channelValues[channel.channelIndex])))
elif channel.channelType == 'YROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((0, 1, 0)),
mm.deg2Rad(channelValues[channel.channelIndex])))
elif channel.channelType == 'ZROTATION':
localR = numpy.dot(
localR,
mm.exp(mm.s2v((0, 0, 1)),
mm.deg2Rad(channelValues[channel.channelIndex])))
# jointPosture.setLocalR(bvhJoint.name, localR)
jointPosture.setLocalR(
jointPosture.skeleton.getElementIndex(bvhJoint.name), localR)
jointPosture.setLocal_t(
jointPosture.skeleton.getElementIndex(bvhJoint.name), local_t)
for i in range(len(bvhJoint.children)):
self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i],
channelValues, scale)
def addJoint(motion, parent_joint_name_or_index, joint_name, offset=None, update=True):
if isinstance(parent_joint_name_or_index, int):
parentIndex = parent_joint_name_or_index
else:
parentIndex = motion[0].skeleton.getElementIndex(parent_joint_name_or_index)
skeleton = motion[0].skeleton
parentJoint = skeleton.getElement(parentIndex)
childOffset = (0, 0., 0.)
if offset != None :
childOffset = offset
newJoint = parentJoint.addChild(joint_name, childOffset)#ym.Joint(joint_name, parentJoint)
motion[0].skeleton.addElement(newJoint, joint_name)
motion[0].skeleton.initialize()
localR = mm.I_SO3()
for jointPosture in motion:
jointPosture.addJoint(skeleton, localR)
jointPosture.setLocalR(jointPosture.skeleton.getElementIndex(joint_name), localR)
jointPosture.updateGlobalT()
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
return mcfg, wcfg, stepsPerFrame, config
#===============================================================================
# biped config
#===============================================================================
# motion, mesh config
g_motionDirConfigMap = {}
g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.I_SO3()}
g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \
{'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.exp(mm.v3(1,0,0), .01)}
g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .0)}
g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .03)}
#===============================================================================
# # reloadable config
#===============================================================================
def buildMassMap():
def swing_foot_placement(self, phys_model, motion_edit, frame, is_extended):
global prev_R_swp
t_swing_foot_placement = swf_placement_func(t)
if is_extended:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3()
R_swp_cor = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
# R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
# if np.dot(direction, diff_CMr_sag) < 0:
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
# else:
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement))
# R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.clampExp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement, math.pi/12.))
R_swp_cor = np.dot(R_swp_cor, mm.clampExp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement, math.pi/12.))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.clampExp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement, math.pi/12.))
else:
R_swp_sag = np.dot(R_swp_sag, mm.clampExp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement, math.pi/12.))
R_swp_cor = np.dot(R_swp_cor, mm.clampExp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement, math.pi/12.))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, )
# hwangpil
# temporal code.... for heel strike and ankle pushup
# motion_swf_placement[frame].mulJointOrientationGlobal(swingFoot, mm.exp([0., 0., -0.17*t_swing_foot_placement]))
# motion_swf_placement[frame].mulJointOrientationGlobal(swingFoot, mm.exp([0.2*t_swing_foot_placement, 0., 0.]))
# hwangpil
# foot placement based on difference
# # CM = dartModel.getBody("Hips").com()
# swf = dartModel.getJointPositionGlobal(swingFoot)
# CMr_swf = CM - swf
#
# # CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# swf_tar = motion_seg.getJointPositionGlobal(swingFoot, prev_frame)
# CMr_swf_tar = CM_tar - swf_tar
#
# diff_CMr_swf = mm.projectionOnPlane(CMr_swf-CMr_swf_tar, (1,0,0), (0,0,1))
#
# newPosition = motion_swf_placement[frame].getJointPositionGlobal(swingFoot)
# # newPosition += (diff_CMr_swf + diff_dCM)*t_swing_foot_placement
# newPosition += 0.1*diff_CMr_swf * t_swing_foot_placement
# aik.ik_analytic(motion_swf_placement[frame], swingFoot, newPosition)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)