def position(self, t): if len(self.positionKeyFrames) == 0: if np.ndim(t) == 0: return vectors.nan() else: return vectors.nan(len(t)) else: return self.positionKeyFrames(t)
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()