def create_capsule(motionFile='foot3.bvh'):
# motion
motion = yf.readBvhFile(motionFile, .05)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('root')
node.geom = 'MyFoot3'
node.length = 1.
node.mass = .5
node = mcfg.getNode('foot_0_0')
node.geom = 'MyFoot4'
node.mass = .5
def mcfgFix(_mcfg):
for v in _mcfg.nodes.itervalues():
if len(v.geoms) == 0:
v.geoms.append(v.geom)
v.geomMass.append(v.mass)
v.geomTs.append(None)
mcfgFix(mcfg)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 40
simulSpeedInv = 1.
wcfg.timeStep = (1/30.*simulSpeedInv)/stepsPerFrame
# parameter
config = dict([])
config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
config['stepsPerFrame'] = stepsPerFrame
config['simulSpeedInv'] = simulSpeedInv
# etc
config['weightMap'] = {'root': 2., 'foot_0_0': 1., 'foot_1_0': 1., 'foot_2_0': 1., 'foot_0_1': .2, 'foot_1_1': .2, 'foot_2_1': .2}
config['weightMapTuple'] = (1., .5, .2, .5, .2, .5, .2)
# config['supLink'] = 'link0'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_chiken_foot():
# motion
motion = yf.readBvhFile('chikenFoot.bvh', 1)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('root')
node.geom = 'MyFoot3'
node.length = .1
# node.width = .05
node.mass = 5.
node = mcfg.getNode('foot00')
node.geom = 'MyFoot3'
node.mass = .5
node = mcfg.getNode('foot10')
node.geom = 'MyFoot3'
node.mass = .5
node = mcfg.getNode('foot01')
node.geom = 'MyFoot4'
node.mass = .5
node = mcfg.getNode('foot11')
node.geom = 'MyFoot4'
node.mass = .5
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 120
wcfg.timeStep = (1/30.)/stepsPerFrame
# parameter
config = dict([])
config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
# etc
config['weightMap'] = {'root': 2., 'foot00': 1., 'foot10': 1., 'foot01': .2, 'foot11': .2}
config['weightMapTuple'] = (2., 1., .2, 1., .2)
# config['supLink'] = 'link0'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_vchain_5():
# motion
motion = yf.readBvhFile('vchain_5_rotate_root0.bvh', 1)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
#node = mcfg.getNode('link0')
node = mcfg.getNode('Hips')
node.width = .3
node.mass = 6.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / stepsPerFrame
# parameter
config = {}
config['Kt'] = 20
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = 1
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 5000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
# etc
config['weightMap'] = {}
#config['supLink'] = 'link0'
config['supLink'] = 'Hips'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_zygote_two_seg():
#motion
motionName = 'wd2_tiptoe_zygote.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
motion = yf.readBvhFile(motionName, .01)
# yme.removeJoint(motion, 'Head', False)
# yme.removeJoint(motion, 'Head', False)
# yme.removeJoint(motion, 'RightShoulder', False)
# yme.removeJoint(motion, 'LeftShoulder1', False)
# yme.removeJoint(motion, 'RightToes_Effector', False)
# yme.removeJoint(motion, 'LeftToes_Effector', False)
# yme.removeJoint(motion, 'RightHand_Effector', False)
# yme.removeJoint(motion, 'LeftHand_Effector', False)
# yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
# yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False)
yme.removeJoint(motion, 'RightFoot_Effector', False)
yme.removeJoint(motion, 'LeftFoot_Effector', False)
yme.addJoint(motion, 'LeftFoot', 'LeftToes', [0., 0., 0.12], False)
yme.addJoint(motion, 'LeftToes', 'LeftToes_Effector', [0., 0., 0.07],
False)
yme.addJoint(motion, 'RightFoot', 'RightToes', [0., 0., 0.12], False)
yme.addJoint(motion, 'RightToes', 'RightToes_Effector', [0., 0., 0.07],
False)
yme.updateGlobalT(motion)
# motion.translateByOffset((0, -0.07, 0))
# motion.translateByOffset((0, -0.06, 0))
motion.extend([motion[-1]] * 300)
del motion[:270]
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
# 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('Hips')
# node.length = .2
# node.width = .25
node = mcfg.getNode('Hips')
node.length = 4. / 27.
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .177
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .8
node.offset = (0., 0., -0.02)
node = mcfg.getNode('LeftFoot')
node.length = .177
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .8
node.offset = (0., 0., -0.02)
node = mcfg.getNode('RightToes')
node.length = .053
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .218
# node.offset = (0,0,0.1)
node = mcfg.getNode('LeftToes')
node.length = .053
# node.length = .18
#node.length = .2
#node.width = .15
node.width = .1
node.mass = .218
# node.offset = (0,0,0.1)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# 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']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
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,
'RightToes': .2,
'LeftToes': .2
}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped(motionName='wd2_n_kick.bvh',
SEGMENT_FOOT=True,
SEGMENT_FOOT_MAG=.03,
SEGMENT_FOOT_RAD=None):
"""
:param motionName: motion file name
:param SEGMENT_FOOT: whether segment foot is
:param SEGMENT_FOOT_MAG:
:return:
"""
# :rtype: ym.JointMotion, ypc.ModelConfig, ypc.WorldConfig, int, dict[str, float|dict[str, float]], float
if SEGMENT_FOOT_RAD is None:
SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5
SEGMENT_FOOT_SEPARATE = False
SEGMENT_FOOT_OUTSIDE_JOINT_FIRST = True
SEGMENT_FOOT_ARC = True
SEGMENT_BETWEEN_SPACE = 1.2
SEGMENT_METATARSAL_LEN = 2.5
SEGMENT_THIRD_PHA_LEN = 1.8
SEGMENT_FOURTH_PHA_RATIO = 5. / 6.
SEGMENT_HEEL_LEN = 1.2
# motion
# motionName = 'wd2_n_kick.bvh'
# motionName = 'wd2_tiptoe.bvh'
# motionName = 'wd2_n_kick_zygote.bvh'
# motionName = 'wd2_jump.bvh'
# motionName = 'wd2_stand.bvh'
bvh = yf.readBvhFileAsBvh(motionName)
bvh.set_scale(.01)
if SEGMENT_FOOT:
# partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh'
current_path = os.path.dirname(os.path.abspath(__file__))
partBvhFilePath = current_path + '/../../PyCommon/modules/samples/'
if SEGMENT_FOOT_SEPARATE:
partBvhFilePath = partBvhFilePath + 'simpleJump_long_test5.bvh'
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST:
# partBvhFilePath = partBvhFilePath + 'simpleJump_long_test3.bvh'
# partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh'
partBvhFilePath = partBvhFilePath + 'foot_model_01.bvh'
else:
partBvhFilePath = partBvhFilePath + 'simpleJump_long_test4.bvh'
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partSkeleton = partBvh.toJointSkeleton(1., False)
SEGMENT_BETWEEN_SPACE = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_1'))[0]
SEGMENT_METATARSAL_LEN = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_1_0'))[2]
SEGMENT_THIRD_PHA_LEN = partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_0_0_0_Effector'))[2]
SEGMENT_HEEL_LEN = abs(
partSkeleton.getOffset(
partSkeleton.getElementIndex('foot_1_0_Effector'))[2])
bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG)
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partBvh.mirror('YZ')
bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG)
motion = bvh.toJointMotion(1., False) # type: ym.JointMotion
# motion.translateByOffset((0., 0.15, 0.))
# motion.translateByOffset((0., -0.12, 0.))
# motion.rotateByOffset(mm.rotZ(math.pi*1./18.))
# motion = yf.readBvhFile(motionName, .01)
# yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
# yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
# yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.1), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.1), False)
# yme.removeJoint(motion, 'RightFoot_foot_1_1')
# yme.removeJoint(motion, 'RightFoot_foot_1_2')
# yme.removeJoint(motion, 'LeftFoot_foot_1_1')
# yme.removeJoint(motion, 'LeftFoot_foot_1_2')
if motionName == 'wd2_n_kick.bvh' or motionName == 'wd2_n_kick_zygote.bvh':
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01),
False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.04, 0))
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
motion.extend([motion[-1]] * 300)
elif motionName == 'wd2_tiptoe.bvh' or motionName == 'wd2_tiptoe_zygote.bvh':
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01),
False)
yme.rotateJointLocal(motion, 'LeftFoot',
mm.exp(mm.v3(1., 0., 0.), -.1), False)
yme.rotateJointLocal(motion, 'RightFoot',
mm.exp(mm.v3(1., 0., 0.), -.1), False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.06, 0))
# if motionName == 'wd2_tiptoe.bvh':
# motion.translateByOffset((0, 0.06, 0))
# else:
# motion.translateByOffset((0, -0.03, 0))
del motion[:270]
for i in range(2000):
motion.data.insert(0, copy.deepcopy(motion[0]))
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
# stepsPerFrame = 30
frame_rate = 30
wcfg.timeStep = 1. / (frame_rate * stepsPerFrame)
# wcfg.timeStep = (1/30.)/(stepsPerFrame)
# wcfg.timeStep = (1/1000.)
# width : x axis on body frame
# height: y axis on body frame
# length: z axis on body frame
node = mcfg.getNode('Hips')
node.length = 4. / 27.
node.width = .25
# node.height = .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 = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
def capsulize(node_name):
node_capsule = mcfg.getNode(node_name)
node_capsule.geom = 'MyFoot4'
node_capsule.width = 0.01
node_capsule.density = 200.
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
# node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1))
# capsulize('RightFoot')
# capsulize('LeftFoot')
if SEGMENT_FOOT:
node = mcfg.getNode('RightFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
node = mcfg.getNode('LeftFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
# bird foot
# capsulize('RightFoot_foot_0_0')
# capsulize('RightFoot_foot_0_1')
# capsulize('RightFoot_foot_1_0')
# capsulize('RightFoot_foot_1_1')
# capsulize('RightFoot_foot_2_0')
# capsulize('RightFoot_foot_2_1')
# capsulize('LeftFoot_foot_0_0')
# capsulize('LeftFoot_foot_0_1')
# capsulize('LeftFoot_foot_1_0')
# capsulize('LeftFoot_foot_1_1')
# capsulize('LeftFoot_foot_2_0')
# capsulize('LeftFoot_foot_2_1')
# human foot
if SEGMENT_FOOT:
footJointType = 'B'
capsulDensity = 400.
if SEGMENT_FOOT_SEPARATE:
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
# node.addGeom('MyFoot3', [SEGMENT_FOOT_MAG*np.array([0., 0., 2.5*0.25]), mm.exp([0., 0., 0.])],
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0. - 1.2, 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0. + 1.2, 0., 0.]),
mm.exp([0., 0., 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and not SEGMENT_FOOT_ARC:
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
# left foot
# outside metatarsals
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
elif SEGMENT_FOOT_OUTSIDE_JOINT_FIRST and SEGMENT_FOOT_ARC:
FIRST_METATARSAL_ANGLE = mm.deg2Rad(30.)
SECOND_METATARSAL_ANGLE = mm.deg2Rad(20.)
THIRD_METATARSAL_ANGLE = mm.deg2Rad(15.)
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.bone_dir_child = 'RightFoot_foot_0_0_0'
# third
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(THIRD_METATARSAL_ANGLE), 0.
]),
mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
# fourth
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0., 0., 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.bone_dir_child = 'LeftFoot_foot_0_0_0'
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(THIRD_METATARSAL_ANGLE), 0.
]),
mm.exp(THIRD_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(THIRD_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0., 0., 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
node.jointType = 'B'
# RightFoot_foot_0_0_0 : outside phalanges
SEGMENT_FOURTH_PHA_OFFSET = .5 * SEGMENT_THIRD_PHA_LEN * (
1. - SEGMENT_FOURTH_PHA_RATIO)
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
# third
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# fourth
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([
-SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET
]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN *
SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([0., 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([
SEGMENT_BETWEEN_SPACE, 0., -SEGMENT_FOURTH_PHA_OFFSET
]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_THIRD_PHA_LEN *
SEGMENT_FOURTH_PHA_RATIO + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
# second
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(SECOND_METATARSAL_ANGLE), 0.
]),
mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
# first
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN *
math.tan(FIRST_METATARSAL_ANGLE), 0.
]),
mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
0., .5 * SEGMENT_METATARSAL_LEN *
math.tan(SECOND_METATARSAL_ANGLE), 0.
]),
mm.exp(SECOND_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(SECOND_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([
-SEGMENT_BETWEEN_SPACE, .5 * SEGMENT_METATARSAL_LEN *
math.tan(FIRST_METATARSAL_ANGLE), 0.
]),
mm.exp(FIRST_METATARSAL_ANGLE * mm.unitX())
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_METATARSAL_LEN /
math.cos(FIRST_METATARSAL_ANGLE) + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['Z']
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG *
np.array([SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# node.jointType = 'R'
# node.jointAxes = ['X']
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., 0.]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., 0.]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([-SEGMENT_BETWEEN_SPACE / 2., 0., .0]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG *
np.array([+SEGMENT_BETWEEN_SPACE / 2., 0., .0]),
mm.exp([0.] * 3)
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * SEGMENT_HEEL_LEN +
2. * SEGMENT_FOOT_RAD))
# node.jointType = footJointType
node.jointType = 'B'
else: # SEGMENT_FOOT_INSIDE_FIRST
# TODO:
# adjust transformation of geometries
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3),
mm.exp([0., math.atan2(1.2, 2.5), 0.])],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom(
'MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4', [
SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)
], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom(
'MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(
capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 + 2. * SEGMENT_FOOT_RAD))
node.jointType = footJointType
# 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'] = 15000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
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
}
if SEGMENT_FOOT:
segfoot_weight = 10.
# segfoot_weight = .1
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,
'RightFoot_foot_0_0': segfoot_weight,
'RightFoot_foot_0_1': segfoot_weight,
'RightFoot_foot_1_0': segfoot_weight,
'RightFoot_foot_1_1': segfoot_weight,
'RightFoot_foot_1_2': segfoot_weight,
'RightFoot_foot_0_0_0': segfoot_weight,
'RightFoot_foot_0_1_0': segfoot_weight,
'LeftFoot_foot_0_0': segfoot_weight,
'LeftFoot_foot_0_1': segfoot_weight,
'LeftFoot_foot_1_0': segfoot_weight,
'LeftFoot_foot_1_1': segfoot_weight,
'LeftFoot_foot_1_2': segfoot_weight,
'LeftFoot_foot_0_0_0': segfoot_weight,
'LeftFoot_foot_0_1_0': segfoot_weight
}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config, frame_rate
def create_biped(SEGMENT_FOOT=True, SEGMENT_FOOT_MAG=.03):
SEGMENT_FOOT_RAD = SEGMENT_FOOT_MAG * .5
#motion
motionName = 'wd2_n_kick.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
bvh = yf.readBvhFileAsBvh(motionName)
bvh.set_scale(.01)
if SEGMENT_FOOT:
# partBvhFilePath = '../PyCommon/modules/samples/simpleJump_long_test2.bvh'
current_path = os.path.dirname(os.path.abspath(__file__))
partBvhFilePath = current_path + '/../../PyCommon/modules/samples/simpleJump_long_test3.bvh'
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
bvh.replaceJointFromBvh('RightFoot', partBvh, SEGMENT_FOOT_MAG)
partBvh = yf.readBvhFileAsBvh(partBvhFilePath)
partBvh.mirror('YZ')
bvh.replaceJointFromBvh('LeftFoot', partBvh, SEGMENT_FOOT_MAG)
motion = bvh.toJointMotion(1., False)
# motion.translateByOffset((0., 0.15, 0.))
# motion.translateByOffset((0., -0.12, 0.))
# motion.rotateByOffset(mm.rotZ(math.pi*1./18.))
# motion = yf.readBvhFile(motionName, .01)
# yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
# yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
# yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False)
# yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False)
# yme.removeJoint(motion, 'RightFoot_foot_1_1')
# yme.removeJoint(motion, 'RightFoot_foot_1_2')
# yme.removeJoint(motion, 'LeftFoot_foot_1_1')
# yme.removeJoint(motion, 'LeftFoot_foot_1_2')
yme.updateGlobalT(motion)
#motion.translateByOffset((0, -0.07, 0))
#motion = motion[40:-58]
#motion[0:0] = [motion[0]]*20
#motion.extend([motion[-1]]*5000)
# motion = motion[40:]
#motion[0:0] = [motion[0]]*50
#motion.extend([motion[-1]]*5000)
#motion = motion[30:151]
motion = motion[30:]
#motion = motion[30:31]
#motion[5:5] = [motion[5]]*30
# motion[0:0] = [motion[0]]*2000
# motion.extend([motion[-1]]*300)
# motion = motion[37:]
motion[0:0] = [motion[0]] * 2000
motion.extend([motion[-1]] * 300)
#motion = motion[40:41]
#motion[0:0] = [motion[0]]*5000
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion[0:0] = [motion[0]]*10
#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]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
frame_rate = 30
wcfg.timeStep = 1. / (frame_rate * stepsPerFrame)
# wcfg.timeStep = (1/30.)/(stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# width : x axis on body frame
# height: y axis on body frame
# length: z axis on body frame
node = mcfg.getNode('Hips')
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 = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .15
node.width = .2
node.mass = 2.
def capsulize(node_name):
node_capsule = mcfg.getNode(node_name)
node_capsule.geom = 'MyFoot4'
node_capsule.width = 0.01
node_capsule.density = 200.
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .2))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., math.pi/4., 0.])], ypc.CapsuleMaterial(1000., .02, .1))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
# node.addGeom('MyFoot4', None, ypc.CapsuleMaterial(1000., .02, .1))
# capsulize('RightFoot')
# capsulize('LeftFoot')
if SEGMENT_FOOT:
node = mcfg.getNode('RightFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
node = mcfg.getNode('LeftFoot')
node.density = 200.
node.geom = 'MyFoot5'
node.width = 0.01
node.jointType = 'B'
# bird foot
# capsulize('RightFoot_foot_0_0')
# capsulize('RightFoot_foot_0_1')
# capsulize('RightFoot_foot_1_0')
# capsulize('RightFoot_foot_1_1')
# capsulize('RightFoot_foot_2_0')
# capsulize('RightFoot_foot_2_1')
# capsulize('LeftFoot_foot_0_0')
# capsulize('LeftFoot_foot_0_1')
# capsulize('LeftFoot_foot_1_0')
# capsulize('LeftFoot_foot_1_1')
# capsulize('LeftFoot_foot_2_0')
# capsulize('LeftFoot_foot_2_1')
# human foot
if SEGMENT_FOOT:
footJointType = 'B'
capsulDensity = 400.
# RightFoot_foot_0_0 : outside metatarsals
capsulize('RightFoot_foot_0_0')
node = mcfg.getNode('RightFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([-0.3 - 1.2, 0., 2.5 * 0.25]),
mm.exp([0., -math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 +
2. * SEGMENT_FOOT_RAD))
# node.addGeom('MyFoot4', [0.02*np.array([-1.2, 0., 0.]), mm.exp([0., 0., 0.])], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = footJointType
# RightFoot_foot_0_0_0 : outside phalanges
capsulize('RightFoot_foot_0_0_0')
node = mcfg.getNode('RightFoot_foot_0_0_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1 : inside metatarsals
capsulize('RightFoot_foot_0_1')
node = mcfg.getNode('RightFoot_foot_0_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_0_1_0 : inside phalanges
capsulize('RightFoot_foot_0_1_0')
node = mcfg.getNode('RightFoot_foot_0_1_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
# RightFoot_foot_1_0 : center heel
capsulize('RightFoot_foot_1_0')
node = mcfg.getNode('RightFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .4]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .4]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 +
2. * SEGMENT_FOOT_RAD))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = footJointType
# RightFoot_foot_1_1 : inside heel
# capsulize('RightFoot_foot_1_1')
# node = mcfg.getNode('RightFoot_foot_1_1')
# node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
# RightFoot_foot_1_2 : outside heel
# capsulize('RightFoot_foot_1_2')
# node = mcfg.getNode('RightFoot_foot_1_2')
# node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
capsulize('LeftFoot_foot_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0')
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3', [
SEGMENT_FOOT_MAG * np.array([0.3 + 1.2, 0., 2.5 * 0.25]),
mm.exp([0., math.atan2(1.2, 2.5), 0.])
],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 2.5 +
2. * SEGMENT_FOOT_RAD))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_0_0')
node = mcfg.getNode('LeftFoot_foot_0_0_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1')
node = mcfg.getNode('LeftFoot_foot_0_1')
node.addGeom('MyFoot3',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_0_1_0')
node = mcfg.getNode('LeftFoot_foot_0_1_0')
node.addGeom('MyFoot4',
[np.array([0.] * 3), mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.addGeom(
'MyFoot4',
[SEGMENT_FOOT_MAG * np.array([-1.2, 0., 0.]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
node.jointType = footJointType
capsulize('LeftFoot_foot_1_0')
node = mcfg.getNode('LeftFoot_foot_1_0')
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([-.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 +
2. * SEGMENT_FOOT_RAD))
node.addGeom(
'MyFoot3',
[SEGMENT_FOOT_MAG * np.array([+.6, 0., .0]),
mm.exp([0.] * 3)],
ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD,
SEGMENT_FOOT_MAG * 1.2 +
2. * SEGMENT_FOOT_RAD))
# node.addGeom('MyFoot4', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(1000., .01, -1))
node.jointType = footJointType
# capsulize('LeftFoot_foot_1_1')
# node = mcfg.getNode('LeftFoot_foot_1_1')
# node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
# capsulize('LeftFoot_foot_1_2')
# node = mcfg.getNode('LeftFoot_foot_1_2')
# node.addGeom('MyFoot3', [np.array([0.]*3), mm.exp([0.]*3)], ypc.CapsuleMaterial(capsulDensity, SEGMENT_FOOT_RAD, -1))
# node.jointType = footJointType
# 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']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
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
}
if SEGMENT_FOOT:
segfoot_weight = .1
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,
'RightFoot_foot_0_0': segfoot_weight,
'RightFoot_foot_0_1': segfoot_weight,
'RightFoot_foot_1_0': segfoot_weight,
'RightFoot_foot_1_1': segfoot_weight,
'RightFoot_foot_1_2': segfoot_weight,
'RightFoot_foot_0_0_0': segfoot_weight,
'RightFoot_foot_0_1_0': segfoot_weight,
'LeftFoot_foot_0_0': segfoot_weight,
'LeftFoot_foot_0_1': segfoot_weight,
'LeftFoot_foot_1_0': segfoot_weight,
'LeftFoot_foot_1_1': segfoot_weight,
'LeftFoot_foot_1_2': segfoot_weight,
'LeftFoot_foot_0_0_0': segfoot_weight,
'LeftFoot_foot_0_1_0': segfoot_weight
}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config, frame_rate
def simulation_test():
Kt = 20.
Dt = 2 * (Kt**.5)
Ks = 2000.
Ds = 2 * (Ks**.5)
mu = 1.
dir = './icmotion_last/'
filename = 'stop_left_normal.temp'
# filename = 'left_left_normal.temp'
# filename = 'right_left_fast.temp'
motion_ori = yf.readBvhFile(dir + filename)
frameTime = 1 / motion_ori.fps
motion_ori[0:0] = [motion_ori[0]] * 20
mcfgfile = open(dir + 'mcfg', 'rb')
mcfg = pickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
# wcfg.lockingVel = c_locking_vel
stepsPerFrame = 30
wcfg.timeStep = (frameTime) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion_ori[0], mcfg)
vpWorld.initialize()
print(controlModel)
controlModel.initializeHybridDynamics()
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [mu] * len(bodyIDsToCheck)
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE))
viewer.doc.addObject('motion_ori', motion_ori)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (0, 150, 255), yr.POLYGON_LINE))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, (200, 200, 200), yr.POLYGON_LINE))
def simulateCallback(frame):
th_r = motion_ori.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_ori.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_ori.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
vpWorld.step()
motionModel.update(motion_ori[frame])
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(frameTime / 1.4)
viewer.show()
Fl.run()
def create_foot(motionFile='foot3.bvh'):
massMap = {}
massMap = massMap.fromkeys(['Hips', 'foot00', 'foot01'], 0.)
massMap['Hips'] = 1.
massMap['foot00'] = 1.
massMap['foot01'] = 1.
# motion
motion = yf.readBvhFile(motionFile, .05)
motion.extend([motion[-1]] * 3000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
# for i in range(motion[0].skeleton.getElementNum()):
# mcfg.addNode(motion[0].skeleton.getElementName(i))
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
# totalMass += node.mass
node = mcfg.getNode('Hips')
# node.geom = 'MyFoot3'
node.geom = 'MyBox'
node.jointType = "B"
# node.length = 1.
node.mass = 1.
node = mcfg.getNode('foot00')
# node.geom = 'MyFoot4'
node.geom = 'MyBox'
node.jointType = "U"
node.mass = 1.
node = mcfg.getNode('foot01')
# node.geom = 'MyFoot4'
node.geom = 'MyBox'
node.jointType = "U"
node.mass = 1.
def mcfgFix(_mcfg):
"""
:param _mcfg: ypc.ModelConfig
:return:
"""
# for v in _mcfg.nodes.itervalues():
for k, v in _mcfg.nodes:
if len(v.geoms) == 0:
v.geoms.append(v.geom)
v.geomMass.append(v.mass)
v.geomTs.append(None)
# mcfgFix(mcfg)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 40
simulSpeedInv = 1.
wcfg.timeStep = (1 / 30. * simulSpeedInv) / stepsPerFrame
# parameter
config = dict([])
config['Kt'] = 20
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = 1
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 5000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
config['stepsPerFrame'] = stepsPerFrame
config['simulSpeedInv'] = simulSpeedInv
# etc
config['weightMap'] = {'root': 1., 'foot00': 1., 'foot01': 1.}
config['weightMapTuple'] = (1., 1., 1.)
# config['supLink'] = 'link0'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
#motionName = 'wd2_jump_ori.bvh'
if 1:
#motionName = 'wd2_stand.bvh'
motionName = 'woddy2_jump0.bvh'
motion = yf.readBvhFile(motionName, .01)
motion.translateByOffset((0., -0.023, 0.))
yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(2.5,-0.0,.3), -.5), False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.2), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.2), -.5), False)
yme.rotateJointLocal(motion, 'LeftUpLeg', mm.exp(mm.v3(0.0,.0,1.), .08), False)
yme.rotateJointLocal(motion, 'LeftLeg', mm.exp(mm.v3(0.0,1.0,0.), -.2), False)
#yme.rotateJointLocal(motion, 'RightLeg', mm.exp(mm.v3(1.0,0.0,0.), -.1), False)
yme.updateGlobalT(motion)
# yf.writeBvhFile('wd2_jump0.bvh', motion)
else :
motionName = 'ww13_41.bvh'
motion = yf.readBvhFile(motionName, 0.056444)
yme.removeJoint(motion, 'LHipJoint', False)
yme.removeJoint(motion, 'RHipJoint', False)
yme.removeJoint(motion, 'Neck', False)
yme.removeJoint(motion, 'Neck1', False)
yme.removeJoint(motion, 'Head', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder', False)
yme.removeJoint(motion, 'RightToeBase_Effector', False)
yme.removeJoint(motion, 'LeftToeBase_Effector', False)
yme.removeJoint(motion, 'LeftHand', False)
yme.removeJoint(motion, 'LeftFingerBase', False)
yme.removeJoint(motion, 'LeftHandIndex1_Effector', False)
yme.removeJoint(motion, 'LThumb', False)
yme.removeJoint(motion, 'RightHand', False)
yme.removeJoint(motion, 'RightFingerBase', False)
yme.removeJoint(motion, 'RightHandIndex1_Effector', False)
yme.removeJoint(motion, 'RThumb', False)
yme.removeJoint(motion, 'LowerBack', False)
yme.offsetJointLocal(motion, 'RightArm', (-.03,-.05,0), False)
yme.offsetJointLocal(motion, 'LeftArm', (.03,-.05,0), False)
#yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(-1.5,0,1), .4), False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1.5,0,1), -.4), False)
yme.updateGlobalT(motion)
#motion = motion[50:-1]
motion = motion[240:-1]
#motion = motion[40:-58]
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion = motion[62:65]
#motion = motion[216:217]
#motion = motion[515:555]
################
motion = motion[515:555]
# motion = motion[164:280]
#motion = motion[96:97]
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('Hips')
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 = .25
node.width = .2
node.mass = 4.
node = mcfg.getNode('LeftFoot')
node.length = .25
node.width = .2
node.mass = 4.
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']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
'''
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':0.5, 'Spine1':0.5, 'RightFoot':1.2, 'LeftFoot':1.2, 'Hips':1.2,\
'RightUpLeg':.9, 'RightLeg':.9, 'LeftUpLeg':.9, 'LeftLeg':.9}
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def preprocess(SEGMENT_FOOT=False):
# dir = './icmotion_test/'
# paths = glob.glob(dir+'*.temp')
dir = './ppmotion/'
# paths = glob.glob(dir+'*.bvh')
paths = None
if SEGMENT_FOOT:
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00_REPEATED.bvh')
else:
paths = glob.glob(dir+'wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal00_REPEATED.bvh')
# paths = glob.glob(dir+'wd2_WalkForwardVFast00.bvh')
'''
paths = glob.glob(dir+'wd2_WalkSameSame01.bvh')
paths = glob.glob(dir+'wd2_u-turn_1.bvh')
paths = glob.glob(dir+'wd2_cross_walk*.bvh')
paths = glob.glob(dir+'*_REPEATED.bvh')
paths = glob.glob(dir+'wd2_pick_walk_1.bvh')
paths = glob.glob(dir+'wd2_WalkSameSame01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardSlow01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkSoldier00_REPEATED_FOOT.bvh')
'''
hRef = .1; vRef = .4
## hRef = .1; vRef = .2
# dir = './ppmotion_long/'
# paths = glob.glob(dir+'wd2_WalkBackward00_REPEATED.bvh')
# hRef = 10.; vRef = .4
# dir = './rawmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
## paths = [dir+'woddy2_walk_normal_to_slope.bvh']
# hRef = 10000.; vRef = .4*100
#
# dir = './ppmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
# hRef = 10000.; vRef = .2
jumpThreshold = 15; jumpBias = 1.
stopThreshold = 15; stopBias = 0.
for path in paths:
motion_ori = yf.readBvhFile(path)
# informations
skeleton = motion_ori[0].skeleton
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
lHip = skeleton.getJointIndex('LeftUpLeg'); rHip = skeleton.getJointIndex('RightUpLeg')
lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg')
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
mcfgfile = open(dir + 'mcfg', 'rb')
mcfg = pickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
# bodyMasses = getBodyMasses()
bodyMasses = motionModel.getBodyMasses()
uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']]
upperMass = sum([bodyMasses[i] for i in uppers])
lc = yma.getElementContactStates(motion_ori, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion_ori, 'RightFoot', hRef, vRef)
# intervals, states = yba.getBipedGaitIntervals(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
intervals, states = yba.getBipedGaitIntervals2(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
seginfos = [{} for i in range(len(intervals))]
for i in range(len(intervals)):
start = intervals[i][0]; end = intervals[i][1]
seginfos[i]['interval'] = intervals[i]
seginfos[i]['state'] = states[i]
# print yba.GaitState.text[states[i]], intervals[i]
stanceHips = []; swingHips = []; stanceFoots = []; swingFoots = []; swingKnees = []
if states[i]==yba.GaitState.LSWING: stanceHips = [rHip]; stanceFoots = [rFoot]; swingHips = [lHip]; swingFoots = [lFoot]; swingKnees = [lKnee]
elif states[i]==yba.GaitState.RSWING: stanceHips = [lHip]; stanceFoots = [lFoot]; swingHips = [rHip]; swingFoots = [rFoot]; swingKnees = [rKnee]
elif states[i]==yba.GaitState.STOP: stanceHips = [rHip, lHip]; stanceFoots = [rFoot, lFoot]
elif states[i]==yba.GaitState.JUMP: swingHips = [rHip, lHip]; swingFoots = [rFoot, lFoot]
seginfos[i]['stanceHips'] = stanceHips
seginfos[i]['swingHips'] = swingHips
seginfos[i]['stanceFoots'] = stanceFoots
seginfos[i]['swingFoots'] = swingFoots
seginfos[i]['swingKnees'] = swingKnees
if start<end:
if SEGMENT_FOOT:
# segmented foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
else:
# box foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
seginfos[i]['max_stf_push_frame'] = None
if len(swingFoots) > 0:
pushes = []
for frame in range(start, int((start+end)//2) + 1):
dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis))
seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes))
# write .seg
inputName = os.path.basename(path)
root = os.path.splitext(inputName)[0]
outputName = root+'.seg'
outputFile = open(dir+outputName, 'wb')
pickle.dump(seginfos, outputFile)
outputFile.close()
print(outputName, 'done')
pprint.pprint(seginfos)
print('FINISHED')
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
def create_biped_basic(motionName):
scale = 1.
motion = yf.readBvhFile(motionName, scale)
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.)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
# 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
def create_legs(motionFile='legs.bvh'):
# motion
motion = yf.readBvhFile(motionFile, .06)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('Hips')
# node.geom = ''
# node.length = 40.
# node.length = 1.
node.mass = 4.
node = mcfg.getNode('Spine')
node.geom = 'MyFoot4'
node.mass = 3.
node.width = .1
node = mcfg.getNode('RightUpLeg')
node.geom = 'MyFoot3'
node.mass = 5.
# node.length = 40.
node = mcfg.getNode('RightLeg')
node.geom = 'MyFoot4'
node.mass = 5.
# node.length = 40.
node = mcfg.getNode('RightFoot')
node.geom = 'MyFoot4'
node.mass = .8
node = mcfg.getNode('RightToe')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('RightCal')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('RightHeel')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('RightCune')
node.geom = 'MyFoot4'
node.mass = .4
node = mcfg.getNode('RightMt5')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('LeftUpLeg')
node.geom = 'MyFoot3'
node.mass = 5.
# node.length = 40.
node = mcfg.getNode('LeftLeg')
node.geom = 'MyFoot4'
node.mass = 5.
# node.length = 40.
node = mcfg.getNode('LeftFoot')
node.geom = 'MyFoot4'
node.mass = .8
node = mcfg.getNode('LeftToe')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('LeftCal')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('LeftHeel')
node.geom = 'MyFoot4'
node.mass = .2
node = mcfg.getNode('LeftCune')
node.geom = 'MyFoot4'
node.mass = .4
node = mcfg.getNode('LeftMt5')
node.geom = 'MyFoot4'
node.mass = .2
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 40
simulSpeedInv = 1.
wcfg.timeStep = (1/30.*simulSpeedInv)/stepsPerFrame
# parameter
config = dict([])
config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
config['stepsPerFrame'] = stepsPerFrame
config['simulSpeedInv'] = simulSpeedInv
# etc
config['weightMap'] = {'root': 2., 'knee': 2., 'foot00': 1., 'foot10': 1., 'foot20': 1., 'foot01': .2, 'foot11': .2, 'foot21': .2}
config['weightMapTuple'] = (2., 2., 1., .2, 1., .2, 1., .2)
# config['supLink'] = 'link0'
return motion, mcfg, wcfg, stepsPerFrame, config
pydart.init()
current_path = os.path.dirname(os.path.abspath(__file__))
motionDir = current_path + '/../DartWalkingFoot/ppmotion/'
filename = 'segfoot_wd2_WalkForwardNormal00.bvh'
bvh = yf.readBvhFileAsBvh(motionDir + filename)
motion_ori = bvh.toJointMotion(1., False)
# motion_ori = yf.readBvhFile(motionDir+filename)
frameTime = 1 / motion_ori.fps
mcfg = buildMcfg()
c_locking_vel = .05
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.lockingVel = c_locking_vel
stepsPerFrame = 50
wcfg.timeStep = frameTime / stepsPerFrame
pydart.init()
dartMotionModel = cpm.DartModel(wcfg, motion_ori[0],
mcfg) # type: cpm.DartModel
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
clock = pygame.time.Clock()
_world = world(gravity=(0, -9.8), doSleep=True) # type: Box2D.b2World
def create_walking_biped():
#motion
motionName = 'wd2_WalkForwardNormal00.bvh'
#motionName = '../motions/wd2_WalkForwardNormal00.bvh'
#motionName = '../motions/wd2_WalkForwardFast00.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, 'Head', False)
#yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45),
False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, 0.0, 0))
#motion = motion[40:-58]
#motion[0:0] = [motion[0]]*20
#motion.extend([motion[-1]]*5000)
motion = motion[40:]
#motion[0:0] = [motion[0]]*50
#motion.extend([motion[-1]]*100)
#motion.extend([motion[-1]]*100)
#motion = motion[30:151]
#motion = motion[30:]
#motion[5:5] = [motion[5]]*30
#motion[0:0] = [motion[0]]*100
#motion.extend([motion[-1]]*5000)
#motion = motion[40:41]
#motion[0:0] = [motion[0]]*5000
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion[0:0] = [motion[0]]*10
#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('Hips')
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 = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
node.width = .12
#node.width = .2
node.mass = 2.
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
node.width = .12
#node.width = .2
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# 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']={'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}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
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}
#success!!
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
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_any_motion(motion):
#motion
yme.removeJoint(motion, 'Head', False)
#yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45),
False)
yme.updateGlobalT(motion)
# 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('Hips')
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 = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .12
#node.width = .2
node.width = .15
node.mass = 2.
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .12
node.width = .15
#node.width = .2
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
# 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']={'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
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
