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)
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)
def testOffsetTrajectories():
for i in range(10):
T = RandomTrajectory()
t = T.positionKeyFrames.timestamps
dt = t[1] - t[0]
p = T.position(t)
r = T.rotation(t)
offset = randomPosition()
rotationOffset = randomRotation()
oT = OffsetTrajectory(T, offset, rotationOffset)
offsetPositions = p + r.rotateVector(offset)
offsetRotations = r * rotationOffset
yield checkTrajectory, oT, TimeSeries(t,offsetPositions), TimeSeries(t, offsetRotations)
def testOffsetTrajectories():
for i in range(10):
T = RandomTrajectory()
t = T.positionKeyFrames.timestamps
dt = t[1] - t[0]
p = T.position(t)
r = T.rotation(t)
offset = randomPosition()
rotationOffset = randomRotation()
oT = OffsetTrajectory(T, offset, rotationOffset)
offsetPositions = p + r.rotateVector(offset)
offsetRotations = r * rotationOffset
yield checkTrajectory, oT, TimeSeries(t, offsetPositions), TimeSeries(
t, offsetRotations)
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
