def _readMotionData(self):
'''
Read the motion data from the BVH file.
'''
# update joint tree with new data.
frame = 0
lastRotation = {}
for frame in range(self.frameCount):
time = frame * self.framePeriod
channelData = self._readFrame()
for joint in self.model.joints:
for chan in joint.channels:
chanData = channelData.pop(0)
if chan == "Xposition":
xPos = chanData
elif chan == "Yposition":
yPos = chanData
elif chan == "Zposition":
zPos = chanData
elif chan == "Xrotation":
xRot = chanData
elif chan == "Yrotation":
yRot = chanData
elif chan == "Zrotation":
zRot = chanData
else:
raise RuntimeError('Unknown channel: ' + chan)
if not joint.hasParent:
try:
p = np.array([[xPos,yPos,zPos]]).T * \
self.conversionFactor
p = convertCGtoNED(p)
del xPos, yPos, zPos
joint.positionKeyFrames.add(time, p)
except UnboundLocalError:
raise SyntaxError("No position data for root joint")
try:
if joint.hasChildren:
q = Quaternion.fromEuler((zRot, xRot, yRot),
order='zxy')
q = convertCGtoNED(q)
del xRot, yRot, zRot
# Rotation in BVH is relative to parent joint so
# combine the rotations
if joint.hasParent:
q = lastRotation[joint.parent] * q
joint.rotationKeyFrames.add(time, q)
lastRotation[joint] = q
except UnboundLocalError:
raise SyntaxError("No rotation data for a joint that "
"is not an end effector")
def testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0), vector(0, 0, 0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1, 0, 0, 0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10, 0, 0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10, 0, 0)))
assertQuaternionAlmostEqual(
j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0, 10, 0))
assert_almost_equal(j1end.position(0), vector(0, 0, 0))
def testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile()
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0),vector(0,0,0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1,0,0,0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10,0,0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10,0,0)))
assertQuaternionAlmostEqual(j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0,10,0))
assert_almost_equal(j1end.position(0), vector(0,0,0))
def _readJoint(self):
# use jointStack[-1] to peek at the top of the jointStack
currentJoint = self.jointStack[-1]
name = self._token()
# name will be 'Site' for End Site joints.
if name == 'Site':
name = currentJoint.parent.name + "_end"
currentJoint.name = name
self._checkToken("{")
while True:
token = self._token()
if token == "OFFSET":
x = self._floatToken()
y = self._floatToken()
z = self._floatToken()
currentJoint.positionOffset = \
convertCGtoNED(np.array([[x,y,z]]).T*self.conversionFactor)
elif token == "CHANNELS":
n = self._intToken()
channels = []
for i in range(n):
token = self._token()
if token not in ["Xposition","Yposition","Zposition",\
"Xrotation","Yrotation","Zrotation"]:
raise SyntaxError, "Syntax error in line %d: Invalid \
channel name '%s'" % (self.line, token)
else:
channels.append(token)
self.totalChannels += n
currentJoint.channels = channels
elif token in ("JOINT", "End"):
if token == "JOINT":
joint = SampledJoint(currentJoint)
else:
joint = PointTrajectory(currentJoint)
joint.channels = []
self.jointStack.append(joint)
self._readJoint()
elif token == "}":
self.jointStack.pop()
break
else:
raise SyntaxError,\
"Syntax error in line %d: Unknown keyword '%s'" %(
self.line,token)
def testCG_to_NED_Quat():
testing.assert_equal(
transforms.convertCGtoNED(Quaternion()).components,
Quaternion(0.5, -0.5, 0.5, -0.5).components)
def checkCG_to_NED(ned, cg):
assert_almost_equal(transforms.convertCGtoNED(cg), ned)
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
"""
Load motion capture data from an ASF and AMC file pair.
@param asfFileName: Name of the ASF file containing the description of
the rigid body model
@param amcFileName: Name of the AMC file containing the motion data
@param scaleFactor: Scaling factor to convert lengths to m. For data
from the CMU motion capture corpus this should be 2.54/100 to
convert from inches to metres.
@return: A {SampledBodyModel} representing the root of the rigid body
model structure.
"""
with open(asfFileName, 'r') as asfFile:
data = asfParser.parseFile(asfFile)
scale = (1.0/data.units.get('length',1)) * scaleFactor
bones = dict((bone.name,bone) for bone in data.bones)
asfModel = ASFRoot(data.root)
for entry in data.hierarchy:
parent = asfModel.getBone(entry.parent)
for childName in entry.children:
ASFBone(parent, bones[childName], scale)
imusimModel = SampledBodyModel('root')
for subtree in asfModel.children:
for bone in subtree:
if not bone.isDummy:
offset = vector(0,0,0)
ancestors = bone.ascendTree()
while True:
ancestor = ancestors.next().parent
offset += ancestor.childoffset
if not ancestor.isDummy:
break
SampledJoint(parent=imusimModel.getJoint(ancestor.name),
name=bone.name,
offset=offset)
if not bone.hasChildren:
PointTrajectory(
parent=imusimModel.getJoint(bone.name),
name=bone.name+'_end',
offset=bone.childoffset
)
with open(amcFileName) as amcFile:
motion = amcParser.parseFile(amcFile)
t = 0
for frame in motion.frames:
for bone in frame.bones:
bonedata = asfModel.getBone(bone.name)
if bone.name == 'root':
data = dict((chan.lower(), v) for chan,v in
zip(bonedata.channels,bone.channels))
position = convertCGtoNED(scale * vector(data['tx'],
data['ty'],data['tz']))
imusimModel.positionKeyFrames.add(t, position)
axes, angles = zip(*[(chan[-1], angle) for chan, angle in
zip(bonedata.channels, bone.channels) if
chan.lower().startswith('r')])
rotation = (bonedata.rotationOffset.conjugate *
Quaternion.fromEuler(angles[::-1], axes[::-1]))
joint = imusimModel.getJoint(bone.name)
if joint.hasParent:
parentRot = joint.parent.rotationKeyFrames.latestValue
parentRotOffset = bonedata.parent.rotationOffset
rotation = parentRot * parentRotOffset * rotation
else:
rotation = convertCGtoNED(rotation)
joint.rotationKeyFrames.add(t, rotation)
t += framePeriod
return imusimModel
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
"""
Load motion capture data from an ASF and AMC file pair.
@param asfFileName: Name of the ASF file containing the description of
the rigid body model
@param amcFileName: Name of the AMC file containing the motion data
@param scaleFactor: Scaling factor to convert lengths to m. For data
from the CMU motion capture corpus this should be 2.54/100 to
convert from inches to metres.
@return: A {SampledBodyModel} representing the root of the rigid body
model structure.
"""
with open(asfFileName, 'r') as asfFile:
data = asfParser.parseFile(asfFile)
scale = (1.0 / data.units.get('length', 1)) * scaleFactor
bones = dict((bone.name, bone) for bone in data.bones)
asfModel = ASFRoot(data.root)
for entry in data.hierarchy:
parent = asfModel.getBone(entry.parent)
for childName in entry.children:
ASFBone(parent, bones[childName], scale)
imusimModel = SampledBodyModel('root')
for subtree in asfModel.children:
for bone in subtree:
if not bone.isDummy:
offset = vector(0, 0, 0)
ancestors = bone.ascendTree()
while True:
ancestor = ancestors.next().parent
offset += ancestor.childoffset
if not ancestor.isDummy:
break
SampledJoint(parent=imusimModel.getJoint(ancestor.name),
name=bone.name,
offset=offset)
if not bone.hasChildren:
PointTrajectory(parent=imusimModel.getJoint(bone.name),
name=bone.name + '_end',
offset=bone.childoffset)
with open(amcFileName) as amcFile:
motion = amcParser.parseFile(amcFile)
t = 0
for frame in motion.frames:
for bone in frame.bones:
bonedata = asfModel.getBone(bone.name)
if bone.name == 'root':
data = dict((chan.lower(), v) for chan, v in zip(
bonedata.channels, bone.channels))
position = convertCGtoNED(
scale * vector(data['tx'], data['ty'], data['tz']))
imusimModel.positionKeyFrames.add(t, position)
axes, angles = zip(*[(chan[-1], angle)
for chan, angle in zip(
bonedata.channels, bone.channels)
if chan.lower().startswith('r')])
rotation = (bonedata.rotationOffset.conjugate *
Quaternion.fromEuler(angles[::-1], axes[::-1]))
joint = imusimModel.getJoint(bone.name)
if joint.hasParent:
parentRot = joint.parent.rotationKeyFrames.latestValue
parentRotOffset = bonedata.parent.rotationOffset
rotation = parentRot * parentRotOffset * rotation
else:
rotation = convertCGtoNED(rotation)
joint.rotationKeyFrames.add(t, rotation)
t += framePeriod
return imusimModel
def testCG_to_NED_Quat():
testing.assert_equal(
transforms.convertCGtoNED(Quaternion()).components,
Quaternion(0.5, -0.5, 0.5, -0.5).components)
def checkCG_to_NED(ned, cg):
assert_almost_equal(transforms.convertCGtoNED(cg), ned)
