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 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 setUp(self):
self.ds = Dataset.from_file('data/example_dataset.h5')
environment = SceneEnvironment(dataset=self.ds)
sim = Simulation(environment=environment)
trajectory = self.ds.trajectory
camera_model = PinholeModel(CAMERA_MATRIX, (1920, 1080), 1. / 35, 30.0)
camera = CameraPlatform(camera_model,
simulation=sim,
trajectory=trajectory)
camera_behaviour = BasicCameraBehaviour(camera, trajectory.endTime)
imu = IdealIMU(simulation=sim, trajectory=trajectory)
imu_behaviour = BasicIMUBehaviour(imu, 1. / 100) # 100Hz sample rate
sim.time = trajectory.startTime
self.simulation = sim
self.camera = camera
def testIdealRadioEnvironment():
sim = Simulation()
tx = TestPlatform(sim)
rx = TestPlatform(sim)
packet = TestPacket()
tx.sendPacket(packet)
assert len(tx.packetsReceived) == 0
assert len(rx.packetsReceived) == 1
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 calibrate(self, imu):
calibration = {}
position = vectors.vector(0, 0, 0)
expected = {}
measured = {}
for sensor in imu.sensors:
expected[sensor] = []
measured[sensor] = []
for trajectory in self.testTrajectories():
sim = Simulation(environment=self.environment)
sim.time = 0
imu.simulation = sim
imu.trajectory = trajectory
BasicIMUBehaviour(imu, self.samplingPeriod)
sim.run(self.samplingPeriod * self.samples, printProgress=False)
t = sensor.rawMeasurements.timestamps
for sensor in imu.sensors:
expected[sensor].append(sensor.trueValues(t))
measured[sensor].append(sensor.rawMeasurements.values)
for sensor in imu.sensors:
calibration[sensor] = ScaleAndOffsetCalibration.fromMeasurements(
np.hstack(expected[sensor]), np.hstack(measured[sensor]))
return calibration
def calibrate(self, imu):
calibration = {}
position = vectors.vector(0,0,0)
expected = {}
measured = {}
for sensor in imu.sensors:
expected[sensor] = []
measured[sensor] = []
for trajectory in self.testTrajectories():
sim = Simulation(environment=self.environment)
sim.time = 0
imu.simulation = sim
imu.trajectory = trajectory
BasicIMUBehaviour(imu, self.samplingPeriod)
sim.run(self.samplingPeriod * self.samples,
printProgress=False)
t = sensor.rawMeasurements.timestamps
for sensor in imu.sensors:
expected[sensor].append(sensor.trueValues(t))
measured[sensor].append(sensor.rawMeasurements.values)
for sensor in imu.sensors:
calibration[sensor] = ScaleAndOffsetCalibration.fromMeasurements(
np.hstack(expected[sensor]), np.hstack(measured[sensor]))
return calibration
def _setup(self):
# Trajectory used by the simulation
# Not the same as the dataset to account for the relative pose
# between camera and IMU
self.simulation_trajectory = transform_trajectory(
self.config.dataset.trajectory, self.config.Rci, self.config.pci)
from numpy.testing import assert_equal
assert_equal(self.simulation_trajectory.startTime,
self.config.dataset.trajectory.startTime)
assert_equal(self.simulation_trajectory.endTime,
self.config.dataset.trajectory.endTime)
self.environment = SceneEnvironment(self.config.dataset)
self.simulation = Simulation(environment=self.environment)
# Configure camera
self.camera = CameraPlatform(self.config.camera_model,
self.config.Rci,
self.config.pci,
simulation=self.simulation,
trajectory=self.simulation_trajectory)
self.camera_behaviour = BasicCameraBehaviour(self.camera,
self.config.end_time)
# Configure IMU
imu_conf = self.config.imu_config
assert imu_conf['type'] == 'DefaultIMU'
self.imu = DefaultIMU(imu_conf['accelerometer']['bias'],
imu_conf['accelerometer']['noise'],
imu_conf['gyroscope']['bias'],
imu_conf['gyroscope']['noise'],
simulation=self.simulation,
trajectory=self.simulation_trajectory)
imu_dt = 1. / self.config.imu_config['sample_rate']
self.imu_behaviour = BasicIMUBehaviour(
self.imu, imu_dt, initialTime=self.config.start_time)
import matplotlib.pyplot as plt from imusim.behaviours.imu import BasicIMUBehaviour from imusim.platforms.imus import IdealIMU from imusim.simulation.base import Simulation from imusim.testing.random_data import RandomTrajectory from rsimusim.camera import * from rsimusim_legacy.world import * # Create environment that contains landmarks num_landmarks = 50 world_points = np.random.uniform(-100, 100, size=(3, num_landmarks)) world = NonBlockableWorld(world_points) world_environment = WorldEnvironment(world) sim = Simulation(environment=world_environment) trajectory = RandomTrajectory() # Create camera platform camera_model = PinholeModel(np.eye(3), (1920, 1080), 1. / 35, 30.0) camera = CameraPlatform(camera_model, simulation=sim, trajectory=trajectory) # IMU platform imu = IdealIMU(simulation=sim, trajectory=trajectory) # Set up a behaviour that runs on the # simulated Camera behaviour_camera = DefaultCameraBehaviour(camera) # Set up a behaviour that runs on the # simulated IMU
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)
from imusim.simulation.base import Simulation
from imusim.io.bvh import loadBVHFile
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.platforms.imus import IdealIMU
from imusim.behaviours.imu import BasicIMUBehaviour
from imusim.algorithms.orientation import OrientCF
from imusim.visualisation.plotting import plot
from imusim.maths.quaternions import QuaternionArray
from pylab import show, array, arccos, dot, empty, figure, shape, transpose, l2norm, degrees, mean, title, xlabel, ylabel, legend
from numpy.random import random_sample
# Create the simulation, load a splined body model of walking from existing mocap data that can be
# evaluated at any time.
sim = Simulation()
env = sim.environment
samplingPeriod = 1.0/800
oversampling = 16
# worst case for offsets is probably +/-1
accel_offset = 0
gyro_offset = 0
mag_offset = 0
probDrippedPacket = 0.0
transmissionPeriod = samplingPeriod * oversampling
print "transmission rate: ", 1 / transmissionPeriod, " Hz"
simModel = SplinedBodyModel(loadBVHFile('walk.bvh', conversionFactor=0.01))
sim.time = simModel.startTime
def angle(a,b):
return arccos(dot(a,b) / (l2norm(a) * l2norm(b)))
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
#!/usr/bin/env python import time import numpy as np #from IPython import embed # Import all public symbols from IMUSim from imusim.behaviours.imu import BasicIMUBehaviour from imusim.trajectories.base import StaticTrajectory from imusim.simulation.base import Simulation from rsimusim.mpu9250 import MPU9250IMU sim = Simulation() trajectory = StaticTrajectory() #wconst = np.array([0, 1, 0]).reshape(3,1) #trajectory = StaticContinuousRotationTrajectory(wconst) imu = MPU9250IMU(simulation=sim, trajectory=trajectory) GYRO_SAMPLE_RATE = 1000. dt = 1. / GYRO_SAMPLE_RATE # Set up a behaviour that runs on the # simulated IMU behaviour1 = BasicIMUBehaviour(imu=imu, samplingPeriod=dt) # Set the time inside the simulation sim.time = trajectory.startTime # Run the simulation till the desired # end time
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)
