def checkStaticConvergence(filterClass, performTest=True):
"""
Test that an orientation estimation algorithm converges within a reasonable
number of samples
"""
gyro = np.zeros((3, 1))
accel = -GRAVITY
mag = NORTH
SAMPLES = 500
initialRotation = Quaternion.fromEuler((60, 45, 0))
filter = filterClass(initialRotation=initialRotation,
initialTime=0,
initialRotationalVelocity=np.zeros((3, 1)),
**filterParameters.get(filterClass, {}))
estimatedQuaternions = QuaternionArray(np.empty((SAMPLES, 4)))
for i in range(SAMPLES):
filter(accel, mag, gyro, dt * (1 + i))
estimatedQuaternions[i] = filter.rotation.latestValue
if performTest:
assertMaximumErrorAngle(Quaternion(), filter.rotation.latestValue, 2)
return estimatedQuaternions
def checkStaticConvergence(filterClass, performTest=True):
"""
Test that an orientation estimation algorithm converges within a reasonable
number of samples
"""
gyro = np.zeros((3,1))
accel = -GRAVITY
mag = NORTH
SAMPLES = 500
initialRotation = Quaternion.fromEuler((60,45,0))
filter = filterClass(initialRotation = initialRotation,
initialTime=0,
initialRotationalVelocity = np.zeros((3,1)),
**filterParameters.get(filterClass, {}))
estimatedQuaternions = QuaternionArray(np.empty((SAMPLES,4)))
for i in range(SAMPLES):
filter(accel, mag, gyro, dt*(1+i))
estimatedQuaternions[i] = filter.rotation.latestValue
if performTest:
assertMaximumErrorAngle(Quaternion(),
filter.rotation.latestValue, 2)
return estimatedQuaternions
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 checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)], [0], [math.sin(inclination)]])
g = np.array([[0, 0, 1]], dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q, vo(g, m))
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 checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)],[0],
[math.sin(inclination)]])
g = np.array([[0,0,1]],dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q,vo(g,m))
def __init__(self, rootdata):
"""
Construct a new ASFRoot.
@param rootdata: The L{ParseResults} object representing the root
segment data from the ASF file.
"""
TreeNode.__init__(self, None)
self.name = 'root'
self.childoffset = vector(0,0,0)
self.isDummy = False
axis = vector(*rootdata.axis)[::-1]
rotOrder = rootdata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.channels = rootdata.channels
def __init__(self, rootdata):
"""
Construct a new ASFRoot.
@param rootdata: The L{ParseResults} object representing the root
segment data from the ASF file.
"""
TreeNode.__init__(self, None)
self.name = 'root'
self.childoffset = vector(0, 0, 0)
self.isDummy = False
axis = vector(*rootdata.axis)[::-1]
rotOrder = rootdata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.channels = rootdata.channels
def geodesicQuaternionPath():
"""
Generate an array of quaternions following a geodesic path
Returns
-------
t : :class:`~numpy.ndarray`
timestamps of quaternions
quaternionPath : :class:`~imusim.maths.quaternions.QuaternionArray`
array of quaternions following a geodesic path
"""
t = np.arange(0,10,0.1)
q = Quaternion.fromEuler((45,0,0))
return t, QuaternionArray([q**T for T in t])
def __init__(self, parent, bonedata, scale):
"""
Consruct a new ASFBone.
@param parent: The parent bone of this bone.
@param bonedata: The L{ParseResults} object representing the bone
data from the ASF file.
"""
TreeNode.__init__(self, parent)
self.name = bonedata.name
axis = vector(*bonedata.axis)[::-1]
rotOrder = bonedata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.childoffset = self.rotationOffset.rotateVector(
vector(*bonedata.direction) * bonedata.length * scale)
self.isDummy = bonedata.channels is ''
self.channels = bonedata.channels
def __init__(self, parent, bonedata, scale):
"""
Consruct a new ASFBone.
@param parent: The parent bone of this bone.
@param bonedata: The L{ParseResults} object representing the bone
data from the ASF file.
"""
TreeNode.__init__(self, parent)
self.name = bonedata.name
axis = vector(*bonedata.axis)[::-1]
rotOrder = bonedata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.childoffset = self.rotationOffset.rotateVector(
vector(*bonedata.direction) * bonedata.length * scale)
self.isDummy = bonedata.channels is ''
self.channels = bonedata.channels
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
