public LittleWallsWithIncreasingHeightPlanarRegionEnvironment() { generator.translate(2.0, 0.0, -0.01); generator.addCubeReferencedAtBottomMiddle(6.0, 1.0, 0.01); generator.translate(-2.0, 0.0, 0.0); generator.translate(0.35, 0.2, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.1); generator.translate(0.62, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.14); generator.translate(0.3, -0.3, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 0.1, 0.15); generator.translate(0.4, 0.1, 0.0); generator.addCubeReferencedAtBottomMiddle(0.1, 1.0, 0.11); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public VaryingStairsPlanarRegionEnvironment(double startX, double startZ, double[] stepTreads, double[] stepRises) { generator.translate(startX, 0.0, startZ); for (int i = 0; i < stepTreads.length; i++) { generator.addCubeReferencedAtBottomNegativeXEdgeCenter(stepTreads[i], 1.0, -0.1); generator.translate(stepTreads[i] + 0.01, 0.0, stepRises[i]); } addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public GapPlanarRegionEnvironment(double platform1Length, double platform2Length, double platformWidth, double forwardGapSize, double sideGapSize) { generator.translate(0.0, 0.0, -0.01); generator.addCubeReferencedAtBottomMiddle(platform1Length, platformWidth, 0.01); // ground double platform2Center = 0.5 * (platform1Length + platform2Length) + forwardGapSize; generator.translate(0.5 * (platform1Length + platform2Length) + forwardGapSize, 0.0, 0.0); generator.addCubeReferencedAtBottomMiddle(platform2Length, platformWidth, 0.01); // ground double sideWidth = 0.18; double sideLength = platform1Length + platform2Length + forwardGapSize; double distanceToCenter = 0.5 * sideLength - 0.5 * platform1Length; generator.translate(-platform2Center + distanceToCenter, 0.5 * platformWidth + sideGapSize + 0.5 * sideWidth, 0.0); generator.addCubeReferencedAtBottomMiddle(sideLength, sideWidth, 0.01); // ground addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
private static void setUpFlatBlock(PlanarRegionsListGenerator generator, double xInWorld, double yInWorld, double bottomSurfaceHeightLowestPoint, double blockHeight) { generator.translate(xInWorld, yInWorld, bottomSurfaceHeightLowestPoint); generator.addCubeReferencedAtBottomMiddle(cinderBlockLength, cinderBlockLength, blockHeight); generator.identity(); }
public static PlanarRegionsList generateSteppingStoneField(double steppingStoneWidth, double steppingStoneLength, double stepWidth, double stepLength, int numberOfSteps) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); double platformLength = 0.6; double platformWidth = 1.0; generator.addRectangle(platformLength, platformWidth); generator.translate(0.5 * platformLength, 0.0, 0.0); for (int i = 0; i < numberOfSteps; i++) { RobotSide side = (i % 2 == 0) ? RobotSide.LEFT : RobotSide.RIGHT; double xOffset = stepLength; double yOffset = side.negateIfRightSide(0.5 * stepWidth); generator.translate(xOffset, yOffset, 0.0); generator.addRectangle(steppingStoneLength, steppingStoneWidth); } generator.translate(stepLength + 0.5 * platformLength, 0.0, 0.0); generator.addRectangle(platformLength, platformWidth); return generator.getPlanarRegionsList(); }
public void addRamp(double length, double deltaZ) { checkHasNotBeenGenerated(); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); generator.rotate(-Math.atan2(deltaZ, length), Axis.Y); generator.addRectangle(Math.sqrt(MathTools.square(length) + MathTools.square(deltaZ)), rampWidth); generator.rotate(Math.atan2(deltaZ, length), Axis.Y); generator.translate(length / 2.0, 0.0, deltaZ / 2.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public AlternatingSlopesEnvironment(double rampWidth, double landingLength) { super(); this.rampWidth = rampWidth; generator.addRectangle(Math.sqrt(MathTools.square(landingLength)), rampWidth); generator.translate(landingLength / 2.0, 0.0, 0.0); addPlanarRegionsToTerrain(YoAppearance.Grey()); }
private static PlanarRegionsList simpleOccludedEnvironment(boolean includeGoalPlane) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(2.0, 4.0); generator.translate(1.0, -1.0, 0.5); generator.rotate(0.5 * Math.PI, Axis.Y); generator.addRectangle(0.9, 1.9); if (includeGoalPlane) { generator.identity(); generator.translate(2.0, -1.0, 0.0); generator.addRectangle(1.0, 1.0); } return generator.getPlanarRegionsList(); }
public TwoBollardEnvironment(double barrelSeparation) { // ground plane generator.addRectangle(1.0, barrelSeparation + 0.5); generator.translate(2.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); generator.translate(-4.0, 0.0, 0.0); generator.addRectangle(3.0, 3.0); addPlanarRegionsToTerrain(YoAppearance.RGBColor(110 / 256.0, 121 / 256.0, 121 / 256.0)); // first bollard generator.identity(); generator.translate(0.0, 0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); // second bollard generator.identity(); generator.translate(0.0, -0.5 * barrelSeparation, 0.0); generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight); addPlanarRegionsToTerrain(YoAppearance.Yellow()); }
public ZigZagSlopeEnvironment(double rampSlope, double rampLength, int numberOfRamps, double heightAtRampMiddle) { double rampLengthX = rampLength * Math.cos(rampSlope); double startX = -0.5 * rampLengthX * (numberOfRamps - 1); double slopeSign = -1.0; for (int i = 0; i < numberOfRamps; i++) { generator.identity(); generator.translate(startX + i * rampLengthX, 0.0, heightAtRampMiddle); generator.rotate(rampSlope * slopeSign, Axis.Y); generator.addRectangle(rampLength, 1.0); slopeSign *= -1.0; } addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
private static PlanarRegionsList generateRandomTerrain(Random random) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(0.0, 0.0, 0.001); generator.addRectangle(14.0, 3.0); // floor plane double length = RandomNumbers.nextDouble(random, 0.3, 1.0); double width = RandomNumbers.nextDouble(random, 0.3, 1.0); double height = RandomNumbers.nextDouble(random, 0.07, 0.3); for (int i = 0; i < 100; i++) { generator.identity(); Vector3D translationVector = RandomGeometry.nextVector3D(random, -5.0, -1.0, -0.05, 5.0, 1.0, 0.0); generator.translate(translationVector); Quaternion rotation = RandomGeometry.nextQuaternion(random, Math.toRadians(15.0)); generator.rotate(rotation); generator.addCubeReferencedAtBottomMiddle(length, width, height); } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
private PlanarRegionsList createSimpleOcclusionField(Point3D startPoseToPack, Point3D goalPoseToPack) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); // generator.rotate(Math.toRadians(10.0), Axis.X); generator.addRectangle(6.0, 6.0); generator.translate(-1.0, -1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); generator.identity(); // generator.rotate(Math.toRadians(10.0), Axis.X); generator.translate(1.0, 1.0, 0.5); generator.rotate(-Math.PI / 2.0, Axis.Y); generator.addRectangle(1.0, 4.0); startPoseToPack.set(-2.0, -2.0, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), startPoseToPack, startPoseToPack); goalPoseToPack.set(2.0, 2.0, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), goalPoseToPack, goalPoseToPack); return generator.getPlanarRegionsList(); }
@Before public void setUp() { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(CINDER_BLOCK_START_X, CINDER_BLOCK_START_Y, 0.001); PlanarRegionsListExamples.generateCinderBlockField(generator, CINDER_BLOCK_SIZE, CINDER_BLOCK_HEIGHT, CINDER_BLOCK_COURSE_WIDTH_X_IN_NUMBER_OF_BLOCKS, CINDER_BLOCK_COURSE_LENGTH_Y_IN_NUMBER_OF_BLOCKS, CINDER_BLOCK_HEIGHT_VARIATION, - 0.03, 0.6); cinderBlockField = generator.getPlanarRegionsList(); SimulationTestingParameters parameters = SimulationTestingParameters.createFromSystemProperties(); simulationTestHelper = new DRCSimulationTestHelper(parameters, getRobotModel(), createCommonAvatarInterface(cinderBlockField)); }
public VaryingHeightTiledGroundEnvironment(double tileWidth, int courseLengthInNumberOfTiles, int courseWidthInNumberOfTiles, double nominalGroundHeight, double maxHeightVariation) { generator.identity(); double startCoordinateX = - 0.5 * tileWidth * (courseLengthInNumberOfTiles - 1); double startCoordinateY = - 0.5 * tileWidth * (courseWidthInNumberOfTiles- 1); for (int i = 0; i < courseLengthInNumberOfTiles; i++) { for (int j = 0; j < courseWidthInNumberOfTiles; j++) { double tilePositionX = startCoordinateX + tileWidth * i; double tilePositionY = startCoordinateY + tileWidth * j; double tilePositionZ = nominalGroundHeight + EuclidCoreRandomTools.nextDouble(random, 0.5 * maxHeightVariation); generator.translate(tilePositionX, tilePositionY, tilePositionZ); generator.addRectangle(tileWidth, tileWidth); generator.identity(); } } addPlanarRegionsToTerrain(YoAppearance.Grey()); } }
public static PlanarRegionsList generateRandomObjects(Random random, int numberOfRandomObjects, double maxX, double maxY, double maxZ) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); double length = RandomNumbers.nextDouble(random, 0.2, 1.0); double width = RandomNumbers.nextDouble(random, 0.2, 1.0); double height = RandomNumbers.nextDouble(random, 0.2, 1.0); for (int i = 0; i < numberOfRandomObjects; i++) { generator.identity(); Vector3D translationVector = RandomGeometry.nextVector3D(random, -maxX, -maxY, 0.0, maxX, maxY, maxZ); generator.translate(translationVector); Quaternion rotation = RandomGeometry.nextQuaternion(random); generator.rotate(rotation); generator.addCubeReferencedAtBottomMiddle(length, width, height); } PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); return planarRegionsList; }
@Test(timeout = TIMEOUT) @ContinuousIntegrationTest(estimatedDuration = 10.0) public void testFlatGroundWithWall() { Point3D startPose = new Point3D(-4.805, 0.001, 0.0); Point3D goalPose = new Point3D(4.805, 0.001, 0.0); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(10.0, 5.0); generator.translate(0.0, 0.0, 1.0); generator.rotate(Math.PI / 2.0, Axis.Y); generator.rotate(Math.PI / 2.0, Axis.Z); generator.addRectangle(3.0, 2.0); PlanarRegionsList regions = generator.getPlanarRegionsList(); runTest(startPose, goalPose, regions, OcclusionMethod.OCCLUSION, defaultMaxAllowedSolveTime); }
public BoxTestData() { super(box); PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.translate(2.0, 0.0, 0.0001); generator.addRectangle(5.0, 5.0); generator.addCubeReferencedAtBottomMiddle(1.0, 1.0, 1.0); PlanarRegionsList planarRegionsList = generator.getPlanarRegionsList(); // define start and goal conditions FramePose3D initialStanceFootPose = new FramePose3D(worldFrame); initialStanceFootPose.setPosition(0.0, 0.15, 0.0); RobotSide initialStanceSide = RobotSide.LEFT; FramePose3D goalPose = new FramePose3D(worldFrame); goalPose.setPosition(4.0, 0.0, 0.0); setStartPose(initialStanceFootPose); setGoalPose(goalPose); setPlanarRegions(planarRegionsList); setStartSide(initialStanceSide); } }