self.add_bodies(et_skeleton) # add Joint self.add_joints(et_skeleton) self.tree = et_skel_tree def save(self, filename): with open(filename, 'w') as f: f.write(prettifyXML(self.tree.getroot())) pass if __name__ == '__main__': import PyCommon.modules.Motion.ysHierarchyEdit as yme motion = yf.readBvhFile('test.bvh', 0.01) yme.removeJoint(motion, 'LHipJoint', False) yme.removeJoint(motion, 'RHipJoint', False) yme.removeJoint(motion, 'LowerBack', False) yme.removeJoint(motion, 'LeftToeBase', False) yme.removeJoint(motion, 'RightToeBase', False) yme.removeJoint(motion, 'Neck', False) yme.removeJoint(motion, 'Head', False) yme.removeJoint(motion, 'LeftHandIndex1', False) yme.removeJoint(motion, 'RightHandIndex1', False) yme.removeJoint(motion, 'LeftFingerBase', False) yme.removeJoint(motion, 'RightFingerBase', False) yme.removeJoint(motion, 'LThumb_Effector', False) yme.removeJoint(motion, 'RThumb_Effector', False) yme.removeJoint(motion, 'LThumb', False) yme.removeJoint(motion, 'RThumb', False)
def __init__(self, rnn_len): skel_file = '../data/cmu_with_ground.xml' self.world = pydart.World(1./1200., skel_file) self.world.control_skel = self.world.skeletons[1] self.skel = self.world.skeletons[1] self.Kp, self.Kd = 800., 40. self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.) self.ref_motion = yf.readBvhFile("../data/cmu_tpose.bvh")[:rnn_len] self.ref_motion_len = rnn_len self.ref_world = pydart.World(1./1200., skel_file) self.ref_skel = self.ref_world.skeletons[1] self.step_per_frame = 40 self.rsi = True # self.w_g = 0.3 # self.w_p = 0.65 * .7 # self.w_v = 0.1 * .7 # self.w_e = 0.15 * .7 # self.w_c = 0.1 * .7 self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_g = 2.5 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.num_bodynodes() self.idx_e = [self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftHand'), self.skel.bodynode_index('RightHand')] self.body_e = list(map(self.skel.body, self.idx_e)) self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps state_num = 2 + (3*3 + 4) * self.body_num + (3 + 4) * self.body_num action_num = self.skel.num_dofs() - 6 state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi*10./2.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32) self.viewer = None self.phase_frame = 0 self.goals_in_world_frame = list() self.goal = np.zeros(2) self.prev_ref_q = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_p_e_hat = np.asarray([body.world_transform()[:3, 3] for body in self.ref_body_e]).flatten() self.prev_ref_com = self.ref_skel.com() self.prev_goal = np.zeros(2)
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
def simulation_test(): Kt = 20. Dt = 2 * (Kt**.5) Ks = 2000. Ds = 2 * (Ks**.5) mu = 1. dir = './icmotion_last/' filename = 'stop_left_normal.temp' # filename = 'left_left_normal.temp' # filename = 'right_left_fast.temp' motion_ori = yf.readBvhFile(dir + filename) frameTime = 1 / motion_ori.fps motion_ori[0:0] = [motion_ori[0]] * 20 mcfgfile = open(dir + 'mcfg', 'rb') mcfg = pickle.load(mcfgfile) mcfgfile.close() wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False # wcfg.lockingVel = c_locking_vel stepsPerFrame = 30 wcfg.timeStep = (frameTime) / stepsPerFrame vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion_ori[0], mcfg) vpWorld.initialize() print(controlModel) controlModel.initializeHybridDynamics() bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [mu] * len(bodyIDsToCheck) viewer = ysv.SimpleViewer() # viewer.record(False) # viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE)) viewer.doc.addObject('motion_ori', motion_ori) viewer.doc.addRenderer( 'motionModel', cvr.VpModelRenderer(motionModel, (0, 150, 255), yr.POLYGON_LINE)) viewer.doc.addRenderer( 'controlModel', cvr.VpModelRenderer(controlModel, (200, 200, 200), yr.POLYGON_LINE)) def simulateCallback(frame): th_r = motion_ori.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_ori.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_ori.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce( bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() vpWorld.step() motionModel.update(motion_ori[frame]) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(frameTime / 1.4) viewer.show() Fl.run()
def __init__(self): self.world = pydart.World(1./1200., "../data/woody_with_ground.xml") self.world.control_skel = self.world.skeletons[1] self.skel = self.world.skeletons[1] self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.) self.Kp, self.Kd = 400., 40. self.ik = DartIk(self.skel) self.ref_motions = list() # type: list[ym.Motion] self.ref_motions.append(yf.readBvhFile("../data/woody_walk_normal.bvh")[40:]) self.ref_motions.append(yf.readBvhFile("../data/wd2_WalkForwardVFast00.bvh")) self.ref_motions.append(yf.readBvhFile("../data/wd2_WalkForwardSlow01.bvh")) self.ref_motions.append(yf.readBvhFile("../data/walk_right_45degree.bvh")[21:134]) self.ref_motions.append(yf.readBvhFile("../data/walk_left_45degree.bvh")[26:141]) # self.ref_motions.append(yf.readBvhFile("../data/walk_left_90degree.bvh")[21:153]) # self.ref_motions.append(yf.readBvhFile('../data/wd2_WalkSukiko00.bvh')) # self.ref_motions.append(yf.readBvhFile("../data/wd2_jump0.bvh")[164:280]) # self.ref_motions.append(yf.readBvhFile("../data/wd2_u-turn.bvh")[25:214]) # self.ref_motions[-1].translateByOffset([0., 0.03, 0.]) self.motion_num = len(self.ref_motions) self.reward_weights_by_fps = [self.ref_motions[0].fps / self.ref_motions[i].fps for i in range(self.motion_num)] self.ref_motion = self.ref_motions[0] self.ref_world = pydart.World(1./1200., "../data/woody_with_ground.xml") self.ref_skel = self.ref_world.skeletons[1] self.step_per_frame = None self.rsi = True self.specified_motion_num = 0 self.is_motion_specified = False # self.w_g = 0.3 # self.w_p = 0.65 * .7 # self.w_v = 0.1 * .7 # self.w_e = 0.15 * .7 # self.w_c = 0.1 * .7 self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_g = 2.5 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.num_bodynodes() self.idx_e = [self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm')] self.body_e = list(map(self.skel.body, self.idx_e)) self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.goals_in_world_frame = list() self.goal = np.zeros(2) self.goals = list() self.goal_window = 45 state_num = 3*self.goal_window + (3*3 + 4) * self.body_num # action_num = self.skel.num_dofs() - 6 action_num = self.skel.num_dofs() state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi*2.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32) self.viewer = None self.phase_frame = 0 self.prev_ref_q = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_dq = np.zeros(self.ref_skel.num_dofs()) self.prev_ref_p_e_hat = np.asarray([body.world_transform()[:3, 3] for body in self.ref_body_e]).flatten() self.prev_ref_com = self.ref_skel.com() # setting for reward self.reward_joint = list() self.reward_joint.append('j_RightUpLeg') self.reward_joint.append('j_RightLeg') self.reward_joint.append('j_LeftUpLeg') self.reward_joint.append('j_LeftLeg') self.reward_joint.append('j_Spine') self.reward_joint.append('j_Spine1') self.reward_joint.append('j_RightArm') self.reward_joint.append('j_RightForeArm') self.reward_joint.append('j_LeftArm') self.reward_joint.append('j_LeftForeArm')
def create_walking_biped(): #motion motionName = 'wd2_WalkForwardNormal00.bvh' #motionName = '../motions/wd2_WalkForwardNormal00.bvh' #motionName = '../motions/wd2_WalkForwardFast00.bvh' #motionName = 'wd2_jump.bvh' #motionName = 'wd2_stand.bvh' motion = yf.readBvhFile(motionName, .01) yme.removeJoint(motion, 'Head', False) #yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45), False) yme.updateGlobalT(motion) motion.translateByOffset((0, 0.0, 0)) #motion = motion[40:-58] #motion[0:0] = [motion[0]]*20 #motion.extend([motion[-1]]*5000) motion = motion[40:] #motion[0:0] = [motion[0]]*50 #motion.extend([motion[-1]]*100) #motion.extend([motion[-1]]*100) #motion = motion[30:151] #motion = motion[30:] #motion[5:5] = [motion[5]]*30 #motion[0:0] = [motion[0]]*100 #motion.extend([motion[-1]]*5000) #motion = motion[40:41] #motion[0:0] = [motion[0]]*5000 #motion = motion[56:-248] #motion = motion[-249:-248] #motion[0:0] = [motion[0]]*10 #motion.extend([motion[-1]]*5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .25 #node.length = .2 node.width = .12 #node.width = .2 node.mass = 2. #node.mass = 1. node = mcfg.getNode('LeftFoot') node.length = .25 #node.length = .2 node.width = .12 #node.width = .2 node.mass = 2. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 #stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #wcfg.timeStep = (1/1000.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} #success!! #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def preprocess(SEGMENT_FOOT=False): tasks = [] outputDir = './ppmotion/' if SEGMENT_FOOT: outputDir = './ppmotion_segfoot/' dir = './ori_motion/' config = None if SEGMENT_FOOT: # segmented foot # config = {'repeat':False, 'footRot': mm.rotX(.07), 'yOffset':0., 'halfFootHeight': 0.0944444444444, 'scale':.01, 'type':'woody2'} config = { 'repeat': False, 'footRot': mm.rotX(-.07), 'yOffset': 0., 'halfFootHeight': 0.0944444444444, 'scale': .01, 'type': 'woody2' } else: # box foot config = { 'repeat': False, 'footRot': mm.rotX(-.4), 'yOffset': 0., 'halfFootHeight': 0.0444444444444, 'scale': .01, 'type': 'woody2' } paths = [] # rotX = -0.07 paths.append(dir + 'wd2_1foot_contact_run2_edit.bvh') paths.append(dir + 'wd2_fast_2foot_hop_edit.bvh') paths.append(dir + 'wd2_long_broad_jump_edit.bvh') paths.append(dir + 'wd2_n_kick_edit.bvh') paths.append(dir + 'wd2_short_broad_jump_edit.bvh') paths.append(dir + 'wd2_slow_2foot_hop_edit.bvh') # rotX = 0.07 # paths.append(dir+'wd2_boxing_round_round_girl1_edit.bvh') # paths.append(dir+'wd2_u-turn_edit.bvh') tasks.append({'config': config, 'paths': paths}) VISUALIZE = False for task in tasks: config = task['config'] paths = task['paths'] for path in paths: motion = yf.readBvhFile(path) normalizeSkeleton(motion, config) adjustHeight(motion, config['halfFootHeight']) additionalEdit(motion, path) outputPath = outputDir + os.path.basename(path) if SEGMENT_FOOT: outputPath = outputDir + "segfoot_" + os.path.basename(path) yf.writeBvhFile(outputPath, motion) print(outputPath, 'done') if 'repeat' in config and config['repeat']: hRef = .1 vRef = .3 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) interval = yba.getWalkingCycle2(motion, lc) transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10 motion = ymt.repeatCycle(motion, interval, 50, transitionLength) outputName = os.path.splitext( os.path.basename(path))[0] + '_REPEATED.bvh' outputPath = outputDir + outputName if SEGMENT_FOOT: outputPath = outputDir + 'segfoot_' + outputName yf.writeBvhFile(outputPath, motion) print(outputPath, 'done') if VISUALIZE: viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion', yr.JointMotionRenderer(motion, (0, 100, 255), yr.LINK_LINE)) viewer.doc.addObject('motion', motion) viewer.startTimer(1 / 30.) viewer.show() Fl.run() print('FINISHED')
def preprocess(SEGMENT_FOOT=False): # dir = './icmotion_test/' # paths = glob.glob(dir+'*.temp') dir = './ppmotion/' # paths = glob.glob(dir+'*.bvh') paths = None if SEGMENT_FOOT: paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00.bvh') paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00_REPEATED.bvh') else: paths = glob.glob(dir+'wd2_WalkForwardNormal00.bvh') paths = glob.glob(dir+'wd2_WalkForwardNormal00_REPEATED.bvh') # paths = glob.glob(dir+'wd2_WalkForwardVFast00.bvh') ''' paths = glob.glob(dir+'wd2_WalkSameSame01.bvh') paths = glob.glob(dir+'wd2_u-turn_1.bvh') paths = glob.glob(dir+'wd2_cross_walk*.bvh') paths = glob.glob(dir+'*_REPEATED.bvh') paths = glob.glob(dir+'wd2_pick_walk_1.bvh') paths = glob.glob(dir+'wd2_WalkSameSame01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkForwardSlow01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkForwardNormal01_REPEATED_FOOT.bvh') paths = glob.glob(dir+'wd2_WalkSoldier00_REPEATED_FOOT.bvh') ''' hRef = .1; vRef = .4 ## hRef = .1; vRef = .2 # dir = './ppmotion_long/' # paths = glob.glob(dir+'wd2_WalkBackward00_REPEATED.bvh') # hRef = 10.; vRef = .4 # dir = './rawmotion_slope/' # paths = glob.glob(dir+'*.bvh') ## paths = [dir+'woddy2_walk_normal_to_slope.bvh'] # hRef = 10000.; vRef = .4*100 # # dir = './ppmotion_slope/' # paths = glob.glob(dir+'*.bvh') # hRef = 10000.; vRef = .2 jumpThreshold = 15; jumpBias = 1. stopThreshold = 15; stopBias = 0. for path in paths: motion_ori = yf.readBvhFile(path) # informations skeleton = motion_ori[0].skeleton lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') lHip = skeleton.getJointIndex('LeftUpLeg'); rHip = skeleton.getJointIndex('RightUpLeg') lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg') lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') mcfgfile = open(dir + 'mcfg', 'rb') mcfg = pickle.load(mcfgfile) mcfgfile.close() wcfg = ypc.WorldConfig() vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg) # bodyMasses = getBodyMasses() bodyMasses = motionModel.getBodyMasses() uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']] upperMass = sum([bodyMasses[i] for i in uppers]) lc = yma.getElementContactStates(motion_ori, 'LeftFoot', hRef, vRef) rc = yma.getElementContactStates(motion_ori, 'RightFoot', hRef, vRef) # intervals, states = yba.getBipedGaitIntervals(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias) intervals, states = yba.getBipedGaitIntervals2(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias) seginfos = [{} for i in range(len(intervals))] for i in range(len(intervals)): start = intervals[i][0]; end = intervals[i][1] seginfos[i]['interval'] = intervals[i] seginfos[i]['state'] = states[i] # print yba.GaitState.text[states[i]], intervals[i] stanceHips = []; swingHips = []; stanceFoots = []; swingFoots = []; swingKnees = [] if states[i]==yba.GaitState.LSWING: stanceHips = [rHip]; stanceFoots = [rFoot]; swingHips = [lHip]; swingFoots = [lFoot]; swingKnees = [lKnee] elif states[i]==yba.GaitState.RSWING: stanceHips = [lHip]; stanceFoots = [lFoot]; swingHips = [rHip]; swingFoots = [rFoot]; swingKnees = [rKnee] elif states[i]==yba.GaitState.STOP: stanceHips = [rHip, lHip]; stanceFoots = [rFoot, lFoot] elif states[i]==yba.GaitState.JUMP: swingHips = [rHip, lHip]; swingFoots = [rFoot, lFoot] seginfos[i]['stanceHips'] = stanceHips seginfos[i]['swingHips'] = swingHips seginfos[i]['stanceFoots'] = stanceFoots seginfos[i]['swingFoots'] = swingFoots seginfos[i]['swingKnees'] = swingKnees if start<end: if SEGMENT_FOOT: # segmented foot seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]]) else: # box foot seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]]) seginfos[i]['max_stf_push_frame'] = None if len(swingFoots) > 0: pushes = [] for frame in range(start, int((start+end)//2) + 1): dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis)) seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes)) # write .seg inputName = os.path.basename(path) root = os.path.splitext(inputName)[0] outputName = root+'.seg' outputFile = open(dir+outputName, 'wb') pickle.dump(seginfos, outputFile) outputFile.close() print(outputName, 'done') pprint.pprint(seginfos) print('FINISHED')
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' if MOTION == STAND: #motionName = 'wd2_stand.bvh' motionName = 'wd2_stand2.bvh' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == KICK: motionName = 'wd2_n_kick.bvh' elif MOTION == WALK: motionName = 'wd2_WalkForwardNormal00.bvh' elif MOTION == TIPTOE: motionName = './MotionFile/cmu/15_07_15_07.bvh' #motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK: scale = 1.0 elif MOTION == TIPTOE: scale = 0.01 motion = yf.readBvhFile(motionName, scale) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False) #addFootSegment yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_3, (0.045, -0.06, -0.05)) #-0.0037 yme.addJoint(motion, LEFT_TALUS_1, LEFT_METATARSAL_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_TALUS_3, LEFT_METATARSAL_3, (0.0, 0.0, 0.1)) #-0.0037 yme.addJoint(motion, LEFT_TALUS_2, LEFT_METATARSAL_2, (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2, (0.0, 0.0, 0.07)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1', (0.0, 0.0, 0.06)) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Effector3', (0.0, 0.0, 0.06)) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2', (0.0, 0.0, 0.06)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.04)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.04)) #yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.04)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.05)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1', (0., 0.0, -0.07)) yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2', (0., 0.0, -0.07)) yme.addJoint(motion, LEFT_CALCANEUS_3, 'LEFT_CALCANEUS_Effector3', (0., 0.0, -0.07)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_3, (-0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_METATARSAL_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_METATARSAL_2, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_TALUS_3, RIGHT_METATARSAL_3, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3, (0.0, 0.0, 0.07)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1', (0.0, 0.0, 0.06)) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2', (0.0, 0.0, 0.06)) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Effector3', (0.0, 0.0, 0.06)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.04)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.04)) #yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.04)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.05)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1', (0.0, 0.0, -0.07)) yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2', (0.0, 0.0, -0.07)) yme.addJoint(motion, RIGHT_CALCANEUS_3, 'RIGHT_CALCANEUS_Effector3', (0.0, 0.0, -0.07)) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), .5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_3, mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step #motion = motion[0:31+40] ## Taekwondo turning-kick motion = motion[108:-1] #motion = motion[108:109] elif MOTION == KICK: #motion = motion[141:-1] #motion = motion[100:-1] #motion = motion[58:-1] motion = motion[82:-1] #motion = motion[0:-1] elif MOTION == STAND2: motion = motion[1:-1] elif MOTION == TIPTOE: #motion = motion[183:440] #motion = motion[350:410] motion = motion[350:550] motion[0:0] = [motion[0]] * 40 motion.extend([motion[-1]] * 5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .1 node.width = .1 node = mcfg.getNode('LeftFoot') node.length = .1 node.width = .1 #return mcfg # #mass0 = .4 #width0 = 0.028 #length0 = 0.1 # ##Metatarsal1 #length1 = .12 #width1 = 0.03 #mass1 = 0.4 # #length2 = .1 #width2 = 0.026 #mass2 = 0.4 # ##Metatarsal3 #length3 = .08 #width3 = 0.024 #mass3 = 0.4 # ##Calcaneus1 #length4 = .1 #width4 = 0.032 #mass4 = 0.4 # ##Phalange1 #length5 = .08 #width5 = 0.01 #mass5 = 0.4 ##Phalange3 #length7 = length5 #width7 = width5 #mass7 = mass5 # ##Talus ##length8 = .13 ##width8 = width0*3 ##mass8 = mass0*2. # #length8 = .1 #width8 = width0*3 #mass8 = mass0*1.5 width0 = 0.028 length0 = 0.1 mass0 = .4 #Metatarsal1 length1 = .1 width1 = 0.03 mass1 = 0.4 length2 = length1 width2 = width1 mass2 = 0.4 #Metatarsal3 length3 = length1 width3 = width1 mass3 = 0.4 #Calcaneus1 length4 = length1 width4 = width1 mass4 = 0.4 #Phalange1 length5 = length1 width5 = width1 mass5 = 0.4 #Phalange3 length7 = length1 width7 = width1 mass7 = 0.4 #Talus #length8 = .13 #width8 = width0*3 #mass8 = mass0*2. length8 = .1 width8 = width0 * 3 mass8 = mass0 * 1.5 node = mcfg.getNode(RIGHT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(RIGHT_TALUS_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_3) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_2) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_METATARSAL_1) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_3) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_2) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_2) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_2) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_3) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(LEFT_TALUS_1) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_3) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_2) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_METATARSAL_1) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_3) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_2) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_2) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_3) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_CALCANEUS_1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_2) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_3) node.length = length4 node.width = width4 node.mass = mass4 #node.offset = (0.0, -0.025, 0.0) node = mcfg.getNode('LeftFoot') node = mcfg.getNode(RIGHT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_PHALANGE_3) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_CALCANEUS_3) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_CALCANEUS_3) node.geom = 'MyFoot3' wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .5, SPINE1: .3, RIGHT_FOOT: .7, LEFT_FOOT: .7, HIP: .5, RIGHT_UP_LEG: .7, RIGHT_LEG: .7, LEFT_UP_LEG: .7, LEFT_LEG: .7, LEFT_TALUS_1: .7, RIGHT_TALUS_1: .7, LEFT_TALUS_2: .7, RIGHT_TALUS_2: .7, LEFT_TALUS_3: .7, RIGHT_TALUS_3: .7, LEFT_METATARSAL_1: .7, RIGHT_METATARSAL_1: .7, LEFT_METATARSAL_2: .7, RIGHT_METATARSAL_2: .7, LEFT_METATARSAL_3: .7, RIGHT_METATARSAL_3: .7, RIGHT_CALCANEUS_1: .7, LEFT_CALCANEUS_1: .7, RIGHT_CALCANEUS_2: .7, LEFT_CALCANEUS_2: .7, RIGHT_CALCANEUS_3: .7, LEFT_CALCANEUS_3: .7, LEFT_PHALANGE_1: .4, LEFT_PHALANGE_2: .4, LEFT_PHALANGE_3: .4, RIGHT_PHALANGE_1: .4, RIGHT_PHALANGE_2: .4, RIGHT_PHALANGE_3: .4 } config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .2, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .3, RIGHT_UP_LEG: .1, RIGHT_LEG: .2, LEFT_UP_LEG: .1, LEFT_LEG: .2, LEFT_TALUS_1: .1, RIGHT_TALUS_1: .1, LEFT_TALUS_2: .1, RIGHT_TALUS_2: .1, LEFT_TALUS_3: .1, RIGHT_TALUS_3: .1, LEFT_METATARSAL_1: .1, RIGHT_METATARSAL_1: .1, LEFT_METATARSAL_2: .1, RIGHT_METATARSAL_2: .1, LEFT_METATARSAL_3: .1, RIGHT_METATARSAL_3: .1, RIGHT_CALCANEUS_1: .2, LEFT_CALCANEUS_1: .2, RIGHT_CALCANEUS_2: .2, LEFT_CALCANEUS_2: .2, RIGHT_CALCANEUS_3: .2, LEFT_CALCANEUS_3: .2, LEFT_PHALANGE_1: .1, LEFT_PHALANGE_2: .1, LEFT_PHALANGE_3: .1, RIGHT_PHALANGE_1: .1, RIGHT_PHALANGE_2: .1, RIGHT_PHALANGE_3: .1 } ''' config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, LEFT_TALUS_1:.01, RIGHT_TALUS_1:.01, LEFT_TALUS_2:.01, RIGHT_TALUS_2:.01, LEFT_TALUS_3:.01, RIGHT_TALUS_3:.01, LEFT_METATARSAL_1:.01, RIGHT_METATARSAL_1:.01, LEFT_METATARSAL_2:.01, RIGHT_METATARSAL_2:.01, LEFT_METATARSAL_3:.01, RIGHT_METATARSAL_3:.01, RIGHT_CALCANEUS_1:.01, LEFT_CALCANEUS_1:.01, RIGHT_CALCANEUS_2:.01, LEFT_CALCANEUS_2:.01, RIGHT_CALCANEUS_3:.01, LEFT_CALCANEUS_3:.01, LEFT_PHALANGE_1:.01, LEFT_PHALANGE_2:.01, LEFT_PHALANGE_3:.01, RIGHT_PHALANGE_1:.01, RIGHT_PHALANGE_2:.01, RIGHT_PHALANGE_3:.01} ''' #config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, # RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, # LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_3:.1, RIGHT_TALUS_3:.1, # LEFT_METATARSAL_1:.1, RIGHT_METATARSAL_1:.1, LEFT_METATARSAL_3:.1, RIGHT_METATARSAL_3:.1, # RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_3:.2, LEFT_CALCANEUS_3:.2, # LEFT_PHALANGE_1:.1, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_3:.1} # #config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5, # RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7, # LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_3:.7, RIGHT_TALUS_3:.7, # LEFT_METATARSAL_1:.7, RIGHT_METATARSAL_1:.7, LEFT_METATARSAL_3:.7, RIGHT_METATARSAL_3:.7, # RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_3:.7, LEFT_CALCANEUS_3:.7, # LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_3:.4} config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT #config['end'] = HIP config['end'] = SPINE1 config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM config['FootLPart'] = [ LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_CALCANEUS_3, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_TALUS_3, LEFT_METATARSAL_1, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2, LEFT_PHALANGE_3 ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_CALCANEUS_3, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_TALUS_3, RIGHT_METATARSAL_1, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3 ] #config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3] #config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3] return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_basic(motionName): scale = 1. motion = yf.readBvhFile(motionName, scale) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 120. wcfg.timeStep = (1/30.)/stepsPerFrame #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] # parameter config = {} config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1.#0.5 config['Bh'] = 1. config['weightMap'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_2:.1, RIGHT_TALUS_2:.1, RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_2:.2, LEFT_CALCANEUS_2:.2, LEFT_PHALANGE_1:.1, LEFT_PHALANGE_2:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_2:.1} config['weightMap2'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5, RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7, LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_2:.7, RIGHT_TALUS_2:.7, RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_2:.7, LEFT_CALCANEUS_2:.7, LEFT_PHALANGE_1:.4, LEFT_PHALANGE_2:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_2:.4} ''' (1, 'RightUpLeg') (2, 'RightLeg') (3, 'RightFoot') (4, 'Spine') (5, 'Spine1') (6, 'LeftArm') (7, 'LeftForeArm') (8, 'RightArm') (9, 'RightForeArm') (10, 'LeftUpLeg') (11, 'LeftLeg') (12, 'LeftFoot') (13, 'LeftTalus_1') (14, 'LeftTalus_2') (15, 'RightTalus_1') (16, 'RightTalus_2') (17, 'LeftPhalange_1') (18, 'LeftPhalange_2') (19, 'RightPhalange_1') (20, 'RightPhalange_2') (21, 'LeftCalcaneus_1') (22, 'LeftCalcaneus_2') (23, 'RightCalcaneus_1') (24, 'RightCalcaneus_2') ''' config['trackWMap']={10, 5, .1, 20, 10, 5, 2, 10, 5, .1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.10, 0.01} config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT # config['end'] = HIP config['end'] = SPINE1 config['trunk'] = SPINE config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2] config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2] return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped(): # motion # motionName = 'wd2_n_kick.bvh' if MOTION == STAND: # motionName = 'wd2_stand.bvh' motionName = 'wd2_stand2.bvh' # motionName = 'woddy2_jump0_short.bvh' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO : motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2 : motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == KICK : motionName = 'wd2_n_kick.bvh' elif MOTION == WALK : motionName = 'wd2_WalkForwardNormal00.bvh' elif MOTION == TIPTOE: motionName = './MotionFile/cmu/15_07_15_07.bvh' # motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK : scale = 1.0 elif MOTION == TIPTOE: scale = 0.01 motion = yf.readBvhFile(motionName, scale) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0.), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0.), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1., 0., 0.), .01), False) # yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0,0,1), -.01), False) # addFootSegment # Talus length1 = .15 width1 = .03 mass1 = .4 # Phalange length3 = length1*0.75 width3 = width1 mass3 = 0.4 length3_2 = length1*0.6 width3_2 = width1 mass3_2 = 0.4 #Calcaneus length4 = 0.05 width4 = width1 mass4 = 0.4 width1 = 0.03 yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_TALUS_1, LEFT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 )) yme.addJoint(motion, LEFT_TALUS_2, LEFT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2)) yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 )) yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 )) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 )) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, ( 0.025, -0.06, -0.05)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.025, -0.06, -0.05)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1', (0.0, 0.0, -length4)) yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2', (0.0, 0.0, -length4)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1', (0.0, 0.0, -length4)) yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2', (0.0, 0.0, -length4)) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(0., 1., 0.), 1.57), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False) #yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step #motion = motion[0:31+40] ## Taekwondo turning-kick motion = motion[108:-1] #motion = motion[108:109] elif MOTION == KICK: #motion = motion[141:-1] #motion = motion[100:-1] #motion = motion[58:-1] motion = motion[82:-1] #motion = motion[0:-1] elif MOTION == STAND2: motion = motion[1:-1] elif MOTION == TIPTOE: #motion = motion[183:440] #motion = motion[350:410] motion = motion[350:550] motion[0:0] = [motion[0]]*40 motion.extend([motion[-1]]*2000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0,0,0.1) node = mcfg.getNode(SPINE) node.width = .22 # node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .1 node.width = .1 node = mcfg.getNode('LeftFoot') node.length = .1 node.width = .1 #Talus #length1 = .12 #width1 = 0.03 #mass1 = 0.4 #Phalange #length3 = length1 #width3 = width1 #mass3 = 0.4 #length3_2 = length1*0.8 #width3_2 = width1 #mass3_2 = 0.4 #Calcaneus1 #length4 = 0.1 #width4 = width1 #mass4 = 0.4 #Foot length8 = .1 width8 = 0.028*3 mass8 = .4*1.5 node = mcfg.getNode(RIGHT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(RIGHT_TALUS_1) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_TALUS_2) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_PHALANGE_2) node.length = length3_2 node.width = width3_2 node.mass = mass3_2 node = mcfg.getNode(RIGHT_CALCANEUS_1) node.length = length4 - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_CALCANEUS_2) node.length = length4 - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_FOOT) node.length = length8 node.width = width8 node.mass = mass8 node = mcfg.getNode(LEFT_TALUS_1) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_TALUS_2) node.length = length1 - width1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_PHALANGE_1) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_PHALANGE_2) node.length = length3_2 node.width = width3_2 node.mass = mass3_2 node = mcfg.getNode(LEFT_CALCANEUS_1) node.length = length4# - width4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_CALCANEUS_2) node.length = length4# - width4 node.width = width4 node.mass = mass4 #node.offset = (0.0, -0.025, 0.0) node = mcfg.getNode('LeftFoot') node = mcfg.getNode(RIGHT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_TALUS_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_1) node.geom = 'MyFoot3' node = mcfg.getNode(LEFT_METATARSAL_2) node.geom = 'MyFoot3' node = mcfg.getNode(RIGHT_PHALANGE_1) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_PHALANGE_2) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_PHALANGE_1) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_PHALANGE_2) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_CALCANEUS_1) node.geom = 'MyFoot4' node = mcfg.getNode(RIGHT_CALCANEUS_2) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_CALCANEUS_1) node.geom = 'MyFoot4' node = mcfg.getNode(LEFT_CALCANEUS_2) node.geom = 'MyFoot4' wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 120 wcfg.timeStep = (1/30.)/(stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1.#0.5 config['Bh'] = 1. config['weightMap'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3, RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2, LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_2:.1, RIGHT_TALUS_2:.1, RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_2:.2, LEFT_CALCANEUS_2:.2, LEFT_PHALANGE_1:.1, LEFT_PHALANGE_2:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_2:.1} config['weightMap2'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5, RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7, LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_2:.7, RIGHT_TALUS_2:.7, RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_2:.7, LEFT_CALCANEUS_2:.7, LEFT_PHALANGE_1:.4, LEFT_PHALANGE_2:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_2:.4} ''' (1, 'RightUpLeg') (2, 'RightLeg') (3, 'RightFoot') (4, 'Spine') (5, 'Spine1') (6, 'LeftArm') (7, 'LeftForeArm') (8, 'RightArm') (9, 'RightForeArm') (10, 'LeftUpLeg') (11, 'LeftLeg') (12, 'LeftFoot') (13, 'LeftTalus_1') (14, 'LeftTalus_2') (15, 'RightTalus_1') (16, 'RightTalus_2') (17, 'LeftPhalange_1') (18, 'LeftPhalange_2') (19, 'RightPhalange_1') (20, 'RightPhalange_2') (21, 'LeftCalcaneus_1') (22, 'LeftCalcaneus_2') (23, 'RightCalcaneus_1') (24, 'RightCalcaneus_2') ''' config['trackWMap']={10, 5, .1, 20, 10, 5, 2, 10, 5, .1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.10, 0.01} config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT # config['end'] = HIP config['end'] = SPINE1 config['trunk'] = SPINE config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2] config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2] return motion, mcfg, wcfg, stepsPerFrame, config
def create_legs(motionFile='legs.bvh'): # motion motion = yf.readBvhFile(motionFile, .06) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for i in range(motion[0].skeleton.getElementNum()): mcfg.addNode(motion[0].skeleton.getElementName(i)) node = mcfg.getNode('Hips') # node.geom = '' # node.length = 40. # node.length = 1. node.mass = 4. node = mcfg.getNode('Spine') node.geom = 'MyFoot4' node.mass = 3. node.width = .1 node = mcfg.getNode('RightUpLeg') node.geom = 'MyFoot3' node.mass = 5. # node.length = 40. node = mcfg.getNode('RightLeg') node.geom = 'MyFoot4' node.mass = 5. # node.length = 40. node = mcfg.getNode('RightFoot') node.geom = 'MyFoot4' node.mass = .8 node = mcfg.getNode('RightToe') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('RightCal') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('RightHeel') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('RightCune') node.geom = 'MyFoot4' node.mass = .4 node = mcfg.getNode('RightMt5') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('LeftUpLeg') node.geom = 'MyFoot3' node.mass = 5. # node.length = 40. node = mcfg.getNode('LeftLeg') node.geom = 'MyFoot4' node.mass = 5. # node.length = 40. node = mcfg.getNode('LeftFoot') node.geom = 'MyFoot4' node.mass = .8 node = mcfg.getNode('LeftToe') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('LeftCal') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('LeftHeel') node.geom = 'MyFoot4' node.mass = .2 node = mcfg.getNode('LeftCune') node.geom = 'MyFoot4' node.mass = .4 node = mcfg.getNode('LeftMt5') node.geom = 'MyFoot4' node.mass = .2 wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 40 simulSpeedInv = 1. wcfg.timeStep = (1/30.*simulSpeedInv)/stepsPerFrame # parameter config = dict([]) config['Kt'] = 20; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 1; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 5000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. config['Bh'] = 1. config['stepsPerFrame'] = stepsPerFrame config['simulSpeedInv'] = simulSpeedInv # etc config['weightMap'] = {'root': 2., 'knee': 2., 'foot00': 1., 'foot10': 1., 'foot20': 1., 'foot01': .2, 'foot11': .2, 'foot21': .2} config['weightMapTuple'] = (2., 2., 1., .2, 1., .2, 1., .2) # config['supLink'] = 'link0' return motion, mcfg, wcfg, stepsPerFrame, config
def __init__(self, env_slaves=1): self.world = pydart.World(1. / 1200., "../data/woody_with_ground.xml") self.world.control_skel = self.world.skeletons[1] self.skel = self.world.skeletons[1] self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.) self.Kp, self.Kd = 400., 40. self.ref_motions = list() # type: list[ym.Motion] self.ref_motions.append( yf.readBvhFile("../data/woody_walk_normal.bvh")[40:]) # self.ref_motions.append(yf.readBvhFile("../data/wd2_jump0.bvh")[164:280]) self.ref_motions.append(yf.readBvhFile('../data/wd2_WalkSukiko00.bvh')) self.ref_motions.append( yf.readBvhFile("../data/walk_left_90degree.bvh")) self.ref_motions.append( yf.readBvhFile("../data/wd2_u-turn.bvh")[25:214]) self.ref_motions[-1].translateByOffset([0., 0.03, 0.]) self.ref_motions.append( yf.readBvhFile("../data/wd2_WalkForwardVFast00.bvh")) self.motion_num = len(self.ref_motions) self.reward_weights_by_fps = [ self.ref_motions[0].fps / self.ref_motions[i].fps for i in range(self.motion_num) ] self.ref_motion = self.ref_motions[0] self.ref_world = pydart.World(1. / 1200., "../data/woody_with_ground.xml") self.ref_skel = self.ref_world.skeletons[1] self.step_per_frame = None self.rsi = True self.specified_motion_num = 0 self.is_motion_specified = False self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.num_bodynodes() self.idx_e = [ self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm') ] self.body_e = list(map(self.skel.body, self.idx_e)) self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. state_num = 2 + (3 * 3 + 4) * self.body_num action_num = self.skel.num_dofs() - 6 state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi * 10. / 2.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32) self.viewer = None
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' #motionName = 'wd2_jump_ori.bvh' if 1: #motionName = 'wd2_stand.bvh' motionName = 'woddy2_jump0.bvh' motion = yf.readBvhFile(motionName, .01) motion.translateByOffset((0., -0.023, 0.)) yme.removeJoint(motion, 'HEad', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder1', False) yme.removeJoint(motion, 'RightToes_Effector', False) yme.removeJoint(motion, 'LeftToes_Effector', False) yme.removeJoint(motion, 'RightHand_Effector', False) yme.removeJoint(motion, 'LeftHand_Effector', False) yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False) yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(2.5,-0.0,.3), -.5), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(2.5,0.0,-.3), -.5), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.2), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.2), -.5), False) yme.rotateJointLocal(motion, 'LeftUpLeg', mm.exp(mm.v3(0.0,.0,1.), .08), False) yme.rotateJointLocal(motion, 'LeftLeg', mm.exp(mm.v3(0.0,1.0,0.), -.2), False) #yme.rotateJointLocal(motion, 'RightLeg', mm.exp(mm.v3(1.0,0.0,0.), -.1), False) yme.updateGlobalT(motion) # yf.writeBvhFile('wd2_jump0.bvh', motion) else : motionName = 'ww13_41.bvh' motion = yf.readBvhFile(motionName, 0.056444) yme.removeJoint(motion, 'LHipJoint', False) yme.removeJoint(motion, 'RHipJoint', False) yme.removeJoint(motion, 'Neck', False) yme.removeJoint(motion, 'Neck1', False) yme.removeJoint(motion, 'Head', False) yme.removeJoint(motion, 'RightShoulder', False) yme.removeJoint(motion, 'LeftShoulder', False) yme.removeJoint(motion, 'RightToeBase_Effector', False) yme.removeJoint(motion, 'LeftToeBase_Effector', False) yme.removeJoint(motion, 'LeftHand', False) yme.removeJoint(motion, 'LeftFingerBase', False) yme.removeJoint(motion, 'LeftHandIndex1_Effector', False) yme.removeJoint(motion, 'LThumb', False) yme.removeJoint(motion, 'RightHand', False) yme.removeJoint(motion, 'RightFingerBase', False) yme.removeJoint(motion, 'RightHandIndex1_Effector', False) yme.removeJoint(motion, 'RThumb', False) yme.removeJoint(motion, 'LowerBack', False) yme.offsetJointLocal(motion, 'RightArm', (-.03,-.05,0), False) yme.offsetJointLocal(motion, 'LeftArm', (.03,-.05,0), False) #yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False) yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(-1.5,0,1), .4), False) yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1.5,0,1), -.4), False) yme.updateGlobalT(motion) #motion = motion[50:-1] motion = motion[240:-1] #motion = motion[40:-58] #motion = motion[56:-248] #motion = motion[-249:-248] #motion = motion[62:65] #motion = motion[216:217] #motion = motion[515:555] ################ motion = motion[515:555] # motion = motion[164:280] #motion = motion[96:97] motion[0:0] = [motion[0]]*100 motion.extend([motion[-1]]*5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode('Hips') node.length = .2 node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0,0,0.1) node = mcfg.getNode('Spine') node.width = .22 #node.length = .2 #### node = mcfg.getNode('RightFoot') node.length = .25 node.width = .2 node.mass = 4. node = mcfg.getNode('LeftFoot') node.length = .25 node.width = .2 node.mass = 4. wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 30 wcfg.timeStep = (1/30.)/(stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1.#0.5 config['Bh'] = 1. config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\ 'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3} ''' config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':0.5, 'Spine1':0.5, 'RightFoot':1.2, 'LeftFoot':1.2, 'Hips':1.2,\ 'RightUpLeg':.9, 'RightLeg':.9, 'LeftUpLeg':.9, 'LeftLeg':.9} ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def testtest(): motionName = 'wd2_n_kick.bvh' motion = yf.readBvhFile(motionName) yf.writeBvhFile()
def create_foot(motionFile='foot3.bvh'): massMap = {} massMap = massMap.fromkeys(['Hips', 'foot00', 'foot01'], 0.) massMap['Hips'] = 1. massMap['foot00'] = 1. massMap['foot01'] = 1. # motion motion = yf.readBvhFile(motionFile, .05) motion.extend([motion[-1]] * 3000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. # for i in range(motion[0].skeleton.getElementNum()): # mcfg.addNode(motion[0].skeleton.getElementName(i)) for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] # totalMass += node.mass node = mcfg.getNode('Hips') # node.geom = 'MyFoot3' node.geom = 'MyBox' node.jointType = "B" # node.length = 1. node.mass = 1. node = mcfg.getNode('foot00') # node.geom = 'MyFoot4' node.geom = 'MyBox' node.jointType = "U" node.mass = 1. node = mcfg.getNode('foot01') # node.geom = 'MyFoot4' node.geom = 'MyBox' node.jointType = "U" node.mass = 1. def mcfgFix(_mcfg): """ :param _mcfg: ypc.ModelConfig :return: """ # for v in _mcfg.nodes.itervalues(): for k, v in _mcfg.nodes: if len(v.geoms) == 0: v.geoms.append(v.geom) v.geomMass.append(v.mass) v.geomTs.append(None) # mcfgFix(mcfg) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 40 simulSpeedInv = 1. wcfg.timeStep = (1 / 30. * simulSpeedInv) / stepsPerFrame # parameter config = dict([]) config['Kt'] = 20 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = 1 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 5000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. config['Bh'] = 1. config['stepsPerFrame'] = stepsPerFrame config['simulSpeedInv'] = simulSpeedInv # etc config['weightMap'] = {'root': 1., 'foot00': 1., 'foot01': 1.} config['weightMapTuple'] = (1., 1., 1.) # config['supLink'] = 'link0' return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_zygote_two_seg(): #motion motionName = 'wd2_tiptoe_zygote.bvh' #motionName = 'wd2_jump.bvh' #motionName = 'wd2_stand.bvh' motion = yf.readBvhFile(motionName, .01) # yme.removeJoint(motion, 'Head', False) # yme.removeJoint(motion, 'Head', False) # yme.removeJoint(motion, 'RightShoulder', False) # yme.removeJoint(motion, 'LeftShoulder1', False) # yme.removeJoint(motion, 'RightToes_Effector', False) # yme.removeJoint(motion, 'LeftToes_Effector', False) # yme.removeJoint(motion, 'RightHand_Effector', False) # yme.removeJoint(motion, 'LeftHand_Effector', False) # yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False) # yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False) yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False) # yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.5,0), -.6), False) # yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.5,0), -.6), False) yme.removeJoint(motion, 'RightFoot_Effector', False) yme.removeJoint(motion, 'LeftFoot_Effector', False) yme.addJoint(motion, 'LeftFoot', 'LeftToes', [0., 0., 0.12], False) yme.addJoint(motion, 'LeftToes', 'LeftToes_Effector', [0., 0., 0.07], False) yme.addJoint(motion, 'RightFoot', 'RightToes', [0., 0., 0.12], False) yme.addJoint(motion, 'RightToes', 'RightToes_Effector', [0., 0., 0.07], False) yme.updateGlobalT(motion) # motion.translateByOffset((0, -0.07, 0)) # motion.translateByOffset((0, -0.06, 0)) motion.extend([motion[-1]] * 300) del motion[:270] for i in range(2000): motion.data.insert(0, copy.deepcopy(motion[0])) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] # node = mcfg.getNode('Hips') # node.length = .2 # node.width = .25 node = mcfg.getNode('Hips') node.length = 4. / 27. node.width = .25 node = mcfg.getNode('Spine1') node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode('Spine') node.width = .22 node = mcfg.getNode('RightFoot') node.length = .177 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .8 node.offset = (0., 0., -0.02) node = mcfg.getNode('LeftFoot') node.length = .177 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .8 node.offset = (0., 0., -0.02) node = mcfg.getNode('RightToes') node.length = .053 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .218 # node.offset = (0,0,0.1) node = mcfg.getNode('LeftToes') node.length = .053 # node.length = .18 #node.length = .2 #node.width = .15 node.width = .1 node.mass = .218 # node.offset = (0,0,0.1) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 60 #stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #wcfg.timeStep = (1/1000.) # parameter config = {} ''' config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 2.5 config['Bh'] = 1. ''' config['Kt'] = 200 config['Dt'] = 2. * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2. * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2. * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2. * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\ #'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.} config['weightMap'] = { 'RightArm': .2, 'RightForeArm': .2, 'LeftArm': .2, 'LeftForeArm': .2, 'Spine': .6, 'Spine1': .6, 'RightFoot': .2, 'LeftFoot': .2, 'Hips': 0.5, 'RightUpLeg': .1, 'RightLeg': .3, 'LeftUpLeg': .1, 'LeftLeg': .3, 'RightToes': .2, 'LeftToes': .2 } #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':1., 'Hips':0.5,\ #'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.5, 'LeftLeg':1.5} #success!! ''' config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ 'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\ 'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.} ''' #config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\ #'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\ #'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5} config['supLink'] = 'LeftFoot' config['supLink1'] = 'LeftFoot' config['supLink2'] = 'RightFoot' #config['end'] = 'Hips' config['end'] = 'Spine1' return motion, mcfg, wcfg, stepsPerFrame, config
def preprocess(SEGMENT_FOOT=False): tasks = [] outputDir = './ppmotion/' dir = '../Data/woody2/Motion/Physics2/' config = None if SEGMENT_FOOT: # segmented foot config = { 'repeat': True, 'footRot': mm.rotX(.07), 'yOffset': 0., 'halfFootHeight': 0.0944444444444, 'scale': .01, 'type': 'woody2' } else: # box foot config = { 'repeat': True, 'footRot': mm.rotX(-.4), 'yOffset': 0., 'halfFootHeight': 0.0444444444444, 'scale': .01, 'type': 'woody2' } paths = [] paths.append(dir + 'wd2_WalkSameSame01.bvh') paths.append(dir + 'wd2_WalkForwardSlow01.bvh') paths.append(dir + 'wd2_WalkForwardNormal00.bvh') paths.append(dir + 'wd2_WalkHandWav00.bvh') paths.append(dir + 'wd2_WalkForwardFast00.bvh') paths.append(dir + 'wd2_WalkForwardVFast00.bvh') paths.append(dir + 'wd2_WalkLean00.bvh') paths.append(dir + 'wd2_WalkAzuma01.bvh') paths.append(dir + 'wd2_WalkSoldier00.bvh') paths.append(dir + 'wd2_WalkSukiko00.bvh') # paths.append(dir+'wd2_WalkBackward00.bvh') paths.append(dir + 'wd2_WalkTongTong00.bvh') tasks.append({'config': config, 'paths': paths}) ## # dir = '../Data/woody2/Motion/Balancing/' # config = {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'} # paths = [] # paths.append(dir+'wd2_2foot_walk_turn.bvh') # paths.append(dir+'wd2_2foot_walk_turn2.bvh') # paths.append(dir+'wd2_slow_2foot_hop.bvh') # paths.append(dir+'wd2_short_broad_jump.bvh') # paths.append(dir+'wd2_long_broad_jump.bvh') # paths.append(dir+'wd2_ffast_cancan_run.bvh') # paths.append(dir+'wd2_fast_cancan_run.bvh') # paths.append(dir+'wd2_fast_2foot_hop.bvh') # paths.append(dir+'wd2_1foot_contact_run.bvh') # paths.append(dir+'wd2_1foot_contact_run2.bvh') ## tasks.append({'config':config, 'paths':paths}) # ## dir = '../Data/woody2/Motion/Samsung/' ## config = {'footRot': mm.rotX(-.46), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} ## paths = [] ## paths.append(dir+'wd2_left_turn.bvh') ## paths.append(dir+'wd2_right_turn.bvh') ## paths.append(dir+'wd2_pose_inner1.bvh') ## paths.append(dir+'wd2_pose_inner2.bvh') ## tasks.append({'config':config, 'paths':paths}) # # dir = '../Data/woody2/Motion/Picking/' # config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'} # paths = [] # paths.append(dir+'wd2_pick_walk_1.bvh') ## tasks.append({'config':config, 'paths':paths}) # # dir = '../Data/woody2/Motion/VideoMotion/' # config = {'footRot': mm.rotX(-.48), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} ## paths = glob.glob(dir+'*.bvh') # paths = [] # paths.append(dir+'wd2_cross_walk_90d_fast_27.bvh') ## tasks.append({'config':config, 'paths':paths}) # # dir = '../Data/woody2/Motion/Walking/' # config = {'footRot': mm.rotX(-.40), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} # paths = glob.glob(dir+'*.bvh') ## tasks.append({'config':config, 'paths':paths}) # ## dir = '../Data/woody2/Motion/samsung_boxing/round/' ## config = {'footRot': mm.rotX(-.65), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} ## paths = glob.glob(dir+'*.bvh') ## tasks.append({'config':config, 'paths':paths}) # ## dir = '../Data/woody2/Motion/samsung_boxing/boxman/' ## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} ## paths = glob.glob(dir+'*.bvh') ## tasks.append({'config':config, 'paths':paths}) # # dir = '../Data/woody2/Motion/motion_edit/' # config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'} # paths = glob.glob(dir+'*.bvh') ## tasks.append({'config':config, 'paths':paths}) # ## outputDir = './icmotion_test/' ## dir = './rawmotion/' ## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'} ## paths = glob.glob(dir+'*.temp') ## tasks.append({'config':config, 'paths':paths}) # # outputDir = './icmotion_last/' # dir = './lastmotion/add/' ## config = {'rootRot':mm.rotZ(.05), 'footRot': mm.rotX(-.5), 'leftFootR':mm.rotZ(-.1), \ # config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.5), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \ # 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'} # paths = glob.glob(dir+'*.temp') # tasks.append({'config':config, 'paths':paths}) # outputDir = './ppmotion_slope/' # dir = './rawmotion_slope_extracted/' # config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.55), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \ # 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'} # paths = glob.glob(dir+'*.bvh') # tasks.append({'config':config, 'paths':paths}) VISUALIZE = False for task in tasks: config = task['config'] paths = task['paths'] for path in paths: motion = yf.readBvhFile(path) normalizeSkeleton(motion, config) adjustHeight(motion, config['halfFootHeight']) additionalEdit(motion, path) outputPath = outputDir + os.path.basename(path) if SEGMENT_FOOT: outputPath = outputDir + "segfoot_" + os.path.basename(path) yf.writeBvhFile(outputPath, motion) print(outputPath, 'done') if 'repeat' in config and config['repeat']: hRef = .1 vRef = .3 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) interval = yba.getWalkingCycle2(motion, lc) transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10 motion = ymt.repeatCycle(motion, interval, 50, transitionLength) outputName = os.path.splitext( os.path.basename(path))[0] + '_REPEATED.bvh' outputPath = outputDir + outputName if SEGMENT_FOOT: outputPath = outputDir + 'segfoot_' + outputName yf.writeBvhFile(outputPath, motion) print(outputPath, 'done') if VISUALIZE: viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion', yr.JointMotionRenderer(motion, (0, 100, 255), yr.LINK_LINE)) viewer.doc.addObject('motion', motion) viewer.startTimer(1 / 30.) viewer.show() Fl.run() print('FINISHED')
def __init__(self, env_name='walk'): self.world = pydart.World(1. / 1200., "../data/woody_with_ground_v2.xml") self.world.control_skel = self.world.skeletons[1] self.skel = self.world.skeletons[1] self.Kp, self.Kd = 400., 40. self.pdc = PDController(self.skel, self.world.time_step(), 400., 40.) self.env_name = env_name self.ref_motion = None # type: ym.Motion if env_name == 'walk': self.ref_motion = yf.readBvhFile( "../data/woody_walk_normal.bvh")[40:] elif env_name == 'spiral_walk': self.ref_motion = yf.readBvhFile( "../data/wd2_spiral_walk_normal05.bvh") elif env_name == 'walk_spin': self.ref_motion = yf.readBvhFile( "../data/wd2_2foot_walk_turn2.bvh") elif env_name == 'jump': self.ref_motion = yf.readBvhFile("../data/wd2_jump0.bvh")[164:280] elif env_name == 'walk_fast': self.ref_motion = yf.readBvhFile( "../data/wd2_WalkForwardVFast00.bvh") elif env_name == 'walk_left_90': self.ref_motion = yf.readBvhFile("../data/walk_left_90degree.bvh") elif env_name == 'walk_left_45': self.ref_motion = yf.readBvhFile("../data/walk_left_45degree.bvh") elif env_name == 'walk_pick': self.ref_motion = yf.readBvhFile("../data/wd2_pick_walk_1.bvh") elif env_name == 'walk_u_turn': self.ref_motion = yf.readBvhFile("../data/wd2_u-turn.bvh")[25:214] self.ref_motion.translateByOffset([0., 0.03, 0.]) elif env_name == 'jump_whole': self.ref_motion = yf.readBvhFile("../data/wd2_jump0.bvh")[315:966] elif env_name == 'walk_u_turn_whole': self.ref_motion = yf.readBvhFile("../data/wd2_u-turn.bvh") self.ref_motion.translateByOffset([0., 0.03, 0.]) self.ref_world = pydart.World(1. / 1200., "../data/woody_with_ground.xml") self.ref_skel = self.ref_world.skeletons[1] # self.step_per_frame = round((1./self.world.time_step()) / self.ref_motion.fps) self.step_per_frame = 40 self.rsi = True self.w_p = 0.65 self.w_v = 0.1 self.w_e = 0.15 self.w_c = 0.1 self.exp_p = 2. self.exp_v = 0.1 self.exp_e = 40. self.exp_c = 10. self.body_num = self.skel.num_bodynodes() self.idx_e = [ self.skel.bodynode_index('LeftFoot'), self.skel.bodynode_index('RightFoot'), self.skel.bodynode_index('LeftForeArm'), self.skel.bodynode_index('RightForeArm') ] self.body_e = list(map(self.skel.body, self.idx_e)) self.ref_body_e = list(map(self.ref_skel.body, self.idx_e)) self.motion_len = len(self.ref_motion) self.motion_time = len(self.ref_motion) / self.ref_motion.fps self.time_offset = 0. state_num = 1 + (3 * 3 + 4) * self.body_num action_num = self.skel.num_dofs() - 6 state_high = np.array([np.finfo(np.float32).max] * state_num) action_high = np.array([pi * 10. / 2.] * action_num) self.action_space = gym.spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = gym.spaces.Box(-state_high, state_high, dtype=np.float32) self.viewer = None self.phase_frame = 0 # max training time self.training_time = 3. self.evaluation = False self.visualize = False self.visualize_select_motion = 0
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