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 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 testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0), vector(0, 0, 0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1, 0, 0, 0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10, 0, 0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10, 0, 0)))
assertQuaternionAlmostEqual(
j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0, 10, 0))
assert_almost_equal(j1end.position(0), vector(0, 0, 0))
def testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile()
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0),vector(0,0,0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1,0,0,0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10,0,0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10,0,0)))
assertQuaternionAlmostEqual(j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0,10,0))
assert_almost_equal(j1end.position(0), vector(0,0,0))
def __init__(self, rootdata):
"""
Construct a new ASFRoot.
@param rootdata: The L{ParseResults} object representing the root
segment data from the ASF file.
"""
TreeNode.__init__(self, None)
self.name = 'root'
self.childoffset = vector(0,0,0)
self.isDummy = False
axis = vector(*rootdata.axis)[::-1]
rotOrder = rootdata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.channels = rootdata.channels
def __init__(self, rootdata):
"""
Construct a new ASFRoot.
@param rootdata: The L{ParseResults} object representing the root
segment data from the ASF file.
"""
TreeNode.__init__(self, None)
self.name = 'root'
self.childoffset = vector(0, 0, 0)
self.isDummy = False
axis = vector(*rootdata.axis)[::-1]
rotOrder = rootdata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.channels = rootdata.channels
def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1,
translation=vector(0,0,0)):
"""
Construct an affine transform.
A generic transformation matrix can be supplied, or a transform
composed of rotation and scaling can be built.
Generating rotations using supplied Euler angles is performed from the
perspective of a fixed camera viewing a rotating object, i.e. points
rotate within a fixed reference frame. Rotations are applied in
aerospace (ZYX) order.
@param transform: 3x3 linear transformation matrix.
@param rx: X rotation angle in radians.
@param ry: Y rotation angle in radians.
@param rz: Z rotation angle in radians.
@param scale: Scaling factor
@param translation: 3x1 translation vector.
"""
if transform is not None:
self._transform = transform
else:
self._transform = scale * matrixFromEuler((rz,ry,rx),'zyx',False)
self._inverseTransform = np.linalg.inv(self._transform)
self._translation = translation
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 __init__(self, magnitude=STANDARD_GRAVITY):
"""
Construct field model.
@param magnitude: Magnitude of field in m/s^2. Default is standard
Earth gravity.
"""
ConstantVectorField.__init__(self, vector(0, 0, magnitude))
def __init__(self, magnitude=STANDARD_GRAVITY):
"""
Construct field model.
@param magnitude: Magnitude of field in m/s^2. Default is standard
Earth gravity.
"""
ConstantVectorField.__init__(self, vector(0, 0, magnitude))
def __init__(self, parent, bonedata, scale):
"""
Consruct a new ASFBone.
@param parent: The parent bone of this bone.
@param bonedata: The L{ParseResults} object representing the bone
data from the ASF file.
"""
TreeNode.__init__(self, parent)
self.name = bonedata.name
axis = vector(*bonedata.axis)[::-1]
rotOrder = bonedata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.childoffset = self.rotationOffset.rotateVector(
vector(*bonedata.direction) * bonedata.length * scale)
self.isDummy = bonedata.channels is ''
self.channels = bonedata.channels
def __init__(self, parent, bonedata, scale):
"""
Consruct a new ASFBone.
@param parent: The parent bone of this bone.
@param bonedata: The L{ParseResults} object representing the bone
data from the ASF file.
"""
TreeNode.__init__(self, parent)
self.name = bonedata.name
axis = vector(*bonedata.axis)[::-1]
rotOrder = bonedata.axisRotationOrder[::-1]
self.rotationOffset = Quaternion.fromEuler(axis, rotOrder).conjugate
self.childoffset = self.rotationOffset.rotateVector(
vector(*bonedata.direction) * bonedata.length * scale)
self.isDummy = bonedata.channels is ''
self.channels = bonedata.channels
def __init__(self, platform, positionOffset=vector(0,0,0),
rotationOffset=Quaternion(1,0,0,0)):
"""
Initialise sensor.
@param platform: L{Platform} this sensor will be attached to.
@param positionOffset: Position offset vector of this sensor
in the local co-ordinate frame of its host platform.
@param rotationOffset: Rotation offset quaternion of this sensor
in the local co-ordinate frame of its host platform.
"""
self._positionOffset = positionOffset
self._rotationOffset = rotationOffset
Component.__init__(self, platform)
def __init__(self, platform, positionOffset=vector(0,0,0),
rotationOffset=Quaternion(1,0,0,0)):
"""
Initialise sensor.
@param platform: L{Platform} this sensor will be attached to.
@param positionOffset: Position offset vector of this sensor
in the local co-ordinate frame of its host platform.
@param rotationOffset: Rotation offset quaternion of this sensor
in the local co-ordinate frame of its host platform.
"""
self._positionOffset = positionOffset
self._rotationOffset = rotationOffset
Component.__init__(self, platform)
def __init__(self,
positionOffset=vector(0, 0, 0),
rotationOffset=Quaternion(1, 0, 0, 0),
simulation=None,
trajectory=None):
self.accelerometer = IdealAccelerometer(self,
positionOffset=positionOffset,
rotationOffset=rotationOffset)
self.magnetometer = IdealMagnetometer(self,
positionOffset=positionOffset,
rotationOffset=rotationOffset)
self.gyroscope = IdealGyroscope(self,
positionOffset=positionOffset,
rotationOffset=rotationOffset)
self.adc = IdealADC(self)
self.timer = IdealTimer(self)
self.radio = IdealRadio(self)
StandardIMU.__init__(self, simulation, trajectory)
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 __init__(self, magnitude=50e-6, inclination=70, declination=0):
"""
Construct field model.
Default parameters result in a field with a magnitude of 50
microTesla, a 70 degree downwards inclination and 0 degrees
declination. These defaults are based on the nominal values for
Edinburgh, UK, with declination set to 0 degrees so that magnetic
and true North are equivalent.
@param magnitude: Magnitude of the field, in Teslas.
@param inclination: Inclination of the field to the horizontal,
in degrees.
@param declination: Delcination of the field from true North,
in degrees.
"""
theta = math.radians(declination)
ctheta = math.cos(theta)
stheta = math.sin(theta)
phi = math.radians(inclination)
cphi = math.cos(phi)
sphi = math.sin(phi)
value = magnitude * vectors.vector(cphi*ctheta, cphi*stheta, sphi)
ConstantVectorField.__init__(self, value)
def checkBaseField(field, nominal):
assert_almost_equal(field.nominalValue, nominal)
assert_almost_equal(field(vector(0,0,0), 0), nominal)
# 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/>.
import numpy as np
from imusim.testing.random_data import RandomRotationTrajectory
from imusim.testing.quaternions import assert_quaternions_correlated, assertMaximumErrorAngle
from imusim.testing.inspection import getImplementations
from imusim.algorithms import orientation
from imusim.maths.quaternions import Quaternion, QuaternionArray
from imusim.trajectories.rigid_body import Joint
from imusim.maths.vectors import vector
dt = 0.01
GRAVITY = vector(0,0,1)
NORTH = vector(1,0,0)
filterParameters = {
orientation.OrientCF : {
'k' : 1,
'aT' : 1},
orientation.BachmannCF : {
'k' : 1},
orientation.DistLinAccelCF : {
'k' : 1,
'joint' : Joint(None),
'offset' : np.zeros((3,1)),
'gravFieldReference' : GRAVITY,
'magFieldReference' : NORTH
},
assert_almost_equal(transforms.polarToCartesian(*polar), cartesian)
def checkCartesianToPolar(cartesian, polar):
assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar)
def testCartesianPolarConversion():
for testParams in CARTESIAN_POLAR_TEST_VALUES:
cartesian, polar = testParams
yield checkPolarToCartesian, cartesian, polar
yield checkCartesianToPolar, cartesian, polar
# each element of the list is a tuple of vectors in NED and CG co-ords
NED_CG_TEST_VALUES = [(vector(1, 0, 0), vector(0, 0, 1)),
(vector(0, 1, 0), vector(-1, 0, 0)),
(vector(0, 0, 1), vector(0, -1, 0))]
def checkNED_to_CG(ned, cg):
assert_almost_equal(transforms.convertNEDtoCG(ned), cg)
def checkCG_to_NED(ned, cg):
assert_almost_equal(transforms.convertCGtoNED(cg), ned)
def testCoordinateChange():
for ned, cg in NED_CG_TEST_VALUES:
yield checkNED_to_CG, ned, cg
class DistLinAccelCF(OrientationFilter):
"""
Implementation of the multi-IMU orientation algorithm by Young & Ling.
See A. D. Young, M. J. Ling and D. K. Arvind. "Distributed Estimation of
Linear Acceleration for Improved Accuracy in Wireless Inertial Motion
Capture", in Proc. 9th ACM/IEEE International Conference on Information
Processing in Sensor Networks, ACM, 2010, pp. 256-267.
@ivar qMeas: Result of latest vector observation (L{Quaternion}).
@ivar vectorObservation: L{TimeSeries} of vector observation results.
@ivar jointAccel: Latest estimate of joint acceleration
(3x1 L{np.ndarray}).
@ivar jointAcceleration: L{TimeSeries} of acceleration estimates.
"""
GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)
@prepend_method_doc(BachmannCF)
def __init__(self, initialTime, initialRotation, k, joint, offset,
**kwargs):
"""
@param joint: The L{Joint} the IMU is attached to.
@param offset: The offset of the IMU in the joint's co-ordinate frame
(3x1 L{np.ndarray})
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self.qHat = initialRotation.copy()
self.k = k
self.joint = joint
self.offset = offset
self._vectorObservation = vector_observation.GramSchmidt()
self.qMeas = Quaternion.nan()
self.jointAccel = vectors.nan()
self.gyroFIFO = collections.deque([], maxlen=3)
self.accelFIFO = collections.deque([], maxlen=2)
self.magFIFO = collections.deque([], maxlen=2)
self.isRoot = not joint.hasParent
self.children = joint.childJoints
self.vectorObservation = TimeSeries()
self.jointAcceleration = TimeSeries()
def handleLinearAcceleration(self, jointAcceleration, dt):
"""
Perform drift correction based on incoming joint acceleration estimate.
@param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
"""
self.jointAccel = jointAcceleration
if len(self.gyroFIFO) < 3:
return
# Estimate linear acceleration
o = self.offset
w = self.gyroFIFO
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = (q.rotateFrame(self.jointAccel) + lt + lr)
# Apply drift correction
self.qMeas = self._vectorObservation(-(self.accelFIFO[1] - l),
self.magFIFO[1])
if q.dot(self.qMeas) < 0:
self.qMeas.negate()
q = q + 1.0 / self.k * dt * (self.qMeas - q)
dotq = 0.5 * self.qHat * Quaternion(0, *w[0])
self.qHat = q + dotq * dt
self.qHat.normalise()
def childAcceleration(self, o, dt):
"""
Compute acceleration for child joint.
@param o: Offset of child joint (3x1 L{np.ndarray}).
"""
w = self.gyroFIFO
if len(w) < 3:
return None
q = self.qHat_minus_1
a = (w[0] - w[2]) / (2 * dt)
lt = vectors.cross(a, o)
lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
l = self.jointAccel + q.rotateVector(lt + lr)
return l
def _update(self, accel, mag, gyro, dt, t):
# Store measurements in queues for processing
self.gyroFIFO.appendleft(gyro)
self.accelFIFO.appendleft(accel)
self.magFIFO.appendleft(mag)
# save current qHat for drift correction
self.qHat_minus_1 = copy.copy(self.qHat)
# Inertial update
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
self.qHat.normalise()
if self.isRoot and len(self.accelFIFO) >= 2:
# Root can't receive acceleration data so must estimate it.
measuredAccel = self.qHat_minus_1.rotateVector(self.accelFIFO[1])
# Assume root joint acceleration was zero.
self.jointAccel = np.zeros((3, 1))
# Calculate tangential acceleration for root joint IMU offset.
linAccel = self.childAcceleration(self.offset, dt)
if linAccel is not None:
# Calculate expected measurement.
expectedAccel = linAccel - self.GRAVITY_VECTOR
# Set root joint acceleration to the residual.
self.jointAccel = measuredAccel - expectedAccel
self.handleLinearAcceleration(self.jointAccel, dt)
self.rotation.add(t, self.qHat)
self.vectorObservation.add(t, self.qMeas)
self.jointAcceleration.add(t, self.jointAccel)
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 noiseVoltages(self, t):
return vector(0,0,0)
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
# 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 imusim.environment.magnetic_fields import EarthMagneticField
from imusim.environment.magnetic_fields import DistortedMagneticField
from imusim.environment.magnetic_fields import SolenoidMagneticField
from numpy.testing import assert_almost_equal
from imusim.maths.vectors import vector
from imusim.maths.transforms import AffineTransform
from numpy import random
# Tuple of constructor args and nominal field vectors for Earth fields
TEST_PARAMS = [
((1, 0, 0), vector(1,0,0)),
((1, 90, 0), vector(0,0,1)),
((1, 0, 90), vector(0,1,0))
]
def checkBaseField(field, nominal):
assert_almost_equal(field.nominalValue, nominal)
assert_almost_equal(field(vector(0,0,0), 0), nominal)
def testEarthField():
for args, nominal in TEST_PARAMS:
yield checkBaseField, EarthMagneticField(*args), nominal
def checkZeroHeadingVariation(field, position):
assert_almost_equal(field.headingVariation(position, 0), 0)
def checkBaseField(field, nominal):
assert_almost_equal(field.nominalValue, nominal)
assert_almost_equal(field(vector(0, 0, 0), 0), nominal)
# 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 imusim.environment.magnetic_fields import EarthMagneticField
from imusim.environment.magnetic_fields import DistortedMagneticField
from imusim.environment.magnetic_fields import SolenoidMagneticField
from numpy.testing import assert_almost_equal
from imusim.maths.vectors import vector
from imusim.maths.transforms import AffineTransform
from numpy import random
# Tuple of constructor args and nominal field vectors for Earth fields
TEST_PARAMS = [((1, 0, 0), vector(1, 0, 0)), ((1, 90, 0), vector(0, 0, 1)),
((1, 0, 90), vector(0, 1, 0))]
def checkBaseField(field, nominal):
assert_almost_equal(field.nominalValue, nominal)
assert_almost_equal(field(vector(0, 0, 0), 0), nominal)
def testEarthField():
for args, nominal in TEST_PARAMS:
yield checkBaseField, EarthMagneticField(*args), nominal
def checkZeroHeadingVariation(field, position):
assert_almost_equal(field.headingVariation(position, 0), 0)
]
def checkPolarToCartesian(cartesian, polar):
assert_almost_equal(transforms.polarToCartesian(*polar), cartesian)
def checkCartesianToPolar(cartesian, polar):
assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar)
def testCartesianPolarConversion():
for testParams in CARTESIAN_POLAR_TEST_VALUES:
cartesian, polar = testParams
yield checkPolarToCartesian, cartesian, polar
yield checkCartesianToPolar, cartesian, polar
# each element of the list is a tuple of vectors in NED and CG co-ords
NED_CG_TEST_VALUES = [
(vector(1,0,0), vector(0,0,1)),
(vector(0,1,0), vector(-1,0,0)),
(vector(0,0,1), vector(0,-1,0))
]
def checkNED_to_CG(ned, cg):
assert_almost_equal(transforms.convertNEDtoCG(ned), cg)
def checkCG_to_NED(ned, cg):
assert_almost_equal(transforms.convertCGtoNED(cg), ned)
def testCoordinateChange():
for ned, cg in NED_CG_TEST_VALUES:
yield checkNED_to_CG, ned, cg
yield checkCG_to_NED, ned, cg
# 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/>.
import numpy as np
from imusim.testing.random_data import RandomRotationTrajectory
from imusim.testing.quaternions import assert_quaternions_correlated, assertMaximumErrorAngle
from imusim.testing.inspection import getImplementations
from imusim.algorithms import orientation
from imusim.maths.quaternions import Quaternion, QuaternionArray
from imusim.trajectories.rigid_body import Joint
from imusim.maths.vectors import vector
dt = 0.01
GRAVITY = vector(0, 0, 1)
NORTH = vector(1, 0, 0)
filterParameters = {
orientation.OrientCF: {
'k': 1,
'aT': 1
},
orientation.BachmannCF: {
'k': 1
},
orientation.DistLinAccelCF: {
'k': 1,
'joint': Joint(None),
'offset': np.zeros((3, 1)),
'gravFieldReference': GRAVITY,
def noiseVoltages(self, t):
return vector(0,0,0)
