class ContinuousRotationTrajectory(RotationTrajectory):
"""
A trajectory with constant rotational velocity.
"""
def __init__(self, rotationalVelocity, initialRotation=None):
self.initialRotation = Quaternion() \
if initialRotation is None else initialRotation
self._rotationalVelocity = rotationalVelocity
w = rotationalVelocity
W = np.asmatrix([[ 0, -w[2,0], w[1,0]],
[ w[2,0], 0, -w[0,0]],
[-w[1,0], w[0,0], 0 ]])
self.qW = Quaternion()
self.qW.setFromMatrix(expm(W))
def rotation(self, t):
if np.ndim(t) == 0:
return self.qW ** t
else:
return QuaternionArray([self.qW ** ti for ti in t])
def rotationalVelocity(self, t):
omega = np.empty((3, len(np.atleast_1d(t))))
omega[:] = self._rotationalVelocity
return omega
def rotationalAcceleration(self, t):
return np.zeros((3, len(np.atleast_1d(t))))
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 checkStaticConvergence(filterClass, performTest=True):
"""
Test that an orientation estimation algorithm converges within a reasonable
number of samples
"""
gyro = np.zeros((3, 1))
accel = -GRAVITY
mag = NORTH
SAMPLES = 500
initialRotation = Quaternion.fromEuler((60, 45, 0))
filter = filterClass(initialRotation=initialRotation,
initialTime=0,
initialRotationalVelocity=np.zeros((3, 1)),
**filterParameters.get(filterClass, {}))
estimatedQuaternions = QuaternionArray(np.empty((SAMPLES, 4)))
for i in range(SAMPLES):
filter(accel, mag, gyro, dt * (1 + i))
estimatedQuaternions[i] = filter.rotation.latestValue
if performTest:
assertMaximumErrorAngle(Quaternion(), filter.rotation.latestValue, 2)
return estimatedQuaternions
def _update(self, accel, mag, gyro, dt, t):
# Note: Measurement matrix H is equivalent to the identity matrix
# so is left out to simplify calculations
# Calculate Kalman gain
K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
# Construct measurment vector z
self.vo = self._vectorObservation(-accel, mag)
if self.vo.dot(Quaternion(*self.xHat_minus[3:])) < 0:
self.vo.negate()
z = np.vstack((gyro, np.vstack(self.vo.components)))
# Calculate state update
xHat = self.xHat_minus + K * (z - self.xHat_minus)
P = (np.eye(7) - K) * self.P_minus
# Normalise quaternion component of state vector
qHat = Quaternion(*xHat[3:]).normalise()
xHat[3:] = np.vstack(qHat.components)
# Calculate prediction
self.xHat_minus = xHat + self.dotx(xHat) * dt
Phi = self.Phi(xHat, dt)
self.Q[[0,1,2],[0,1,2]] = \
(self.D / (2 * self.tau)) * (1 - math.e**((-2 * dt) / self.tau))
self.P_minus = Phi * P * Phi.T + self.Q
self.rotation.add(t, qHat, P[3:, 3:])
self.vectorObservation.add(t, self.vo)
self.rotationalVelocity.add(t, xHat[:3], P[:3, :3])
def __init__(self):
"""
Initialise vector observation.
"""
alpha = np.radians(20)
self.qAlpha = Quaternion(math.cos(alpha/2), 0, math.sin(alpha/2), 0)
self.thetaThreshold = 0.1
def _process(self, *meas):
meas = [m / vectors.norm(m) for m in meas]
EPS = 1e-8
# Rotate the frame as necessary to avoid singularities.
# See Shuster 1979 eqs 74-76.
# Note differences in quaternion permutations from Shuster's paper.
# These ones work, why don't the ones given in the paper?
p = self._apply(self.refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[1,-1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,-1,1]]).T for r in self.refs]
p = self._apply(refs, meas)
q = Quaternion(p[3], p[2], -p[1], -p[0])
else:
q = Quaternion(p[2], -p[3], -p[0], p[1])
else:
q = Quaternion(-p[1], p[0], -p[3], p[2])
else:
q = Quaternion(p[0], p[1], p[2], p[3])
q.normalise()
return q
class ContinuousRotationTrajectory(RotationTrajectory):
"""
A trajectory with constant rotational velocity.
"""
def __init__(self, rotationalVelocity, initialRotation=None):
self.initialRotation = Quaternion() \
if initialRotation is None else initialRotation
self._rotationalVelocity = rotationalVelocity
w = rotationalVelocity
W = np.asmatrix([[ 0, -w[2,0], w[1,0]],
[ w[2,0], 0, -w[0,0]],
[-w[1,0], w[0,0], 0 ]])
self.qW = Quaternion()
self.qW.setFromMatrix(expm(W))
def rotation(self, t):
if np.ndim(t) == 0:
return self.qW ** t
else:
return QuaternionArray([self.qW ** ti for ti in t])
def rotationalVelocity(self, t):
omega = np.empty((3, len(np.atleast_1d(t))))
omega[:] = self._rotationalVelocity
return omega
def rotationalAcceleration(self, t):
return np.zeros((3, len(np.atleast_1d(t))))
def __init__(self, rotationalVelocity, initialRotation=None):
self.initialRotation = Quaternion() \
if initialRotation is None else initialRotation
self._rotationalVelocity = rotationalVelocity
w = rotationalVelocity
W = np.asmatrix([[0, -w[2, 0], w[1, 0]], [w[2, 0], 0, -w[0, 0]],
[-w[1, 0], w[0, 0], 0]])
self.qW = Quaternion()
self.qW.setFromMatrix(expm(W))
def _process(self, g, m):
# Algorithm assumes that gravity vector is -z in default position
# This is contrary to the other algorithm so we invert
g = -g
g /= vectors.norm(g)
# Pitch
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
flag = cosTheta <= self.thetaThreshold
if flag:
# If flag is set then theta is close to zero. Rotate coord frame 20
# degrees to reduce errors
g = self.qAlpha.rotateFrame(g)
m = self.qAlpha.rotateFrame(m)
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
cosHalfTheta = self.cosHalfAngle(cosTheta)
sinHalfTheta = self.sinHalfAngle(cosTheta, sinTheta)
qp = Quaternion(cosHalfTheta, 0, sinHalfTheta, 0)
# Roll
cosPhi = -g[2] / cosTheta
sinPhi = -g[1] / cosTheta
cosHalfPhi = self.cosHalfAngle(cosPhi)
sinHalfPhi = self.sinHalfAngle(cosPhi, sinPhi)
qr = Quaternion(cosHalfPhi, sinHalfPhi, 0, 0)
# Yaw
m = qr.rotateVector(m)
m = qp.rotateVector(m)
Mx = m[0]
My = m[1]
scale = 1 / math.sqrt(Mx**2 + My**2)
Mx *= scale
My *= scale
# From Yun paper
#cosPsi = Mx * self.Nx + My * self.Ny
#sinPsi = Mx * self.Ny - My * self.Nx
# but we know that Ny is 0 so this simplifies to
cosPsi = Mx
sinPsi = -My
cosHalfPsi = self.cosHalfAngle(cosPsi)
sinHalfPsi = self.sinHalfAngle(cosPsi, sinPsi)
qy = Quaternion(cosHalfPsi, 0, 0, sinHalfPsi)
combined = qy * qp * qr
if flag:
return combined * self.qAlpha.conjugate
else:
return combined
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 __init__(self):
"""
Initialise vector observation.
"""
alpha = np.radians(20)
self.qAlpha = Quaternion(math.cos(alpha/2), 0, math.sin(alpha/2), 0)
self.thetaThreshold = 0.1
def checkStaticConvergence(filterClass, performTest=True):
"""
Test that an orientation estimation algorithm converges within a reasonable
number of samples
"""
gyro = np.zeros((3,1))
accel = -GRAVITY
mag = NORTH
SAMPLES = 500
initialRotation = Quaternion.fromEuler((60,45,0))
filter = filterClass(initialRotation = initialRotation,
initialTime=0,
initialRotationalVelocity = np.zeros((3,1)),
**filterParameters.get(filterClass, {}))
estimatedQuaternions = QuaternionArray(np.empty((SAMPLES,4)))
for i in range(SAMPLES):
filter(accel, mag, gyro, dt*(1+i))
estimatedQuaternions[i] = filter.rotation.latestValue
if performTest:
assertMaximumErrorAngle(Quaternion(),
filter.rotation.latestValue, 2)
return estimatedQuaternions
def _process(self, g, m):
z = g / vectors.norm(g)
x = m - (z * vectors.dot(m, z))
x /= vectors.norm(x)
y = vectors.cross(z, x)
return Quaternion.fromVectors(x, y, z)
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 _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0, *gyro)
self.qHat += dotq * dt
if abs(vectors.norm(accel) - 1) < self._aT:
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1 / self._k) * dt * qError
else:
qMeas = Quaternion.nan()
self.vectorObservation.add(t, qMeas)
self.qHat.normalise()
self.rotation.add(t, self.qHat)
def __init__(self, rotation=None):
"""
Initialise the trajectory.
@param rotation: the constant rotation of the object (L{Quaternion})
"""
self.staticRotation = Quaternion() if rotation is None else rotation
def transform_trajectory(trajectory, R, p):
"""Create new trajectory relative the given transformation
If R1(t) and p1(t) is the rotation and translation given by inout trajectory, then
this function returns a new trajectory that fulfills the following.
Let X1 = R1(t).T[X - p1(t)] be the coordinate of point X in input trajectory coordinates.
Then X2 = R X1 + p is the same point in the coordinate frame of the new trajectory
Since X2 = R2(t).T [X - p2(t)] then we have
R2(t) = R1(t)R and p2(t) = p1(t) + R1(t)p
"""
ts_q1 = trajectory.sampled.rotationKeyFrames
q1 = ts_q1.values
ts_p1 = trajectory.sampled.positionKeyFrames
p1 = ts_p1.values
# Rotation
Q = Quaternion.fromMatrix(R)
q2 = q1 * Q
ts_q2 = TimeSeries(ts_q1.timestamps, q2)
# Translation
p2 = p1 + q1.rotateVector(p)
ts_p2 = TimeSeries(ts_p1.timestamps, p2)
sampled = SampledTrajectory(positionKeyFrames=ts_p2,
rotationKeyFrames=ts_q2)
smoothen = False # MUST be the same as used in Dataset class
splined = SplinedTrajectory(sampled, smoothRotations=smoothen)
return splined
def _process(self, g, m):
z = g / vectors.norm(g)
x = m - (z * vectors.dot(m, z))
x /= vectors.norm(x)
y = vectors.cross(z, x)
return Quaternion.fromVectors(x, y, z)
def _process(self, *meas):
"""
Estimate the orientation Quaternion from the observed vectors.
@param meas: The observed vectors (3x1 L{np.ndarray})
@return: The least squares optimal L{Quaternion}
"""
B = sum([
a * np.dot(b, r.T)
for a, b, r in zip(self.weights, meas, self.refs)
])
S = B + B.T
z = sum([
a * vectors.cross(b, r)
for a, b, r in zip(self.weights, meas, self.refs)
])
sigma = np.trace(B)
K = np.empty((4, 4))
K[0:3, 0:3] = S - sigma * np.identity(3)
K[0:3, 3] = z[:, 0]
K[3, 0:3] = z[:, 0]
K[3, 3] = sigma
eigenValues, eigenVectors = np.linalg.eig(K)
q = np.asarray(eigenVectors[:, np.argmax(eigenValues)])
return Quaternion(q[3], q[0], q[1], q[2]).normalise()
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 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 __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 rotation(self, t):
if len(self.rotationKeyFrames) == 0:
if np.ndim(t) == 0:
return Quaternion.nan()
else:
return QuaternionArray.nan(len(t))
else:
return self.rotationKeyFrames(t)
def __init__(self, rotationalVelocity, initialRotation=None):
self.initialRotation = Quaternion() \
if initialRotation is None else initialRotation
self._rotationalVelocity = rotationalVelocity
w = rotationalVelocity
W = np.asmatrix([[ 0, -w[2,0], w[1,0]],
[ w[2,0], 0, -w[0,0]],
[-w[1,0], w[0,0], 0 ]])
self.qW = Quaternion()
self.qW.setFromMatrix(expm(W))
def __call__(self, *measurements):
"""
Estimate the orientation Quaternion from the observed vectors.
@param measurements: The observed vectors (3x1 L{np.ndarray})
@return: The estimated orientation L{Quaternion}
"""
if any(map(lambda a: np.any(np.isnan(a)), measurements)):
return Quaternion(np.nan,np.nan,np.nan,np.nan)
else:
return self._process(*measurements)
def _readMotionData(self):
'''
Read the motion data from the BVH file.
'''
# update joint tree with new data.
frame = 0
lastRotation = {}
for frame in range(self.frameCount):
time = frame * self.framePeriod
channelData = self._readFrame()
for joint in self.model.joints:
for chan in joint.channels:
chanData = channelData.pop(0)
if chan == "Xposition":
xPos = chanData
elif chan == "Yposition":
yPos = chanData
elif chan == "Zposition":
zPos = chanData
elif chan == "Xrotation":
xRot = chanData
elif chan == "Yrotation":
yRot = chanData
elif chan == "Zrotation":
zRot = chanData
else:
raise RuntimeError('Unknown channel: ' + chan)
if not joint.hasParent:
try:
p = np.array([[xPos,yPos,zPos]]).T * \
self.conversionFactor
p = convertCGtoNED(p)
del xPos, yPos, zPos
joint.positionKeyFrames.add(time, p)
except UnboundLocalError:
raise SyntaxError("No position data for root joint")
try:
if joint.hasChildren:
q = Quaternion.fromEuler((zRot, xRot, yRot),
order='zxy')
q = convertCGtoNED(q)
del xRot, yRot, zRot
# Rotation in BVH is relative to parent joint so
# combine the rotations
if joint.hasParent:
q = lastRotation[joint.parent] * q
joint.rotationKeyFrames.add(time, q)
lastRotation[joint] = q
except UnboundLocalError:
raise SyntaxError("No rotation data for a joint that "
"is not an end effector")
def _process(self, *meas):
meas = [m / vectors.norm(m) for m in meas]
EPS = 1e-8
# Rotate the frame as necessary to avoid singularities.
# See Shuster 1979 eqs 74-76.
# Note differences in quaternion permutations from Shuster's paper.
# These ones work, why don't the ones given in the paper?
p = self._apply(self.refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[1,-1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,1,-1]]).T for r in self.refs]
p = self._apply(refs, meas)
if abs(p[0]) < EPS:
refs = [r*np.array([[-1,-1,1]]).T for r in self.refs]
p = self._apply(refs, meas)
q = Quaternion(p[3], p[2], -p[1], -p[0])
else:
q = Quaternion(p[2], -p[3], -p[0], p[1])
else:
q = Quaternion(-p[1], p[0], -p[3], p[2])
else:
q = Quaternion(p[0], p[1], p[2], p[3])
q.normalise()
return q
def checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)], [0], [math.sin(inclination)]])
g = np.array([[0, 0, 1]], dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q, vo(g, m))
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 _process(self, g, m):
# Algorithm assumes that gravity vector is -z in default position
# This is contrary to the other algorithm so we invert
g = -g
g /= vectors.norm(g)
# Pitch
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
flag = cosTheta <= self.thetaThreshold
if flag:
# If flag is set then theta is close to zero. Rotate coord frame 20
# degrees to reduce errors
g = self.qAlpha.rotateFrame(g)
m = self.qAlpha.rotateFrame(m)
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
cosHalfTheta = self.cosHalfAngle(cosTheta)
sinHalfTheta = self.sinHalfAngle(cosTheta, sinTheta)
qp = Quaternion(cosHalfTheta, 0, sinHalfTheta, 0)
# Roll
cosPhi = -g[2] / cosTheta
sinPhi = -g[1] / cosTheta
cosHalfPhi = self.cosHalfAngle(cosPhi)
sinHalfPhi = self.sinHalfAngle(cosPhi, sinPhi)
qr = Quaternion(cosHalfPhi, sinHalfPhi, 0, 0)
# Yaw
m = qr.rotateVector(m)
m = qp.rotateVector(m)
Mx = m[0]
My = m[1]
scale = 1 / math.sqrt(Mx**2 + My**2)
Mx *= scale
My *= scale
# From Yun paper
#cosPsi = Mx * self.Nx + My * self.Ny
#sinPsi = Mx * self.Ny - My * self.Nx
# but we know that Ny is 0 so this simplifies to
cosPsi = Mx
sinPsi = -My
cosHalfPsi = self.cosHalfAngle(cosPsi)
sinHalfPsi = self.sinHalfAngle(cosPsi, sinPsi)
qy = Quaternion(cosHalfPsi, 0, 0, sinHalfPsi)
combined = qy * qp * qr
if flag:
return combined * self.qAlpha.conjugate
else:
return combined
def _process(self, g, m):
"""
Estimate the orientation Quaternion from the observed vectors.
@param g: the observed gravitational field vector (3x1 L{np.ndarray})
@param m: the observed magnetic field vector (3x1 L{np.ndarray})
@return: The estimated orientation L{Quaternion}
"""
z = g / vectors.norm(g)
y = vectors.cross(z, m)
y /= vectors.norm(y)
x = vectors.cross(y, z)
return Quaternion.fromVectors(x, y, z)
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 checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)],[0],
[math.sin(inclination)]])
g = np.array([[0,0,1]],dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q,vo(g,m))
def _process(self, g, m):
"""
Estimate the orientation Quaternion from the observed vectors.
@param g: the observed gravitational field vector (3x1 L{np.ndarray})
@param m: the observed magnetic field vector (3x1 L{np.ndarray})
@return: The estimated orientation L{Quaternion}
"""
z = g / vectors.norm(g)
y = vectors.cross(z, m)
y /= vectors.norm(y)
x = vectors.cross(y, z)
return Quaternion.fromVectors(x, y, z)
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 geodesicQuaternionPath():
"""
Generate an array of quaternions following a geodesic path
Returns
-------
t : :class:`~numpy.ndarray`
timestamps of quaternions
quaternionPath : :class:`~imusim.maths.quaternions.QuaternionArray`
array of quaternions following a geodesic path
"""
t = np.arange(0,10,0.1)
q = Quaternion.fromEuler((45,0,0))
return t, QuaternionArray([q**T for T in t])
def _update(self, accel, mag, gyro, dt, t):
dotq = 0.5 * self.qHat * Quaternion(0,*gyro)
self.qHat += dotq * dt
if abs(vectors.norm(accel) - 1) < self._aT:
qMeas = self._vectorObservation(-accel, mag)
if self.qHat.dot(qMeas) < 0:
qMeas.negate()
qError = qMeas - self.qHat
self.qHat += (1/self._k) * dt * qError
else:
qMeas = Quaternion.nan()
self.vectorObservation.add(t, qMeas)
self.qHat.normalise()
self.rotation.add(t, self.qHat)
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 testDynamicConvergence():
for i in range(3):
t = np.arange(0, 10, dt)
trajectory = RandomRotationTrajectory(t)
for imp in getImplementations(orientation,
orientation.OrientationFilter):
params = filterParameters.get(imp, {})
filter = imp(initialRotation=Quaternion(),
initialTime=trajectory.startTime,
initialRotationalVelocity=trajectory.rotation(
trajectory.startTime).rotateFrame(
trajectory.rotationalVelocity(
trajectory.startTime)),
**params)
if imp not in [
orientation.GyroIntegrator, orientation.DistLinAccelCF
]:
yield checkDynamicConvergence, filter, trajectory
def __init__(self, parent, positionOffset=None, rotationOffset=None):
"""
Initialise trajectory.
@param parent: the L{AbstractTrajectory} which this trajectory follows.
@param positionOffset: the offset vector from the parent trajectory in the
co-ordinate frame of the parent. (3x1 L{np.ndarray})
@param rotationOffset: the rotational offset from the parent trajectory
(L{Quaternion})
"""
self.parent = parent
if positionOffset is None:
self.positionOffset = np.zeros((3,1))
else:
self.positionOffset = positionOffset
if rotationOffset is None:
self.rotationOffset = Quaternion()
else:
self.rotationOffset = rotationOffset
class FQA(VectorObservation):
"""
Implementation of the Factored Quaternion Algorithm by Yun et al.
The algorithm is designed to estimation the orientation based on
observations of the Earth gravitational and magnetic fields based on the
assumption of a (X,Y,Z) -> (North,East,Down) co-ordinate frame
See X. Yun, E. Bachmann, and R. McGhee "A Simplified Quaternion-Based
Algorithm for Orientation Estimation from Earth Gravity and Magnetic
Field Measurements" In IEEE Trans. on Instrumentation and Measurement
v. 57 pp 638-650 2008
"""
def __init__(self):
"""
Initialise vector observation.
"""
alpha = np.radians(20)
self.qAlpha = Quaternion(math.cos(alpha/2), 0, math.sin(alpha/2), 0)
self.thetaThreshold = 0.1
def cosHalfAngle(self, cosAngle):
"""
Compute the value of cos(angle/2) from cos(angle)
@param cosAngle: cosine of angle (float)
"""
if -1.0001 < cosAngle <= -1:
cosAngle = -1
return math.sqrt((1+cosAngle)/2)
def sinHalfAngle(self, cosAngle, sinAngle):
"""
Compute the value of sin(angle/2) from cos(angle) and sin(angle)
@param cosAngle: cosine of angle (float)
@param sinAngle: sine of angle (float)
"""
if 1 <= cosAngle < 1.0001:
cosAngle = 1
sign = 1 if sinAngle >= 0 else -1
return sign * math.sqrt((1-cosAngle)/2)
@prepend_method_doc(TRIAD)
def _process(self, g, m):
# Algorithm assumes that gravity vector is -z in default position
# This is contrary to the other algorithm so we invert
g = -g
g /= vectors.norm(g)
# Pitch
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
flag = cosTheta <= self.thetaThreshold
if flag:
# If flag is set then theta is close to zero. Rotate coord frame 20
# degrees to reduce errors
g = self.qAlpha.rotateFrame(g)
m = self.qAlpha.rotateFrame(m)
sinTheta = g[0]
cosTheta = math.sqrt(1 - sinTheta**2)
cosHalfTheta = self.cosHalfAngle(cosTheta)
sinHalfTheta = self.sinHalfAngle(cosTheta, sinTheta)
qp = Quaternion(cosHalfTheta, 0, sinHalfTheta, 0)
# Roll
cosPhi = -g[2] / cosTheta
sinPhi = -g[1] / cosTheta
cosHalfPhi = self.cosHalfAngle(cosPhi)
sinHalfPhi = self.sinHalfAngle(cosPhi, sinPhi)
qr = Quaternion(cosHalfPhi, sinHalfPhi, 0, 0)
# Yaw
m = qr.rotateVector(m)
m = qp.rotateVector(m)
Mx = m[0]
My = m[1]
scale = 1 / math.sqrt(Mx**2 + My**2)
Mx *= scale
My *= scale
# From Yun paper
#cosPsi = Mx * self.Nx + My * self.Ny
#sinPsi = Mx * self.Ny - My * self.Nx
# but we know that Ny is 0 so this simplifies to
cosPsi = Mx
sinPsi = -My
cosHalfPsi = self.cosHalfAngle(cosPsi)
sinHalfPsi = self.sinHalfAngle(cosPsi, sinPsi)
qy = Quaternion(cosHalfPsi, 0, 0, sinHalfPsi)
combined = qy * qp * qr
if flag:
return combined * self.qAlpha.conjugate
else:
return combined
def loadQualisysTSVFile(filename):
"""
Load 3DOF or 6DOF marker data from a Qualisys Track Manager TSV file.
@param filename: Name of TSV file to load.
@return: A L{MarkerCapture} instance.
"""
# Open file to read header.
datafile = open(filename, 'r')
# Count of header lines in file.
headerLines = 0
capture = MarkerCapture()
markerIndices = dict()
while True:
# Read and count each file header line.
line = datafile.readline().strip('\r\n')
headerLines = headerLines + 1
# Key and values are tab-separated.
items = line.split('\t')
key = items[0]
values = items[1:]
# Handle relevant fields.
if key == 'FREQUENCY':
# Frame rate.
capture.frameRate = int(values[0])
capture.framePeriod = 1/capture.frameRate
elif key == 'DATA_INCLUDED':
# 3D or 6D data.
type = values[0]
if (type == '3D'):
# 3D data fields are implicitly XYZ co-ordinates.
fieldNames = ['X', 'Y', 'Z']
elif key == 'BODY_NAMES' or key == 'MARKER_NAMES':
# List of markers or 6DOF body names.
for i in range(len(values)):
name = values[i]
if type == '6D':
markerClass = Marker6DOF
else:
markerClass = Marker3DOF
markerIndices[markerClass(capture, name)] = i
if (type == '6D'):
# Field names are on a line after the body names, each
# beginning with a space.
fieldNames = [x.strip(' ') for x in
datafile.readline().strip('\r\n').split('\t')]
headerLines = headerLines + 1
# The above keys are always on the last line in the header.
datafile.close()
break
# Load values from file, skipping header.
data = np.loadtxt(filename, skiprows = headerLines)
capture.frameCount = len(data)
capture.frameTimes = np.arange(0, capture.frameCount / capture.frameRate,
capture.framePeriod)
# Find index in data array for a given marker and marker field name.
def index(marker, name):
offset = markerIndices[marker] * len(fieldNames)
return offset + fieldNames.index(name)
# Return data for a given marker and marker field names.
def fields(marker, names):
indices = [index(marker, name) for name in names]
return data[:,indices[0]:indices[-1]+1]
# Iterate through markers to fill in their data.
for marker in capture.markers:
# Get position data.
positions = fields(marker, ['X', 'Y', 'Z']) / 1000
if type == '6D':
# Get rotation matrices.
matrix_coeffs = fields(marker, ['Rot[%d]' % i for i in range(9)])
# Mark invalid data (residual = -1) with NaNs.
invalid = fields(marker, ['Residual'])[:,0] == -1.0
matrix_coeffs[invalid] = np.nan
positions[invalid] = np.nan
# Convert to quaternions and unflip.
quats = QuaternionArray(np.empty((capture.frameCount,4)))
for i in range(len(quats)):
q = Quaternion()
q.setFromMatrix(matrix_coeffs[i].reshape((3,3)).T)
quats[i] = q
rotations = quats.unflipped()
# Add rotation data to marker.
for time, rotation in zip(capture.frameTimes, rotations):
marker.rotationKeyFrames.add(time, rotation)
# Add position data to marker.
for time, position in zip(capture.frameTimes, positions):
marker.positionKeyFrames.add(time, position.reshape(3,1))
return capture
def testNED_to_CG_Quat():
testing.assert_equal(
transforms.convertNEDtoCG(Quaternion()).components,
Quaternion(0.5, 0.5, -0.5, 0.5).components)
def testCG_to_NED_Quat():
testing.assert_equal(
transforms.convertCGtoNED(Quaternion()).components,
Quaternion(0.5, -0.5, 0.5, -0.5).components)
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
