def testVideoGeneration():
samp = loadBVHFile(os.path.join(os.path.dirname(__file__),
'../system/walk.bvh'), 0.01)
spl = SplinedBodyModel(samp)
positionEstimator = ContactTrackingKalmanFilter(
SampledBodyModel.structureCopy(samp),
initialTime = spl.startTime,
initialPosition = spl.position(spl.startTime),
initialVelocity = spl.velocity(spl.startTime))
dt = 0.01
sampleTimes = np.arange(spl.startTime + dt, spl.endTime, dt)
for t in sampleTimes:
data = [{'jointName' : p.name,
'linearAcceleration' : p.acceleration(t)}
for p in spl.joints]
positionEstimator(data, t)
outdir = tempfile.mkdtemp()
filename = os.path.join(outdir, 'movie.avi')
autoPositionCamera()
renderSolenoidCoil(
SolenoidMagneticField(200,20,0.05,0.2, AffineTransform()))
createVideo(filename, [BodyModelRenderer(spl),
VelocityVectorRenderer(spl.getPoint('ltoes_end')),
ContactProbabilityRenderer(spl)],
spl.startTime, spl.startTime + 1, 3, (320, 200))
assert os.path.exists(filename)
shutil.rmtree(outdir)
def checkSystemSimulation(filterClass):
sim = Simulation()
env = sim.environment
samplingPeriod = 1.0 / 100
calibrator = ScaleAndOffsetCalibrator(env, 1000, samplingPeriod, 20)
simModel = SplinedBodyModel(loadBVHFile(testMotion, conversionFactor=0.01))
sim.time = simModel.startTime
slotTime = 0.001
schedule = Schedule(slotTime, slotTime, range(len(list(simModel.joints))))
def setupIMU(id, joint):
platform = MagicIMU()
calibration = calibrator.calibrate(platform)
platform.simulation = sim
platform.trajectory = joint
filter = filterClass(
initialRotation=joint.rotation(simModel.startTime),
initialTime=sim.time,
initialRotationalVelocity=joint.rotation(
simModel.startTime).rotateFrame(
joint.rotationalVelocity(simModel.startTime)),
gravFieldReference=env.gravitationalField.nominalValue,
magFieldReference=env.magneticField.nominalValue,
**filterParameters.get(filterClass, {}))
def handleSample(behaviour):
rotation = filter.rotation.latestValue
packet = DataPacket(id, [('rotation', rotation)])
behaviour.mac.queuePacket(packet)
behaviour = BasicIMUBehaviour(platform,
samplingPeriod,
calibration,
filter,
handleSample,
initialTime=sim.time)
behaviour.mac = SlaveMAC(platform.radio, behaviour.timerMux, schedule,
id)
return behaviour
imus = [setupIMU(i, joint) for i, joint in enumerate(simModel.joints)]
basestation = IdealBasestation(sim, StaticTrajectory())
recModel = SampledBodyModel.structureCopy(simModel)
reconstructor = BodyModelReconstructor(recModel, initialTime=sim.time)
def handleFrame(packets):
for packet in packets:
packet['jointName'] = recModel.jointNames[packet.source]
reconstructor.handleData(packets, schedule.framePeriod)
MasterMAC(basestation.radio, TimerMultiplexer(basestation.timer), schedule,
handleFrame)
sim.run(simModel.endTime)
for simJoint, recJoint in zip(simModel.joints, recModel.joints):
times = simJoint.rotationKeyFrames.timestamps
assert_quaternions_correlated(simJoint.rotation(times),
recJoint.rotation(times), 0.9)
def checkSystemSimulation(filterClass):
sim = Simulation()
env = sim.environment
samplingPeriod = 1.0/100
calibrator = ScaleAndOffsetCalibrator(env, 1000, samplingPeriod, 20)
simModel = SplinedBodyModel(loadBVHFile(testMotion, conversionFactor=0.01))
sim.time = simModel.startTime
slotTime = 0.001
schedule = Schedule(slotTime, slotTime, range(len(list(simModel.joints))))
def setupIMU(id, joint):
platform = MagicIMU()
calibration = calibrator.calibrate(platform)
platform.simulation = sim
platform.trajectory = joint
filter = filterClass(initialRotation=joint.rotation(simModel.startTime),
initialTime=sim.time,
initialRotationalVelocity=joint.rotation(simModel.startTime)
.rotateFrame(joint.rotationalVelocity(simModel.startTime)),
gravFieldReference=env.gravitationalField.nominalValue,
magFieldReference=env.magneticField.nominalValue,
**filterParameters.get(filterClass, {}))
def handleSample(behaviour):
rotation = filter.rotation.latestValue
packet = DataPacket(id, [('rotation', rotation)])
behaviour.mac.queuePacket(packet)
behaviour = BasicIMUBehaviour(platform, samplingPeriod, calibration,
filter, handleSample, initialTime=sim.time)
behaviour.mac = SlaveMAC(platform.radio, behaviour.timerMux,
schedule, id)
return behaviour
imus = [setupIMU(i, joint) for i, joint in enumerate(simModel.joints)]
basestation = IdealBasestation(sim, StaticTrajectory())
recModel = SampledBodyModel.structureCopy(simModel)
reconstructor = BodyModelReconstructor(recModel, initialTime=sim.time)
def handleFrame(packets):
for packet in packets:
packet['jointName'] = recModel.jointNames[packet.source]
reconstructor.handleData(packets, schedule.framePeriod)
MasterMAC(basestation.radio, TimerMultiplexer(basestation.timer),
schedule, handleFrame)
sim.run(simModel.endTime)
for simJoint, recJoint in zip(simModel.joints, recModel.joints):
times = simJoint.rotationKeyFrames.timestamps
assert_quaternions_correlated(simJoint.rotation(times),
recJoint.rotation(times), 0.9)
def _readHeader(self):
'''
Read the header information from the BVH file.
'''
assert self.bvhFile.tell() == 0, \
'Must be at the start of file to read header'
self._checkToken("HIERARCHY")
self._checkToken("ROOT")
self.model = SampledBodyModel()
self.model.channels = []
self.jointStack = []
self.jointStack.append(self.model)
self._readJoint()
del self.jointStack
self._checkToken("MOTION")
self._checkToken("Frames:")
self.frameCount = self._intToken()
self._checkToken("Frame")
self._checkToken("Time:")
self.framePeriod = self._floatToken()
def checkAlgorithm(algorithm):
sampledModel = SampledBodyModel.structureCopy(referenceModel)
for t in sampleTimes:
for referenceJoint, sampledJoint in zip(referenceModel.joints,
sampledModel.joints):
sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t))
positionEstimator = algorithm(sampledModel,
initialTime = referenceModel.startTime,
initialPosition = referenceModel.position(referenceModel.startTime),
initialVelocity = referenceModel.velocity(referenceModel.startTime))
for t in sampleTimes[1:]:
data = [{'jointName' : referenceModel.name,
'linearAcceleration' : referenceModel.acceleration(t)}]
positionEstimator(data, t)
result = sampledModel.positionKeyFrames.values
assert np.shape(result) == referenceModel.position(sampleTimes).shape
assert np.all(np.isfinite(result))
def checkAlgorithm(algorithm):
sampledModel = SampledBodyModel.structureCopy(referenceModel)
for t in sampleTimes:
for referenceJoint, sampledJoint in zip(referenceModel.joints,
sampledModel.joints):
sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t))
positionEstimator = algorithm(
sampledModel,
initialTime=referenceModel.startTime,
initialPosition=referenceModel.position(referenceModel.startTime),
initialVelocity=referenceModel.velocity(referenceModel.startTime))
for t in sampleTimes[1:]:
data = [{
'jointName': referenceModel.name,
'linearAcceleration': referenceModel.acceleration(t)
}]
positionEstimator(data, t)
result = sampledModel.positionKeyFrames.values
assert np.shape(result) == referenceModel.position(sampleTimes).shape
assert np.all(np.isfinite(result))
def testDistLinAccSimulation():
sim = Simulation()
samplingPeriod = 1.0 / 100
calibrator = ScaleAndOffsetCalibrator(sim.environment, 1000, samplingPeriod, 20)
bvhFile = path.join(path.dirname(__file__), "walk.bvh")
sampledModel = loadBVHFile(bvhFile, conversionFactor=0.01)
posTimes = sampledModel.positionKeyFrames.timestamps
sampledModel.positionKeyFrames = TimeSeries(posTimes, np.zeros((3, len(posTimes))))
simModel = SplinedBodyModel(sampledModel)
joints = list(simModel.joints)
sim.time = simModel.startTime
k = 128
slotTime = 0.001
txSlots = range(2, len(joints)) + [0, 1]
auxRxSlots = range(1, len(joints))
auxTxSlots = [joints.index(j.parent) for j in joints[1:]]
schedule = InterSlaveSchedule(slotTime, slotTime, txSlots, auxTxSlots, auxRxSlots)
def setupIMU(id, joint):
offset = randomPosition((-0.1, 0.1))
platform = IdealIMU()
calibration = calibrator.calibrate(platform)
platform.simulation = sim
platform.trajectory = OffsetTrajectory(joint, offset)
filter = DistLinAccelCF(samplingPeriod, platform.trajectory.rotation(simModel.startTime), k, joint, offset)
def updateChildren():
for child in filter.children:
childID = joints.index(child)
childAccel = filter.childAcceleration(child.positionOffset, samplingPeriod)
if childAccel is not None:
auxPacket = AuxPacket(id, childID, [("linearAcceleration", childAccel)])
mac.queueAuxPacket(auxPacket)
def handlePacket(packet):
filter.handleLinearAcceleration(packet["linearAcceleration"], samplingPeriod)
updateChildren()
def handleSample(behaviour):
rotation = filter.rotation.latestValue
packet = DataPacket(id, [("rotation", rotation)])
mac.queuePacket(packet)
if not filter.joint.hasParent:
updateChildren()
behaviour = BasicIMUBehaviour(platform, samplingPeriod, calibration, filter, handleSample, initialTime=sim.time)
mac = InterSlaveMAC(platform.radio, behaviour.timerMux, schedule, id, handlePacket)
imus = [setupIMU(i, joint) for i, joint in enumerate(joints)]
basestation = IdealBasestation(sim, StaticTrajectory())
recModel = SampledBodyModel.structureCopy(simModel)
reconstructor = BodyModelReconstructor(recModel, initialTime=sim.time)
def handleFrame(packets):
for packet in packets:
packet["jointName"] = recModel.jointNames[packet.source]
reconstructor.handleData(packets, schedule.framePeriod)
basestation.radio.channel = schedule.dataChannel
MasterMAC(basestation.radio, TimerMultiplexer(basestation.timer), schedule, handleFrame)
sim.run(simModel.endTime)
for simJoint, recJoint in zip(joints, recModel.joints):
times = simJoint.rotationKeyFrames.timestamps
assert_quaternions_correlated(simJoint.rotation(times), recJoint.rotation(times), 0.85)
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 testAgainstReality():
dir = path.dirname(__file__)
filebase = path.join(dir, "swing")
refbase = path.join(dir, "stand")
magbases = [path.join(dir, f) for f in ['magsweep1', 'magsweep2']]
maglookup = {'Upper Leg IMU': '66', 'Orient 8': '8', 'Orient 43': '43'}
magSamples = 2000
refTime = 1.0
posStdDev = 0.0005
rotStdDev = 0.004
ref3D = SplinedMarkerCapture(loadQualisysTSVFile(refbase + "_3D.tsv"),
positionStdDev=posStdDev)
ref6D = SplinedMarkerCapture(loadQualisysTSVFile(refbase + "_6D.tsv"),
rotationStdDev=rotStdDev)
capture3D = SplinedMarkerCapture(loadQualisysTSVFile(filebase + "_3D.tsv"),
positionStdDev=posStdDev)
captureSD = SensorDataCapture.load(filebase + ".sdc")
hip, thigh, knee, shin, ankle = \
['Hip', 'Thigh', 'Knee Hinge', 'Shin', 'Ankle']
jointNames = ['Upper Leg', 'Lower Leg', 'Foot']
jointAbbrevs = ['femur', 'tibia', 'foot']
orientIDs = ['66', '43', '8']
jointMarkerNames = [hip, knee, ankle]
refMarkerNames = [[thigh, knee], [shin, ankle], []]
imuMarkerNames = \
[[j + ' IMU - ' + str(i) for i in range(1,4)] for j in jointNames]
jointMarkerSets = lambda c: [
list(map(c.marker, jointMarkerNames)),
[list(map(c.marker, r)) for r in refMarkerNames],
[list(map(c.marker, i)) for i in imuMarkerNames]
]
imuMarkerSets = lambda c: [[
c.marker(i[0]) for i in imuMarkerNames
], [list(map(c.marker, i[1:])) for i in imuMarkerNames]]
jointRefTrajectories = [
MultiMarkerTrajectory(j, r + i, refTime=refTime)
for j, r, i in zip(*(jointMarkerSets(ref3D)))
]
jointTrajectories = [
MultiMarkerTrajectory(j, r + i, refVectors=m.refVectors) \
for j, r, i, m in \
zip(*(jointMarkerSets(capture3D) + [jointRefTrajectories]))]
imuRefTrajectories = [
MultiMarkerTrajectory(p, r, refTime=refTime)
for p, r in zip(*(imuMarkerSets(ref3D)))
]
imuVecTrajectories = [
MultiMarkerTrajectory(p, r, refVectors=m.refVectors)
for p, r, m in zip(*(imuMarkerSets(capture3D) + [imuRefTrajectories]))
]
imuRefMarkers = [ref6D.marker(j + ' IMU') for j in jointNames]
imuOffsets = [
i.position(refTime) - j.position(refTime)
for i, j in zip(imuRefTrajectories, jointRefTrajectories)
]
imuRotations = [
t.rotation(refTime).conjugate * m.rotation(refTime)
for t, m in zip(imuRefTrajectories, imuRefMarkers)
]
imuTrajectories = [
OffsetTrajectory(v, o, r)
for v, o, r in zip(imuVecTrajectories, imuOffsets, imuRotations)
]
imuData = [captureSD.device(i) for i in orientIDs]
joints = []
for i in range(len(jointNames)):
name = jointNames[i]
traj = jointTrajectories[i]
if i == 0:
model = SampledBodyModel(name)
model.positionKeyFrames = traj.posMarker.positionKeyFrames
joint = model
else:
parent = joints[-1]
refTraj = jointRefTrajectories[i]
parentRefTraj = jointRefTrajectories[i - 1]
pos = refTraj.position(refTime)
parentPos = parentRefTraj.position(refTime)
joint = SampledJoint(joints[-1], name, offset=(pos - parentPos))
joint.rotationKeyFrames = traj.rotationKeyFrames
joints.append(joint)
model = SplinedBodyModel(model)
joints = model.joints
imuJointTrajectories = [
OffsetTrajectory(j, o, r)
for j, o, r in zip(joints, imuOffsets, imuRotations)
]
positionSets = []
valueSets = []
for magbase in magbases:
orient = SensorDataCapture.load(magbase + '.sdc')
optical = loadQualisysTSVFile(magbase + '_6D.tsv')
for marker in optical.markers:
device = orient.device(maglookup[marker.id])
magData = device.sensorData('magnetometer').values
positionSets.append(marker.positionKeyFrames.values)
valueSets.append(
marker.rotationKeyFrames.values.rotateVector(magData))
positions = np.hstack(positionSets)
values = np.hstack(valueSets)
valid = ~np.any(np.isnan(positions), axis=0) & ~np.any(np.isnan(values),
axis=0)
dev = values - np.median(values[:, valid], axis=1).reshape((3, 1))
step = np.shape(values[:, valid])[1] // magSamples
posSamples = positions[:, valid][:, ::step]
valSamples = values[:, valid][:, ::step]
environment = Environment()
environment.magneticField = \
NaturalNeighbourInterpolatedField(posSamples, valSamples)
sim = Simulation(environment=environment)
sim.time = model.startTime
distortIMUs = []
dt = 1 / capture3D.sampled.frameRate
for traj in imuJointTrajectories:
platform = IdealIMU(sim, traj)
distortIMUs.append(BasicIMUBehaviour(platform, dt))
sim.run(model.endTime)
for imu in range(3):
for sensorName in ['accelerometer', 'magnetometer', 'gyroscope']:
sim = getattr(distortIMUs[imu].imu, sensorName).rawMeasurements
true = imuData[imu].sensorData(sensorName)(sim.timestamps +
model.startTime)
yield assert_vectors_correlated, sim.values, true, 0.8
def testDistLinAccSimulation():
sim = Simulation()
samplingPeriod = 1.0/100
calibrator = ScaleAndOffsetCalibrator(sim.environment, 1000,
samplingPeriod, 20)
bvhFile = path.join(path.dirname(__file__), 'walk.bvh')
sampledModel = loadBVHFile(bvhFile, conversionFactor=0.01)
posTimes = sampledModel.positionKeyFrames.timestamps
sampledModel.positionKeyFrames = TimeSeries(
posTimes, np.zeros((3,len(posTimes))))
simModel = SplinedBodyModel(sampledModel)
joints = list(simModel.joints)
sim.time = simModel.startTime
k = 128
slotTime = 0.001
txSlots = list(range(2, len(joints))) + [0,1]
auxRxSlots = range(1, len(joints))
auxTxSlots = [joints.index(j.parent) for j in joints[1:]]
schedule = InterSlaveSchedule(slotTime, slotTime, txSlots,
auxTxSlots, auxRxSlots)
def setupIMU(id, joint):
offset = randomPosition((-0.1, 0.1))
platform = IdealIMU()
calibration = calibrator.calibrate(platform)
platform.simulation = sim
platform.trajectory = OffsetTrajectory(joint, offset)
filter = DistLinAccelCF(samplingPeriod,
platform.trajectory.rotation(simModel.startTime),
k, joint, offset)
def updateChildren():
for child in filter.children:
childID = joints.index(child)
childAccel = filter.childAcceleration(child.positionOffset, samplingPeriod)
if childAccel is not None:
auxPacket = AuxPacket(id, childID,
[('linearAcceleration', childAccel)])
mac.queueAuxPacket(auxPacket)
def handlePacket(packet):
filter.handleLinearAcceleration(packet['linearAcceleration'], samplingPeriod)
updateChildren()
def handleSample(behaviour):
rotation = filter.rotation.latestValue
packet = DataPacket(id, [('rotation', rotation)])
mac.queuePacket(packet)
if not filter.joint.hasParent:
updateChildren()
behaviour = BasicIMUBehaviour(platform, samplingPeriod, calibration,
filter, handleSample, initialTime=sim.time)
mac = InterSlaveMAC(platform.radio, behaviour.timerMux, schedule,
id, handlePacket)
imus = [setupIMU(i, joint) for i, joint in enumerate(joints)]
basestation = IdealBasestation(sim, StaticTrajectory())
recModel = SampledBodyModel.structureCopy(simModel)
reconstructor = BodyModelReconstructor(recModel, initialTime=sim.time)
def handleFrame(packets):
for packet in packets:
packet['jointName'] = recModel.jointNames[packet.source]
reconstructor.handleData(packets, schedule.framePeriod)
basestation.radio.channel = schedule.dataChannel
MasterMAC(basestation.radio, TimerMultiplexer(basestation.timer),
schedule, handleFrame)
sim.run(simModel.endTime)
for simJoint, recJoint in zip(joints, recModel.joints):
times = simJoint.rotationKeyFrames.timestamps
assert_quaternions_correlated(simJoint.rotation(times),
recJoint.rotation(times), 0.85)
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 testAgainstReality():
dir = path.dirname(__file__)
filebase = path.join(dir, "swing")
refbase = path.join(dir, "stand")
magbases = [path.join(dir, f) for f in ['magsweep1', 'magsweep2']]
maglookup = {
'Upper Leg IMU' : '66',
'Orient 8' : '8',
'Orient 43': '43'}
magSamples = 2000
refTime = 1.0
posStdDev = 0.0005
rotStdDev = 0.004
ref3D = SplinedMarkerCapture(
loadQualisysTSVFile(refbase + "_3D.tsv"), positionStdDev=posStdDev)
ref6D = SplinedMarkerCapture(
loadQualisysTSVFile(refbase + "_6D.tsv"), rotationStdDev=rotStdDev)
capture3D = SplinedMarkerCapture(
loadQualisysTSVFile(filebase + "_3D.tsv"), positionStdDev=posStdDev)
captureSD = SensorDataCapture.load(filebase + ".sdc")
hip, thigh, knee, shin, ankle = \
['Hip', 'Thigh', 'Knee Hinge', 'Shin', 'Ankle']
jointNames = ['Upper Leg', 'Lower Leg', 'Foot']
jointAbbrevs = ['femur', 'tibia', 'foot']
orientIDs = ['66', '43', '8']
jointMarkerNames = [hip, knee, ankle]
refMarkerNames = [[thigh, knee], [shin, ankle], []]
imuMarkerNames = \
[[j + ' IMU - ' + str(i) for i in range(1,4)] for j in jointNames]
jointMarkerSets = lambda c: [
map(c.marker, jointMarkerNames),
[map(c.marker, r) for r in refMarkerNames],
[map(c.marker, i) for i in imuMarkerNames]]
imuMarkerSets = lambda c: [
[c.marker(i[0]) for i in imuMarkerNames],
[map(c.marker,i[1:]) for i in imuMarkerNames]]
jointRefTrajectories = [MultiMarkerTrajectory(j, r + i, refTime=refTime)
for j, r, i in zip(*(jointMarkerSets(ref3D)))]
jointTrajectories = [
MultiMarkerTrajectory(j, r + i, refVectors=m.refVectors) \
for j, r, i, m in \
zip(*(jointMarkerSets(capture3D) + [jointRefTrajectories]))]
imuRefTrajectories = [MultiMarkerTrajectory(p, r, refTime=refTime)
for p, r in zip(*(imuMarkerSets(ref3D)))]
imuVecTrajectories = [MultiMarkerTrajectory(p, r, refVectors=m.refVectors)
for p, r, m in zip(*(imuMarkerSets(capture3D) + [imuRefTrajectories]))]
imuRefMarkers = [ref6D.marker(j + ' IMU') for j in jointNames]
imuOffsets = [i.position(refTime) - j.position(refTime)
for i, j in zip(imuRefTrajectories, jointRefTrajectories)]
imuRotations = [t.rotation(refTime).conjugate * m.rotation(refTime)
for t, m in zip(imuRefTrajectories, imuRefMarkers)]
imuTrajectories = [OffsetTrajectory(v, o, r)
for v, o, r in zip(imuVecTrajectories, imuOffsets, imuRotations)]
imuData = [captureSD.device(i) for i in orientIDs]
joints = []
for i in range(len(jointNames)):
name = jointNames[i]
traj = jointTrajectories[i]
if i == 0:
model = SampledBodyModel(name)
model.positionKeyFrames = traj.posMarker.positionKeyFrames
joint = model
else:
parent = joints[-1]
refTraj = jointRefTrajectories[i]
parentRefTraj = jointRefTrajectories[i - 1]
pos = refTraj.position(refTime)
parentPos = parentRefTraj.position(refTime)
joint = SampledJoint(joints[-1],name, offset=(pos - parentPos))
joint.rotationKeyFrames = traj.rotationKeyFrames
joints.append(joint)
model = SplinedBodyModel(model)
joints = model.joints
imuJointTrajectories = [OffsetTrajectory(j, o, r)
for j, o, r in zip(joints, imuOffsets, imuRotations)]
positionSets = []
valueSets = []
for magbase in magbases:
orient = SensorDataCapture.load(magbase + '.sdc')
optical = loadQualisysTSVFile(magbase + '_6D.tsv')
for marker in optical.markers:
device = orient.device(maglookup[marker.id])
magData = device.sensorData('magnetometer').values
positionSets.append(marker.positionKeyFrames.values)
valueSets.append(
marker.rotationKeyFrames.values.rotateVector(magData))
positions = np.hstack(positionSets)
values = np.hstack(valueSets)
valid = ~np.any(np.isnan(positions),axis=0) & ~np.any(np.isnan(values),axis=0)
dev = values - np.median(values[:,valid],axis=1).reshape((3,1))
step = np.shape(values[:,valid])[1] / magSamples
posSamples = positions[:,valid][:,::step]
valSamples = values[:,valid][:,::step]
environment = Environment()
environment.magneticField = \
NaturalNeighbourInterpolatedField(posSamples, valSamples)
sim = Simulation(environment=environment)
sim.time = model.startTime
distortIMUs = []
dt = 1/capture3D.sampled.frameRate
for traj in imuJointTrajectories:
platform = IdealIMU(sim, traj)
distortIMUs.append(BasicIMUBehaviour(platform, dt))
sim.run(model.endTime)
for imu in range(3):
for sensorName in ['accelerometer', 'magnetometer', 'gyroscope']:
sim = getattr(distortIMUs[imu].imu,sensorName).rawMeasurements
true = imuData[imu].sensorData(sensorName)(sim.timestamps + model.startTime)
yield assert_vectors_correlated, sim.values, true, 0.8
