public synchronized void addTimeStampedTransform(TimeStampedTransform3D timeStampedTransform3D) { transforms[currentIndex].setTransform3D(timeStampedTransform3D.getTransform3D()); transforms[currentIndex].setTimeStamp(timeStampedTransform3D.getTimeStamp()); newestTimestamp = timeStampedTransform3D.getTimeStamp(); currentIndex++; if (currentIndex >= size) { currentIndex = 0; } if (transforms[currentIndex].getTimeStamp() == Long.MIN_VALUE) { oldestTimestamp = newestTimestamp; } else { oldestTimestamp = transforms[currentIndex].getTimeStamp(); } }
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(); } }
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(); } }
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(); } }
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); } }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
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); } }
@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); } };
@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); } };
@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, RosLocalizationConstants.DEFAULT_OVERLAP); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); controllerCommunicationBridge.send(posePacket); } };
@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()); packetCommunicator.send(posePacket); } };
@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()); rosModulePacketCommunicator.send(posePacket); } };
@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, RosLocalizationConstants.DEFAULT_OVERLAP); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); controllerCommunicationBridge.send(posePacket); } };
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 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()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetters() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); long expectedTimestamp = 0; assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); Random random = new Random(3213620L); expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); toBeTested.setTimeStamp(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)); expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); toBeTested.setTransform3D(expectedTransform); assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); TimeStampedTransform3D expectedTimeStampedTransform = new TimeStampedTransform3D(expectedTransform, expectedTimestamp); toBeTested.set(expectedTimeStampedTransform); assertEquals("Timestamp is different from what was expected", expectedTimeStampedTransform.getTimeStamp(), toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTimeStampedTransform.getTransform3D().epsilonEquals(toBeTested.getTransform3D(), EPSILON)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testEmptyConstructor() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); long expectedTimestamp = 0; assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); 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); } }
/** * 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); }