def test_body_force_funcs():
bvhFilePath = '../samples/chain_1.bvh'
# bvhFilePath = '../samples/chain_6.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.gravity = (0,0,0)
stepsPerFrame = 60
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.setBodyPositionGlobal(0, (0,1,0))
###################################################################
# apply force == add force
# controlModel.applyBodyGenForceGlobal(0, (0,0,100), (0,-100,0), (0,0,0))
# above 1 line == below 2 lines
controlModel.applyBodyTorqueGlobal(0, (0,0,100))
controlModel.applyBodyForceGlobal(0, (0,-100,0))
p = [mm.O_Vec3()]*controlModel.getBodyNum()
forces = [mm.O_Vec3()] * controlModel.getBodyNum()
netForces = [mm.O_Vec3()] * controlModel.getBodyNum()
gravityForces = [mm.O_Vec3()] * controlModel.getBodyNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('forces', yr.VectorsRenderer(forces, p, (255,0,0)))
# viewer.doc.addRenderer('netForces', yr.VectorsRenderer(netForces, p, (0,255,0)))
# viewer.doc.addRenderer('gravityForces', yr.VectorsRenderer(gravityForces, p, (0,0,255)))
viewer.setMaxFrame(500)
def simulateCallback(frame):
p[:] = controlModel.getBodyPositionsGlobal()
forces[0] = controlModel.getBodyForceLocal(0)
netForces[0] = controlModel.getBodyNetForceLocal(0)
gravityForces[0] = controlModel.getBodyGravityForceLocal(0)
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def normal_mcfg():
massMap = buildMassMap()
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.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .25
node.width = .1
return mcfg
def test_OdeControlModel():
# mcfg.defaultContactMode = ode.ContactBounce | ode.ContactSoftERP | ode.ContactSoftCFM
# mcfg.defaultJointAxes = []
# mcfg.defaultJointLoStop = mm.deg2Rad(-5)
# mcfg.defaultJointHiStop = mm.deg2Rad(5)
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
motion = motion[30:]
AX = numpy.array([1, 0, 0])
AY = numpy.array([0, 1, 0])
AZ = numpy.array([0, 0, 1])
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
mcfg.defaultKp = 100.
mcfg.defaultKd = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('body2')
node.length = 1
node.offset = (0, 0, .2)
# node.jointAxes = [AY]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel.fixRootBody()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
def simulateCallback(frame):
for i in range(stepsPerFrame):
torques = controlModel.calcPDTorque(motion[frame])
controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def buildMcfg():
massMap = buildMassMap()
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
totalMass = 0.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
totalMass += node.mass
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 = .27
# node.offset = (0,0,0.01)
node.width = .1
node.geom = 'MyFoot1'
node = mcfg.getNode('LeftFoot')
node.length = .25
# node.length = .27
# node.offset = (0,0,0.01)
node.width = .1
node.geom = 'MyFoot1'
def capsulize(node_name):
node = mcfg.getNode(node_name)
node.geom = 'MyFoot4'
node.width = 0.02
capsulize('RightFoot')
capsulize('LeftFoot')
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')
return mcfg
def test_hybridDynamics_gravity():
bvhFilePath = '../samples/chain_1.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
# wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
controlModel2.initializeHybridDynamics()
controlModel2.translateByOffset((1,0,0))
p = []
torques = []
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('model2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('torques', yr.VectorsRenderer(torques, p, (255,0,0)))
viewer.setMaxFrame(100)
def simulateCallback(frame):
p[:] = controlModel.getInternalJointPositionsGlobal() + controlModel2.getInternalJointPositionsGlobal()
for i in range(stepsPerFrame):
controlModel.solveHybridDynamics()
print 'a', controlModel2.getBodyForceLocal(0)
controlModel2.applyBodyForceGlobal(0, (0,10,0))
print 'b', controlModel2.getBodyForceLocal(0)
controlModel2.solveHybridDynamics()
torques[:] = controlModel.getInternalJointTorquesLocal() + controlModel2.getInternalJointTorquesLocal()
vpWorld.step()
print 'c', controlModel2.getBodyForceLocal(0)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_getInternalJointOrientationsGlobal():
# bvhFilePath = '../samples/block_tree_rotate.bvh'
bvhFilePath = '../samples/chain_3_rotate_expt_root.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.translateByOffset((0,0,1))
controlModel.fixBody(0)
jointPositions = []
localFrames = []
globalFrames = []
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('localFrames', yr.FramesRenderer(localFrames, (255,0,0)))
viewer.doc.addRenderer('globalFrames', yr.FramesRenderer(globalFrames, (0,255,0)))
def simulateCallback(frame):
controlModel.setJointAngVelocityLocal(1, (0,.5,0))
controlModel.setJointAngVelocityLocal(2, (0,.5,0))
for i in range(stepsPerFrame):
vpWorld.step()
jointPositions[:] = motion[frame].getInternalJointPositionsGlobal() + controlModel.getInternalJointPositionsGlobal()
localFrames[:] = motion.getInternalJointOrientationsLocal(frame) + controlModel.getInternalJointOrientationsLocal()
localFrames[:] = map(mm.Rp2T, localFrames, jointPositions)
globalFrames[:] = motion[frame].getInternalJointOrientationsGlobal() + controlModel.getInternalJointOrientationsGlobal()
globalFrames[:] = map(mm.Rp2T, globalFrames, jointPositions)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
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 test_delNode():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultBoneRatio = .8
node = mcfg.addNode('body1')
# node = mcfg.addNode('body2')
node = mcfg.addNode('body3')
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel.fixRootBody()
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
for i in range(stepsPerFrame):
torques = controlModel.calcPDTorque(motion[frame])
controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
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 test_joint_force_funcs():
bvhFilePath = '../samples/block_tree_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.fixBody(0)
joint_p = []
joint_t = []
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('joint_p', yr.PointsRenderer(joint_p, (0,255,0)))
viewer.doc.addRenderer('joint_t', yr.VectorsRenderer(joint_t, joint_p, (0,255,0)))
def simulateCallback(frame):
controlModel.setJointTorqueLocal(1, (0,0,1))
joint_p[:] = controlModel.getInternalJointPositionsGlobal()
joint_t[:] = controlModel.getInternalJointTorquesLocal()
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_inertia_matrix():
bvhFilePath = '../samples/chain_1.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel2.translateByOffset((0,0,1))
controlModel2.rotate(mm.exp(mm.v3(0,1,0), math.pi/2))
print 'model local'
print controlModel.getBodyInertiasLocal()
print
print 'model global'
print controlModel.getBodyInertiasGlobal()
print
print 'model2 global'
print controlModel2.getBodyInertiasGlobal()
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
# viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_LINE))
viewer.setMaxFrame(100)
viewer.show()
Fl.run()
def test_OdeMotionModel():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
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('body1')
node.mass = 1.
node = mcfg.getNode('body2')
node.mass = 1.
node.boneRatio = .5
node.offset = (0, 0, .1)
node = mcfg.getNode('body3')
node.mass = 1.
node.length = .1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
odeWorld = yow.OdeWorld(wcfg, mcfg)
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
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.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'
return motion, mcfg, wcfg, stepsPerFrame, config
def test_VpControlModel():
bvhFilePath = '../samples/block_3_rotate.bvh'
# bvhFilePath = '../samples/block_tree_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
motion = motion[30:]
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('body2')
node.length = 1
node.offset = (0,0,.2)
# mcfg.delNode('body2')
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
viewer = ysv.SimpleViewer()
# viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_body_pos_vel_acc_funcs():
# bvhFilePath = '../samples/chain_6.bvh'
# bvhFilePath = '../samples/block_3_rotate.bvh'
bvhFilePath = '../samples/block_tree_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
bvhFilePath = '../samples/chain_1.bvh'
motion2 = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
# mcfg.getNode('root').density = 1000000.
mcfg2 = ypc.ModelConfig()
for i in range(motion2[0].skeleton.getElementNum()):
mcfg2.addNode(motion2[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
# wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion2[0], mcfg2)
vpWorld.initialize()
controlModel.fixBody(0)
motionModel.recordVelByFiniteDiff()
controlModel2.setBodyPositionGlobal(0, (0,1,-1))
cm_p = [mm.O_Vec3()]*controlModel.getBodyNum()
cm_v = [mm.O_Vec3()]*controlModel.getBodyNum()
cm_a = [mm.O_Vec3()]*controlModel.getBodyNum()
cm_av = [mm.O_Vec3()]*controlModel.getBodyNum()
cm_aa = [mm.O_Vec3()]*controlModel.getBodyNum()
mm_p = [mm.O_Vec3()]*motionModel.getBodyNum()
mm_v = [mm.O_Vec3()]*motionModel.getBodyNum()
mm_a = [mm.O_Vec3()]*motionModel.getBodyNum()
mm_av = [mm.O_Vec3()]*motionModel.getBodyNum()
mm_aa = [mm.O_Vec3()]*motionModel.getBodyNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (100,100,100), yr.POLYGON_LINE))
viewer.doc.addRenderer('controlModel2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('cm_p', yr.PointsRenderer(cm_p, (0,0,255)))
viewer.doc.addRenderer('cm_v', yr.VectorsRenderer(cm_v, cm_p, (255,0,0)))
# viewer.doc.addRenderer('cm_a', yr.VectorsRenderer(cm_a, cm_p, (0,255,0)))
# viewer.doc.addRenderer('cm_av', yr.VectorsRenderer(cm_av, cm_p, (255,255,0)))
# viewer.doc.addRenderer('cm_aa', yr.VectorsRenderer(cm_aa, cm_p, (0,255,255)))
# viewer.doc.addRenderer('mm_p', yr.PointsRenderer(mm_p, (200,200,0)))
viewer.doc.addRenderer('mm_v', yr.VectorsRenderer(mm_v, mm_p, (200,200,0)))
# viewer.doc.addRenderer('mm_a', yr.VectorsRenderer(mm_a, mm_p, (200,200,0)))
# viewer.doc.addRenderer('mm_av', yr.VectorsRenderer(mm_av, mm_p, (0,200,0)))
controlModel.applyBodyTorqueGlobal(1, (0,0,200))
controlModel2.applyBodyTorqueGlobal(0, (0,0,200))
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
motionModel.update(motion[frame])
# controlModel.applyBodyTorqueGlobal(1, (0,0,10))
# controlModel2.applyBodyTorqueGlobal(0, (0,0,10))
cm_p[:] = controlModel.getBodyPositionsGlobal() + controlModel2.getBodyPositionsGlobal()
cm_v[:] = controlModel.getBodyVelocitiesGlobal() + controlModel2.getBodyVelocitiesGlobal()
# cm_p.append(controlModel.getBodyPositionGlobal(1, (0,0,.25)))
# cm_v.append(controlModel.getBodyVelocityGlobal(1, (0,0,.25)))
# cm_p.append(controlModel.getBodyPositionGlobal(1, (0,0,-.25)))
# cm_v.append(controlModel.getBodyVelocityGlobal(1, (0,0,-.25)))
cm_a[:] = controlModel.getBodyAccelerationsGlobal() + controlModel2.getBodyAccelerationsGlobal()
cm_av[:] = controlModel.getBodyAngVelocitiesGlobal() + controlModel2.getBodyAngVelocitiesGlobal()
cm_aa[:] = controlModel.getBodyAngAccelerationsGlobal() + controlModel2.getBodyAngAccelerationsGlobal()
mm_p[:] = motionModel.getBodyPositionsGlobal()
mm_v[:] = motionModel.getBodyVelocitiesGlobal()
mm_av[:] = motionModel.getBodyAngVelocitiesGlobal()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V002.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 or FOOT_PART_NUM == 5 or FOOT_PART_NUM == 7:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 4:
yme.addJoint(motion, LEFT_FOOT, LEFT_TOES_SIDE_R)
yme.addJoint(motion, LEFT_TOES_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TOES_SIDE_R)
yme.addJoint(motion, RIGHT_TOES_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_TOES_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
elif FOOT_PART_NUM == 5:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS, (0.0, 0., -.1))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.1, 0., 0.0))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1, (0.1, 0., .2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4', (0., 0., .1))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
#yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS)
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3)
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3, (0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3, (-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
'''
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .0), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False)
'''
elif FOOT_PART_NUM == 7:
sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
print(newOffset)
yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3')
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_2,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3,
(0.0025, -0.4209, -0.0146))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1)
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4')
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2)
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_Dummy5')
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3)
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6')
sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT)
skeleton = motion[0].skeleton
sibJoint = skeleton.getElement(sibIndex)
newOffset = sibJoint.offset.copy()
yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3')
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_2,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3,
(-0.0025, -0.4209, -0.0146))
yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1)
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4')
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2)
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_Dummy5')
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3)
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6')
#yme.offsetJointLocal(motion, RIGHT_CALCANEUS, (0.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_2, (.0,-1.55,0), False)
#yme.offsetJointLocal(motion, RIGHT_METATARSAL_3, (.0,-1.55,0), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_2,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(-1., 0.0, 0.0), .3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS,
mm.exp(mm.v3(-1., 0.0, 0.0), .20), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
motion = motion[0:31 + 40]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 4:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .095
mass2 = .75
elif FOOT_PART_NUM == 5:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .18
width1 = width0 * 3
mass1 = mass0 * 2.8
#Metatarsal3
length3 = .17
width3 = width0 * 3
mass3 = mass0 * 2.7
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2.3
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 * 0.9
#Phalange3
length7 = .05
width7 = width3
mass7 = mass0 * 0.7
elif FOOT_PART_NUM == 7:
mass0 = .4
width0 = 0.028
#Metatarsal1
length1 = .2
width1 = width0 * 2
mass1 = mass0 * 2
#Metatarsal2
length2 = .2
width2 = width0
mass2 = mass0
#Metatarsal3
length3 = .19
width3 = width0 * 3
mass3 = mass0 * 2.8
#Calcaneus
length4 = .08
width4 = 0.15
mass4 = mass0 * 2
#Phalange1
length5 = .06
width5 = width1
mass5 = mass0 / 2.
#Phalange2
length6 = .06
width6 = width2
mass6 = mass0 / 4.
#Phalange3
length7 = .05
width7 = width3
mass7 = mass6 * 2.8
if FOOT_PART_NUM == 7:
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.offset = (0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.offset = (0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.offset = (0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.055, -0.03, 0.04)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_2)
node.offset = (-0.01, -0.03, 0.065)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.05, -0.03, 0.055)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.07)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.055, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_2)
node.offset = (-0.01, -0.03, 0.19)
node.length = length6
node.width = width6
node.mass = mass6
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.05, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 5:
'''
node = mcfg.getNode(RIGHT_FOOT)
node.offset = (0.05,-0.03,0.06)
node.length = length1
node.width = width1
node.mass = mass1
'''
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.offset = (-.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
#node = mcfg.getNode(RIGHT_PHALANGE_1)
#node.offset = (0.05,-0.03,0.19)
#node.length = length5
#node.width = width5
#node.mass = mass5
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.offset = (-.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
node = mcfg.getNode(LEFT_FOOT)
node.offset = (-0.05, -0.03, 0.06)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.offset = (.04, -0.03, 0.075)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS)
node.offset = (.0, -0.03, -0.05)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_PHALANGE_1)
node.offset = (-0.05, -0.03, 0.19)
node.length = length5
node.width = width5
node.mass = mass5
node = mcfg.getNode(LEFT_PHALANGE_3)
node.offset = (.04, -0.03, 0.18)
node.length = length7
node.width = width7
node.mass = mass7
elif FOOT_PART_NUM == 4:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, -0.03)
node = mcfg.getNode(LEFT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.08, 0.0, 0.1)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0.05, 0.0, -0.03)
node = mcfg.getNode(RIGHT_TOES_SIDE_R)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (-0.05, 0.0, 0.12)
elif FOOT_PART_NUM == 3:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
'''
elif FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07,0.0,0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07,0.0,0.015)
'''
if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5:
node = mcfg.getNode(LEFT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0, 0.0, -0.08)
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 4:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TOES_SIDE_R: .3,
RIGHT_TOES_SIDE_R: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_3: .3
}
elif FOOT_PART_NUM == 7:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_METATARSAL_2: .2,
LEFT_METATARSAL_3: .2,
RIGHT_METATARSAL_2: .2,
RIGHT_METATARSAL_3: .2,
LEFT_PHALANGE_1: .2,
LEFT_PHALANGE_2: .2,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .2,
RIGHT_PHALANGE_2: .2,
RIGHT_PHALANGE_3: .2
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.,
LEFT_FOOT: 2.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_METATARSAL_2: .3,
LEFT_METATARSAL_3: .3,
RIGHT_METATARSAL_2: .3,
RIGHT_METATARSAL_3: .3,
LEFT_PHALANGE_1: .3,
LEFT_PHALANGE_2: .3,
LEFT_PHALANGE_3: .3,
RIGHT_PHALANGE_1: .3,
RIGHT_PHALANGE_2: .3,
RIGHT_PHALANGE_3: .3
}
'''
elif FOOT_PART_NUM == 5:
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:1., SPINE1:.3, RIGHT_FOOT:2.5, LEFT_FOOT:2.5, HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3,
LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 7:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_2, LEFT_PHALANGE_3, LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
elif FOOT_PART_NUM == 5:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3,
LEFT_CALCANEUS
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3,
RIGHT_CALCANEUS
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
#yme.removeJoint(motion, RIGHT_TOES_END, False)
#yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
#yme.offsetJointLocal(motion, LEFT_TOES, (.03,-.05,0), False)
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCA)
yme.addJoint(motion, RIGHT_CALCA, 'RIGHT_Dummy')
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCA)
yme.addJoint(motion, LEFT_CALCA, 'LEFT_Dummy')
if 0:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.2), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .2), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .7), False)
else:
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_CALCA,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[100:-1]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode(RIGHT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_FOOT)
node.length = .1
node.width = .2
node.mass = 1.5
node = mcfg.getNode(LEFT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_CALCA)
node.length = .1
node.width = .2
node.mass = 1.5
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.3, RIGHT_CALCA:.3}
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,\
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3}
'''
config['IKweightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:0.5, SPINE1:0.5, RIGHT_FOOT:1.2, LEFT_FOOT:1.2, HIP:1.2,\
RIGHT_UP_LEG:.9, RIGHT_LEG:.9, LEFT_UP_LEG:.9, LEFT_LEG:.9}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2,\
SPINE:1.5, LEFT_FOOT:1., HIP:1.5,\
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.1, RIGHT_TOES:.1, LEFT_CALCA:.1, RIGHT_CALCA:.1}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['leftForeFoot'] = LEFT_TOES
config['rightForeFoot'] = RIGHT_TOES
config['leftRearFoot'] = LEFT_CALCA
config['rightRearFoot'] = RIGHT_CALCA
return motion, mcfg, wcfg, stepsPerFrame, config
def test_CM_CP():
# bvhFilePath = '../samples/chain_6.bvh'
# bvhFilePath = '../samples/block_3_rotate.bvh'
# bvhFilePath = '../samples/block_tree_rotate.bvh'
bvhFilePath = '../samples/chain_2.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('link0')
node.density = 200.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
# wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
motionModel.recordVelByFiniteDiff()
controlModel.translateByOffset((0,.5,.5))
CMPos_cm = [mm.O_Vec3()]
CMVel_cm = [mm.O_Vec3()]
CMPos_mm = [mm.O_Vec3()]
CPPos_cm = [mm.O_Vec3()]
CPVel_cm = [mm.O_Vec3()]
bodyMasses = controlModel.getBodyMasses()
totalMass = 0.
for m in bodyMasses:
totalMass += m
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [.5]*len(bodyIDsToCheck)
contactPositions= []
contactForces = []
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
# viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (100,100,100), yr.POLYGON_LINE))
viewer.doc.addRenderer('CMPos_cm', yr.PointsRenderer(CMPos_cm, (255,0,255)))
viewer.doc.addRenderer('CMVel_cm', yr.VectorsRenderer(CMVel_cm, CMPos_cm, (255,0,255)))
viewer.doc.addRenderer('CMPos_mm', yr.PointsRenderer(CMPos_mm, (200,0,200)))
viewer.doc.addRenderer('CPPos_cm', yr.PointsRenderer(CPPos_cm, (0,255,0)))
viewer.doc.addRenderer('CPVel_cm', yr.VectorsRenderer(CPVel_cm, CPPos_cm, (0,255,0)))
viewer.doc.addRenderer('contactPositions', yr.PointsRenderer(contactPositions, (0,0,255), yr.POINT_POINT))
viewer.doc.addRenderer('contactForces', yr.VectorsRenderer(contactForces, contactPositions,(0,0,255)))
def simulateCallback(frame):
controlModel.applyBodyTorqueGlobal(1, (0,10,0))
for i in range(stepsPerFrame):
bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, 1000, 2*(1000**.5))
vpWorld.applyPenaltyForce(bodyIDs, positionLocals, forces)
vpWorld.step()
del contactPositions[:]
del contactForces[:]
contactPositions[:] = positions
contactForces[:] = forces
motionModel.update(motion[frame])
bodyPositions = controlModel.getBodyPositionsGlobal()
bodyVelocities = controlModel.getBodyVelocitiesGlobal()
CMPos_cm[0] = yrp.getCM(bodyPositions, bodyMasses, totalMass)
CMVel_cm[0] = yrp.getCM(bodyVelocities, bodyMasses, totalMass)
bodyPositions_ref = motionModel.getBodyPositionsGlobal()
bodyVelocities_ref = motionModel.getBodyVelocitiesGlobal()
CMPos_mm[0] = yrp.getCM(bodyPositions_ref, bodyMasses, totalMass)
CPPos_cm_old = CPPos_cm[0]
CPPos_cm[0] = yrp.getCP(positions, forces)
if CPPos_cm_old==None or CPPos_cm[0]==None:
CPVel_cm[0] = None
else:
CPVel_cm[0] = (CPPos_cm[0] - CPPos_cm_old)/(1./30.)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_momentum_standard():
np.set_printoptions(precision=2, linewidth=200)
bvhFilePath = '../samples/chain_1_long.bvh'
motion1 = yf.readBvhFile(bvhFilePath)
bvhFilePath = '../samples/chain_2.bvh'
motion2 = yf.readBvhFile(bvhFilePath)
mcfg1 = ypc.ModelConfig()
mcfg1.defaultDensity = 1000.
mcfg1.defaultBoneRatio = 1.
for i in range(motion1[0].skeleton.getElementNum()):
mcfg1.addNode(motion1[0].skeleton.getElementName(i))
mcfg2 = ypc.ModelConfig()
mcfg2.defaultDensity = 1000.
mcfg2.defaultBoneRatio = 1.
for i in range(motion2[0].skeleton.getElementNum()):
mcfg2.addNode(motion2[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.gravity = (0, 0, 0)
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
m1 = cvm.VpControlModel(vpWorld, motion1[0], mcfg1)
m2 = cvm.VpControlModel(vpWorld, motion2[0], mcfg2)
vpWorld.initialize()
force = 1000
# force = 0
torque = 400
m1.translateByOffset((0, 1, 1))
m1.applyBodyTorqueGlobal(0, (0, 0, torque))
m1.applyBodyForceGlobal(0, (force, 0, 0))
m2.translateByOffset((0, 1, 0))
m2.applyBodyTorqueGlobal(0, (0, 0, torque / 2.))
m2.applyBodyTorqueGlobal(1, (0, 0, torque / 2.))
m2.applyBodyForceGlobal(0, (force / 2., 0, 0))
m2.applyBodyForceGlobal(1, (force / 2., 0, 0))
masses_m1 = m1.getBodyMasses()
masses_m2 = m2.getBodyMasses()
totalMass_m1 = m1.getTotalMass()
totalMass_m2 = m2.getTotalMass()
p = []
v = []
CM = []
L_std = []
H_std = []
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'model', cvr.VpModelRenderer(m2, (255, 240, 255), yr.POLYGON_LINE))
viewer.doc.addRenderer(
'model2', cvr.VpModelRenderer(m1, (255, 240, 255),
yr.POLYGON_LINE))
# viewer.doc.addRenderer('v', yr.VectorsRenderer(v, p, (0,255,0)))
viewer.doc.addRenderer('L_std',
yr.VectorsRenderer(L_std, CM, (255, 0, 0)))
viewer.doc.addRenderer('H_std',
yr.VectorsRenderer(H_std, CM, (255, 0, 0)))
viewer.setMaxFrame(100)
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
velocities_m1 = m1.getBodyVelocitiesGlobal()
velocities_m2 = m2.getBodyVelocitiesGlobal()
positions_m1 = m1.getBodyPositionsGlobal()
positions_m2 = m2.getBodyPositionsGlobal()
CM_m1 = m1.getBodyPositionGlobal(0)
CM_m2 = yrp.getCM(positions_m2, masses_m2, totalMass_m2)
inertias_m1 = m1.getBodyInertiasGlobal()
inertias_m2 = m2.getBodyInertiasGlobal()
angVelocities_m1 = m1.getBodyAngVelocitiesGlobal()
angVelocities_m2 = m2.getBodyAngVelocitiesGlobal()
L1_std = getLinearMomentum(masses_m1, velocities_m1)
L2_std = getLinearMomentum(masses_m2, velocities_m2)
H1_std = getAngularMomentum(CM_m1, inertias_m1, angVelocities_m1,
positions_m1, masses_m1, velocities_m1)
H2_std = getAngularMomentum(CM_m2, inertias_m2, angVelocities_m2,
positions_m2, masses_m2, velocities_m2)
#===============================================================================
# for rendering
#===============================================================================
p[:] = m1.getBodyPositionsGlobal() + m2.getBodyPositionsGlobal()
v[:] = m1.getBodyVelocitiesGlobal() + m2.getBodyVelocitiesGlobal()
CM[:] = [
yrp.getCM(m1.getBodyPositionsGlobal(), m1.getBodyMasses()),
CM_m2
]
L_std[:] = [L1_std, L2_std]
H_std[:] = [H1_std, H2_std]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_biped_delNode():
def buildMassMap():
massMap = {}
massMap = massMap.fromkeys([
'Head', 'Head_Effector', 'Hips', 'LeftArm', 'LeftFoot',
'LeftForeArm', 'LeftHand', 'LeftHand_Effector', 'LeftLeg',
'LeftShoulder1', 'LeftToes', 'LeftToes_Effector', 'LeftUpLeg',
'RightArm', 'RightFoot', 'RightForeArm', 'RightHand',
'RightHand_Effector', 'RightLeg', 'RightShoulder', 'RightToes',
'RightToes_Effector', 'RightUpLeg', 'Spine', 'Spine1'
], 0.)
# torso : 10
massMap['Hips'] += 2.
massMap['Spine'] += 6.
massMap['Spine1'] += 2.
# head : 3
massMap['Spine1'] += 1.
massMap['Head'] += 2.
# right upper arm : 2
massMap['Spine1'] += .5
massMap['RightShoulder'] += .5
massMap['RightArm'] += 1.
# left upper arm : 2
massMap['Spine1'] += .5
massMap['LeftShoulder1'] += .5
massMap['LeftArm'] += 1.
# right lower arm : 1
massMap['RightForeArm'] = .8
massMap['RightHand'] = .2
# left lower arm : 1
massMap['LeftForeArm'] = .8
massMap['LeftHand'] = .2
# right thigh : 7
massMap['Hips'] += 2.
massMap['RightUpLeg'] += 5.
# left thigh : 7
massMap['Hips'] += 2.
massMap['LeftUpLeg'] += 5.
# right shin : 5
massMap['RightLeg'] += 5.
# left shin : 5
massMap['LeftLeg'] += 5.
# right foot : 4
massMap['RightFoot'] += 2.
massMap['RightToes'] += 2.
# left foot : 4
massMap['LeftFoot'] += 2.
massMap['LeftToes'] += 2.
return massMap
massMap = buildMassMap()
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
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('Head')
node.length = .2
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
node = mcfg.getNode('LeftFoot')
node.length = .25
mcfg.delNode('Spine1')
mcfg.delNode('RightShoulder')
mcfg.delNode('LeftShoulder1')
mcfg.delNode('RightHand')
mcfg.delNode('LeftHand')
mcfg.delNode('RightToes')
mcfg.delNode('LeftToes')
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 10
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
# controlModel.fixRootBody()
print controlModel
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (0, 0, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
for i in range(stepsPerFrame):
# torques = controlModel.calcPDTorque(motion[frame])
# controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_penalty_model():
bvhFilePath = '../samples/chain_1.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.gravity = (0,-9.8,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel3 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.translateByOffset((0,1,0))
controlModel2.translateByOffset((0,1,.5))
controlModel3.translateByOffset((0,1,1.))
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1., .2, .1]
Ks = 1000; Ds = 2*(Ks**.5)
contactPositions= []
contactForces = []
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model3', cvr.VpModelRenderer(controlModel3, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('contactPositions', yr.PointsRenderer(contactPositions, (0,255,0), yr.POINT_POINT))
viewer.doc.addRenderer('contactForces', yr.VectorsRenderer(contactForces, contactPositions,(0,255,0)))
viewer.setMaxFrame(500)
def simulateCallback(frame):
for i in range(stepsPerFrame):
controlModel.applyBodyForceGlobal(0, (1,0,0))
controlModel2.applyBodyForceGlobal(0, (1,0,0))
controlModel3.applyBodyForceGlobal(0, (1,0,0))
# get penalty forces
bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, 1000, 10)
# apply penalty forces
vpWorld.applyPenaltyForce(bodyIDs, positionLocals, forces)
vpWorld.step()
contactPositions[:] = positions
contactForces[:] = forces
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
if MOTION != WALK:
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
elif (FOOT_PART_NUM == 5 or FOOT_PART_NUM == 6) and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
if MOTION != WALK:
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
if FOOT_PART_NUM == 1:
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(2.5, -0.0, .3), -.5), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False)
if MOTION == WALK:
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1., -0.0, .0),
.4), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1., 0.0, -.0), .4), False)
#yme.rotateJointLocal(motion, SPINE1, mm.exp(mm.v3(1.,0.0,0.0), -.8), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM == 6:
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1,
(-0.045, -0.06, 0.03))
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3,
(0.045, -0.06, 0.03)) #-0.0037
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (0.0, -0.06, -0.02))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1,
(0.045, -0.06, 0.03))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3,
(-0.045, -0.06 - 0.0062, 0.03 + 0.0035))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1,
(0.0, -0.06, -0.02))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
'''
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
'''
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2012), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2172), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2283), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -0.05), False)
'''
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0656), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0871), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0087), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0933), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0746), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -6.3773e-03), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.5), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .35
width1 = .2
mass1 = 4.3
elif FOOT_PART_NUM == 6:
mass0 = .4
width0 = 0.028
length0 = 0.1
#Metatarsal1
length1 = .15
width1 = width0 * 3
mass1 = mass0 * 1.8
#Metatarsal3
length3 = .11
width3 = width0 * 3
mass3 = mass0 * 1.86
#Calcaneus1
length4 = .08
width4 = 0.15 * 1
mass4 = mass0 * 1.2 * 2.
#Phalange1
length5 = .08
width5 = width1
mass5 = mass0 * 1.355
#Phalange3
length7 = length5
width7 = width5
mass7 = mass5
#Talus
length8 = .13
width8 = width0 * 3
mass8 = mass0 * 2.
if FOOT_PART_NUM == 6:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 6:
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3,
LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS:1.2, LEFT_CALCANEUS:1.2,
LEFT_PHALANGE_1:.2, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.2, RIGHT_PHALANGE_3:.2}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.8, SPINE1:.3, RIGHT_FOOT:1., LEFT_FOOT:1., HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1.,
LEFT_METATARSAL_3:1., RIGHT_METATARSAL_3:1., RIGHT_CALCANEUS:.3, LEFT_CALCANEUS:.3,
LEFT_PHALANGE_1:.3, LEFT_PHALANGE_3:.3, RIGHT_PHALANGE_1:.3, RIGHT_PHALANGE_3:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 6:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_3, LEFT_CALCANEUS_1
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
#motionName = 'ww13_41_V001.bvh'
motion = yf.readBvhFile(motionName, .01)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5),
False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM > 1:
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS)
yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1')
yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS)
yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1')
yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0),
.45), False)
yme.rotateJointLocal(motion, RIGHT_TOES,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
yme.rotateJointLocal(motion, RIGHT_TARSUS,
mm.exp(mm.v3(1., 0.0, 0.0), .52), False)
if FOOT_PART_NUM == 5:
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_L)
yme.addJoint(motion, LEFT_FOOT_SIDE_L, 'LEFT_Dummy2')
yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_R)
yme.addJoint(motion, LEFT_FOOT_SIDE_R, 'LEFT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_L)
yme.addJoint(motion, RIGHT_FOOT_SIDE_L, 'RIGHT_Dummy2')
yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_R)
yme.addJoint(motion, RIGHT_FOOT_SIDE_R, 'RIGHT_Dummy2')
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_L,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_R,
mm.exp(mm.v3(1., 0.0, 0.0), .45), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
## Taekwondo turning-kick
#motion = motion[108:-1]
#motion = motion[108:109]
motion[0:0] = [motion[0]] * 100
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .25
width1 = .2
mass1 = 4.
elif FOOT_PART_NUM == 3:
length1 = .1
width1 = .2
mass1 = 1.5
length2 = .1
width2 = .2
mass2 = 1.5
elif FOOT_PART_NUM == 5:
length1 = .1
width1 = .065
mass1 = .5
length2 = .1
width2 = .2
mass2 = 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
if FOOT_PART_NUM == 5:
node = mcfg.getNode(LEFT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(LEFT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_L)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (0.07, 0.0, 0.015)
node = mcfg.getNode(RIGHT_FOOT_SIDE_R)
node.length = length1
node.width = width1
node.mass = mass1
node.offset = (-0.07, 0.0, 0.015)
if FOOT_PART_NUM > 1:
node = mcfg.getNode(LEFT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(RIGHT_TOES)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.02)
node = mcfg.getNode(LEFT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
node = mcfg.getNode(RIGHT_TARSUS)
node.length = length2
node.width = width2
node.mass = mass2
node.offset = (0, 0.0, -0.08)
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 3:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .3,
RIGHT_LEG: .3,
LEFT_UP_LEG: .3,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3
}
elif FOOT_PART_NUM == 5:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 2.5,
LEFT_FOOT: 2.5,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.,
LEFT_TOES: .3,
RIGHT_TOES: .3,
LEFT_TARSUS: .3,
RIGHT_TARSUS: .3,
LEFT_FOOT_SIDE_L: .3,
LEFT_FOOT_SIDE_R: .3,
RIGHT_FOOT_SIDE_L: .3,
RIGHT_FOOT_SIDE_R: .3
}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = 'HIP'
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L,
RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
def test_biped_motion_edit():
massMap = buildMassMap()
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
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.updateGlobalT(motion)
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('Head')
# node.length = .2
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 = mcfg.getNode('LeftFoot')
node.length = .25
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
print motion[0].skeleton
print
print controlModel
viewer = ysv.SimpleViewer()
# viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), cvr.POLYGON_FILL))
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (255,240,255), cvr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_momentum_matrix_build():
bvhFilePath = '../samples/chain_2.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0, 0, 0)
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.fixBody(0)
# controlModel.setJointAngVelocityLocal(0, (.5,0,0))
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
totalDOF = controlModel.getTotalDOF()
jointDOFs = controlModel.getDOFs()
J = yjc.makeEmptyJacobian(jointDOFs, controlModel.getBodyNum())
jointPositions = [None] * totalDOF
VERBOSE = True
# VERBOSE = False
if VERBOSE:
np.set_printoptions(precision=1, linewidth=200)
TO = make_TO(linkMasses)
dTO = make_dTO(len(linkMasses))
if VERBOSE:
print('TO')
print(TO)
print('dTO')
print(dTO)
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
# viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
cvr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_LINE))
def simulateCallback(frame):
linkPositions = controlModel.getBodyPositionsGlobal()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
inertias = controlModel.getBodyInertiasGlobal()
P = getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
inertias)
linkVelocities = controlModel.getBodyVelocitiesGlobal()
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
dP = getPureInertiaMatrixDerivative(
dTO, linkMasses, linkVelocities, dCM,
controlModel.getBodyAngVelocitiesGlobal(), inertias)
if VERBOSE:
print('P')
print(P)
print('dP')
print(dP)
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def get_mcfg():
massMap = {}
massMap = massMap.fromkeys(['Head', 'Head_Effector', 'Hips', 'LeftArm', 'LeftFoot', 'LeftForeArm', 'LeftHand', 'LeftHand_Effector', 'LeftLeg', 'LeftShoulder1', 'LeftToes', 'LeftToes_Effector', 'LeftUpLeg', 'RightArm', 'RightFoot', 'RightForeArm', 'RightHand', 'RightHand_Effector', 'RightLeg', 'RightShoulder', 'RightToes', 'RightToes_Effector', 'RightUpLeg', 'Spine', 'Spine1'], 0.)
# torso : 10
massMap['Hips'] += 2.
massMap['Spine'] += 8.
# head : 3
massMap['Spine1'] += 3.
# right upper arm : 2
massMap['RightArm'] += 2.
# left upper arm : 2
massMap['LeftArm'] += 2.
# right lower arm : 1
# massMap['RightForeArm'] = 1.
massMap['RightForeArm'] = 2.
# left lower arm : 1
# massMap['LeftForeArm'] = 1.
massMap['LeftForeArm'] = 2.
# right thigh : 7
massMap['Hips'] += 2.
massMap['RightUpLeg'] += 5.
# left thigh : 7
massMap['Hips'] += 2.
massMap['LeftUpLeg'] += 5.
# right shin : 5
massMap['RightLeg'] += 5.
# left shin : 5
massMap['LeftLeg'] += 5.
# right foot : 4
massMap['RightFoot'] += 2.
# left foot : 4
massMap['LeftFoot'] += 2.
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 = mcfg.getNode('LeftFoot')
node.length = .25
return mcfg
# left shin : 5
massMap['LeftLeg'] += 5.
# right foot : 4
massMap['RightFoot'] += 2.
# left foot : 4
massMap['LeftFoot'] += 2.
return massMap
massMap = buildMassMap()
if __name__ == '__main__':
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
totalMass = 0.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
totalMass += node.mass
print(totalMass)
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
def test_momentum_matrix():
np.set_printoptions(precision=2, linewidth=200)
# bvhFilePath = '../samples/chain_1_long.bvh'
bvhFilePath = '../samples/chain_3_rotate.bvh'
motion1 = yf.readBvhFile(bvhFilePath)
mcfg1 = ypc.ModelConfig()
mcfg1.defaultDensity = 1000.
mcfg1.defaultBoneRatio = 1.
for i in range(motion1[0].skeleton.getElementNum()):
mcfg1.addNode(motion1[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0, 0, 0)
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
m1 = cvm.VpControlModel(vpWorld, motion1[0], mcfg1)
vpWorld.initialize()
# momentum matrix information
masses = m1.getBodyMasses()
totalMass = m1.getTotalMass()
TO = make_TO(masses)
v_sol = ype.makeNestedList([6] * m1.getBodyNum())
# jacobian for internal joints
DOFs_internal = m1.getInternalJointDOFs()
totalDOF_internal = m1.getTotalInternalJointDOF()
J_internal = yjc.makeEmptyJacobian(DOFs_internal, m1.getBodyNum())
linkJointMasks_internal = yjc.getAllLinkInternalJointMasks(
motion1[0].skeleton)
dth_flat_internal = ype.makeFlatList(totalDOF_internal)
# momentum matrix for all joints
DOFs_all = m1.getDOFs()
totalDOF_all = m1.getTotalDOF()
J_all = yjc.makeEmptyJacobian(DOFs_all, m1.getBodyNum())
linkJointMasks_all = yjc.getAllLinkJointMasks(motion1[0].skeleton)
dth_flat_all = ype.makeFlatList(totalDOF_all)
p = []
v = []
rd_CM = []
rd_L_std = []
rd_L_jacob_internal = []
rd_L_jacob_all = []
rd_H_std = []
rd_H_jacob_internal = []
rd_H_jacob_all = []
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'model', cvr.VpModelRenderer(m1, (255, 240, 255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('v', yr.VectorsRenderer(v, p, (0,255,0)))
viewer.doc.addRenderer(
'L_std', yr.VectorsRenderer(rd_L_std, rd_CM, (255, 0, 0)))
# viewer.doc.addRenderer('L_jacob_internal', yr.VectorsRenderer(rd_L_jacob_internal, rd_CM, (0,255,0)))
viewer.doc.addRenderer(
'L_jacob_all',
yr.VectorsRenderer(rd_L_jacob_all, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer(
'H_std', yr.VectorsRenderer(rd_H_std, rd_CM, (255, 0, 0)))
# viewer.doc.addRenderer('H_jacob_internal', yr.VectorsRenderer(rd_H_jacob_internal, rd_CM, (0,255,0)))
viewer.doc.addRenderer(
'H_jacob_all',
yr.VectorsRenderer(rd_H_jacob_all, rd_CM, (255, 255, 0)))
viewer.setMaxFrame(100)
# force
m1.applyBodyTorqueGlobal(0, (0, 0, 1000))
m1.applyBodyForceGlobal(0, (1000, 0, 0))
m1.applyBodyTorqueGlobal(0, (0, 1000, 0))
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
#===============================================================================
# momentum calculation by standard method
#===============================================================================
velocities = m1.getBodyVelocitiesGlobal()
positions = m1.getBodyPositionsGlobal()
CM = yrp.getCM(positions, masses, totalMass)
inertias = m1.getBodyInertiasGlobal()
angVelocities = m1.getBodyAngVelocitiesGlobal()
L_std = getLinearMomentum(masses, velocities)
H_std = getAngularMomentum(CM, inertias, angVelocities, positions,
masses, velocities)
#===============================================================================
# momentum calculation by centroidal momentum matrix
#===============================================================================
P = getPureInertiaMatrix(TO, masses, positions, CM, inertias)
# momentum matrix for internal joints and addition of total momentum about CM
# jointPositions_internal = m1.getInternalJointPositionsGlobal()
#
# Rs = m1.getInternalJointOrientationsGlobal()
# jointAxeses_internal = [Rs[i].transpose() for i in range(0,len(Rs))]
#
# yjc.computeJacobian2(J_internal, DOFs_internal, jointPositions_internal, jointAxeses_internal, positions, linkJointMasks_internal)
#
# dth = m1.getInternalJointAngVelocitiesLocal()
# ype.flatten(dth, dth_flat_internal)
#
# PJ_internal = np.dot(P, J_internal)
# LH_internal = np.dot(PJ_internal, dth_flat_internal)
# L2_jacob_internal, H2_jacob_internal = np.hsplit(LH_internal, 2)
#
# p_root = m1.getBodyPositionGlobal(0)
# v_root = m1.getBodyVelocityGlobal(0)
# w_root = m1.getBodyAngVelocityGlobal(0)
#
# L_jacob_internal = mm.v3(0.,0.,0.)
# L_jacob_internal += (totalMass * v_root)
# L_jacob_internal += (-totalMass * np.cross( (CM - p_root), w_root))
#
L_jacob_internal = None
H_jacob_internal = None
# momentum matrix for all joints
jointPositions_all = m1.getJointPositionsGlobal()
jointAxeses_all = m1.getDOFAxeses()
yjc.computeJacobian2(J_all, DOFs_all, jointPositions_all,
jointAxeses_all, positions,
linkJointMasks_all)
dth = m1.getDOFVelocities()
ype.flatten(dth, dth_flat_all)
PJ_all = np.dot(P, J_all)
LH_all = np.dot(PJ_all, dth_flat_all)
L_jacob_all, H_jacob_all = np.hsplit(LH_all, 2)
#===============================================================================
# for rendering
#===============================================================================
p[:] = positions
v[:] = velocities
rd_CM[:] = [CM]
rd_L_std[:] = [L_std]
rd_L_jacob_internal[:] = [L_jacob_internal]
rd_L_jacob_all[:] = [L_jacob_all]
rd_H_std[:] = [H_std]
rd_H_jacob_internal[:] = [H_jacob_internal]
rd_H_jacob_all[:] = [H_jacob_all]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_getDesiredAngAccelerations():
# motion = yf.readBvhFile('block_3_rotate.bvh', 1)
motion = yf.readBvhFile('../samples/block_tree_rotate.bvh', 1)
motion = motion[0:]
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0, 0, 0)
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
controlModel.fixBody(0)
p = []
ddth_des = []
viewer = ysv.SimpleViewer()
# viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 255, 255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (255, 240, 255), yr.POLYGON_LINE))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer('ddth_des',
yr.VectorsRenderer(ddth_des, p, (255, 0, 0)))
def simulateCallback(frame):
th_r = motion.getInternalJointOrientationsLocal(frame)
th = controlModel.getInternalJointOrientationsLocal()
dth_r = motion.getInternalJointAngVelocitiesLocal(frame)
dth = controlModel.getInternalJointAngVelocitiesLocal()
ddth_r = motion.getInternalJointAngAccelerationsLocal(frame)
ddth_des[:] = getDesiredAngAccelerations(th_r, th, dth_r, dth,
ddth_r, 1, 1)
for i in range(stepsPerFrame):
controlModel.setInternalJointAngAccelerationsLocal(ddth_des)
controlModel.solveHybridDynamics()
vpWorld.step()
motionModel.update(motion[frame])
p[:] = motion.getInternalJointPositionsGlobal(frame)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_joint_pos_vel_acc_funcs_and_tracking():
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None]*len(th_r)
for i in range(len(th_r)):
ddth_des[i] = Kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + Dt*(dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
def getDesiredAcceleration(p_r, p, v_r, v, a_r, Kt, Dt):
return Kt*(p_r - p) + Dt*(v_r - v) + a_r
# bvhFilePath = '../samples/chain_1.bvh'
# bvhFilePath = '../samples/block_tree_rotate.bvh'
bvhFilePath = '../samples/chain_3_rotate_freely_move.bvh'
# bvhFilePath = '../samples/chain_3_rotate_freely.bvh'
# bvhFilePath = '../samples/chain_3_rotate_freely_expt_root.bvh'
# bvhFilePath = '../samples/chain_3_rotate.bvh'
# bvhFilePath = '../samples/chain_3_rotate_expt_root.bvh'
# bvhFilePath = '../samples/chain_6_rotate_expt_root.bvh'
# bvhFilePath = '../samples/chain_2_rotate_2axes.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0,0,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics(False)
# controlModel.fixBody(0)
controlModel.rotate(mm.exp(mm.v3(0,1,0)))
print controlModel
motion_p = []; motion_v = []; motion_a = []
motion_ap = []; motion_av = []; motion_aa = []
motion_ap_local = []; motion_av_local = []; motion_aa_local = []
model_p = []; model_v = []; model_a = []
model_ap = []; model_av = []; model_aa = []
model_ap_local = []; model_av_local = []; model_aa_local = []
model_body_p = []; model_body_a = []
prev_model_v = [(0.,0.,0.)]*controlModel.getJointNum()
prev_model_av = [(0.,0.,0.)]*controlModel.getJointNum()
prev_model_av_local = [(0.,0.,0.)]*controlModel.getJointNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('motion_p', yr.PointsRenderer(motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_p', yr.PointsRenderer(model_p, (0,255,0)))
#
# viewer.doc.addRenderer('motion_v', yr.VectorsRenderer(motion_v, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_v', yr.VectorsRenderer(model_v, model_p, (0,255,0)))
viewer.doc.addRenderer('motion_a', yr.VectorsRenderer(motion_a, motion_p, (255,0,0)))
viewer.doc.addRenderer('model_a', yr.VectorsRenderer(model_a, model_p, (0,255,0)))
# viewer.doc.addRenderer('model_body_a', yr.VectorsRenderer(model_body_a, model_body_p, (255,255,0)))
# viewer.doc.addRenderer('motion_ap', yr.OrientationsRenderer(motion_ap, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_ap', yr.OrientationsRenderer(model_ap, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_ap_local', yr.OrientationsRenderer(motion_ap_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_ap_local', yr.OrientationsRenderer(model_ap_local, model_p, (100,255,100)))
# viewer.doc.addRenderer('motion_av', yr.VectorsRenderer(motion_av, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_av', yr.VectorsRenderer(model_av, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_av_local', yr.VectorsRenderer(motion_av_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_av_local', yr.VectorsRenderer(model_av_local, model_p, (100,255,100)))
# viewer.doc.addRenderer('motion_aa', yr.VectorsRenderer(motion_aa, motion_p, (255,0,0)))
# viewer.doc.addRenderer('model_aa', yr.VectorsRenderer(model_aa, model_p, (0,255,0)))
# viewer.doc.addRenderer('motion_aa_local', yr.VectorsRenderer(motion_aa_local, motion_p, (255,100,100)))
# viewer.doc.addRenderer('model_aa_local', yr.VectorsRenderer(model_aa_local, model_p, (100,255,100)))
Kt = 200; Dt = 2*(Kt**.5)
def simulateCallback(frame):
th_r = motion.getJointOrientationsLocal(frame)
th = controlModel.getJointOrientationsLocal()
dth_r = motion.getJointAngVelocitiesLocal(frame)
dth = controlModel.getJointAngVelocitiesLocal()
ddth_r = motion.getJointAngAccelerationsLocal(frame)
ddth_des = getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
p_r = motion.getJointPositionGlobal(0, frame)
p = controlModel.getJointPositionGlobal(0)
v_r = motion.getJointVelocityGlobal(0, frame)
v = controlModel.getJointVelocityGlobal(0)
a_r = motion.getJointAccelerationGlobal(0, frame)
a_des = getDesiredAcceleration(p_r, p, v_r, v, a_r, Kt, Dt)
th_r0 = motion.getJointOrientationGlobal(0, frame)
th0 = controlModel.getJointOrientationGlobal(0)
dth_r0 = motion.getJointAngVelocityGlobal(0, frame)
dth0 = controlModel.getJointAngVelocityGlobal(0)
ddth_r0 = motion.getJointAngAccelerationGlobal(0, frame)
ddth_des0 = getDesiredAngAccelerations([th_r0], [th0], [dth_r0], [dth0], [ddth_r0], Kt, Dt)[0]
for i in range(stepsPerFrame):
# controlModel.setBodyAccelerationGlobal(0, a_des)
# controlModel.setJointAngAccelerationsLocal(ddth_des)
controlModel.setJointAccelerationGlobal(0, a_des)
controlModel.setJointAngAccelerationGlobal(0, ddth_des0)
controlModel.setInternalJointAngAccelerationsLocal(ddth_des[1:])
controlModel.solveHybridDynamics()
vpWorld.step()
motion_p[:] = motion.getJointPositionsGlobal(frame)
motion_v[:] = motion.getJointVelocitiesGlobal(frame)
motion_a[:] = motion.getJointAccelerationsGlobal(frame)
motion_ap[:] = motion.getJointOrientationsGlobal(frame)
motion_av[:] = motion.getJointAngVelocitiesGlobal(frame)
motion_aa[:] = motion.getJointAngAccelerationsGlobal(frame)
motion_ap_local[:] = motion.getJointOrientationsLocal(frame)
motion_av_local[:] = motion.getJointAngVelocitiesLocal(frame)
motion_aa_local[:] = motion.getJointAngAccelerationsLocal(frame)
model_p[:] = controlModel.getJointPositionsGlobal()
model_v[:] = controlModel.getJointVelocitiesGlobal()
# model_a[:] = controlModel.getJointAccelerationsGlobal()
model_a[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_v, prev_model_v)
prev_model_v[:] = model_v
model_ap[:] = controlModel.getJointOrientationsGlobal()
model_av[:] = controlModel.getJointAngVelocitiesGlobal()
# model_aa[:] = controlModel.getJointAngAccelerationsGlobal()
model_aa[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_av, prev_model_av)
prev_model_av[:] = model_av
model_ap_local[:] = controlModel.getJointOrientationsLocal()
model_av_local[:] = controlModel.getJointAngVelocitiesLocal()
# model_aa_local[:] = controlModel.getJointAngAccelerationsLocal()
model_aa_local[:] = map(lambda v1,v0: (v1-v0)/(1/30.), model_av_local, prev_model_av_local)
prev_model_av_local[:] = model_av_local
model_body_p[:] = controlModel.getBodyPositionsGlobal()
model_body_a[:] = controlModel.getBodyAccelerationsGlobal()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
