def init(): global motion global mcfg global wcfg global stepsPerFrame global config global mcfg_motion global viewer np.set_printoptions(precision=4, linewidth=200) motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_basic( '../2_Test01_Dynamic.bvh') mcfg_motion = mit.normal_mcfg() viewer = hsv.hpSimpleViewer([100, 100, 1280, 960]) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion))
import Renderer.ysRenderer as yr import Renderer.csVpRenderer as cvr import Simulator.csVpWorld as cvw import Simulator.csVpModel as cvm import GUI.ysSimpleViewer as ysv import Optimization.ysAnalyticConstrainedOpt as yac import ArticulatedBody.ysJacobian as yjc import Util.ysPythonEx as ype import ArticulatedBody.ysReferencePoints as yrp import ArticulatedBody.ysMomentum as ymt import ArticulatedBody.ysControl as yct import GUI.ysMultiViewer as ymv import Motion.ysHierarchyEdit as yme import Simulator.ysPhysConfig as ypc import numpy.linalg as npl import mtOptimize as mot import mtInitialize_Simple as mit motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() mcfg_motion = mit.normal_mcfg() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics()
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 main(): np.set_printoptions(precision=4, linewidth=200) #motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() mcfg_motion = mit.normal_mcfg() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) footPartNum = config['FootPartNum'] if footPartNum > 1: elasticity = 2000 damping = 2 * (elasticity**.5) springBody1 = 5 springBody2 = 6 springBody1Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) springBody2Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) initialDist = mm.length(springBody1Pos - springBody2Pos) * 1. node = mcfg.getNode(mit.LEFT_PHALANGE_1) initialDist -= node.width #0.084 v1 = (-node.width * 0.5, 0.0, node.length * 0.4) v2 = (node.width * 0.5, 0.0, node.length * 0.4) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) #elasticity = 10 #damping = 2*(elasticity**.5) #springBody1 = 3 #springBody2 = 4 #node = mcfg.getNode(mit.LEFT_PHALANGE_1) #springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) #springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) #initialDist = mm.length(springBody1Pos - springBody2Pos)*1. #initialDist -= node.width#0.084 #v1 = (-node.width*0.5,0.0,-node.length*0.4) #v2 = (node.width*0.5,0.0,-node.length*0.4) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) vpWorld.initialize() controlModel.initializeHybridDynamics() #ModelOffset = (1.5, -0.01, 0) ModelOffset = (1.5, 0.04, 0) controlModel.translateByOffset(ModelOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2']) #w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) rootB = motion[0].skeleton.getJointIndex(config['root']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) #constBody = motion[0].skeleton.getJointIndex('LeftForeArm') constBody = motion[0].skeleton.getJointIndex(config['const']) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() JsupPre = Jsup.copy() Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() Jcom = yjc.makeEmptyJacobian(DOFs, 1, False) dJcom = Jcom.copy() JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False) dJcomAng = JcomAng.copy() ############### indexFootL = [None] * footPartNum indexFootR = [None] * footPartNum jFootL = [None] * footPartNum dJFootL = [None] * footPartNum jFootR = [None] * footPartNum dJFootR = [None] * footPartNum jointMasksFootL = [None] * footPartNum jointMasksFootR = [None] * footPartNum for i in range(footPartNum): jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootL[i] = jFootL[i].copy() jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootR[i] = jFootR[i].copy() indexFootL[i] = motion[0].skeleton.getJointIndex( config['FootLPart'][i]) indexFootR[i] = motion[0].skeleton.getJointIndex( config['FootRPart'][i]) jointMasksFootL[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i]) ] jointMasksFootR[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i]) ] constJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0], indexFootR[0]]) ] #constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])] #constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks = [ yjc.getLinkJointMask(motion[0].skeleton, selectedBody) ] #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks[0][0] = 0 #comUpperJointMasks[0][1] = 1 #comUpperJointMasks[0][10] = 1 comUpperJointMasks[0][2] = 1 comUpperJointMasks[0][11] = 1 #print(comUpperJointMasks) comLowerJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [ motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg') ]) ] ''' maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks] parentArray = [supL, supR, supL, supR] effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR] for j in range(4) : for i in range(len(foreSupLJointMasks)) : if i == parentArray[j] or i == effectorArray[j] : maskArray[j][0][i] = 1 else : maskArray[j][0][i] = 0 ''' # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization problem = yac.LSE(totalDOF, 6) a_sup = (0, 0, 0, 0, 0, 0) #L #a_sup2 = (0,0,0, 0,0,0)#R a_sup2 = [0, 0, 0, 0, 0, 0] #R a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [1.] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) d_th_IK = ype.makeNestedList(DOFs) d_th_IK_L = ype.makeNestedList(DOFs) d_th_IK_R = ype.makeNestedList(DOFs) dd_th_IK = ype.makeNestedList(DOFs) dd_th_IK_flat = ype.makeFlatList(totalDOF) d_th_IK_flat = ype.makeFlatList(totalDOF) ddth_c_flat = ype.makeFlatList(totalDOF) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM_plane_ref = [None] rd_CM_ref = [None] rd_CM_des = [None] rd_CM = [None] rd_CM_vec = [None] rd_CM_ref_vec = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_footCenter_des = [None] rd_exf_des = [None] rd_root_des = [None] rd_soft_const_vec = [None] rd_root = [None] rd_footL_vec = [None] rd_footR_vec = [None] rd_CMP = [None] rd_DesPosL = [None] rd_DesPosR = [None] rd_DesForePosL = [None] rd_DesForePosR = [None] rd_DesRearPosL = [None] rd_DesRearPosR = [None] rd_Joint = [None] rd_Joint2 = [None] rd_Joint3 = [None] rd_Joint4 = [None] rd_desPoints = [None] rd_contactForces = [None] rd_contactPositions = [None] #rd_contactForces = [None]*10000 #rd_contactPositions = [None]*10000 rd_virtualForce = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] normalVector = [[0, 2, 0]] if MULTI_VIEWER: viewer = ymv.MultiViewer(800, 655) #viewer = ymv.MultiViewer(1600, 1255) viewer.setRenderers1([ cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) viewer.setRenderers2([ cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) else: 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, (100, 100, 100), yr.POLYGON_FILL)) #(150,150,255) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_LINE)) #viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) #viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) ) #viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL)) #viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255, 0, 255))) viewer.doc.addRenderer('rd_CM_des', yr.PointsRenderer(rd_CM_des, (64, 64, 255))) viewer.doc.addRenderer( 'rd_CM_vec', yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3)) #viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 128))) # viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001)) viewer.doc.addRenderer( 'rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009, 0.04)) #viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255))) #viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200))) #viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250))) #viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200))) #viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250))) #viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3)) #viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3)) #viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3)) #viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) #viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0))) #viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3)) #viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3)) #viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3)) #viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3)) viewer.doc.addRenderer( 'rd_contactForces', yr.VectorsRenderer(rd_contactForces, rd_contactPositions, (0, 255, 0), .1)) #viewer.doc.addRenderer('rd_virtualForce', yr.ForcesRenderer(rd_virtualForce, rd_CM, (50,255,0), 0.5, 0.02)) #viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0))) #viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0))) #viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255))) #viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0))) #viewer.doc.addRenderer('rd_desPoints', yr.PointsRenderer(rd_desPoints, (255,0,0))) stage = STATIC_BALANCING contactRendererName = [] #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)) desCOMOffset = 0.0 pt = [0.] timeReport = [0.] * 7 viewer.objectInfoWnd.comOffsetY.value(-0.05) viewer.objectInfoWnd.comOffsetZ.value(0.00) viewer.objectInfoWnd.begin() viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc') viewer.objectInfoWnd.Bc.value(0.1) viewer.objectInfoWnd.end() viewer.objectInfoWnd.labelKt.value(50) viewer.objectInfoWnd.labelKk.value(17) def simulateCallback(frame): print "main:frame : ", frame curTime = time.time() if frame % 30 == 1: pt[0] = time.time() global g_initFlag global forceShowFrame global forceApplyFrame global JsysPre global JsupPreL global JsupPreR global JsupPre global softConstPoint global stage global contactRendererName global desCOMOffset motionModel.update(motion[0]) Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) Dt = 2 * (Kt**.5) Dk = 2 * (Kk**.5) Dl = 2 * (Kl**.5) Dh = 2 * (Kh**.5) Dsc = 2 * (Ksc**.5) # tracking th_r_ori = motion.getDOFPositions(frame) th_r = copy.copy(th_r_ori) ############################ #Reference motion modulation dCM_k = 10. linkVelocities = controlModel.getBodyVelocitiesGlobal() dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. global leftHipTimer if viewer.objectInfoWnd.onLeftHip: leftHipTimer = 60 viewer.objectInfoWnd.onLeftHip = False if leftHipTimer > 0: viewer.objectInfoWnd.comOffsetX.value( 0.08 * np.sin(2 * 3.14 * leftHipTimer / 60.)) #viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.)) #B_Hipd = viewer.objectInfoWnd.labelLeftHip.value() #newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.) #idx = motion[0].skeleton.getJointIndex('LeftUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) #idx = motion[0].skeleton.getJointIndex('RightUpLeg') #th_r[idx] = np.dot(th_r[idx], newR1) leftHipTimer -= 1 timeReport[0] += time.time() - curTime curTime = time.time() th = controlModel.getDOFPositions() dth_r = motion.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) ddth_c = controlModel.getDOFAccelerations() ype.flatten(ddth_des, ddth_des_flat) ype.flatten(dth, dth_flat) ype.flatten(ddth_c, ddth_c_flat) # jacobian refFootL = motionModel.getBodyPositionGlobal(supL) refFootR = motionModel.getBodyPositionGlobal(supR) positionFootL = [None] * footPartNum positionFootR = [None] * footPartNum for i in range(footPartNum): positionFootL[i] = controlModel.getBodyPositionGlobal( indexFootL[i]) positionFootR[i] = controlModel.getBodyPositionGlobal( indexFootR[i]) linkPositions = controlModel.getBodyPositionsGlobal() linkVelocities = controlModel.getBodyVelocitiesGlobal() linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal() linkInertias = controlModel.getBodyInertiasGlobal() jointPositions = controlModel.getJointPositionsGlobal() jointAxeses = controlModel.getDOFAxeses() CM = yrp.getCM(linkPositions, linkMasses, totalMass) dCM = yrp.getCM(linkVelocities, linkMasses, totalMass) CM_plane = copy.copy(CM) CM_plane[1] = 0. dCM_plane = copy.copy(dCM) dCM_plane[1] = 0. linkPositions_ref = motionModel.getBodyPositionsGlobal() linkVelocities_ref = motionModel.getBodyVelocitiesGlobal() linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal() linkInertias_ref = motionModel.getBodyInertiasGlobal() CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass) CM_plane_ref = copy.copy(CM_ref) CM_plane_ref[1] = 0. P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias) dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias) timeReport[1] += time.time() - curTime curTime = time.time() yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks) timeReport[2] += time.time() - curTime curTime = time.time() # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks) if frame > 0: dJsys = (Jsys - JsysPre) * 30. else: dJsys = (Jsys - Jsys) JsysPre = Jsys.copy() timeReport[3] += time.time() - curTime curTime = time.time() lcpBodyIDs, lcpContactPositions, lcpContactPositionLocals, lcpContactForces = hls.calcLCPForces( motion, vpWorld, controlModel, bodyIDsToCheck, 1., 4, None) # bodyIDs : IDs for Virtual Physics, not VpModel !!! bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) CP = yrp.getCP(contactPositions, contactForces) if (CP is not None): CP[1] = 0. for i in range(controlModel.getBodyNum()): controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255) contactFlagFootL = [0] * footPartNum contactFlagFootR = [0] * footPartNum for i in range(len(bodyIDs)): controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200) index = controlModel.id2index(bodyIDs[i]) for j in range(len(indexFootL)): if index == indexFootL[j]: contactFlagFootL[j] = 1 for j in range(len(indexFootR)): if index == indexFootR[j]: contactFlagFootR[j] = 1 for j in range(0, footPartNum): jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy() dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy() if footPartNum == 1: desFCL = (controlModel.getBodyPositionGlobal(supL)) desFCR = (controlModel.getBodyPositionGlobal(supR)) else: r = .5 + desCOMOffset desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r + controlModel.getBodyPositionGlobal(indexFootL[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootL[1]) desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r + controlModel.getBodyPositionGlobal(indexFootR[1]) * (1.0 - r) ) #controlModel.getBodyPositionGlobal(indexFootR[1]) desFC = desFCL + (desFCR - desFCL) / 2.0 desFC[1] = 0 rd_footCenter_des[0] = desFC.copy() curRelCMVec = CM_plane - desFC vecRatio = mm.length(curRelCMVec) * 0. #print(frame, vecRatio) footCenter = desFC - curRelCMVec * (vecRatio) #/10.0 footCenter = ( getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4. #footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4. footCenter_ref = refFootL + (refFootR - refFootL) / 2.0 #footCenter_ref[1] = 0. footCenter[1] = 0. footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), 0, viewer.objectInfoWnd.comOffsetZ.value() ]) #footCenter += footCenterOffset vecRatio = mm.length(curRelCMVec) * 0. softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0 #print(frame, vecRatio, softConstPointOffset) desForeSupLAcc = [0, 0, 0] desForeSupRAcc = [0, 0, 0] totalNormalForce = [0, 0, 0] for i in range(len(contactForces)): totalNormalForce[0] += contactForces[i][0] totalNormalForce[1] += contactForces[i][1] totalNormalForce[2] += contactForces[i][2] #print((totalMass*mm.s2v(wcfg.gravity))[1]) footCenterOffset = np.array([ viewer.objectInfoWnd.comOffsetX.value(), viewer.objectInfoWnd.comOffsetY.value(), viewer.objectInfoWnd.comOffsetZ.value() ]) ###################### # optimization terms ###################### # linear momentum CM_ref_plane = footCenter + footCenterOffset dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] - CM[1]) - Dl * totalMass * dCM[1] #dL_des_plane[1] = 0. #print 'dL_des_plane', dL_des_plane # angular momentum CP_ref = footCenter + footCenterOffset CP_ref[1] = 0. timeStep = 30. if (CP_old[0] is None) or (CP is None): dCP = None else: dCP = (CP - CP_old[0]) * timeStep CP_old[0] = CP if (CP is not None) and (dCP is not None): ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP) CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * ( (1 / timeStep)**2) #print 'dCP: ', dCP #print 'ddCP_des: ', ddCP_des #print 'CP_des: ', CP_des #dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity))) dH_des = np.cross( (CP_des - CM_plane), (dL_des_plane + totalMass * mm.s2v(wcfg.gravity))) else: dH_des = None # momentum matrix RS = np.dot(P, Jsys) R, S = np.vsplit(RS, 2) rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat) r_bias, s_bias = np.hsplit(rs, 2) flagContact = True if (dH_des is None) or np.any(np.isnan(dH_des)) == True: flagContact = False #viewer.doc.showRenderer('rd_grf_des', False) #viewer.motionViewWnd.update(1, viewer.doc) #else: #viewer.doc.showRenderer('rd_grf_des', True) #viewer.motionViewWnd.update(1, viewer.doc) ''' 0 : initial 1 : contact 2 : fly 3 : landing ''' #MOTION = FORWARD_JUMP if mit.MOTION == mit.FORWARD_JUMP: frame_index = [136, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO: frame_index = [130, 100] #frame_index = [100000, 100000] elif mit.MOTION == mit.TAEKWONDO2: frame_index = [130 + 40, 100] elif mit.MOTION == mit.WALK: frame_index = [10000, 60] elif mit.MOTION == mit.TIPTOE: frame_index = [1000000, 1000000] #frame_index = [10000, 165] else: frame_index = [1000000, 1000000] #MOTION = TAEKWONDO #frame_index = [135, 100] if frame > frame_index[0]: if stage != POWERFUL_BALANCING: print("#", frame, "-POWERFUL_BALANCING") stage = POWERFUL_BALANCING Kk = Kk * 2 Dk = 2 * (Kk**.5) elif frame > frame_index[1]: if stage != MOTION_TRACKING: print("#", frame, "-MOTION_TRACKING") stage = MOTION_TRACKING trackingW = w #if checkAll(contactFlagFootR, 0) != 1 : if 0: #stage == MOTION_TRACKING: trackingW = w2 #stage = POWERFUL_BALANCING Bt = Bt * 2 # optimization mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat) #mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1) if flagContact == True: if stage != MOTION_TRACKING + 10: mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias) #mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias) # using || dH ||^2 instead mot.addAnotherTerms(problem, totalDOF, Bh, S, -(s_bias + Kh * np.dot(S, dth_flat))) a_sup_2 = None Jsup_2 = None dJsup_2 = None ############################## # Hard constraint Kk2 = Kk * 4.0 Dk2 = 2 * (Kk2**.5) ankleW = 0 ankleOffset = ankleW * curRelCMVec[2] metatarW = 0 metatarOffset = metatarW * curRelCMVec[2] ############################## ############################## # Additional constraint if stage != MOTION_TRACKING and frame > 5: # ankle strategy idx = 0 #LEFT/RIGHT_TOES if mit.FOOT_PART_NUM == 1: yOffset = 0.03 else: yOffset = 0.069 #yOffset = 0.06 # ankleOffset = (footCenter - CM_plane)*4. ankleOffset = footCenterOffset * 10. ankleOffset[1] = 0. #ankleOffset[2] = 0. ankleOffset[2] = ankleOffset[2] * 20. ankleOffsetL = ankleOffset.copy() ankleOffsetR = ankleOffset.copy() #ankleOffset= np.array((0,0,0)) if footCenterOffset[0] > 0.0: ankleOffsetL[0] = 0. else: ankleOffsetR[0] = 0. # print 'ankleOffset=', ankleOffset desLinearAccL, desPosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14) desLinearAccR, desPosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk, Dk, yOffset) ax = [0, 0, -1] aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot') #print np.dot(aaa, ax) if mit.FOOT_PART_NUM == 1: ax = [0, 1, 0] desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetL)) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[idx], Kk, Dk, ax, mm.normalize([0, 1, 0] + ankleOffsetR)) a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)))) Jsup_2 = np.vstack((jFootL[idx], jFootR[idx])) dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx])) #mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1]) #mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:]) #mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:]) mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat)) #mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat)) desCOMOffset = 0.0 rd_DesPosL[0] = desPosL.copy() rd_DesPosR[0] = desPosR.copy() if stage == STATIC_BALANCING and frame > 10: # and False: del rd_desPoints[:] # foot strategy #Kk2 = Kk * 2.5 #Kk2 = Kk * .2 #Dk2 = 2*(Kk2**.5) desForePosL = [0, 0, 0] desForePosR = [0, 0, 0] desRearPosL = [0, 0, 0] desRearPosR = [0, 0, 0] footPartPos = [] footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('LeftPhalange_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightCalcaneus_1'))) footPartPos.append( controlModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex('RightPhalange_1'))) for i in range(1, footPartNum): contactFlagFootL[i] = 1 contactFlagFootR[i] = 1 SupPts = np.vstack( (np.array((footPartPos[0][0], footPartPos[1][0], footPartPos[2][0], footPartPos[3][0])), np.array( (footPartPos[0][2], footPartPos[1][2], footPartPos[2][2], footPartPos[3][2])), np.array((1., 1., 1., 1.)))) coordWidthLen = 2. coordLengthLen = 1.5 SupUV = np.vstack( (np.array((-coordWidthLen, -coordWidthLen, coordWidthLen, coordWidthLen)), np.array((-coordLengthLen, coordLengthLen, -coordLengthLen, coordLengthLen)), np.array((1., 1., 1., 1.)))) SupMap = np.dot(np.dot(SupUV, SupUV.T), np.linalg.inv(np.dot(SupPts, SupUV.T))) #print SupMap desFootCenter = footCenter + footCenterOffset footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1)) #print np.dot(SupMap, footCenterPts) #print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0))) CM_plane_2D = np.array((CM[0], CM[2], 1)) # CM_plane_UV = np.dot(SupMap, CM_plane_2D) CM_plane_UV = np.dot(SupMap, footCenterPts) # print CM_plane_UV # for i in range(1, footPartNum): if CM_plane_UV[1] > .5: # com is in front for i in range(1, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 elif CM_plane_UV[1] < -.5: # com is back for i in range(3, footPartNum): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 else: # com is in middle position for i in range(3, 5): contactFlagFootL[i] = 0 contactFlagFootR[i] = 0 contactFlagFoot = contactFlagFootL if CM_plane_UV[0] < 0.: contactFlagFoot = contactFlagFootR # CM_plane_UV[0] = -CM_plane_UV[0] if abs(CM_plane_UV[0]) > 1.: for j in range(0, 3): contactFlagFoot[2 * j + 2] = 0 # print 'footL : ',contactFlagFootL # print 'footR : ',contactFlagFootR for i in range(1, footPartNum): axis = [0, 0, 1] if i == 1 or i == 2: axis = [0, 0, -1] desAng = [0, 0, 1] if i == 1 or i == 2: desAng = [0, 0, -1] desY = 0.029 if contactFlagFootL[i] == 1: desLinearAccL, desForePosL = getDesFootLinearAcc( motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccL = getDesFootAngularAcc( motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack((desLinearAccL, desAngularAccL)) Jsup_2 = jFootL[i].copy() dJsup_2 = dJFootL[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat)) rd_desPoints.append(desForePosL.copy()) if contactFlagFootR[i] == 1: desLinearAccR, desForePosR = getDesFootLinearAcc( motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY) desAngularAccR = getDesFootAngularAcc( motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng) a_sup_2 = np.hstack((desLinearAccR, desAngularAccR)) Jsup_2 = jFootR[i].copy() dJsup_2 = dJFootR[i].copy() mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat)) #mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat)) rd_desPoints.append(desForePosR.copy()) rd_DesForePosL[0] = desForePosL rd_DesForePosR[0] = desForePosR rd_DesRearPosL[0] = desRearPosL rd_DesRearPosR[0] = desRearPosR ############################## #if Jsup_2 is not None: # mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2) timeReport[4] += time.time() - curTime curTime = time.time() r = problem.solve() #print frame #Ashape = np.shape(problem.A) #if len(Ashape) >0 : # for i in range(0, Ashape[0]): # print problem.A[i] #print problem.A[] #print problem.b #print r problem.clear() #print r['x'] ype.nested(r['x'], ddth_sol) #print ddth_sol rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody) localPos = [[0, 0, 0]] ########################################### ##Jacobian Transpose control # COM Position control #fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane) w1 = 10 #10.1 w2 = 1 #1#2*(w1**.5) if frame > 100: w1 = 10.1 #10.1 w2 = 1 footToCMVec = CM - footCenter desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]] #print("desCMPos", desCMPos) #print("CM", CM) fCom = w1 * (desCMPos - CM) + w2 * (-dCM) #print("fCom", fCom) #fCom[0] = 0. #fCom[1] = 0 #fCom[2] = 0 rd_virtualForce[0] = fCom.copy() #hipPos = controlModel.getBodyPositionGlobal(rootB) headPos = controlModel.getBodyPositionGlobal(selectedBody) hipPos = controlModel.getBodyPositionGlobal(rootB) yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) #yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False) JcomT = Jcom.T TauJT = np.dot(JcomT, fCom) # Angular Momentum Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities, linkPositions, linkMasses, linkVelocities) Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref, linkAngVelocities_ref, linkPositions_ref, linkMasses, linkVelocities_ref) Wam = .05 Tam = Wam * (Href - Hc) #print("Tam", Tam) yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses, [headPos], comUpperJointMasks) TauAM = np.dot(JcomAng.T, Tam) timeReport[5] += time.time() - curTime curTime = time.time() for i in range(stepsPerFrame): # apply penalty force bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) #print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) extraForce[0] = viewer.GetForce() if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0): forceApplyFrame += 1 #vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce) controlModel.applyBodyForceGlobal(selectedBody, extraForce[0]) applyedExtraForce[0] = extraForce[0] if forceApplyFrame * wcfg.timeStep > 0.1: viewer.ResetForce() forceApplyFrame = 0 #print ddth_sol controlModel.setDOFAccelerations(ddth_sol) controlModel.solveHybridDynamics() vpWorld.step() #if frame%30==0: print 'elapsed time for 30 frames:', time.time()-pt[0] # rendering rd_footCenter[0] = footCenter rd_CM[0] = CM.copy() rd_CM_plane[0] = CM_plane.copy() rd_footCenter_ref[0] = footCenter_ref rd_CM_plane_ref[0] = CM_ref.copy() rd_CM_ref[0] = CM_ref.copy() rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3. rd_CM_vec[0] = (CM - CM_plane) rd_CM_des[0] = CM_ref_plane.copy() rd_CM_des[0][1] = .01 #rd_CM_plane[0][1] = 0. if (CP is not None) and (dCP is not None): rd_CP[0] = CP rd_CP_des[0] = CP_des rd_dL_des_plane[0] = dL_des_plane rd_dH_des[0] = dH_des rd_grf_des[ 0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) rd_exf_des[0] = applyedExtraForce[0] rd_root_des[0] = rootPos[0] rd_CMP[0] = softConstPoint rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal( constBody) - softConstPoint del rd_contactForces[:] del rd_contactPositions[:] if CP is not None: for i in range(len(lcpBodyIDs)): rd_contactForces.append(lcpContactForces[i].copy() / 200.) rd_contactPositions.append(lcpContactPositions[i].copy()) timeReport[6] += time.time() - curTime # print timeReport viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def init(): global motion global mcfg global wcfg global stepsPerFrame global config global mcfg_motion global viewer global rd_jointPos global rd_tibia global rd_forefoot global rd_backfoot global rd_midfoot global infoNames global infoData global markerName global markerPosi global tibia_marker_idx global backfoot_marker_idx global forefoot_marker_idx global midfoot_marker_idx global rd_tibia_cog global rd_backfoot_cog global rd_midfoot_cog global rd_tibia_tri global rd_backfoot_tri global rd_midfoot_tri global rd_forefoot_tri np.set_printoptions(precision=4, linewidth=200) motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_basic( '../2_Test01_Dynamic.bvh') mcfg_motion = mit.normal_mcfg() infoNames, infoData, markerName, markerPosi = trc.import_trc( '../trc/2_test01_dynamic.trc') # infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/2_test02_dynamic.trc') # infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_1_test01_dynamic.trc') # infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_2_test01_dynamic.trc') # infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_3_test01_dynamic.trc') info = dict(zip(infoNames, infoData)) numFrame = int(info['NumFrames']) - 1 motion[:] = [motion[0]] * numFrame rd_jointPos = [None] rd_tibia = [None] rd_backfoot = [None] rd_midfoot = [None] rd_forefoot = [None] rd_tibia_tri = [None] rd_backfoot_tri = [None] rd_midfoot_tri = [None] rd_forefoot_tri = [None] rd_tibia_cog = [None] rd_backfoot_cog = [None] rd_midfoot_cog = [None] viewer = hsv.hpSimpleViewer([100, 100, 1280, 960]) viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion)) viewer.doc.addRenderer( 'rd_jointPos', yr.PointsRenderer(rd_jointPos, color=(255, 0, 0), pointStyle=1)) viewer.doc.addRenderer( 'rd_tibia', yr.PointsRenderer(rd_tibia, color=(255, 0, 255), pointStyle=1)) viewer.doc.addRenderer('rd_tibia_tri', yr.LinesRenderer(rd_tibia_tri, color=(255, 0, 255))) viewer.doc.addRenderer( 'rd_backfoot', yr.PointsRenderer(rd_backfoot, color=(0, 0, 255), pointStyle=1)) viewer.doc.addRenderer( 'rd_backfoot_tri', yr.LinesRenderer(rd_backfoot_tri, color=(0, 0, 255))) viewer.doc.addRenderer( 'rd_midfoot', yr.PointsRenderer(rd_midfoot, color=(0, 255, 0), pointStyle=1)) viewer.doc.addRenderer('rd_midfoot_tri', yr.LinesRenderer(rd_midfoot_tri, color=(0, 255, 0))) viewer.doc.addRenderer( 'rd_forefoot', yr.PointsRenderer(rd_forefoot, color=(0, 0, 0), pointStyle=1)) viewer.doc.addRenderer('rd_forefoot_tri', yr.LinesRenderer(rd_forefoot_tri, color=(0, 0, 0))) # viewer.doc.addRenderer('rd_tibia_cog', yr.PointsRenderer(rd_tibia_cog, color=(255,0,255), pointStyle=0)) # viewer.doc.addRenderer('rd_backfoot_cog', yr.PointsRenderer(rd_backfoot_cog, color=(0,0,255), pointStyle=0)) # viewer.doc.addRenderer('rd_midfoot_cog', yr.PointsRenderer(rd_midfoot_cog, color=(0,255,0), pointStyle=0)) # viewer.doc.addRenderer('rd_tibia_cog', yr.LinesRenderer(rd_tibia_cog, color=(255,0,255))) # viewer.doc.addRenderer('rd_backfoot_cog', yr.LinesRenderer(rd_backfoot_cog, color=(0,0,255))) # viewer.doc.addRenderer('rd_midfoot_cog', yr.LinesRenderer(rd_midfoot_cog, color=(0,255,0))) viewer.objectInfoWnd.add1DSlider('PD gain', minVal=0., maxVal=1000., initVal=180., valStep=.1) viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump) viewer.objectInfoWnd.addBtn('movie', viewer.motionViewWnd.dumpMov) # viewer.cForceWnd.addDataSet('midfoot_height', FL_BLACK) # viewer.cForceWnd.addDataSet('midfoot_top_angle', FL_GREEN) # viewer.cForceWnd.addDataSet('midfoot_front_angle', FL_RED) print info print markerName ''' ['Top.Head', 'Front.Head', 'Rear.Head', 'R.Shoulder', 'R.Offset', 'R.Elbow', 'R.Wrist', 'L.Shoulder', 'L.Elbow', 'L.Wrist', 'R.ASIS', 'L.ASIS', 'V.Sacral', 'R.Thigh', 'R.Knee', 'R.Shank', 'R.Ankle', 'R.Heel', 'R.Toe', 'L.Thigh', 'L.Knee', 'L.Shank', 'L.Ankle', 'L.Heel', 'L.Toe', 'R1', 'R2', 'R3', 'R4', 'R5', 'R6', 'R7', 'L1', 'L2', 'L3', 'L4', 'L5', 'L6', 'L7', 'V_Mid_ASIS', 'V_Pelvis_Origin', 'V_R.Hip_JC', 'V_L.Hip_JC', 'V_R.Knee_JC', 'V_L.Knee_JC', 'V_R.Ankle_JC', 'V_L.Ankle_JC', 'V_Mid_Hip', 'V_Mid_Shoulder', 'V_R.Hand', 'V_L.Hand', 'V_R.Toe_Offset_Static', 'V_L.Toe_Offset_Static', 'V_R.Toe_Offset', 'V_L.Toe_Offset'] ''' ''' ['LFWT', 'RFWT', 'VBWT', 'LKNE', 'LANK', 'LMT5', 'LMT1', 'LHEL', 'LTOE', 'RKNE', 'RANK', 'RMT5', 'RMT1', 'RHEL', 'RTOE', 'T10', 'FHEAD', 'THEAD', 'RHEAD', 'LBSH', 'LELB', 'LOWR', 'RBSH', 'RELB', 'ROWR', 'LTHI', 'RTHI', 'LSHAN', 'RSHAN', 'LIHEL', 'LOHEL', 'RIHEL', 'ROHEL', 'LMT3', 'LNAVI', 'LCUNE', 'RMT3', 'RNAVI', 'RCUNE'] ''' tibia_marker = ['RKNE', 'RSHAN', 'RANK', 'LKNE', 'LSHAN', 'LANK'] forefoot_marker = [ 'RTOE', 'RMT1', 'RMT3', 'RMT5', 'LTOE', 'LMT1', 'LMT3', 'LMT5' ] midfoot_marker = ['RCUNE', 'RNAVI', 'LCUNE', 'LNAVI'] backfoot_marker = ['RHEL', 'RIHEL', 'ROHEL', 'LHEL', 'LIHEL', 'LOHEL'] tibia_marker_idx = [markerName.index(i) for i in tibia_marker] midfoot_marker_idx = [markerName.index(i) for i in midfoot_marker] forefoot_marker_idx = [markerName.index(i) for i in forefoot_marker] backfoot_marker_idx = [markerName.index(i) for i in backfoot_marker] print tibia_marker_idx print forefoot_marker_idx print midfoot_marker_idx print backfoot_marker_idx midfoot_height = [] midfoot_top_angle = [] midfoot_front_angle = [] midfoot_length = [] for frame in range(numFrame): tibia_points = [ np.array(markerPosi[frame][i]) for i in tibia_marker_idx[:3] ] backfoot_points = [ np.array(markerPosi[frame][i]) for i in backfoot_marker_idx[:3] ] midfoot_points = [ np.array(markerPosi[frame][i]) for i in midfoot_marker_idx[:2] ] forefoot_points = [ np.array(markerPosi[frame][i]) for i in forefoot_marker_idx[:4] ] center, basis = getCenterAndBasisFromPoints(backfoot_points) basis = [basis[2], basis[0], basis[1]] rotation = np.zeros((3, 3)) for i in range(3): rotation[i, :] = basis[i] offset = np.array((0., 0., 0.)) tibia_trans_points = [ np.dot(rotation, point - center) + offset for point in tibia_points ] backfoot_trans_points = [ np.dot(rotation, point - center) + offset for point in backfoot_points ] midfoot_trans_points = [ np.dot(rotation, point - center) + offset for point in midfoot_points ] forefoot_trans_points = [ np.dot(rotation, point - center) + offset for point in forefoot_points ] # viewer.cForceWnd.insertData('midfoot_height', frame, sum(midfoot_trans_points)[1]*1000/2.) midfoot_height.append(sum(midfoot_trans_points)[1] * 1000. / 2.) mf_ang_vec = midfoot_trans_points[1] - midfoot_trans_points[0] mf_front_ang = math.asin( abs(mf_ang_vec[1]) / npl.norm(mf_ang_vec)) * 180. / math.pi mf_top_ang = math.atan2(mf_ang_vec[2], -mf_ang_vec[0]) * 180. / math.pi midfoot_top_angle.append(mf_top_ang) midfoot_front_angle.append(mf_front_ang) midfoot_length.append( npl.norm(midfoot_trans_points[1] - midfoot_trans_points[0])) # viewer.cForceWnd.insertData('midfoot_top_angle', frame, mf_top_ang*50) # viewer.cForceWnd.insertData('midfoot_front_angle', frame, mf_front_ang) plt.figure(1) plt.subplot(311) # plt.ylim(-100, 100) plt.ylabel('height(mm)') plt.plot(midfoot_length) # plt.plot(midfoot_height) plt.subplot(312) plt.ylim(0, 30) plt.ylabel('front angle(deg)') plt.plot(midfoot_front_angle) plt.subplot(313) plt.ylim(40, 65) plt.ylabel('top angle(deg)') plt.plot(midfoot_top_angle) plt.savefig('hehe.pdf')
def main(): np.set_printoptions(precision=4, linewidth=200) # motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5() motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped() mcfg_motion = mit.normal_mcfg() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) motionModel.recordVelByFiniteDiff() motionOriModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) footPartNum = config['FootPartNum'] if footPartNum > 1: elasticity = 2000 damping = 2 * (elasticity**.5) springBody1 = 5 springBody2 = 6 springBody1Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) springBody2Pos = motionModel.getBodyPositionGlobal( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) initialDist = mm.length(springBody1Pos - springBody2Pos) * 1. node = mcfg.getNode(mit.LEFT_PHALANGE_1) initialDist -= node.width #0.084 v1 = (-node.width * 0.5, 0.0, node.length * 0.4) v2 = (node.width * 0.5, 0.0, node.length * 0.4) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) controlModel.setSpring( motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) #elasticity = 10 #damping = 2*(elasticity**.5) #springBody1 = 3 #springBody2 = 4 #node = mcfg.getNode(mit.LEFT_PHALANGE_1) #springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1])) #springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2])) #initialDist = mm.length(springBody1Pos - springBody2Pos)*1. #initialDist -= node.width#0.084 #v1 = (-node.width*0.5,0.0,-node.length*0.4) #v2 = (node.width*0.5,0.0,-node.length*0.4) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist) ##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist) vpWorld.initialize() controlModel.initializeHybridDynamics() #ModelOffset = (1.5, -0.01, 0) ModelOffset = (1.5, 0.04, 0) controlModel.translateByOffset(ModelOffset) totalDOF = controlModel.getTotalDOF() DOFs = controlModel.getDOFs() # parameter Kt = config['Kt'] Dt = config['Dt'] # tracking gain Kl = config['Kl'] Dl = config['Dl'] # linear balance gain Kh = config['Kh'] Dh = config['Dh'] # angular balance gain Ks = config['Ks'] Ds = config['Ds'] # penalty force spring gain Bt = config['Bt'] Bl = config['Bl'] Bh = config['Bh'] w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap']) w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2']) #w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap']) supL = motion[0].skeleton.getJointIndex(config['supLink']) supR = motion[0].skeleton.getJointIndex(config['supLink2']) rootB = motion[0].skeleton.getJointIndex(config['root']) selectedBody = motion[0].skeleton.getJointIndex(config['end']) #constBody = motion[0].skeleton.getJointIndex('LeftForeArm') constBody = motion[0].skeleton.getJointIndex(config['const']) # jacobian Jsup = yjc.makeEmptyJacobian(DOFs, 1) dJsup = Jsup.copy() JsupPre = Jsup.copy() Jsys_IK = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum()) dJsys = Jsys.copy() JsysPre = Jsys.copy() Jconst = yjc.makeEmptyJacobian(DOFs, 1) dJconst = Jconst.copy() Jcom = yjc.makeEmptyJacobian(DOFs, 1, False) dJcom = Jcom.copy() JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False) dJcomAng = JcomAng.copy() ############### jFootL_IK = [None] * footPartNum jFootR_IK = [None] * footPartNum indexFootL = [None] * footPartNum indexFootR = [None] * footPartNum jFootL = [None] * footPartNum dJFootL = [None] * footPartNum jFootR = [None] * footPartNum dJFootR = [None] * footPartNum jointMasksFootL = [None] * footPartNum jointMasksFootR = [None] * footPartNum for i in range(footPartNum): jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootL[i] = jFootL[i].copy() jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1) dJFootR[i] = jFootR[i].copy() indexFootL[i] = motion[0].skeleton.getJointIndex( config['FootLPart'][i]) indexFootR[i] = motion[0].skeleton.getJointIndex( config['FootRPart'][i]) jointMasksFootL[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i]) ] jointMasksFootR[i] = [ yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i]) ] constJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0], indexFootR[0]]) ] #constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])] #constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)] allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton) #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks = [ yjc.getLinkJointMask(motion[0].skeleton, selectedBody) ] #comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])] comUpperJointMasks[0][0] = 0 #comUpperJointMasks[0][1] = 1 #comUpperJointMasks[0][10] = 1 comUpperJointMasks[0][2] = 1 comUpperJointMasks[0][11] = 1 #print(comUpperJointMasks) comLowerJointMasks = [ yjc.getLinksJointMask(motion[0].skeleton, [ motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg') ]) ] ''' maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks] parentArray = [supL, supR, supL, supR] effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR] for j in range(4) : for i in range(len(foreSupLJointMasks)) : if i == parentArray[j] or i == effectorArray[j] : maskArray[j][0][i] = 1 else : maskArray[j][0][i] = 0 ''' # momentum matrix linkMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() TO = ymt.make_TO(linkMasses) dTO = ymt.make_dTO(len(linkMasses)) # optimization qps = hqp.QPSimulator() problem = yac.LSE(totalDOF, 6) a_sup = (0, 0, 0, 0, 0, 0) #L #a_sup2 = (0,0,0, 0,0,0)#R a_sup2 = [0, 0, 0, 0, 0, 0] #R a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] CP_old = [mm.v3(0., 0., 0.)] # penalty method bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [1.] * len(bodyIDsToCheck) # flat data structure ddth_des_flat = ype.makeFlatList(totalDOF) dth_flat = ype.makeFlatList(totalDOF) ddth_sol = ype.makeNestedList(DOFs) dth_IK = ype.makeNestedList(DOFs) d_th_IK = ype.makeNestedList(DOFs) d_th_IK_L = ype.makeNestedList(DOFs) d_th_IK_R = ype.makeNestedList(DOFs) dd_th_IK = ype.makeNestedList(DOFs) dd_th_IK_flat = ype.makeFlatList(totalDOF) d_th_IK_flat = ype.makeFlatList(totalDOF) ddth_c_flat = ype.makeFlatList(totalDOF) # viewer rd_footCenter = [None] rd_footCenter_ref = [None] rd_footCenterL = [None] rd_footCenterR = [None] rd_CM_plane = [None] rd_CM_plane_ref = [None] rd_CM_ref = [None] rd_CM_des = [None] rd_CM = [None] rd_CM_vec = [None] rd_CM_ref_vec = [None] rd_CP = [None] rd_CP_des = [None] rd_dL_des_plane = [None] rd_dH_des = [None] rd_grf_des = [None] rd_footCenter_des = [None] rd_exf_des = [None] rd_root_des = [None] rd_soft_const_vec = [None] rd_root = [None] rd_footL_vec = [None] rd_footR_vec = [None] rd_CMP = [None] rd_DesPosL = [None] rd_DesPosR = [None] rd_DesForePosL = [None] rd_DesForePosR = [None] rd_DesRearPosL = [None] rd_DesRearPosR = [None] rd_Joint = [None] rd_Joint2 = [None] rd_Joint3 = [None] rd_Joint4 = [None] rd_desPoints = [None] #rd_contactForces = [None]*10000 #rd_contactPositions = [None]*10000 rd_virtualForce = [None] rootPos = [None] selectedBodyId = [selectedBody] extraForce = [None] applyedExtraForce = [None] applyedExtraForce[0] = [0, 0, 0] normalVector = [[0, 2, 0]] if MULTI_VIEWER: viewer = ymv.MultiViewer(800, 655) #viewer = ymv.MultiViewer(1600, 1255) viewer.setRenderers1([ cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) viewer.setRenderers2([ cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL) ]) else: 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, (100, 100, 100), yr.POLYGON_FILL)) #(150,150,255) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)) #viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_LINE)) #viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter)) #viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) ) #viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL)) #viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR)) viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255, 255, 0))) viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255, 0, 255))) viewer.doc.addRenderer('rd_CM_des', yr.PointsRenderer(rd_CM_des, (64, 64, 255))) viewer.doc.addRenderer( 'rd_CM_vec', yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3)) #viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0))) viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255, 0, 128))) # viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0))) #viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001)) viewer.doc.addRenderer( 'rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009, 0.04)) #viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255))) #viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255))) #viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200))) #viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250))) #viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200))) #viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250))) #viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3)) #viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3)) #viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3)) #viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref)) #viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0))) #viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3)) #viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3)) #viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3)) #viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3)) #viewer.doc.addRenderer('rd_contactForces', yr.ForcesRenderer(rd_contactForces, rd_contactPositions, (0,255,0), .009, 0.009)) #viewer.doc.addRenderer('rd_virtualForce', yr.ForcesRenderer(rd_virtualForce, rd_CM, (50,255,0), 0.5, 0.02)) #viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0))) #viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0))) #viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255))) #viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0))) viewer.doc.addRenderer('rd_desPoints', yr.PointsRenderer(rd_desPoints, (255, 0, 0))) stage = STATIC_BALANCING contactRendererName = [] for i in range(motion[0].skeleton.getJointNum()): print(i, motion[0].skeleton.getJointName(i)) desCOMOffset = 0.0 pt = [0.] timeReport = [0.] * 7 viewer.objectInfoWnd.comOffsetY.value(-0.05) viewer.objectInfoWnd.comOffsetZ.value(0.00) viewer.objectInfoWnd.begin() viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc') viewer.objectInfoWnd.Bc.value(0.1) viewer.objectInfoWnd.ankleAngleX = Fl_Value_Input(50, 510, 40, 10, 'Ankle X') viewer.objectInfoWnd.ankleAngleX.value(0) viewer.objectInfoWnd.ankleAngleY = Fl_Value_Input(110, 510, 40, 10, 'Y') viewer.objectInfoWnd.ankleAngleY.value(1) viewer.objectInfoWnd.ankleAngleZ = Fl_Value_Input(170, 510, 40, 10, 'Z') viewer.objectInfoWnd.ankleAngleZ.value(0) viewer.objectInfoWnd.end() viewer.objectInfoWnd.labelKt.value(50) viewer.objectInfoWnd.labelKk.value(17) config['Phalange'] = [ motion[0].skeleton.getJointIndex('LeftPhalange_1'),\ motion[0].skeleton.getJointIndex('LeftPhalange_2'),\ motion[0].skeleton.getJointIndex('RightPhalange_1'),\ motion[0].skeleton.getJointIndex('RightPhalange_2')] config['Talus'] = [ motion[0].skeleton.getJointIndex('LeftTalus_1'),\ motion[0].skeleton.getJointIndex('LeftTalus_2'),\ motion[0].skeleton.getJointIndex('RightTalus_1'),\ motion[0].skeleton.getJointIndex('RightTalus_2')] config['Calcaneus'] = [ motion[0].skeleton.getJointIndex('LeftCalcaneus_1'),\ motion[0].skeleton.getJointIndex('LeftCalcaneus_2'),\ motion[0].skeleton.getJointIndex('RightCalcaneus_1'),\ motion[0].skeleton.getJointIndex('RightCalcaneus_2')] pose = motion[0].copy() timeReport = [0.] * 2 def simulateCallback(frame): curTime = time.time() Ke = 0.0 Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) motionModel.update(motion[frame]) controlToMotionOffset = [-2.0, 0., 0.] motionModel.translateByOffset(controlToMotionOffset) stepsPerFrame = 10 for i in range(stepsPerFrame): Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam( ) #Kt, Kl, Kh, Bl, Bh, Ke = viewer.GetParam() #qps.setupWeight(Kt, Kl, Kh, Ke, Bt, Btau, Bcon, Bl, Bh, Be) qps.setupWeight(Kt, Kl, Kh, Ke, 10., .1, .1, Bl, Bh, 10.) cPositions, CP, CM, footCenter, dL_des, CM_ref = qps.setupQP( frame, motion, mcfg, controlModel, vpWorld, config, 1. / (30. * stepsPerFrame)) CM_ref[1] = 0. timeReport[0] += time.time() - curTime curTime = time.time() #forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()]) #extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce) #extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) #if viewer.GetForceState() : # qps.addExternalForces(extraForce[0], selectedBody, viewer.objectInfoWnd.labelForceDur.value()); # viewer.ResetForceState() x, cForce = qps.stepQP(controlModel, 1. / (30. * stepsPerFrame)) timeReport[1] += time.time() - curTime curTime = time.time() print timeReport if frame % 30 == 0: print 'elapsed time for 30 frames:', time.time() - pt[0] # rendering #rd_footCenter[0] = footCenter # #rd_CM[0] = CM.copy() # #rd_CM_plane[0] = CM_plane.copy() # #rd_footCenter_ref[0] = footCenter_ref #rd_CM_plane_ref[0] = CM_ref.copy() #rd_CM_ref[0] = CM_ref.copy() #rd_CM_ref_vec[0] = (CM_ref - footCenter_ref)*3. #rd_CM_vec[0] = (CM - CM_plane) #rd_CM_des[0] = CM_ref_plane.copy() #rd_CM_des[0][1] = .01 #rd_CM_plane[0][1] = 0. #if CP!=None and dCP!=None: # # rd_CP[0] = CP # # rd_CP_des[0] = CP_des # # # #rd_dL_des_plane[0] = dL_des_plane # #rd_dH_des[0] = dH_des # # # #rd_grf_des[0] = totalNormalForce# - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity) # # # #rd_exf_des[0] = applyedExtraForce[0] # #rd_root_des[0] = rootPos[0] # # #rd_CMP[0] = softConstPoint # # #rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint # # # ##indexL = motion[0].skeleton.getJointIndex('Hips') # ##indexR = motion[0].skeleton.getJointIndex('Spine1') # #indexL = indexFootL[0] # #indexR = indexFootR[0] # # #curAng = [controlModel.getBodyOrientationGlobal(indexL)] # #curAngY = np.dot(curAng, np.array([0,0,1])) # # #rd_footL_vec[0] = np.copy(curAngY[0]) # #rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL) # # # #curAng = [controlModel.getBodyOrientationGlobal(indexR)] # #curAngY = np.dot(curAng, np.array([0,0,1])) # #rd_footR_vec[0] = np.copy(curAngY[0]) #rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR) # #if (forceApplyFrame == 0) : # applyedExtraForce[0] = [0, 0, 0] viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
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))