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()
