def writeFrame(self, t):
for j in self.model.joints:
if hasattr(j, 'channels') and len(j.channels) > 0:
if not j.hasParent:
pos = j.position(t) * self.conversionFactor
pos = convertNEDtoCG(pos)
rot = j.rotation(t)
if j.hasParent:
# BVH rotations are relative to parent joint
# RigidBodyModel rotations are absolute
rot = j.parent.rotation(t).conjugate * rot
rot = convertNEDtoCG(rot)
rot = rot.toEuler(order='zxy')
for chan in j.channels:
if chan == 'Xposition':
self.bvhFile.write("%f " % pos[0])
elif chan == 'Yposition':
self.bvhFile.write("%f " % pos[1])
elif chan == 'Zposition':
self.bvhFile.write("%f " % pos[2])
elif chan == 'Zrotation':
self.bvhFile.write("%f " % rot[0])
elif chan == 'Xrotation':
self.bvhFile.write("%f " % rot[1])
elif chan == 'Yrotation':
self.bvhFile.write("%f " % rot[2])
self.bvhFile.write("\n")
def writeHeader(self):
"""
Write the header of the BVH file.
"""
self.bvhFile.write('HIERARCHY\n')
pad = " "
def post(j):
self.bvhFile.write("%s}\n" % (pad * j.depth))
for p in self.model.preorderTraversal(postFunc=post):
d = p.depth
if not p.hasParent:
self.bvhFile.write("%sROOT %s\n" % (pad * d, p.name))
elif p.isJoint:
self.bvhFile.write("%sJOINT %s\n" % (pad * d, p.name))
else:
self.bvhFile.write("%sEnd Site\n" % (pad * d))
self.bvhFile.write("%s{\n" % (pad * d))
self.bvhFile.write("%sOFFSET" % (pad * (d + 1)))
for v in convertNEDtoCG(p.positionOffset):
self.bvhFile.write(" %.8f" % (v * self.conversionFactor))
self.bvhFile.write("\n")
if p.isJoint:
if not hasattr(p, "channels"):
if not p.hasParent:
p.channels = [
"Xposition", "Yposition", "Zposition", "Zrotation",
"Xrotation", "Yrotation"
]
else:
p.channels = ["Zrotation", "Xrotation", "Yrotation"]
self.bvhFile.write("%sCHANNELS %d" %
(pad * (d + 1), len(p.channels)))
for chan in p.channels:
self.bvhFile.write(" %s" % chan)
self.bvhFile.write("\n")
self.bvhFile.write("MOTION\nFrames: %d\nFrame Time: %.8f\n" %
(self.frames, self.samplePeriod))
def testNED_to_CG_Quat():
testing.assert_equal(
transforms.convertNEDtoCG(Quaternion()).components,
Quaternion(0.5, 0.5, -0.5, 0.5).components)
def checkNED_to_CG(ned, cg):
assert_almost_equal(transforms.convertNEDtoCG(ned), cg)
def testNED_to_CG_Quat():
testing.assert_equal(
transforms.convertNEDtoCG(Quaternion()).components,
Quaternion(0.5, 0.5, -0.5, 0.5).components)
def checkNED_to_CG(ned, cg):
assert_almost_equal(transforms.convertNEDtoCG(ned), cg)
