Exemplo n.º 1
0
def testOrient3Model():
    env = Environment()
    calibrator = ScaleAndOffsetCalibrator(env, 1000, 1 / 100, 10)
    for i in range(3):
        imu = Orient3IMU()
        cal = calibrator.calibrate(imu)
        sim = Simulation(environment=env)
        traj = RandomTrajectory()
        imu.simulation = sim
        imu.trajectory = traj
        sim.time = imu.trajectory.startTime
        BasicIMUBehaviour(imu, 1 / 256, calibration=cal, initialTime=sim.time)
        sim.run(traj.endTime)
        t = imu.accelerometer.calibratedMeasurements.timestamps
        r = traj.rotation(t)
        a = r.rotateFrame(
            traj.acceleration(t) - env.gravitationalField(traj.position(t), t))
        m = r.rotateFrame(env.magneticField(traj.position(t), t))
        w = r.rotateFrame(traj.rotationalVelocity(t))
        g = env.gravitationalField.nominalMagnitude
        a_within_5g = abs(a) < 5 * g
        for i in range(3):
            yield assert_array_almost_equal, \
                np.array([a[i ,a_within_5g[i]]]), \
                np.array([imu.accelerometer.calibratedMeasurements(t)
                    [i, a_within_5g[i]]]), -2
        yield assert_array_almost_equal, m, \
                imu.magnetometer.calibratedMeasurements(t), 0
        yield assert_array_almost_equal, w, \
                imu.gyroscope.calibratedMeasurements(t), 0
Exemplo n.º 2
0
def testOrient3Model():
    env = Environment()
    calibrator = ScaleAndOffsetCalibrator(env, 1000, 1/100, 10)
    for i in range(3):
        imu = Orient3IMU()
        cal = calibrator.calibrate(imu)
        sim = Simulation(environment=env)
        traj = RandomTrajectory()
        imu.simulation = sim
        imu.trajectory = traj
        sim.time = imu.trajectory.startTime
        BasicIMUBehaviour(imu, 1/256, calibration=cal, initialTime=sim.time)
        sim.run(traj.endTime)
        t = imu.accelerometer.calibratedMeasurements.timestamps
        r = traj.rotation(t)
        a = r.rotateFrame(traj.acceleration(t) -
                env.gravitationalField(traj.position(t), t))
        m = r.rotateFrame(env.magneticField(traj.position(t), t))
        w = r.rotateFrame(traj.rotationalVelocity(t))
        g = env.gravitationalField.nominalMagnitude
        a_within_5g = abs(a) < 5*g
        for i in range(3):
            yield assert_array_almost_equal, \
                np.array([a[i ,a_within_5g[i]]]), \
                np.array([imu.accelerometer.calibratedMeasurements(t)
                    [i, a_within_5g[i]]]), -2
        yield assert_array_almost_equal, m, \
                imu.magnetometer.calibratedMeasurements(t), 0
        yield assert_array_almost_equal, w, \
                imu.gyroscope.calibratedMeasurements(t), 0
Exemplo n.º 3
0
def testBERRadioEnvironment():
    env = Environment(radioEnvironment=BERRadioEnvironment(1e-4, seed=0))

    sim = Simulation(environment=env)
    tx = TestPlatform(sim)
    rx = TestPlatform(sim)
    packet = TestPacket()

    for _ in range(1000):
        tx.sendPacket(packet)

    assert len(tx.packetsReceived) == 0
    assert 0 < len(rx.packetsReceived) < 1000
Exemplo n.º 4
0
    def __init__(self,
                 seed=None,
                 environment=None,
                 engine=SimPy.Simulation.Simulation):
        """
        Initialise simulation.

        @param seed: Seed value for L{np.random.RandomState}.
        @param environment: L{Environment} to simulate in.
        @param engine: L{SimPy.Simulation} subclass to drive simulation.
        """
        self.environment = Environment(
        ) if environment is None else environment
        self.engine = engine()
        self.engine.initialize()
        self.rng = np.random.RandomState(seed)
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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