def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global vpWorld
global controlModel
global totalDOF
global DOFs
global bodyIDsToCheck
global torques_nested
global ddth_des_flat
global dth_flat
global ddth_sol
global rd_cForces
global rd_cPositions
global rd_jointPos
global rd_cForcesControl
global rd_cPositionsControl
global rd_ForceControl
global rd_ForceDes
global rd_Position
global rd_PositionDes
global viewer
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_1()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_chiken_foot()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('fastswim.bvh')
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot(
'simpleJump.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_leg('kneeAndFoot.bvh')
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.SetIntegrator("RK4")
# vpWorld.SetIntegrator("IMPLICIT_EULER_FAST")
vpWorld.SetGlobalDamping(0.9999)
# controlModel.initializeHybridDynamics()
controlModel.initializeForwardDynamics()
ModelOffset = np.array([0., .7, 0.])
controlModel.translateByOffset(ModelOffset)
vpWorld.initialize()
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
bodyIDsToCheck = range(vpWorld.getBodyNum())
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
torques_nested = ype.makeNestedList(DOFs)
rd_cForces = [None]
rd_cPositions = [None]
rd_cForcesControl = [None]
rd_cPositionsControl = [None]
rd_ForceControl = [None]
rd_ForceDes = [None]
rd_Position = [None]
rd_PositionDes = [None]
rd_jointPos = [None]
viewer = hsv.hpSimpleViewer(title='main_Test')
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL))
viewer.doc.addRenderer(
'rd_contactForcesControl',
yr.VectorsRenderer(rd_cForcesControl, rd_cPositionsControl,
(255, 0, 0), .1))
viewer.doc.addRenderer(
'rd_contactForces',
yr.VectorsRenderer(rd_cForces, rd_cPositions, (0, 255, 0), .1))
viewer.doc.addRenderer(
'rd_contactForceControl',
yr.VectorsRenderer(rd_ForceControl, rd_Position, (0, 0, 255), .1))
viewer.doc.addRenderer(
'rd_contactForceDes',
yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1))
# viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos))
viewer.objectInfoWnd.add1DSlider('PD gain',
minVal=0.,
maxVal=200.,
initVal=10.,
valStep=.1)
viewer.objectInfoWnd.add1DSlider('Joint Damping',
minVal=1.,
maxVal=2000.,
initVal=35.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('steps per frame',
minVal=1.,
maxVal=200.,
initVal=config['stepsPerFrame'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('1/simul speed',
minVal=1.,
maxVal=100.,
initVal=config['simulSpeedInv'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force min',
minVal=0.,
maxVal=1000.,
initVal=80.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force max',
minVal=0.,
maxVal=1000.,
initVal=80.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force begin',
minVal=0.,
maxVal=len(motion) - 1,
initVal=70.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force dur',
minVal=0.,
maxVal=len(motion) - 1,
initVal=20.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('force weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('tracking weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('tau weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump)
viewer.cForceWnd.addDataSet('expForce', FL_BLACK)
viewer.cForceWnd.addDataSet('desForceMin', FL_RED)
viewer.cForceWnd.addDataSet('desForceMax', FL_RED)
viewer.cForceWnd.addDataSet('realForce', FL_GREEN)
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
print "(index, id, name)"
for i in range(controlModel.getBodyNum()):
print(i, controlModel.index2id(i), controlModel.index2name(i))
def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global vpWorld
global controlModel
global totalDOF
global DOFs
global bodyIDsToCheck
global torques_nested
global ddth_des_flat
global dth_flat
global ddth_sol
global rd_cForces
global rd_cPositions
global rd_jointPos
global rd_cForcesControl
global rd_cPositionsControl
global rd_ForceControl
global rd_ForceDes
global rd_Position
global rd_PositionDes
global viewer
global motionModel
global solver
global IKModel
global dartModel
global pdController
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_1()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_chiken_foot()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('fastswim.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot_2('simpleJump_2.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_capsule('simpleJump_onebody.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump_long.bvh')
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot(
'simpleJump_long.bvh')
mcfg_motion = mit.normal_mcfg()
dartModel = cpm.DartModel(wcfg, motion[0], mcfg, False)
pdController = PDController(dartModel.skeleton,
dartModel.world.time_step())
# dartWorld.skeletons[1].controller = PDController(dartWorld.skeletons[1], dartWorld.dt)
# solver = hik.numIkSolver(wcfg, motion[0], mcfg)
ModelOffset = np.array([0., .12, 0.])
dartModel.translateByOffset(ModelOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
bodyIDsToCheck = range(dartModel.getBodyNum())
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
torques_nested = ype.makeNestedList(DOFs)
rd_cForces = [None]
rd_cPositions = [None]
rd_cForcesControl = [None]
rd_cPositionsControl = [None]
rd_ForceControl = [None]
rd_ForceDes = [None]
rd_Position = [None]
rd_PositionDes = [None]
rd_jointPos = [None]
viewer = hsv.hpSimpleViewer(title='main_Test')
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('dartModel', yr.DartModelRenderer(dartWorld, CHARACTER_COLOR2))
viewer.doc.addRenderer('dartModel',
yr.DartModelRenderer(dartModel, CHARACTER_COLOR2))
viewer.doc.addRenderer(
'rd_contactForcesControl',
yr.VectorsRenderer(rd_cForcesControl, rd_cPositionsControl,
(255, 0, 0), .1, 'rd_c1'))
viewer.doc.addRenderer(
'rd_contactForces',
yr.VectorsRenderer(rd_cForces, rd_cPositions, (0, 255, 0), .1,
'rd_c2'))
viewer.doc.addRenderer(
'rd_contactForceControl',
yr.VectorsRenderer(rd_ForceControl, rd_Position, (0, 0, 255), .1,
'rd_c3'))
# viewer.doc.addRenderer('rd_contactForceDes', yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1))
# viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos))
viewer.objectInfoWnd.add1DSlider('PD gain',
minVal=0.,
maxVal=200.,
initVal=10.,
valStep=.1)
viewer.objectInfoWnd.add1DSlider('Joint Damping',
minVal=1.,
maxVal=2000.,
initVal=35.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('steps per frame',
minVal=1.,
maxVal=200.,
initVal=config['stepsPerFrame'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('1/simul speed',
minVal=1.,
maxVal=100.,
initVal=config['simulSpeedInv'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force min',
minVal=0.,
maxVal=1000.,
initVal=40.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force max',
minVal=0.,
maxVal=1000.,
initVal=40.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force begin',
minVal=0.,
maxVal=len(motion) - 1,
initVal=50.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force dur',
minVal=1.,
maxVal=len(motion) - 1,
initVal=5.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('force weight',
minVal=-10.,
maxVal=10.,
initVal=-1.3,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('LCP weight',
minVal=-10.,
maxVal=10.,
initVal=1.3,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('tau weight',
minVal=-10.,
maxVal=10.,
initVal=-3.,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('ref',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump)
viewer.objectInfoWnd.addBtn('image seq dump', viewer.motionViewWnd.dumpMov)
viewer.cForceWnd.addDataSet('expForce', FL_BLACK)
viewer.cForceWnd.addDataSet('desForceMin', FL_RED)
viewer.cForceWnd.addDataSet('desForceMax', FL_RED)
viewer.cForceWnd.addDataSet('realForce', FL_GREEN)
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global vpWorld
global controlModel
global totalDOF
global DOFs
global bodyIDsToCheck
global torques_nested
global ddth_des_flat
global dth_flat
global ddth_sol
global rd_cForces
global rd_cPositions
global rd_jointPos
global rd_cForcesControl
global rd_cPositionsControl
global rd_ForceControl
global rd_ForceDes
global rd_Position
global rd_PositionDes
global viewer
global motionModel
global solver
global IKModel
global dartModel
global pdController
def create_foot(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.geom = 'MyBox'
# node.length = 1.
node.mass = 5.
node = mcfg.getNode('foot00')
node.geom = 'MyFoot4'
# node.geom = 'MyBox'
node.mass = 5.
node = mcfg.getNode('foot01')
node.geom = 'MyFoot4'
# node.geom = 'MyBox'
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 = 20
simulSpeedInv = 1.
wcfg.timeStep = (1/30.*simulSpeedInv)/stepsPerFrame
# parameter
config = dict([])
config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.
config['Bh'] = 1.
config['stepsPerFrame'] = stepsPerFrame
config['simulSpeedInv'] = simulSpeedInv
# etc
config['weightMap'] = {'root': 1., 'foot00': 1., 'foot01': 1.}
config['weightMapTuple'] = (1., 1., 1.)
# config['supLink'] = 'link0'
return motion, mcfg, wcfg, stepsPerFrame, config
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_1()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_chiken_foot()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('fastswim.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot_2('simpleJump_2.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_capsule('simpleJump_onebody.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump.bvh')
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump_long.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('DartFootProject/simpleJump_long.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = create_foot('test2.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = create_foot('DartFootProject/test2.bvh')
dartModel = cpm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
pdController = PDController(dartModel.skeleton, dartModel.world.time_step())
# dartWorld.skeletons[1].controller = PDController(dartWorld.skeletons[1], dartWorld.dt)
# solver = hik.numIkSolver(wcfg, motion[0], mcfg)
ModelOffset = np.array([0., .1, 0.])
dartModel.translateByOffset(ModelOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
bodyIDsToCheck = range(dartModel.getBodyNum())
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
torques_nested = ype.makeNestedList(DOFs)
rd_cForces = [None]
rd_cPositions = [None]
rd_cForcesControl = [None]
rd_cPositionsControl = [None]
rd_ForceControl = [None]
rd_ForceDes = [None]
rd_Position = [None]
rd_PositionDes = [None]
rd_jointPos = [None]
viewer = hsv.hpSimpleViewer(title='main_Test')
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('dartModel', yr.DartModelRenderer(dartWorld, CHARACTER_COLOR2))
viewer.doc.addRenderer('dartModel', yr.DartModelRenderer(dartModel, CHARACTER_COLOR2))
viewer.doc.addRenderer('rd_contactForcesControl', yr.VectorsRenderer(rd_cForcesControl, rd_cPositionsControl, (255, 0, 0), .1, 'rd_c1'))
viewer.doc.addRenderer('rd_contactForces', yr.VectorsRenderer(rd_cForces, rd_cPositions, (0, 255, 0), .1, 'rd_c2'))
viewer.doc.addRenderer('rd_contactForceControl', yr.VectorsRenderer(rd_ForceControl, rd_Position, (0, 0, 255), .1, 'rd_c3'))
# viewer.doc.addRenderer('rd_contactForceDes', yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1))
# viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos))
viewer.doc.addRenderer('rd_contactPosition', yr.PointsRenderer(rd_cPositions, (255, 0, 0)))
viewer.objectInfoWnd.add1DSlider('PD gain', minVal=0., maxVal=200., initVal=10., valStep=.1)
viewer.objectInfoWnd.add1DSlider('Joint Damping', minVal=1., maxVal=2000., initVal=35., valStep=1.)
viewer.objectInfoWnd.add1DSlider('steps per frame', minVal=1., maxVal=200., initVal=config['stepsPerFrame'], valStep=1.)
viewer.objectInfoWnd.add1DSlider('1/simul speed', minVal=1., maxVal=100., initVal=config['simulSpeedInv'], valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force min', minVal=0., maxVal=1000., initVal=40., valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force max', minVal=0., maxVal=1000., initVal=40., valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force begin', minVal=0., maxVal=len(motion) - 1, initVal=50., valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force dur', minVal=1., maxVal=len(motion) - 1, initVal=5., valStep=1.)
viewer.objectInfoWnd.add1DSlider('force weight', minVal=-10., maxVal=10., initVal=-1.3, valStep=.01)
viewer.objectInfoWnd.add1DSlider('LCP weight', minVal=-10., maxVal=10., initVal=1.3, valStep=.01)
viewer.objectInfoWnd.add1DSlider('tau weight', minVal=-10., maxVal=10., initVal=-3., valStep=.01)
viewer.objectInfoWnd.add1DSlider('ref', minVal=-10., maxVal=10., initVal=0., valStep=.01)
viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump)
viewer.objectInfoWnd.addBtn('image seq dump', viewer.motionViewWnd.dumpMov)
viewer.cForceWnd.addDataSet('expForce', FL_BLACK)
viewer.cForceWnd.addDataSet('desForceMin', FL_RED)
viewer.cForceWnd.addDataSet('desForceMax', FL_RED)
viewer.cForceWnd.addDataSet('realForce', FL_GREEN)
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
