public TimeStampedTransformBuffer(int size) { this.size = size; this.buffer = new TimeStampedTransform3D[size]; for(int i = 0; i < buffer.length; i++) { this.buffer[i] = new TimeStampedTransform3D(); this.buffer[i].setTimeStamp(Long.MAX_VALUE); } currentIndex = 0; }
public TimeStampedTransformBuffer(int size) { this.size = size; this.buffer = new TimeStampedTransform3D[size]; for(int i = 0; i < buffer.length; i++) { this.buffer[i] = new TimeStampedTransform3D(); this.buffer[i].setTimeStamp(Long.MAX_VALUE); } currentIndex = 0; }
public StampedPosePacket(Random random) { this("/pelvis", new TimeStampedTransform3D(RigidBodyTransform.generateRandomTransform(random), random.nextLong()), 1.0); } }
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); } }
@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(); } } } };
TimeStampedTransform3D toTest = new TimeStampedTransform3D(); TimeStampedTransform3D newTimeStampedTransform = new TimeStampedTransform3D(newestPelvisPose, timeStamp); pelvisPosesRegistered.add(newTimeStampedTransform); timeStampedPelvisPoseBuffer.put(newestPelvisPose, timeStamp);
private boolean yawBigInDoubleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
@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 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)); } }
@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)); }
private boolean yawBigInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1, 1, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform yawTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1, 1, 0.8, 0, 0, Math.PI); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(yawTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10); }
private boolean localizeOutsideOfFootInSingleSupport(ExternalPelvisPoseCreator externalPelvisPoseCreator) throws SimulationExceededMaximumTimeException { // FootPosePacket packet = new FootPosePacket(RobotSide.RIGHT, new Point3D(1.0, 1.0, 0.3), new Quat4d(), 0.6); // drcSimulationTestHelper.send(packet); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); RigidBodyTransform outsideOfFootTransform = TransformTools.createTransformFromTranslationAndEulerAngles(1.5, 1.0, 0.8, 0.0, 0.0, 0.0); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(outsideOfFootTransform, timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0); externalPelvisPoseCreator.setNewestPose(posePacket); return drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(10.0); }
@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)); }
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); }
TimeStampedTransform3D pelvisTimeStampedTransformInThePast = new TimeStampedTransform3D(); stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast); pelvisTransformWithOffset.multiply(offsetRotationTransform); TimeStampedTransform3D timeStampedTransform3D = new TimeStampedTransform3D(pelvisTransformWithOffset, timestamp); stampedPosePublisher.publish(HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform3D, 1.0));
TimeStampedTransform3D pelvisTimeStampedTransformInThePast = new TimeStampedTransform3D(); stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast); pelvisTransformWithOffset.multiply(offsetRotationTransform); TimeStampedTransform3D timeStampedTransform3D = new TimeStampedTransform3D(pelvisTransformWithOffset, timestamp); StampedPosePacket stampedPosePacket = new StampedPosePacket("/pelvis", timeStampedTransform3D, 1.0);
TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot);
TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot);
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3d(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quat4d(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3D(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quaternion(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }