def joint2mm_Motion(jointMotion):
pointMotion = ym.Motion()
pointSkeleton = joint2mm_Skeleton(jointMotion[0].skeleton)
for jointPosture in jointMotion:
pointMotion.append(joint2mm_Posture(jointPosture, pointSkeleton))
pointMotion.fps = jointMotion.fps
return pointMotion
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_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 mm2Joint_Motion(pointMotion, rootName):
jointMotion = ym.Motion()
jointSkeleton = mm2Joint_Skeleton(pointMotion[0], rootName)
for pointPosture in pointMotion:
jointMotion.append(mm2Joint_Posture(pointPosture, pointMotion[0], jointSkeleton))
jointMotion.fps = pointMotion.fps
return jointMotion
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_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 _readOgreSkeletonAnimations(dom, jointSkeleton, initialRs, scale=1.0):
jointMotions = []
animationEles = dom.getElementsByTagName('animation')
for animationEle in animationEles:
jointMotion = ym.Motion()
# jointMotion.resourceName = animationEle.getAttribute('name').encode()
trackEles = animationEle.getElementsByTagName('track')
first_keyframes = trackEles[0].getElementsByTagName('keyframe')
len_keyframes = len(first_keyframes)
time2frameMap = {}
for i in range(len_keyframes):
jointPosture = ym.JointPosture(jointSkeleton)
# jointPosture.initLocalRMap(initialRMap)
jointPosture.initLocalRs(initialRs)
jointMotion.append(jointPosture)
# because each bone may not have same number of keyframes
time2frameMap[first_keyframes[i].getAttribute('time')] = i
for trackEle in trackEles:
# print i, trackEle.getAttribute('bone'), len(trackEle.getElementsByTagName('keyframe'))
keyframeEles = trackEle.getElementsByTagName('keyframe')
for keyframeEle in keyframeEles:
keyframeTime = keyframeEle.getAttribute('time')
# because each bone may not have same number of keyframes
frame = time2frameMap[keyframeTime]
jointPosture = jointMotion[frame]
boneName = trackEle.getAttribute('bone').encode()
if boneName == jointSkeleton.root.name:
transEle = keyframeEle.getElementsByTagName('translate')[0]
jointPosture.rootPos[0] = float(
transEle.getAttribute('x')) * scale
jointPosture.rootPos[1] = float(
transEle.getAttribute('y')) * scale
jointPosture.rootPos[2] = float(
transEle.getAttribute('z')) * scale
rotEle = keyframeEle.getElementsByTagName('rotate')[0]
angle = float(rotEle.getAttribute('angle'))
axisEle = rotEle.getElementsByTagName('axis')[0]
axis = mmMath.v3(float(axisEle.getAttribute('x')),
float(axisEle.getAttribute('y')),
float(axisEle.getAttribute('z')))
R = mmMath.exp(axis, angle)
# jointPosture.mulLocalR(boneName, R)
jointPosture.mulLocalR(jointSkeleton.getElementIndex(boneName),
R)
jointPosture.updateGlobalT()
jointMotions.append(jointMotion)
return jointMotions
def meshAnimation2PointMotion(skinMesh, jointMotion):
sm = copy.deepcopy(skinMesh)
pointSkeleton = ym.PointSkeleton()
for i in range(sm.getVertexNum()):
pointSkeleton.addElement(None, None)
pointMotion = ym.Motion()
for jointPosture in jointMotion:
sm.update(jointPosture)
pointMotion.append(mesh2PointPosture(sm, pointSkeleton))
return pointMotion
def readTrcFile(trcFilePath, scale=1.0):
f = open(trcFilePath)
fileLines = f.readlines()
pointMotion = ym.Motion()
i = 0
while i != len(fileLines):
splited = fileLines[i].split()
boneNames = []
if i == 2:
dataRate = float(splited[0])
numFrames = int(splited[2])
numMarkers = int(splited[3])
# print numFrames, numMarkers
elif i == 3:
markerNames = [name.split(':')[1] for name in splited[2:]]
skeleton = ym.PointSkeleton()
for name in markerNames:
skeleton.addElement(None, name)
# print markerNames
elif i > 5:
markerPositions = splited[2:]
# print markerPositions
# print 'i', i
pointPosture = ym.PointPosture(skeleton)
for m in range(numMarkers):
point = numpy.array([
float(markerPositions[m * 3]),
float(markerPositions[m * 3 + 1]),
float(markerPositions[m * 3 + 2])
])
point = numpy.dot(
mm.exp(numpy.array([1, 0, 0]), -math.pi / 2.),
point) * scale
# pointPosture.addPoint(markerNames[m], point)
pointPosture.setPosition(m, point)
# print 'm', m
# print markerNames[m], (markerPositions[m*3],markerPositions[m*3+1],markerPositions[m*3+2])
pointMotion.append(pointPosture)
i += 1
f.close()
pointMotion.fps = dataRate
return pointMotion
plot.addYdata('rFoot_lat', size_laterals_map[RFOOT], False)
plot.addXlines('rPeakFrames', rPeakFrames, False)
plot.showModeless()
contactStatesMap = yma.getMotionContactStates(motion, hRef, vRef)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'ys_motion',
yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_BONE))
viewer.doc.addObject('ys_motion', motion)
viewer.doc.addRenderer(
'tpose',
yr.JointMotionRenderer(ym.Motion([motion[0].getTPose()]),
(255, 100, 100), yr.LINK_BONE), False)
viewer.doc.addObject('tpose', motion)
def viewer_onClose(data):
plot.close()
viewer.onClose(data)
viewer.callback(viewer_onClose)
def extraDraw():
frame = viewer.getCurrentFrame()
for footName in footNames:
# if footName == RFOOT:
if footName == LFOOT:
def test_plot_LineCollection_speed():
import GUI.ysViewer as yv
import Motion.ysMotion as ym
import numpy as np
from matplotlib import collections
import time
pt = time.time()
count = 5
# plot
xs = []
ys = []
# lines = []
for i in range(count):
xs.append([])
ys.append([])
# line, = plot(range(100), range(100))
# lines.append(line)
def beforeFrameCallback(frame):
# print frame
for i in range(count):
xs[i].append(frame)
ys[i].append(frame + i)
# plot(xs[i], ys[i], 'b')
plot([frame, frame + 1], [frame + i, frame + i + 1], 'b')
# lines[i].set_xydata(frame,frame+i)
# xys = []
# for i in range(count):
# xys.append(xs[i])
# xys.append(ys[i])
# plot(xys)
xlim(0, 100)
ylim(0, 100)
draw()
if frame == 90:
print time.time() - pt
## # LineCollection
# fig = figure()
# a = fig.add_subplot(111)
# xys = []
# for i in range(count):
# xys.append([])
# def beforeFrameCallback(frame):
## print frame
# for i in range(count):
# xys[i].append((frame, frame+i))
#
## col = collections.LineCollection([xys[i]])
# col = collections.LineCollection([[(frame, frame+i), (frame+1, frame+i+1)]])
# a.add_collection(col)
#
## col = collections.LineCollection(xys)
## a.add_collection(col)
#
# xlim(0,100)
# ylim(0,100)
# draw()
# if frame == 90:
# print time.time() - pt
tempMotion = ym.Motion([None] * 100)
motionSystem = ym.MotionSystem()
motionSystem.addMotion(tempMotion)
viewer = yv.Viewer(100, 100, 800, 650, None, motionSystem, [])
viewer.beforeFrameCallback = beforeFrameCallback
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def readMMFile(mmFilePath):
names = {}
names[0] = 'root'
names[2] = 'lKnee'
names[4] = 'neck'
names[6] = 'rKnee'
names[8] = 'rFoot'
names[10] = 'rToe'
names[12] = 'head'
names[14] = 'rArm'
names[16] = 'lArm'
names[18] = 'lHand'
names[20] = 'rHand'
# names[14] = 'lArm'
# names[16] = 'rArm'
# names[18] = 'rHand'
# names[20] = 'lHand'
names[22] = 'lFoot'
names[24] = 'lToe'
skeleton = ym.MMSkeleton()
skeleton.addLink("head", "neck")
skeleton.addLink("neck", "lArm")
skeleton.addLink("lArm", "lHand")
skeleton.addLink("neck", "rArm")
skeleton.addLink("rArm", "rHand")
skeleton.addLink("neck", "root")
skeleton.addLink("root", "lKnee")
skeleton.addLink("lKnee", "lFoot")
skeleton.addLink("lFoot", "lToe")
skeleton.addLink("root", "rKnee")
skeleton.addLink("rKnee", "rFoot")
skeleton.addLink("rFoot", "rToe")
# # lowest foot height finding code
# lowest = 100
# for mmFilePath in paths:
# pointMotion = yf.readMMFile(mmFilePath)
# i = 0
# for name, point in pointMotion[i].pointMap.items():
# if name == 'rFoot' or name == 'lFoot' or name == 'rToe' or name == 'lToe':
# if point[1] < lowest:
# print mmFilePath, i
# print name
# print point[1]
# lowest = point[1]
lowest = .15
f = open(mmFilePath)
fileLines = f.readlines()
pointMotion = ym.Motion()
i = 0
while i != len(fileLines):
if fileLines[i].isspace() == False:
splited = fileLines[i].split()
pointPosture = ym.MMPosture(skeleton)
for j in range(0, len(splited), 2):
point = numpy.array(
[float(splited[j]),
float(splited[j + 1]), 0.])
point[1] -= lowest
pointPosture.addPoint(names[j], point)
pointMotion.append(pointPosture)
i += 1
f.close()
pointMotion.fps = 30.
return pointMotion
