def testTrajectories(self):
position = vectors.vector(0,0,0)
# Accelerometer test trajectories
for angles in [
(0,-90,0),(0,90,0), # X down, X up (pitch -/+90 deg)
(0,0,90),(0,0,-90), # Y down, Y up (roll +/-90 deg)
(0,0,0),(0,0,180)]: # Z down, Z up (roll 0/180 deg)
rotation = Quaternion().setFromEuler(angles)
yield StaticTrajectory(rotation=rotation)
# Magnetometer test trajectories
field = self.environment.magneticField(position, 0)
N = field / vectors.norm(field)
E = vectors.vector(-N[1,0], N[0,0], 0)
E /= vectors.norm(E)
D = vectors.cross(N, E)
for vectorset in [
(N,E,D), (-N,E,-D), # X north, X south
(E,-N,D), (-E,N,D), # Y north, Y south
(D,E,-N), (-D,E,N)]: # Z north, Z south
rotation = Quaternion().setFromVectors(*vectorset)
yield StaticTrajectory(rotation=rotation)
# Gyro test trajectories
for axis in range(3):
omega = vectors.vector(0,0,0)
omega[axis] = self.rotationalVelocity
yield StaticContinuousRotationTrajectory(omega)
yield StaticContinuousRotationTrajectory(-omega)
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 testStaticTrajectory():
p = randomPosition()
r = randomRotation()
s = StaticTrajectory(p, r)
assert_equal(s.position(0), p)
assert_equal(s.velocity(0), vector(0,0,0))
assert_equal(s.acceleration(0), vector(0,0,0))
assert_equal(s.rotation(0).components, r.components)
assert_equal(s.rotationalVelocity(0), vector(0,0,0))
assert_equal(s.rotationalAcceleration(0), vector(0,0,0))
assert s.startTime == 0
assert s.endTime == np.inf
qa = s.rotation((1,2))
assert type(qa) == QuaternionArray
assert len(qa) == 2
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)
#!/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