class Simulation(object):
    """
    Main IMUSim simulation environment.

    @ivar rng: L{np.random.RandomState} instance for random number generation.
    @ivar environment: L{Environment} in use for simulation.
    @ivar engine: L{SimPy.Simulation} instance driving simulation.
    """

    def __init__(self, seed=None, environment=None,
            engine=SimPy.Simulation.Simulation):
        """
        Initialise simulation.

        @param seed: Seed value for L{np.random.RandomState}.
        @param environment: L{Environment} to simulate in.
        @param engine: L{SimPy.Simulation} subclass to drive simulation.
        """
        self.environment = Environment() if environment is None else environment
        self.engine = engine()
        self.engine.initialize()
        self.rng = np.random.RandomState(seed)

    @property
    def time(self):
        """ The current true time within the simulation (float). """
        assert hasattr(self, '_startTime'), 'Simulation time has not been set.'
        return self.engine.now() + self._startTime

    @time.setter
    def time(self, time):
        assert not hasattr(self, '_startTime'), 'Simulation time already set.'
        self._startTime = time

    class ProgressMonitor(SimPy.Simulation.Process):
        """
        Simple process to print status and remaining simulation time at 5%
        progress intervals.
        """
        def __init__(self, sim, duration):
            SimPy.Simulation.Process.__init__(self, sim=sim)
            self.startTime = sim.now()
            self.duration = duration
            sim.activate(self, self.printProgress())

        def printProgress(self):
            for i in range(0,20):
                start = time.time()
                yield SimPy.Simulation.hold, self, self.duration / 20.0
                now = self.sim.now() - self.startTime
                end = time.time()

                remaining = (20-i) * (end-start)
                print ("Simulated %.1fs of %.1fs (%3.0f%%). " +
                        "Estimated time remaining %.1fs") \
                        %(now, self.duration, (i + 1) * 5, remaining)

    def run(self, endTime, printProgress=True):
        """
        Advance the simulation up to the given time.

        @param endTime: Simulation time at which to stop (float).
        @param printProgress: Whether to print progress information (bool).
        """

        if printProgress:
            print "Simulating..."
            startTime = self.time
            duration = endTime - startTime
            progressMonitor = self.ProgressMonitor(self.engine, duration)

        startWallTime = time.time()
        self.engine.simulate(endTime - self._startTime)
        endWallTime = time.time()

        if printProgress:
            print "Simulation complete."
            print "Simulated %.1f seconds in %.1f seconds." % (
                    endTime - startTime, endWallTime - startWallTime)

    def subrng(self):
        """
        Obtain a new random number generator seeded from the main RNG.

        @return: A L{np.random.RandomState} instance.
        """
        return np.random.RandomState(seed=int(self.rng.uniform(0,2**31))) def loadBVHFile(filename, conversionFactor=1):
    """
    Load sampled body model data from a BVH file.

    @param filename: Name of file to load.
    @param conversionFactor: Scale factor to apply to position values, e.g.
        to convert meters to centimeters use conversionFactor=100.

    @return: A L{SampledBodyModel} object.
    """
    with open(filename,'r') as bvhFile:
        loader = BVHLoader(bvhFile,conversionFactor)
        loader._readHeader()
        loader._readMotionData()
    return loader.model class OrientationFilter(object):
