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 checkDynamicConvergence(filter, trajectory):
trueOrientations, estimatedOrientations = runOrientationFilter(filter,
trajectory)
assert_quaternions_correlated(trueOrientations[-100:],
estimatedOrientations[-100:])
def checkOrientationFilter(filter, trajectory):
trueOrientations, estimatedOrientations = runOrientationFilter(filter,
trajectory)
assert_quaternions_correlated(estimatedOrientations, trueOrientations)
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 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 checkDynamicConvergence(filter, trajectory):
trueOrientations, estimatedOrientations = runOrientationFilter(
filter, trajectory)
assert_quaternions_correlated(trueOrientations[-100:],
estimatedOrientations[-100:])
def checkOrientationFilter(filter, trajectory):
trueOrientations, estimatedOrientations = runOrientationFilter(
filter, trajectory)
assert_quaternions_correlated(estimatedOrientations, trueOrientations)
