@Test// timeout = 30000 public void testCreateMultiGradient() { Color[] gradient = Gradient.createMultiGradient(new Color[]{Color.BLUE, Color.YELLOW}, 5); assertTrue("Color[" + 0 + "] not correct: " + gradient[0], gradient[0].equals(new Color(0, 0, 255))); assertTrue("Color[" + 1 + "] not correct: " + gradient[1], gradient[1].equals(new Color(51, 51, 204))); assertTrue("Color[" + 2 + "] not correct: " + gradient[2], gradient[2].equals(new Color(102, 102, 153))); assertTrue("Color[" + 3 + "] not correct: " + gradient[3], gradient[3].equals(new Color(153, 153, 102))); assertTrue("Color[" + 4 + "] not correct: " + gradient[4], gradient[4].equals(new Color(204, 204, 51))); Assertions.assertExceptionThrown(IllegalArgumentException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { Gradient.createMultiGradient(new Color[]{Color.BLUE}, 5); } }); } }
Assertions.assertExceptionThrown(EmptySupportPolygonException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(EmptySupportPolygonException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(RuntimeException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(RuntimeException.class, new RunnableThatThrows()
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetConstant() throws Exception { Random random = new Random(3453); for (int i = 0; i < ITERATIONS; i++) { int maxNumberOfCoefficients = random.nextInt(10); Trajectory trajectory = new Trajectory(maxNumberOfCoefficients); double t0 = random.nextDouble(); double tf = t0 + random.nextDouble(); double z = RandomNumbers.nextDouble(random, 1.0); if (maxNumberOfCoefficients < 1) { Assertions.assertExceptionThrown(RuntimeException.class, () -> trajectory.setConstant(t0, tf, z)); continue; } trajectory.setConstant(t0, tf, z); for (double t = t0; t <= tf; t += (tf - t0) / 1000) { trajectory.compute(t); assertEquals(z, trajectory.getPosition(), SMALL_EPSILON); assertEquals(0, trajectory.getVelocity(), SMALL_EPSILON); assertEquals(0, trajectory.getAcceleration(), SMALL_EPSILON); } } }
final QuadrupedSupportPolygon secondPoly = createSimplePolygon(); Assertions.assertExceptionThrown(IllegalArgumentException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(IllegalArgumentException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(IllegalArgumentException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(IllegalArgumentException.class, new RunnableThatThrows()
Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows()
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAssertLidarScanRangesEqual() { Random random = new Random(); float[] ranges1 = RandomNumbers.nextFloatArray(random, 720, 0, 5000); float[] ranges2 = RandomNumbers.nextFloatArray(random, 720, 0, 5000); final LidarScan lidarScan1 = new LidarScan(new LidarScanParameters(), new RigidBodyTransform(), new RigidBodyTransform(), ranges1, random.nextInt()); final LidarScan lidarScan2 = new LidarScan(new LidarScanParameters(), new RigidBodyTransform(), new RigidBodyTransform(), ranges2, random.nextInt()); Assertions.assertExceptionThrown(AssertionError.class, new RunnableThatThrows() { @Override public void run() throws Throwable { assertLidarScanRangesEqual(lidarScan1, lidarScan2, 1e-7); } }); LidarScan lidarScan3 = new LidarScan(new LidarScanParameters(), new RigidBodyTransform(), new RigidBodyTransform(), ranges1, random.nextInt()); assertLidarScanRangesEqual(lidarScan1, lidarScan3, 1e-7); float[] ranges1Shortened = ArrayUtils.subarray(ranges1, 0, 3000); LidarScan lidarScan4 = new LidarScan(new LidarScanParameters(), new RigidBodyTransform(), new RigidBodyTransform(), ranges1Shortened, random.nextInt()); assertLidarScanRangesEqual(lidarScan1, lidarScan4, 1e-7); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAssertLidarScanTransformsEqual() { Random random = new Random(); float[] ranges1 = RandomNumbers.nextFloatArray(random, 720, 0, 5000); float[] ranges2 = RandomNumbers.nextFloatArray(random, 720, 0, 5000); RigidBodyTransform randomTransform1 = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform randomTransform2 = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform randomTransform3 = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform randomTransform4 = EuclidCoreRandomTools.nextRigidBodyTransform(random); final LidarScan lidarScan1 = new LidarScan(new LidarScanParameters(), randomTransform1, randomTransform2, ranges1); final LidarScan lidarScan2 = new LidarScan(new LidarScanParameters(), randomTransform1, randomTransform2, ranges2); assertLidarScanTransformsEqual(lidarScan1, lidarScan2, 1e-7); final LidarScan lidarScan3 = new LidarScan(new LidarScanParameters(), randomTransform3, randomTransform4, ranges2); Assertions.assertExceptionThrown(AssertionError.class, new RunnableThatThrows() { @Override public void run() throws Throwable { assertLidarScanTransformsEqual(lidarScan1, lidarScan3, 1e-7); } }); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetInCircleRadius() { final QuadrupedSupportPolygon poly = createSimplePolygon(); assertEquals("not 0.5 radius", 0.5, poly.getInCircleRadius2d(), 1e-7); poly.removeFootstep(RobotQuadrant.FRONT_LEFT); FramePoint3D inCircleCenterToPack = new FramePoint3D(); double radius = poly.getInCircle2d(inCircleCenterToPack); assertEquals("not 0.292893 radius", 0.292893, radius, 1e-5); assertTrue("not equal", inCircleCenterToPack.epsilonEquals(new Vector3D(0.7071067, 0.292893, 0.0), 1e-5)); poly.removeFootstep(RobotQuadrant.FRONT_RIGHT); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { FramePoint3D intersectionToPack = new FramePoint3D(); poly.getInCirclePoint2d(intersectionToPack); } }); }
Assertions.assertExceptionThrown(EmptySupportPolygonException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(EmptySupportPolygonException.class, new RunnableThatThrows()
assertFalse("success should be false", success); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() poly.setFootstep(RobotQuadrant.FRONT_LEFT, new FramePoint3D()); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows()
assertFalse("trot", QuadrupedSupportPolygonTools.isValidTrotPolygon(createPitchedDownPolygon)); Assertions.assertExceptionThrown(RuntimeException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(RuntimeException.class, new RunnableThatThrows()
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testStanceLength() { QuadrupedSupportPolygon create3LegPolygon; create3LegPolygon = createPolygonWithoutLeg(RobotQuadrant.FRONT_LEFT); assertEquals("not 1.0", 1.0, QuadrupedSupportPolygonTools.getStanceLength(create3LegPolygon, RobotSide.RIGHT), 1e-7); create3LegPolygon = createPolygonWithoutLeg(RobotQuadrant.FRONT_RIGHT); assertEquals("not 1.0", 1.0, QuadrupedSupportPolygonTools.getStanceLength(create3LegPolygon, RobotSide.LEFT), 1e-7); create3LegPolygon = createPolygonWithoutLeg(RobotQuadrant.FRONT_LEFT); assertEquals("not 1.0", 1.0, QuadrupedSupportPolygonTools.getStanceLength(create3LegPolygon, RobotSide.RIGHT), 1e-7); create3LegPolygon = createPolygonWithoutLeg(RobotQuadrant.HIND_RIGHT); assertEquals("not 1.0", 1.0, QuadrupedSupportPolygonTools.getStanceLength(create3LegPolygon, RobotSide.LEFT), 1e-7); final QuadrupedSupportPolygon createEmptyPolygon = createEmptyPolygon(); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { QuadrupedSupportPolygonTools.getStanceLength(createEmptyPolygon, RobotSide.LEFT); } }); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSwingDurations() { assertEquals("not correct", 0.05, QuadrupedGaitCycle.SAFE_WALK.getRemainingPhaseDuration(0.0), 1e-7); assertEquals("not correct", 0.04, QuadrupedGaitCycle.SAFE_WALK.getRemainingPhaseDuration(0.01), 1e-7); assertEquals("not correct", 0.01, QuadrupedGaitCycle.SAFE_WALK.getRemainingPhaseDuration(0.99), 1e-7); assertEquals("not correct", 0.20, QuadrupedGaitCycle.SAFE_WALK.getRemainingSwingDuration(RobotQuadrant.FRONT_LEFT, 0.05), 1e-7); assertEquals("not correct", 0.73, QuadrupedGaitCycle.BOUND.getRemainingSwingDuration(RobotQuadrant.HIND_RIGHT, 0.27), 1e-7); assertEquals("not correct", 0.72, QuadrupedGaitCycle.BOUND.getRemainingSwingDuration(RobotQuadrant.HIND_RIGHT, 0.28), 1e-7); assertEquals("not correct", 0.35, QuadrupedGaitCycle.TRANSVERSE_GALLOP.getRemainingSwingDuration(RobotQuadrant.FRONT_LEFT, 0.0), 1e-7); Assertions.assertExceptionThrown(RuntimeException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { // Front right is not a swing leg at 0.05 QuadrupedGaitCycle.SAFE_WALK.getRemainingSwingDuration(RobotQuadrant.FRONT_RIGHT, 0.05); } }); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetLegPairs() { final QuadrupedSupportPolygon quadrupedSupportPolygon = createSimplePolygon(); RobotQuadrant[][] legPairs = quadrupedSupportPolygon.getLegPairs(); assertEquals("not 4", 4, legPairs.length); quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_LEFT); legPairs = quadrupedSupportPolygon.getLegPairs(); assertEquals("not 3", 3, legPairs.length); quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_RIGHT); legPairs = quadrupedSupportPolygon.getLegPairs(); assertEquals("not 1", 1, legPairs.length); quadrupedSupportPolygon.removeFootstep(RobotQuadrant.HIND_LEFT); for (RobotQuadrant robotQuadrant : RobotQuadrant.values) { QuadrupedSupportPolygon createPolygonWithoutLeg = createPolygonWithoutLeg(robotQuadrant); assertEquals("not 3", 3, createPolygonWithoutLeg.getLegPairs().length); } Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { quadrupedSupportPolygon.getLegPairs(); } }); }
Assertions.assertExceptionThrown(RuntimeException.class, () -> trajectory.setLinear(t0, tf, z0, zf)); continue;
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPitchAboutPoint() { ReferenceFrame theFrame = ReferenceFrame.constructARootFrame("theFrame"); double epsilon = 1e-10; final FramePoint3D pointToPitchAboutException = new FramePoint3D(theFrame, 0.0, 0.0, 0.0); final FramePoint3D pointException = new FramePoint3D(ReferenceFrame.getWorldFrame(), 1.0, 1.0, 1.0); final FramePoint3D resultException = new FramePoint3D(); final double pitchException = Math.PI; Assertions.assertExceptionThrown(ReferenceFrameMismatchException.class, new RunnableThatThrows() { @Override public void run() throws Throwable { GeometryTools.yawAboutPoint(pointException, pointToPitchAboutException, pitchException, resultException); } }); FramePoint3D pointToPitchAbout = new FramePoint3D(theFrame, 0, 0, 0); FramePoint3D point = new FramePoint3D(theFrame, 1, 1, 1); double pitch = Math.PI; FramePoint3D result = new FramePoint3D(); GeometryTools.pitchAboutPoint(point, pointToPitchAbout, pitch, result); // System.out.println(result); assertEquals("These should be equal", -1.0, result.getX(), epsilon); assertEquals("These should be equal", 1.0, result.getY(), epsilon); assertEquals("These should be equal", -1.0, result.getZ(), epsilon); }
Assertions.assertExceptionThrown(RuntimeException.class, () -> trajectory.setQuadratic(t0, tf, z0, zd0, zf)); continue;
Assertions.assertExceptionThrown(RuntimeException.class, () -> trajectory.setCubic(t0, tf, z0, zd0, zf, zdf)); continue;
final QuadrupedSupportPolygon poly6 = new QuadrupedSupportPolygon(); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() final QuadrupedSupportPolygon poly9 = new QuadrupedSupportPolygon(); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows() final QuadrupedSupportPolygon poly12 = new QuadrupedSupportPolygon(); Assertions.assertExceptionThrown(UndefinedOperationException.class, new RunnableThatThrows()
final FramePoint3D resultException = new FramePoint3D(); final double yawException = Math.PI; Assertions.assertExceptionThrown(ReferenceFrameMismatchException.class, new RunnableThatThrows()