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 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 __init__(self, initialTime, initialRotation, initialRotationalVelocity,
initialCovariance, measurementCovariance, D, tau, **kwargs):
"""
@param initialRotationalVelocity: Initial angular rate estimate
(3x1 L{np.ndarray}).
@param initialCovariance: Initial state covariance matrix (7x7).
@param measurementCovariance: Measurement noise covariance (7x7).
@param D: Variance of process noise model in rad^2/s^2 (float).
@param tau: Time constant of process noise model in seconds (float).
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation, initialCovariance[3:,
3:])
self.tau = float(tau)
self.D = float(D)
self._vectorObservation = vector_observation.FQA()
# Process noise covariance matrix, updated on each call.
self.Q = np.asmatrix(np.zeros((7, 7)))
# Measurement noise covariance matrix
self.R = measurementCovariance
# Prior covariance estimate and state vector initialisation.
self.P_minus = initialCovariance
self.xHat_minus = np.vstack(
(initialRotationalVelocity, np.vstack(initialRotation.components)))
self.vectorObservation = TimeSeries()
self.rotationalVelocity = TimeSeries()
def testSplineTrajectories():
for i in range(100):
t = randomTimeSequence()
p = TimeSeries(t, randomPositionSequence(t))
r = TimeSeries(t, randomRotationSequence(t))
T = SplinedTrajectory(SampledTrajectory(p, r), smoothRotations=False)
yield checkTrajectory, T, p, r
T = SplinedTrajectory(SampledTrajectory(p, r), smoothRotations=True)
yield checkTrajectory, T, p, TimeSeries(t, r.values.smoothed())
def fromArrays(time, position, rotation):
"""
Construct a L{SampledTrajectory} from time, position and rotation arrays.
@param time: sequence of sample times.
@param position: sequence of position samples.
@param rotation: sequence of rotation samples.
"""
return SampledTrajectory(TimeSeries(time, position),
TimeSeries(time, rotation))
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 __init__(self, keyFrames=None):
"""
Initialise trajectory.
@param keyFrames: A L{TimeSeries} of rotation key frame samples.
"""
self.rotationKeyFrames = TimeSeries() if keyFrames is None else keyFrames
def __init__(self, model, initialTime=0,
initialPosition=np.zeros((3,1)),
initialVelocity=np.zeros((3,1)),
accelerationVar=0.2, velocityVar=0.5,
rejectionProbabilityThreshold=0.4,
**kwargs):
"""
@param initialVelocity: Initial velocity estimate (3x1 L{np.ndarray})
@param accelerationVar: Acceleration variance (float).
@param velocityVar: Velocity variance (float).
@param rejectionProbabilityThreshold: Probability below which a
velocity update will be rejected (float, 0 <= p <= 1)
"""
PositionEstimator.__init__(self, model, initialTime, initialPosition)
self._accelerationSigma = math.sqrt(accelerationVar)
self._measurementVariance = np.diag([velocityVar]*3)
self._rejectionThreshold = rejectionProbabilityThreshold
self._points = list(model.points)
for point in self._points:
point.contactProbabilities = TimeSeries()
self.kalman = KalmanFilter(
stateTransitionMatrix = self.transitionMatrix(1),
controlMatrix = self.controlMatrix(1),
measurementMatrix = np.hstack((np.zeros((3,3)),np.eye(3))),
state = np.vstack((initialPosition,initialVelocity)),
stateCovariance = self.processCovariance(1),
processCovariance = self.processCovariance(1),
measurementCovariance = self._measurementVariance)
def save_trajectory(trajectory, h5f):
traj_group = h5f.create_group('trajectory')
save_timeseries(trajectory.sampled.positionKeyFrames,
traj_group.create_group('position'))
rot_ts = TimeSeries(
trajectory.sampled.rotationKeyFrames.timestamps,
trajectory.sampled.rotationKeyFrames.values.array.T)
save_timeseries(rot_ts, traj_group.create_group('rotation'))
def load_trajectory(group):
pos_ts = load_timeseries(group['position'])
rot_array_ts = load_timeseries(group['rotation'])
rot_ts = TimeSeries(rot_array_ts.timestamps,
QuaternionArray(rot_array_ts.values.T))
sampled = SampledTrajectory(positionKeyFrames=pos_ts,
rotationKeyFrames=rot_ts)
splined = SplinedTrajectory(sampled, smoothRotations=False)
return splined
def __init__(self, initialTime, initialRotation):
"""
Initialise orientation filter.
@param initialTime: Initial time.
@param initialRotation: Initial rotation (L{Quaternion}).
"""
assert isinstance(initialRotation, Quaternion)
self.rotation = TimeSeries()
def __init__(self, camera_platform, end_time):
self.camera_platform = camera_platform
self.end_time = end_time
camera = self.camera_platform.camera
camera.measurements = TimeSeries()
# Start the sampling process
timer = self.camera_platform.timer
timer.callback = self._timer_callback
period = 1. / camera.frame_rate
timer.start(period, repeat=True)
def __init__(self, initialTime, initialRotation, k, **kwargs):
"""
@param k: Filter blending co-efficient (float). At each timestep, a
correction will be applied of M{1/k} times the difference between
the gyro integration and vector observation results.
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self._k = float(k)
self.qHat = initialRotation.copy()
self._vectorObservation = vector_observation.FQA()
self.vectorObservation = TimeSeries()
def __init__(self, initialTime, initialRotation, k, aT, **kwargs):
"""
@param aT: acceleration threshold (float). The correction step will
only be performed if the condition M{abs(norm(accel) - 1) <= aT}
is met.
"""
OrientationFilter.__init__(self, initialTime, initialRotation)
self.rotation.add(initialTime, initialRotation)
self._k = float(k)
self._aT = float(aT)
self.qHat = initialRotation.copy()
self._vectorObservation = vector_observation.GramSchmidt()
self.vectorObservation = TimeSeries()
def __init__(self,
imu,
samplingPeriod,
calibration=None,
filter=None,
sampleCallback=None,
initialTime=0):
"""
Initialise IMU behaviour.
@param imu: L{IMU} on which the behaviour executes.
@param samplingPeriod: Interval at which to sample and process (float).
@param calibration: Mapping from IMU L{Sensor}s to
L{SensorCalibration}s (dict).
@param filter: L{OrientationFilter} to update with each set of samples.
@param sampleCallback: Function to call after each sample. The
behaviour object will be passed as a single argument.
@param initialTime: Initial time for local timekeeping.
"""
self.imu = imu
self.samplingPeriod = samplingPeriod
self.calibration = calibration
self.filter = filter
for sensor in imu.sensors:
sensor.rawMeasurements = TimeSeries()
if self.calibration is not None:
sensor.calibratedMeasurements = TimeSeries()
self.sampleCallback = sampleCallback
self.timerMux = TimerMultiplexer(imu.timer)
PeriodicSampler(imu.adc, imu.sensors, VirtualTimer(self.timerMux),
samplingPeriod, self._handleSample)
self._time = initialTime
def load_observations(h5_file):
framegroup = h5_file['camera']
frames = sorted(framegroup.keys())
ts = TimeSeries()
for fkey in frames:
group = framegroup[fkey]
obs_dict = {
lm_id: ip.reshape(2, 1)
for lm_id, ip in zip(group['landmarks'].value,
group['measurements'].value.T)
}
t = group['time'].value
ts.add(t, obs_dict)
return ts
def __init__(self, posMarker, refMarkers, refVectors=None, refTime=0):
self.posMarker = posMarker
self.refMarkers = refMarkers
if refVectors is None:
self.refVectors = [
refMarker.position(refTime) - posMarker.position(refTime)
for refMarker in self.refMarkers
]
else:
self.refVectors = refVectors
assert (~np.any(np.isnan(self.refVectors)))
self.positionKeyFrames = self.posMarker.positionKeyFrames
t = self.positionKeyFrames.timestamps
vectorObservation = DavenportQ(self.refVectors)
measVectors = np.array([
ref.position(t) - self.posMarker.position(t)
for ref in self.refMarkers
])
rotations = QuaternionArray([
vectorObservation(*vectors).conjugate
for vectors in np.split(measVectors, len(t), axis=2)
])
self.rotationKeyFrames = TimeSeries(t, rotations)
def rotationKeyFrames(self):
t = self.sampled.rotationKeyFrames.timestamps
t = t[(t >= self.startTime) & (t <= self.endTime)]
r = self.rotation(t)
return TimeSeries(t, r)
def load_timeseries(group):
times = group['timestamps']
values = group['data']
return TimeSeries(timestamps=times, values=values)
def positionKeyFrames(self):
t = self.sampled.positionKeyFrames.timestamps
t = t[(t >= self.startTime) & (t <= self.endTime)]
p = self.position(t)
return TimeSeries(t, p)
def load_timeseries(group):
timestamps = group['timestamps'].value
data = group['data'].value
if data.shape[1] == 4:
data = QuaternionArray(data)
return TimeSeries(timestamps, data)
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)
def __init__(self, t):
sampled = SampledTrajectory(None, TimeSeries(t, randomRotationSequence(t)))
SplinedRotationTrajectory.__init__(self, sampled)
def __init__(self):
t = randomTimeSequence()
sampled = SampledTrajectory(
TimeSeries(t, randomPositionSequence(t)),
TimeSeries(t, randomRotationSequence(t)))
SplinedTrajectory.__init__(self, sampled)
