public void getInterpolatedTransform(int index, RigidBodyTransform target) { transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, target, index / (double) (params.pointsPerSweep - 1)); }
TransformInterpolationCalculator transformInterpolationCalculator = new TransformInterpolationCalculator(); transformInterpolationCalculator.interpolate(firstTimeStampedTransform, secondTimeStampedTransform, toTestTimeStampedTransform, 216515L); fail("Should have thrown a RuntimeException as firstTimestamp is smaller than secondTimestamp."); transformInterpolationCalculator.interpolate(firstTimeStampedTransform, secondTimeStampedTransform, toTestTimeStampedTransform, secondTimeStampedTransform.getTimeStamp() + 1); fail("Should have thrown a RuntimeException as the given timestamp is outside bounds."); transformInterpolationCalculator.interpolate(firstTimeStampedTransform, secondTimeStampedTransform, toTestTimeStampedTransform, firstTimeStampedTransform.getTimeStamp() - 1); fail("Should have thrown a RuntimeException as the given timestamp is outside bounds."); transformInterpolationCalculator.interpolate(firstTimeStampedTransform, secondTimeStampedTransform, toTestTimeStampedTransform, timeStampForInterpolation); transformInterpolationCalculator.computeInterpolation(firstTimeStampedTransform.getTransform3D(), secondTimeStampedTransform.getTransform3D(), expectedTransform, alpha);
TimeStampedTransform3D expected = new TimeStampedTransform3D(); TransformInterpolationCalculator transformInterpolationCalculator = new TransformInterpolationCalculator(); transformInterpolationCalculator.interpolate(previous, next, expected, currentTimeStamp); timeStampedPelvisPoseBuffer.findTransform(currentTimeStamp, toTest);
transformInterpolationCalculator.interpolate(floorData, ceilingData, timeStampedTransform3DToPack, timestamp); return true;
PoseReferenceFrame frame2 = new PoseReferenceFrame("frame2", framePose2); TransformInterpolationCalculator transformInterpolationCalculator = new TransformInterpolationCalculator(); transformInterpolationCalculator.computeInterpolation(transform1, transform2, toTestTransform, alpha);
TimeStampedTransform3D expected = new TimeStampedTransform3D(); TransformInterpolationCalculator transformInterpolationCalculator = new TransformInterpolationCalculator(); transformInterpolationCalculator.interpolate(previous, next, expected, currentTimeStamp); timeStampedPelvisPoseBuffer.findTransform(currentTimeStamp, toTest);
transformInterpolationCalculator.interpolate(floorData, ceilingData, timeStampedTransform3DToPack, timestamp); return true;
public void getInterpolatedTransform(int index, RigidBodyTransform target) { transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, target, index / (double) (params.pointsPerSweep - 1)); }
transformInterpolationCalculator.interpolate(floorData, ceilingData, timeStampedTransform3DToPack, timestamp); return true;
public void setWorldTransforms(RigidBodyTransform start, RigidBodyTransform end) { this.worldTransformStart = start; this.worldTransformEnd = end; this.averageTransform = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, averageTransform, .5); }
public void setWorldTransforms(RigidBodyTransform start, RigidBodyTransform end) { this.worldTransformStart = start; this.worldTransformEnd = end; this.averageTransform = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, averageTransform, .5); }
public void interpolate(YoReferencePose start, YoReferencePose goal, double alpha) { start.getTransformToDesiredFrame(interpolationStartingPosition, parentFrame); goal.getTransformToDesiredFrame(interpolationGoalPosition, parentFrame); transformInterpolationCalculator.computeInterpolation(interpolationStartingPosition, interpolationGoalPosition, output, alpha); setAndUpdate(output); }
public void interpolate(YoReferencePose start, YoReferencePose goal, double alpha) { start.getTransformToDesiredFrame(interpolationStartingPosition, getParent()); goal.getTransformToDesiredFrame(interpolationGoalPosition, getParent()); transformInterpolationCalculator.computeInterpolation(interpolationStartingPosition, interpolationGoalPosition, output, alpha); setAndUpdate(output); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationOne() throws Exception { RigidBodyTransform t1 = new RigidBodyTransform(); t1.setIdentity(); RigidBodyTransform t2 = new RigidBodyTransform(); t2.setIdentity(); RigidBodyTransform t3 = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 0.0); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 1.0); assertTrue(t2.equals(t3)); }
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 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); } }
transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t2.epsilonEquals(t3, 1e-6)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t2.epsilonEquals(t3, 1e-6)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationForRotationRoll() throws Exception { double yaw1, pitch1, roll1; double yaw2, pitch2, roll2; RigidBodyTransform t1 = new RigidBodyTransform(); RigidBodyTransform t2 = new RigidBodyTransform(); RigidBodyTransform t3 = new RigidBodyTransform(); double alpha; double[] yawPitchRoll = new double[3]; yaw1 = 0.0; pitch1 = 0.0; roll1 = 0.0; yaw2 = 0.0; pitch2 = 0.0; roll2 = 1.0; t1.setRotationEulerAndZeroTranslation(new Vector3D(roll1, pitch1, yaw1)); t2.setRotationEulerAndZeroTranslation(new Vector3D(roll2, pitch2, yaw2)); alpha = 0.0; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); alpha = 1.0; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t2.epsilonEquals(t3, 1e-6)); alpha = 0.25; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); assertEquals(yawPitchRoll[2], (alpha-1)*roll1 + alpha * roll2, 1e-6); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationForRotationPitch() throws Exception { double yaw1, pitch1, roll1; double yaw2, pitch2, roll2; RigidBodyTransform t1 = new RigidBodyTransform(); RigidBodyTransform t2 = new RigidBodyTransform(); RigidBodyTransform t3 = new RigidBodyTransform(); double alpha; double[] yawPitchRoll = new double[3]; yaw1 = 0.0; pitch1 = 0.0; roll1 = 0.0; yaw2 = 0.0; pitch2 = 1.0; roll2 = 0.0; t1.setRotationEulerAndZeroTranslation(new Vector3D(roll1, pitch1, yaw1)); t2.setRotationEulerAndZeroTranslation(new Vector3D(roll2, pitch2, yaw2)); alpha = 0.0; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); alpha = 1.0; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t2.epsilonEquals(t3, 1e-6)); alpha = 0.25; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); assertEquals(yawPitchRoll[1], (alpha-1)*pitch1 + alpha * pitch2, 1e-6); }
transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3);