class GyroIntegrator(OrientationFilter):
class BachmannCF(OrientationFilter):
class OrientCF(OrientationFilter):
class YunEKF(OrientationFilter):
class DistLinAccelCF(OrientationFilter):
    def __init__(self, initialTime, initialRotation):
    def __call__(self, accel, mag, gyro, t):
    def _update(self, accel, mag, gyro, dt, t):
    def __init__(self, initialTime, initialRotation, **kwargs):
    def _update(self, accel, mag, gyro, dt, t):
    def __init__(self, initialTime, initialRotation, k, **kwargs):
    def _update(self, accel, mag, gyro, dt, t):
    def __init__(self, initialTime, initialRotation, k, aT, **kwargs):
    def _update(self, accel, mag, gyro, dt, t):
    def __init__(self, initialTime, initialRotation, initialRotationalVelocity,
            initialCovariance, measurementCovariance, D, tau, **kwargs):
    def _update(self, accel, mag, gyro, dt, t):
    def dotx(self, xHat):
    def Phi(self, xHat, dt):
    def __init__(self, initialTime, initialRotation, k, joint, offset,
            **kwargs):
    def handleLinearAcceleration(self, jointAcceleration, dt):
    def childAcceleration(self, o, dt):
    def _update(self, accel, mag, gyro, dt, t):
        K = self.P_minus * np.linalg.inv(self.P_minus + self.R)
        P = (np.eye(7) - K) * self.P_minus
    GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY) class OrientationFilter(object):
    """
    Base class for orientation estimation filters.

    An orientation filter takes accelerometer, magnetometer and gyroscope
    readings from an IMU and attempts to track the orientation of the IMU
    as a L{Quaternion} from these measurements.

    @ivar rotation: L{TimeSeries} of quaternion orientation estimates.
    """

    __metaclass__ = ABCMeta

    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 __call__(self, accel, mag, gyro, t):
        """
        Run filter update.

        @param accel: Accelerometer sample (3x1 L{np.ndarray})
        @param mag: Magnetometer sample (3x1 L{np.ndarray})
        @param gyro: Gyroscope sample (3x1 L{np.ndarray})
        @param t: Time at which samples were taken.
        """
        dt = t - self.rotation.latestTime
        self._update(accel, mag, gyro, dt, t)

    @abstractmethod
    def _update(self, accel, mag, gyro, dt, t):
        """
        Internal filter update routine.

        Should add its results to the self.rotation L{TimeSeries}.
        """
        pass class MagicIMU(StandardIMU):
    """
    An IMU with idealised components including a fictional gravity sensor.
    """
    def __init__(self, simulation=None, trajectory=None):
        self.accelerometer = IdealGravitySensor(self)
        self.magnetometer = IdealMagnetometer(self)
        self.gyroscope = IdealGyroscope(self)
        self.adc = IdealADC(self)
        self.timer = IdealTimer(self)
        self.radio = IdealRadio(self)
        StandardIMU.__init__(self, simulation, trajectory) class ScaleAndOffsetCalibrator(Calibrator):
    """
    Simple calibration procedure that obtains scale and offset factors.

    This procedure simulates testing the device in 18 placements:

        1. Stationary on a flat surface in 6 orthogonal orientations, such
           that gravity is aligned with each direction of each IMU axis.

        2. Stationary on an inclined surface in 6 orthogonal orientations,
           such that magnetic north is aligned with each direction of each
           IMU axis.

        3. On a rotating stage of known angular velocity in 6 orthogonal
           orientations, such that the axis of rotation is aligned with each
           direction of each IMU axis.
    """

    @prepend_method_doc(Calibrator)
    def __init__(self, environment, samples, samplingPeriod,
            rotationalVelocity):
        """
        @param samples: Number of samples to take in each placement (int).
        @param samplingPeriod: Time between samples (float, s).
        @param rotationalVelocity: Angular rate of rotations (float, rad/s).
        """
        Calibrator.__init__(self, environment)
        self.samples = samples
        self.samplingPeriod = samplingPeriod
        self.rotationalVelocity = rotationalVelocity

    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 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 class BasicIMUBehaviour(object):
    """
    Basic behaviour for an IMU that performs periodic sampling.

    @ivar imu: L{IMU} on which the behaviour executes.
    @ivar samplingPeriod: Interval at which to sample and process (float).
    @ivar calibration: Mapping from IMU L{Sensor}s to L{SensorCalibration}s
        (dict).
    @ivar filter: L{OrientationFilter} to update with each set of samples.
    @ivar sampleCallback: Function to call after each sample. The behaviour
        object will be passed as a single argument.
    @ivar timerMux: L{TimerMultiplexer} for IMU timer.
    """

    __metaclass__ = ABCMeta

    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 _handleSample(self, *measurements):
        measurements = list(measurements)
        self._time += self.samplingPeriod
        for sensor, measurement in zip(self.imu.sensors, measurements):
            sensor.rawMeasurements.add(self._time, measurement)
            if self.calibration is not None:
                measurement = self.calibration[sensor].apply(measurement)
                sensor.calibratedMeasurements.add(self._time, measurement)
        if self.filter is not None:
            self.filter(*measurements + [self._time])
        if self.sampleCallback != None:
            self.sampleCallback(self) class TimerMultiplexer(object):
    """
    Multiplexer for running multiple virtual timers on one hardware timer.
    """

    __metaclass__ = ABCMeta

    def __init__(self, timer):
        """
        Create timer multiplexer.

        @param timer: L{Timer} to multiplex.
        """
        self._timer = timer
        self._vtimers = []
        self._period = None
        self._inHandler = False
        timer.clear()
        timer.callback = self._timerHandler

    def _timerHandler(self):
        """
        Timer event handler.
        """

        self._inHandler = True
        for vtimer in self._vtimers:
            if vtimer._period is not None and not vtimer._done:
                vtimer._remaining -= self._period
                if vtimer._remaining <= 0:
                    if vtimer._repeat:
                        vtimer._remaining = vtimer._period
                    else:
                        vtimer._period = None
                    if vtimer.callback is not None:
                        vtimer.callback()
        self._inHandler = False

        for vtimer in self._vtimers:
            vtimer._done = False

        waits = [vtimer._remaining for vtimer in self._vtimers
                if vtimer._period is not None]

        if len(waits) > 0:
            self._period = min(waits)
            self._timer.start(self._period, self._timerHandler)
        else:
            self._period = None class MasterMAC(MAC):
    """
    Master MAC implementation.
    """

    @prepend_method_doc(MAC)
    def __init__(self, radio, timerMux, schedule, frameHandler=None):
        """
        @param schedule: L{Schedule} object giving transmission scheduling.
        @param frameHandler: Handler function to call at the end of each frame.
            The packets received in that frame will be passed as a list.
        """
        MAC.__init__(self, radio, timerMux)
        self.schedule = schedule
        self.frameHandler = frameHandler
        self.frameTimer = VirtualTimer(timerMux, self._frameTimerHandler)
        self.packets = []
        self.frameTimer.start(self.schedule.framePeriod, repeat=True)

    def _frameTimerHandler(self):
        if self.frameHandler is not None:
            self.frameHandler(self.packets)
        self.packets = []
        self.radio.transmit(SyncPacket())

    def queuePacket(self, packet):
        raise NotImplementedError

    def handleIncomingPacket(self, packet):
        if isinstance(packet, DataPacket):
            self.packets.append(packet) class SlaveMAC(MAC):
    """
    Slave MAC implementation.
    """

    @prepend_method_doc(MAC)
    def __init__(self, radio, timerMux, schedule, id):
        """
        @param schedule: L{Schedule} object giving transmission scheduling.
        @param id: The device ID of this node.
        """
        MAC.__init__(self, radio, timerMux)
        self.schedule = schedule
        self.id = id
        self.dataTxSlot = self.schedule.dataTxSlot(self.id)
        self.slotTimer = VirtualTimer(timerMux, self._slotTimerHandler)
        self.slot = len(self.schedule.dataSlots)
        self.txPacket = None

    def _slotTimerHandler(self):
        if self.slot == len(self.schedule.dataSlots):
            # End of sync slot, start of first data slot.
            self.slot = 0
        else:
            self.slot += 1

        if self.slot == len(self.schedule.dataSlots):
            # Start of sync slot.
            self.slotTimer.start(self.schedule.syncSlotTime)
        else:
            self.slotTimer.start(self.schedule.dataSlotTime)

        if self.slot == self.dataTxSlot:
            # Our transmit slot.
            if self.txPacket is not None:
                self.radio.transmit(self.txPacket)
                self.txPacket = None

    def queuePacket(self, packet):
        if isinstance(packet, DataPacket):
            self.txPacket = packet
        else:
            raise ValueError

    def handleIncomingPacket(self, packet):
        if isinstance(packet, SyncPacket):
            self.slotTimer.start(self.schedule.syncSlotTime/2) class Schedule(object):
    """
    Transmission schedule.
    """

    def __init__(self, dataSlotTime, syncSlotTime, dataSlots):
        """
        Initialise schedule.

        @param dataSlotTime: Data slot duration (float, s).
        @param syncSlotTime: Synchronisation slot duration (float, s).
        @param dataSlots: List of device IDs indexed by allocated slot.
        """
        self.dataSlotTime = dataSlotTime
        self.syncSlotTime = syncSlotTime
        self.dataSlots = dataSlots

    @property
    def framePeriod(self):
        """ Duration of one complete frame. """
        return len(self.dataSlots) * self.dataSlotTime + self.syncSlotTime

    def dataTxSlot(self, id):
        """ Transmission slot number for given device ID. """
        return self.dataSlots.index(id) class DataPacket(RadioPacket):
    """
    Data packet from slave to master.

    @ivar source: Source device ID.
    """
    def __init__(self, source, data):
        """
        Initialise data packet.
        @param source: Source device ID.
        @param data: Payload data.
        """
        RadioPacket.__init__(self, data)
        self.source = source

    @property
    def bytes(self):
        size = 2
        for key in self.keys():
            data = self[key]
            if isinstance(data, Quaternion):
                size += array(data.components).nbytes
            elif isinstance(data, numpy.ndarray):
                size += data.nbytes
        return size class StaticTrajectory(ConstantPositionTrajectory, ConstantRotationTrajectory):
    """
    Represents the trajectory of a static object.
    """

    def __init__(self, position=None, rotation=None):
        """
        Construct static trajectory.

        @param position: the constant position of the object (3x1 L{np.ndarray})
        @param rotation: the constant rotation of the object (L{Quaternion})
        """
        ConstantPositionTrajectory.__init__(self, position)
        ConstantRotationTrajectory.__init__(self, rotation)

    @property
    def startTime(self):
        return 0

    @property
    def endTime(self):
        return np.inf class IdealBasestation(Platform):
    """
    A simple basestation with ideal components.

    @ivar timer: The basestation L{Timer}.
    @ivar radio: The basestation L{Radio}
    """

    def __init__(self, simulation=None, trajectory=None):
        self.timer = IdealTimer(self)
        self.radio = IdealRadio(self)
        Platform.__init__(self, simulation, trajectory)

    @property
    def components(self):
        return [self.timer, self.radio] class SampledBodyModel(SampledPositionTrajectory, SampledJoint):
    """A body model with sampled positions and rotations."""

    def __init__(self, name=None):
        """
        Initialise body model.

        @param name: The name of the root L{Joint} of the body model
        """
        SampledJoint.__init__(self, None, name)
        SampledPositionTrajectory.__init__(self)

    @classmethod
    def structureCopy(cls, ref, parent=None):
        model = cls(ref.name)
        for child in ref.children:
            if child.isJoint:
                SampledJoint.structureCopy(child, model)
            else:
                PointTrajectory(model, child.name, child.positionOffset)
        return model class SplinedBodyModel(SplinedPositionTrajectory, SplinedJoint):
    """A body model with positions and rotations splined from sampled data."""

    def __init__(self, sampled, **kwargs):
        """
        Initialise splined body model.

        @param sampled: The L{SampledBodyModel} from which to generated
            splined trajectories
        """
        SplinedJoint.__init__(self, None, sampled, **kwargs)
        SplinedPositionTrajectory.__init__(self, sampled, **kwargs)

    @property
    def startTime(self):
        return max(self._positionStartTime, self._rotationStartTime)

    @property
    def endTime(self):
        return min(self._positionEndTime, self._rotationEndTime) class BodyModelReconstructor(object):
    """
    Reconstructs body model movements from individual joint data.

    This behaviour combines a L{PostureReconstructor} and
    a L{PositionEstimator} to update a L{SampledBodyModel}.

    @ivar bodyModel: L{SampledBodyModel} being updated.
    """

    def __init__(self, bodyModel, initialPosition=np.zeros((3,1)),
            postureReconstructor=SimpleForwardKinematics,
            positionEstimator=ConstantPosition, initialTime=0):
        """
        Initialise behaviour.

        @param bodyModel: L{SampledBodyModel} to update.
        @param postureReconstructor: L{PostureReconstructor} subclass to use.
        @param positionEstimator: L{PositionEstimator} subclass to use.
        @param initialTime: Initial time.
        """
        self.bodyModel = bodyModel
        self._postureEstimator = postureReconstructor(self.bodyModel)
        self._positionEstimator = positionEstimator(self.bodyModel,
                initialPosition = initialPosition)
        self._time = initialTime

    def handleData(self, data, dt):
        """
        Update the body model with new data.

        @param data: A L{dict} containing data, passed directly to both the
            posture and position estimators.
        @param dt: Time elapsed since last call to this method or __init__.
        """
        self._time += dt
        self._postureEstimator(data, self._time)
        self._positionEstimator(data, self._time)
