def test_writeBvhFile():
# # bvh
# bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
# bvh = readBvhFileAsBvh(bvhFilePath)
#
# tempFilePath = '../samples/bvh_wd2_WalkSameSame00.bvh.temp'
# bvh.writeBvhFile(tempFilePath)
# motion
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = readBvhFile(bvhFilePath, .01)
# motion[0] = motion[0].getTPose()
tempFilePath = '../samples/motion_temp_wd2_WalkSameSame00.bvh.temp'
writeBvhFile(tempFilePath, motion)
motion2 = readBvhFile(tempFilePath)
# motion2[0] = motion2[0].getTPose()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion',
yr.JointMotionRenderer(motion, (0, 255, 0)))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motion2',
yr.JointMotionRenderer(motion2, (255, 0, 0)))
viewer.doc.addObject('motion2', motion2)
viewer.startTimer(1 / motion.fps)
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_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.vec3(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(
'ys_motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('ys_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_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_ObjectInfoWnd():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
jointMotion1, frameTime = yf.readBvhFileAsJointMotion(bvhFilePath, .01)
bvhFilePath = '../samples/wd2_WalkForwardVFast00.bvh'
jointMotion2, frameTime = yf.readBvhFileAsJointMotion(bvhFilePath, .01)
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
skinMesh, js = yol.readOgreDataFiles(meshFilePath, .01)
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
mesh = yol.readOgreMeshFileAsMesh(meshFilePath, .01)
ysu.mergePoints(skinMesh)
ysu.mergePoints(mesh)
skinMesh.update(js[0][80])
viewer = SimpleViewer()
viewer.doc.addRenderer('motion1(%s)'%jointMotion1.resourceName, yr.JointMotionRenderer(jointMotion1, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('motion1(%s)'%jointMotion1.resourceName, jointMotion1)
viewer.doc.addRenderer('motion2(%s)'%jointMotion2.resourceName, yr.JointMotionRenderer(jointMotion2, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('motion2(%s)'%jointMotion1.resourceName, jointMotion2)
viewer.doc.addRenderer('skinMesh', yr.MeshRenderer(skinMesh))
viewer.doc.addObject('skinMesh', skinMesh)
viewer.doc.addRenderer('mesh', yr.MeshRenderer(mesh))
viewer.doc.addObject('mesh', mesh)
viewer.startTimer(frameTime)
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_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_JTSolver_IKTree_COM():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
skeleton = motion[0].skeleton
origPosture = ym.Motion([motion[0]])
newPosture = copy.deepcopy(origPosture)
# effectorNames = ['body3_Effector']
targetPositions = [(.5,.5,0)]
# tree = jointSkeleton2IKTree(skeleton)
# solver = JTSolver_IKTree_COM(tree, effectorNames)
# solver.solveCOM(newPosture[0], targetPositions)
# COM = getCOMPos()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('origPosture', yr.JointMotionRenderer(origPosture, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addRenderer('newPosture', yr.JointMotionRenderer(newPosture, (0,255,0), yr.LINK_WIREBOX))
viewer.doc.addRenderer('targetPosition', yr.PointsRenderer(targetPositions))
# viewer.doc.addRenderer('COM', yr.PointsRenderer([COM]))
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_JTSolver_IKTree():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
skeleton = motion[0].skeleton
origPosture = ym.Motion([motion[0]])
newPosture = copy.deepcopy(origPosture)
effectorNames = ['body3_Effector']
# effectorNames = ['body1']
targetPositions = [(1.,0,0)]
tree = jointSkeleton2IKTree(skeleton)
# print tree
# tree.changeRootNode('body3_Effector')
# print tree
solver = JTSolver_IKTree(tree, effectorNames)
solver.solve(newPosture[0], targetPositions)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('origPosture', yr.JointMotionRenderer(origPosture, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addRenderer('newPosture', yr.JointMotionRenderer(newPosture, (0,255,0), yr.LINK_WIREBOX))
viewer.doc.addRenderer('targetPosition', yr.PointsRenderer(targetPositions))
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_JTSolver_Skeleton():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
skeleton = motion[0].skeleton
origPosture = ym.Motion([motion[0]])
newPosture = copy.deepcopy(origPosture)
effectorNames = ['body3_Effector']
# effectorNames = ['body1']
targetPositions = [(1.,0,0)]
print newPosture[0].skeleton
newPosture[0].skeleton.changeRoot('body2')
print newPosture[0].skeleton
solver = JTSolver_Skeleton(skeleton, effectorNames)
solver.solve(newPosture[0], targetPositions)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('origPosture', yr.JointMotionRenderer(origPosture, (0,0,255), yr.LINK_SOLIDBOX))
viewer.doc.addRenderer('newPosture', yr.JointMotionRenderer(newPosture, (0,255,0), yr.LINK_SOLIDBOX))
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
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_blendedSegment():
frameTime = 1 / 30.
# source motions
# bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
# motion0 = yf.readBvhFile(bvhFilePath, .01)
# motion1 = motion0.copy()
# motion1.rotateTrajectory(mm.rot_y(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_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.maxsize
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_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('ys_motion[%d]~ys_motion[%d] => ' %
(steps[lateCycle][0], steps[lateCycle][1]),
end=' ')
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(
'ys_motion',
yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_LINE))
viewer.doc.addObject('ys_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(
'ys_motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addObject('ys_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_joint2Point_Motion():
bvhFilePath = '../samples/physics2_WalkSameSame00.bvh'
jointMotion, frameTime = yf.readBvhFileAsJointMotion(bvhFilePath, .1)
# bvhFilePath = '../../../Walking/ppmotion/wd2_WalkForwardNormal00.bvh'
# jointMotion = yf.readBvhFile(bvhFilePath)
# frameTime = 1/jointMotion.fps
pointMotion = joint2Point_Motion(jointMotion)
# pointMotion = joint2mm_Motion(jointMotion)
print('jointMotion.skeleton', jointMotion[0].skeleton)
print('pointMotion.skeleton', pointMotion[0].skeleton)
print('jointMotion.skeleton - pointMotion.skeleton', set(jointMotion[0].skeleton.joints.keys()) - pointMotion[0].skeleton.pointSet)
print('pointMotion.skeleton - jointMotion.skeleton', pointMotion[0].skeleton.pointSet - set(jointMotion[0].skeleton.joints.keys()))
motionSystem = ym.MotionSystem()
motionSystem.addMotion(pointMotion)
motionSystem.addMotion(jointMotion)
renderers = []
renderers.append(yr.PointMotionRenderer(pointMotion))
renderers.append(yr.JointMotionRenderer(jointMotion, (0,255,0), yr.LINK_LINE))
viewer = yv.Viewer2(100, 100, 800, 650, None, motionSystem, renderers)
# viewer = yv.MotionViewer(100, 100, 800, 650)
viewer.setRenderers(renderers)
viewer.setMotionSystem(motionSystem)
viewer.startTimer(frameTime)
viewer.show()
Fl.run()
def test_body_force_funcs():
bvhFilePath = '../samples/chain_1.bvh'
# bvhFilePath = '../samples/chain_6.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.gravity = (0,0,0)
stepsPerFrame = 60
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.setBodyPositionGlobal(0, (0,1,0))
###################################################################
# apply force == add force
# controlModel.applyBodyGenForceGlobal(0, (0,0,100), (0,-100,0), (0,0,0))
# above 1 line == below 2 lines
controlModel.applyBodyTorqueGlobal(0, (0,0,100))
controlModel.applyBodyForceGlobal(0, (0,-100,0))
p = [mm.O_Vec3()]*controlModel.getBodyNum()
forces = [mm.O_Vec3()] * controlModel.getBodyNum()
netForces = [mm.O_Vec3()] * controlModel.getBodyNum()
gravityForces = [mm.O_Vec3()] * controlModel.getBodyNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('forces', yr.VectorsRenderer(forces, p, (255,0,0)))
# viewer.doc.addRenderer('netForces', yr.VectorsRenderer(netForces, p, (0,255,0)))
# viewer.doc.addRenderer('gravityForces', yr.VectorsRenderer(gravityForces, p, (0,0,255)))
viewer.setMaxFrame(500)
def simulateCallback(frame):
p[:] = controlModel.getBodyPositionsGlobal()
forces[0] = controlModel.getBodyForceLocal(0)
netForces[0] = controlModel.getBodyNetForceLocal(0)
gravityForces[0] = controlModel.getBodyGravityForceLocal(0)
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_readOgreSkeletonFile_SkeletonAnimation():
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
mesh = readOgreMeshFileAsMesh(meshFilePath, .01)
# skeletonFilePath = '../samples/woody2_15.skeleton.xml'
skeletonFilePath = '../samples/physics2_woody_binding1.skeleton.xml'
jointMotions = readOgreSkeletonFile_SkeletonAnimations(
skeletonFilePath, .01)
viewer = ysv.SimpleViewer()
for i in range(len(jointMotions)):
viewer.doc.addMotion(jointMotions[i])
viewer.doc.addRenderer(
jointMotions[i].resourceName,
yr.JointMotionRenderer(jointMotions[i], (0, 0, 255),
yr.LINK_LINE))
viewer.doc.addRenderer('mesh', yr.MeshRenderer(mesh))
def extraDrawCallback():
frame = viewer.getCurrentFrame()
for i in range(jointMotions[0][0].skeleton.getElementNum()):
ygh.drawPoint(jointMotions[0][frame].getPosition(i))
viewer.setExtraDrawCallback(extraDrawCallback)
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])
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_point2Joint_Motion():
mmFilePath = '../samples/physics2_WalkSameSame01.mm'
pointMotion = yf.readMMFile(mmFilePath)
frameTime = 1. / 30.
pointMotion[0].pointMap['lFoot'][1] -= 0.1
# pointMotion = pointMotion[0:1]
jointMotion = point2Joint_Motion(pointMotion, 'root')
pointMotion2 = joint2Point_Motion(jointMotion)
print 'pointMotion.skeleton', pointMotion[0].skeleton.links
print pointMotion[0].pointMap.keys()
print 'jointMotion.skeleton', jointMotion[0].skeleton.joints.keys()
print 'pointMotion2.skeleton', pointMotion2[0].skeleton.links
print pointMotion2[0].pointMap.keys()
motionSystem = ym.MotionSystem()
motionSystem.addMotion(pointMotion)
motionSystem.addMotion(jointMotion)
renderers = []
# renderers.append(yr.PointMotionRenderer(pointMotion))
renderers.append(yr.PointMotionRenderer(pointMotion2))
renderers.append(
yr.JointMotionRenderer(jointMotion, (0, 255, 0), yr.LINK_LINE))
viewer = yv.Viewer2(100, 100, 800, 650, None, motionSystem, renderers)
viewer.startTimer(frameTime)
viewer.show()
Fl.run()
def test_joint2Point_Motion():
bvhFilePath = '../samples/physics2_WalkSameSame00.bvh'
jointMotion, frameTime = yf.readBvhFileAsJointMotion(bvhFilePath, .1)
pointMotion = joint2Point_Motion(jointMotion)
print 'jointMotion.skeleton', jointMotion[0].skeleton
print 'pointMotion.skeleton', pointMotion[0].skeleton
print 'jointMotion.skeleton - pointMotion.skeleton', set(
jointMotion[0].skeleton.joints.keys(
)) - pointMotion[0].skeleton.pointSet
print 'pointMotion.skeleton - jointMotion.skeleton', pointMotion[
0].skeleton.pointSet - set(jointMotion[0].skeleton.joints.keys())
motionSystem = ym.MotionSystem()
motionSystem.addMotion(pointMotion)
motionSystem.addMotion(jointMotion)
renderers = []
renderers.append(yr.PointMotionRenderer(pointMotion))
renderers.append(
yr.JointMotionRenderer(jointMotion, (0, 255, 0), yr.LINK_LINE))
viewer = yv.Viewer2(100, 100, 800, 650, None, motionSystem, renderers)
viewer.startTimer(frameTime)
viewer.show()
Fl.run()
def test_readOgreDataFiles():
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
skeletonFilePath = '../samples/physics2_woody_binding1.skeleton.xml'
skinMesh, jointMotions = readOgreDataFiles(meshFilePath, .01,
skeletonFilePath)
for jointPosture in jointMotions[0]:
jointPosture.updateGlobalT()
viewer = ysv.SimpleViewer()
for i in range(len(jointMotions)):
viewer.doc.addObject('motion%d' % i, jointMotions[i])
viewer.doc.addRenderer(
'motion%d' % i,
yr.JointMotionRenderer(jointMotions[i], (0, 0, 255),
yr.LINK_LINE))
viewer.doc.addRenderer('skinMesh', yr.MeshRenderer(skinMesh))
def preFrameCallback(frame):
skinMesh.update(jointMotions[0][frame])
viewer.setPreFrameCallback(preFrameCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_readOgreMeshFileAsSkinMesh():
skeletonFilePath = '../samples/woody2_15.skeleton.xml'
# jointSkeleton, initialRMap = readOgreSkeletonFile_Skeleton(skeletonFilePath, .1)
# jointMotions = readOgreSkeletonFile_SkeletonAnimations(skeletonFilePath, .1)
jointSkeleton, initialRs, jointMotions = readOgreSkeletonFile(
skeletonFilePath, .01)
meshFilePath = '../samples/woody2_15.mesh.xml'
skinMesh = readOgreMeshFileAsSkinMesh(meshFilePath, jointSkeleton,
initialRs, .01)
jointMotion = yf.readBvhFile('../samples/wd2_WalkSameSame00.bvh', .01)
viewer = ysv.SimpleViewer()
# for i in range(len(jointMotions)):
# viewer.doc.addMotion(jointMotions[i])
# viewer.doc.addRenderer(jointMotions[i].resourceName, yr.JointMotionRenderer(jointMotions[i], (0, 0, 255), yr.LINK_LINE))
viewer.doc.addRenderer(
'jointMotion',
yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_BONE))
viewer.doc.addObject('jointMotion', jointMotion)
viewer.doc.addRenderer('skinMesh', yr.MeshRenderer(skinMesh))
def preFrameCallback(frame):
skinMesh.update(jointMotion[frame])
viewer.setPreFrameCallback(preFrameCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_mergePoints():
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
skinMesh, jointMotions = yol.readOgreDataFiles(meshFilePath, .1)
mergedSkinMesh = copy.deepcopy(skinMesh)
mergePoints(mergedSkinMesh)
# print skinMesh
# print mergedSkinMesh
viewer = ysv.SimpleViewer()
viewer.record(False)
for i in range(len(jointMotions)):
viewer.doc.addRenderer(jointMotions[i].resourceName, yr.JointMotionRenderer(jointMotions[i], (0, 0, 255), yr.LINK_LINE))
viewer.doc.addMotion2(jointMotions[i].resourceName, jointMotions[i])
viewer.doc.addRenderer('skinMesh', yr.MeshRenderer(skinMesh))
viewer.doc.addObject('skinMesh', skinMesh)
viewer.doc.addRenderer('mergedSkinMesh', yr.MeshRenderer(mergedSkinMesh, (0,255,0)))
viewer.doc.addObject('mergedSkinMesh', mergedSkinMesh)
def preFrameCallback(frame):
skinMesh.update(jointMotions[0][frame])
mergedSkinMesh.update(jointMotions[0][frame])
viewer.setPreFrameCallback(preFrameCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_readOgreSkeletonFile_Skeleton():
# meshFilePath = '../samples/woody2_15.mesh.xml'
meshFilePath = '../samples/physics2_woody_binding1.mesh.xml'
mesh = readOgreMeshFileAsMesh(meshFilePath, .01)
# skeletonFilePath = '../samples/woody2_15.skeleton.xml'
skeletonFilePath = '../samples/physics2_woody_binding1.skeleton.xml'
jointSkeleton, initialRs = readOgreSkeletonFile_Skeleton(
skeletonFilePath, .01)
skeletonPosture = ym.JointPosture(jointSkeleton)
skeletonPosture.initLocalRs(initialRs)
# skeletonPosture.initLocalRs()
skeletonMotion = ym.Motion([skeletonPosture])
viewer = ysv.SimpleViewer()
viewer.doc.addMotion(skeletonMotion)
viewer.doc.addRenderer(
'skeleton',
yr.JointMotionRenderer(skeletonMotion, (0, 0, 255), yr.LINK_LINE))
viewer.doc.addRenderer('mesh', yr.MeshRenderer(mesh))
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_OdeControlModel():
# mcfg.defaultContactMode = ode.ContactBounce | ode.ContactSoftERP | ode.ContactSoftCFM
# mcfg.defaultJointAxes = []
# mcfg.defaultJointLoStop = mm.deg2Rad(-5)
# mcfg.defaultJointHiStop = mm.deg2Rad(5)
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
motion = motion[30:]
AX = numpy.array([1, 0, 0])
AY = numpy.array([0, 1, 0])
AZ = numpy.array([0, 0, 1])
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
mcfg.defaultKp = 100.
mcfg.defaultKd = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('body2')
node.length = 1
node.offset = (0, 0, .2)
# node.jointAxes = [AY]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel.fixRootBody()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
def simulateCallback(frame):
for i in range(stepsPerFrame):
torques = controlModel.calcPDTorque(motion[frame])
controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
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_getInternalJointOrientationsGlobal():
# bvhFilePath = '../samples/block_tree_rotate.bvh'
bvhFilePath = '../samples/chain_3_rotate_expt_root.bvh'
motion = yf.readBvhFile(bvhFilePath)
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)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.translateByOffset((0,0,1))
controlModel.fixBody(0)
jointPositions = []
localFrames = []
globalFrames = []
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('localFrames', yr.FramesRenderer(localFrames, (255,0,0)))
viewer.doc.addRenderer('globalFrames', yr.FramesRenderer(globalFrames, (0,255,0)))
def simulateCallback(frame):
controlModel.setJointAngVelocityLocal(1, (0,.5,0))
controlModel.setJointAngVelocityLocal(2, (0,.5,0))
for i in range(stepsPerFrame):
vpWorld.step()
jointPositions[:] = motion[frame].getInternalJointPositionsGlobal() + controlModel.getInternalJointPositionsGlobal()
localFrames[:] = motion.getInternalJointOrientationsLocal(frame) + controlModel.getInternalJointOrientationsLocal()
localFrames[:] = map(mm.Rp2T, localFrames, jointPositions)
globalFrames[:] = motion[frame].getInternalJointOrientationsGlobal() + controlModel.getInternalJointOrientationsGlobal()
globalFrames[:] = map(mm.Rp2T, globalFrames, jointPositions)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_invMref():
skeletonFilePath = '../samples/physics2_woody_binding1.skeleton.xml'
bindJointName = 'LeftHand'
jointMotions = yol.readOgreSkeletonFile_SkeletonAnimations(skeletonFilePath, .01)
jointMotion = jointMotions[0]
frameTime = 1./30.
# print jointMotion[0].skeleton
initialPosture = jointMotion[0].getTPose()
viewer = ysv.SimpleViewer()
viewer.doc.addMotion(jointMotion)
viewer.doc.addRenderer('initialPosture', yr.JointMotionRenderer(ym.Motion([initialPosture]), (255, 0, 0), yr.LINK_LINE))
viewer.doc.addRenderer('targetPosture', yr.JointMotionRenderer(jointMotion, (0, 0, 255), yr.LINK_LINE))
# v = numpy.array([0,2,2])
v = initialPosture.getGlobalPos(bindJointName) + (1,0,0)
# v = mmMath.p2T(numpy.array([0,0,2]))
# print 'v', v
v_new = [numpy.zeros(3)]
def preFrameCallback(frame):
targetPosture = jointMotion[frame]
invMref = mmMath.invertSE3(initialPosture.calcGlobalT(bindJointName))
v_local = mmMath.T2p(numpy.dot(invMref, mmMath.p2T(v)))
# v_local = numpy.dot(invMref, v)
# print 'v_local', v_local
M = targetPosture.calcGlobalT(bindJointName)
v_new[0] = mmMath.T2p(numpy.dot(M, mmMath.p2T(v_local)))
# v_new = numpy.dot(M, v_local)
# print 'v_new', v_new
def extraDrawCallback():
ygh.drawLine(initialPosture.getGlobalPos(bindJointName), v, (255, 178, 178))
ygh.drawLine(jointMotion[jointMotion.frame].getGlobalPos(bindJointName), v_new[0], (178, 178, 255))
viewer.setPreFrameCallback(preFrameCallback)
viewer.setExtraDrawCallback(extraDrawCallback)
viewer.startTimer(frameTime)
viewer.show()
Fl.run()
def test_readBvhFile():
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = readBvhFile(bvhFilePath, .01)
motion2 = readBvhFile(bvhFilePath, .01, True)
print motion[0].skeleton
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer('motion',
yr.JointMotionRenderer(motion, (0, 255, 0)))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motion2',
yr.JointMotionRenderer(motion2, (255, 0, 0)))
viewer.doc.addObject('motion2', motion2)
viewer.startTimer(1 / motion.fps)
viewer.show()
Fl.run()
