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
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
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
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)
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 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