def testASFInput():
dir = path.dirname(__file__)
asfFile = path.join(dir, "16.asf")
amcFile = path.join(dir, "16_15.amc")
bvhFile = path.join(dir, "16_15.bvh")
asfModel = loadASFFile(asfFile,
amcFile,
scaleFactor=2.54 / 100,
framePeriod=1.0 / 120)
bvhModel = loadBVHFile(bvhFile, 1.0 / 100)
assert set(j.name for j in asfModel) == set(j.name for j in bvhModel)
for asfNode in asfModel:
if asfNode == asfModel:
# skip the root as position offsets are defined in different ways
continue
bvhNode = bvhModel.getPoint(asfNode.name)
assert asfNode.parent.name == bvhNode.parent.name
assert_almost_equal(asfNode.positionOffset,
bvhNode.positionOffset,
err_msg='Wrong offset for joint %s' % asfNode.name,
decimal=6)
assert_almost_equal(asfModel.positionKeyFrames.values,
bvhModel.positionKeyFrames.values,
decimal=4,
err_msg="Model position trajectories do not match.")
for asfJoint in asfModel.joints:
bvhJoint = bvhModel.getJoint(asfJoint.name)
assertMaximumRMSErrorAngle(asfJoint.rotationKeyFrames.values,
bvhJoint.rotationKeyFrames.values,
maxRMSE=5,
errMsg="Joint %s" % asfJoint.name)
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 testASFInput():
dir = path.dirname(__file__)
asfFile = path.join(dir, "16.asf")
amcFile = path.join(dir, "16_15.amc")
bvhFile = path.join(dir, "16_15.bvh")
asfModel = loadASFFile(asfFile, amcFile, scaleFactor=2.54/100,
framePeriod=1.0/120)
bvhModel = loadBVHFile(bvhFile, 1.0/100)
assert set(j.name for j in asfModel) == set(j.name for j in bvhModel)
for asfNode in asfModel:
if asfNode == asfModel:
# skip the root as position offsets are defined in different ways
continue
bvhNode = bvhModel.getPoint(asfNode.name)
assert asfNode.parent.name == bvhNode.parent.name
assert_almost_equal(asfNode.positionOffset, bvhNode.positionOffset,
err_msg='Wrong offset for joint %s'%asfNode.name, decimal=6)
assert_almost_equal(asfModel.positionKeyFrames.values,
bvhModel.positionKeyFrames.values, decimal=4,
err_msg="Model position trajectories do not match.")
for asfJoint in asfModel.joints:
bvhJoint = bvhModel.getJoint(asfJoint.name)
assertMaximumRMSErrorAngle(asfJoint.rotationKeyFrames.values,
bvhJoint.rotationKeyFrames.values, maxRMSE=5,
errMsg="Joint %s"%asfJoint.name)
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 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)
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim. If not, see <http://www.gnu.org/licenses/>.
from os import path
from imusim.io.bvh import loadBVHFile
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.algorithms import position
from imusim.testing.inspection import getImplementations
import numpy as np
referenceModel = SplinedBodyModel(
loadBVHFile(path.join(path.dirname(__file__), 'test.bvh'), 0.01))
dt = 0.01
sampleTimes = np.arange(referenceModel.startTime, referenceModel.endTime, dt)
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,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim. If not, see <http://www.gnu.org/licenses/>.
from os import path
from imusim.io.bvh import loadBVHFile
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.algorithms import position
from imusim.testing.inspection import getImplementations
import numpy as np
referenceModel = SplinedBodyModel(
loadBVHFile(path.join(path.dirname(__file__), 'test.bvh'), 0.01))
dt = 0.01
sampleTimes = np.arange(referenceModel.startTime, referenceModel.endTime, dt)
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))
# 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 setupIMU(id, joint):
# Set up an ideal IMU with the trajectory of the supplied joint, which samples at samplingPeriod.
imu = IdealIMU()
imu.simulation = sim
imu.trajectory = joint
def handleSample(behaviour):
pass
behaviour = BasicIMUBehaviour(imu, samplingPeriod, sampleCallback=handleSample, initialTime=sim.time)
