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()