Esempio n. 1
0
    def __init__(self, parent, name=None, offset=None):
        """
        Construct point trajectory.

        @param parent: Parent L{Joint} in the body model hierarchy.
        @param name: Name of the point.
        @param offset: Offset of the point in the parent joint
            co-ordinate frame.
        """
        Point.__init__(self, parent, name, offset)
        OffsetTrajectory.__init__(self, parent, offset)
    def __init__(self, parent, name=None, offset=None):
        """
        Construct point trajectory.

        @param parent: Parent L{Joint} in the body model hierarchy.
        @param name: Name of the point.
        @param offset: Offset of the point in the parent joint
            co-ordinate frame.
        """
        Point.__init__(self, parent, name, offset)
        OffsetTrajectory.__init__(self, parent, offset)
Esempio n. 3
0
    def __init__(self, parent, name=None, offset=None):
        """
        Initialise sampled joint.

        @param parent: Parent L{Joint} in the body model hierarchy.
        @param name: Name of the joint.
        @param offset: Offset of the joint in the parent joint
            co-ordinate frame.
        """
        Joint.__init__(self, parent, name, offset)
        OffsetTrajectory.__init__(self, parent, offset)
        SampledRotationTrajectory.__init__(self)
    def __init__(self, parent, name=None, offset=None):
        """
        Initialise sampled joint.

        @param parent: Parent L{Joint} in the body model hierarchy.
        @param name: Name of the joint.
        @param offset: Offset of the joint in the parent joint
            co-ordinate frame.
        """
        Joint.__init__(self, parent, name, offset)
        OffsetTrajectory.__init__(self, parent, offset)
        SampledRotationTrajectory.__init__(self)
Esempio n. 5
0
    def __init__(self, parent, sampled, **kwargs):
        """
        Initialise splined joint.

        @param sampled: the L{SampledJoint} from which to generate splined
            trajectories
        @param parent: the parent joint in the body model hierarchy
        """
        Joint.__init__(self, parent, sampled.name, sampled.positionOffset)
        OffsetTrajectory.__init__(self, parent, sampled.positionOffset)
        SplinedRotationTrajectory.__init__(self, sampled, **kwargs)
        self.children = [
                (SplinedJoint(self, child, **kwargs) if child.isJoint
                else PointTrajectory(self, child.name, child.positionOffset))
                for child in sampled.children]
Esempio n. 6
0
    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)
