def testVideoGeneration(): samp = loadBVHFile(os.path.join(os.path.dirname(__file__), '../system/walk.bvh'), 0.01) spl = SplinedBodyModel(samp) positionEstimator = ContactTrackingKalmanFilter( SampledBodyModel.structureCopy(samp), initialTime = spl.startTime, initialPosition = spl.position(spl.startTime), initialVelocity = spl.velocity(spl.startTime)) dt = 0.01 sampleTimes = np.arange(spl.startTime + dt, spl.endTime, dt) for t in sampleTimes: data = [{'jointName' : p.name, 'linearAcceleration' : p.acceleration(t)} for p in spl.joints] positionEstimator(data, t) outdir = tempfile.mkdtemp() filename = os.path.join(outdir, 'movie.avi') autoPositionCamera() renderSolenoidCoil( SolenoidMagneticField(200,20,0.05,0.2, AffineTransform())) createVideo(filename, [BodyModelRenderer(spl), VelocityVectorRenderer(spl.getPoint('ltoes_end')), ContactProbabilityRenderer(spl)], spl.startTime, spl.startTime + 1, 3, (320, 200)) assert os.path.exists(filename) shutil.rmtree(outdir)
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 _readHeader(self): ''' Read the header information from the BVH file. ''' assert self.bvhFile.tell() == 0, \ 'Must be at the start of file to read header' self._checkToken("HIERARCHY") self._checkToken("ROOT") self.model = SampledBodyModel() self.model.channels = [] self.jointStack = [] self.jointStack.append(self.model) self._readJoint() del self.jointStack self._checkToken("MOTION") self._checkToken("Frames:") self.frameCount = self._intToken() self._checkToken("Frame") self._checkToken("Time:") self.framePeriod = self._floatToken()
def checkAlgorithm(algorithm): sampledModel = SampledBodyModel.structureCopy(referenceModel) for t in sampleTimes: for referenceJoint, sampledJoint in zip(referenceModel.joints, sampledModel.joints): sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t)) positionEstimator = algorithm(sampledModel, initialTime = referenceModel.startTime, initialPosition = referenceModel.position(referenceModel.startTime), initialVelocity = referenceModel.velocity(referenceModel.startTime)) for t in sampleTimes[1:]: data = [{'jointName' : referenceModel.name, 'linearAcceleration' : referenceModel.acceleration(t)}] positionEstimator(data, t) result = sampledModel.positionKeyFrames.values assert np.shape(result) == referenceModel.position(sampleTimes).shape assert np.all(np.isfinite(result))
def checkAlgorithm(algorithm): sampledModel = SampledBodyModel.structureCopy(referenceModel) for t in sampleTimes: for referenceJoint, sampledJoint in zip(referenceModel.joints, sampledModel.joints): sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t)) positionEstimator = algorithm( sampledModel, initialTime=referenceModel.startTime, initialPosition=referenceModel.position(referenceModel.startTime), initialVelocity=referenceModel.velocity(referenceModel.startTime)) for t in sampleTimes[1:]: data = [{ 'jointName': referenceModel.name, 'linearAcceleration': referenceModel.acceleration(t) }] positionEstimator(data, t) result = sampledModel.positionKeyFrames.values assert np.shape(result) == referenceModel.position(sampleTimes).shape assert np.all(np.isfinite(result))
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 loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod): """ Load motion capture data from an ASF and AMC file pair. @param asfFileName: Name of the ASF file containing the description of the rigid body model @param amcFileName: Name of the AMC file containing the motion data @param scaleFactor: Scaling factor to convert lengths to m. For data from the CMU motion capture corpus this should be 2.54/100 to convert from inches to metres. @return: A {SampledBodyModel} representing the root of the rigid body model structure. """ with open(asfFileName, 'r') as asfFile: data = asfParser.parseFile(asfFile) scale = (1.0/data.units.get('length',1)) * scaleFactor bones = dict((bone.name,bone) for bone in data.bones) asfModel = ASFRoot(data.root) for entry in data.hierarchy: parent = asfModel.getBone(entry.parent) for childName in entry.children: ASFBone(parent, bones[childName], scale) imusimModel = SampledBodyModel('root') for subtree in asfModel.children: for bone in subtree: if not bone.isDummy: offset = vector(0,0,0) ancestors = bone.ascendTree() while True: ancestor = ancestors.next().parent offset += ancestor.childoffset if not ancestor.isDummy: break SampledJoint(parent=imusimModel.getJoint(ancestor.name), name=bone.name, offset=offset) if not bone.hasChildren: PointTrajectory( parent=imusimModel.getJoint(bone.name), name=bone.name+'_end', offset=bone.childoffset ) with open(amcFileName) as amcFile: motion = amcParser.parseFile(amcFile) t = 0 for frame in motion.frames: for bone in frame.bones: bonedata = asfModel.getBone(bone.name) if bone.name == 'root': data = dict((chan.lower(), v) for chan,v in zip(bonedata.channels,bone.channels)) position = convertCGtoNED(scale * vector(data['tx'], data['ty'],data['tz'])) imusimModel.positionKeyFrames.add(t, position) axes, angles = zip(*[(chan[-1], angle) for chan, angle in zip(bonedata.channels, bone.channels) if chan.lower().startswith('r')]) rotation = (bonedata.rotationOffset.conjugate * Quaternion.fromEuler(angles[::-1], axes[::-1])) joint = imusimModel.getJoint(bone.name) if joint.hasParent: parentRot = joint.parent.rotationKeyFrames.latestValue parentRotOffset = bonedata.parent.rotationOffset rotation = parentRot * parentRotOffset * rotation else: rotation = convertCGtoNED(rotation) joint.rotationKeyFrames.add(t, rotation) t += framePeriod return imusimModel
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)
def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod): """ Load motion capture data from an ASF and AMC file pair. @param asfFileName: Name of the ASF file containing the description of the rigid body model @param amcFileName: Name of the AMC file containing the motion data @param scaleFactor: Scaling factor to convert lengths to m. For data from the CMU motion capture corpus this should be 2.54/100 to convert from inches to metres. @return: A {SampledBodyModel} representing the root of the rigid body model structure. """ with open(asfFileName, 'r') as asfFile: data = asfParser.parseFile(asfFile) scale = (1.0 / data.units.get('length', 1)) * scaleFactor bones = dict((bone.name, bone) for bone in data.bones) asfModel = ASFRoot(data.root) for entry in data.hierarchy: parent = asfModel.getBone(entry.parent) for childName in entry.children: ASFBone(parent, bones[childName], scale) imusimModel = SampledBodyModel('root') for subtree in asfModel.children: for bone in subtree: if not bone.isDummy: offset = vector(0, 0, 0) ancestors = bone.ascendTree() while True: ancestor = ancestors.next().parent offset += ancestor.childoffset if not ancestor.isDummy: break SampledJoint(parent=imusimModel.getJoint(ancestor.name), name=bone.name, offset=offset) if not bone.hasChildren: PointTrajectory(parent=imusimModel.getJoint(bone.name), name=bone.name + '_end', offset=bone.childoffset) with open(amcFileName) as amcFile: motion = amcParser.parseFile(amcFile) t = 0 for frame in motion.frames: for bone in frame.bones: bonedata = asfModel.getBone(bone.name) if bone.name == 'root': data = dict((chan.lower(), v) for chan, v in zip( bonedata.channels, bone.channels)) position = convertCGtoNED( scale * vector(data['tx'], data['ty'], data['tz'])) imusimModel.positionKeyFrames.add(t, position) axes, angles = zip(*[(chan[-1], angle) for chan, angle in zip( bonedata.channels, bone.channels) if chan.lower().startswith('r')]) rotation = (bonedata.rotationOffset.conjugate * Quaternion.fromEuler(angles[::-1], axes[::-1])) joint = imusimModel.getJoint(bone.name) if joint.hasParent: parentRot = joint.parent.rotationKeyFrames.latestValue parentRotOffset = bonedata.parent.rotationOffset rotation = parentRot * parentRotOffset * rotation else: rotation = convertCGtoNED(rotation) joint.rotationKeyFrames.add(t, rotation) t += framePeriod return imusimModel
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