private FootstepDataListMessage createFootstepsForWalkingDownRampMediumSteps(ScriptedFootstepGenerator scriptedFootstepGenerator) { Quaternion quaternion = new Quaternion(Math.PI, 0.205, 0.0); double[] quatArray = new double[4]; quaternion.get(quatArray); double[][][] footstepLocationsAndOrientations = new double[][][] {{{6.955, 0.12, 0.722}, {0.0, 0.0, 1.0, 0.0}}, {{6.571, -0.08, 0.722}, {0.0, 0.0, 1.0, 0.0}}, {{6.175, 0.12, 0.644}, quatArray}, {{5.804, -0.08, 0.581}, quatArray}, {{5.419, 0.12, 0.490}, quatArray}, {{5.047, -0.08, 0.410}, quatArray}, {{4.671, 0.12, 0.345}, quatArray}, {{4.289, -0.08, 0.261}, quatArray}, {{3.912, 0.12, 0.170}, quatArray}, {{3.541, -0.08, 0.100}, {0.0, 0.0, 1.0, 0.0}}, {{3.170, 0.12, 0.090}, {0.0, 0.0, 1.0, 0.0}}, {{3.168, -0.08, 0.090}, {0.0, 0.0, 1.0, 0.0}},}; RobotSide[] robotSides = drcSimulationTestHelper.createRobotSidesStartingFrom(RobotSide.RIGHT, footstepLocationsAndOrientations.length); return scriptedFootstepGenerator.generateFootstepsFromLocationsAndOrientations(robotSides, footstepLocationsAndOrientations); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAgainstInterpolation() throws Exception { for (int nTest = 0; nTest < 10; nTest++) { double epsilon = 1.0e-15; Random random = new Random(56416456L); Quaternion quat1 = RandomGeometry.nextQuaternion(random); Quaternion quat2 = RandomGeometry.nextQuaternion(random); Quaternion expectedAverageQuat = new Quaternion(); expectedAverageQuat.interpolate(quat1, quat2, 0.5); AverageQuaternionCalculator averageQuaternionCalculator = new AverageQuaternionCalculator(); averageQuaternionCalculator.queueQuaternion(quat1); averageQuaternionCalculator.queueQuaternion(quat2); averageQuaternionCalculator.compute(); Quaternion actualAverageQuat = new Quaternion(); averageQuaternionCalculator.getAverageQuaternion(actualAverageQuat); if (expectedAverageQuat.getS() * actualAverageQuat.getS() < 0.0) expectedAverageQuat.negate(); double[] expecteds = new double[4]; expectedAverageQuat.get(expecteds); double[] actuals = new double[4]; actualAverageQuat.get(actuals); assertArrayEquals(expecteds, actuals, epsilon); } }
Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); DenseMatrix64F denseMatrix = new DenseMatrix64F(7, 1); quaternion.get(denseMatrix); translation.get(4, denseMatrix); actualTransform.set(denseMatrix); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); double[] array = new double[7]; quaternion.get(array); translation.get(4, array); actualTransform.set(array);
Quaternion randomQuaternion = EuclidCoreRandomTools.nextQuaternion(random); double[] array = new double[4]; randomQuaternion.get(array); FrameQuaternion frameQuaternion = new FrameQuaternion(randomFrame, array); assertTrue(frameQuaternion.getReferenceFrame() == randomFrame); Quaternion randomQuaternion = EuclidCoreRandomTools.nextQuaternion(random); DenseMatrix64F denseMatrix = new DenseMatrix64F(4, 1); randomQuaternion.get(denseMatrix); FrameQuaternion frameQuaternion = new FrameQuaternion(randomFrame, denseMatrix); assertTrue(frameQuaternion.getReferenceFrame() == randomFrame);
public void initialize(DenseMatrix64F accel, DenseMatrix64F pqr, double heading) { setMatrix(bias, pqr); // euler = accel2euler(accel, heading); accel2euler(accel, heading); Quaternion quaternion = new Quaternion(); quaternion.setYawPitchRoll(eulerAngles); quaternion.get(q); }
Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); DenseMatrix64F denseMatrix = new DenseMatrix64F(7, 1); quaternion.get(denseMatrix); translation.get(4, denseMatrix); QuaternionBasedTransform transform = new QuaternionBasedTransform(denseMatrix); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); double[] array = new double[7]; quaternion.get(array); translation.get(4, array); QuaternionBasedTransform transform = new QuaternionBasedTransform(array);