def test_getStitchedNextMotion_y_angle(): bvhFilePath = '../samples/walk_left_90degree.bvh' motion0 = yf.readBvhFile(bvhFilePath) bvhFilePath = '../samples/walk_left_90degree.bvh' motion1 = yf.readBvhFile(bvhFilePath) # stitched_motion = ymb.stitch(motion0, motion1, 20, yfg.hermite2nd) transitionLength = 20 transitionFunc = lambda x:1.-yfg.hermite2nd(x) m0 = motion0.copy() # m1 = getStitchedNextMotion(motion1, motion0[-1], transitionLength, transitionFunc, alignOrientation=False) m1 = getStitchedNextMotion_y_angle(motion1, motion0[-1], motion0[0], math.pi/4, transitionLength,transitionFunc) stitched_motion = m0 + m1 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,255,0), yr.LINK_LINE)) viewer.doc.addObject('motion1', motion1) viewer.doc.addRenderer('stitched_motion', yr.JointMotionRenderer(stitched_motion, (255,255,0), yr.LINK_LINE)) viewer.doc.addObject('stitched_motion', stitched_motion) 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 test_blendedSegment(): frameTime = 1/30. # source motions # bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' # motion0 = yf.readBvhFile(bvhFilePath, .01) # motion1 = motion0.copy() # motion1.rotateTrajectory(mm.rotY(math.pi/2.)) # segment0 = motion0[55:95] # segment1 = motion1[55:95].copy() ## blendedS = blendSegmentSmooth(segment0, segment1, True, True) ## blendedF = blendSegmentFixed(segment0, segment1, .5, True, True) # blendedS = blendSegmentSmooth(segment0, segment1, True, False) # blendedF = blendSegmentFixed(segment0, segment1, .5, True, False) ## blendedS = blendSegmentSmooth(segment0, segment1, False, True) ## blendedF = blendSegmentFixed(segment0, segment1, .5, False, True) ## blendedS = blendSegmentSmooth(segment0, segment1, False, False) ## blendedF = blendSegmentFixed(segment0, segment1, .5, False, False) bvhFilePath = '../samples/woddy2_walk_straight_fast.bvh' motion0 = yf.readBvhFile(bvhFilePath, .01) bvhFilePath = '../samples/woddy2_walk_leftfoot_turn_normal.bvh' motion1 = yf.readBvhFile(bvhFilePath, .01) segment0 = motion1[56:100] segment1 = motion1[83:125] blendedS = blendSegmentSmooth(segment0, segment1) blendedF = blendSegmentFixed(segment0, segment1, .5) viewer = ysv.SimpleViewer() viewer.record(False) # viewer.doc.addRenderer('motion0', yr.JointMotionRenderer(motion0, (0,0,255), yr.LINK_BONE)) # viewer.doc.addObject('motion0', motion0) # viewer.doc.addRenderer('motion1', yr.JointMotionRenderer(motion1, (0,0,255), yr.LINK_BONE)) # viewer.doc.addObject('motion1', motion1) viewer.doc.addRenderer('segment0', yr.JointMotionRenderer(segment0, (0,0,255), yr.LINK_BONE)) viewer.doc.addObject('segment0', segment0) viewer.doc.addRenderer('segment1', yr.JointMotionRenderer(segment1, (0,0,255), yr.LINK_BONE)) viewer.doc.addObject('segment1', segment1) viewer.doc.addRenderer('blendedS', yr.JointMotionRenderer(blendedS, (255,0,0), yr.LINK_BONE)) viewer.doc.addObject('blendedS', blendedS) viewer.doc.addRenderer('blendedF', yr.JointMotionRenderer(blendedF, (0,255,0), yr.LINK_BONE)) viewer.doc.addObject('blendedF', blendedF) # viewer.startTimer(frameTime/1.4) 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 test_setPositionTarget(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) print motion[0].skeleton lFoot = motion[0].skeleton.getJointIndex('LeftFoot') oripos = motion[0].getJointPositionGlobal(lFoot) newpos = oripos + mm.v3(0,1,0) motion_edit1 = copy.deepcopy(motion) setPositionTarget(motion_edit1, lFoot, newpos) motion_edit2 = copy.deepcopy(motion) setPositionTarget(motion_edit2, lFoot, newpos, [10,20], 5, yfg.identity) print len(motion), len(motion_edit2) 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_edit1', yr.JointMotionRenderer(motion_edit1, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('motion_edit1', motion_edit1) viewer.doc.addRenderer('motion_edit2', yr.JointMotionRenderer(motion_edit2, (255,255,0), yr.LINK_LINE)) viewer.doc.addObject('motion_edit2', motion_edit2) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_blendedSegment_posture_warping(): 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) frameTime = 1/30. firstMotion = motion0[0:75] # midMotionSmooth1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], None) # midMotionFixed1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], .5) # midMotionFixed2 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], 1.) midMotionSmooth1 = firstMotion + getAttachedNextMotion(blendSegmentSmooth(motion0[75-1:95], motion1[120-1:150]), firstMotion[-1]) midMotionFixed1 = firstMotion + getAttachedNextMotion(blendSegmentFixed(motion0[75-1:95], motion1[120-1:150], .5), firstMotion[-1]) midMotionFixed2 = firstMotion + getAttachedNextMotion(blendSegmentFixed(motion0[75-1:95], motion1[120-1:150], 1.), firstMotion[-1]) midMotion = midMotionSmooth1 secondMotion = midMotion + getAttachedNextMotion(motion1[150:], midMotion[-1]) # print len(midMotionSmooth1), len(midMotionSmooth2) # print len(midMotionFixed1), len(midMotionFixed2) 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('firstMotion', yr.JointMotionRenderer(firstMotion, (255,0,0), yr.LINK_LINE)) viewer.doc.addObject('firstMotion', firstMotion) viewer.doc.addRenderer('midMotionSmooth1', yr.JointMotionRenderer(midMotionSmooth1, (255,255,0), yr.LINK_LINE)) viewer.doc.addObject('midMotionSmooth1', midMotionSmooth1) viewer.doc.addRenderer('midMotionFixed1', yr.JointMotionRenderer(midMotionFixed1, (255,0,255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed1', midMotionFixed1) viewer.doc.addRenderer('midMotionFixed2', yr.JointMotionRenderer(midMotionFixed2, (0,255,255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed2', midMotionFixed2) # viewer.doc.addRenderer('secondMotion', yr.JointMotionRenderer(secondMotion, (0,255,0), yr.LINK_LINE)) # viewer.doc.addObject('secondMotion', secondMotion) # viewer.startTimer(frameTime/1.4) viewer.show() Fl.run()
def test_InteractivePlot(): import Resource.ysMotionLoader as yf import Renderer.ysRenderer as yr import GUI.ysSimpleViewer as ysv bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) jointMotion = jointMotion[0:200] hips = jointMotion[0].skeleton.getJointIndex('Hips') lFoot = jointMotion[0].skeleton.getJointIndex('LeftFoot') rFoot = jointMotion[0].skeleton.getJointIndex('RightFoot') plot = InteractivePlot() plot.setXlimit(0, len(jointMotion)) plot.setYlimit(0, 2) plot.addDataSet('root.y') plot.addDataSet('leftFoot.y') plot.addDataSet('rightFoot.y') viewer = ysv.SimpleViewer() viewer.doc.addRenderer('motion(%s)'%jointMotion.resourceName, yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion(%s)'%jointMotion.resourceName, jointMotion) pt = [None] updated = [False] def preFrameCallback(frame): if updated[0] == False: plot.addDataPoint('root.y', frame, jointMotion[frame].getJointPositionGlobal(hips)[1]) plot.addDataPoint('leftFoot.y', frame, jointMotion[frame].getJointPositionGlobal(lFoot)[1]) plot.addDataPoint('rightFoot.y', frame, jointMotion[frame].getJointPositionGlobal(rFoot)[1]) plot.updatePoints() if frame == viewer.getMaxFrame(): updated[0] = True if frame==0: pt[0] = time.time() if frame==50: print time.time()-pt[0] def preFrameCallback_Always(frame): plot.updateVline(frame) def viewer_onClose(data): plot.close() viewer.onClose(data) viewer.setPreFrameCallback(preFrameCallback) viewer.setPreFrameCallback_Always(preFrameCallback_Always) viewer.callback(viewer_onClose) viewer.startTimer(1/30.) 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]
def profile_stitch_blend(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) # global part1 = motion[:50].copy() part2 = motion[49:].copy() # motion_stitched_trans_rot += getStitchedNextMotion(stepMotions[i].copy(), motion_stitched_trans_rot[-1], len(stepMotions[i])-1, transitionFunc, True, True) profileDataFile = '../samples/profile_stitch_blend_%s.profile'%datetime.today().strftime('%y%m%d_%H%M%S') # cProfile.runctx('getStitchedNextMotion(part2, part1[-1], 50)', globals(), locals(), profileDataFile) cProfile.runctx('blendSegmentSmooth(part2, part1)', globals(), locals(), profileDataFile) os.system('c:\python25\python.exe ../Tools/pprofui.py %s'%profileDataFile)
def test_showModeless(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) # jointMotion = jointMotion[0:10] plot = SmartPlot() plot.setXdata('frame', [0]) plot.showModeless() viewer = ysv.SimpleViewer() viewer.doc.addRenderer('motion(%s)'%jointMotion.resourceName, yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion(%s)'%jointMotion.resourceName, jointMotion) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_distancFuncs(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) rankNum = 5 srcFrame = 100 searchIntervals = [[0,80], [120,200]] print 'srcFrame', srcFrame print 'searchIntervals', searchIntervals def isIn(i, searchIntervals): for searchInterval in searchIntervals: if i >= searchInterval[0] and i <= searchInterval[1]: return True return False distances = [None]*len(jointMotion) for i in range(len(jointMotion)): if isIn(i, searchIntervals): distances[i] = distanceByRelPos(jointMotion[srcFrame], jointMotion[i]) else: distances[i] = sys.maxint sorted = copy.copy(distances) sorted.sort() print 'rank \t frame \t distance' rankedPostures = [None]*rankNum for i in range(1,rankNum): for j in range(len(jointMotion)): if sorted[i] == distances[j]: rankedPostures[i] = jointMotion[j] print '%d \t %d \t %f'%(i, j, distances[j]) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('jointMotion', yr.JointMotionRenderer(jointMotion, (0,0,255), yr.LINK_BONE)) viewer.doc.addObject('jointMotion', jointMotion) viewer.doc.addRenderer('srcFrame', yr.JointMotionRenderer(jointMotion[srcFrame:srcFrame+1], (0,255,0), yr.LINK_BONE)) for i in range(1, rankNum): viewer.doc.addRenderer('%d'%i, yr.JointMotionRenderer(ym.Motion([rankedPostures[i]]), numpy.array([255,255,255])*(1-(i-1)*.2), yr.LINK_BONE)) viewer.startTimer(1/30.) viewer.show() Fl.run()
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]
def test_stitch_fix_foot(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) print motion[0].skeleton print hRef = .1; vRef = .3 lfoot = motion[0].skeleton.getElementIndex('LeftFoot') rfoot = motion[0].skeleton.getElementIndex('RightFoot') rtoe = motion[0].skeleton.getElementIndex('RightToes_Effector') lc = yma.getElementContactStates(motion, lfoot, hRef, vRef) rc = yma.getElementContactStates(motion, rfoot, hRef, vRef) intervals, states = yba.getBipedGaitIntervals(lc, rc, 10, .1) segments = yma.splitMotionIntoSegments(motion, intervals) print 'wd2_WalkSameSame00' print intervals print [yba.GaitState.text[state] for state in states]
def test_timescale(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) frameTime = 1/30. scaledMotion = timescale(jointMotion, 400) # scaledMotion = timescale(jointMotion, 400, None) print len(jointMotion), len(scaledMotion) 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('scaledMotion', yr.JointMotionRenderer(scaledMotion, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('scaledMotion', scaledMotion) 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_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_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_offsetJointLocal(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) print motion[0].skeleton shortLegs = copy.deepcopy(motion) offsetJointLocal(shortLegs, 'LeftLeg', (0, .2, 0)) offsetJointLocal(shortLegs, 'RightLeg', (0, .2, 0)) offsetJointLocal(shortLegs, 'LeftFoot', (0, .2, 0)) offsetJointLocal(shortLegs, 'RightFoot', (0, .2, 0)) 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('shortLegs', yr.JointMotionRenderer(shortLegs, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('shortLegs', shortLegs) viewer.startTimer(1/30.) viewer.show() Fl.run()
def test_SimpleViewer(): # mmFilePath = '../samples/physics2_WalkSameSame01.mm' # pointMotion1 = yf.readMMFile(mmFilePath) # mmFilePath = '../samples/physics2_WalkForwardFast00.mm' # pointMotion2 = yf.readMMFile(mmFilePath) # frameTime = 1./30. # # viewer = SimpleViewer() ## viewer = SimpleViewer((200,200,400,400)) # viewer.doc.addMotion(pointMotion1) # viewer.doc.addMotion(pointMotion2) # viewer.doc.addRenderer('WalkSameSame', yr.PointMotionRenderer(pointMotion1)) # viewer.doc.addRenderer('WalkForwardFast', yr.PointMotionRenderer(pointMotion2)) ## viewer.renderersWnd.when(FL_WHEN_CHANGED) ## print FL_WHEN_RELEASE ## print viewer.renderersWnd.when() # # viewer.startTimer(frameTime) # viewer.show() # Fl.run() pointMotion = yf.readTrcFile('../samples/Day7_Session2_Take01_-_walk.trc', .01) jointMotion = yf.readBvhFile('../samples/wd2_WalkSameSame00.bvh', .01) print 'pointSkeleton' print pointMotion[0].skeleton print 'jointSkeleton' print jointMotion[0].skeleton viewer = SimpleViewer() viewer.record(False) viewer.doc.addRenderer('pointMotion', yr.PointMotionRenderer(pointMotion, (0,255,0))) viewer.doc.addObject('pointMotion', pointMotion) viewer.doc.addRenderer('jointMotion', yr.JointMotionRenderer(jointMotion, (0,255,0))) viewer.doc.addObject('jointMotion', jointMotion) viewer.startTimer(1/pointMotion.fps) viewer.show() Fl.run()
def test_ik_analytic(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) ik_target = [(0, .3, .3)] jointMotion2 = copy.deepcopy(jointMotion) for i in range(len(jointMotion2)): ik_analytic(jointMotion2[i], 'LeftFoot', ik_target[0]) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('jointMotion', yr.JointMotionRenderer(jointMotion, (0,150,255))) viewer.doc.addObject('jointMotion', jointMotion) viewer.doc.addRenderer('jointMotion2', yr.JointMotionRenderer(jointMotion2, (0,255,0))) viewer.doc.addObject('jointMotion2', jointMotion2) viewer.doc.addRenderer('ik_target', yr.PointsRenderer(ik_target, (255,0,0))) viewer.startTimer(1/30.) viewer.show() Fl.run() pass
def test_blendSegment_time_warping(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) frameTime = 1/30. firstMotion = jointMotion[0:70] midMotionSmooth1 = firstMotion + blendSegmentSmooth(jointMotion[70:120], timescale(jointMotion[70:120], len(jointMotion[70:120])*2)) midMotionFixed1 = firstMotion + blendSegmentFixed(jointMotion[70:120], timescale(jointMotion[70:120], len(jointMotion[70:120])*2), .5) midMotionFixed2 = firstMotion + blendSegmentFixed(jointMotion[70:120], timescale(jointMotion[70:120], len(jointMotion[70:120])*2), 0.) midMotion = midMotionFixed1 secondMotion = midMotion + timescale(jointMotion[120:], len(jointMotion[120:])*2) # print len(midMotionSmooth1), len(midMotionSmooth2) # print len(midMotionFixed1), len(midMotionFixed2) 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('firstMotion', yr.JointMotionRenderer(firstMotion, (255,0,0), yr.LINK_LINE)) viewer.doc.addObject('firstMotion', firstMotion) # viewer.doc.addRenderer('midMotionSmooth1', yr.JointMotionRenderer(midMotionSmooth1, (255,255,0), yr.LINK_LINE)) # viewer.doc.addObject('midMotionSmooth1', midMotionSmooth1) ## viewer.doc.addRenderer('midMotionSmooth2', yr.JointMotionRenderer(midMotionSmooth2, (255,255,0), yr.LINK_LINE)) ## viewer.doc.addObject('midMotionSmooth2', midMotionSmooth2) viewer.doc.addRenderer('midMotionFixed1', yr.JointMotionRenderer(midMotionFixed1, (255,0,255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed1', midMotionFixed1) viewer.doc.addRenderer('midMotionFixed2', yr.JointMotionRenderer(midMotionFixed2, (255,0,255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed2', midMotionFixed2) viewer.doc.addRenderer('secondMotion', yr.JointMotionRenderer(secondMotion, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('secondMotion', secondMotion) viewer.startTimer(frameTime/1.4) viewer.show() Fl.run()
def test_getAllParentIndexes(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath) skeleton = motion[0].skeleton print 'getAllParentIndexes' masks = getAllParentIndexes(skeleton) print masks for i in range(len(masks)): print 'parents of', skeleton.getElementName(i), ':', for parent in masks[i]: print skeleton.getElementName(parent), print print print 'getAllParentJointIndexes' masks = getAllParentJointIndexes(skeleton) print masks for i in range(len(masks)): print 'parents of', skeleton.getJointName(i), ':', for parent in masks[i]: print skeleton.getJointName(parent), print print
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
# 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', 'r') # mcfg = cPickle.load(mcfgfile) # mcfgfile.close() # wcfg = ypc.WorldConfig() # vpWorld = cvw.VpWorld(wcfg) # motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
def test_getStitchedNextMotion_analysis(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) # global part1 = motion[:50].copy() part2 = motion[49:].copy() R_offset_orig_global = mm.rotY(math.pi/2) part2.rotateTrajectory(R_offset_orig_global) print 'R_offset_orig_global =\n', R_offset_orig_global R_part1 = part1[-1].getJointOrientationLocal(0) R_part2 = part2[0].getJointOrientationLocal(0) print 'R_part2 =\n', np.dot(R_offset_orig_global, R_part1) R_offset_global = np.dot(part2[0].localRs[0], part1[-1].localRs[0].T) print 'R_offset_global =\n', np.dot(part2[0].localRs[0], part1[-1].localRs[0].T) part2_attached_global = part2.copy() part2_attached_global.translateByOffset((0,0,1)) part2_attached_global.rotateTrajectory(R_offset_global.T) part2_attached_global.translateByOffset((0,0,-1)) # local part1 = motion[:50].copy() part2 = motion[49:].copy() R_offset_orig_local= mm.rotY(math.pi/2) part2.rotateTrajectoryLocal(R_offset_orig_local) print 'R_offset_orig_local =\n', R_offset_orig_local R_part1 = part1[-1].getJointOrientationLocal(0) R_part2 = part2[0].getJointOrientationLocal(0) print 'R_part2 =\n', np.dot(R_part1, R_offset_orig_local) d = part2[0] - part1[-1] # np.dot(part1[-1].localRs[0].T, part2[0].localRs[0]) R_offset_local = d.getJointOrientationLocal(0) print 'R_offset_local =\n', R_offset_local part2_attached_local = part2.copy() part2_attached_local.translateByOffset((0,0,1)) part2_attached_local.rotateTrajectoryLocal(R_offset_local.T) part2_attached_local.translateByOffset((0,0,-1)) 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('part1', yr.JointMotionRenderer(part1, (255,100,255), yr.LINK_LINE)) viewer.doc.addObject('part1', part1) viewer.doc.addRenderer('part2', yr.JointMotionRenderer(part2, (100,255,255), yr.LINK_LINE)) viewer.doc.addObject('part2', part2) # viewer.doc.addRenderer('part1[-1]', yr.JointMotionRenderer(ym.JointMotion([part1[-1]]), (255,100,255), yr.LINK_LINE)) # viewer.doc.addRenderer('part2[0]', yr.JointMotionRenderer(ym.JointMotion([part2[0]]), (100,255,255), yr.LINK_LINE)) # viewer.doc.addRenderer('part2_attached_global', yr.JointMotionRenderer(part2_attached_global, (255,0,0), yr.LINK_LINE)) # viewer.doc.addObject('part2_attached_global', part2_attached_global) # viewer.doc.addRenderer('part2_attached_local', yr.JointMotionRenderer(part2_attached_local, (0,255,0), yr.LINK_LINE)) # viewer.doc.addObject('part2_attached_local', part2_attached_local) viewer.startTimer(1/30.) viewer.show() Fl.run()
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.length = 1. node.mass = .5 node = mcfg.getNode('foot_0_0') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot_0_1') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot_1_0') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot_1_1') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot_2_0') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot_2_1') node.geom = 'MyFoot4' node.mass = .5 def mcfgFix(_mcfg): for v in _mcfg.nodes.itervalues(): if len(v.geoms) == 0: v.geoms.append(v.geom) v.geomMass.append(v.mass) v.geomTs.append(None) # mcfgFix(mcfg) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 40 simulSpeedInv = 1. wcfg.timeStep = (1 / 30. * simulSpeedInv) / stepsPerFrame # parameter config = dict([]) config['Kt'] = 20 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = 1 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 5000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. config['Bh'] = 1. config['stepsPerFrame'] = stepsPerFrame config['simulSpeedInv'] = simulSpeedInv # etc config['weightMap'] = { 'root': 1., 'foot_0_0': 1., 'foot_1_0': 1., 'foot_2_0': 1., 'foot_0_1': .5, 'foot_1_1': .5, 'foot_2_1': .5 } config['weightMapTuple'] = (1., 1., .5, 1., .5, 1., .5) # config['supLink'] = 'link0' return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped(): # motion #motionName = 'wd2_n_kick.bvh' if MOTION == STAND: motionName = 'wd2_stand.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2: motionName = './MotionFile/wd2_098_V002.bvh' #motionName = 'ww13_41_V001.bvh' motion = yf.readBvhFile(motionName, .01) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) if FOOT_PART_NUM == 1 or FOOT_PART_NUM == 5: yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False) yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5, -0.0, .3), -.5), False) yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False) if MOTION == FORWARD_JUMP: yme.rotateJointLocal(motion, LEFT_UP_LEG, mm.exp(mm.v3(0.0, .0, 1.), .08), False) yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.), -.2), False) if FOOT_PART_NUM > 1 and FOOT_PART_NUM != 5: yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS) yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1') yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS) yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1') yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_TOES, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, LEFT_TARSUS, mm.exp(mm.v3(1., 0.0, 0.0), .52), False) yme.rotateJointLocal(motion, RIGHT_TARSUS, mm.exp(mm.v3(1., 0.0, 0.0), .52), False) if FOOT_PART_NUM == 4: yme.addJoint(motion, LEFT_FOOT, LEFT_TOES_SIDE_R) yme.addJoint(motion, LEFT_TOES_SIDE_R, 'LEFT_Dummy2') yme.addJoint(motion, RIGHT_FOOT, RIGHT_TOES_SIDE_R) yme.addJoint(motion, RIGHT_TOES_SIDE_R, 'RIGHT_Dummy2') yme.rotateJointLocal(motion, LEFT_TOES_SIDE_R, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_TOES_SIDE_R, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) elif FOOT_PART_NUM == 5: yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1) yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy2') yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_2) yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_Dummy3') yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1) yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy2') yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_2) yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_Dummy3') yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_2, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) #yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False) #yme.rotateJointLocal(motion, RIGHT_PHALANGE_2, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_1, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_2, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False) #yme.rotateJointLocal(motion, LEFT_PHALANGE_2, mm.exp(mm.v3(1.,0.0,0.0), .45), False) ''' elif FOOT_PART_NUM == 5 : yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_L) yme.addJoint(motion, LEFT_FOOT_SIDE_L, 'LEFT_Dummy2') yme.addJoint(motion, LEFT_FOOT, LEFT_FOOT_SIDE_R) yme.addJoint(motion, LEFT_FOOT_SIDE_R, 'LEFT_Dummy2') yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_L) yme.addJoint(motion, RIGHT_FOOT_SIDE_L, 'RIGHT_Dummy2') yme.addJoint(motion, RIGHT_FOOT, RIGHT_FOOT_SIDE_R) yme.addJoint(motion, RIGHT_FOOT_SIDE_R, 'RIGHT_Dummy2') yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_L, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, LEFT_FOOT_SIDE_R, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_L, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_FOOT_SIDE_R, mm.exp(mm.v3(1.,0.0,0.0), .45), False) ''' yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step motion = motion[0:31 + 40] ## Taekwondo turning-kick #motion = motion[108:-1] #motion = motion[108:109] motion[0:0] = [motion[0]] * 100 motion.extend([motion[-1]] * 5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### if FOOT_PART_NUM == 1: length1 = .25 width1 = .2 mass1 = 4. elif FOOT_PART_NUM == 3: length1 = .1 width1 = .2 mass1 = 1.5 length2 = .1 width2 = .2 mass2 = 1.5 elif FOOT_PART_NUM == 4: length1 = .1 width1 = .2 mass1 = 1.5 length2 = .1 width2 = .095 mass2 = .75 elif FOOT_PART_NUM == 5: length1 = .25 width1 = .2 mass1 = 4. length2 = .18 width2 = .18 mass2 = 2. length3 = .08 width3 = .065 mass3 = .24 length4 = .08 width4 = .09 mass4 = .44 length5 = .06 width5 = .09 mass5 = .36 length6 = .05 width6 = .065 mass6 = .2 ''' elif FOOT_PART_NUM == 5: length1 = .1 width1 = .065 mass1 = .5 length2 = .1 width2 = .2 mass2 = 1.5 ''' node = mcfg.getNode(RIGHT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 if FOOT_PART_NUM == 5: node = mcfg.getNode(RIGHT_FOOT) node.offset = (0, 0.0, -0.01) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_1) node.offset = (0.035, 0.01, 0.133) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_METATARSAL_2) node.offset = (-0.05, 0.0, 0.133) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_PHALANGE_1) node.offset = (0.038, 0.0, 0.20) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(RIGHT_PHALANGE_2) node.offset = (-0.038, 0.0, 0.20) node.length = length6 node.width = width6 node.mass = mass6 node = mcfg.getNode(LEFT_FOOT) node.offset = (0, 0.0, -0.01) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_1) node.offset = (-0.035, 0.01, 0.133) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_METATARSAL_2) node.offset = (0.05, 0.0, 0.133) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_PHALANGE_1) node.offset = (-0.038, 0.0, 0.20) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(LEFT_PHALANGE_2) node.offset = (0.038, 0.0, 0.20) node.length = length6 node.width = width6 node.mass = mass6 elif FOOT_PART_NUM == 4: node = mcfg.getNode(LEFT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (-0.05, 0.0, -0.03) node = mcfg.getNode(LEFT_TOES_SIDE_R) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0.08, 0.0, 0.1) node = mcfg.getNode(RIGHT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0.05, 0.0, -0.03) node = mcfg.getNode(RIGHT_TOES_SIDE_R) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (-0.05, 0.0, 0.12) elif FOOT_PART_NUM == 3: node = mcfg.getNode(LEFT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0, 0.0, -0.02) node = mcfg.getNode(RIGHT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0, 0.0, -0.02) ''' elif FOOT_PART_NUM == 5: node = mcfg.getNode(LEFT_FOOT_SIDE_L) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0.07,0.0,0.015) node = mcfg.getNode(LEFT_FOOT_SIDE_R) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (-0.07,0.0,0.015) node = mcfg.getNode(RIGHT_FOOT_SIDE_L) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0.07,0.0,0.015) node = mcfg.getNode(RIGHT_FOOT_SIDE_R) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (-0.07,0.0,0.015) ''' if FOOT_PART_NUM > 1 and FOOT_PART_NUM != 5: node = mcfg.getNode(LEFT_TARSUS) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0, 0.0, -0.08) node = mcfg.getNode(RIGHT_TARSUS) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0, 0.0, -0.08) wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. if FOOT_PART_NUM == 1: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 1., LEFT_FOOT: 1., HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1. } elif FOOT_PART_NUM == 3: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_TOES: .3, RIGHT_TOES: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_TOES: .3, RIGHT_TOES: .3 } elif FOOT_PART_NUM == 4: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_TOES: .3, RIGHT_TOES: .3, LEFT_TOES_SIDE_R: .3, RIGHT_TOES_SIDE_R: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_TOES: .3, RIGHT_TOES: .3, LEFT_TOES_SIDE_R: .3, RIGHT_TOES_SIDE_R: .3 } elif FOOT_PART_NUM == 5: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_METATARSAL_1: .2, LEFT_METATARSAL_2: .2, RIGHT_METATARSAL_1: .2, RIGHT_METATARSAL_2: .2, LEFT_PHALANGE_1: .2, LEFT_PHALANGE_2: .2, RIGHT_PHALANGE_1: .2, RIGHT_PHALANGE_2: .2 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_METATARSAL_1: .3, LEFT_METATARSAL_2: .3, RIGHT_METATARSAL_1: .3, RIGHT_METATARSAL_2: .3, LEFT_PHALANGE_1: .3, LEFT_PHALANGE_2: .3, RIGHT_PHALANGE_1: .3, RIGHT_PHALANGE_2: .3 } ''' elif FOOT_PART_NUM == 5: config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5, RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3, LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3} config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:1., SPINE1:.3, RIGHT_FOOT:2.5, LEFT_FOOT:2.5, HIP:1., RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3, LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3} ''' config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT #config['end'] = 'HIP' config['end'] = SPINE1 config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM if FOOT_PART_NUM == 5: config['FootLPart'] = [ LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2 ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2 ] else: config['FootLPart'] = [ LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R, RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R ] return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped_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' elif MOTION == STAND2: motionName = 'ww13_41_V001.bvh' elif MOTION == FORWARD_JUMP: motionName = 'woddy2_jump0.bvh' elif MOTION == TAEKWONDO: motionName = './MotionFile/wd2_098_V001.bvh' elif MOTION == TAEKWONDO2: motionName = './MotionFile/wd2_098_V002.bvh' #motionName = 'ww13_41_V001.bvh' motion = yf.readBvhFile(motionName, .01) yme.removeJoint(motion, HEAD, False) yme.removeJoint(motion, RIGHT_SHOULDER, False) yme.removeJoint(motion, LEFT_SHOULDER, False) if FOOT_PART_NUM == 1 or FOOT_PART_NUM == 5 or FOOT_PART_NUM == 7: yme.removeJoint(motion, RIGHT_TOES, False) yme.removeJoint(motion, RIGHT_TOES_END, False) yme.removeJoint(motion, LEFT_TOES, False) yme.removeJoint(motion, LEFT_TOES_END, False) yme.removeJoint(motion, RIGHT_HAND_END, False) yme.removeJoint(motion, LEFT_HAND_END, False) yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False) yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False) yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False) #yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False) #yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(2.5,-0.0,.3), -.5), False) #yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(2.5,0.0,-.3), -.5), False) if MOTION == FORWARD_JUMP: yme.rotateJointLocal(motion, LEFT_UP_LEG, mm.exp(mm.v3(0.0, .0, 1.), .08), False) yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.), -.2), False) if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5: yme.addJoint(motion, RIGHT_FOOT, RIGHT_TARSUS) yme.addJoint(motion, RIGHT_TARSUS, 'RIGHT_Dummy1') yme.addJoint(motion, LEFT_FOOT, LEFT_TARSUS) yme.addJoint(motion, LEFT_TARSUS, 'LEFT_Dummy1') yme.rotateJointLocal(motion, LEFT_TOES, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_TOES, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, LEFT_TARSUS, mm.exp(mm.v3(1., 0.0, 0.0), .52), False) yme.rotateJointLocal(motion, RIGHT_TARSUS, mm.exp(mm.v3(1., 0.0, 0.0), .52), False) if FOOT_PART_NUM == 4: yme.addJoint(motion, LEFT_FOOT, LEFT_TOES_SIDE_R) yme.addJoint(motion, LEFT_TOES_SIDE_R, 'LEFT_Dummy2') yme.addJoint(motion, RIGHT_FOOT, RIGHT_TOES_SIDE_R) yme.addJoint(motion, RIGHT_TOES_SIDE_R, 'RIGHT_Dummy2') yme.rotateJointLocal(motion, LEFT_TOES_SIDE_R, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_TOES_SIDE_R, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) elif FOOT_PART_NUM == 5: sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT) skeleton = motion[0].skeleton sibJoint = skeleton.getElement(sibIndex) newOffset = sibJoint.offset.copy() yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS, (0.0, 0., -.1)) yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3', (0.0, 0., -.1)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.1, 0., 0.1)) yme.addJoint(motion, RIGHT_FOOT, 'RIGHT_METATARSAL_Dummy3', (0.1, 0., 0.1)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1, (0., 0., .2)) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4', (0., 0., .1)) yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3, (-.15, 0., .1)) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6', (0., 0., .1)) yme.addJoint(motion, RIGHT_METATARSAL_3, 'RIGHT_METATARSAL_Dummy6', (-.2, 0., 0)) yme.addJoint(motion, RIGHT_METATARSAL_3, 'RIGHT_METATARSAL_Dummy7', (-.15, 0., -.1)) sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT) skeleton = motion[0].skeleton sibJoint = skeleton.getElement(sibIndex) newOffset = sibJoint.offset.copy() yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS, (0.0, 0., -.1)) yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3', (0.0, 0., -.1)) yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3, (0.1, 0., 0.1)) yme.addJoint(motion, LEFT_FOOT, 'LEFT_METATARSAL_Dummy3', (-0.1, 0., 0.1)) yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1, (0., 0., .2)) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4', (0., 0., .1)) yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (.15, 0., .1)) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6', (0., 0., .1)) yme.addJoint(motion, LEFT_METATARSAL_3, 'LEFT_METATARSAL_Dummy6', (.2, 0., 0)) yme.addJoint(motion, LEFT_METATARSAL_3, 'LEFT_METATARSAL_Dummy7', (.15, 0., -.1)) yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False) ''' yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS, (0.0025, -0.4209, -0.0146)) yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3') yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3, (0.0025, -0.4209, -0.0146)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4') yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6') sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT) skeleton = motion[0].skeleton sibJoint = skeleton.getElement(sibIndex) newOffset = sibJoint.offset.copy() yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS, (-0.0025, -0.4209, -0.0146)) yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3') yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3, (-0.0025, -0.4209, -0.0146)) yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4') yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6') ''' ''' yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .0), False) yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(1.,0.0,0.0), .45), False) ''' elif FOOT_PART_NUM == 7: sibIndex = motion[0].skeleton.getElementIndex(RIGHT_FOOT) skeleton = motion[0].skeleton sibJoint = skeleton.getElement(sibIndex) newOffset = sibJoint.offset.copy() print(newOffset) yme.addJoint(motion, RIGHT_LEG, RIGHT_CALCANEUS, (0.0025, -0.4209, -0.0146)) yme.addJoint(motion, RIGHT_CALCANEUS, 'RIGHT_Dummy3') yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_2, (0.0025, -0.4209, -0.0146)) yme.addJoint(motion, RIGHT_LEG, RIGHT_METATARSAL_3, (0.0025, -0.4209, -0.0146)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_PHALANGE_1) yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_Dummy4') yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2) yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_Dummy5') yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3) yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_Dummy6') sibIndex = motion[0].skeleton.getElementIndex(LEFT_FOOT) skeleton = motion[0].skeleton sibJoint = skeleton.getElement(sibIndex) newOffset = sibJoint.offset.copy() yme.addJoint(motion, LEFT_LEG, LEFT_CALCANEUS, (-0.0025, -0.4209, -0.0146)) yme.addJoint(motion, LEFT_CALCANEUS, 'LEFT_Dummy3') yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_2, (-0.0025, -0.4209, -0.0146)) yme.addJoint(motion, LEFT_LEG, LEFT_METATARSAL_3, (-0.0025, -0.4209, -0.0146)) yme.addJoint(motion, LEFT_FOOT, LEFT_PHALANGE_1) yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_Dummy4') yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2) yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_Dummy5') yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3) yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_Dummy6') #yme.offsetJointLocal(motion, RIGHT_CALCANEUS, (0.0,-1.55,0), False) #yme.offsetJointLocal(motion, RIGHT_METATARSAL_2, (.0,-1.55,0), False) #yme.offsetJointLocal(motion, RIGHT_METATARSAL_3, (.0,-1.55,0), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_2, mm.exp(mm.v3(-1., 0.0, 0.0), .3), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_2, mm.exp(mm.v3(-1., 0.0, 0.0), .3), False) yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(-1., 0.0, 0.0), .3), False) yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(-1., 0.0, 0.0), .3), False) yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1., 0.0, 0.0), .45), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS, mm.exp(mm.v3(-1., 0.0, 0.0), .20), False) yme.rotateJointLocal(motion, RIGHT_CALCANEUS, mm.exp(mm.v3(-1., 0.0, 0.0), .20), False) yme.updateGlobalT(motion) ################ if MOTION == FORWARD_JUMP: motion = motion[515:555] elif MOTION == TAEKWONDO: ## Taekwondo base-step motion = motion[0:31] #motion = motion[564:600] elif MOTION == TAEKWONDO2: ## Taekwondo base-step motion = motion[0:31 + 40] ## Taekwondo turning-kick #motion = motion[108:-1] #motion = motion[108:109] motion[0:0] = [motion[0]] * 100 motion.extend([motion[-1]] * 5000) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .9 for name in massMap: node = mcfg.addNode(name) node.mass = massMap[name] node = mcfg.getNode(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0, 0, 0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### if FOOT_PART_NUM == 1: length1 = .25 width1 = .2 mass1 = 4. elif FOOT_PART_NUM == 3: length1 = .1 width1 = .2 mass1 = 1.5 length2 = .1 width2 = .2 mass2 = 1.5 elif FOOT_PART_NUM == 4: length1 = .1 width1 = .2 mass1 = 1.5 length2 = .1 width2 = .095 mass2 = .75 elif FOOT_PART_NUM == 5: mass0 = .4 width0 = 0.028 #Metatarsal1 length1 = .18 width1 = width0 * 3 mass1 = mass0 * 2.8 #Metatarsal3 length3 = .17 width3 = width0 * 3 mass3 = mass0 * 2.7 #Calcaneus length4 = .08 width4 = 0.15 mass4 = mass0 * 2.3 #Phalange1 length5 = .06 width5 = width1 mass5 = mass0 * 0.9 #Phalange3 length7 = .05 width7 = width3 mass7 = mass0 * 0.7 elif FOOT_PART_NUM == 7: mass0 = .4 width0 = 0.028 #Metatarsal1 length1 = .2 width1 = width0 * 2 mass1 = mass0 * 2 #Metatarsal2 length2 = .2 width2 = width0 mass2 = mass0 #Metatarsal3 length3 = .19 width3 = width0 * 3 mass3 = mass0 * 2.8 #Calcaneus length4 = .08 width4 = 0.15 mass4 = mass0 * 2 #Phalange1 length5 = .06 width5 = width1 mass5 = mass0 / 2. #Phalange2 length6 = .06 width6 = width2 mass6 = mass0 / 4. #Phalange3 length7 = .05 width7 = width3 mass7 = mass6 * 2.8 if FOOT_PART_NUM == 7: node = mcfg.getNode(RIGHT_FOOT) node.offset = (0.055, -0.03, 0.04) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_METATARSAL_2) node.offset = (0.01, -0.03, 0.065) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(RIGHT_METATARSAL_3) node.offset = (-.05, -0.03, 0.055) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(RIGHT_CALCANEUS) node.offset = (.0, -0.03, -0.07) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_PHALANGE_1) node.offset = (0.055, -0.03, 0.19) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(RIGHT_PHALANGE_2) node.offset = (0.01, -0.03, 0.19) node.length = length6 node.width = width6 node.mass = mass6 node = mcfg.getNode(RIGHT_PHALANGE_3) node.offset = (-.05, -0.03, 0.18) node.length = length7 node.width = width7 node.mass = mass7 node = mcfg.getNode(LEFT_FOOT) node.offset = (-0.055, -0.03, 0.04) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_METATARSAL_2) node.offset = (-0.01, -0.03, 0.065) node.length = length2 node.width = width2 node.mass = mass2 node = mcfg.getNode(LEFT_METATARSAL_3) node.offset = (.05, -0.03, 0.055) node.length = length3 node.width = width3 node.mass = mass3 node = mcfg.getNode(LEFT_CALCANEUS) node.offset = (.0, -0.03, -0.07) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_PHALANGE_1) node.offset = (-0.055, -0.03, 0.19) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(LEFT_PHALANGE_2) node.offset = (-0.01, -0.03, 0.19) node.length = length6 node.width = width6 node.mass = mass6 node = mcfg.getNode(LEFT_PHALANGE_3) node.offset = (.05, -0.03, 0.18) node.length = length7 node.width = width7 node.mass = mass7 elif FOOT_PART_NUM == 5: node = mcfg.getNode(RIGHT_FOOT) node.offset = (0.05, -0.03, 0.06) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(RIGHT_METATARSAL_3) node.offset = (-.01, -0.03, -0.14) node.length = width3 node.width = length3 node.mass = mass3 node = mcfg.getNode(RIGHT_CALCANEUS) node.offset = (.0, -0.03, 0.1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(RIGHT_PHALANGE_1) #node.offset = (0.05,-0.03,0.19) node.offset = (0.05, -0.03, -0.02) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(RIGHT_PHALANGE_3) node.offset = (.2, -0.03, -0.025) #node.offset = (-.04,-0.03,0.18) node.length = length7 node.width = width7 node.mass = mass7 node = mcfg.getNode(LEFT_FOOT) node.offset = (-0.05, -0.03, 0.06) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_METATARSAL_3) node.offset = (.01, -0.03, -0.14) node.length = width3 node.width = length3 node.mass = mass3 node = mcfg.getNode(LEFT_CALCANEUS) node.offset = (.0, -0.03, 0.1) node.length = length4 node.width = width4 node.mass = mass4 node = mcfg.getNode(LEFT_PHALANGE_1) #node.offset = (0.05,-0.03,0.19) node.offset = (-0.05, -0.03, -0.02) node.length = length5 node.width = width5 node.mass = mass5 node = mcfg.getNode(LEFT_PHALANGE_3) node.offset = (-.2, -0.03, -0.025) #node.offset = (-.04,-0.03,0.18) node.length = length7 node.width = width7 node.mass = mass7 elif FOOT_PART_NUM == 4: node = mcfg.getNode(LEFT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (-0.05, 0.0, -0.03) node = mcfg.getNode(LEFT_TOES_SIDE_R) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0.08, 0.0, 0.1) node = mcfg.getNode(RIGHT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0.05, 0.0, -0.03) node = mcfg.getNode(RIGHT_TOES_SIDE_R) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (-0.05, 0.0, 0.12) elif FOOT_PART_NUM == 3: node = mcfg.getNode(LEFT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0, 0.0, -0.02) node = mcfg.getNode(RIGHT_TOES) node.length = length2 node.width = width2 node.mass = mass2 node.offset = (0, 0.0, -0.02) ''' elif FOOT_PART_NUM == 5: node = mcfg.getNode(LEFT_FOOT_SIDE_L) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0.07,0.0,0.015) node = mcfg.getNode(LEFT_FOOT_SIDE_R) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (-0.07,0.0,0.015) node = mcfg.getNode(RIGHT_FOOT_SIDE_L) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0.07,0.0,0.015) node = mcfg.getNode(RIGHT_FOOT_SIDE_R) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (-0.07,0.0,0.015) ''' if FOOT_PART_NUM > 1 and FOOT_PART_NUM < 5: node = mcfg.getNode(LEFT_TARSUS) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0, 0.0, -0.08) node = mcfg.getNode(RIGHT_TARSUS) node.length = length1 node.width = width1 node.mass = mass1 node.offset = (0, 0.0, -0.08) if FOOT_PART_NUM < 5: node = mcfg.getNode(RIGHT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 node = mcfg.getNode(LEFT_FOOT) node.length = length1 node.width = width1 node.mass = mass1 wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 30 wcfg.timeStep = (1 / 30.) / (stepsPerFrame) #stepsPerFrame = 10 #wcfg.timeStep = (1/120.)/(stepsPerFrame) #wcfg.timeStep = (1/1800.) # parameter config = {} config['Kt'] = 200 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = .10 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 0.1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 20000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. #0.5 config['Bh'] = 1. if FOOT_PART_NUM == 1: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 1., LEFT_FOOT: 1., HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1. } elif FOOT_PART_NUM == 3: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_TOES: .3, RIGHT_TOES: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_TOES: .3, RIGHT_TOES: .3 } elif FOOT_PART_NUM == 4: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_TOES: .3, RIGHT_TOES: .3, LEFT_TOES_SIDE_R: .3, RIGHT_TOES_SIDE_R: .3 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_TOES: .3, RIGHT_TOES: .3, LEFT_TOES_SIDE_R: .3, RIGHT_TOES_SIDE_R: .3 } elif FOOT_PART_NUM == 5: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_METATARSAL_3: .2, RIGHT_METATARSAL_3: .2, 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: 1., SPINE1: .3, RIGHT_FOOT: 2.5, LEFT_FOOT: 2.5, HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_METATARSAL_3: .3, RIGHT_METATARSAL_3: .3, RIGHT_CALCANEUS: .3, LEFT_CALCANEUS: .3, LEFT_PHALANGE_1: .3, LEFT_PHALANGE_3: .3, RIGHT_PHALANGE_1: .3, RIGHT_PHALANGE_3: .3 } elif FOOT_PART_NUM == 7: config['weightMap'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: .3, SPINE1: .3, RIGHT_FOOT: .3, LEFT_FOOT: .3, HIP: .5, RIGHT_UP_LEG: .1, RIGHT_LEG: .3, LEFT_UP_LEG: .1, LEFT_LEG: .3, LEFT_METATARSAL_2: .2, LEFT_METATARSAL_3: .2, RIGHT_METATARSAL_2: .2, RIGHT_METATARSAL_3: .2, LEFT_PHALANGE_1: .2, LEFT_PHALANGE_2: .2, LEFT_PHALANGE_3: .3, RIGHT_PHALANGE_1: .2, RIGHT_PHALANGE_2: .2, RIGHT_PHALANGE_3: .2 } config['weightMap2'] = { RIGHT_ARM: .2, RIGHT_FORE_ARM: .2, LEFT_ARM: .2, LEFT_FORE_ARM: .2, SPINE: 1., SPINE1: .3, RIGHT_FOOT: 2., LEFT_FOOT: 2., HIP: 1., RIGHT_UP_LEG: 1., RIGHT_LEG: 1., LEFT_UP_LEG: 1., LEFT_LEG: 1., LEFT_METATARSAL_2: .3, LEFT_METATARSAL_3: .3, RIGHT_METATARSAL_2: .3, RIGHT_METATARSAL_3: .3, LEFT_PHALANGE_1: .3, LEFT_PHALANGE_2: .3, LEFT_PHALANGE_3: .3, RIGHT_PHALANGE_1: .3, RIGHT_PHALANGE_2: .3, RIGHT_PHALANGE_3: .3 } ''' elif FOOT_PART_NUM == 5: config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5, RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3, LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3} config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:1., SPINE1:.3, RIGHT_FOOT:2.5, LEFT_FOOT:2.5, HIP:1., RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1., LEFT_TOES:.3, RIGHT_TOES:.3, LEFT_TARSUS:.3, RIGHT_TARSUS:.3, LEFT_FOOT_SIDE_L:.3, LEFT_FOOT_SIDE_R:.3, RIGHT_FOOT_SIDE_L:.3, RIGHT_FOOT_SIDE_R:.3} ''' config['supLink'] = LEFT_FOOT config['supLink2'] = RIGHT_FOOT #config['end'] = 'HIP' config['end'] = SPINE1 config['const'] = HIP config['root'] = HIP config['FootPartNum'] = FOOT_PART_NUM if FOOT_PART_NUM == 7: config['FootLPart'] = [ LEFT_FOOT, LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2, LEFT_PHALANGE_3, LEFT_CALCANEUS ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2, RIGHT_PHALANGE_3, RIGHT_CALCANEUS ] elif FOOT_PART_NUM == 5: config['FootLPart'] = [ LEFT_FOOT, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3, LEFT_CALCANEUS ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS ] else: config['FootLPart'] = [ LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R ] config['FootRPart'] = [ RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R, RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R ] return motion, mcfg, wcfg, stepsPerFrame, config
def test_rotateJoint_amc(): #======================================================================= # bvh #======================================================================= bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' bvh_motion = yf.readBvhFile(bvhFilePath, .01) removeJoint(bvh_motion, 'Head') removeJoint(bvh_motion, 'RightShoulder') removeJoint(bvh_motion, 'LeftShoulder1') removeJoint(bvh_motion, 'RightToes_Effector') removeJoint(bvh_motion, 'LeftToes_Effector') removeJoint(bvh_motion, 'RightHand_Effector') removeJoint(bvh_motion, 'LeftHand_Effector') offsetJointLocal(bvh_motion, 'RightArm', (.03,-.05,0)) offsetJointLocal(bvh_motion, 'LeftArm', (-.03,-.05,0)) bvh_tpose = bvh_motion[0].getTPose() bvh_tpose.translateByTarget((0,0,0)) print bvh_tpose.skeleton #======================================================================= # from amc #======================================================================= bvhFilePath = '../samples/wd2_left_turn.bvh' amc_motion = yf.readBvhFile(bvhFilePath, .01 * 2.53999905501) removeJoint(amc_motion, 'RightUpLegDummy') removeJoint(amc_motion, 'SpineDummy') removeJoint(amc_motion, 'HEadDummy') removeJoint(amc_motion, 'LeftShoulder1Dummy') removeJoint(amc_motion, 'RightShoulderDummy') removeJoint(amc_motion, 'LeftUpLegDummy') removeJoint(amc_motion, 'Head') removeJoint(amc_motion, 'RightShoulder') removeJoint(amc_motion, 'LeftShoulder1') removeJoint(amc_motion, 'RightToes_Effector') removeJoint(amc_motion, 'LeftToes_Effector') removeJoint(amc_motion, 'RightHand_Effector') removeJoint(amc_motion, 'LeftHand_Effector') offsetJointLocal(amc_motion, 'RightArm', (.03,-.05,0)) offsetJointLocal(amc_motion, 'LeftArm', (-.03,-.05,0)) amc_tpose = amc_motion[0].getTPose() amc_tpose.translateByTarget((0,0,0)) print amc_tpose.skeleton # edited amc amc_motion2 = copy.deepcopy(amc_motion) amc_tpose2 = amc_motion2[0].getTPose() amc_tpose2.translateByTarget((0,0,0)) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('bvh_tpose', yr.JointMotionRenderer(ym.JointMotion([bvh_tpose]), (255,0,0), yr.LINK_LINE)) viewer.doc.addObject('bvh_tpose', bvh_tpose) viewer.doc.addRenderer('amc_tpose', yr.JointMotionRenderer(ym.JointMotion([amc_tpose]), (255,255,0), yr.LINK_LINE)) viewer.doc.addObject('amc_tpose', amc_tpose) viewer.doc.addRenderer('amc_tpose2', yr.JointMotionRenderer(ym.JointMotion([amc_tpose2]), (0,255,255), yr.LINK_LINE)) viewer.doc.addObject('amc_tpose2', amc_tpose2) viewer.doc.addRenderer('amc_motion', yr.JointMotionRenderer(amc_motion, (0,100,255), yr.LINK_LINE)) viewer.doc.addObject('amc_motion', amc_motion) viewer.doc.addRenderer('amc_motion2', yr.JointMotionRenderer(amc_motion2, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('amc_motion2', amc_motion2) viewer.startTimer(1/30.) viewer.show() Fl.run()
import sys if '..' not in sys.path: sys.path.append('..') import Math.mmMath as mmMath import Resource.ysMotionLoader as yf import Motion.ysMotionAnalysis as yma import Motion.ysBipedAnalysis as yba import Renderer.ysRenderer as yr import GUI.ysSimpleViewer as ysv import Util.ysGlHelper as ygh import Util.ysPythonEx as ype import Util.ysMatplotEx as ymp if __name__ == "__main__": bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) LFOOT = motion[0].skeleton.getElementIndex('LeftFoot') RFOOT = motion[0].skeleton.getElementIndex('RightFoot') LTOE = motion[0].skeleton.getElementIndex('LeftToes') RTOE = motion[0].skeleton.getElementIndex('RightToes') hRef = .1 vRef = .5 clHeel = yma.getElementContactStates(motion, LFOOT, hRef, vRef) crHeel = yma.getElementContactStates(motion, RFOOT, hRef, vRef) clToe = yma.getElementContactStates(motion, LTOE, hRef, vRef) crToe = yma.getElementContactStates(motion, RTOE, hRef, vRef) clFoot = map(op.or_, clHeel, clToe) crFoot = map(op.or_, crHeel, crToe) # gaitStates = yba.getBipedGaitStates(clFoot, crFoot)
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) 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) 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[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 create_chiken_foot(): # motion motion = yf.readBvhFile('chikenFoot.bvh', 1) # world, model mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = 1. for i in range(motion[0].skeleton.getElementNum()): mcfg.addNode(motion[0].skeleton.getElementName(i)) node = mcfg.getNode('root') node.geom = 'MyFoot3' node.length = .1 # node.width = .05 node.mass = 5. node = mcfg.getNode('foot00') node.geom = 'MyFoot3' node.mass = .5 node = mcfg.getNode('foot10') node.geom = 'MyFoot3' node.mass = .5 node = mcfg.getNode('foot01') node.geom = 'MyFoot4' node.mass = .5 node = mcfg.getNode('foot11') node.geom = 'MyFoot4' node.mass = .5 wcfg = ypc.WorldConfig() wcfg.planeHeight = 0. wcfg.useDefaultContactModel = False stepsPerFrame = 120 wcfg.timeStep = (1 / 30.) / stepsPerFrame # parameter config = dict([]) config['Kt'] = 20 config['Dt'] = 2 * (config['Kt']**.5) # tracking gain config['Kl'] = 1 config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain config['Kh'] = 1 config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain config['Ks'] = 5000 config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain config['Bt'] = 1. config['Bl'] = 1. config['Bh'] = 1. # etc config['weightMap'] = { 'root': 2., 'foot00': 1., 'foot10': 1., 'foot01': .2, 'foot11': .2 } config['weightMapTuple'] = (2., 1., .2, 1., .2) # config['supLink'] = 'link0' return motion, mcfg, wcfg, stepsPerFrame, config
def test_extendByIntegration(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion_ref = yf.readBvhFile(bvhFilePath, .01) linkStyle = yr.LINK_LINE # bvhFilePath = '../samples/chain_2_rotate_expt_root.bvh' # motion_ref = yf.readBvhFile(bvhFilePath, 1.) # linkStyle = yr.LINK_WIREBOX cut = 96 motion = motion_ref[:cut] skeleton = motion[0].skeleton print(skeleton) lFoot = skeleton.getJointIndex('LeftFoot') rFoot = skeleton.getJointIndex('RightFoot') lKnee = skeleton.getJointIndex('LeftLeg') rKnee = skeleton.getJointIndex('RightLeg') lLeg = skeleton.getJointIndex('LeftUpLeg') rLeg = skeleton.getJointIndex('RightUpLeg') motion_extended_by_5_frames = copy.deepcopy(motion) motion_extended_by_5_frames.extend( extendByIntegration(motion_extended_by_5_frames, 5)) motion_extended_by_one_frame = copy.deepcopy(motion) for i in range(5): motion_extended_by_one_frame.extend( extendByIntegration(motion_extended_by_one_frame, 1)) motion_extended_preserve_foot = copy.deepcopy(motion) for i in range(5): motion_extended_preserve_foot.extend( extendByIntegration(motion_extended_preserve_foot, 1, (lFoot, rFoot))) motion_extended_preserve_leg = copy.deepcopy(motion) for i in range(5): motion_extended_preserve_leg.extend( extendByIntegration(motion_extended_preserve_leg, 1, (lFoot, rFoot, lKnee, rKnee, lLeg, rLeg))) motion_extended_preserve_knee = copy.deepcopy(motion) for i in range(5): motion_extended_preserve_knee.extend( extendByIntegration(motion_extended_preserve_knee, 1, (lFoot, rFoot))) motion_extended_ik = copy.deepcopy(motion) for i in range(5): motion_extended_ik.extend( extendByIntegrationIK( motion_extended_ik, 1, rFoot, (lFoot, rFoot, lKnee, rKnee, lLeg, rLeg))) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer( 'motion_ref', yr.JointMotionRenderer(motion_ref, (100, 100, 255), linkStyle)) viewer.doc.addObject('motion_ref', motion_ref) viewer.doc.addRenderer( 'motion', yr.JointMotionRenderer(motion, (255, 0, 0), linkStyle)) viewer.doc.addObject('motion', motion) # viewer.doc.addRenderer('motion_extended_by_5_frames', yr.JointMotionRenderer(motion_extended_by_5_frames, (0,255,0), linkStyle)) # viewer.doc.addObject('motion_extended_by_5_frames', motion_extended_by_5_frames) # viewer.doc.addRenderer('motion_extended_by_one_frame', yr.JointMotionRenderer(motion_extended_by_one_frame, (255,255,0), linkStyle)) # viewer.doc.addObject('motion_extended_by_one_frame', motion_extended_by_one_frame) # viewer.doc.addRenderer('motion_extended_preserve_foot', yr.JointMotionRenderer(motion_extended_preserve_foot, (255,0,255), linkStyle)) # viewer.doc.addObject('motion_extended_preserve_foot', motion_extended_preserve_foot) # viewer.doc.addRenderer('motion_extended_preserve_leg', yr.JointMotionRenderer(motion_extended_preserve_leg, (0,255,255), linkStyle)) # viewer.doc.addObject('motion_extended_preserve_leg', motion_extended_preserve_leg) viewer.doc.addRenderer( 'motion_extended_preserve_knee', yr.JointMotionRenderer(motion_extended_preserve_knee, (0, 255, 255), linkStyle)) viewer.doc.addObject('motion_extended_preserve_knee', motion_extended_preserve_knee) viewer.doc.addRenderer( 'motion_extended_ik', yr.JointMotionRenderer(motion_extended_ik, (0, 255, 0), linkStyle)) viewer.doc.addObject('motion_extended_ik', motion_extended_ik) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
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', 'r') mcfg = cPickle.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 test_getDesiredAngAccelerations(): # motion = yf.readBvhFile('block_3_rotate.bvh', 1) motion = yf.readBvhFile('../samples/block_tree_rotate.bvh', 1) motion = motion[0:] mcfg = ypc.ModelConfig() mcfg.defaultDensity = 1000. mcfg.defaultBoneRatio = .8 for i in range(motion[0].skeleton.getElementNum()): mcfg.addNode(motion[0].skeleton.getElementName(i)) wcfg = ypc.WorldConfig() wcfg.planeHeight = -1. wcfg.gravity = (0,0,0) stepsPerFrame = 30 wcfg.timeStep = (1/30.)/stepsPerFrame vpWorld = cvw.VpWorld(wcfg) motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg) controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg) vpWorld.initialize() controlModel.initializeHybridDynamics() controlModel.fixBody(0) p = [] ddth_des = [] viewer = ysv.SimpleViewer() # viewer.record(False) viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE)) viewer.doc.addObject('motion', motion) viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (255,240,255), yr.POLYGON_LINE)) viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)) viewer.doc.addRenderer('ddth_des', yr.VectorsRenderer(ddth_des, p, (255,0,0))) def simulateCallback(frame): th_r = motion.getInternalJointOrientationsLocal(frame) th = controlModel.getInternalJointOrientationsLocal() dth_r = motion.getInternalJointAngVelocitiesLocal(frame) dth = controlModel.getInternalJointAngVelocitiesLocal() ddth_r = motion.getInternalJointAngAccelerationsLocal(frame) ddth_des[:] = getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, 1, 1) for i in range(stepsPerFrame): controlModel.setInternalJointAngAccelerationsLocal(ddth_des) controlModel.solveHybridDynamics() vpWorld.step() motionModel.update(motion[frame]) p[:] = motion.getInternalJointPositionsGlobal(frame) viewer.setSimulateCallback(simulateCallback) viewer.startTimer(1/30.) viewer.show() Fl.run()
def push_simbicon_mass(): # Trunk 29.27 # Head 5.89 # Pelvis 16.61 # Thigh 8.35 # Shank 4.16 # Foot 1.34 # Arm 2.79 # Forearm 1.21 # Hand 0.55 class ForceInfo: def __init__(self, startFrame, duration, force): self.startFrame = startFrame # frame self.duration = duration # sec self.force = force # Newton self.targetBody = None #=============================================================================== # load motion #=============================================================================== MULTI_VIEWER = False CAMERA_TRACKING = True TORQUE_PLOT = False # global parameters Kt = 60.; Dt = 2*(Kt**.5) Ks = 4000.; Ds = 2*(Ks**.5) K_stb_vel = .1 mu = 2. # constaants c_min_contact_vel = 100. # c_min_contact_vel = 2. c_min_contact_time = .7 c_landing_duration = .2 c_taking_duration = .3 # c_swf_mid_offset = .0 c_swf_mid_offset = .02 c_swf_stability = .5 c_locking_vel = .05 # c_swf_offset = .0 c_swf_offset = .01 # c_swf_offset = .005 K_stp_pos = 0. # c5 = .5; c6 = .01 c5 = .5; c6 = .02 # c5 = .5; c6 = .05 # c5 = 1.; c6 = .05 # c5 = .0; c6 = .0 K_stb_vel = .1 K_stb_pos = .1 OLD_SWING_HEIGHT = False # OLD_SWING_HEIGHT = True HIGHER_OFFSET = True # HIGHER_OFFSET = False dir = './ppmotion/' # max push # forceInfos = [] # maximum # forceInfos = [ForceInfo(4*i*30, .4, (160,0,0)) for i in range(2,12)] # forceInfos = [ForceInfo(4*i*30, .4, (-130,0,0)) for i in range(2,12)] # forceInfos = [ForceInfo(4*i*30, .4, (0,0,80)) for i in range(2,12)] forceInfos = [ForceInfo(4*i*30+1, .4, (0,0,-105)) for i in range(2,12)] # # maximum with more checking # forceInfos = [ForceInfo(4*i*30, .4, (145,0,0)) for i in range(2,12)] # forceInfos = [ForceInfo(4*i*30+1, .4, (-120,0,0)) for i in range(2,12)] # forceInfos = [ForceInfo(4*i*30+1, .4, (0,0,80)) for i in range(2,12)] # forceInfos = [ForceInfo(4*i*30, .4, (0,0,-105)) for i in range(2,12)] # # for video # forceInfos = [ForceInfo(4*i*30+2, .4, (160,0,0)) for i in range(2,4)] \ # + [ForceInfo(4*i*30+2, .4, (0,0,-105)) for i in range(4,6)] \ # + [ForceInfo(4*i*30+2, .4, (-130,0,0)) for i in range(6,8)] \ # + [ForceInfo(4*i*30+2, .4, (0,0,80)) for i in range(8,10)] # Kt = 40.; Dt = 2*(Kt**.5) # Ks = 3000.; Ds = 2*(Ks**.5) # mu = 1. # c_swf_mid_offset = .04 # K_swp_vel_sag = .0; K_swp_vel_sag_faster = .0; # K_swp_pos_sag = 1.5; K_swp_pos_sag_faster = .1; # K_swp_vel_cor = .25; K_swp_pos_cor = .3 # K_stp_pos = 0. # K_stb_vel = .02 # K_stb_pos = .15 Kt = 40.; Dt = 2*(Kt**.5) Ks = 3000.; Ds = 2*(Ks**.5) mu = 1.5 c_swf_mid_offset = .04 K_swp_vel_sag = .05; K_swp_vel_sag_faster = .0; K_swp_pos_sag = 1.7; K_swp_pos_sag_faster = .1; K_swp_vel_cor = .25; K_swp_pos_cor = .3 # K_stb_vel = .02 # K_stb_pos = .15 filename = 'wd2_WalkSameSame01_REPEATED.bvh' motion_ori = yf.readBvhFile(dir+filename) frameTime = 1/motion_ori.fps if 'REPEATED' in filename: REPEATED = True CAMERA_TRACKING = True else: REPEATED = False #=============================================================================== # options #=============================================================================== SEGMENT_EDITING = True STANCE_FOOT_STABILIZE = True MATCH_STANCE_LEG = True SWING_FOOT_PLACEMENT = True SWING_FOOT_HEIGHT = True if '_FOOT' in filename: SWING_FOOT_ORIENTATION = True else: SWING_FOOT_ORIENTATION = False STANCE_FOOT_PUSH = True STANCE_FOOT_BALANCING = True stitch_func = lambda x : 1. - yfg.hermite2nd(x) stf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_landing_duration]) match_stl_func = yfg.hermite2nd # match_stl_func_y = yfg.hermite2nd swf_placement_func = yfg.hermite2nd swf_height_func = yfg.hermite2nd swf_height_sine_func = yfg.sine # stf_balancing_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_landing_duration]) stf_balancing_func = yfg.hermite2nd #=============================================================================== # initialize character #=============================================================================== # mcfgfile = open(dir + 'mcfg', 'r') mcfgfile = open('mcfg_simbicon', 'r') mcfg = cPickle.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 motionModel.recordVelByFiniteDiff() controlModel.initializeHybridDynamics() #=============================================================================== # load segment info #=============================================================================== skeleton = motion_ori[0].skeleton segname = os.path.splitext(filename)[0]+'.seg' segfile = open(dir+segname, 'r') seginfo = cPickle.load(segfile) segfile.close() intervals = [info['interval'] for info in seginfo] states = [info['state'] for info in seginfo] temp_motion = copy.deepcopy(motion_ori) segments = yma.splitMotionIntoSegments(temp_motion, intervals) print len(intervals), 'segments' for i in range(len(intervals)): print '%dth'%i, yba.GaitState.text[states[i]], intervals[i], ',', print motion_seg_orig = ym.JointMotion() motion_seg_orig += segments[0] motion_seg = ym.JointMotion() motion_seg += segments[0] motion_stitch = ym.JointMotion() motion_stitch += segments[0] motion_stf_stabilize = ym.JointMotion() motion_match_stl = ym.JointMotion() motion_swf_placement = ym.JointMotion() motion_swf_height = ym.JointMotion() motion_swf_orientation = ym.JointMotion() motion_stf_balancing = ym.JointMotion() motion_stf_push = ym.JointMotion() motion_control = ym.JointMotion() motion_debug1 = ym.JointMotion() motion_debug2 = ym.JointMotion() motion_debug3 = ym.JointMotion() P = ym.JointMotion() P_hat = ym.JointMotion() M_tc = ym.JointMotion() M_hat_tc_1 = ym.JointMotion() #=============================================================================== # loop variable #=============================================================================== seg_index = [0] acc_offset = [0] extended = [False] prev_R_swp = [None] stl_y_limit_num = [0] stl_xz_limit_num = [0] avg_dCM = [mm.O_Vec3()] # avg_stf_v = [mm.O_Vec3()] # avg_stf_av = [mm.O_Vec3()] # stf_push_func = [yfg.zero] step_length_cur = [0.] step_length_tar = [0.] step_axis = [mm.O_Vec3()] #=============================================================================== # information #=============================================================================== bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [mu]*len(bodyIDsToCheck) bodyMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() lID = controlModel.name2id('LeftFoot'); rID = controlModel.name2id('RightFoot') lUpLeg = skeleton.getJointIndex('LeftUpLeg');rUpLeg = skeleton.getJointIndex('RightUpLeg') lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg') lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') spine = skeleton.getJointIndex('Spine') uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']] upperMass = sum([bodyMasses[i] for i in uppers]) lLegs = [skeleton.getJointIndex(name) for name in ['LeftUpLeg', 'LeftLeg', 'LeftFoot']] rLegs = [skeleton.getJointIndex(name) for name in ['RightUpLeg', 'RightLeg', 'RightFoot']] allJoints = set(range(skeleton.getJointNum())) halfFootHeight = controlModel.getBodyShape(lFoot)[1] / 2. for fi in forceInfos: fi.targetBody = spine #=========================================================================== # data collection #=========================================================================== rhip_torques = [] rknee_torques = [] rankle_torques = [] #=============================================================================== # rendering #=============================================================================== rd_CM = [None]; rd_CP = [None]; rd_CMP = [None] rd_forces = [None]; rd_force_points = [None] rd_torques = []; rd_joint_positions = [] rd_point1 = [None] rd_point2 = [None] rd_vec1 = [None]; rd_vecori1 = [None] rd_vec2 = [None]; rd_vecori2 = [None] rd_frame1 = [None] rd_frame2 = [None] if MULTI_VIEWER: viewer = ymv.MultiViewer(800, 655, True, wheelWork=True) # viewer = ymv.MultiViewer(1600, 1255) viewer.setRenderers1([cvr.VpModelRenderer(motionModel, MOTION_COLOR, yr.POLYGON_FILL)]) viewer.setRenderers2([cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL), yr.ForcesRenderer(rd_forces, rd_force_points, (255,0,0), ratio=.01, lineWidth=.04, fromPoint=False)]) # viewer.glWindow2.groundOffset[0] -= 10 viewer.glWindow2.groundSize = 100 else: viewer = ysv.SimpleViewer() # viewer.record(False) # 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)) # viewer.doc.addObject('motion_ori', motion_ori) # viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_seg_orig', yr.JointMotionRenderer(motion_seg_orig, (0,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_seg', yr.JointMotionRenderer(motion_seg, (0,150,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stitch', yr.JointMotionRenderer(motion_stitch, (0,255,200), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stf_stabilize', yr.JointMotionRenderer(motion_stf_stabilize, (255,0,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_match_stl', yr.JointMotionRenderer(motion_match_stl, (255,200,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_swf_placement', yr.JointMotionRenderer(motion_swf_placement, (255,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_swf_height', yr.JointMotionRenderer(motion_swf_height, (50,255,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_swf_orientation', yr.JointMotionRenderer(motion_swf_orientation, (255,100,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stf_push', yr.JointMotionRenderer(motion_stf_push, (50,255,200), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stf_balancing', yr.JointMotionRenderer(motion_stf_balancing, (255,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_control', yr.JointMotionRenderer(motion_control, (255,0,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug1', yr.JointMotionRenderer(motion_debug1, (0,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug2', yr.JointMotionRenderer(motion_debug2, (255,0,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug3', yr.JointMotionRenderer(motion_debug3, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('M_tc', yr.JointMotionRenderer(M_tc, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('P_hat', yr.JointMotionRenderer(P_hat, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('P', yr.JointMotionRenderer(P, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('M_hat_tc_1', yr.JointMotionRenderer(M_hat_tc_1, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (255,0,0))) # viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,255,0))) viewer.doc.addRenderer('forces', yr.ForcesRenderer(rd_forces, rd_force_points, (255,0,0), ratio=.01, lineWidth=.04, fromPoint=False)) # viewer.doc.addRenderer('torques', yr.VectorsRenderer(rd_torques, rd_joint_positions, (255,0,0))) # viewer.doc.addRenderer('rd_point1', yr.PointsRenderer(rd_point1, (0,255,0))) # viewer.doc.addRenderer('rd_point2', yr.PointsRenderer(rd_point2, (255,0,0))) # viewer.doc.addRenderer('rd_vec1', yr.VectorsRenderer(rd_vec1, rd_vecori1, (255,0,0))) # viewer.doc.addRenderer('rd_vec2', yr.VectorsRenderer(rd_vec2, rd_vecori2, (0,255,0))) # viewer.doc.addRenderer('rd_frame1', yr.FramesRenderer(rd_frame1, (0,200,200))) # viewer.doc.addRenderer('rd_frame2', yr.FramesRenderer(rd_frame2, (200,200,0))) # viewer.setMaxFrame(len(motion_ori)-1) if not REPEATED: viewer.setMaxFrame(len(motion_ori)-1) else: viewer.setMaxFrame(1440) if CAMERA_TRACKING: if MULTI_VIEWER: cameraTargets1 = [None] * (viewer.getMaxFrame()+1) cameraTargets2 = [None] * (viewer.getMaxFrame()+1) else: cameraTargets = [None] * (viewer.getMaxFrame()+1) if TORQUE_PLOT: rhip_torques = [0.]*viewer.getMaxFrame() rknee_torques = [0.]*viewer.getMaxFrame() rankle_torques = [0.]*viewer.getMaxFrame() pt = [0.] def postFrameCallback_Always(frame): if frame==1: pt[0] = time.time() if frame==31: print 'elapsed time for 30 frames:', time.time()-pt[0] if CAMERA_TRACKING: if MULTI_VIEWER: if cameraTargets1[frame]==None: cameraTargets1[frame] = motionModel.getBodyPositionGlobal(0) # cameraTargets1[frame] = motion_ori[frame].getJointPositionGlobal(0) viewer.setCameraTarget1(cameraTargets1[frame]) if cameraTargets2[frame]==None: cameraTargets2[frame] = controlModel.getJointPositionGlobal(0) viewer.setCameraTarget2(cameraTargets2[frame]) else: if cameraTargets[frame]==None: cameraTargets[frame] = controlModel.getJointPositionGlobal(0) viewer.setCameraTarget(cameraTargets[frame]) if plot!=None: plot.updateVline(frame) viewer.setPostFrameCallback_Always(postFrameCallback_Always) plot = None # plot = ymp.InteractivePlot() if plot!=None: plot.setXlimit(0, len(motion_ori)) plot.setYlimit(0., 1.) plot.addDataSet('zero') plot.addDataSet('diff') plot.addDataSet('debug1') plot.addDataSet('debug2') def viewer_onClose(data): if plot!=None: plot.close() viewer.onClose(data) viewer.callback(viewer_onClose) def simulateCallback(frame): # seginfo segIndex = seg_index[0] curState = seginfo[segIndex]['state'] curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval']) stanceLegs = seginfo[segIndex]['stanceHips'] swingLegs = seginfo[segIndex]['swingHips'] stanceFoots = seginfo[segIndex]['stanceFoots'] swingFoots = seginfo[segIndex]['swingFoots'] swingKnees = seginfo[segIndex]['swingKnees'] groundHeight = seginfo[segIndex]['ground_height'] # maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame'] prev_frame = frame-1 if frame>0 else 0 # prev_frame = frame # information # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers) ## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass) ## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass) # stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame) # CMr_tar = CM_tar - stf_tar dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame) CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass) stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame) CMr_tar = CM_tar - stf_tar dCM = avg_dCM[0] CM = controlModel.getJointPositionGlobal(0) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) stf = controlModel.getJointPositionGlobal(stanceFoots[0]) CMr = CM - stf diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1)) diff_dCM_axis = np.cross((0,1,0), diff_dCM) rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1)) # rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar diff_CMr_axis = np.cross((0,1,0), diff_CMr) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) # direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction) # rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag) diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor) diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction) diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag) diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor) t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0]) t_raw = t if t>1.: t=1. p_root = motion_stitch[frame].getJointPositionGlobal(0) R_root = motion_stitch[frame].getJointOrientationGlobal(0) motion_seg_orig.goToFrame(frame) motion_seg.goToFrame(frame) motion_stitch.goToFrame(frame) motion_debug1.append(motion_stitch[frame].copy()) motion_debug1.goToFrame(frame) motion_debug2.append(motion_stitch[frame].copy()) motion_debug2.goToFrame(frame) motion_debug3.append(motion_stitch[frame].copy()) motion_debug3.goToFrame(frame) # paper implementation M_tc.append(motion_stitch[prev_frame]) M_tc.goToFrame(frame) P_hat.append(M_tc[frame].copy()) P_hat.goToFrame(frame) p_temp = ym.JointPosture(skeleton) p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) P.append(p_temp) P.goToFrame(frame) # stance foot stabilize motion_stf_stabilize.append(motion_stitch[frame].copy()) motion_stf_stabilize.goToFrame(frame) if STANCE_FOOT_STABILIZE: for stanceFoot in stanceFoots: R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot) R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot) motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot) # R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot) # motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # match stance leg motion_match_stl.append(motion_stf_stabilize[frame].copy()) motion_match_stl.goToFrame(frame) if MATCH_STANCE_LEG: if curState!=yba.GaitState.STOP: for i in range(len(stanceLegs)): stanceLeg = stanceLegs[i] stanceFoot = stanceFoots[i] # # motion stance leg -> character stance leg as time goes R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) R_character = controlModel.getJointOrientationGlobal(stanceLeg) motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t))) # t_y = match_stl_func_y(t) # t_xz = match_stl_func(t) # # R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) # R_character = controlModel.getJointOrientationGlobal(stanceLeg) # R = np.dot(R_character, R_motion.T) # R_y, R_xz = mm.projectRotation((0,1,0), R) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz)) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y)) # swing foot placement motion_swf_placement.append(motion_match_stl[frame].copy()) motion_swf_placement.goToFrame(frame) if SWING_FOOT_PLACEMENT: t_swing_foot_placement = swf_placement_func(t); if extended[0]: R_swp_sag = prev_R_swp[0][0] R_swp_cor = prev_R_swp[0][1] else: R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3() R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement)) if np.dot(direction, diff_CMr_sag) < 0: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement)) else: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement)) R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement)) for i in range(len(swingLegs)): swingLeg = swingLegs[i] swingFoot = swingFoots[i] # save swing foot global orientation # R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot) # rotate swing leg motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag) motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor) # restore swing foot global orientation # motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf) prev_R_swp[0] = (R_swp_sag, R_swp_cor) # swing foot height motion_swf_height.append(motion_swf_placement[frame].copy()) motion_swf_height.goToFrame(frame) if SWING_FOOT_HEIGHT: for swingFoot in swingFoots: stanceFoot = stanceFoots[0] # save foot global orientation R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot) R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot) if OLD_SWING_HEIGHT: height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1] # height_tar += c_swf_mid_offset * swf_height_sine_func(t) # motion_debug1[frame] = motion_swf_height[frame].copy() # rotate motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0)) # motion_debug2[frame] = motion_swf_height[frame].copy() # motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) if OLD_SWING_HEIGHT: height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] else: height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1] if OLD_SWING_HEIGHT: offset_height = (height_tar - height_cur) * swf_height_func(t) * c5 else: offset_height = ((height_tar - height_cur) * c5 + (d_height_tar - d_height_cur) * c6) * swf_height_func(t) offset_sine = c_swf_mid_offset * swf_height_sine_func(t) # offset_sine = 0. offset = 0. offset += offset_height offset += offset_sine if offset > 0.: newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot) newPosition[1] += offset aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition) else: if HIGHER_OFFSET: newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot) newPosition[1] -= offset aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition) # return # motion_debug3[frame] = motion_swf_height[frame].copy() # motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) motion_swf_height[frame].rotateByTarget(R_root) # restore foot global orientation motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot) motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot) if plot!=None: plot.addDataPoint('debug1', frame, offset_height) plot.addDataPoint('debug2', frame, height_tar - height_cur) # plot.addDataPoint('diff', frame, diff) # swing foot orientation motion_swf_orientation.append(motion_swf_height[frame].copy()) motion_swf_orientation.goToFrame(frame) if SWING_FOOT_ORIENTATION: swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75]) for swingFoot in swingFoots: R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot) R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t))) # swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration]) # push orientation # for swingFoot in swingFoots: # R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot) # R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) # motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t))) # stance foot push motion_stf_push.append(motion_swf_orientation[frame].copy()) motion_stf_push.goToFrame(frame) if STANCE_FOOT_PUSH: for swingFoot in swingFoots: # max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0]) # stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2]) stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2]) R_swp_sag = mm.I_SO3() # R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t))) # if step_length_cur[0] < step_length_tar[0]: # ratio = step_length_cur[0] / step_length_tar[0] # R_max = maxmaxStfPushFrame # R_zero = R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t))) motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag) # stance foot balancing motion_stf_balancing.append(motion_stf_push[frame].copy()) motion_stf_balancing.goToFrame(frame) if STANCE_FOOT_BALANCING: R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t)) R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * stf_balancing_func(t))) for stanceFoot in stanceFoots: if frame < 5: continue motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb) # control trajectory motion_control.append(motion_stf_balancing[frame].copy()) motion_control.goToFrame(frame) #======================================================================= # tracking with inverse dynamics #======================================================================= th_r = motion_control.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_control.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_control.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) #======================================================================= # simulation #======================================================================= CP = mm.v3(0.,0.,0.) F = mm.v3(0.,0.,0.) avg_dCM[0] = mm.v3(0.,0.,0.) # external force rendering info del rd_forces[:]; del rd_force_points[:] for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): rd_forces.append(fi.force) rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody) + -mm.normalize2(fi.force)*.2) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # apply external force for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() # # apply external force # for fi in forceInfos: # if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): # controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) vpWorld.step() # yvu.align2D(controlModel) if len(contactForces) > 0: CP += yrp.getCP(contactPositions, contactForces) F += sum(contactForces) avg_dCM[0] += controlModel.getJointVelocityGlobal(0) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass) # if len(stanceFoots)>0: # avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0]) # avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0]) CP /= stepsPerFrame F /= stepsPerFrame avg_dCM[0] /= stepsPerFrame # if len(stanceFoots)>0: # avg_stf_v[0] /= stepsPerFrame # avg_stf_av[0] /= stepsPerFrame # rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0. # rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0]) #======================================================================= # segment editing #======================================================================= lastFrame = False if SEGMENT_EDITING: if curState==yba.GaitState.STOP: if frame == len(motion_seg)-1: lastFrame = True elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time: swingID = lID if curState==yba.GaitState.LSWING else rID contact = False if swingID in bodyIDs: minContactVel = 1000. for i in range(len(bodyIDs)): if bodyIDs[i]==swingID: vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i]) vel[1] = 0 contactVel = mm.length(vel) if contactVel < minContactVel: minContactVel = contactVel if minContactVel < c_min_contact_vel: contact = True extended[0] = False if contact: # print frame, 'foot touch' lastFrame = True acc_offset[0] += frame - curInterval[1] elif frame == len(motion_seg)-1: print frame, 'extend frame', frame+1 preserveJoints = [] # preserveJoints = [lFoot, rFoot] # preserveJoints = [lFoot, rFoot, lKnee, rKnee] # preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg] stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee] preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]] diff = 3 motion_seg_orig.extend([motion_seg_orig[-1]]) motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff)) motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff)) # # extend for swing foot ground speed matching & swing foot height lower ## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff) ## extendedPostures = [motion_stitch[-1]] ## # extendFrameNum = frame - curInterval[1] + 1 # k = 1.-extendFrameNum/5. # if k<0.: k=0. # extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k) # ## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.: ## print frame, 'speed matching' ## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0]) ## ## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0]) ## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame) ## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame) ## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime) ## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf) ## ## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf) # # motion_stitch.extend(extendedPostures) extended[0] = True else: if frame == len(motion_seg)-1: lastFrame = True if lastFrame: if segIndex < len(segments)-1: print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval) if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01) if len(stanceFoots)>0 and len(swingFoots)>0: # step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0]) # step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0]) step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1)) step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1)) step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction) step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction) step_length_tar[0] = mm.length(step_tar_sag) if np.inner(step_tar_sag, step_cur_sag) > 0: step_length_cur[0] = mm.length(step_cur_sag) else: step_length_cur[0] = -mm.length(step_cur_sag) step_axis[0] = directionAxis # rd_vec1[0] = step_tar_sag # rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) # rd_vec2[0] = step_cur_sag # rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0]) seg_index[0] += 1 curSeg = segments[seg_index[0]] stl_y_limit_num[0] = 0 stl_xz_limit_num[0] = 0 del motion_seg_orig[frame+1:] motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False)) del motion_seg[frame+1:] del motion_stitch[frame+1:] transitionLength = len(curSeg)-1 # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False)) d = motion_seg[-1] - curSeg[0] d.rootPos[1] = 0. motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False)) d = motion_control[-1] - curSeg[0] d.rootPos[1] = 0. motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False)) # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True)) else: motion_seg_orig.append(motion_seg_orig[-1]) motion_seg.append(motion_seg[-1]) motion_stitch.append(motion_control[-1]) # rendering motionModel.update(motion_ori[frame]) # motionModel.update(motion_seg[frame]) rd_CP[0] = CP rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1]) if plot!=None: plot.addDataPoint('zero', frame, 0) plot.updatePoints() viewer.setSimulateCallback(simulateCallback) if MULTI_VIEWER: viewer.startTimer(frameTime / 1.4) else: viewer.startTimer(frameTime * .1) viewer.show() Fl.run()
def test_removeJoint(): # skeleton 1 bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) print motion[0].skeleton editedMotion = copy.deepcopy(motion) removeJoint(editedMotion, 'Head') removeJoint(editedMotion, 'RightShoulder') removeJoint(editedMotion, 'LeftShoulder1') removeJoint(editedMotion, 'RightToes_Effector') removeJoint(editedMotion, 'LeftToes_Effector') removeJoint(editedMotion, 'RightHand_Effector') removeJoint(editedMotion, 'LeftHand_Effector') offsetJointLocal(editedMotion, 'RightArm', (.03,-.05,0)) offsetJointLocal(editedMotion, 'LeftArm', (-.03,-.05,0)) print editedMotion[0].skeleton editedTPose = editedMotion[0].getTPose() editedTPose.translateByTarget((0,0,0)) # skeleton 2 bvhFilePath = '../samples/wd2_left_turn.bvh' motion = yf.readBvhFile(bvhFilePath, .01 * 2.53999905501) print motion[0].skeleton editedMotion2 = copy.deepcopy(motion) removeJoint(editedMotion2, 'RightUpLegDummy') removeJoint(editedMotion2, 'SpineDummy') removeJoint(editedMotion2, 'HEadDummy') removeJoint(editedMotion2, 'LeftShoulder1Dummy') removeJoint(editedMotion2, 'RightShoulderDummy') removeJoint(editedMotion2, 'LeftUpLegDummy') removeJoint(editedMotion2, 'Head') removeJoint(editedMotion2, 'RightShoulder') removeJoint(editedMotion2, 'LeftShoulder1') removeJoint(editedMotion2, 'RightToes_Effector') removeJoint(editedMotion2, 'LeftToes_Effector') removeJoint(editedMotion2, 'RightHand_Effector') removeJoint(editedMotion2, 'LeftHand_Effector') offsetJointLocal(editedMotion2, 'RightArm', (.03,-.05,0)) offsetJointLocal(editedMotion2, 'LeftArm', (-.03,-.05,0)) print editedMotion2[0].skeleton editedTPose2 = editedMotion2[0].getTPose() editedTPose2.translateByTarget((0,0,0)) 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('editedMotion', yr.JointMotionRenderer(editedMotion, (0,255,0), yr.LINK_LINE)) viewer.doc.addObject('editedMotion', editedMotion) viewer.doc.addRenderer('editedTPose', yr.JointMotionRenderer(ym.JointMotion([editedTPose]), (255,0,0), yr.LINK_LINE)) viewer.doc.addObject('editedTPose', editedTPose) viewer.doc.addRenderer('editedTPose2', yr.JointMotionRenderer(ym.JointMotion([editedTPose2]), (255,255,0), yr.LINK_LINE)) viewer.doc.addObject('editedTPose2', editedTPose2) viewer.startTimer(1/30.) viewer.show() Fl.run()
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 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' #motionName = 'ww13_41_V001.bvh' scale = 0.01 if MOTION == WALK : scale = 1.0 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 == 7) 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) 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 == 7: yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1, (-0.05, -0.06, 0.03)) yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3, (0.05, -0.06, 0.03)) yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.1)) 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.05, -0.06, -0.02)) yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.05, -0.06, -0.02)) yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1', (0.0, 0.0, -0.1)) yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Dummy2', (0.0, 0.0, -0.1)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1, (0.05, -0.06, 0.03)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3, (-0.05, -0.06, 0.03)) yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1, (0.0, 0.0, 0.1)) yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3, (0.0, 0.0, 0.1)) 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.05, -0.06, -0.02)) yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.05, -0.06, -0.02)) yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1', (0.0, 0.0, -0.1)) yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Dummy2', (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_CALCANEUS_2, mm.exp(mm.v3(.0,0.0,1.0), 3.14), False) yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, 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.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] 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(HIP) node.length = .2 node.width = .25 node = mcfg.getNode(SPINE1) node.length = .2 node.offset = (0,0,0.1) node = mcfg.getNode(SPINE) node.width = .22 #node.length = .2 #### if FOOT_PART_NUM == 1 : length1 = .25 width1 = .2 mass1 = 4. elif FOOT_PART_NUM == 7: mass0 = .4 width0 = 0.028 length0 = 0.1 #Metatarsal1 length1 = .15 width1 = width0*3 mass1 = mass0*2.6 #Metatarsal3 length3 = .14 width3 = width0*3 mass3 = mass0*2.5 #Calcaneus1 length4 = .08 width4 = 0.15*0.5 mass4 = mass0*1.3 #Phalange1 length5 = .06 width5 = width1 mass5 = mass0*0.9 #Phalange3 length7 = .05 width7 = width3 mass7 = mass0*0.8#mass0*0.7 #Talus length8 = .13 width8 = width0*3 mass8 = mass0*2. if FOOT_PART_NUM == 7: 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 = mcfg.getNode(RIGHT_PHALANGE_3) node.length = length7 node.width = width7 node.mass = mass7 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(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 = mcfg.getNode(LEFT_PHALANGE_3) node.length = length7 node.width = width7 node.mass = mass7 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 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 == 7: 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, RIGHT_CALCANEUS_2:.7, LEFT_CALCANEUS_2:.7, LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.7, RIGHT_PHALANGE_3:.7} config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.4, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5, RIGHT_UP_LEG:.3, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3, LEFT_METATARSAL_1:.2, RIGHT_METATARSAL_1:.2, LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_2:.2, LEFT_CALCANEUS_2:.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:.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 == 7: config['FootLPart'] = [LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2] config['FootRPart'] = [RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2] else : config['FootLPart'] = [LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R, LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R ] config['FootRPart'] = [RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R, RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R] return motion, mcfg, wcfg, stepsPerFrame, config
def 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 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 test_blendedSegment_posture_warping(): 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) frameTime = 1 / 30. firstMotion = motion0[0:75] # midMotionSmooth1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], None) # midMotionFixed1 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], .5) # midMotionFixed2 = firstMotion + getBlendedNextMotion2(motion0[75:95], motion1[120:150], firstMotion[-1], 1.) midMotionSmooth1 = firstMotion + getAttachedNextMotion( blendSegmentSmooth(motion0[75 - 1:95], motion1[120 - 1:150]), firstMotion[-1]) midMotionFixed1 = firstMotion + getAttachedNextMotion( blendSegmentFixed(motion0[75 - 1:95], motion1[120 - 1:150], .5), firstMotion[-1]) midMotionFixed2 = firstMotion + getAttachedNextMotion( blendSegmentFixed(motion0[75 - 1:95], motion1[120 - 1:150], 1.), firstMotion[-1]) midMotion = midMotionSmooth1 secondMotion = midMotion + getAttachedNextMotion( motion1[150:], midMotion[-1]) # print len(midMotionSmooth1), len(midMotionSmooth2) # print len(midMotionFixed1), len(midMotionFixed2) 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( 'firstMotion', yr.JointMotionRenderer(firstMotion, (255, 0, 0), yr.LINK_LINE)) viewer.doc.addObject('firstMotion', firstMotion) viewer.doc.addRenderer( 'midMotionSmooth1', yr.JointMotionRenderer(midMotionSmooth1, (255, 255, 0), yr.LINK_LINE)) viewer.doc.addObject('midMotionSmooth1', midMotionSmooth1) viewer.doc.addRenderer( 'midMotionFixed1', yr.JointMotionRenderer(midMotionFixed1, (255, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed1', midMotionFixed1) viewer.doc.addRenderer( 'midMotionFixed2', yr.JointMotionRenderer(midMotionFixed2, (0, 255, 255), yr.LINK_LINE)) viewer.doc.addObject('midMotionFixed2', midMotionFixed2) # viewer.doc.addRenderer('secondMotion', yr.JointMotionRenderer(secondMotion, (0,255,0), yr.LINK_LINE)) # viewer.doc.addObject('secondMotion', secondMotion) # viewer.startTimer(frameTime / 1.4) viewer.show() Fl.run()
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_getStitchedNextMotion_analysis(): bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' motion = yf.readBvhFile(bvhFilePath, .01) # global part1 = motion[:50].copy() part2 = motion[49:].copy() R_offset_orig_global = mm.rotY(math.pi / 2) part2.rotateTrajectory(R_offset_orig_global) print('R_offset_orig_global =\n', R_offset_orig_global) R_part1 = part1[-1].getJointOrientationLocal(0) R_part2 = part2[0].getJointOrientationLocal(0) print('R_part2 =\n', np.dot(R_offset_orig_global, R_part1)) R_offset_global = np.dot(part2[0].localRs[0], part1[-1].localRs[0].T) print('R_offset_global =\n', np.dot(part2[0].localRs[0], part1[-1].localRs[0].T)) part2_attached_global = part2.copy() part2_attached_global.translateByOffset((0, 0, 1)) part2_attached_global.rotateTrajectory(R_offset_global.T) part2_attached_global.translateByOffset((0, 0, -1)) # local part1 = motion[:50].copy() part2 = motion[49:].copy() R_offset_orig_local = mm.rotY(math.pi / 2) part2.rotateTrajectoryLocal(R_offset_orig_local) print('R_offset_orig_local =\n', R_offset_orig_local) R_part1 = part1[-1].getJointOrientationLocal(0) R_part2 = part2[0].getJointOrientationLocal(0) print('R_part2 =\n', np.dot(R_part1, R_offset_orig_local)) d = part2[0] - part1[ -1] # np.dot(part1[-1].localRs[0].T, part2[0].localRs[0]) R_offset_local = d.getJointOrientationLocal(0) print('R_offset_local =\n', R_offset_local) part2_attached_local = part2.copy() part2_attached_local.translateByOffset((0, 0, 1)) part2_attached_local.rotateTrajectoryLocal(R_offset_local.T) part2_attached_local.translateByOffset((0, 0, -1)) 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( 'part1', yr.JointMotionRenderer(part1, (255, 100, 255), yr.LINK_LINE)) viewer.doc.addObject('part1', part1) viewer.doc.addRenderer( 'part2', yr.JointMotionRenderer(part2, (100, 255, 255), yr.LINK_LINE)) viewer.doc.addObject('part2', part2) # viewer.doc.addRenderer('part1[-1]', yr.JointMotionRenderer(ym.JointMotion([part1[-1]]), (255,100,255), yr.LINK_LINE)) # viewer.doc.addRenderer('part2[0]', yr.JointMotionRenderer(ym.JointMotion([part2[0]]), (100,255,255), yr.LINK_LINE)) # viewer.doc.addRenderer('part2_attached_global', yr.JointMotionRenderer(part2_attached_global, (255,0,0), yr.LINK_LINE)) # viewer.doc.addObject('part2_attached_global', part2_attached_global) # viewer.doc.addRenderer('part2_attached_local', yr.JointMotionRenderer(part2_attached_local, (0,255,0), yr.LINK_LINE)) # viewer.doc.addObject('part2_attached_local', part2_attached_local) viewer.startTimer(1 / 30.) viewer.show() Fl.run()
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
if __name__ == "__main__": # mesh, js = yol.readOgreDataFiles('Data/woody2_7.mesh.xml', .01) mesh, js = yol.readOgreDataFiles('../samples/woody2_15.mesh.xml', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkSameSame00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkForwardSlow01.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkForwardFast00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkForwardVFast00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkBackward00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkAzuma00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkSoldier00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkLean00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_WalkSukiko00.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_2foot_walk_turn2.bvh', .01) # motion, f = yf.readBvhFileAsJointMotion('Data/wd2_2foot_walk_turn.bvh', .01) motion = yf.readBvhFile('../samples/wd2_WalkSameSame00.bvh', .01) meshMotion = ysu.meshAnimation2PointMotion(mesh, motion) vertexMasses = ysu.getDistributedVertexMasses(mesh, massMap) upperVertexIndices, upperMass = ysu.getUpperInfo(mesh, vertexMasses) totalMass = ysu.getTotalMass(vertexMasses) viewer = ysv.SimpleViewer() viewer.record(False) viewer.doc.addRenderer('mesh', yr.MeshRenderer(mesh)) viewer.doc.addObject('mesh', mesh) viewer.doc.addRenderer('meshMotion', yr.PointMotionRenderer(meshMotion)) viewer.doc.addObject('meshMotion', meshMotion) def preFrameCallback(frame): mesh.update(motion[frame])
def preprocess(): tasks = [] # outputDir = './ppmotion/' # # dir = '../Data/woody2/Motion/Physics2/' # 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) 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 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'
# 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', 'r') # mcfg = cPickle.load(mcfgfile)
def preprocess(): tasks = [] # outputDir = './ppmotion/' # # dir = '../Data/woody2/Motion/Physics2/' # 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) 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 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 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', 'r') mcfg = cPickle.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 save_simulation(): class ForceInfo: def __init__(self, startFrame, duration, force): self.startFrame = startFrame # frame self.duration = duration # sec self.force = force # Newton self.targetBody = None #=============================================================================== # load motion #=============================================================================== MULTI_VIEWER = True CAMERA_TRACKING = True TORQUE_PLOT = False NO_FOOT_SLIDING = True SAVE_SIMULATION = True SAVE_DIR = './saved_simulation/' # global parameters Kt = 20.; Dt = 2*(Kt**.5) Ks = 2000.; Ds = 2*(Ks**.5) K_stb_vel = .1 mu = 1. # constaants c_min_contact_vel = 100. # c_min_contact_vel = 2. c_min_contact_time = .7 c_landing_duration = .2 c_taking_duration = .3 c_swf_mid_offset = .0 c_swf_stability = .5 c_locking_vel = .05 dir = './ppmotion/' K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0. # K_stp_pos = 0. # K_stb_vel = .2 K_stp_pos = 1. filename = 'wd2_WalkSameSame01.bvh' # filename = 'wd2_WalkSameSame01_REPEATED.bvh' # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0. # K_stp_pos = 1. # filename = 'wd2_WalkForwardSlow01.bvh' ## filename = 'wd2_WalkForwardSlow01_REPEATED.bvh' # 3 frame diff ## mu = 12. # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0. # K_stp_pos = .6 # filename = 'wd2_WalkForwardNormal00.bvh' ## filename = 'wd2_WalkForwardNormal00_REPEATED.bvh' # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0. # K_stp_pos = 0. # filename = 'wd2_WalkHandWav00.bvh' ## filename = 'wd2_WalkHandWav00_REPEATED.bvh' # mu = 2. # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0. # K_stp_pos = 0. ## filename = 'wd2_WalkForwardFast00.bvh' ### filename = 'wd2_WalkForwardFast00_REPEATED.bvh' # mu = 2. # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = .3; K_swp_pos_cor = 0. # K_stp_pos = 0. # filename = 'wd2_WalkAzuma01.bvh' ## filename = 'wd2_WalkAzuma01_REPEATED.bvh' # 2 frame diff # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0. # K_stp_pos = 0. # filename = 'wd2_WalkSoldier00.bvh' # K_swp_pos_sag = .0 ## filename = 'wd2_WalkSoldier00_REPEATED.bvh' # mu = 2. # K_swp_vel_sag = .2; K_swp_vel_cor = .4; K_swp_pos_sag = .5;K_swp_pos_cor = 0. # K_stp_pos = 0. # filename = 'wd2_WalkForwardVFast00.bvh' ## filename = 'wd2_WalkForwardVFast00_REPEATED.bvh' # K_swp_vel_sag = .1; K_swp_vel_cor = .4; K_swp_pos_sag = 1.; K_swp_pos_cor = 0. # K_stp_pos = 0. # K_stb_vel = .2 # filename = 'wd2_WalkBackward00.bvh' ### filename = 'wd2_WalkBackward00_REPEATED.bvh' ## K_swp_vel_sag = .15; K_swp_vel_cor = .35; K_swp_pos_sag = 1.; K_swp_pos_cor = 0. ## K_stp_pos = 0. ### filename = 'wd2_WalkLean00.bvh' ## filename = 'wd2_WalkLean00_REPEATED.bvh' motion_ori = yf.readBvhFile(dir+filename) frameTime = 1/motion_ori.fps if 'REPEATED' in filename: REPEATED = True CAMERA_TRACKING = True else: REPEATED = False #=============================================================================== # options #=============================================================================== SEGMENT_EDITING = True STANCE_FOOT_STABILIZE = True MATCH_STANCE_LEG = True SWING_FOOT_PLACEMENT = True SWING_FOOT_HEIGHT = True SWING_FOOT_ORIENTATION = False STANCE_FOOT_PUSH = True STANCE_FOOT_BALANCING = True stitch_func = lambda x : 1. - yfg.hermite2nd(x) stf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_landing_duration]) match_stl_func = yfg.hermite2nd # match_stl_func_y = yfg.hermite2nd swf_placement_func = yfg.hermite2nd swf_height_func = yfg.hermite2nd swf_height_sine_func = yfg.sine stf_balancing_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_landing_duration]) # forceInfos = [ForceInfo(70, .4, (100,0,0))] forceInfos = [] #=============================================================================== # initialize character #=============================================================================== mcfgfile = open(dir + 'mcfg', 'r') mcfg = cPickle.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 motionModel.recordVelByFiniteDiff() controlModel.initializeHybridDynamics() #=============================================================================== # load segment info #=============================================================================== skeleton = motion_ori[0].skeleton segname = os.path.splitext(filename)[0]+'.seg' segfile = open(dir+segname, 'r') seginfo = cPickle.load(segfile) segfile.close() intervals = [info['interval'] for info in seginfo] states = [info['state'] for info in seginfo] temp_motion = copy.deepcopy(motion_ori) segments = yma.splitMotionIntoSegments(temp_motion, intervals) print len(intervals), 'segments' for i in range(len(intervals)): print '%dth'%i, yba.GaitState.text[states[i]], intervals[i], ',', print motion_seg_orig = ym.JointMotion() motion_seg_orig += segments[0] motion_seg = ym.JointMotion() motion_seg += segments[0] motion_stitch = ym.JointMotion() motion_stitch += segments[0] motion_stf_stabilize = ym.JointMotion() motion_match_stl = ym.JointMotion() motion_swf_placement = ym.JointMotion() motion_swf_height = ym.JointMotion() motion_swf_orientation = ym.JointMotion() motion_stf_balancing = ym.JointMotion() motion_stf_push = ym.JointMotion() motion_control = ym.JointMotion() motion_debug1 = ym.JointMotion() motion_debug2 = ym.JointMotion() motion_debug3 = ym.JointMotion() P = ym.JointMotion() P_hat = ym.JointMotion() M_tc = ym.JointMotion() M_hat_tc_1 = ym.JointMotion() if SAVE_SIMULATION: motion_simulation = ym.JointMotion() #=============================================================================== # loop variable #=============================================================================== seg_index = [0] acc_offset = [0] extended = [False] prev_R_swp = [None] stl_y_limit_num = [0] stl_xz_limit_num = [0] avg_dCM = [mm.O_Vec3()] # avg_stf_v = [mm.O_Vec3()] # avg_stf_av = [mm.O_Vec3()] # stf_push_func = [yfg.zero] step_length_cur = [0.] step_length_tar = [0.] step_axis = [mm.O_Vec3()] #=============================================================================== # information #=============================================================================== bodyIDsToCheck = range(vpWorld.getBodyNum()) mus = [mu]*len(bodyIDsToCheck) bodyMasses = controlModel.getBodyMasses() totalMass = controlModel.getTotalMass() lID = controlModel.name2id('LeftFoot'); rID = controlModel.name2id('RightFoot') lUpLeg = skeleton.getJointIndex('LeftUpLeg');rUpLeg = skeleton.getJointIndex('RightUpLeg') lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg') lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot') spine = skeleton.getJointIndex('Spine') uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']] upperMass = sum([bodyMasses[i] for i in uppers]) lLegs = [skeleton.getJointIndex(name) for name in ['LeftUpLeg', 'LeftLeg', 'LeftFoot']] rLegs = [skeleton.getJointIndex(name) for name in ['RightUpLeg', 'RightLeg', 'RightFoot']] allJoints = set(range(skeleton.getJointNum())) halfFootHeight = controlModel.getBodyShape(lFoot)[1] / 2. for fi in forceInfos: fi.targetBody = spine #=========================================================================== # data collection #=========================================================================== rhip_torques = [] rknee_torques = [] rankle_torques = [] #=============================================================================== # rendering #=============================================================================== rd_CM = [None]; rd_CP = [None]; rd_CMP = [None] rd_forces = [None]; rd_force_points = [None] rd_torques = []; rd_joint_positions = [] rd_point1 = [None] rd_point2 = [None] rd_vec1 = [None]; rd_vecori1 = [None] rd_vec2 = [None]; rd_vecori2 = [None] rd_frame1 = [None] rd_frame2 = [None] if MULTI_VIEWER: viewer = ymv.MultiViewer(800, 655) # viewer = ymv.MultiViewer(1600, 1255) viewer.setRenderers1([cvr.VpModelRenderer(motionModel, MOTION_COLOR, yr.POLYGON_FILL)]) viewer.setRenderers2([cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)]) else: viewer = ysv.SimpleViewer() # viewer.record(False) 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)) # viewer.doc.addObject('motion_ori', motion_ori) # viewer.doc.addRenderer('motion_ori', yr.JointMotionRenderer(motion_ori, (0,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_seg_orig', yr.JointMotionRenderer(motion_seg_orig, (0,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_seg', yr.JointMotionRenderer(motion_seg, (0,150,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stitch', yr.JointMotionRenderer(motion_stitch, (0,255,200), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stf_stabilize', yr.JointMotionRenderer(motion_stf_stabilize, (255,0,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_match_stl', yr.JointMotionRenderer(motion_match_stl, (255,200,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_swf_placement', yr.JointMotionRenderer(motion_swf_placement, (255,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_swf_height', yr.JointMotionRenderer(motion_swf_height, (50,255,255), yr.LINK_BONE)) viewer.doc.addRenderer('motion_swf_orientation', yr.JointMotionRenderer(motion_swf_orientation, (255,100,0), yr.LINK_BONE)) viewer.doc.addRenderer('motion_stf_push', yr.JointMotionRenderer(motion_stf_push, (50,255,200), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_stf_balancing', yr.JointMotionRenderer(motion_stf_balancing, (255,100,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_control', yr.JointMotionRenderer(motion_control, (255,0,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug1', yr.JointMotionRenderer(motion_debug1, (0,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug2', yr.JointMotionRenderer(motion_debug2, (255,0,255), yr.LINK_BONE)) # viewer.doc.addRenderer('motion_debug3', yr.JointMotionRenderer(motion_debug3, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('M_tc', yr.JointMotionRenderer(M_tc, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('P_hat', yr.JointMotionRenderer(P_hat, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('P', yr.JointMotionRenderer(P, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('M_hat_tc_1', yr.JointMotionRenderer(M_hat_tc_1, (255,255,0), yr.LINK_BONE)) # viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM, (255,255,0))) # viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (255,0,0))) # viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,255,0))) # viewer.doc.addRenderer('forces', yr.ForcesRenderer(rd_forces, rd_force_points, (255,0,0), ratio=.01, fromPoint=False)) # viewer.doc.addRenderer('torques', yr.VectorsRenderer(rd_torques, rd_joint_positions, (255,0,0))) # viewer.doc.addRenderer('rd_point1', yr.PointsRenderer(rd_point1, (0,255,0))) # viewer.doc.addRenderer('rd_point2', yr.PointsRenderer(rd_point2, (255,0,0))) # viewer.doc.addRenderer('rd_vec1', yr.VectorsRenderer(rd_vec1, rd_vecori1, (255,0,0))) # viewer.doc.addRenderer('rd_vec2', yr.VectorsRenderer(rd_vec2, rd_vecori2, (0,255,0))) # viewer.doc.addRenderer('rd_frame1', yr.FramesRenderer(rd_frame1, (0,200,200))) # viewer.doc.addRenderer('rd_frame2', yr.FramesRenderer(rd_frame2, (200,200,0))) # viewer.setMaxFrame(len(motion_ori)-1) if not REPEATED: viewer.setMaxFrame(len(motion_ori)-1) else: viewer.setMaxFrame(1000) if CAMERA_TRACKING: if MULTI_VIEWER: cameraTargets1 = [None] * (viewer.getMaxFrame()+1) cameraTargets2 = [None] * (viewer.getMaxFrame()+1) else: cameraTargets = [None] * (viewer.getMaxFrame()+1) if TORQUE_PLOT: rhip_torques = [0.]*viewer.getMaxFrame() rknee_torques = [0.]*viewer.getMaxFrame() rankle_torques = [0.]*viewer.getMaxFrame() # pt = [0.] def postFrameCallback_Always(frame): # if frame==1: pt[0] = time.time() # if frame==31: print 'elapsed time for 30 frames:', time.time()-pt[0] if CAMERA_TRACKING: if MULTI_VIEWER: if cameraTargets1[frame]==None: cameraTargets1[frame] = motionModel.getBodyPositionGlobal(0) # cameraTargets1[frame] = motion_ori[frame].getJointPositionGlobal(0) viewer.setCameraTarget1(cameraTargets1[frame]) if cameraTargets2[frame]==None: cameraTargets2[frame] = controlModel.getJointPositionGlobal(0) viewer.setCameraTarget2(cameraTargets2[frame]) else: if cameraTargets[frame]==None: cameraTargets[frame] = controlModel.getJointPositionGlobal(0) viewer.setCameraTarget(cameraTargets[frame]) if plot!=None: plot.updateVline(frame) viewer.setPostFrameCallback_Always(postFrameCallback_Always) plot = None # plot = ymp.InteractivePlot() if plot!=None: plot.setXlimit(0, len(motion_ori)) plot.setYlimit(0., 1.) plot.addDataSet('zero') plot.addDataSet('diff') plot.addDataSet('debug1') plot.addDataSet('debug2') def viewer_onClose(data): if plot!=None: plot.close() viewer.onClose(data) viewer.callback(viewer_onClose) def simulateCallback(frame): # seginfo segIndex = seg_index[0] curState = seginfo[segIndex]['state'] curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval']) stanceLegs = seginfo[segIndex]['stanceHips'] swingLegs = seginfo[segIndex]['swingHips'] stanceFoots = seginfo[segIndex]['stanceFoots'] swingFoots = seginfo[segIndex]['swingFoots'] swingKnees = seginfo[segIndex]['swingKnees'] groundHeight = seginfo[segIndex]['ground_height'] maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame'] prev_frame = frame-1 if frame>0 else 0 # prev_frame = frame # information # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers) ## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass) ## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass) # stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame) # CMr_tar = CM_tar - stf_tar dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame) CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers) # dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass) # CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass) stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame) CMr_tar = CM_tar - stf_tar dCM = avg_dCM[0] CM = controlModel.getJointPositionGlobal(0) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers) # CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass) stf = controlModel.getJointPositionGlobal(stanceFoots[0]) CMr = CM - stf diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1)) diff_dCM_axis = np.cross((0,1,0), diff_dCM) rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1)) # rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar diff_CMr_axis = np.cross((0,1,0), diff_CMr) direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1))) # direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1))) directionAxis = np.cross((0,1,0), direction) diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction) # rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag) diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor) diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction) diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag) diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor) t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0]) t_raw = t if t>1.: t=1. p_root = motion_stitch[frame].getJointPositionGlobal(0) R_root = motion_stitch[frame].getJointOrientationGlobal(0) motion_seg_orig.goToFrame(frame) motion_seg.goToFrame(frame) motion_stitch.goToFrame(frame) motion_debug1.append(motion_stitch[frame].copy()) motion_debug1.goToFrame(frame) motion_debug2.append(motion_stitch[frame].copy()) motion_debug2.goToFrame(frame) motion_debug3.append(motion_stitch[frame].copy()) motion_debug3.goToFrame(frame) # paper implementation M_tc.append(motion_stitch[prev_frame]) M_tc.goToFrame(frame) P_hat.append(M_tc[frame].copy()) P_hat.goToFrame(frame) p_temp = ym.JointPosture(skeleton) p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) P.append(p_temp) P.goToFrame(frame) # stance foot stabilize motion_stf_stabilize.append(motion_stitch[frame].copy()) motion_stf_stabilize.goToFrame(frame) if STANCE_FOOT_STABILIZE: for stanceFoot in stanceFoots: R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot) R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot) motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot) # R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot) # motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t))) # match stance leg motion_match_stl.append(motion_stf_stabilize[frame].copy()) motion_match_stl.goToFrame(frame) if MATCH_STANCE_LEG: if curState!=yba.GaitState.STOP: for i in range(len(stanceLegs)): stanceLeg = stanceLegs[i] stanceFoot = stanceFoots[i] # # motion stance leg -> character stance leg as time goes R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) R_character = controlModel.getJointOrientationGlobal(stanceLeg) motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t))) # t_y = match_stl_func_y(t) # t_xz = match_stl_func(t) # # R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg) # R_character = controlModel.getJointOrientationGlobal(stanceLeg) # R = np.dot(R_character, R_motion.T) # R_y, R_xz = mm.projectRotation((0,1,0), R) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz)) # motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y)) # swing foot placement motion_swf_placement.append(motion_match_stl[frame].copy()) motion_swf_placement.goToFrame(frame) if SWING_FOOT_PLACEMENT: t_swing_foot_placement = swf_placement_func(t); if extended[0]: R_swp_sag = prev_R_swp[0][0] R_swp_cor = prev_R_swp[0][1] else: R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3() R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement)) if np.dot(direction, diff_CMr_sag) < 0: R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement)) R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement)) for i in range(len(swingLegs)): swingLeg = swingLegs[i] swingFoot = swingFoots[i] # save swing foot global orientation # R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot) # rotate swing leg motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag) motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor) # restore swing foot global orientation # motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf) prev_R_swp[0] = (R_swp_sag, R_swp_cor) # swing foot height motion_swf_height.append(motion_swf_placement[frame].copy()) motion_swf_height.goToFrame(frame) if SWING_FOOT_HEIGHT: for swingFoot in swingFoots: stanceFoot = stanceFoots[0] # save foot global orientation R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot) R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot) height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] # motion_debug1[frame] = motion_swf_height[frame].copy() # rotate motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0)) # motion_debug2[frame] = motion_swf_height[frame].copy() # motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1] offset_height = (height_tar - height_cur) * swf_height_func(t) * c_swf_stability offset_sine = c_swf_mid_offset * swf_height_sine_func(t) offset = 0. offset += offset_height offset += offset_sine if offset > 0.: newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot) newPosition[1] += offset aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition) else: newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot) newPosition[1] -= offset aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition) # return # motion_debug3[frame] = motion_swf_height[frame].copy() # motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0)) motion_swf_height[frame].rotateByTarget(R_root) # restore foot global orientation motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot) motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot) if plot!=None: plot.addDataPoint('debug1', frame, height_tar) plot.addDataPoint('debug2', frame, height_cur) # plot.addDataPoint('diff', frame, diff) # swing foot orientation motion_swf_orientation.append(motion_swf_height[frame].copy()) motion_swf_orientation.goToFrame(frame) if SWING_FOOT_ORIENTATION: # swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75]) for swingFoot in swingFoots: R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot) R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t))) # swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration]) # push orientation # for swingFoot in swingFoots: # R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot) # R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot) # motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t))) # stance foot push motion_stf_push.append(motion_swf_orientation[frame].copy()) motion_stf_push.goToFrame(frame) if STANCE_FOOT_PUSH: for swingFoot in swingFoots: # max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0]) # stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2]) stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2]) R_swp_sag = mm.I_SO3() # R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t))) # if step_length_cur[0] < step_length_tar[0]: # ratio = step_length_cur[0] / step_length_tar[0] # R_max = maxmaxStfPushFrame # R_zero = R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t))) motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag) # stance foot balancing motion_stf_balancing.append(motion_stf_push[frame].copy()) motion_stf_balancing.goToFrame(frame) if STANCE_FOOT_BALANCING: R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t)) for stanceFoot in stanceFoots: if frame < 5: continue motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb) # control trajectory motion_control.append(motion_stf_balancing[frame].copy()) motion_control.goToFrame(frame) #======================================================================= # tracking with inverse dynamics #======================================================================= th_r = motion_control.getDOFPositions(frame) th = controlModel.getDOFPositions() dth_r = motion_control.getDOFVelocities(frame) dth = controlModel.getDOFVelocities() ddth_r = motion_control.getDOFAccelerations(frame) ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt) #======================================================================= # simulation #======================================================================= CP = mm.v3(0.,0.,0.) F = mm.v3(0.,0.,0.) avg_dCM[0] = mm.v3(0.,0.,0.) # external force rendering info del rd_forces[:]; del rd_force_points[:] for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): rd_forces.append(fi.force) rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody)) for i in range(stepsPerFrame): bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds) vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces) # apply external force for fi in forceInfos: if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime): controlModel.applyBodyForceGlobal(fi.targetBody, fi.force) controlModel.setDOFAccelerations(ddth_des) controlModel.solveHybridDynamics() if TORQUE_PLOT: rhip_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rUpLeg)) rknee_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rKnee)) rankle_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rFoot)) rd_torques[:] = [controlModel.getJointTorqueLocal(i)/100. for i in range(skeleton.getJointNum())] rd_joint_positions[:] = controlModel.getJointPositionsGlobal() vpWorld.step() # yvu.align2D(controlModel) if len(contactForces) > 0: CP += yrp.getCP(contactPositions, contactForces) F += sum(contactForces) avg_dCM[0] += controlModel.getJointVelocityGlobal(0) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers) # avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass) # if len(stanceFoots)>0: # avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0]) # avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0]) CP /= stepsPerFrame F /= stepsPerFrame avg_dCM[0] /= stepsPerFrame # if len(stanceFoots)>0: # avg_stf_v[0] /= stepsPerFrame # avg_stf_av[0] /= stepsPerFrame # rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0. # rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0]) #======================================================================= # segment editing #======================================================================= lastFrame = False if SEGMENT_EDITING: if curState==yba.GaitState.STOP: if frame == len(motion_seg)-1: lastFrame = True elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time: swingID = lID if curState==yba.GaitState.LSWING else rID contact = False if swingID in bodyIDs: minContactVel = 1000. for i in range(len(bodyIDs)): if bodyIDs[i]==swingID: vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i]) vel[1] = 0 contactVel = mm.length(vel) if contactVel < minContactVel: minContactVel = contactVel if minContactVel < c_min_contact_vel: contact = True extended[0] = False if contact: # print frame, 'foot touch' lastFrame = True acc_offset[0] += frame - curInterval[1] elif frame == len(motion_seg)-1: print frame, 'extend frame', frame+1 preserveJoints = [] # preserveJoints = [lFoot, rFoot] # preserveJoints = [lFoot, rFoot, lKnee, rKnee] # preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg] stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee] preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]] diff = 3 motion_seg_orig.extend([motion_seg_orig[-1]]) motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff)) motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff)) # # extend for swing foot ground speed matching & swing foot height lower ## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff) ## extendedPostures = [motion_stitch[-1]] ## # extendFrameNum = frame - curInterval[1] + 1 # k = 1.-extendFrameNum/5. # if k<0.: k=0. # extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k) # ## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.: ## print frame, 'speed matching' ## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0]) ## ## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0]) ## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame) ## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame) ## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime) ## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf) ## ## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf) # # motion_stitch.extend(extendedPostures) extended[0] = True else: if frame == len(motion_seg)-1: lastFrame = True if lastFrame: if segIndex < len(segments)-1: print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval) if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01) if len(stanceFoots)>0 and len(swingFoots)>0: # step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0]) # step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0]) step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1)) step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1)) step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction) step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction) step_length_tar[0] = mm.length(step_tar_sag) if np.inner(step_tar_sag, step_cur_sag) > 0: step_length_cur[0] = mm.length(step_cur_sag) else: step_length_cur[0] = -mm.length(step_cur_sag) step_axis[0] = directionAxis # rd_vec1[0] = step_tar_sag # rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0]) # rd_vec2[0] = step_cur_sag # rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0]) seg_index[0] += 1 curSeg = segments[seg_index[0]] stl_y_limit_num[0] = 0 stl_xz_limit_num[0] = 0 del motion_seg_orig[frame+1:] motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False)) del motion_seg[frame+1:] del motion_stitch[frame+1:] transitionLength = len(curSeg)-1 # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False)) d = motion_seg[-1] - curSeg[0] d.rootPos[1] = 0. motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False)) if NO_FOOT_SLIDING: if segIndex == len(segments)-2: Rl = motion_control[-1].getJointOrientationLocal(lUpLeg) Rr = motion_control[-1].getJointOrientationLocal(rUpLeg) Rlk = motion_control[-1].getJointOrientationLocal(lKnee) Rrk = motion_control[-1].getJointOrientationLocal(rKnee) Rlf = motion_control[-1].getJointOrientationLocal(lFoot) Rrf = motion_control[-1].getJointOrientationLocal(rFoot) for p in curSeg: p.setJointOrientationLocal(lUpLeg, Rl, False) p.setJointOrientationLocal(rUpLeg, Rr, False) p.setJointOrientationLocal(lKnee, Rlk, False) p.setJointOrientationLocal(rKnee, Rrk, False) p.setJointOrientationLocal(lFoot, Rlf, False) p.setJointOrientationLocal(rFoot, Rrf, False) p.updateGlobalT() d = motion_control[-1] - curSeg[0] d.rootPos[1] = 0. motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False)) # motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True)) # motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True)) else: motion_seg_orig.append(motion_seg_orig[-1]) motion_seg.append(motion_seg[-1]) motion_stitch.append(motion_control[-1]) # rendering motionModel.update(motion_ori[frame]) # motionModel.update(motion_seg[frame]) if SAVE_SIMULATION: p_temp = ym.JointPosture(skeleton) p_temp.initLocalRs() p_temp.rootPos = controlModel.getJointPositionGlobal(0) p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal()) motion_simulation.append(p_temp) if frame == viewer.getMaxFrame(): saveFilePath = SAVE_DIR+'simulated_'+filename yf.writeBvhFile(saveFilePath, motion_simulation) print saveFilePath, 'saved' viewer.setSimulateCallback(simulateCallback) viewer.startTimer(frameTime / 1.4) # viewer.startTimer(frameTime * .4) viewer.show() Fl.run()
def test_InteractivePlot(): import Resource.ysMotionLoader as yf import Renderer.ysRenderer as yr import GUI.ysSimpleViewer as ysv bvhFilePath = '../samples/wd2_WalkSameSame00.bvh' jointMotion = yf.readBvhFile(bvhFilePath, .01) jointMotion = jointMotion[0:200] hips = jointMotion[0].skeleton.getJointIndex('Hips') lFoot = jointMotion[0].skeleton.getJointIndex('LeftFoot') rFoot = jointMotion[0].skeleton.getJointIndex('RightFoot') plot = InteractivePlot() plot.setXlimit(0, len(jointMotion)) plot.setYlimit(0, 2) plot.addDataSet('root.y') plot.addDataSet('leftFoot.y') plot.addDataSet('rightFoot.y') viewer = ysv.SimpleViewer() viewer.doc.addRenderer( 'motion(%s)' % jointMotion.resourceName, yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE)) viewer.doc.addObject('motion(%s)' % jointMotion.resourceName, jointMotion) pt = [None] updated = [False] def preFrameCallback(frame): if updated[0] == False: plot.addDataPoint( 'root.y', frame, jointMotion[frame].getJointPositionGlobal(hips)[1]) plot.addDataPoint( 'leftFoot.y', frame, jointMotion[frame].getJointPositionGlobal(lFoot)[1]) plot.addDataPoint( 'rightFoot.y', frame, jointMotion[frame].getJointPositionGlobal(rFoot)[1]) plot.updatePoints() if frame == viewer.getMaxFrame(): updated[0] = True if frame == 0: pt[0] = time.time() if frame == 50: print(time.time() - pt[0]) def preFrameCallback_Always(frame): plot.updateVline(frame) def viewer_onClose(data): plot.close() viewer.onClose(data) viewer.setPreFrameCallback(preFrameCallback) viewer.setPreFrameCallback_Always(preFrameCallback_Always) viewer.callback(viewer_onClose) viewer.startTimer(1 / 30.) viewer.show() Fl.run()