def test_getBipedGaitStates(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) hRef = .1; vRef = .3 lc = yma.getElementContactStates(motion, motion[0].skeleton.getElementIndex('LeftFoot'), hRef, vRef) rc = yma.getElementContactStates(motion, motion[0].skeleton.getElementIndex('RightFoot'), hRef, vRef) rawStateList = getBipedGaitStates(lc, rc) cookedStateList = getBipedGaitStates(lc, rc, 10, 1.) intervals, types = yma.states2intervals(cookedStateList) for i in range(len(intervals)): print(intervals[i], GaitState.text[types[i]]) print() print([yma.intIntervalUp(int) for int in intervals]) print(getWalkingSteps(lc, rc, True)) print(getBipedGaitIntervals(lc, rc, 10, 1.)) plot = ymp.SmartPlot() plot.setXdata('frame', range(len(motion))) plot.addYdata('rawState', rawStateList) plot.addYdata('cookedState', cookedStateList) plot.showModeless() viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_LINE)) viewer.doc.addObject('motion', motion) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_late_foottouch(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) lFoot = motion[0].skeleton.getElementIndex('LeftFoot') rFoot = motion[0].skeleton.getElementIndex('RightFoot') hRef = .1 vRef = .3 lc = yma.getElementContactStates(motion, lFoot, hRef, vRef) rc = yma.getElementContactStates(motion, rFoot, hRef, vRef) steps = yba.getWalkingSteps(lc, rc, True) stepMotions = yma.splitMotionIntoSegments(motion, steps) lateCycle = 2 howLate = 5 print('steps[%d] :' % lateCycle) print( 'motion[%d]~motion[%d] => ' % (steps[lateCycle][0], steps[lateCycle][1]), ) print('motion_latetouch[%d]~motion_earyltouch[%d]' % (steps[lateCycle][0], steps[lateCycle][1] + howLate)) motion_latetouch = stepMotions[0] for i in range(1, len(stepMotions)): if i == lateCycle: stepMotions[i].extend( ymt.extendByIntegration(stepMotions[i], howLate)) # stitched = getStitchedNextMotion(stepMotions[i], motion_latetouch[-1], len(stepMotions[i])-1, yfg.halfsine) stitched = getStitchedNextMotion(stepMotions[i], motion_latetouch[-1], len(stepMotions[i]) - 1, yfg.hermite2nd) # stitched = getStitchedNextMotion(stepMotions[i], motion_latetouch[-1], len(stepMotions[i])-1, yfg.identity) motion_latetouch += stitched viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion', yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_LINE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motion_latetouch', yr.JointMotionRenderer(motion_latetouch, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('motion_latetouch', motion_latetouch) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def test_getStitchedNextMotion(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) lFoot = motion[0].skeleton.getElementIndex('LeftFoot') rFoot = motion[0].skeleton.getElementIndex('RightFoot') hRef = .1 vRef = .3 lc = yma.getElementContactStates(motion, lFoot, hRef, vRef) rc = yma.getElementContactStates(motion, rFoot, hRef, vRef) steps = yba.getWalkingSteps(lc, rc, True) stepMotions = yma.splitMotionIntoSegments(motion, steps) motion_stitch = stepMotions[0] motion_getStitchedNextMotion = stepMotions[0] print(len(motion_stitch), len(motion_getStitchedNextMotion)) for i in range(1, len(stepMotions)): stepMotions[i] = stepMotions[i][:-5] motion_stitch = stitch(motion_stitch, stepMotions[i], 5) stitched = getStitchedNextMotion(stepMotions[i], motion_getStitchedNextMotion[-1], 5) motion_getStitchedNextMotion += stitched print(len(motion_stitch), len(motion_getStitchedNextMotion)) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion', yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer( 'motion_stitch', yr.JointMotionRenderer(motion_stitch, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('motion_stitch', motion_stitch) viewer.doc.addRenderer( 'motion_getStitchedNextMotion', yr.JointMotionRenderer(motion_getStitchedNextMotion, (255, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('motion_getStitchedNextMotion', motion_getStitchedNextMotion) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def test_getWalkingCycle(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) lFoot = jointMotion[0].skeleton.getElementIndex('LeftFoot') rFoot = jointMotion[0].skeleton.getElementIndex('RightFoot') hRef = .1; vRef = .3 lc = yma.getElementContactStates(jointMotion, lFoot, hRef, vRef) # rc = getElementContactStates(jointMotion, rFoot, hRef, vRef) interval = getWalkingCycle(jointMotion, lc) cycleMotion = jointMotion[interval[0]:interval[-1]+1] startMotion = cycleMotion[:1] endMotion = cycleMotion[-1:] viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('jointMotion', yr.JointMotionRenderer(jointMotion, (0,0,255), yr.LINK_LINE)) viewer.doc.addObject('jointMotion', jointMotion) viewer.doc.addRenderer('startMotion', yr.JointMotionRenderer(startMotion, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('startMotion', startMotion) viewer.doc.addRenderer('endMotion', yr.JointMotionRenderer(endMotion, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('endMotion', endMotion) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_repeatCycle(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) # bvhFilePath = '../samples/wd2_n_kick.bvh' # jointMotion = yf.readBvhFile(bvhFilePath, .01*2.53) lFoot = jointMotion[0].skeleton.getElementIndex('LeftFoot') rFoot = jointMotion[0].skeleton.getElementIndex('RightFoot') hRef = .1 vRef = .3 lc = yma.getElementContactStates(jointMotion, lFoot, hRef, vRef) interval = yba.getWalkingCycle(jointMotion, lc) # interval = [60, 102] extendedMotion = repeatCycle(jointMotion, interval, 10, 10) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'jointMotion', yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('jointMotion', jointMotion) viewer.doc.addRenderer( 'extendedMotion', yr.JointMotionRenderer(extendedMotion, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('extendedMotion', extendedMotion) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def additionalEdit(motion, path): if 'wd2_left_turn.bvh' in path or 'wd2_right_turn.bvh' in path: lFoot = motion[0].skeleton.getJointIndex('LeftFoot') yte.setPositionTarget(motion, lFoot, motion[0].getJointPositionGlobal(lFoot)+(-.1,0,-.1)\ , [0,58], 150) if 'extended' in path: hRef = .15 vRef = .2 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) interval = yba.getWalkingCycle(motion, lc) motion[:] = ymt.repeatCycle(motion, interval, 50, 10)
def test_getWalkingSteps(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) motion = motion[:-10] lFoot = motion[0].skeleton.getElementIndex('LeftFoot') rFoot = motion[0].skeleton.getElementIndex('RightFoot') hRef = .1; vRef = .3 lc = yma.getElementContactStates(motion, lFoot, hRef, vRef) rc = yma.getElementContactStates(motion, rFoot, hRef, vRef) t, l_landing = yma.getTakingLandingFrames(lc) t, r_landing = yma.getTakingLandingFrames(rc) landing = l_landing + r_landing landing.sort() print('landingFrames', landing) # steps = getWalkingSteps(lc, rc, True) steps = getWalkingSteps(lc, rc, True, False) # steps = getWalkingSteps(lc, rc, False) print('steps', steps) print() stepMotions = yma.splitMotionIntoSegments(motion, steps) for i in range(len(steps)): print('stepMotions[%d]: motion[%d]~motion[%d], len %d'%(i, steps[i][0], steps[i][1], len(stepMotions[i]))) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,100,255), yr.LINK_LINE, 3.)) viewer.doc.addObject('motion', motion) for i in range(len(steps)): viewer.doc.addRenderer('stepMotions[%d]'%i, yr.JointMotionRenderer(stepMotions[i], (0,255,0), yr.LINK_LINE, 3.)) viewer.doc.addObject('stepMotions[%d]', stepMotions[i]) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_blend(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion0 = yf.readBvhFile(bvhFilePath, .01) # bvhFilePath = '../samples/wd2_WalkSukiko00.bvh' bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh' motion1 = yf.readBvhFile(bvhFilePath, .01) hRef = .1 vRef = .3 RHEEL = motion0[0].skeleton.getElementIndex('RightFoot') rc0 = yma.getElementContactStates(motion0, RHEEL, hRef, vRef) rc1 = yma.getElementContactStates(motion1, RHEEL, hRef, vRef) intervals0 = yma.states2intervals(rc0)[0] intervals1 = yma.states2intervals(rc1)[0] blendedMotion = blend(motion0, yma.intIntervalInner(intervals0[3]), motion1, yma.intIntervalInner(intervals1[3])) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion0', yr.JointMotionRenderer(motion0, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion0', motion0) viewer.doc.addRenderer( 'motion1', yr.JointMotionRenderer(motion1, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion1', motion1) viewer.doc.addRenderer( 'blendedMotion', yr.JointMotionRenderer(blendedMotion, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('blendedMotion', blendedMotion) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
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 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 test_getStitchedNextMotion2(): bvhFilePath = '../samples/walk_left_90degree.bvh' motion = yf.readBvhFile(bvhFilePath) lFoot = motion[0].skeleton.getElementIndex('LeftFoot') rFoot = motion[0].skeleton.getElementIndex('RightFoot') hRef = .1 vRef = .3 lc = yma.getElementContactStates(motion, lFoot, hRef, vRef) rc = yma.getElementContactStates(motion, rFoot, hRef, vRef) steps = yba.getWalkingSteps(lc, rc, True) stepMotions = yma.splitMotionIntoSegments(motion, steps) print(steps) earlyCycle = 3 howEarly = 10 transitionFunc = lambda x: 1. - yfg.hermite2nd(x) print('steps[%d] :' % earlyCycle) print( 'motion[%d]~motion[%d] => ' % (steps[earlyCycle][0], steps[earlyCycle][1]), ) print('motion_stitched_trans_rot[%d]~motion_stitched_trans_rot[%d]' % (steps[earlyCycle][0], steps[earlyCycle][1] - howEarly)) motion_stitched_trans = stepMotions[0].copy() motion_stitched_trans_rot = stepMotions[0].copy() motion_stitched_d = stepMotions[0].copy() motion_attached = stepMotions[0].copy() for i in range(1, len(stepMotions)): if i == earlyCycle: stepMotions[i] = stepMotions[i][:-howEarly] # motion_stitched_trans += getStitchedNextMotion(stepMotions[i], motion_stitched_trans[-1], len(stepMotions[i])-1, transitionFunc, True, False) motion_stitched_trans_rot += getStitchedNextMotion( stepMotions[i].copy(), motion_stitched_trans_rot[-1], len(stepMotions[i]) - 1, transitionFunc, True, True) # d = motion_stitched_d[-1] - stepMotions[i][0] # motion_stitched_d += getStitchedNextMotion(stepMotions[i], d, len(stepMotions[i])-1, transitionFunc, True, True) # motion_attached += getStitchedNextMotion(stepMotions[i], motion_attached[-1], len(stepMotions[i])-1, yfg.zero, True, True) motion_attached += getAttachedNextMotion(stepMotions[i], motion_attached[-1], len(stepMotions[i]) - 1, yfg.zero, True, True) frame0 = [mm.I_SE3()] viewer = ysv.SimpleViewer() viewer.record(False) # viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (100,100,255), yr.LINK_LINE)) # viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motion_stitched_trans', yr.JointMotionRenderer(motion_stitched_trans, (255,255,0), yr.LINK_LINE)) # viewer.doc.addObject('motion_stitched_trans', motion_stitched_trans) viewer.doc.addRenderer( 'motion_stitched_trans_rot', yr.JointMotionRenderer(motion_stitched_trans_rot, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('motion_stitched_trans_rot', motion_stitched_trans_rot) viewer.doc.addRenderer( 'motion_attached', yr.JointMotionRenderer(motion_attached, (255, 255, 255), yr.LINK_LINE)) viewer.doc.addObject('motion_attached', motion_attached) viewer.doc.addRenderer('frame0', yr.FramesRenderer(frame0, (255, 255, 0))) def simulateCallback(frame): frame0[0] = motion_stitched_trans_rot[frame].getJointFrame(0) # frame0[0] = motion_attached[frame].getJointFrame(0) viewer.setSimulateCallback(simulateCallback) viewer.startTimer((1 / 30.) / 1.4) viewer.show() Fl.run()
def test_stitch(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion0 = yf.readBvhFile(bvhFilePath, .01) bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh' motion1 = yf.readBvhFile(bvhFilePath, .01) hRef = .1 vRef = .3 LHEEL = motion0[0].skeleton.getElementIndex('LeftFoot') RHEEL = motion0[0].skeleton.getElementIndex('RightFoot') lc0 = yma.getElementContactStates(motion0, LHEEL, hRef, vRef) rc0 = yma.getElementContactStates(motion0, RHEEL, hRef, vRef) lc1 = yma.getElementContactStates(motion1, LHEEL, hRef, vRef) rc1 = yma.getElementContactStates(motion1, RHEEL, hRef, vRef) intervals0, states0 = yba.getBipedGaitIntervals(lc0, rc0, 10, .1) intervals1, states1 = yba.getBipedGaitIntervals(lc1, rc1, 10, .1) print('wd2_WalkSameSame00') print(intervals0) print([yba.GaitState.text[state] for state in states0]) print('wd2_WalkForwardVFast00') print(intervals1) print([yba.GaitState.text[state] for state in states1]) print() seg = 3 tlen = 10 print('motion0[:%d] + motion1[%d:]' % (intervals0[seg][1], intervals1[seg][1])) print('transition length', tlen) print('stitched_0changed', '%d~%d' % (intervals0[seg][1] - tlen, intervals0[seg][1])) print('stitched_1changed', '%d~%d' % (intervals0[seg][1], intervals0[seg][1] + tlen)) print() just_added = motion0[:intervals0[seg][1]] + motion1[intervals1[seg][1]:] print('len(just_added)', len(just_added)) stitched_0changed = stitch(motion0[:intervals0[seg][1]], motion1[intervals1[seg][1] - 1:], tlen, yfg.identity, True) print('len(stitched_0changed)', len(stitched_0changed)) stitched_1changed = stitch(motion0[:intervals0[seg][1]], motion1[intervals1[seg][1] - 1:], tlen, yfg.identity, False) print('len(stitched_1changed)', len(stitched_1changed)) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'just_added', yr.JointMotionRenderer(just_added, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('just_added', just_added) viewer.doc.addRenderer( 'stitched_0changed', yr.JointMotionRenderer(stitched_0changed, (0, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('stitched_0changed', stitched_0changed) viewer.doc.addRenderer( 'stitched_1changed', yr.JointMotionRenderer(stitched_1changed, (255, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('stitched_1changed', stitched_1changed) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
def main2(): import PyCommon.modules.Motion.ysMotionAnalysis as yma pydart.init() world = [ pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml") for _ in range(2) ] motion_files = ['wd2_jump0.bvh'] motion_files = ['woody_walk_normal.bvh'] motion_files = ['walk_left_90degree.bvh'] motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False) # 70, 112, 156 right heel strike mg = hmg.MotionGraph() mg.add_motion(motion) mg.build() total_num = len(mg.transition_processed) # viewer settings rd_contact_positions = [None] rd_contact_forces = [None] viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False) viewer.doc.addRenderer('motionModel[0]', yr.DartRenderer(world[0], (255, 240, 255))) viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1])) motion_state = [0] def callback_0(_): motion_state[0] = 0 def callback_1(_): motion_state[0] = 1 def callback_2(_): motion_state[0] = 2 viewer.objectInfoWnd.addBtn('0', callback_0) viewer.objectInfoWnd.addBtn('1', callback_1) viewer.objectInfoWnd.addBtn('2', callback_2) hRef = 0.1 vRef = 0.4 lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef) rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef) lindex = motion[0].skeleton.getElementIndex('LeftFoot') rindex = motion[0].skeleton.getElementIndex('RightFoot') def postCallback(frame): transition = mg.transition_processed[frame] world[0].skeletons[1].set_positions( motion.get_q(transition.motion_from_idx)) world[1].skeletons[1].set_positions( motion.get_q(transition.motion_to_idx)) print(frame, transition.motion_from_idx, transition.motion_to_idx, transition.dist) print(motion.getPosition(lindex)) print(lc[transition.motion_from_idx], rc[transition.motion_from_idx]) print(lc[transition.motion_to_idx], rc[transition.motion_to_idx]) viewer.setPreFrameCallback_Always(postCallback) viewer.setMaxFrame(total_num - 1) viewer.startTimer(1. / 30.) viewer.show() all_node_set = set() dot = Digraph() for i in range(len(mg.transition_processed)): all_node_set.add(mg.transition_processed[i].motion_from_idx) all_node_set.add(mg.transition_processed[i].motion_to_idx) all_node_list = list(all_node_set) all_node_list.sort() for i in all_node_list: dot.node(str(i)) all_edge_set = set() for i in range(len(all_node_list) - 1): all_edge_set.add((all_node_list[i], all_node_list[i + 1])) for i in range(len(mg.transition_processed)): all_edge_set.add((mg.transition_processed[i].motion_from_idx, mg.transition_processed[i].motion_to_idx)) for i in list(all_edge_set): dot.edge(str(i[0]), str(i[1])) # print(dot.source) dot.render('test', view=True) dot.pipe() Fl.run()
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')