public void getInterpolatedTransform(int index, RigidBodyTransform target) { transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, target, index / (double) (params.pointsPerSweep - 1)); }
public void getInterpolatedTransform(int index, RigidBodyTransform target) { transformInterpolationCalculator.computeInterpolation(worldTransformStart, worldTransformEnd, target, index / (double) (params.pointsPerSweep - 1)); }
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);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationForTranslation() throws Exception { RotationMatrix maxtrixIdentity = new RotationMatrix(); maxtrixIdentity.setIdentity(); Vector3D vector1 = new Vector3D(0.0, 0.0, 0.0); Vector3D vector2 = new Vector3D(5.0, 8.0, 10.0); RigidBodyTransform t1 = new RigidBodyTransform(maxtrixIdentity, vector1); RigidBodyTransform t2 = new RigidBodyTransform(maxtrixIdentity, vector2); RigidBodyTransform t3 = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 0.0); Vector3D interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); assertTrue(vector1.epsilonEquals(interpolatedVector, 1e-8)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 1.0); interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); assertTrue(vector2.epsilonEquals(interpolatedVector, 1e-8)); double alpha = 0.25; transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); interpolatedVector = new Vector3D(); t3.getTranslation(interpolatedVector); Vector3D expectedVector = new Vector3D(); expectedVector.scaleAdd((1- alpha), vector1, expectedVector); expectedVector.scaleAdd(alpha, vector2, expectedVector); assertTrue(expectedVector.epsilonEquals(interpolatedVector, 1e-8)); }
transformInterpolationCalculator.computeInterpolation(transform1, transform2, toTestTransform, alpha);
transformInterpolationCalculator.computeInterpolation(firstTimeStampedTransform.getTransform3D(), secondTimeStampedTransform.getTransform3D(), expectedTransform, alpha);
transformInterpolationCalculator.computeInterpolation(t1, t2, t3, alpha); getYawPitchRoll(yawPitchRoll, t3);