def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 5:
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_L)
yme.addJoint(motion, LEFT_FOOT_SIDE_L, 'LEFT_Dummy2')
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_R)
yme.addJoint(motion, LEFT_FOOT_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_L)
yme.addJoint(motion, RIGHT_FOOT_SIDE_L, 'RIGHT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_R)
yme.addJoint(motion, RIGHT_FOOT_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 5:
length1 = .1
width1 = .065
mass1 = .5
length2 = .1
width2 = .2
mass2 = 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
if FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
if FOOT_PART_NUM > 1:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .3,
RIGHT_LEG: .3,
LEFT_UP_LEG: .3,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L,
RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
#yme.removeJoint(motion, RIGHT_TOES_END, False)
#yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
#yme.offsetJointLocal(motion, LEFT_TOES, (.03,-.05,0), False)
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCA)
yme.addJoint(motion, RIGHT_CALCA, 'RIGHT_Dummy')
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCA)
yme.addJoint(motion, LEFT_CALCA, 'LEFT_Dummy')
if 0:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.2), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .2), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
else:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[100:-1]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode(RIGHT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.3, RIGHT_CALCA:.3}
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3}
'''
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:0.5, SPINE1:0.5, RIGHT_FOOT:1.2, LEFT_FOOT:1.2, HIP:1.2,\
RIGHT_UP_LEG:.9, RIGHT_LEG:.9, LEFT_UP_LEG:.9, LEFT_LEG:.9}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:1.5, LEFT_FOOT:1., HIP:1.5,\
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.1, RIGHT_CALCA:.1}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['leftForeFoot'] = LEFT_TOES
config['rightForeFoot'] = RIGHT_TOES
config['leftRearFoot'] = LEFT_CALCA
config['rightRearFoot'] = RIGHT_CALCA
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V002.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 or FOOT_PART_NUM == 5 or FOOT_PART_NUM == 7:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 4:
yme.addJoint(motion, LEFT_FOOT, LEFT_TOES_SIDE_R)
yme.addJoint(motion, LEFT_TOES_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TOES_SIDE_R)
yme.addJoint(motion, RIGHT_TOES_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
elif FOOT_PART_NUM == 5:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS, (0.0, 0., -.1))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.1, 0., 0.0))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1, (0.1, 0., .2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4', (0., 0., .1))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
#yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS)
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3)
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .0), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
'''
elif FOOT_PART_NUM == 7:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
print(newOffset)
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_2,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2)
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_Dummy5')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_2,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2)
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_Dummy5')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
#yme.offsetJointLocal(motion, RIGHT_CALCANEUS, (0.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_2, (.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_3, (.0,-1.55,0), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
motion = motion[0:31 + 40]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 4:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .095
mass2 = .75
elif FOOT_PART_NUM == 5:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .18
width1 = width0 * 3
mass1 = mass0 * 2.8
#Metatarsal3
length3 = .17
width3 = width0 * 3
mass3 = mass0 * 2.7
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2.3
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 * 0.9
#Phalange3
length7 = .05
width7 = width3
mass7 = mass0 * 0.7
elif FOOT_PART_NUM == 7:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .2
width1 = width0 * 2
mass1 = mass0 * 2
#Metatarsal2
length2 = .2
width2 = width0
mass2 = mass0
#Metatarsal3
length3 = .19
width3 = width0 * 3
mass3 = mass0 * 2.8
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 / 2.
#Phalange2
length6 = .06
width6 = width2
mass6 = mass0 / 4.
#Phalange3
length7 = .05
width7 = width3
mass7 = mass6 * 2.8
if FOOT_PART_NUM == 7:
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.offset = (0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.offset = (0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.offset = (0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_2)
node.offset = (-0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_2)
node.offset = (-0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 5:
'''
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.05,-0.03,0.06)
node.length = length1
node.width = width1
node.mass = mass1
'''
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
#node = mcfg.getNode(RIGHT_PHALANGE_1)
#node.offset = (0.05,-0.03,0.19)
#node.length = length5
#node.width = width5
#node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.05, -0.03, 0.06)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.05, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 4:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, -0.03)
node = mcfg.getNode(LEFT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.08, 0.0, 0.1)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.05, 0.0, -0.03)
node = mcfg.getNode(RIGHT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, 0.12)
elif FOOT_PART_NUM == 3:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
'''
elif FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
'''
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
node = mcfg.getNode(LEFT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 4:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_3: .3
}
elif FOOT_PART_NUM == 7:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_2: .2,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_2: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_2: .2,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_2: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.,
LEFT_FOOT: 2.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_2: .3,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_2: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_2: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_2: .3,
RIGHT_PHALANGE_3: .3
}
'''
elif FOOT_PART_NUM == 5:
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:1., SPINE1:.3, RIGHT_FOOT:2.5, LEFT_FOOT:2.5, HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 7:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_2, LEFT_PHALANGE_3, LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
elif FOOT_PART_NUM == 5:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3,
LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
if MOTION != WALK:
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
elif (FOOT_PART_NUM == 5 or FOOT_PART_NUM == 6) and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
if MOTION != WALK:
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
if FOOT_PART_NUM == 1:
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(2.5, -0.0, .3), -.5), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False)
if MOTION == WALK:
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1., -0.0, .0),
.4), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1., 0.0, -.0), .4), False)
#yme.rotateJointLocal(motion, SPINE1, mm.exp(mm.v3(1.,0.0,0.0), -.8), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM == 6:
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1,
(-0.045, -0.06, 0.03))
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3,
(0.045, -0.06, 0.03)) #-0.0037
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (0.0, -0.06, -0.02))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1,
(0.045, -0.06, 0.03))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3,
(-0.045, -0.06 - 0.0062, 0.03 + 0.0035))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1,
(0.0, -0.06, -0.02))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
'''
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
'''
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2012), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2172), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2283), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -0.05), False)
'''
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0656), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0871), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0087), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0933), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0746), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -6.3773e-03), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.5), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .35
width1 = .2
mass1 = 4.3
elif FOOT_PART_NUM == 6:
mass0 = .4
width0 = 0.028
length0 = 0.1
#Metatarsal1
length1 = .15
width1 = width0 * 3
mass1 = mass0 * 1.8
#Metatarsal3
length3 = .11
width3 = width0 * 3
mass3 = mass0 * 1.86
#Calcaneus1
length4 = .08
width4 = 0.15 * 1
mass4 = mass0 * 1.2 * 2.
#Phalange1
length5 = .08
width5 = width1
mass5 = mass0 * 1.355
#Phalange3
length7 = length5
width7 = width5
mass7 = mass5
#Talus
length8 = .13
width8 = width0 * 3
mass8 = mass0 * 2.
if FOOT_PART_NUM == 6:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 6:
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3,
LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS:1.2, LEFT_CALCANEUS:1.2,
LEFT_PHALANGE_1:.2, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.2, RIGHT_PHALANGE_3:.2}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.8, SPINE1:.3, RIGHT_FOOT:1., LEFT_FOOT:1., HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1.,
LEFT_METATARSAL_3:1., RIGHT_METATARSAL_3:1., RIGHT_CALCANEUS:.3, LEFT_CALCANEUS:.3,
LEFT_PHALANGE_1:.3, LEFT_PHALANGE_3:.3, RIGHT_PHALANGE_1:.3, RIGHT_PHALANGE_3:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 6:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_3, LEFT_CALCANEUS_1
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
#motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
#addFootSegment
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_3,
(0.045, -0.06, -0.05)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_1, LEFT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_TALUS_3, LEFT_METATARSAL_3,
(0.0, 0.0, 0.1)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_2, LEFT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.04))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_3, 'LEFT_CALCANEUS_Effector3',
(0., 0.0, -0.07))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_3, RIGHT_METATARSAL_3, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.04))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_3, 'RIGHT_CALCANEUS_Effector3',
(0.0, 0.0, -0.07))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#return mcfg
#
#mass0 = .4
#width0 = 0.028
#length0 = 0.1
#
##Metatarsal1
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#
#length2 = .1
#width2 = 0.026
#mass2 = 0.4
#
##Metatarsal3
#length3 = .08
#width3 = 0.024
#mass3 = 0.4
#
##Calcaneus1
#length4 = .1
#width4 = 0.032
#mass4 = 0.4
#
##Phalange1
#length5 = .08
#width5 = 0.01
#mass5 = 0.4
##Phalange3
#length7 = length5
#width7 = width5
#mass7 = mass5
#
##Talus
##length8 = .13
##width8 = width0*3
##mass8 = mass0*2.
#
#length8 = .1
#width8 = width0*3
#mass8 = mass0*1.5
width0 = 0.028
length0 = 0.1
mass0 = .4
#Metatarsal1
length1 = .1
width1 = 0.03
mass1 = 0.4
length2 = length1
width2 = width1
mass2 = 0.4
#Metatarsal3
length3 = length1
width3 = width1
mass3 = 0.4
#Calcaneus1
length4 = length1
width4 = width1
mass4 = 0.4
#Phalange1
length5 = length1
width5 = width1
mass5 = 0.4
#Phalange3
length7 = length1
width7 = width1
mass7 = 0.4
#Talus
#length8 = .13
#width8 = width0*3
#mass8 = mass0*2.
length8 = .1
width8 = width0 * 3
mass8 = mass0 * 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.geom = 'MyFoot3'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_TALUS_1: .7,
RIGHT_TALUS_1: .7,
LEFT_TALUS_2: .7,
RIGHT_TALUS_2: .7,
LEFT_TALUS_3: .7,
RIGHT_TALUS_3: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_2: .7,
RIGHT_METATARSAL_2: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
RIGHT_CALCANEUS_2: .7,
LEFT_CALCANEUS_2: .7,
RIGHT_CALCANEUS_3: .7,
LEFT_CALCANEUS_3: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_2: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_2: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_TALUS_1: .1,
RIGHT_TALUS_1: .1,
LEFT_TALUS_2: .1,
RIGHT_TALUS_2: .1,
LEFT_TALUS_3: .1,
RIGHT_TALUS_3: .1,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_2: .1,
RIGHT_METATARSAL_2: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
RIGHT_CALCANEUS_2: .2,
LEFT_CALCANEUS_2: .2,
RIGHT_CALCANEUS_3: .2,
LEFT_CALCANEUS_3: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_2: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_2: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
LEFT_TALUS_1:.01, RIGHT_TALUS_1:.01, LEFT_TALUS_2:.01, RIGHT_TALUS_2:.01, LEFT_TALUS_3:.01, RIGHT_TALUS_3:.01,
LEFT_METATARSAL_1:.01, RIGHT_METATARSAL_1:.01, LEFT_METATARSAL_2:.01, RIGHT_METATARSAL_2:.01, LEFT_METATARSAL_3:.01, RIGHT_METATARSAL_3:.01,
RIGHT_CALCANEUS_1:.01, LEFT_CALCANEUS_1:.01, RIGHT_CALCANEUS_2:.01, LEFT_CALCANEUS_2:.01, RIGHT_CALCANEUS_3:.01, LEFT_CALCANEUS_3:.01,
LEFT_PHALANGE_1:.01, LEFT_PHALANGE_2:.01, LEFT_PHALANGE_3:.01, RIGHT_PHALANGE_1:.01, RIGHT_PHALANGE_2:.01, RIGHT_PHALANGE_3:.01}
'''
#config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
# RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
# LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_3:.1, RIGHT_TALUS_3:.1,
# LEFT_METATARSAL_1:.1, RIGHT_METATARSAL_1:.1, LEFT_METATARSAL_3:.1, RIGHT_METATARSAL_3:.1,
# RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_3:.2, LEFT_CALCANEUS_3:.2,
# LEFT_PHALANGE_1:.1, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_3:.1}
#
#config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5,
# RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7,
# LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_3:.7, RIGHT_TALUS_3:.7,
# LEFT_METATARSAL_1:.7, RIGHT_METATARSAL_1:.7, LEFT_METATARSAL_3:.7, RIGHT_METATARSAL_3:.7,
# RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_3:.7, LEFT_CALCANEUS_3:.7,
# LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_3:.4}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_CALCANEUS_3,
LEFT_TALUS_1, LEFT_TALUS_2, LEFT_TALUS_3, LEFT_METATARSAL_1,
LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2,
LEFT_PHALANGE_3
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_CALCANEUS_3,
RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_TALUS_3, RIGHT_METATARSAL_1,
RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1,
RIGHT_PHALANGE_2, RIGHT_PHALANGE_3
]
#config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3]
#config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
# motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
# motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
# motionName = 'woddy2_jump0_short.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
# motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0.), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0.), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1., 0., 0.), .01), False)
# yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0,0,1), -.01), False)
# addFootSegment
# Talus
length1 = .15
width1 = .03
mass1 = .4
# Phalange
length3 = length1 * 0.75
width3 = width1
mass3 = 0.4
length3_2 = length1 * 0.6
width3_2 = width1
mass3_2 = 0.4
#Calcaneus
length4 = 0.05
width4 = width1
mass4 = 0.4
width1 = 0.03
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_TALUS_1, LEFT_PHALANGE_1,
(0.0, 0.0, length1 - width1 - width3))
yme.addJoint(motion, LEFT_TALUS_2, LEFT_PHALANGE_2,
(0.0, 0.0, length1 - width1 - width3_2))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_PHALANGE_1,
(0.0, 0.0, length1 - width1 - width3))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_PHALANGE_2,
(0.0, 0.0, length1 - width1 - width3_2))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1',
(0.0, 0.0, length3 - width3))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2',
(0.0, 0.0, length3_2 - width3_2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1',
(0.0, 0.0, length3 - width3))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2',
(0.0, 0.0, length3_2 - width3_2))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1',
(0.0, 0.0, -length4))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2',
(0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1',
(0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2',
(0.0, 0.0, -length4))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(0., 1., 0.), 1.57), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 2000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
# node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#Talus
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#Phalange
#length3 = length1
#width3 = width1
#mass3 = 0.4
#length3_2 = length1*0.8
#width3_2 = width1
#mass3_2 = 0.4
#Calcaneus1
#length4 = 0.1
#width4 = width1
#mass4 = 0.4
#Foot
length8 = .1
width8 = 0.028 * 3
mass8 = .4 * 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4 # - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4 # - width4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot4'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 120
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_TALUS_1: .1,
RIGHT_TALUS_1: .1,
LEFT_TALUS_2: .1,
RIGHT_TALUS_2: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
RIGHT_CALCANEUS_2: .2,
LEFT_CALCANEUS_2: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_2: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_2: .1
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_TALUS_1: .7,
RIGHT_TALUS_1: .7,
LEFT_TALUS_2: .7,
RIGHT_TALUS_2: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
RIGHT_CALCANEUS_2: .7,
LEFT_CALCANEUS_2: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_2: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_2: .4
}
'''
(1, 'RightUpLeg')
(2, 'RightLeg')
(3, 'RightFoot')
(4, 'Spine')
(5, 'Spine1')
(6, 'LeftArm')
(7, 'LeftForeArm')
(8, 'RightArm')
(9, 'RightForeArm')
(10, 'LeftUpLeg')
(11, 'LeftLeg')
(12, 'LeftFoot')
(13, 'LeftTalus_1')
(14, 'LeftTalus_2')
(15, 'RightTalus_1')
(16, 'RightTalus_2')
(17, 'LeftPhalange_1')
(18, 'LeftPhalange_2')
(19, 'RightPhalange_1')
(20, 'RightPhalange_2')
(21, 'LeftCalcaneus_1')
(22, 'LeftCalcaneus_2')
(23, 'RightCalcaneus_1')
(24, 'RightCalcaneus_2')
'''
config['trackWMap'] = {
10, 5, .1, 20, 10, 5, 2, 10, 5, .1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01,
0.01, 0.01, 0.01, 0.01, 0.10, 0.01
}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
# config['end'] = HIP
config['end'] = SPINE1
config['trunk'] = SPINE
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1,
LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1,
RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2
]
return motion, mcfg, wcfg, stepsPerFrame, config