from imusim.simulation.base import Simulation
from imusim.io.bvh import loadBVHFile
from imusim.algorithms import orientation
from imusim.algorithms.orientation import OrientationFilter
from imusim.testing.inspection import getImplementations
from imusim.platforms.imus import MagicIMU
from imusim.simulation.calibrators import ScaleAndOffsetCalibrator
from imusim.behaviours.imu import BasicIMUBehaviour
from imusim.behaviours.timing import TimerMultiplexer
from imusim.behaviours.orient_tdma import MasterMAC, SlaveMAC
from imusim.behaviours.orient_tdma import Schedule, DataPacket
from imusim.trajectories.base import StaticTrajectory
from imusim.platforms.basestations import IdealBasestation
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.behaviours.basestation import BodyModelReconstructor
from imusim.testing.quaternions import assert_quaternions_correlated
from os import path
import numpy as np
"""
Tests for overall system simulation.
"""
# Copyright (C) 2009-2011 University of Edinburgh
#
# This file is part of IMUSim.
#
# IMUSim is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# IMUSim is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# 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/>.


testMotion = path.join(path.dirname(__file__), 'walk.bvh')

filterParameters = {
    orientation.OrientCF : {
        'k' : 1,
        'aT' : 1},
    orientation.BachmannCF : {
        'k' : 1},
    orientation.YunEKF : {
        'initialCovariance' : np.asmatrix(
            np.diag([0.01]*3 + [0.0001]*4)),
        'measurementCovariance' : np.asmatrix(
            np.diag([0.01]*3 + [0.0001]*4)),
        'D' : 50,
        'tau' : 0.5}
    }

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))