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)