def addJointSO3FromBvhJoint(self,
jointPosture,
bvhJoint,
channelValues,
scale=1.0):
localR = mm.I_SO3()
for channel in bvhJoint.channels:
if channel.channelType == 'XPOSITION':
jointPosture.rootPos[0] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'YPOSITION':
jointPosture.rootPos[1] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'ZPOSITION':
jointPosture.rootPos[2] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'XROTATION':
localR = numpy.dot(
localR,
mm.rotX(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'YROTATION':
localR = numpy.dot(
localR,
mm.rotY(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'ZROTATION':
localR = numpy.dot(
localR,
mm.rotZ(mm.RAD * channelValues[channel.channelIndex]))
jointPosture.setLocalR(
jointPosture.skeleton.getElementIndex(bvhJoint.name), localR)
for i in range(len(bvhJoint.children)):
self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i],
channelValues)
def getStitchedNextMotion_y_angle(nextMotion, prevEndPosture_or_d, prevStartPosture, y_angle, transitionLength, transitionFunc=lambda x:1.-yfg.identity(x)):
newNextMotion = nextMotion.copy()
d = align_y_angle(newNextMotion, prevEndPosture_or_d, prevStartPosture, alignPosition=True, alignOrientation=True, pos_y_preserve=True, ori_xz_preserve=True)
# d.localRs[0] = np.dot(d.localRs[0], mm.rotY(-y_angle))
newNextMotion.rotateTrajectory(mm.rotY(y_angle))
part1 = stitchSegment(newNextMotion[:transitionLength], transitionFunc, d, False)
part2 = newNextMotion[transitionLength:]
del part1[0]
return part1 + part2
def test_zyx():
x, y, z = (1.,.5,2.)
print 'orig ', x, y, z
R = mm.I_SO3()
R = np.dot(R, mm.rotZ(z))
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotY(y))
nz, nx, ny = cm.R2zxy_r(R)
print 'zxy_r', nx, ny, nz
R = mm.I_SO3()
R = np.dot(R, mm.rotY(y))
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotZ(z))
nz, nx, ny = cm.R2zxy_s(R)
print 'zxy_s', nx, ny, nz
R = mm.I_SO3()
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotY(y))
R = np.dot(R, mm.rotZ(z))
nx, ny, nz = cm.R2xyz_r(R)
print 'xyz_r', nx, ny, nz
def addJointSO3FromBvhJoint(self, jointPosture, bvhJoint, channelValues, scale = 1.0):
localR = mm.I_SO3()
for channel in bvhJoint.channels:
if channel.channelType == 'XPOSITION':
jointPosture.rootPos[0] = channelValues[channel.channelIndex]*scale
elif channel.channelType == 'YPOSITION':
jointPosture.rootPos[1] = channelValues[channel.channelIndex]*scale
elif channel.channelType == 'ZPOSITION':
jointPosture.rootPos[2] = channelValues[channel.channelIndex]*scale
elif channel.channelType == 'XROTATION':
localR = numpy.dot(localR, mm.rotX(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'YROTATION':
localR = numpy.dot(localR, mm.rotY(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'ZROTATION':
localR = numpy.dot(localR, mm.rotZ(mm.RAD * channelValues[channel.channelIndex]))
jointPosture.setLocalR(jointPosture.skeleton.getElementIndex(bvhJoint.name), localR)
for i in range(len(bvhJoint.children)):
self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i], channelValues)
def test_stack():
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
boxNum = 10
bodies = [None]*boxNum
for i in range(boxNum):
bodies[i] = cvb.VpBody(vpWorld)
bodies[i].addBoxGeom((1,1,1), 1)
bodies[i].setPosition((10,i,11))
bodies[i].setOrientation(mm.rotY(10*mm.RAD*random.random()))
# bodies[i].setFrame(mm.Rp2T( mm.rotY(10*mm.RAD*random.random()), (10,i,11)))
bodies2 = [None]*(boxNum*boxNum)
count = 0
for i in range(boxNum):
for j in range(boxNum):
bodies2[count] = cvb.VpBody(vpWorld)
bodies2[count].addBoxGeom((1,1,1), 1)
bodies2[count].setPosition((.1*random.random(),i,j))
# bodies2[count].setOrientation(mm.rotY(10*mm.RAD*random.random()))
count += 1
vpWorld.initialize()
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('box', yr.VpBodiesRenderer(bodies, (255,0,0)))
viewer.doc.addRenderer('box2', yr.VpBodiesRenderer(bodies2, (0,0,255)))
viewer.setMaxFrame(100)
def simulateCallback(frame):
for i in range(stepsPerFrame):
vpWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer((1/30.)*.4)
viewer.show()
Fl.run()
def getStitchedNextMotion_y_angle(
nextMotion,
prevEndPosture_or_d,
prevStartPosture,
y_angle,
transitionLength,
transitionFunc=lambda x: 1. - yfg.identity(x)):
newNextMotion = nextMotion.copy()
d = align_y_angle(newNextMotion,
prevEndPosture_or_d,
prevStartPosture,
alignPosition=True,
alignOrientation=True,
pos_y_preserve=True,
ori_xz_preserve=True)
# d.localRs[0] = np.dot(d.localRs[0], mm.rot_y(-y_angle))
newNextMotion.rotateTrajectory(mm.rotY(y_angle))
part1 = stitchSegment(newNextMotion[:transitionLength], transitionFunc, d,
False)
part2 = newNextMotion[transitionLength:]
del part1[0]
return part1 + part2
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 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(
'ys_motion',
yr.JointMotionRenderer(motion, (100, 100, 255), yr.LINK_LINE))
viewer.doc.addObject('ys_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()