Esempio n. 7
0
 def _trajectoryChange(self):
     parentTrajectory = self.platform.trajectory
     if parentTrajectory is None:
         self.trajectory = None
     else:
         self.trajectory = OffsetTrajectory(parentTrajectory,
             self._positionOffset, self._rotationOffset)
    def __init__(self, parent, sampled, **kwargs):
        """
        Initialise splined joint.

        @param sampled: the L{SampledJoint} from which to generate splined
            trajectories
        @param parent: the parent joint in the body model hierarchy
        """
        Joint.__init__(self, parent, sampled.name, sampled.positionOffset)
        OffsetTrajectory.__init__(self, parent, sampled.positionOffset)
        SplinedRotationTrajectory.__init__(self, sampled, **kwargs)
        self.children = [
            (SplinedJoint(self, child, **kwargs) if child.isJoint else
             PointTrajectory(self, child.name, child.positionOffset))
            for child in sampled.children
        ]
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)
Esempio n. 10
0
def testAgainstReality():
    dir = path.dirname(__file__)
    filebase = path.join(dir, "swing")
    refbase = path.join(dir, "stand")
    magbases = [path.join(dir, f) for f in ['magsweep1', 'magsweep2']]
    maglookup = {'Upper Leg IMU': '66', 'Orient 8': '8', 'Orient 43': '43'}
    magSamples = 2000
    refTime = 1.0
    posStdDev = 0.0005
    rotStdDev = 0.004
    ref3D = SplinedMarkerCapture(loadQualisysTSVFile(refbase + "_3D.tsv"),
                                 positionStdDev=posStdDev)
    ref6D = SplinedMarkerCapture(loadQualisysTSVFile(refbase + "_6D.tsv"),
                                 rotationStdDev=rotStdDev)
    capture3D = SplinedMarkerCapture(loadQualisysTSVFile(filebase + "_3D.tsv"),
                                     positionStdDev=posStdDev)
    captureSD = SensorDataCapture.load(filebase + ".sdc")
    hip, thigh, knee, shin, ankle = \
            ['Hip', 'Thigh', 'Knee Hinge', 'Shin', 'Ankle']
    jointNames = ['Upper Leg', 'Lower Leg', 'Foot']
    jointAbbrevs = ['femur', 'tibia', 'foot']
    orientIDs = ['66', '43', '8']
    jointMarkerNames = [hip, knee, ankle]
    refMarkerNames = [[thigh, knee], [shin, ankle], []]
    imuMarkerNames = \
            [[j + ' IMU - ' + str(i) for i in range(1,4)] for j in jointNames]
    jointMarkerSets = lambda c: [
        list(map(c.marker, jointMarkerNames)),
        [list(map(c.marker, r)) for r in refMarkerNames],
        [list(map(c.marker, i)) for i in imuMarkerNames]
    ]
    imuMarkerSets = lambda c: [[
        c.marker(i[0]) for i in imuMarkerNames
    ], [list(map(c.marker, i[1:])) for i in imuMarkerNames]]
    jointRefTrajectories = [
        MultiMarkerTrajectory(j, r + i, refTime=refTime)
        for j, r, i in zip(*(jointMarkerSets(ref3D)))
    ]
    jointTrajectories = [
        MultiMarkerTrajectory(j, r + i, refVectors=m.refVectors) \
            for j, r, i, m in \
                zip(*(jointMarkerSets(capture3D) + [jointRefTrajectories]))]
    imuRefTrajectories = [
        MultiMarkerTrajectory(p, r, refTime=refTime)
        for p, r in zip(*(imuMarkerSets(ref3D)))
    ]
    imuVecTrajectories = [
        MultiMarkerTrajectory(p, r, refVectors=m.refVectors)
        for p, r, m in zip(*(imuMarkerSets(capture3D) + [imuRefTrajectories]))
    ]
    imuRefMarkers = [ref6D.marker(j + ' IMU') for j in jointNames]
    imuOffsets = [
        i.position(refTime) - j.position(refTime)
        for i, j in zip(imuRefTrajectories, jointRefTrajectories)
    ]
    imuRotations = [
        t.rotation(refTime).conjugate * m.rotation(refTime)
        for t, m in zip(imuRefTrajectories, imuRefMarkers)
    ]
    imuTrajectories = [
        OffsetTrajectory(v, o, r)
        for v, o, r in zip(imuVecTrajectories, imuOffsets, imuRotations)
    ]
    imuData = [captureSD.device(i) for i in orientIDs]
    joints = []
    for i in range(len(jointNames)):
        name = jointNames[i]
        traj = jointTrajectories[i]
        if i == 0:
            model = SampledBodyModel(name)
            model.positionKeyFrames = traj.posMarker.positionKeyFrames
            joint = model
        else:
            parent = joints[-1]
            refTraj = jointRefTrajectories[i]
            parentRefTraj = jointRefTrajectories[i - 1]
            pos = refTraj.position(refTime)
            parentPos = parentRefTraj.position(refTime)
            joint = SampledJoint(joints[-1], name, offset=(pos - parentPos))
        joint.rotationKeyFrames = traj.rotationKeyFrames
        joints.append(joint)
    model = SplinedBodyModel(model)
    joints = model.joints
    imuJointTrajectories = [
        OffsetTrajectory(j, o, r)
        for j, o, r in zip(joints, imuOffsets, imuRotations)
    ]
    positionSets = []
    valueSets = []
    for magbase in magbases:
        orient = SensorDataCapture.load(magbase + '.sdc')
        optical = loadQualisysTSVFile(magbase + '_6D.tsv')
        for marker in optical.markers:
            device = orient.device(maglookup[marker.id])
            magData = device.sensorData('magnetometer').values
            positionSets.append(marker.positionKeyFrames.values)
            valueSets.append(
                marker.rotationKeyFrames.values.rotateVector(magData))
    positions = np.hstack(positionSets)
    values = np.hstack(valueSets)
    valid = ~np.any(np.isnan(positions), axis=0) & ~np.any(np.isnan(values),
                                                           axis=0)
    dev = values - np.median(values[:, valid], axis=1).reshape((3, 1))
    step = np.shape(values[:, valid])[1] // magSamples
    posSamples = positions[:, valid][:, ::step]
    valSamples = values[:, valid][:, ::step]
    environment = Environment()
    environment.magneticField = \
            NaturalNeighbourInterpolatedField(posSamples, valSamples)
    sim = Simulation(environment=environment)
    sim.time = model.startTime
    distortIMUs = []
    dt = 1 / capture3D.sampled.frameRate
    for traj in imuJointTrajectories:
        platform = IdealIMU(sim, traj)
        distortIMUs.append(BasicIMUBehaviour(platform, dt))
    sim.run(model.endTime)
    for imu in range(3):
        for sensorName in ['accelerometer', 'magnetometer', 'gyroscope']:
            sim = getattr(distortIMUs[imu].imu, sensorName).rawMeasurements
            true = imuData[imu].sensorData(sensorName)(sim.timestamps +
                                                       model.startTime)
            yield assert_vectors_correlated, sim.values, true, 0.8