@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { Random random = new Random(3213620L); RigidBodyTransform expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); long expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(expectedTransform, expectedTimestamp); assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); assertTrue("TimestampedTransform should only copy the given transform into an internal variable", expectedTransform != toBeTested.getTransform3D()); }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
if (floorData.getTimeStamp() == timestamp) timeStampedTransform3DToPack.set(floorData); return true; else if (floorData.getTimeStamp() < timestamp)
public HeadToSubFrameTransformBuffer(int size) { this.size = size; transforms = new TimeStampedTransform3D[size]; for (int i = 0; i < size; i++) { transforms[i] = new TimeStampedTransform3D(); transforms[i].setTimeStamp(Long.MIN_VALUE); } }
public synchronized void addRawTransformAndTimestamp(long timestamp, RigidBodyTransform transform3D) { transforms[currentIndex].setTimeStamp(timestamp); transforms[currentIndex].setTransform3D(transform3D); newestTimestamp = timestamp; currentIndex++; if (currentIndex >= size) { currentIndex = 0; } if (transforms[currentIndex].getTimeStamp() == Long.MIN_VALUE) { oldestTimestamp = newestTimestamp; } else { oldestTimestamp = transforms[currentIndex].getTimeStamp(); } }
TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp);
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = new StampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); packetCommunicator.send(posePacket); } };
TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D expected = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); TimeStampedTransform3D next = pelvisPosesRegistered.get(poseIndex + 1); for (long currentTimeStamp = previous.getTimeStamp(); currentTimeStamp <= next.getTimeStamp(); currentTimeStamp++) assertTrue(expected.epsilonEquals(toTest, 1.0e-10));
public StampedPosePacket(Random random) { this("/pelvis", new TimeStampedTransform3D(RigidBodyTransform.generateRandomTransform(random), random.nextLong()), 1.0); } }
public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIfInRange(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
public static StampedPosePacket createStampedPosePacket(String frameId, TimeStampedTransform3D transform, double confidenceFactor) { StampedPosePacket message = new StampedPosePacket(); message.getFrameId().append(frameId); message.getPose().set(transform.getTransform3D()); message.setTimestamp(transform.getTimeStamp()); message.setConfidenceFactor(confidenceFactor); return message; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransform() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); // Test that the getter returns the original transform and not a copy. toBeTested.getTransform3D().set(expectedTransform); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); } }
/** * pulls the corrected pose from the buffer, check that the nonprocessed buffer has * corresponding pelvis poses and calculates the total error */ private void processNewPacket() { StampedPosePacket newPacket = pelvisPoseCorrectionCommunicator.getNewExternalPose(); TimeStampedTransform3D timeStampedExternalPose = newPacket.getTransform(); if (stateEstimatorPelvisPoseBuffer.isInRange(timeStampedExternalPose.getTimeStamp())) { double confidence = newPacket.getConfidenceFactor(); confidence = MathTools.clipToMinMax(confidence, 0.0, 1.0); confidenceFactor.set(confidence); addNewExternalPose(timeStampedExternalPose); } }
TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp);
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); stampedPosePublisher.publish(posePacket); } };
TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp); timeStampedPelvisPoseBuffer.findTransform(expected.getTimeStamp(), toTest); assertTrue(expected.epsilonEquals(toTest, 1.0e-10));
public void put(RigidBodyTransform newestTransform, long timeStamp) { TimeStampedTransform3D recycledTimestampedTransformFromBuffer = buffer[currentIndex]; recycledTimestampedTransformFromBuffer.setTimeStamp(timeStamp); recycledTimestampedTransformFromBuffer.setTransform3D(newestTransform); newestTimestamp = timeStamp; currentIndex++; if (currentIndex >= size) { currentIndex = 0; filledBufferAtleastOnce = true; } if (oldestTimeStamp == Long.MAX_VALUE) { oldestTimeStamp = newestTimestamp; } else if(filledBufferAtleastOnce) { oldestTimeStamp = buffer[currentIndex].getTimeStamp(); } }
@Override public void run() { while (running()) { try { Thread.sleep(900); pelvisTransform.set(pelvis.getJointTransform3D()); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); Thread.sleep((int) (random.nextDouble() * 200)); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(pelvisTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); externalPelvisPoseCreator.setNewestPose(posePacket); } catch (InterruptedException e) { e.printStackTrace(); } } } };
public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIntervalContains(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
/** * 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); }