def test_getBipedGaitStates():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
hRef = .1; vRef = .3
lc = yma.getElementContactStates(motion, motion[0].skeleton.getElementIndex('LeftFoot'), hRef, vRef)
rc = yma.getElementContactStates(motion, motion[0].skeleton.getElementIndex('RightFoot'), hRef, vRef)
rawStateList = getBipedGaitStates(lc, rc)
cookedStateList = getBipedGaitStates(lc, rc, 10, 1.)
intervals, types = yma.states2intervals(cookedStateList)
for i in range(len(intervals)):
print(intervals[i], GaitState.text[types[i]])
print()
print([yma.intIntervalUp(int) for int in intervals])
print(getWalkingSteps(lc, rc, True))
print(getBipedGaitIntervals(lc, rc, 10, 1.))
plot = ymp.SmartPlot()
plot.setXdata('frame', range(len(motion)))
plot.addYdata('rawState', rawStateList)
plot.addYdata('cookedState', cookedStateList)
plot.showModeless()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_late_foottouch():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
lFoot = motion[0].skeleton.getElementIndex('LeftFoot')
rFoot = motion[0].skeleton.getElementIndex('RightFoot')
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, lFoot, hRef, vRef)
rc = yma.getElementContactStates(motion, rFoot, hRef, vRef)
steps = yba.getWalkingSteps(lc, rc, True)
stepMotions = yma.splitMotionIntoSegments(motion, steps)
lateCycle = 2
howLate = 5
print('steps[%d] :' % lateCycle)
print(
'motion[%d]~motion[%d] => ' %
(steps[lateCycle][0], steps[lateCycle][1]), )
print('motion_latetouch[%d]~motion_earyltouch[%d]' %
(steps[lateCycle][0], steps[lateCycle][1] + howLate))
motion_latetouch = stepMotions[0]
for i in range(1, len(stepMotions)):
if i == lateCycle:
stepMotions[i].extend(
ymt.extendByIntegration(stepMotions[i], howLate))
# stitched = getStitchedNextMotion(stepMotions[i], motion_latetouch[-1], len(stepMotions[i])-1, yfg.halfsine)
stitched = getStitchedNextMotion(stepMotions[i],
motion_latetouch[-1],
len(stepMotions[i]) - 1,
yfg.hermite2nd)
# stitched = getStitchedNextMotion(stepMotions[i], motion_latetouch[-1], len(stepMotions[i])-1, yfg.identity)
motion_latetouch += stitched
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motion_latetouch',
yr.JointMotionRenderer(motion_latetouch, (0, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('motion_latetouch', motion_latetouch)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_getStitchedNextMotion():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
lFoot = motion[0].skeleton.getElementIndex('LeftFoot')
rFoot = motion[0].skeleton.getElementIndex('RightFoot')
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, lFoot, hRef, vRef)
rc = yma.getElementContactStates(motion, rFoot, hRef, vRef)
steps = yba.getWalkingSteps(lc, rc, True)
stepMotions = yma.splitMotionIntoSegments(motion, steps)
motion_stitch = stepMotions[0]
motion_getStitchedNextMotion = stepMotions[0]
print(len(motion_stitch), len(motion_getStitchedNextMotion))
for i in range(1, len(stepMotions)):
stepMotions[i] = stepMotions[i][:-5]
motion_stitch = stitch(motion_stitch, stepMotions[i], 5)
stitched = getStitchedNextMotion(stepMotions[i],
motion_getStitchedNextMotion[-1],
5)
motion_getStitchedNextMotion += stitched
print(len(motion_stitch), len(motion_getStitchedNextMotion))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (0, 0, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motion_stitch',
yr.JointMotionRenderer(motion_stitch, (0, 255, 0), yr.LINK_LINE))
viewer.doc.addObject('motion_stitch', motion_stitch)
viewer.doc.addRenderer(
'motion_getStitchedNextMotion',
yr.JointMotionRenderer(motion_getStitchedNextMotion, (255, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('motion_getStitchedNextMotion',
motion_getStitchedNextMotion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_getWalkingCycle():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
jointMotion = yf.readBvhFile(bvhFilePath, .01)
lFoot = jointMotion[0].skeleton.getElementIndex('LeftFoot')
rFoot = jointMotion[0].skeleton.getElementIndex('RightFoot')
hRef = .1; vRef = .3
lc = yma.getElementContactStates(jointMotion, lFoot, hRef, vRef)
# rc = getElementContactStates(jointMotion, rFoot, hRef, vRef)
interval = getWalkingCycle(jointMotion, lc)
cycleMotion = jointMotion[interval[0]:interval[-1]+1]
startMotion = cycleMotion[:1]
endMotion = cycleMotion[-1:]
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('jointMotion', yr.JointMotionRenderer(jointMotion, (0,0,255), yr.LINK_LINE))
viewer.doc.addObject('jointMotion', jointMotion)
viewer.doc.addRenderer('startMotion', yr.JointMotionRenderer(startMotion, (0,255,0), yr.LINK_LINE))
viewer.doc.addObject('startMotion', startMotion)
viewer.doc.addRenderer('endMotion', yr.JointMotionRenderer(endMotion, (0,255,0), yr.LINK_LINE))
viewer.doc.addObject('endMotion', endMotion)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_repeatCycle():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
jointMotion = yf.readBvhFile(bvhFilePath, .01)
# bvhFilePath = '../samples/wd2_n_kick.bvh'
# jointMotion = yf.readBvhFile(bvhFilePath, .01*2.53)
lFoot = jointMotion[0].skeleton.getElementIndex('LeftFoot')
rFoot = jointMotion[0].skeleton.getElementIndex('RightFoot')
hRef = .1
vRef = .3
lc = yma.getElementContactStates(jointMotion, lFoot, hRef, vRef)
interval = yba.getWalkingCycle(jointMotion, lc)
# interval = [60, 102]
extendedMotion = repeatCycle(jointMotion, interval, 10, 10)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'jointMotion',
yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('jointMotion', jointMotion)
viewer.doc.addRenderer(
'extendedMotion',
yr.JointMotionRenderer(extendedMotion, (0, 255, 0), yr.LINK_LINE))
viewer.doc.addObject('extendedMotion', extendedMotion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def additionalEdit(motion, path):
if 'wd2_left_turn.bvh' in path or 'wd2_right_turn.bvh' in path:
lFoot = motion[0].skeleton.getJointIndex('LeftFoot')
yte.setPositionTarget(motion, lFoot, motion[0].getJointPositionGlobal(lFoot)+(-.1,0,-.1)\
, [0,58], 150)
if 'extended' in path:
hRef = .15
vRef = .2
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
interval = yba.getWalkingCycle(motion, lc)
motion[:] = ymt.repeatCycle(motion, interval, 50, 10)
def test_getWalkingSteps():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
motion = motion[:-10]
lFoot = motion[0].skeleton.getElementIndex('LeftFoot')
rFoot = motion[0].skeleton.getElementIndex('RightFoot')
hRef = .1; vRef = .3
lc = yma.getElementContactStates(motion, lFoot, hRef, vRef)
rc = yma.getElementContactStates(motion, rFoot, hRef, vRef)
t, l_landing = yma.getTakingLandingFrames(lc)
t, r_landing = yma.getTakingLandingFrames(rc)
landing = l_landing + r_landing
landing.sort()
print('landingFrames', landing)
# steps = getWalkingSteps(lc, rc, True)
steps = getWalkingSteps(lc, rc, True, False)
# steps = getWalkingSteps(lc, rc, False)
print('steps', steps)
print()
stepMotions = yma.splitMotionIntoSegments(motion, steps)
for i in range(len(steps)):
print('stepMotions[%d]: motion[%d]~motion[%d], len %d'%(i, steps[i][0], steps[i][1], len(stepMotions[i])))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,100,255), yr.LINK_LINE, 3.))
viewer.doc.addObject('motion', motion)
for i in range(len(steps)):
viewer.doc.addRenderer('stepMotions[%d]'%i, yr.JointMotionRenderer(stepMotions[i], (0,255,0), yr.LINK_LINE, 3.))
viewer.doc.addObject('stepMotions[%d]', stepMotions[i])
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_blend():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion0 = yf.readBvhFile(bvhFilePath, .01)
# bvhFilePath = '../samples/wd2_WalkSukiko00.bvh'
bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh'
motion1 = yf.readBvhFile(bvhFilePath, .01)
hRef = .1
vRef = .3
RHEEL = motion0[0].skeleton.getElementIndex('RightFoot')
rc0 = yma.getElementContactStates(motion0, RHEEL, hRef, vRef)
rc1 = yma.getElementContactStates(motion1, RHEEL, hRef, vRef)
intervals0 = yma.states2intervals(rc0)[0]
intervals1 = yma.states2intervals(rc1)[0]
blendedMotion = blend(motion0, yma.intIntervalInner(intervals0[3]),
motion1, yma.intIntervalInner(intervals1[3]))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion0',
yr.JointMotionRenderer(motion0, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('motion0', motion0)
viewer.doc.addRenderer(
'motion1',
yr.JointMotionRenderer(motion1, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('motion1', motion1)
viewer.doc.addRenderer(
'blendedMotion',
yr.JointMotionRenderer(blendedMotion, (0, 255, 0), yr.LINK_LINE))
viewer.doc.addObject('blendedMotion', blendedMotion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def preprocess(SEGMENT_FOOT=False):
tasks = []
outputDir = './ppmotion/'
dir = '../Data/woody2/Motion/Physics2/'
config = None
if SEGMENT_FOOT:
# segmented foot
config = {
'repeat': True,
'footRot': mm.rotX(.07),
'yOffset': 0.,
'halfFootHeight': 0.0944444444444,
'scale': .01,
'type': 'woody2'
}
else:
# box foot
config = {
'repeat': True,
'footRot': mm.rotX(-.4),
'yOffset': 0.,
'halfFootHeight': 0.0444444444444,
'scale': .01,
'type': 'woody2'
}
paths = []
paths.append(dir + 'wd2_WalkSameSame01.bvh')
paths.append(dir + 'wd2_WalkForwardSlow01.bvh')
paths.append(dir + 'wd2_WalkForwardNormal00.bvh')
paths.append(dir + 'wd2_WalkHandWav00.bvh')
paths.append(dir + 'wd2_WalkForwardFast00.bvh')
paths.append(dir + 'wd2_WalkForwardVFast00.bvh')
paths.append(dir + 'wd2_WalkLean00.bvh')
paths.append(dir + 'wd2_WalkAzuma01.bvh')
paths.append(dir + 'wd2_WalkSoldier00.bvh')
paths.append(dir + 'wd2_WalkSukiko00.bvh')
# paths.append(dir+'wd2_WalkBackward00.bvh')
paths.append(dir + 'wd2_WalkTongTong00.bvh')
tasks.append({'config': config, 'paths': paths})
##
# dir = '../Data/woody2/Motion/Balancing/'
# config = {'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_2foot_walk_turn.bvh')
# paths.append(dir+'wd2_2foot_walk_turn2.bvh')
# paths.append(dir+'wd2_slow_2foot_hop.bvh')
# paths.append(dir+'wd2_short_broad_jump.bvh')
# paths.append(dir+'wd2_long_broad_jump.bvh')
# paths.append(dir+'wd2_ffast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_cancan_run.bvh')
# paths.append(dir+'wd2_fast_2foot_hop.bvh')
# paths.append(dir+'wd2_1foot_contact_run.bvh')
# paths.append(dir+'wd2_1foot_contact_run2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/Samsung/'
## config = {'footRot': mm.rotX(-.46), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = []
## paths.append(dir+'wd2_left_turn.bvh')
## paths.append(dir+'wd2_right_turn.bvh')
## paths.append(dir+'wd2_pose_inner1.bvh')
## paths.append(dir+'wd2_pose_inner2.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Picking/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01, 'type':'woody2'}
# paths = []
# paths.append(dir+'wd2_pick_walk_1.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/VideoMotion/'
# config = {'footRot': mm.rotX(-.48), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
# paths = []
# paths.append(dir+'wd2_cross_walk_90d_fast_27.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/Walking/'
# config = {'footRot': mm.rotX(-.40), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/round/'
## config = {'footRot': mm.rotX(-.65), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## dir = '../Data/woody2/Motion/samsung_boxing/boxman/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
## paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
# dir = '../Data/woody2/Motion/motion_edit/'
# config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':.01*2.54, 'type':'woody2amc'}
# paths = glob.glob(dir+'*.bvh')
## tasks.append({'config':config, 'paths':paths})
#
## outputDir = './icmotion_test/'
## dir = './rawmotion/'
## config = {'footRot': mm.rotX(-.5), 'yOffset': .0, 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
## paths = glob.glob(dir+'*.temp')
## tasks.append({'config':config, 'paths':paths})
#
# outputDir = './icmotion_last/'
# dir = './lastmotion/add/'
## config = {'rootRot':mm.rotZ(.05), 'footRot': mm.rotX(-.5), 'leftFootR':mm.rotZ(-.1), \
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.5), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.temp')
# tasks.append({'config':config, 'paths':paths})
# outputDir = './ppmotion_slope/'
# dir = './rawmotion_slope_extracted/'
# config = {'rootRot':mm.rotZ(.0), 'footRot': np.dot(mm.rotX(-.55), mm.rotZ(.04)), 'leftFootR':mm.rotZ(-.2), \
# 'halfFootHeight': 0.0444444444444, 'scale':1., 'type':'woody2_new'}
# paths = glob.glob(dir+'*.bvh')
# tasks.append({'config':config, 'paths':paths})
VISUALIZE = False
for task in tasks:
config = task['config']
paths = task['paths']
for path in paths:
motion = yf.readBvhFile(path)
normalizeSkeleton(motion, config)
adjustHeight(motion, config['halfFootHeight'])
additionalEdit(motion, path)
outputPath = outputDir + os.path.basename(path)
if SEGMENT_FOOT:
outputPath = outputDir + "segfoot_" + os.path.basename(path)
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if 'repeat' in config and config['repeat']:
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef,
vRef)
interval = yba.getWalkingCycle2(motion, lc)
transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10
motion = ymt.repeatCycle(motion, interval, 50,
transitionLength)
outputName = os.path.splitext(
os.path.basename(path))[0] + '_REPEATED.bvh'
outputPath = outputDir + outputName
if SEGMENT_FOOT:
outputPath = outputDir + 'segfoot_' + outputName
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if VISUALIZE:
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 100, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
print('FINISHED')
def preprocess(SEGMENT_FOOT=False):
# dir = './icmotion_test/'
# paths = glob.glob(dir+'*.temp')
dir = './ppmotion/'
# paths = glob.glob(dir+'*.bvh')
paths = None
if SEGMENT_FOOT:
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'segfoot_wd2_WalkForwardNormal00_REPEATED.bvh')
else:
paths = glob.glob(dir+'wd2_WalkForwardNormal00.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal00_REPEATED.bvh')
# paths = glob.glob(dir+'wd2_WalkForwardVFast00.bvh')
'''
paths = glob.glob(dir+'wd2_WalkSameSame01.bvh')
paths = glob.glob(dir+'wd2_u-turn_1.bvh')
paths = glob.glob(dir+'wd2_cross_walk*.bvh')
paths = glob.glob(dir+'*_REPEATED.bvh')
paths = glob.glob(dir+'wd2_pick_walk_1.bvh')
paths = glob.glob(dir+'wd2_WalkSameSame01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardSlow01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkForwardNormal01_REPEATED_FOOT.bvh')
paths = glob.glob(dir+'wd2_WalkSoldier00_REPEATED_FOOT.bvh')
'''
hRef = .1; vRef = .4
## hRef = .1; vRef = .2
# dir = './ppmotion_long/'
# paths = glob.glob(dir+'wd2_WalkBackward00_REPEATED.bvh')
# hRef = 10.; vRef = .4
# dir = './rawmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
## paths = [dir+'woddy2_walk_normal_to_slope.bvh']
# hRef = 10000.; vRef = .4*100
#
# dir = './ppmotion_slope/'
# paths = glob.glob(dir+'*.bvh')
# hRef = 10000.; vRef = .2
jumpThreshold = 15; jumpBias = 1.
stopThreshold = 15; stopBias = 0.
for path in paths:
motion_ori = yf.readBvhFile(path)
# informations
skeleton = motion_ori[0].skeleton
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
lHip = skeleton.getJointIndex('LeftUpLeg'); rHip = skeleton.getJointIndex('RightUpLeg')
lKnee = skeleton.getJointIndex('LeftLeg'); rKnee = skeleton.getJointIndex('RightLeg')
lFoot = skeleton.getJointIndex('LeftFoot'); rFoot = skeleton.getJointIndex('RightFoot')
mcfgfile = open(dir + 'mcfg', 'rb')
mcfg = pickle.load(mcfgfile)
mcfgfile.close()
wcfg = ypc.WorldConfig()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion_ori[0], mcfg)
# bodyMasses = getBodyMasses()
bodyMasses = motionModel.getBodyMasses()
uppers = [skeleton.getJointIndex(name) for name in ['Hips', 'Spine', 'Spine1', 'LeftArm', 'LeftForeArm', 'RightArm', 'RightForeArm']]
upperMass = sum([bodyMasses[i] for i in uppers])
lc = yma.getElementContactStates(motion_ori, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion_ori, 'RightFoot', hRef, vRef)
# intervals, states = yba.getBipedGaitIntervals(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
intervals, states = yba.getBipedGaitIntervals2(lc, rc, jumpThreshold, jumpBias, stopThreshold, stopBias)
seginfos = [{} for i in range(len(intervals))]
for i in range(len(intervals)):
start = intervals[i][0]; end = intervals[i][1]
seginfos[i]['interval'] = intervals[i]
seginfos[i]['state'] = states[i]
# print yba.GaitState.text[states[i]], intervals[i]
stanceHips = []; swingHips = []; stanceFoots = []; swingFoots = []; swingKnees = []
if states[i]==yba.GaitState.LSWING: stanceHips = [rHip]; stanceFoots = [rFoot]; swingHips = [lHip]; swingFoots = [lFoot]; swingKnees = [lKnee]
elif states[i]==yba.GaitState.RSWING: stanceHips = [lHip]; stanceFoots = [lFoot]; swingHips = [rHip]; swingFoots = [rFoot]; swingKnees = [rKnee]
elif states[i]==yba.GaitState.STOP: stanceHips = [rHip, lHip]; stanceFoots = [rFoot, lFoot]
elif states[i]==yba.GaitState.JUMP: swingHips = [rHip, lHip]; swingFoots = [rFoot, lFoot]
seginfos[i]['stanceHips'] = stanceHips
seginfos[i]['swingHips'] = swingHips
seginfos[i]['stanceFoots'] = stanceFoots
seginfos[i]['swingFoots'] = swingFoots
seginfos[i]['swingKnees'] = swingKnees
if start<end:
if SEGMENT_FOOT:
# segmented foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
else:
# box foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
seginfos[i]['max_stf_push_frame'] = None
if len(swingFoots) > 0:
pushes = []
for frame in range(start, int((start+end)//2) + 1):
dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis))
seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes))
# write .seg
inputName = os.path.basename(path)
root = os.path.splitext(inputName)[0]
outputName = root+'.seg'
outputFile = open(dir+outputName, 'wb')
pickle.dump(seginfos, outputFile)
outputFile.close()
print(outputName, 'done')
pprint.pprint(seginfos)
print('FINISHED')
def test_getStitchedNextMotion2():
bvhFilePath = '../samples/walk_left_90degree.bvh'
motion = yf.readBvhFile(bvhFilePath)
lFoot = motion[0].skeleton.getElementIndex('LeftFoot')
rFoot = motion[0].skeleton.getElementIndex('RightFoot')
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, lFoot, hRef, vRef)
rc = yma.getElementContactStates(motion, rFoot, hRef, vRef)
steps = yba.getWalkingSteps(lc, rc, True)
stepMotions = yma.splitMotionIntoSegments(motion, steps)
print(steps)
earlyCycle = 3
howEarly = 10
transitionFunc = lambda x: 1. - yfg.hermite2nd(x)
print('steps[%d] :' % earlyCycle)
print(
'motion[%d]~motion[%d] => ' %
(steps[earlyCycle][0], steps[earlyCycle][1]), )
print('motion_stitched_trans_rot[%d]~motion_stitched_trans_rot[%d]' %
(steps[earlyCycle][0], steps[earlyCycle][1] - howEarly))
motion_stitched_trans = stepMotions[0].copy()
motion_stitched_trans_rot = stepMotions[0].copy()
motion_stitched_d = stepMotions[0].copy()
motion_attached = stepMotions[0].copy()
for i in range(1, len(stepMotions)):
if i == earlyCycle:
stepMotions[i] = stepMotions[i][:-howEarly]
# motion_stitched_trans += getStitchedNextMotion(stepMotions[i], motion_stitched_trans[-1], len(stepMotions[i])-1, transitionFunc, True, False)
motion_stitched_trans_rot += getStitchedNextMotion(
stepMotions[i].copy(), motion_stitched_trans_rot[-1],
len(stepMotions[i]) - 1, transitionFunc, True, True)
# d = motion_stitched_d[-1] - stepMotions[i][0]
# motion_stitched_d += getStitchedNextMotion(stepMotions[i], d, len(stepMotions[i])-1, transitionFunc, True, True)
# motion_attached += getStitchedNextMotion(stepMotions[i], motion_attached[-1], len(stepMotions[i])-1, yfg.zero, True, True)
motion_attached += getAttachedNextMotion(stepMotions[i],
motion_attached[-1],
len(stepMotions[i]) - 1,
yfg.zero, True, True)
frame0 = [mm.I_SE3()]
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (100,100,255), yr.LINK_LINE))
# viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motion_stitched_trans', yr.JointMotionRenderer(motion_stitched_trans, (255,255,0), yr.LINK_LINE))
# viewer.doc.addObject('motion_stitched_trans', motion_stitched_trans)
viewer.doc.addRenderer(
'motion_stitched_trans_rot',
yr.JointMotionRenderer(motion_stitched_trans_rot, (0, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('motion_stitched_trans_rot',
motion_stitched_trans_rot)
viewer.doc.addRenderer(
'motion_attached',
yr.JointMotionRenderer(motion_attached, (255, 255, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion_attached', motion_attached)
viewer.doc.addRenderer('frame0',
yr.FramesRenderer(frame0, (255, 255, 0)))
def simulateCallback(frame):
frame0[0] = motion_stitched_trans_rot[frame].getJointFrame(0)
# frame0[0] = motion_attached[frame].getJointFrame(0)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer((1 / 30.) / 1.4)
viewer.show()
Fl.run()
def test_stitch():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion0 = yf.readBvhFile(bvhFilePath, .01)
bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh'
motion1 = yf.readBvhFile(bvhFilePath, .01)
hRef = .1
vRef = .3
LHEEL = motion0[0].skeleton.getElementIndex('LeftFoot')
RHEEL = motion0[0].skeleton.getElementIndex('RightFoot')
lc0 = yma.getElementContactStates(motion0, LHEEL, hRef, vRef)
rc0 = yma.getElementContactStates(motion0, RHEEL, hRef, vRef)
lc1 = yma.getElementContactStates(motion1, LHEEL, hRef, vRef)
rc1 = yma.getElementContactStates(motion1, RHEEL, hRef, vRef)
intervals0, states0 = yba.getBipedGaitIntervals(lc0, rc0, 10, .1)
intervals1, states1 = yba.getBipedGaitIntervals(lc1, rc1, 10, .1)
print('wd2_WalkSameSame00')
print(intervals0)
print([yba.GaitState.text[state] for state in states0])
print('wd2_WalkForwardVFast00')
print(intervals1)
print([yba.GaitState.text[state] for state in states1])
print()
seg = 3
tlen = 10
print('motion0[:%d] + motion1[%d:]' %
(intervals0[seg][1], intervals1[seg][1]))
print('transition length', tlen)
print('stitched_0changed',
'%d~%d' % (intervals0[seg][1] - tlen, intervals0[seg][1]))
print('stitched_1changed',
'%d~%d' % (intervals0[seg][1], intervals0[seg][1] + tlen))
print()
just_added = motion0[:intervals0[seg][1]] + motion1[intervals1[seg][1]:]
print('len(just_added)', len(just_added))
stitched_0changed = stitch(motion0[:intervals0[seg][1]],
motion1[intervals1[seg][1] - 1:], tlen,
yfg.identity, True)
print('len(stitched_0changed)', len(stitched_0changed))
stitched_1changed = stitch(motion0[:intervals0[seg][1]],
motion1[intervals1[seg][1] - 1:], tlen,
yfg.identity, False)
print('len(stitched_1changed)', len(stitched_1changed))
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'just_added',
yr.JointMotionRenderer(just_added, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('just_added', just_added)
viewer.doc.addRenderer(
'stitched_0changed',
yr.JointMotionRenderer(stitched_0changed, (0, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('stitched_0changed', stitched_0changed)
viewer.doc.addRenderer(
'stitched_1changed',
yr.JointMotionRenderer(stitched_1changed, (255, 255, 0),
yr.LINK_LINE))
viewer.doc.addObject('stitched_1changed', stitched_1changed)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main2():
import PyCommon.modules.Motion.ysMotionAnalysis as yma
pydart.init()
world = [
pydart.World(1. / 1200., "../DartDeep/data/woody_with_ground_v2.xml")
for _ in range(2)
]
motion_files = ['wd2_jump0.bvh']
motion_files = ['woody_walk_normal.bvh']
motion_files = ['walk_left_90degree.bvh']
motion = yf.readBvhFileAsBvh(motion_files[0]).toPmLinearMotion(1., False)
# 70, 112, 156 right heel strike
mg = hmg.MotionGraph()
mg.add_motion(motion)
mg.build()
total_num = len(mg.transition_processed)
# viewer settings
rd_contact_positions = [None]
rd_contact_forces = [None]
viewer = hsv.hpSimpleViewer(rect=(0, 0, 1200, 800), viewForceWnd=False)
viewer.doc.addRenderer('motionModel[0]',
yr.DartRenderer(world[0], (255, 240, 255)))
viewer.doc.addRenderer('motionModel[1]', yr.DartRenderer(world[1]))
motion_state = [0]
def callback_0(_):
motion_state[0] = 0
def callback_1(_):
motion_state[0] = 1
def callback_2(_):
motion_state[0] = 2
viewer.objectInfoWnd.addBtn('0', callback_0)
viewer.objectInfoWnd.addBtn('1', callback_1)
viewer.objectInfoWnd.addBtn('2', callback_2)
hRef = 0.1
vRef = 0.4
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef, vRef)
rc = yma.getElementContactStates(motion, 'RightFoot', hRef, vRef)
lindex = motion[0].skeleton.getElementIndex('LeftFoot')
rindex = motion[0].skeleton.getElementIndex('RightFoot')
def postCallback(frame):
transition = mg.transition_processed[frame]
world[0].skeletons[1].set_positions(
motion.get_q(transition.motion_from_idx))
world[1].skeletons[1].set_positions(
motion.get_q(transition.motion_to_idx))
print(frame, transition.motion_from_idx, transition.motion_to_idx,
transition.dist)
print(motion.getPosition(lindex))
print(lc[transition.motion_from_idx], rc[transition.motion_from_idx])
print(lc[transition.motion_to_idx], rc[transition.motion_to_idx])
viewer.setPreFrameCallback_Always(postCallback)
viewer.setMaxFrame(total_num - 1)
viewer.startTimer(1. / 30.)
viewer.show()
all_node_set = set()
dot = Digraph()
for i in range(len(mg.transition_processed)):
all_node_set.add(mg.transition_processed[i].motion_from_idx)
all_node_set.add(mg.transition_processed[i].motion_to_idx)
all_node_list = list(all_node_set)
all_node_list.sort()
for i in all_node_list:
dot.node(str(i))
all_edge_set = set()
for i in range(len(all_node_list) - 1):
all_edge_set.add((all_node_list[i], all_node_list[i + 1]))
for i in range(len(mg.transition_processed)):
all_edge_set.add((mg.transition_processed[i].motion_from_idx,
mg.transition_processed[i].motion_to_idx))
for i in list(all_edge_set):
dot.edge(str(i[0]), str(i[1]))
# print(dot.source)
dot.render('test', view=True)
dot.pipe()
Fl.run()
def preprocess(SEGMENT_FOOT=False):
tasks = []
outputDir = './ppmotion/'
if SEGMENT_FOOT:
outputDir = './ppmotion_segfoot/'
dir = './ori_motion/'
config = None
if SEGMENT_FOOT:
# segmented foot
# config = {'repeat':False, 'footRot': mm.rotX(.07), 'yOffset':0., 'halfFootHeight': 0.0944444444444, 'scale':.01, 'type':'woody2'}
config = {
'repeat': False,
'footRot': mm.rotX(-.07),
'yOffset': 0.,
'halfFootHeight': 0.0944444444444,
'scale': .01,
'type': 'woody2'
}
else:
# box foot
config = {
'repeat': False,
'footRot': mm.rotX(-.4),
'yOffset': 0.,
'halfFootHeight': 0.0444444444444,
'scale': .01,
'type': 'woody2'
}
paths = []
# rotX = -0.07
paths.append(dir + 'wd2_1foot_contact_run2_edit.bvh')
paths.append(dir + 'wd2_fast_2foot_hop_edit.bvh')
paths.append(dir + 'wd2_long_broad_jump_edit.bvh')
paths.append(dir + 'wd2_n_kick_edit.bvh')
paths.append(dir + 'wd2_short_broad_jump_edit.bvh')
paths.append(dir + 'wd2_slow_2foot_hop_edit.bvh')
# rotX = 0.07
# paths.append(dir+'wd2_boxing_round_round_girl1_edit.bvh')
# paths.append(dir+'wd2_u-turn_edit.bvh')
tasks.append({'config': config, 'paths': paths})
VISUALIZE = False
for task in tasks:
config = task['config']
paths = task['paths']
for path in paths:
motion = yf.readBvhFile(path)
normalizeSkeleton(motion, config)
adjustHeight(motion, config['halfFootHeight'])
additionalEdit(motion, path)
outputPath = outputDir + os.path.basename(path)
if SEGMENT_FOOT:
outputPath = outputDir + "segfoot_" + os.path.basename(path)
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if 'repeat' in config and config['repeat']:
hRef = .1
vRef = .3
lc = yma.getElementContactStates(motion, 'LeftFoot', hRef,
vRef)
interval = yba.getWalkingCycle2(motion, lc)
transitionLength = 20 if 'wd2_WalkAzuma01.bvh' in path else 10
motion = ymt.repeatCycle(motion, interval, 50,
transitionLength)
outputName = os.path.splitext(
os.path.basename(path))[0] + '_REPEATED.bvh'
outputPath = outputDir + outputName
if SEGMENT_FOOT:
outputPath = outputDir + 'segfoot_' + outputName
yf.writeBvhFile(outputPath, motion)
print(outputPath, 'done')
if VISUALIZE:
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 100, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
print('FINISHED')
