@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testEmptyBuffer() { Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(5151); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), 0L); assertEquals(timeStampedPelvisPoseBuffer.getOldestTimestamp(), Long.MAX_VALUE); for (int i = 0; i < 1000; i++) assertFalse(timeStampedPelvisPoseBuffer.isInRange(random.nextLong())); }
public void getStateEstimatorTransform(long timestamp, TimeStampedTransform3D timeStampedTransform3DToPack) { stateEstimatorTimeStampedTransformBuffer.findTransform(timestamp, timeStampedTransform3DToPack); }
/** * Puts the stateEstimatorTransform in the buffer with the corresponding timeStamp * @param stateEstimatorTransform * @param timeStamp */ public void putStateEstimatorTransformInBuffer(RigidBodyTransform stateEstimatorTransform, long timeStamp) { stateEstimatorTimeStampedTransformBuffer.put(stateEstimatorTransform, timeStamp); }
Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(bufferSize); TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp); assertEquals(timeStampedPelvisPoseBuffer.getOldestTimestamp(), expectedOldestTimeStamp); timeStampedPelvisPoseBuffer.findTransform(expected.getTimeStamp(), toTest);
stateEstimatorPelvisPoseBuffer.put(lastPelvisPoseInWorldFrame, TimeTools.secondsToNanoSeconds(yoTime.getDoubleValue())); stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast);
/** * @param timeStamp * @return true if the timeStamp is in range of the stateEstimatorTimeStampedBuffer */ public boolean stateEstimatorTimeStampedBufferIsInRange(long timeStamp) { return stateEstimatorTimeStampedTransformBuffer.isInRange(timeStamp); }
stateEstimatorTimeStampedTransformBuffer = new TimeStampedTransformBuffer(stateEstimatorBufferSize); stateEstimatorTimeStampedTransformInPast = new TimeStampedTransform3D(); stateEstimatorPoseInThePast = new FramePose(worldFrame);
public long getStateEstimatorTimeStampedBufferNewestTimestamp() { return stateEstimatorTimeStampedTransformBuffer.getNewestTimestamp(); }
public long getStateEstimatorTimeStampedBufferOldestTimestamp() { return stateEstimatorTimeStampedTransformBuffer.getOldestTimestamp(); }
Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(bufferSize); TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp); timeStampedPelvisPoseBuffer.findTransform(expected.getTimeStamp(), toTest);
stateEstimatorPelvisPoseBuffer.put(lastPelvisPoseInWorldFrame, Conversions.secondsToNanoseconds(yoTime.getDoubleValue())); stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast);
if (!isInRange(timestamp)) return false;
parentRegistry.addChild(registry); stateEstimatorPelvisPoseBuffer = new TimeStampedTransformBuffer(pelvisBufferSize);
public void manuallyTriggerLocalizationUpdate() { confidenceFactor.set(1.0); RigidBodyTransform pelvisPose = new RigidBodyTransform(); Quat4d rotation = new Quat4d(); pelvisPose.getRotation(rotation); RotationTools.convertYawPitchRollToQuaternion(manualRotationOffsetInRadZ.getDoubleValue(), manualRotationOffsetInRadY.getDoubleValue(), manualRotationOffsetInRadX.getDoubleValue(), rotation); pelvisPose.setRotation(rotation); Vector3d translation = new Vector3d(); pelvisPose.getTranslation(translation); translation.setX(manualTranslationOffsetX.getDoubleValue()); translation.setY(manualTranslationOffsetY.getDoubleValue()); translation.setZ(manualTranslationOffsetZ.getDoubleValue()); pelvisPose.setTranslation(translation); TimeStampedTransform3D manualTimeStampedTransform3D = new TimeStampedTransform3D(pelvisPose, stateEstimatorPelvisPoseBuffer.getNewestTimestamp()); addNewExternalPose(manualTimeStampedTransform3D); manuallyTriggerLocalizationUpdate.set(false); }
Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(bufferSize); TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D expected = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp); timeStampedPelvisPoseBuffer.findTransform(currentTimeStamp, toTest);
if (!isInRange(timestamp)) return false;
minMaxIcpTranslationOffset.set(0.06); previousIcpPacketSentTime = new DoubleYoVariable("DiagnosticBehaviorPreviousIcpPacketSentTime", registry); stateEstimatorPelvisPoseBuffer = new TimeStampedTransformBuffer(10000); icpTimeDelay = new DoubleYoVariable(getName() + "IcpTimeDelay", registry); icpTimeDelay.set(0.99);
/** * Calculates the difference between the external at t with the state estimated pelvis pose at t and stores it * @param localizationPose - the corrected pelvis pose */ public void calculateAndStoreErrorInPast(TimeStampedTransform3D timestampedlocalizationPose) { long timeStamp = timestampedlocalizationPose.getTimeStamp(); RigidBodyTransform localizationPose = timestampedlocalizationPose.getTransform3D(); localizationPose.getTranslation(localizationTranslationInPast); newLocalizationTranslationFrame.setAndUpdate(localizationTranslationInPast); localizationPose.getRotation(localizationRotationInPast); newLocalizationRotationFrame.setAndUpdate(localizationRotationInPast); stateEstimatorPelvisPoseBuffer.findTransform(timeStamp, seTimeStampedPose); RigidBodyTransform sePose = seTimeStampedPose.getTransform3D(); sePose.getTranslation(seTranslationInPast); pelvisStateAtLocalizationTimeTranslationFrame.setAndUpdate(seTranslationInPast); sePose.getRotation(seRotationInPast); pelvisStateAtLocalizationTimeRotationFrame.setAndUpdate(seRotationInPast); newLocalizationTranslationFrame.getTransformToDesiredFrame(translationErrorInPastTransform, pelvisStateAtLocalizationTimeTranslationFrame); newLocalizationRotationFrame.getTransformToDesiredFrame(rotationErrorInPastTransform, pelvisStateAtLocalizationTimeRotationFrame); totalTranslationErrorFrame.setAndUpdate(translationErrorInPastTransform); totalRotationErrorFrame.setAndUpdate(rotationErrorInPastTransform); }
/** * adds noncorrected pelvis poses to buffer for pelvis pose lookups in past * @param pelvisPose non-corrected pelvis pose * @param timeStamp robot timestamp of pelvis pose */ private void addPelvisePoseToPelvisBuffer(RigidBodyTransform pelvisPose, long timeStamp) { seNonProcessedPelvisTimeStamp.set(timeStamp); stateEstimatorPelvisPoseBuffer.put(pelvisPose, timeStamp); }
Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(bufferSize); TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D expected = new TimeStampedTransform3D(); for (int j = 0; j < RandomNumbers.nextInt(random, 1, bufferSize); j++) timeStampedPelvisPoseBuffer.put(EuclidCoreRandomTools.nextRigidBodyTransform(random), timeStamp); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp); timeStampedPelvisPoseBuffer.findTransform(currentTimeStamp, toTest);